SLAM+路径规划：巡检机器人算法设计_research on slam and path planning method of inspe

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

作者：Xiaohui Wang，Xi Ma，Zhaowei Li

编译：东岸因为

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

入群邀请：7个专业方向交流群+1个资料需求群

原文：SLAM+路径规划：巡检机器人算法设计

00 摘要

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

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

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

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

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

01 简介

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

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

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

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

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

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

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

我们首先使用点云分割来滤除噪声。通过使用投影方法将一帧3D点云投影到2D深度图上，进行地面分割以分离非地面点。令为在时刻获取到的激光雷达的点云数据，其中 为中的一个点。这些点被投影到深度图像上，并且空间中的3D点变成了空间中的2D像素。

投影后，点到传感器的欧氏距离被获得。由于3D点云包含了大量的地面信息，因此需要对点云进行过滤以提高特征提取的效率和准确性。首先，地面点被标记，标记的地面点在后续步骤中不再进行分割。将非地面点进行聚类处理。经过此模块处理，每个点都有自己的分割标签（地面或非地面），在深度图像中的行列索引和到传感器的欧几里得距离。

（2）特征提取：根据平滑性，将投影的深度图水平地分成若干子图。对于每个子图，执行以下过程：令为深度图中同一行内连续点的集合，并计算点的平滑度。

其中和分别为点和到传感器的欧几里得距离。根据公式（1），可以计算每个点的平滑度，然后将平滑度排序。排序后，选择特征点。基于设定的阈值，不同类型的特征被分割。平滑度大于的边沿点被选为集合，平滑度小于的平面点被选为集合。

从所有子图中选出具有最大值的个边缘点和具有最小值的个平面点，形成边缘特征点集合和平面特征点集合 。然后，从集合中选取不属于地面点的最大值的个边缘点，形成集合；从集合中选取属于地面点的最小值的个平面点，形成集合。显然，且。

（3）雷达里程计：里程计模块使用雷达传感器估计相邻帧之间机器人的姿态变化。在估计过程中，使用标签匹配来缩小匹配范围并提高准确性，并使用两步LM优化方法来找到两个连续帧之间的变换关系。

第一步使用地面特征点获取，第二步将从分割点云中提取的边沿特征与之匹配，以获得变换 。最后，通过融合和，获得六自由度位姿变换矩阵 。

（4）雷达建图：在使用雷达里程计获取相邻帧之间的姿态变化后，将时间的特征集合、与周围的点云进行匹配，以进一步精细化位姿变换。然后，使用LM优化来获得最终的变换姿态，并将位姿图发送到GTSAM进行地图优化，从而更新传感器估计的位姿和当前地图。

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

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

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

3.1 运动模型

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

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

在图4中，机器人的运动由两个独立驱动的轮完成。设驱动轮半径为，并将两个驱动轮的中点定义为。两个轮之间的距离是，在是惯性笛卡尔坐标系，而在是机器人的局部坐标系。是左驱动轮的速度，是右驱动轮的速度，是机器人中心的速度。如果，则可以得到角速度 。根据机器人模型，前进速度取决于轮的速度。

角速度由左右驱动轮之间的速度差以及它们之间的距离来确定。

在理想情况下，根据刚性运动原理，机器人的轨迹是一个圆，半径可以表示为

机器人的运动学方程可以表示为

上面的公式是姿态状态矩阵和运动状态矩阵，最后的公式可以写成：

其中是3×2光滑线性无关矩阵，是机器人的运动矩阵。

3.2 路径规划

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

（1）全局路径规划

为确保巡逻机器人能够在全局和局部上有效地避开障碍物，并考虑到实际道路场景的网格地图相对简单，因此采用A-star算法作为全局路径规划方法，通过实时规划为机器人提供准确的避障方向。A-star结合了启发式搜索和广度优先算法，通过代价函数选择搜索方向并在起始点周围展开。启发式函数计算每个周围节点的代价值，并选择最小代价值作为下一个扩展点。这个过程重复进行直到到达终点，生成从起点到终点的路径。在搜索过程中，由于路径上的每个节点都是代价最小的节点，获得的路径代价也是最小的。A-star算法的代价函数是

其中，是当前位置的代价函数，是搜索空间中从起始位置到当前位置的代价值，是从当前位置到目标位置的代价值。在A-star算法中，启发式函数的选择至关重要。由于建图环境是一个带有障碍物的网格地图，曼哈顿距离被用作启发式函数，其定义为：

其中，、分别代表当前位置和目标位置的坐标。在A-star算法中的路径规划中，使用Closelist和Openlist来存储节点。已经被搜索且生成了代价值的节点被存储在Openlist中。具有最小估计代价的节点被存储在Closelist中，移动轨迹是通过处理Closelist中每个节点的轨迹来形成的。具体步骤如下：

步骤1：机器人的起始点是第一个计算出的点，将周围的节点添加到Openlist 中，并计算每个点的代价函数。

步骤2：搜索Openlist，选取代价值最小的节点作为当前处理节点，将其从 Openlist中移除并放入Closelist。

步骤3：如果从当前处理节点到起始点的相邻节点的实际代价值小于原始值，则将相邻节点的父节点设置为当前处理节点；如果大于，则从Closelist 中移除当前处理节点，并选择代价值次小的节点作为当前处理节点。

步骤4：重复以上步骤，直到目标点被添加到Closelist中。遍历每个父节点，所得到的节点坐标即为路径。

（2）局部路径规划

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

其中，代表机器人坐标系中的第个姿态，包括位置信息、和角度。相邻姿态和 之间的时间间隔用表示，如图5所示。

在优化过程中，TEB算法将机器人相邻时间间隔和状态作为节点，应用图优化算法，并使用机器人的速度、加速度和非完整约束作为边。它还考虑到障碍物信息、计划轨迹的离散间隔以及相邻时间和空间序列约束。最终，使用G2O求解器计算控制变量（其中和分别表示机器人的线性速度和角速度），以获取最佳轨迹。TEB算法通过加权多目标优化获得最佳姿态点，其中目标函数的数学描述为：

其中，是考虑各种约束的目标函数，是约束函数，是每一项的权重，而是最佳的TEB轨迹。TEB算法有四个约束函数（详见原文）。

（3）混合路径规划算法算法生成的导航路径仅包含起点、关键点和目标点，但无法避开环境中的未知障碍物。TEB算法具有良好的局部避障能力，但只有一个单一的目标点作为引导，容易陷入局部最优。因此，我们提出了一种混合路径规划算法，结合了两种算法的优势。具体的算法流程如图6所示。

全局路径规划以静态障碍物代价地图作为输入，不考虑机器人的机械性能和运动学约束在规划路径时。它使用算法从机器人当前位置到目标位置规划最优路径，并为局部路径规划提供初始值。

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

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

04 实验与分析

4.1 实验设置

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

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

4.2 性能评估

4.2.1 视觉SLAM算法性能评估

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

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

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

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

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

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

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

4.2.3 路径规划性能评估

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

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

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

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

05  结论和展望

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

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

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

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

1. 书籍下载-《ROS小白入门：机器人简易编程》

2. 多机器人协同在线构建三维场景图

3. 书籍推荐-《机器人路径规划与协作：基础、算法与实验》

4. 书籍下载-《视觉SLAM十四讲：从理论到实践（第2版）》

5. 书籍推荐-《复杂地形中，SLAM技术在移动机器人中的应用》