标题：Research on SLAM and Path Planning Method of Inspection Robot in Complex Scenarios

作者：Xiaohui Wang，Xi Ma，Zhaowei Li

编译：东岸因为

编辑：郑欣欣@一点人工一点智能

工厂安全检查对于保持生产环境的安全至关重要。

目前，检查主要依靠人工定期进行，这使得效率低下并且需要投入大量人力。因此，利用巡检机器人可以显著提高任务的可靠性和效率。机器人的定位技术和路径规划技术的发展确保了工厂巡检机器人能够在复杂环境中自主完成任务。针对工厂检查的应用需求，本文研究了机器人地图制作、定位和路径规划方法。

由于相机和激光传感器在不同环境中的适用性存在差异，同时也受到它们固有特性的限制，因此本文提出了基于多线激光雷达和视觉感知的SLAM应用系统，以满足不同场景的需求。

为有效解决检查任务中低效率的问题，引入了一种混合路径规划算法，结合了

算法和TEB算法（time elastic band method）。这个算法有效地解决了在复杂环境中路径规划陷入局部最优解的问题，从而提高了机器人的检查效率。

实验结果证明，设计的SLAM和路径规划方法可以满足复杂场景中机器人的检查要求，并表现出良好的可靠性和稳定性。

随着“工业4.0”的引入，机器人技术迅速发展。在这些技术中，巡检机器人由于其优异的灵活性、移动性和功能性，在航空航天、制造业、农业、服务行业及其他领域得到广泛应用。

随着检查应用场景的日益多样化和复杂化，对机器人自主导航性能的要求也越来越高。机器人导航技术主要包括SLAM技术和路径规划技术。通过携带的传感器在周围环境中确定自己的位置并创建地图，SLAM技术是移动机器人定位和地图构建的过程。而路径规划技术则根据不同任务目标与要求为机器人制定最优导航路径。

基于不同的传感器，SLAM技术可以分为两类：基于视觉的和基于激光雷达的。

Davison等人提出的MonoSLAM方法通过逐帧提取稀疏特征点来估计相机姿态，这是第一个使用单目相机的实时视觉SLAM系统。随后，Klein等人提出了PTAM方法，该方法引入了非线性优化和关键帧机制，解决了MonoSLAM中计算复杂度高的问题。Newcombe等人提出了一种基于RGB-D相机的逐像素密集方法，可以实现实时跟踪和重建。

对于基于激光雷达的SLAM方法，Grisetti等人提出了一种基于粒子滤波的2D SLAM算法，通过减少粒子数量解决了重采样引起的粒子耗散问题。Konolige等人通过使用高度优化和非迭代的平方根因子分解来稀疏和解耦系统，提出了第一个基于图优化的开源算法。Kohlbrecher等人设计了Hector SLAM算法，该算法将当前帧的激光雷达数据与因子图相匹配，并使用高斯-牛顿方法优化姿态，以获得最优解和偏差。

传感器融合也是至关重要的。

Xia等人提出了一种基于一致性和车辆运动学/动力学综合的自动驾驶车辆侧偏角估计算法，该算法利用包含GNSS、IMU和车载传感器的多传感器融合框架解决了航向误差的可观测性问题。这种方法提高了在各种自动驾驶条件下侧偏角估计的可靠性和准确性。

Xia等人通过分段常数系统（PWCS）和奇异值分解（SVD）理论分析偏航偏差的可观察性，提出了一种IMU和车载传感器自主估计偏航偏差的方法，解决了直接测量偏航偏差的困难。

刘等人提出了一种新的YOLOv5-tassel算法，该算法引入了SimAM的注意力机制。这提高了基于无人机的RGB图像中tassel检测的准确性。

Xia等人提出了一种基于连接自动车辆（CAV）协同感知的自动驾驶系统（ADS）数据采集和处理平台，用于车辆轨迹提取、重建和评估，解决了物体检测和跟踪中的噪声、异常值和ID切换等挑战。

路径规划的目的是在已知环境中从起点到终点规划出一条最优的无碰撞路径。根据是否提前了解环境信息，路径规划可以分为全局和局部路径规划。

Dijkstra等提出了最短路径规划算法，采用广度优先搜索来搜索路径。Hart等提出了A*算法，通过使用启发式评估函数减少搜索节点数，提高了路径搜索的效率。Fox等提出了动态窗口法（DWA），根据机器人的运动学模型和当前运动参数动态采样机器人取样空间内的速度，并选择最佳轨迹。

为了解决DWA评估函数不足的问题，Chang等提出了基于Q-learning改进的DWA算法，修改和扩展了评估函数并增加了两个评估函数，以提高导航性能，在复杂未知环境中实现更高的导航效率和成功率。Rösmann等提出了基于多目标优化的TEB算法（time elastic band method），确保机器人在满足其运动约束的前提下输出平滑的轨迹。

LiDAR和相机在工厂检查中都是必不可少的，但它们的适用性取决于具体的应用场景和工厂环境。

LiDAR适用于长距离和高精度测量，如大型仓库的库存管理和机器人操作中的位置控制。然而，LiDAR在处理细节和颜色方面存在局限性，因此不适合对视觉要求较高的场景。

相机具有图像识别、检测和跟踪等功能，非常适合需要高精度视觉检测和识别的环境，例如检测产品尺寸、形状和颜色以及在生产线上进行自动化视觉检查的装配过程。此外，相机在细节和颜色方面具有强大的处理能力，提供更详细的图像信息。然而，相机高度依赖环境光线，这可能会限制其在光线不足或存在障碍物时的有效性。

本文介绍了一种专为复杂现实场景设计的巡检机器人导航系统，如图1所示。

我们设计了基于多线激光雷达和视觉传感器的两组SLAM系统，以满足不同环境下机器人的检测需求。

对于视觉SLAM系统，首先收集RGB-D图像作为外部输入，然后提取图像的特征，进行回环检测，使用全局建图优化来获得点云地图。

对于激光SLAM系统，首先收集环境的激光雷达扫描数据，然后从扫描数据中提取角点和边缘等特征。最后，使用这些特征构建环境的3D点云地图。

在获取视觉或激光SLAM系统的点云地图之后，采用A*算法进行全局路径规划，以提高导航效率并帮助机器人快速规划最佳路径。然后，引入TEB算法根据环境和障碍物的变化进行实时路径规划。最终，在复杂场景下实现了巡检机器人的自主导航和避障功能。该研究的主要贡献如下：

· 我们设计了一个基于多线激光雷达和视觉传感器的SLAM应用系统，可应用于不同场景。

· 我们提出了一种混合路径规划算法，将A算法和TEB算法相结合。它有效地解决了复杂环境下路径规划中局部最优问题，提高了机器人检测效率。

· 这两个SLAM应用系统共享一套混合路径规划算法，实现高精度导航检测任务。

本文的整体结构如下。第2节介绍了基于视觉传感器和多线激光雷达传感器的SLAM系统。第3节介绍了将A算法作为全局路径规划算法和TEB算法作为局部路径规划算法相结合的混合路径规划算法。第4节描述了在ROS平台上对视觉SLAM、激光雷达SLAM和混合路径规划算法的性能评估。最后，第5节总结了本工作并提供未来展望。

02 巡检机器人SLAM系统

2.1 视觉SLAM算法设计与实现

在工厂环境中，巡检机器人和设备的位置和运动可能会发生快速变化，因此需要实时获取和处理传感器数据以进行准确的定位和建图。为了实现这一目标，我们选择了一种具有时间和尺度不变性的基于外观的定位和建图方法，如图2所示的结构图。

图2 基于视觉的SLAM系统结构图

在图2中，可以看到RGB-D图像作为外部输入，并使用ORB算法从RGB-D图像中提取特征点。然后，采用基于词袋的图像匹配方法对相邻帧之间的特征点进行匹配。同时引入回环检测机制来消除漂移。

当机器人返回之前访问过的区域时，回环检测可以识别该区域并将新观察到的数据与先前的地图数据匹配，从而解决累积漂移问题。接着，进行图优化，机器人姿态表示为图中节点，观测到的数据表示为边。然后，最小二乘法被用来优化所有节点的位置，最小化观测和预测数据之间的误差。最终，可以生成稠密地图。

这种基于视觉的建图和定位算法利用全局和局部回环检测技术，能够识别和处理传感器数据中的误差和漂移，提高了定位和建图的稳健性和准确性，增强了巡检机器人的效率和实时性能。

2.2 基于多线激光雷达的SLAM算法设计与实现

在室内工厂环境下，光照条件可能随时间和位置而变化，这可能会导致视觉传感器对物体的误识别或定位不准确。然而，激光传感器不需要外部光源，因为它们有自己的激光束，不受光照问题的影响。因此，我们采用了一种轻量级的地面优化激光里程计和建图方法，其结构图如图3所示。

图3 基于多线激光雷达的SLAM系统结构图

首先，通过将3D激光投射到2D深度图上，根据俯仰角分割出地面，聚类非地面点云并获取标记点云数据，将激光点云数据降维。接着，基于平滑性进行特征提取，得到四组特征点云。为特征点云集合建立约束关系，使用Levenberg-Marquardt（LM）优化方法求解6自由度姿态变换矩阵。然后，使用迭代最近点算法（ICP）进行回环检测，最后基于图优化将当前点云映射到全局地图上，完成了高精度地图的建立。

（1）点云分割：由于检测环境的复杂性和其他因素，激光点云数据中可能存在噪声。

此外，采集到的激光点云数据中可能存在噪声。为了实现在地图上的高精度定位，需要对高精度地图进行预处理。我们使用基于统计的鲁棒滤波器来去除离群值，使用通过滤波器来剪辑在指定坐标范围内的点云，使用体素网格滤波器对点云进行下采样。对于需要高精度实时定位的检测任务，我们通过点云配准在构建的高精度地图上进行实时定位。

首先，参考点云（即高精度地图）被转换为多元正态分布[26]。如果变换参数能够准确匹配参考点云和当前点云，则参考帧中的变换点具有较高的概率密度。因此，可以使用优化方法计算最大化概率密度总和的变换参数。在这种情况下，两个激光点云数据集匹配效果最佳。具体算法参照论文原文。

03 巡检机器人路径规划系统

3.1 运动模型

目前，巡检机器人的底盘主要由腿式、履带式和轮式组成，在不同的环境中各有优缺点。腿式巡检机器人具有较强的地形适应性，但其结构和控制系统较为复杂。履带式巡检机器人在室外、沙地、泥泞等复杂地形中具有较高的牵引力和较强的适用性，但速度相对较低，运动噪声较大。轮式巡检机器人具有速度快、效率高、运动噪声低等特点，应用广泛。

在本文中，我们关注复杂的室内工厂环境，并采用了两轮差速轮式机器人，其运动学模型如图4所示。

3.2 路径规划

巡检机器人的路径规划主要依靠构建的网格图，通过指定起点和目标位置来生成安全无碰撞的路径。机器人路径规划可分为全局路径规划和局部路径规划。

（1）全局路径规划

（2）局部路径规划巡检机器人的工作环境并非总是固定不变的。在沿着全局路径移动的过程中，可能会出现实时障碍物。为了避免碰撞，使用具有时间弹性的局部路径规划的技术——定时弹性带（TEB）算法，在全局路径规划的基础上实现实时障碍物避开。TEB算法是一种优化算法，它遵循全局路径规划器生成的路径。它生成的局部轨迹由一系列连续的时间和姿态序列组成，机器人的姿态被定义为：

图6 混合路径规划算法流程图

全局路径规划以静态障碍物代价地图作为输入，不考虑机器人的机械性能和运动学约束在规划路径时。它使用

算法从机器人当前位置到目标位置规划最优路径，并为局部路径规划提供初始值。

局部路径规划收集全局最优路径上的路径节点，并优化机器人当前节点和收集的路径节点之间的全局路径子集。它结合了静态障碍物代价地图和动态障碍物代价地图，并使用TEB算法，在局部路径规划范围内连续调整机器人的姿态和方向，考虑其形状、动态模型和运动性能。遇到动态障碍物时，它会删除旧的机器人姿态并添加新的机器人姿态，使得每次迭代都可以生成新路径，并通过连续迭代获得优化路径。

通过融合导航算法，在移动机器人导航过程中实现最优全局路径规划和实时避障功能。

04 实验与分析

4.1 实验设置

为验证导航系统在实际应用中的有效性，我们在运行内存为4GB、64位Ubuntu18.04操作系统上的ROS平台中，使用Turtlebot3移动机器人进行了不同类型场景的实验，这些场景在Gazebo仿真平台中构建。

如图7所示，我们搭建了一个家庭环境和一个工厂仓库环境来模拟实际世界环境。使用Rviz中的实时定位与地图构建功能，我们扫描仿真环境，构建对应的地图，并执行路径规划。本文中使用的代码是开源的，可在https://github.com/Mxiii99/RSPP_CS.git上获取。

图7 仿真环境。（a）家庭场景；（b）工厂仓库场景

4.2 性能评估

4.2.1 视觉SLAM算法性能评估

本文提出了一种在ROS环境下使用深度相机构建对应点云地图的方法。

首先将深度相机数据读入ROS环境中，然后执行前端和后端线程以构建稀疏特征点地图，并连续更新以创建实时点云地图。前端的关键帧被传递到点云构建线程中以生成点云地图。通过图8中的对应点云地图验证了所提出的地图生成算法的有效性，该地图展示了室内环境中构建地图的良好三维效果。

如图8所示，该算法检测到了物体的运动轨迹，与实际轨迹一致。虽然检测到的轨迹与实际轨迹之间存在偏差，但没有严重偏差，这满足了机器人的感知要求。当物体的运动轨迹发生显著变化时，仍然没有严重偏差，也符合机器人的感知要求。

图8 基于视觉的地图生成结果。（a）家庭场景；（b）工厂仓库场景。

4.2.2 基于多线激光的SLAM算法性能评估

通过对比点云地图和视觉地图，可以观察到使用多线激光扫描构建的地图比使用视觉算法构建的地图更清晰，从而减少了累计误差，并提供了更好的边缘轮廓处理。此外，为了验证算法的可行性、可靠性和准确性，还比较了地图生成时间、地图生成效果和CPU利用率。

为了确保实验的准确性，进行了多次测试。机器人固定在一个特定的位置，标记为原点（0,0），并将输出的物体运动数据与实际的物体运动数据进行比较。结果如图9所示。

图9 基于激光的建图结果。（a）房屋场景；（b）工厂仓库场景

该算法检测到点云地图基本与模拟场景一致，并且检测到的轨迹与实际轨迹没有明显偏差，这满足了机器人的感知要求。如图所示，当物体的运动轨迹发生大的变化时，检测到的轨迹与实际轨迹之间存在轻微偏差，但没有出现严重的偏差，仍然符合机器人的感知要求。

4.2.3 路径规划性能评估

经过测试，A-star算法规划出的路径与障碍物保持了一定距离，避免了机器人与其相撞。同时，全局路径规划效果良好，可以准确到达设定的目标点位置，满足精确定位导航的要求。机器人沿着障碍物（正方形）路径前进，在遇到障碍物时，通过局部路径规划自主避让。局部路径规划的过程和结果如图10所示。

图10 局部路径规划地图。（a）家庭场景；（b）工厂仓库场景

在配置相关参数后，通过RViz中的3D视图区域观察机器人的导航环境。地图环境显示为全局代价地图，而机器人周围的环境是局部代价地图。蓝色区域是障碍物的扩展层，它在地图上向外扩展，以避免机器人与障碍物之间的碰撞。通过在RViz中添加路径插件，可以看到机器人移动的路径。绿色线条表示全局路径规划的路线，红色线条表示局部路径规划的路线。

从图10中可以看出，巡检机器人的局部路径规划路线平稳，规划路线没有进入障碍物的扩展区域，因此可以合理避开周围的障碍物，具有良好的避障效果。全局路径规划的路线显示在图11中。

图11 全局路径规划地图。（a）家庭场景；（b）工厂仓库场景

经过测试，巡检机器人可以准确实现自主避障，并完成设定目标点的局部路径规划，满足精确定位导航的要求。

05  结论和展望

利用智能巡检机器人可以提高生产效率并降低成本。然而，由于车间环境复杂，充满机械设备、管道、电缆等障碍物，因此精准巡检面临一定挑战。为解决这个问题，我们开发了一个专门针对复杂工厂现场的高精度导航巡检系统。

该系统配备两种类型的传感器：相机和激光雷达，在环境信息感知和定位和地图建立方面提供了充足的支持。通过将A∗ 算法和TEB算法（动态规划）相结合，实现了最优路径规划。

为了评估导航系统性能，在ROS系统中使用Gazebo仿真软件进行了两个场景下的仿真测试：住宅区和工厂仓库。结果表明，该导航系统具有实时定位和地图构建功能，可实现不同场景下的移动平台导航和实时避障。因此，该技术可以应用于各种复杂环境下的巡检机器人轮式导航与定位，并具有重要的参考价值。

随着巡检场景的日益复杂，单个移动机器人可能很难准确地完成导航任务。因此，未来可进一步研究多个移动机器人之间的协作导航和避障问题。