即时定位与建图技术（Simultaneous Localization and Mapping, SLAM）是实现移动机器人在未知环境下自主定位的关键技术，其核心思想是利用已经建立的地图对机器人进行定位，再根据新的定位信息增量式地建立环境地图。针对 AGV小车SLAM 中存在的运动畸变、回环检测等不能完全优化地图的问题，本文主要研究内容与成果如下：

搭建AGV小车，以STM23单片机为下层控制单元，接收驱动轮的里程计信息，同时发布小车速度控制指令，将下层封装好的消息发布给上层控制系统；上层控制系统则采用工控机作为控制单元，接收激光雷达和惯性测量单元的封装话题，完成建图工作；上下层采用CAN总线进行通信。

在上述基础上，针对AGV运动过程中出现的运动畸变问题，优化了一种使用高频率的IMU数据来对一个采集周期内的点云数据进行运动补偿的方法。在较短时间内里程计的位移数据和IMU的角度数据噪声小，精度高。先将激光雷达获得的一帧点云数据的初始时刻和这一时刻下的IMU在时间上运用插值的方式进行对齐，然后将此点云数据的最后一帧数据转换到第一帧数据的坐标系下，完成单帧扫描数据在时间和空间上的校正。利用IMU的预积分模型结合激光点云构建联合优化误差函数，迭代求解雷达位姿。在完成点云校正后得到精确地三维激光雷达数据。

针对定位与建图过程中激光雷达扫描帧仅与当前子图进行匹配，随着子地图的不断增多，误差的不断积累，导致全局地图首尾不能闭环，全局地图失真等问题，提出一种基于图优化的分段式回环检测方法。根据IMU角速度数据，当其角位移变化幅度较大，此时系统将根据动态权重分配系数，将IMU数据信任值减小，并以此时刻作为分段起点，重新进行局部子图的构建并为其添加回环约束。将下一帧雷达数据作为初始位姿再次构建局部子图，依次循环，使用全局图优化的方法对各个局部子图进行全局优化，构建完整地图。最后搭建AGV软件和硬件实验平台，对本文优化的运动畸变去除算法和基于IMU数据的分段式回环检测方法进行实验验证。

Simultaneous Localization and Mapping (SLAM) is the key technology to realize the autonomous positioning of mobile robots in unknown environments, and its core idea is to use the established map to locate the robot, and then incrementally establish an environmental map according to the new positioning information. In view of the problem that motion distortion and loop detection in the AGV trolley SLAM cannot be fully optimized, the main research content and results of this paper are as follows:

Analyze and calculate the speed motion model of the AGV chassis, and use the STM32 microcontroller as the motion controller of the layer to realize the reversible conversion of speed control commands and real time motor speed; Control of the drive motor via CAN bus; Obtain posture data of inertial measurement unit and publish topics; The industrial computer is used as the upper control unit, which is mainly responsible for the operation of the algorithm. On this basis, aiming at the motion distortion problem that occurs during AGV motion, a method of using IMU high-frequency IMU data and odometer data of IMU is optimized to compensate for the motion of point cloud data in one acquisition cycle. In a short period of time, the displacement data of the odometer and the angle data of the IMU have low noise and high accuracy. First, the initial moment when the lidar obtains a frame of point cloud data is aligned with the IMU at this moment, and then the last frame of data of this point cloud data is converted to the coordinate system of the first frame of data, and the time and space correction of the single frame scan data is completed. The pre-integration model of IMU is combined with the laser point cloud to construct a joint optimization error function, and the radar pose is solved iteratively. Accurate 3D lidar data is obtained after point cloud correction is completed.

 Aiming at the process of simultaneous positioning and mapping of traditional lidar, the laser radar scanning frame only matches the current sub-map, and with the continuous increase of sub-maps, the error continues to accumulate, resulting in the global map can not be closed loop, the global map distortion and other issues, a segmented loop detection method based on map optimization is proposed. According to the IMU angular velocity data, when the IMU angular displacement changes greatly, the system will reduce the trust value of the IMU data at this time according to the dynamic weight distribution coefficient, and use this moment as the starting point of the segment, reconstruct the local subgraph, and add loopback constraints to it. The next frame of radar data is used as the initial pose to construct the local submap again, cycle through it in turn, and finally optimize each local submap to build a complete map by means of global map optimization. Finally, an AGV software and hardware experimental platform is built to experimentally verify the motion distortion removal algorithm and the segmented loopback detection method based on IMU data proposed in this paper.

[1]Tuan Le-Anh, M.B.M. De Koster. A Review Of Design And Control Of Automated Guided Vehicle Systems[J]. European Journal of Operational Research, 2006, 171(01):1-23.

[2]武启平,金亚萍,任平,查振元.自动导引车(AGV)关键技术现状及其发展趋势[J].制造业自动化,2013,35(10):106-109+121.

[3]Srivastava S C, Choudhary A K, Kumar S, et al. Development of an intelligent agent-based AGV controller for a flexible manufacturing system[J]. The International Journal of Advanced Manufacturing Technology, 2008, 36(07): 780-797.

[4]秦汉.机器人技术的发展与应用综述[J].赤峰学院学报(自然科学版),2018,34(02):38-40.

[5]徐国保,尹怡欣,周美娟.智能移动机器人技术现状及展望[J].机器人技术与应用,2007(02):29-34.

[6]陈绍磊. 磁导航AGV系统关键技术研究[D]. 广州：华南理工大学, 2015.

[7]王雨. 基于视觉导航AGV的定位技术与路径规划研究[D].合肥：安徽大学,2019.

[8]徐京邦. 基于激光反射板的激光导航AGV定位与地图构建方法研究[D].北京：机械科学研究总院,2020.

[9]刘媛媛. 激光自然导航AGV地图构建与定位研究[D].北京：机械科学研究总院,2020.

[10]曾奇. 激光测距与视觉融合的室内导航系统研究[D].成都:电子科技大学,2018.

[11]徐清. 自动导引小车系统的设计与实现[D].苏州：苏州大学,2006.

[12]Chung T, Kim J, Yi K. Human-in-the-loop evaluation of a vehicle stability controller using a vehicle simulator[J]. International Journal of Automotive Technology, 2004, 5(02): 109-114.

[13]刘嘉伟. 一种面向激光与视觉导航的AGV控制系统设计与实现[D].厦门：厦门大学,2019.

[14]秦闯. 基于DSP的AGV控制系统的设计及仿真研究[D].哈尔滨：哈尔滨工业大学,2014.

[15]吴雄喜.AGV自主导引机器人应用现状及发展趋势[J].机器人技术与应用,2012，(03):16-17.

[16]杨文华.AGV技术发展综述[J].物流技术与应用, 2015, 20(11):93-95.

[17]刘婷. 基于混合导航方式的AGV导航定位技术研究[D].成都：西华大学,2018.

[18]唐文伟.AGV在物流领域中的应用前景分析[J].物流技术,2001(03):7-8.

[19]张辰贝西,黄志球.自动导航车(AGV)发展综述[J].中国制造业信息化,2010,39(01):53-59.

[20]王天然,曲道奎.研究开发机器人系列产品促进制造业技术进步[J].中国科学院院刊,1999(06):453-455.

[21]王荣本,储江伟,冯炎,游峰,纪寿文.一种视觉导航的实用型AGV设计[J].机械工程学报,2002(11):135-138.

[22]郑少华. 视觉导航AGV定位与路径规划技术研究[D].广州市：华南理工大学,2016.

[23]工业和信息化部关于推进工业机器人产业发展的指导意见[J].硅谷,2014,7(01):15.

[24]宋杰.全球最大单体全自动化码头 上海洋山港四期开港[J].中国经济周刊,2017(49):58-60.

[25]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.

[26]Wu D , Meng Y , Zhan K , et al. A LIDAR SLAM Based on Point-Line Features for Underground Mining Vehicle[C]// 2018 Chinese Automation Congress (CAC). 2018.

[27]Engel J , Schps T , Cremers D . LSD-SLAM: Large-scale direct monocular SLAM[C]// European Conference on Computer Vision. Springer, Cham, 2014.

[28]Wang X H , Li P F . Improved Data Association Method in Binocular Vision-SLAM[C]// International Conference on Intelligent Computation Technology & Automation. IEEE, 2010.

[29]Xu Y , Ou Y , Xu T . SLAM of Robot based on the Fusion of Vision and LIDAR[C]// 2018 IEEE International Conference on Cyborg and Bionic Systems (CBS). IEEE, 2018.

[30]Shin Y S , Park Y S , Kim A . Direct Visual SLAM Using Sparse Depth for Camera-LiDAR System[C]// 2018:1-8.

[31]张国荣,王志亮,赵振栋,王明涛,张心印.移动机器人同时定位与建图研究综述[J].软件导刊,2019,18(10):5-7+22.

[32]刘铮铮. 基于层次分析法的商业银行信用评级模型研究[D].西安：西北工业大学,2006.

[33]Kaess M, Johannsson H, Roberts R, et al. iSAM2: Incremental smoothing and mapping using the Bayes tree[J]. The International Journal of Robotics Research, 2012, 31(02): 216-235.

[34]Dellaert F, Kaess M . Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing[J]. The International Journal of Robotics Research, 2006, 25(12):1181-1203.

[35]刘艳, 程诚, 裴少婧. 鲁棒自适应无迹卡尔曼滤波的 SLAM 算法[J]. 电光与控制, 2019, 26(08): 12-16.

[36]石鹏, 赖际舟, 吕品, 等. 复杂环境下微小飞行器惯性/激光雷达Robust-SLAM方法[J]. 导航定位与授时, 2019, 6(01):18-25.

[37]李秀智, 贾松敏. 基于多源融合式SLAM的机器人三维环境建模[J]. 北京理工大学学报, 2015(03):46-51.

[38]周美锋. 基于Mecanum轮的全方位移动机器人研究[D].南京：南京航空航天大学,2014.

[39]王冠. 全方位移动平台运动控制技术研究[D].北京 ：北京理工大学,2015.魏杨. 基于激光雷达的即时定位与制图技术[D].哈尔滨：哈尔滨工业大学,2017.

[40]Muir P F, Neuman C P. Kinematic modeling for feedback control of an omnidirectional wheeled mobile robot[M]//Autonomous robot vehicles. Springer, New York, NY, 1990: 25-31.

[41]杨荣. AGV车载控制器设计与研究[D].西安：西安科技大学,2020.

[42]周召发,胡文,张志利,徐梓皓,陈河.一种新的捷联惯性导航系统姿态四元数方程求解方法[J].兵工学报,2018,39(03):511-518.

[43]周晓雷. 未知环境下无人机自主导航技术研究[D].西安：西安电子科技大学,2019.

[44]丛楚滢. 未知环境下无人机自主导航的SLAM方法研究[D].南京：南京航空航天大学,2016.

[45]雷亚莉. 基于概率方法的机器人定位[D].吉林：东北电力大学,2007.

[46]危双丰,庞帆,刘振彬,师现杰.基于激光雷达的同时定位与地图构建方法综述[J].计算机应用研究,2020,37(02):327-332.

[47]Grisetti G, Stachniss C, Burgard W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE transactions on Robotics, 2007, 23(01): 34-46.

[48]Blanco J L , Gonzalez J , Fernandez-Madrigal J A . Optimal Filtering for Non-parametric Observation Models: Applications to Localization and SLAM[J]. The International Journal of Robotics Research, 2010, 29(14):1726-1742.

[49]任彧,胡海荣.基于聚类和融合算法的AGV路径搜索研究[J].杭州电子科技大学学报(自然科学版),2019,39(01):39-44.

[50]Lu F , Milios E . Globally Consistent Range Scan Alignment for Environment Mapping[J]. Autonomous Robots, 1997, 4(04):333-349.

[51] Gutmann J S, Konolige K. Incremental mapping of large cyclic environments[C]// Proceedings 1999 IEEE International Symposium on Computational Intelligence in Robotics and Automation. CIRA'99 (Cat. No. 99EX375). IEEE, 1999: 318-325.

[52]Konolige K , Grisetti G , R Kümmerle, et al. Efficient Sparse Pose Adjustment for 2D mapping[C]// 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, October 18-22, 2010, Taipei, Taiwan. IEEE, 2010.

[53]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.

[54]Kohlbrecher S , Stryk O V , Meyer J , et al. A flexible and scalable SLAM system with full 3D motion estimation[C]// IEEE International Symposium on Safety. IEEE, 2011.

[55]Olson E. M3RSM: Many-to-many multi-resolution scan matching[C]//2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015: 5815-5821.

[56]Martin F , Triebel R , Moreno L , et al. Two different tools for three-dimensional mapping: DE-based scan matching and feature-based loop detection[J]. Robotica, 2014, 32(01):19-41.

[57] Kummerle R , Grisetti G , Strasdat H , et al. G2o: A general framework for graph optimization[C]// IEEE International Conference on Robotics & Automation. IEEE, 2011.

[58]冯东恒. 基于Cartographer的激光SLAM系统关键技术分析与评估[D].山东:山东科技大学, 2018.

[59]Endres F, Hess J, Sturm J, et al. 3-D mapping with an RGB-D camera[J]. IEEE transactions on robotics, 2013, 30(01): 177-187.

[60]Ahnel D H . An efficient fastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[C]// Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453). IEEE, 2003.

[61]杨阔. 动态场景下的2D SLAM方法研究[D].北京：北京交通大学,2018.

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

[63] Li T , Sun S , Sattar T P . Adapting sample size in particle filters through KLD-resampling[J]. Electronics Letters, 2013, 49(12):740-741.

[64]劳彩莲,李鹏,冯宇.基于改进A*与DWA算法融合的温室机器人路径规划[J].农业机械学报,2021,52(01):14-22.

[65]孙弋,张松.基于ROS的服务机器人关键技术研究与实现[J].软件,2019,40(11):186-190 +194.

[66]Jiang Z, Gong Y, Zhai J, et al. Message passing optimization in robot operating system[J]. International Journal of Parallel Programming, 2020, 48(1): 119-136.