众所周知，煤炭矿产地质环境特殊且煤矿井下巷道内部环境错综复杂，井下一旦发生矿难，因通信中断所造成的人员物品损失不计其数。因此，设计一款井下智能安全巡检机器人，使其在井下巷道内部进行安全巡检工作可以有效降低矿产行业的事故发生率和人员死亡率，对于井下安全巡检具有十分重要的意义。

针对井下安全巡检机器人的实时高精度定位要求，本文对井下组合导航系统的工作原理进行阐述，分析了捷联惯性导航系统（SINS）由于时间和路程所造成的累计误差以及RFID射频标签工作原理，设计出了一种以SINS系统为主、RFID系统为辅的煤矿井下组合导航实时高精度定位系统，其利用RFID电子射频标签内部预设的真实地理位置信息并结合扩展卡尔曼滤波算法（EKF），对SINS系统的累积误差进行修正，从而提高了井下安全巡检机器人的定位精度。

安全巡检机器人的多功能化使其重量和体积也逐步增大，难以适应煤矿井下巷道内部的狭小环境，所以多机器人协同作业应运而生。本文针对煤矿井下内部空间狭小的特点，设计了一种结合图论与一阶一致性算法的领航-跟随型多移动机器人编队算法模型，该模型将图论的概念引入到领航-跟随方式的编队模型中，同时针对该模型分别设计了理想与加入障碍点两种仿真环境。在Matlab软件仿真平台对本文的实时高精度组合导航定位系统算法和多移动机器人编队算法进行仿真实验，实验结果验证了这两种算法的可行性与可靠性。

本文搭建了机器人硬件平台，IMU模块选用国产的CH100DK模块，M10-HA电子阅读器读取ISO 14443A近耦合IC卡内部预设的真实地理位置信息，在煤矿井下巷道实验室，以机器人为平台对组合导航实时高精度定位系统进行实验测试，测试结果表明定位误差控制在厘米级，对煤矿井下安全巡检有一定的实用价值。

As we all know, the geological environment of coal mining is special and the internal environment of underground roadway of coal mine is complex. Once a mining accident occurs underground, countless losses of personnel and goods will be caused by communication interruption. Therefore, designing an underground intelligent safety inspection robot to conduct safety inspection in the underground roadway can effectively reduce the accident rate and personnel mortality in the mineral industry, it is of great significance for underground safety inspection.

According to the real-time and high-precision positioning requirements of underground safety inspection robot, the working principles of underground integrated navigation system were described in this paper. After analyzing the cumulative errors caused by time and distance of the strapdown inertial navigation system (SINS) and the working principle of the RFID radio frequency tag, a real-time high-precision positioning system for underground combined navigation based on SINS system and supplemented by RFID system is designed for coal mine, which uses the preset real geographical location information in RFID electronic RF tag and combined with extended Kalman filter algorithm (EKF) to correct the cumulative error of SINS system, so the positioning accuracy of underground safety inspection robot is improved.

The multi-function of safety inspection robot makes its weight and volume gradually increase, which is difficult to adapt to the narrow environment inside the coal mine roadway, so the multi robot cooperative operation came into being. According to the characteristics of narrow internal space in coal mine, a pilot follow multi mobile robot formation algorithm model combining graph theory and first-order consistency algorithm is designed in this paper. The model introduces the concept of graph theory into the pilot follow formation model. At the same time, two simulation environments including ideal and obstacle points are designed for the model. The real-time high-precision integrated navigation and positioning system algorithm and multi mobile robot formation algorithm are simulated on the Matlab software simulation platform. The experimental results verify the feasibility and reliability of the two algorithms.

In this paper, the robot hardware platform is built. The IMU module selects the domestic CH100DK module, and the M10-HA e-reader reads the real geographic location information preset inside the ISO 14443A near coupled IC card. In the coal mine tunnel laboratory, the integrated navigation real-time high-precision positioning system is tested on the platform of the robot. The test results show that the positioning error is controlled at the centimeter level, which has certain practical value for safety inspection in coal mine.

[1] 张晓莉, 王张哲. 井下巡检机器人实时高精度定位方法[J]. 矿业研究与开发, 2021, 41(10): 158-161.

[2] 刘送永, 崔玉明. 煤矿井下定位导航技术研究进展[J]. 矿业研究与开发, 2019, 39(7): 114-120.

[3] 裴文良, 张树生, 李军伟. 矿用机器人设计及其应用[J]. 制造业自动化, 2017, 39(2): 73-74.

[4] 李学民. 矿用巡检机器人关键技术分析[J]. 煤矿机械, 2018, 39(5): 71-73.

[5] 沈超. 矿用巡检机器人在黄陵一号煤矿的应用[J]. 陕西煤炭, 2020, 39(2): 118-120.

[6] 王剑, 周子健, 姜维, 等. GPS-BDS联合解算的列车高精度实时定位方法[J]. 交通运输工程学报, 2021, 21(05): 286-296.

[7] 问靖, 王建华, 郑翔, 等. 基于GPS和IMU的欠驱动无人水面艇动力定位方法[J]. 传感器与微系统, 2022, 41(01): 54-57.

[8] 陈显宝, 金隼, 罗磊, 等. 基于视觉定位的AGV定位精度提高方法[J]. 机械设计与研究, 2021, 37(01): 36-40.

[9] Kai H, Ma X. Research on Avoidance Obstacle Strategy of Coal Underground Inspection Robot Based on Binocular Vision[C]//2017 29th Chinese Control and Decision Conference (CDCC). IEEE, 2017: 6732-6737.

[10] Xu Z, Yang W, You K, et al. Vehicle autonomous localization in local area of coal mine tunnel based on vision sensors and ultrasonic sensors[J]. PloS one, 2017, 12(1): e0171012.

[11] 张强, 高颂. 基于红外同步的室内超声定位系统的设计[J]. 国外电子测量技术, 2017, 36(10): 86-91.

[12] 袁帅, 刘美妍. 浅析UWB定位技术的应用[J]. 电子世界, 2021(23): 180-181.

[13] 周军, 魏国亮, 田昕, 等. 融合UWB和IMU数据的新型室内定位算法[J]. 小型微型计算机系统, 2021, 42(08): 1741-1746.

[14] 张海军, 孙学成, 赵小虎, 等. 煤矿井下UWB人员定位系统研究[J]. 工矿自动化, 2022, 48(02): 29-34.

[15] 孙泽鹏. 浅谈惯导发展及其分类[J]. 山东工业技术, 2017(17): 277-277.

[16] 马宏伟, 张璞, 毛清华, 等. 基于捷联惯导和里程计的井下机器人定位方法研究[J]. 工矿自动化, 2019, 45(04): 35-42.

[17] Fan Q, Li W, Hui J, et al. Integrated Positioning for Coal Mining Machinery in Enclosed Underground Mine Based on SINS/WSN[J]. The Scientific World Journal, 2014, 2014(1): 1-12.

[18] Song X, Li X, Tang W, et al. A Hybrid Positioning Strategy for Vehicles in a Tunnel Based on RFID and In-Vehicle Sensors[J]. Sensors, 2014, 14(12): 23095-23118.

[19] 杨立炜, 付丽霞, 李萍. 多智能体系统编队控制发展综述[J]. 电子测量技术, 2020, 43(24): 18-27.

[20] Huang J, Zhou N, M C. Adaptive Fuzzy Behavioral Control of Seconde-Order Autonomous Agents with Prioritized Missions: Theory and Experiments[J]. IEEE Transactions on Industrial Electronics, 2019, 66(12): 9612-9622.

[21] Lin S, Jia R, Yue M, et al. On Composite Leader–follower Formation Control for Wheeled Mobile Robots with Adaptive Disturbance Rejection[J]. Applied Artificial Intelligence, 2019, 33(14): 1-21.

[22] 刘安东, 李佳, 滕游. 多移动机器人编队的鲁棒预测控制[J]. 控制工程, 2018, 25(2): 267-272.

[23] Hassan M F, Hammuda M. Leader-follower formation control of mobile nonholonomic robots via a new observer-based controller[J]. International Journal of Systems Science, 2020, 51(7): 1243-1265.

[24] Li Y, Zhu L, Y G. Observer-based multivariable fixed-time formation control of mobile robots[J]. Journal of Systems Engineering and Electronics, 2020, 31(2): 403-414.

[25] Grandiemail R, Melchiorri C. A Distributed Multi-level PSO Control Algorithm for Autonomous Underwater Vehicles[C]. International Conference on Bio-Inspired Models of Network, Information, and Computing Systems, Springer, Cham, 2012: 75-90.

[26] 韩青, 张常亮. Leader-Followers多机器人编队控制方法[J]. 机床与液压, 2017, 45(9): 1-4.

[27] 林子键, 王秋阳, 肖杨. 多机器人编队围捕算法研究[J]. 计算机仿真, 2017, 34(4): 350-355.

[28] 王钦钊, 程金勇, 李小龙. 基于Back Stepping的全向机器人编队控制[J]. 四川兵工学报, 2017, 38(8): 98-102.

[29] 张大伟, 孟森森, 邓计才. 多移动微小型机器人编队控制与协作避碰研究[J]. 仪器仪表学报, 2017, 38(3): 578-585.

[30] Feng X, Minzhou D, Juntao F. An Improved Attitude Compensation Algorithm for SINS/GNS Integrated Navigation System[J]. Journal of Sensors, 2021, 245: 323-329.

[31] 高社生, 何鹏举, 杨波. 组合导航原理及应用[M]. 西安: 西北工业大学出版社, 2012.

[32] Zhang T, Shi H, Chen L, et al. AUV Positioning Method Based on Tightly Coupled SINS/LBL for Underwater Acoustic Multipath Propagation[J]. Sensors, 2016, 16(3): 357-357.

[33] 杨海, 李威, 罗成名, 等. 基于捷联惯导的采煤机定位定姿技术实验研究[J]. 煤炭学报, 2014, 39(12): 2550-2556.

[34] 陈宇, 付书堂, 刘建团. 基于机载捷联惯导计算导弹初始姿态的算法[J]. 航空兵器, 2016, 23(2): 29-31.

[35] 胡杰, 王子卉, 朱倚娴. 基于运动检测的飞行区车辆差分北斗/ SINS组合导航[J]. 大地测量与地球动力学, 2021, 41(4): 346-350.

[36] 王可, 刘立刚, 周斌, 等. 基于SINS/GNSS/轮速的列车组合导航算法研究[J]. 系统仿真学报, 2021, 33(1): 189-195.

[37] 张庆, 王学文, 谢嘉成, 等. 基于捷联惯导系统的采煤机定位与姿态调整[J]. 工矿自动化, 2017, 43(10): 83-89.

[38] 张金尧, 李威, 杨海, 等. 采煤机捷联惯导定位方法研究[J]. 工矿自动化, 2016, 42(3): 52-55.

[39] 李彩玲, 高雅萍, 李春花, 等. GNSS/SINS组合导航模块在不同车载环境下的定位性能分析[C]. 第十二届中国卫星导航年会论文集——S06 时间基准与精密授时.中国卫星导航学术年会组委会, 2021: 23-30.

[40] 刘公绪, 史凌峰. 室内导航与定位技术发展综述[J]. 导航定位学报, 2018, 6(2): 7-14.

[41] L.Gay J, E.Carmichael K, C.Laflammea C, et al. Novel use of radio frequency identification (RFID) provides a valid measure of indoor stair-based physical activity[J]. Applied Ergonomics, 2021, 95: 103431.

[42] Balog M, Szilágyi E, Dupláková D, et al. Effect verification of external factor to readability of RFID transponder using least square method[J]. Measurement, 2016, 94(7): 233-238.

[43] Liu Y, Fan X, Lv C, et al. An innovative information fusion method with adaptive Kalman filter for integrated INS/GPS navigation of autonomous vehicles[J]. Mechanical Systems and Signal Processing, 2018, 100: 605-616.

[44] 崔凡, 张兴敢, 柏业超. 基于扩展卡尔曼滤波的横向速度估计[J]. 南京大学学报(自然科学), 2021, 57(5): 864-869.

[45] Sabzevari D, Chatraei A. INS/GPS Sensor Fusion based on Adaptive Fuzzy EKF with Sensitivity to Disturbances[J]. IET Radar, Sonar & Navigation, 2021, 15(11): 1535-1549.

[46] Li Z, Feng L, Yang A, et al. Fusion Based on Visible Light Positioning and Inertial Navigation Using Extended Kalman Filters[J]. Sensors, 2017, 17(5): 1093-1093.

[47] 赵悦, 兰英, 屈贤. 基于MEMS传感器的煤矿井下人员定位系统设计[J]. 工矿自动化, 2018, 44(8): 87-91.

[48] 潘祥生, 陈晓晶. 矿用智能巡检机器人关键技术研究[J]. 工矿自动化, 2020, 46(10): 43-48.

[49] 邓叶, 姜香菊. 基于改进人工势场法的四旋翼无人机航迹规划算法[J]. 传感器与微系统, 2021, 40(7): 130-133.

[50] Antonio L, Janset D, Alvarez J N. Leader-Follower Formation and Tracking Control of Mobile Robots Along Straight Paths[J]. IEEE transactions on control systems technology: A publication of the IEEE Control Systems Society, 2016, 24(2): 727-732.

[51] Changchun H, Yafeng L, Xinping G. Leader-Following Consensus for High-Order Nonlinear Stochastic Multiagent Systems[J]. IEEE transactions on cybernetics, 2017, 47(8): 1882-1891.

[52] Tan G, Zhuang J, Zou J, et al. Coordination control for multiple unmanned surface vehicles using hybrid behavior-based method[J]. Ocean Engineering, 2021, 232: 1-17.

[53] 李正平, 鲜斌. 基于虚拟结构法的分布式多无人机鲁棒编队控制[J]. 控制理论与应用, 2020, 37(11): 2423-2431.

[54] 刘安东, 秦冬冬. 基于虚拟结构法的多移动机器人分布式预测控制[J]. 控制与决策, 2021, 36(5): 1273-1280.

[55] 高鹏程, 刘小雄, 黄剑雄, 等. 基于一致性的多无人机编队算法研究[J]. 计算机测量与控制, 2021, 29(3): 197-202.

[56] Olfati-Saber R, Murray R M. Consensus problems in networks of agents with switching topology and time-delays[J]. IEEE Transactions on Automatic Control, 2004, 49(9): 1520-1533.

[57] 张晓莉, 王张哲. 基于领航-跟随模型的井下多移动机器人编队研究[J]. 矿业研究与开发, 2022, 42(02): 179-182.

[58] 黄健飞, 马彦. 基于跟随领航者的车辆自适应编队控制[J]. 吉林大学学报(信息科学版), 2019, 37(03): 253-259.

[59] 胡阳修, 贺亮, 赵长春, 等. 基于路径跟随的改进领航-跟随无人机协同编队方法[J]. 飞控与探测, 2021, 4(02): 26-35.