(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210849438.1 (22)申请日 2022.07.19 (71)申请人 无锡军工智能电气股份有限公司 地址 214000 江苏省无锡市新吴区硕放经 发五路26号 (72)发明人 叶承宗　孙志年　王颖杰　周鑫龙　 刘飞龙　 (74)专利代理 机构 无锡市兴为专利代理事务所 (特殊普通 合伙) 32517 专利代理师 屠志力 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种井下巡检机器人双修正磁导航控制方 法 (57)摘要 本发明提供一种井下巡检机器人双修正磁 导航控制方法， 包括： 在巡检机器人上前后布设 两个磁导航传感器， 通过两个磁导航传感器监测 机器人的偏移量之差和方向角， 再计算速度调节 量， 与给定参考值比较得到左右两侧轮子的速度 调节量； 所述两个磁导航传感器分别是第一磁导 航传感器A和第二磁导航传感器B， 第一磁导航传 感器A位于第二磁导航传感器B的前侧。 本发明能 够增强寻迹效率， 使得巡检机器人运动更顺滑， 减少左右摆动。 权利要求书1页 说明书3页 附图2页 CN 115042188 A 2022.09.13 CN 115042188 A 1.一种井下巡检机器人双修正磁导航控制方法， 其特征在于， 包括： 在巡检机器人上前 后布设两个磁导航传感器， 通过两个磁导航传感器监测机器人的偏移量之差和方向角， 再 计算速度调节量， 与给定参考值比较得到左右两侧轮子的速度调节量； 所述两个磁导航传 感器分别是第一磁导航传感器A和第二磁导航传感器B， 第一磁导航传感器A位于第二磁导 航传感器B的前侧。 2.如权利要求1所述的井下巡检机器人双修正磁导航控制方法， 其特征在于， 所述井下 巡检机器人双修 正磁导航控制方法具体包括以下步骤： 步骤S10， 分别采集第一磁导航传感器A的中心距离 给定磁路轨迹的距 离x1， 第二磁导航 传感器B的中心 距离给定磁路轨 迹的距离x2， 以及两个 磁导航传感器之间的距离y； ； 步骤S20， 计算巡检机器人的偏移量之差xs： xs＝x2‑x1                (1) 步骤S30， 计算巡检机器人的方向角 θ： 步骤S40， 将理想状态下巡检机器人的偏移量0与第一磁导航传感器A的中心距离给定 磁路轨迹的距离x1作差， 即误差： Δx1＝0‑x1＝‑x1           (3) 再将误差值Δx1输入第一PI调节器， 得 出第一速度调节量Δv1； 步骤S50， 将理想状态下巡检机器人的方向角0与巡检机器人计算得到的方向角 θ作差， 即误差： Δθ ＝0‑θ ＝‑θ            (4) 再将误差值Δθ 输入第二PI调节器， 得 出第二速度调节量Δv2； 步骤S60， 将第一速度调节量Δv1与第二速度调节量Δv2相加得出速度调节量Δv， 即： Δv＝Δv1+Δv2             (5) 步骤S70， 将给定参考值速度V*与速度调节量Δv相加得出左侧轮子速度调节量Δv左， 即： Δv左＝V*+Δv             (6) 步骤S80： 将给定参考值速度V*与速度调节量Δv作差得出右侧轮子速度调节量Δv右， 即： Δv右＝V*‑Δv             (7)。 3.如权利要求1所述的井下巡检机器人双修 正磁导航控制方法， 其特 征在于， 第一磁导 航传感器A和第二磁导 航传感器B 在巡检机器人 上左右对称布设。权　利　要　求　书 1/1 页 2 CN 115042188 A 2一种井下巡检机 器人双修正磁导航控制方 法 技术领域 [0001]本发明涉及矿井巡检机器人的导航技术领域， 尤其是一种井下巡检机器人双 修正 磁导航控制方法。 背景技术 [0002]我国是煤炭大国， 煤炭使用量和煤炭储存量都位居世界前列， 然而由于煤矿开采 环境复杂， 危险性高， 煤矿安全事故时有发生。 为了保证煤矿工人的生命安全以及开采工作 顺利进行， 需要经常对井下的环境进 行巡检。 传统的巡检方式是采用人工巡检为主， 但人工 巡检效率低、 劳动强度大， 而且依赖于巡检工人 的主观判断， 可靠性差。 随着机器人技术的 发展， 巡检机器人能够实现地面对井下环境进行实时监控反馈， 因此在煤矿开采中得到了 越来越多的应用。 [0003]巡检机器人在井下行走过程中需要对其进行导航定位， 但是全球卫星导航系统通 常只能应用于地面之上进行导航定位， 对于煤矿矿井， 导航信号无法穿过地表对巡检机器 人进行精准导航； 且矿井下地况复杂、 电磁信号干扰严重， 对定位效果产生了更多负面的影 响。 此外， 井下防爆 激光导航、 视觉导航等导航方式设备成本高昂， 受扰严重， 因此目前应用 都不太成熟。 而磁导航由于航向稳定、 准确度高的优势， 在井下巡检机器人导航定位过程中 得到了应用。 但传统使用单个磁导航传感器的磁导航方式， 存在着机器人沿磁路轨迹左右 摆动、 寻迹效率低下、 运动能耗大等问题。 发明内容 [0004]本发明的目的是在于克服现有技术中存在的不足， 提供一种井下巡检机器人双修 正磁导航控制方法， 能够 使得井下巡检机器人运动更顺滑， 减少 左右摆动。 为 实现以上技术 目的， 本发明实施例采用的技 术方案是： [0005]本发明实施例提供了一种井下巡检机器人双修正磁导航控制方法， 包括： 在巡检 机器人上前后布设两个磁导航传感器， 通过两个磁导航传感器监测机器人的偏移量之差和 方向角， 再计算速度调节量， 与给定参考值比较得到左右两侧轮子的速度调节 量； 所述两个 磁导航传感器分别是第一磁导航传感器A和第二磁导航传感器B， 第一磁导航传感器A位于 第二磁导 航传感器B的前侧。 [0006]进一步地， 所述井下巡检机器人双修 正磁导航控制方法具体包括以下步骤： [0007]步骤S10， 分别采集第一磁导航传感器A的中心距离给定磁路轨迹的距离x1， 第二 磁导航传感器B的中心 距离给定磁路轨 迹的距离x2， 以及两个 磁导航传感器之间的距离y； ； [0008]步骤S20， 计算巡检机器人的偏移量之差xs： [0009]xs＝x2‑x1                (1) [0010]步骤S30， 计算巡检机器人的方向角 θ： [0011] 说　明　书 1/3 页 3 CN 115042188 A 3