00  摘要

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

01  简介

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

02  巡检机器人SLAM系统

2.1 视觉SLAM算法设计与实现

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

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

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

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

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

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

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

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

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

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

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

3.1 运动模型

在图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算法是一种优化算法，它遵循全局路径规划器生成的路径。它生成的局部轨迹由一系列连续的时间和姿态序列组成，机器人的姿态被定义为：

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

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

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

（3）混合路径规划算法

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

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

04  实验与分析

4.1 实验设置

4.2 性能评估

4.2.1 视觉SLAM算法性能评估

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

4.2.3 路径规划性能评估

05  结论和展望

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