煤矿机器人是实现煤矿智能化的主力军，提高移动机器人在煤矿巷道自主导航的鲁棒性、可靠性和稳定性迫在眉睫。本文以构建基于激光雷达与惯导紧耦合的SLAM系统为核心框架，以机器人自主导航为应用方向，聚焦煤矿巷道移动机器人环境感知、精确定位、地图构建、路径规划等难题，深入研究三维激光点云配准、激光惯性里程计状态估计、煤矿巷道全局一致地图构建、煤矿巷道移动机器人路径规划等关键技术问题，旨在提高移动机器人在煤矿巷道环境中的自主导航能力。

      移动机器人激光点云配准是利用三维激光雷达在煤矿巷道进行环境感知的基础，由于激光点云具有局部稀疏、运动畸变的特征，而巷道环境又存在弱纹理、特征退化的特点，使用现有的方法直接进行点云配准，导致状态估计或点云配准的精度下降。针对此问题，提出了融合IMU预积分的三维激光点云配准方法，包括IMU预积分、点云预处理、点云帧间匹配和点云优化配准四个模块，通过构建IMU预积分误差方程和激光点云配准误差方程，利用高斯牛顿求解对激光雷达相邻两帧进行优化配准。实验结果表明，该方法具有更高的点云配准精度、配准成功率和计算效率。

       移动机器人状态估计精度直接影响激光SLAM系统的整体性能，由于激光雷达本身会受稀疏性和运动扰动的影响，导致对巷道环境感知准确度降低，并且在巷道环境发生特征退化时，其感知到的特征点数量严重减少，导致激光里程计的精度降低。针对此问题，提出了基于迭代卡尔曼滤波的激光惯性里程计构建方法，通过建立迭代卡尔曼滤波器，经过状态传播、状态更新、状态合成的滤波过程，将机器人的先验位姿转化为更加准确的后验位姿，如此循环迭代得到紧耦合的激光惯性里程计。实验结果表明，该方法能使移动机器人具有更高的状态估计精度和鲁棒性。

       移动机器人全局一致地图构建是实现自主导航的关键，由于激光惯性里程计属于增量式运动估计，使机器人的位姿误差随时间增加而不断累积，导致所构建的全局地图出现一定程度的漂移。针对此问题，提出了位姿图优化的全局一致地图构建方法，通过构建位姿图优化系统框架，分别定义了激光里程计约束、IMU预积分约束、巷道平面约束、回环检测约束和机器人状态节点，并在此优化框架中添加上述四种不同约束条件，优化机器人相邻关键帧之间的相对位姿。实验结果表明，该方法能满足系统实时性的需求，有效降低了系统的累积误差，确保了所构建地图的全局一致性。

      移动机器人在煤矿巷道实现自主导航必须开展路径规划方面的研究，需要在精确定位和地图构建的基础上进行全局与局部路径规划，并在路径规划的结果进行轨迹生成与轨迹优化。机器人在煤矿巷道环境中进行路径规划存在全局搜索范围广、局部可通行区域狭窄、不满足运动学约束等问题。针对此问题，提出了改进Hybrid A*全局路径规划+占据概率局部路径规划的方法，通过构建启发函数进行前向搜索，使得机器人在连续空间中进行Hybrid A*的全局搜索，扩展当前位置节点直到目标节点，实现全局路径规划。建立Minimum Snap的目标函数，进行轨迹生成。通过分别构建“软硬约束”和可通行区域约束，进行优化轨迹。并通过构建占据概率模型实时判断机器人前进方向上存在障碍物的概率，实现局部路径规划。实验结果证明，该方法分别在仿真环境和模拟巷道真实环境中验证了移动机器人能找到由起始位置到目标位置的最优路径，并避免发生碰撞，实现了自主导航。

   Coal mine robots are the main force to realize the intelligence of coal mine, and it is urgent to improve the robustness, reliability and stability of mobile robots for autonomous navigation in coal mine roadway. This paper takes the construction of a tightly coupled SLAM system based on LiDAR-IMU as the core framework and robot autonomous navigation as the application direction, focusing on the challenges of environment sensing, precise positioning, map construction and path planning for mobile robots in coal mine roadway, and deeply investigates the key technical problems such as 3D LiDAR point cloud registration, LiDAR-inertial odometry state estimation, global consistent map construction in coal mine roadway, and path planning for mobile robots in coal mine roadway, aiming to improve the autonomous navigation capability of mobile robots in coal mine roadway environment.

  Mobile robot LiDAR point cloud registration is the foundation of using 3D LiDAR to perceive the environment in coal mine roadway. Due to the characteristics of local sparsity and motion distortion in LiDAR point clouds, as well as the weak texture and feature degradation in roadway environments, using existing methods for point cloud registration directly leads to a decrease in the accuracy of state estimation or point cloud registration. To address this problem, a 3D LiDAR point cloud registration method integrating IMU pre-integration is proposed, including four modules: IMU pre-integration, point cloud pre-processing, point cloud scan-to-scan frame matching and point cloud optimal registration. By constructing the IMU pre-integration error equation and the LiDAR point cloud registration error equation, a Gaussian Newton solution is used to optimize the registration of two adjacent LiDAR frames. The experimental results show that the method has higher point cloud registration accuracy, registration success rate and computational efficiency.

   The accuracy of mobile robot state estimation directly affects the overall performance of the LiDAR SLAM system. Because the LiDAR itself will be affected by sparsity and motion disturbance, the perception accuracy of the roadway environment will be reduced. When the characteristics of the roadway environment degenerate, the number of characteristic points it perceives will be seriously reduced, leading to the reduction of the accuracy of the LiDAR odometry. To solve this problem, an iterative Kalman filter-based LiDAR-Inertial odometry construction method is proposed. By establishing an iterative Kalman filter, the robot a priori positional is filtered through the process of state propagation, state update, and state synthesis to make its a posteriori positional more accurate. In this way, a tightly coupled LiDAR-Inertial odometry is obtained through cyclic iteration.The experimental results show that the proposed method can make the mobile robot have higher accuracy and robustness of state estimation.

   Global consistent map construction for mobile robots is the key to achieve autonomous navigation. Since LiDAR-Inertial odometry is an incremental motion estimation, it makes the robot's position and pose error accumulate with time, resulting in a certain degree of drift in the constructed global map. To address this problem, a globally consistent map construction method for positional map optimization is proposed. By constructing a framework for the pose graph optimization system, LiDAR odometry constraint, IMU pre-integration constraint, roadway plane constraints, loop detection constraints and robot state node are defined, and four different constraints mentioned above are added to this optimization framework to optimize the relative position and pose between adjacent key frames of the robot. The experimental results show that the method can meet the demand of system real-time, effectively reduce the cumulative error of the system, and ensure the global consistency of the constructed maps.

   Research on path planning is necessary for mobile robots to achieve autonomous navigation in coal mine roadway, which requires global and local path planning based on precise positioning and map construction, and trajectory generation and trajectory optimization for the results of path planning. Path planning in coal mine roadway environment has the problems of wide global search range, narrow local passable area, and not satisfying kinematic constraints. To address this problem, an improved Hybrid A* global path planning and occupancy probability local path planning method is proposed, which enables the robot to perform Hybrid A* global search in continuous space by constructing a heuristic function for forward search, expanding the current position node until the target node to achieve global path planning. The objective function of Minimum Snap is established for trajectory generation. The trajectory is optimized by constructing the "soft and hard constraints" and the relevant constraints of the passable area respectively. And the probability of obstacles in the forward direction of the robot is judged in real time by constructing the occupation probability model to realize local path planning. The experimental results demonstrate that this method validates the mobile robot's ability to find the optimal path from the starting position to the target position in both simulated and real roadway environments, and avoids collisions, achieving autonomous navigation.

[1] 王国法, 赵国瑞, 任怀伟. 智慧煤矿与智能化开采关键核心技术分析[J]. 煤炭学报, 2019,44(01):34-41.

[2] 葛世荣,胡而已,裴文良. 煤矿机器人体系及关键技术[J]. 煤炭学报, 2020,45(01): 455-463.

[3] 李芳玮,胡而已,张冬阳. 煤矿机器人研发应用现状及趋势[J]. 中国煤炭, 2019,45(07):

28-32.

[4] 张树生,马静雅,岑强. 煤矿综采工作面巡检机器人系统研究[J]. 煤炭科学技术, 2019,47(10): 136-140.

[5] 杨林,马宏伟,王岩. 煤矿井下移动机器人基于激光惯性的融合SLAM算法[J]. 煤炭学报, 2022,47(9): 3522-3534.

[6] 马宏伟,王岩,杨林. 煤矿井下移动机器人深度视觉自主导航研究[J]. 煤炭学报, 2020,45(6): 2193-2206.

[7] 李猛钢. 煤矿救援机器人导航系统研究[D]. 中国矿业大学, 2017.

[8] Fairfield N . Localization, mapping, and planning in three-dimensional environments.[D]. Carnegie Mellon University. 2009.

[9] Thrun S. Simultaneous localization and mapping[M]//Robotics and cognitive approaches to spatial mapping. Springer, Berlin, Heidelberg, 2007: 13-41.

[10] Bailey T, Durrant-Whyte H. Simultaneous localization and mapping (SLAM): Part II[J]. IEEE robotics & automation magazine, 2006, 13(3): 108-117.

[11] Mountney P, Stoyanov D, Davison A, et al. Simultaneous stereoscope localization and soft-tissue mapping for minimal invasive surgery[C]//International Conference on Medical Image Computing and Computer-Assisted Intervention. Springer, Berlin, Heidelberg, 2006: 347-354.

[12] Fuentes-Pacheco J, Ruiz-Ascencio J, Rendón-Mancha J M. Visual simultaneous localization and mapping: a survey[J]. Artificial intelligence review, 2015, 43(1): 55-81.

[13] Yamauchi B M. PackBot: a versatile platform for military robotics[C]//Unmanned ground vehicle technology VI. International Society for Optics and Photonics, 2004, 5422: 228-237.

[14] 宗文鹏, 李广云, 李明磊,等. 激光扫描匹配方法研究综述[J]. 中国光学, 2018, 11(6): 914-930.

[15] Besl P J, McKay N D. Method for registration of 3-D shapes[C]//Sensor fusion IV: control paradigms and data structures. Spie, 1992, 1611: 586-606.

[16] Chen Y, Medioni G. Object modelling by registration of multiple range images[J]. Image and vision computing, 1992, 10(3): 145-155.

[17] Segal A, Haehnel D, Thrun S. Generalized-icp[C]//Robotics: science and systems. 2009, 2(4): 435-436.

[18] Pomerleau F, Colas F, Siegwart R, et al. Comparing ICP variants on real-world data sets[J]. Autonomous Robots, 2013, 34(3): 133-148.

[19] Yoshitaka H, Hirohiko K, Akihisa O, et al. Mobile robot localization and mapping by scan matching using laser reflection intensity of the sokuiki sensor[C]//IECON 2006-32nd Annual Conference on IEEE Industrial Electronics. IEEE, 2006: 3018-3023.

[20] Bosse M, Zlot R. Continuous 3D scan-matching with a spinning 2D laser[C]//2009 IEEE International Conference on Robotics and Automation. IEEE, 2009: 4312-4319.

[21] 李明磊, 李广云, 王力,等. 采用八叉树体素生长的点云平面提取[J]. 光学精密工程, 2018, 26(1):172-183.

[22] Alismail H, Baker L D, Browning B. Continuous trajectory estimation for 3D SLAM from actuated lidar[C]//2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014: 6096-6101.

[23] Biber P, Straßer W. The normal distributions transform: A new approach to laser scan matching[C]//Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)(Cat. No. 03CH37453). IEEE, 2003, 3: 2743-2748.

[24] Magnusson M, Nuchter A, Lorken C, et al. Evaluation of 3D registration reliability and speed-A comparison of ICP and NDT[C]//2009 IEEE International Conference on Robotics and Automation. IEEE, 2009: 3907-3912.

[25] Magnusson M, Lilienthal A J, Duckett T, et al. Scan Registration for Autonomous Mining Vehicles Using 3D-NDT[J]. Journal of Field Robotics, 2007, 24(10):803-827.

[26] Magnusson M, The three-dimensional normal-distributions transform: an efficient representation forregistration, surface analysis, and loop detection[D]. Orebro University, 2009.

[27] Stoyanov T, Magnusson M, Andreasson H, et al. Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations[J]. The International Journal of Robotics Research, 2012, 31(12):1377-1393.

[28] Jensfelt P, Kristensen S. Active global localization for a mobile robot using multiple hypothesis tracking[J]. IEEE Transactions on Robotics and Automation, 2001, 17(5): 748-760.

[29] Nakamura T, Tashita Y. Congruence Transformation Invariant Feature Descriptor for Robust 2D Scan Matching[C]. IEEE International Conference on Systems, Man, and Cybernetics, 2014:1648-1653.

[30] Nakamura T, Wakita S. Robust global scan matching method using congruence transformation invariant feature descriptors and a geometric constraint between keypoints[J]. Transactions of the Society of Instrument & Control Engineers, 2015, 51(5):309-318.

[31] Taleghani S, Sharbafi M A, Haghighat A T, et al. ICE Matching, a Robust Mobile Robot Localization with Application to SLAM[C]. IEEE International Conference on TOOLS with Artificial Intelligence, 2010:186-192.

[32] Tipaldi G D, Braun M, Arras K O. FLIRT: Interest regions for 2D range data with applications to robot navigation[C]. Experimental Robotics. Springer Berlin Heidelberg, 2014:695-710.

[33] Tombari F, Salti S, Stefano L D, et al. Performance Evaluation of 3D Keypoint Detectors[J]. International Journal of Computer Vision, 2013, 102(1): 198-220.

[34] Guo Y, Bennamoun M, Sohel F, et al. A Comprehensive Performance Evaluation of 3D Local Feature Descriptors[J]. International Journal of Computer Vision, 2016, 116(1):66-89.

[35] Liu S, Atia M M, Gao Y, et al. Adaptive Covariance Estimation Method for LiDAR-Aided Multi-Sensor Integrated Navigation Systems[J]. Micromachines, 2015, 6(2): 196-215.

[36] Siadat A, Kaske A, Klausmann S, et al. An Optimized Segmentation Method for a 2D Laser-Scanner Applied to Mobile Robot Navigation[J]. IFAC Proceedings Volumes, 1997, 30(7): 149-154.

[37] Censi A, Iocchi L, Grisetti G, et al. Scan Matching in the Hough Domain[C]. IEEE InternationalConference on Robotics and Automation, 2006:2739-2744.

[38] Yuan X, Zhao C, Tang Z, et al. Lidar Scan-Matching for Mobile Robot Localization[J]. Information Technology Journal, 2010, 9(1): 27-33.

[39] Li J, Zhong R, Hu Q, et al. Feature-Based Laser Scan Matching and Its Application for Indoor Mapping[J]. Sensors, 2016, 16(8):1265.

[40] Tomono M. A scan matching method using euclidean invariant signature for global localization and map building[C]//IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA'04. 2004. IEEE, 2004, 1: 866-871.

[41] Nobili S, Scona R, Caravagna M, et al. Overlap-based ICP tuning for robust localization of a humanoid robot[C]//2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017: 4721-4728.

[42] Zhang J, Singh S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots, 2017, 41(2): 401-416.

[43] Grant W S, Voorhies R C, Itti L. Finding planes in LiDAR point clouds for real-time registration[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013: 4347-4354.

[44] Lynen S, Achtelik M W, Weiss S, et al. A robust and modular multi-sensor fusion approach applied to mav navigation[C]//2013 IEEE/RSJ international conference on intelligent robots and systems. IEEE, 2013: 3923-3929.

[45] Yang S, Zhu X, Nian X, et al. A robust pose graph approach for city scale LiDAR mapping[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 1175-1182.

[46] Demir M, Fujimura K. Robust localization with low-mounted multiple lidars in urban environments[C]//2019 IEEE Intelligent Transportation Systems Conference (ITSC). IEEE, 2019: 3288-3293.

[47] Gao Y, Liu S, Atia M M, et al. INS/GPS/LiDAR integrated navigation system for urban and indoor environments using hybrid scan matching algorithm[J]. Sensors, 2015, 15(9): 23286-23302.

[48] Hening S, Ippolito C A, Krishnakumar K S, et al. 3D LiDAR SLAM integration with GPS/INS for UAVs in urban GPS-degraded environments[M]//AIAA Information Systems-AIAA Infotech@ Aerospace. 2017: 0448.

[49] Zhang J, Singh S. LOAM: Lidar Odometry and Mapping in Real-time[C]//Robotics: Science and Systems. 2014, 2(9): 1-9.

[50] Shan T, Englot B. Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4758-4765.

[51] Behley J, Stachniss C. Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments[C]//Robotics: Science and Systems. 2018, 2018: 59.

[52] Sun K, Mohta K, Pfrommer B, et al. Robust stereo visual inertial odometry for fast autonomous flight[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 965-972.

[53] Qin T, Li P, Shen S. Vins-mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

[54] Zhang S, Guo Y, Zhu Q, et al. Lidar-IMU and wheel odometer based autonomous vehicle localization system[C]//2019 Chinese Control And Decision Conference (CCDC). IEEE, 2019: 4950-4955.

[55] Hess W, Kohler D, Rapp H, et al. Real-time loop closure in 2D LIDAR SLAM[C]//2016 IEEE international conference on robotics and automation (ICRA). IEEE, 2016: 1271-1278.

[56] Li M, Kim B H, Mourikis A I. Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera[C]//2013 IEEE International Conference on Robotics and Automation. IEEE, 2013: 4712-4719.

[57] Huai Z, Huang G. Robocentric visual-inertial odometry[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 6319-6326.

[58] Zhang J, Singh S. Visual-lidar odometry and mapping: Low-drift, robust, and fast[C]//2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015: 2174-2181.

[59] Graeter J, Wilczynski A, Lauer M. Limo: Lidar-monocular visual odometry[C]//2018 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018: 7872-7879.

[60] Ye H, Chen Y, Liu M. Tightly coupled 3d lidar inertial odometry and mapping[C]//2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019: 3144-3150.

[61] Neuhaus F, Koß T, Kohnen R, et al. Mc2slam: Real-time inertial lidar odometry using two-scan motion compensation[C]//Pattern Recognition: 40th German Conference, GCPR 2018, Stuttgart, Germany, October 9-12, 2018, Proceedings 40. Springer International Publishing, 2019: 60-72.

[62] Shan T, Englot B, Meyers D, et al. Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020: 5135-5142.

[63] Shan T, Englot B, Ratti C, et al. Lvi-sam: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021: 5692-5698.

[64] Huang G P, Mourikis A I, Roumeliotis S I. A quadratic-complexity observability-constrained unscented Kalman filter for SLAM[J]. IEEE Transactions on Robotics, 2013, 29(5): 1226-1243.

[65] Qin, C.; Ye, H.; Pranata, C.E.; Han, J.; Zhang, S.; Liu, M. Lins: A lidar-inertial state estimator for robust and efficient navigation. In Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 31 May–31 August 2020; pp. 8899–8906.

[66] Qin C, Ye H, Pranata C, et al. R-lins: A robocentric LiDAR-inertial state estimator for robust and efficient navigation. arXiv 2019[J]. arXiv preprint arXiv:1907.02233.

[67] Xu W, Zhang F. Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 3317-3324.

[68] Xu W, Cai Y, He D, et al. Fast-lio2: Fast direct lidar-inertial odometry[J]. IEEE Transactions on Robotics, 2022, 38(4): 2053-2073.

[69] 崔宇洋. 数字化三维矿井巷道模型的研究与实现[D]. 西安科技大学, 2021.

[70] 史瑶. 基于草图输入的煤矿巷道建模方法的研究[D]. 中国矿业大学, 2020.

[71] 刘乐. 矿井巷道三维建模及可视化技术研究与应用[D]. 西安建筑科技大学, 2016.

[72] 车德福,吴立新. 数字矿山基础平台-GeoMo~(3D)的功能结构与应用[C]//. 第七届全国矿山测量学术会议论文集, 2007:147-153.

[73] 韩瑞栋. 煤矿三维可视化系统关键技术研究与实现[D]. 山东科技大学, 2007.

[74] 陈安国. 基于OpenGL的煤矿三维可视化信息系统的设计与实现[D]. 燕山大学, 2018.

[75] 王清. 基于WebGL的煤矿三维巷道可视化研究[D]. 山东科技大学, 2018.

[76] 谢嘉成,王学文,李祥,等. 虚拟现实技术在煤矿领域的研究现状及展望[J]. 煤炭科学技术, 2019,47(03): 53-59.

[77] Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//2007 6th IEEE and ACM international symposium on mixed and augmented reality. IEEE, 2007: 225-234.

[78] Mur-ArtalRaúl, D Tardósjuan. ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras[J]. IEEE Transactions on Robotics, 33(5):1255-1262.

[79] Kohlbrecher S, Von Stryk O, Meyer J, et al. A flexible and scalable SLAM system with full 3D motion estimation[C]//2011 IEEE international symposium on safety, security, and rescue robotics. IEEE, 2011: 155-160.

[80] Grisetti G , Stachniss C , Burgard W . Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters[J]. IEEE Transactions on Robotics, 2007, 23(1):34-46.

[81] González D, Pérez J, Milanés V, et al. A review of motion planning techniques for automated vehicles[J]. IEEE Transactions on intelligent transportation systems, 2015, 17(4): 1135-1145.

[82] Bast H, Delling D, Goldberg A, et al. Route planning in transportation networks[M]//Algorithm engineering. Springer, Cham, 2016: 19-80.

[83] 李云. 基于遗传算法的动态路径优化[D]. 太原理工大学, 2013.

[84] 陈尔奎, 吴梅花, 张英杰. 复杂环境下煤矿救灾机器人路径规划[J]. 煤炭技术, 2018, 37(10): 301-304.

[85] 张良,李首滨,黄曾华,等. 煤矿综采工作面无人化开采的内涵与实现[J]. 煤炭科学技术, 2014, 42(09): 26-29.

[86] 牛剑峰. 基于视频巡检的综采工作面无人化关键技术研究[J]. 煤炭科学技术, 2019, 47(10): 141-146.

[87] 郑学召, 闫兴, 崔嘉明,等. 煤矿救灾机器人灾变环境侦测技术探讨[J]. 工矿自动化, 2019, 45(10): 29-32.

[88] 朱锋, 银皓, 白海通,等. 面向星间链路高动态网络的路由规划算法[J]. 电子技术应用, 2020, 46(11):18-22.

[89] 李淑霞, 杨俊成. 一种改进的全覆盖路径规划算法[J]. 计算机与现代化, 2021(02):100-103.

[90] Dijkstra E W. A note on two problems in connexion with graphs[J]. Numerische mathematik, 1959, 1(1): 269-271.

[91] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.

[92] Korf R E, Reid M, Edelkamp S. Time complexity of iterative-deepening-A∗[J]. Artificial Intelligence, 2001, 129(1-2): 199-218.

[93] Koenig S, Likhachev M, Furcy D. Lifelong planning A∗[J]. Artificial Intelligence, 2004, 155(1-2): 93-146.

[94] Nannicini G, Delling D, Liberti L, et al. Bidirectional A∗ search for time-dependent fast paths[C]//International Workshop on Experimental and Efficient Algorithms. Springer, Berlin, Heidelberg, 2008: 334-346.

[95] Springer, Boston, MA, 1997: 203-220.Stentz A. Optimal and efficient path planning for partially-known environments[C]//Proceedings of the 1994 IEEE international conference on robotics and automation. IEEE, 1994: 3310-3317.

[96] 王帅军, 胡立坤, 王一飞. 基于改进D~*算法的室内移动机器人路径规划[J]. 计算机工程与设计, 2020, 41(04): 1118-1124.

[97] 张希闻, 肖本贤. 改进D*算法的移动机器人路径规划[J]. 传感器与微系统, 2018, 037(012):52-54,58.

[98] 张飞, 白伟, 乔耀华,等. 基于改进D*算法的无人机室内路径规划[J]. 智能系统学报, 2019, 14(4): 662-669.

[99] 史久根, 李凯业. 基于分层改进D*算法的室内路径规划[J]. 计算机应用研究, 2015, 32(12): 3609-3612.

[100] 朱蟋蟋, 孙兵, 朱大奇. 基于改进D*算法的AUV三维动态路径规划[J]. 控制工程, 2021, 28(04): 736-743.

[101] Nash A, Daniel K, Koenig S, et al. Theta*: Any-angle path planning on grids[C]//AAAI. 2007, 7: 1177-1183.

[102] Nash A, Koenig S, Tovey C. Lazy Theta*: Any-angle path planning and path length analysis in 3D[C]//Proceedings of the AAAI Conference on Artificial Intelligence. 2010, 24(1): 147-154.

[103] Montemerlo M, Becker J, Bhat S, et al. Junior: The stanford entry in the urban challenge[J]. Journal of field Robotics, 2008, 25(9): 569-597.

[104] Dolgov D, Thrun S, Montemerlo M, et al. Practical search techniques in path planning for autonomous driving[J]. Ann Arbor, 2008, 1001(48105): 18-80.

[105] Dolgov D, Thrun S, Montemerlo M, et al. Path planning for autonomous vehicles in unknown semi-structured environments[J]. The international journal of robotics research, 2010, 29(5): 485-501.

[106] Shamsudin A U, Ohno K, Hamada R, et al. Two-stage hybrid A* path-planning in large petrochemical complexes[C]//2017 IEEE International Conference on Advanced Intelligent Mechatronics (AIM). IEEE, 2017: 1619-1626.

[107] Chen C, Rickert M, Knoll A. Kinodynamic motion planning with space-time exploration guided heuristic search for car-like robots in dynamic environments[C]//2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2015: 2666-2671.

[108] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning[J]. The international journal of robotics research, 2011, 30(7): 846-894.

[109] Karaman S, Frazzoli E. Incremental sampling-based algorithms for optimal motion planning[J]. Robotics Science and Systems VI, 2010, 104(2)：654-664.

[110] Adiyatov O, Varol H A. Rapidly-exploring random tree based memory efficient motion planning[C]//2013 IEEE international conference on mechatronics and automation. IEEE, 2013: 354-359.

[111] Islam F, Nasir J, Malik U, et al. Rrt∗-smart: Rapid convergence implementation of rrt∗ towards optimal solution[C]//2012 IEEE international conference on mechatronics and automation. IEEE, 2012: 1651-1656.

[112] Véras L G D O, Medeiros F L L, Guimaráes L N F. Systematic literature review of sampling process in rapidly-exploring random trees[J]. IEEE Access, 2019, 7: 50933-50953.

[113] Brunner M, Brüggemann B, Schulz D. Hierarchical rough terrain motion planning using an optimal sampling-based method[C]//2013 IEEE International Conference on Robotics and Automation. IEEE, 2013: 5539-5544.

[114] Gammell J D, Srinivasa S S, Barfoot T D. Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2014: 2997-3004.

[115] Naderi K, Rajamäki J, Hämäläinen P. RT-RRT* a real-time path planning algorithm based on RRT[C]//Proceedings of the 8th ACM SIGGRAPH Conference on Motion in Games. 2015: 113-118.

[116] Kavraki L E, Svestka P, Latombe J C, et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces[J]. IEEE transactions on Robotics and Automation, 1996,12(4): 566-580.

[117] Bohlin R, Kavraki L E. Path planning using lazy PRM[C]//Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065). IEEE, 2000, 1: 521-528.

[118] Kelly A, Nagy B. Reactive nonholonomic trajectory generation via parametric optimal control[J]. The International Journal of Robotics Research, 2003, 22(7-8): 583-601.

[119] 冀杰,唐志荣,吴明阳,等. 面向车道变换的路径规划及模型预测轨迹跟踪[J]. 中国公路学报, 2018,31(04): 172-179.

[120] 肖宗鑫,李晓杰,肖宗烁,等. 基于RBF神经网络优化的无人驾驶车辆增量线性模型预测轨迹跟踪控制研究[J]. 重庆理工大学学报(自然科学), 2021,35(03): 36-45.

[121] 梁晓辉, 慕永辉, 吴北华,等. 关于路径规划的相关算法综述[J]. 价值工程, 2020,39(03): 295-299.

[122] Foody G M, McCulloch M B, Yates W B. The effect of training set size and composition on artificial neural network classification[J]. International Journal of remote sensing, 1995,16(9): 1707-1723.

[123] 耿晓龙, 李长江. 基于人工神经网络的并行强化学习自适应路径规划[J]. 科学技术与工程, 2011,11(04): 756-759.

[124] Dorigo M, Stützle T. Ant colony optimization: overview and recent advances[M]. Springer International Publishing, 2019.

[125] 马国兵. 灾后矿井环境探测机器人的路径规划研究[D]. 中国矿业大学, 2019.

[126] 赵天亮,张小俊,张明路,等. 基于改进融合蚁群算法的机器人路径规划方法研究[J]. 制造业自动化, 2022,44(05): 185-190.

[127] 黄辰,费继友,刘洋,等. 基于动态反馈A*蚁群算法的平滑路径规划方法[J]. 农业机械学报, 2017,48(04): 34-40+102.

[128] Holland J H. Adaptation in natural and artificial systems: an introductory analysis with applications to biology, control, and artificial intelligence[M]. MIT press, 1992.

[129] 李俊,舒志兵. 基于改进D* Lite遗传算法路径规划研究[J]. 机床与液压, 2019,47(11): 39-42.

[130] 魏彤,龙琛. 基于改进遗传算法的移动机器人路径规划[J]. 北京航空航天大学学报, 2020,46(04): 703-711.

[131] Du K L, Swamy M N S, Du K L, et al. Particle swarm optimization[J]. Search and Optimization by Metaheuristics: Techniques and Algorithms Inspired by Nature, 2016: 153-173.

[132] 敖永才,师奕兵,张伟,等. 自适应惯性权重的改进粒子群算法[J]. 电子科技大学学报, 2014,43(06): 874-880.

[133] Gong D, Zhang J, Zhang Y. Multi-objective Particle Swarm Optimization for Robot Path Planning in Environment with Danger Sources[J]. J. Comput., 2011, 6(8): 1554-1561.

[134] Shi Y, Eberhart R C. Parameter selection in particle swarm optimization[C]//Evolutionary Programming VII: 7th International Conference, EP98 San Diego, California, USA, March 25–27, 1998 Proceedings 7. Springer Berlin Heidelberg,1998: 591-600.

[135] 陈嘉林,魏国亮,田昕. 改进粒子群算法的移动机器人平滑路径规划[J]. 小型微型计算机系统, 2019,40(12): 2550-2555.

[136] 贾会群,魏仲慧,何昕,等. 基于改进粒子群算法的路径规划[J]. 农业机械学报, 2018,49(12): 371-377.

[137] Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[M]//Autonomous robot vehicles. Springer, New York, NY, 1986: 396-404.

[138] 李奕铭. 基于人工势场法的移动机器人避障研究[D]. 合肥工业大学, 2013.

[139] 罗乾又,张华,王姮,等. 改进人工势场法在机器人路径规划中的应用[J]. 计算机工程与设计, 2011,32(4): 1411-1413.

[140] 张建英,赵志萍,刘暾. 基于人工势场法的机器人路径规划[J]. 哈尔滨工业大学学报, 2006,38(8): 1306-1309.

[141] 于振中,闫继宏,赵杰,等. 改进人工势场法的移动机器人路径规划[D]. 北京交通大学, 2011.

[142] 郭枭鹏. 基于改进人工势场法的路径规划算法研究[D]. 哈尔滨工业大学, 2017.

[143] 丁家如，杜昌平，赵耀，等. 基于改进人工势场法的无人机路径规划算法[J]. 计算机应用, 2016, 36(01): 287-290.

[144] Alonso-Mora J, Breitenmoser A, Rufli M, et al. Optimal reciprocal collision avoidance for multiple non-holonomic robots[M]//Distributed autonomous robotic systems. Springer, Berlin, Heidelberg, 2013: 203-216.

[145] Seder M, Petrovic I. Dynamic window based approach to mobile robot motion control in the presence of moving obstacles[C]//Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007: 1986-1991.

[146] Fox D, Burgard W, Thrun S. The dynamic window approach to collision avoidance[J]. IEEE Robotics & Automation Magazine, 1997, 4(1): 23-33.

[147] 王永雄,田永永,李璇,等. 穿越稠密障碍物的自适应动态窗口法[J]. 控制与决策, 2019 (5): 927-936.

[148] 何壮壮,丁德锐. 基于D*和DWA的改进机器人导航方法[J]. 电子测量技术, 2019, 42(12):122-128.

[149] Quigley M, Conley K, Gerkey B, et al. ROS: an open-source Robot Operating System[C]//ICRA workshop on open source software. 2009, 3(3.2): 5.

[150] 程传奇,郝向阳,李建胜,等. 融合改进A*算法和动态窗口法的全局动态路径规划[J]. 西安交通大学学报, 2017,51(11): 137-143.

[151] Liu W, Li Z, Malekian R, et al. A novel multifeature based on-site calibration method for LiDAR-IMU system[J]. IEEE Transactions on Industrial Electronics, 2019, 67(11): 9851-9861.

[152] Le Gentil C, Vidal-Calleja T, Huang S. 3d lidar-imu calibration based on upsampled preintegrated measurements for motion distortion correction[C]//2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018: 2149-2155.

[153] Horaud R, Dornaika F. Hand-eye calibration[J]. The international journal of robotics research, 1995, 14(3): 195-210.

[154] Grisetti G, Kümmerle R, Stachniss C, et al. A tutorial on graph-based SLAM[J]. IEEE Intelligent Transportation Systems Magazine, 2010, 2(4): 31-43.

[155] Kümmerle R, Grisetti G, Strasdat H, et al. g2o: A general framework for graph optimization[C]//2011 IEEE International Conference on Robotics and Automation. IEEE, 2011: 3607-3613.

[156] Forster C, Carlone L, Dellaert F, et al. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation[C]. Georgia Institute of Technology, 2015.

[157] 周红进,钟云海,易成涛. MEMS惯性导航传感器[J]. 舰船科学技术, 2014,36(01): 115-121.

[158] Miller S, Childers D. Probability and random processes: With applications to signal processing and communications[M]. Academic Press, 2012.

[159] Stuelpnagel J. On the parametrization of the three-dimensional rotation group[J]. SIAM review, 1964, 6(4): 422-430.

[160] Jones E M, Fjeld P. Gimbal angles, gimbal lock, and a fourth gimbal for Christmas[J]. Retrieved September, 2011, 24: 2017.

[161] Schnabel R, Wahl R, Klein R. Efficient RANSAC for point-cloud shape detection[C]//Computer graphics forum. Oxford, UK: Blackwell Publishing Ltd, 2007, 26(2): 214-226.

[162] FISCHLER M A, BOLLES R C. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981, 24(6): 381-395.

[163] ZHANG J, SINGH S.“Low-drift and Real-time Lidar Odometry and. Mapping,” Autonomous Robots, vol. 41(2): 401-416, 2017.

[164] Wu B, Ma J, Chen G, et al. Feature interactive representation for point cloud registration[C]//Proceedings of the IEEE/CVF International Conference on Computer Vision. 2021: 5530-5539.

[165] Low K L. Linear least-squares optimization for point-to-plane icp surface registration[J]. Chapel Hill, University of North Carolina, 2004, 4(10): 1-3.

[166] Madyastha V, Ravindra V, Mallikarjunan S, et al. Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation[C]//AIAA Guidance, Navigation, and Control Conference. 2011: 6615.

[167] Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.

[168] Shin E H. Estimation techniques for low-cost inertial navigation[J]. UCGE report, 2005, 20219.

[169] Lupton T, Sukkarieh S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics, 2011, 28(1): 61-76.

[170] Pomerleau F, Colas F, Siegwart R. A review of point cloud registration algorithms for mobile robotics[J]. Foundations and Trends in Robotics, 2015, 4(1): 1-104.

[171] Qin T, Shen S. Online temporal calibration for monocular visual-inertial systems[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 3662-3669.

[172] Steder B, Ruhnke M, Grzonka S, et al. Place recognition in 3D scans using a combination of bag of words and point feature based relative pose estimation[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2011: 1249-1255.

[173] Kim G, Kim A. Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4802-4809.

[174] Absil P A, Mahony R, Sepulchre R. Optimization algorithms on matrix manifolds[M]//Optimization Algorithms on Matrix Manifolds. Princeton University Press, 2009.

[175] Kim G, Kim A. Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4802-4809.

[176] Sibley G, Matthies L, Sukhatme G. Sliding window filter with application to planetary landing[J]. Journal of Field Robotics, 2010, 27(5): 587-608.

[177] Dong-Si T C, Mourikis A I. Motion tracking with fixed-lag smoothing: Algorithm and consistency analysis[C]//2011 IEEE International Conference on Robotics and Automation. IEEE, 2011: 5655-5662.

[178] Yin J, Li A, Li T, et al. M2dgr: A multi-sensor and multi-scenario slam dataset for ground robots[J]. IEEE Robotics and Automation Letters, 2021, 7(2): 2266-2273.

[179] Zhang L, Zapata R, Lepinay P. Self-adaptive Monte Carlo localization for mobile robots using range finders[J]. Robotica, 2012, 30(2): 229-244.

[180] Hornung A, Wurm K M, Bennewitz M, et al. OctoMap: An efficient probabilistic 3D mapping framework based on octrees[J]. Autonomous robots, 2013, 34: 189-206.

[181] Lozano-Perez T. Spatial planning: A configuration space approach[M]//Autonomous robot vehicles. Springer, New York, NY, 1990: 259-271.

[182] Kingston Z, Kavraki L E. Robowflex: Robot motion planning with MoveIt made easy[C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022: 3108-3114.

[183] Meng T, Yang T, Huang J, et al. Improved Hybrid A-Star Algorithm for Path Planning in Autonomous Parking System Based on Multi-Stage Dynamic Optimization[J]. International Journal of Automotive Technology, 2023, 24(2): 459-468.

[184] Sheng W, Li B, Zhong X. Autonomous Parking Trajectory Planning With Tiny Passages: A Combination of Multistage Hybrid A-Star Algorithm and Numerical Optimal Control[J]. IEEE Access, 2021, 9: 102801-102810.

[185] Reeds J, Shepp L. Optimal paths for a car that goes both forwards and backwards[J]. Pacific journal of mathematics, 1990, 145(2): 367-393.

[186] MELLINGER D, KUMAR V. Minimum snap trajectory generation and control for quadrotors[C]// IEEE International Conference on Robotics and Automation (ICRA),, 2011: 2520-2525.

[187] CHEN J, LIU T, SHEN S. Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments[C]// IEEE International Conference on Robotics and Automation (ICRA), 2016: 1476-1483.