陈纪廷， 郭 晨， 刘 毅

(大连海事大学船舶电气工程学院， 大连 116026)

自主移动机器人被广泛应用于物流派送、工件加工等诸多场景，随着机器人技术的突飞猛进的发展，人们对机器人的各方面功能提出了更高的要求。路径规划是机器人实现自主导航功能的关键技术，它指的是移动机器人通过传感器获得所处环境的全局信息后，通过路径规划算法规划出一条最优路径，使得移动机器人安全且快速的由起始点运动到目标点[1]。

对于机器人而言，单靠传感器去感知环境很难适应各种场景，所以人们提出了全局路径规划和局部路径规划的概念，一般说来，路径规划是在当前场景已构建好地图的条件下进行的[2]。

关于路径规划已有大量的研究。Kuderer等[3]提出了一种新颖的在线规划方法，根据从给定地图计算出的Voronoi图生成初始轨迹，然后优化这些轨迹以获得对应于不同同伦类的有效替代轨迹，它有效地克服了导航代价函数的局部最小值问题。Rösmann等[4]针对传统局部规划无法通过障碍物而陷入局部最优轨迹的现象，提出了通过优化和维护不同源的候选轨迹子集的方式，通过在子集中的当前局部最优轨迹的切换，使得机器人具有较好的规避动态障碍物的能力。Rösmann等[5]提出了一种新颖的针对具有最小转向半径的移动机器人的在线规划方法，充分考虑了车型机器人的运动特性，并将运动学约束和避障定义为一个稀疏优化问题来解决。Rsmann等[6]通过构建运动模型对机器人未来状态进行预测，采用二次规划的方式优化轨迹，预测控制方案使得机器人具有对感知领域的障碍物实时做出反应的能力。

中国在路径规划方面也有相当一部分有价值的研究成果。潘虎[7]针对复杂环境下的路径规划，将改进的A*算法和改进的人工势场法相结合，提出了双层混合路径规划方法，提升了移动机器人路径规划的效果。刘子豪等[8]结合跳跃点搜索和反向搜索，用先验关键点减小计算量，并且通过二次规划提升了路径的平滑度。陈志军等[9]提出基于元胞自动机和聚焦搜索的机器人路径规划，旨在解决传统寻优算法容易陷入局部最优和不收敛的问题。吴桐[10]提出了基于栅格—八叉树混合地图的探索策略，在自主探索建立好八叉树地图的基础上用改进的动态窗口算法进行路径规划，提升了路径规划的稳定性。

路径规划又分为全局路径规划和局部路径规划，全局路径规划需要已知完整的地图信息，而局部路径规划依赖传感器的实时数据根据机器人运行时的障碍物情况分析出局部地图的信息，在线进行路径规划。在移动机器人进行路径规划时，一般将二者结合起来以获得更好的规划效果。在服务机器人和自动交通系统的背景下，需要移动机器人来安全地导航人类和其他机器人所居住的环境。在这种情况下，普遍适用的运动规划策略在移动机器人应用中至关重要。在线计划比其他方法更受青睐，因为它可以响应动态环境，地图不一致或机器人运动不确定性。此外，在线轨迹优化可以在部分冲突的目标之间进行调和，如控制点、路径偏差、总路径长度或过渡时间。大多数路径规划算法都只考虑了目标的轨迹优化，而忽略了时空运动方面的动态约束。

为此，在原有的导航框架下引入新的局部规划算法：时间弹性带算法，该算法明确考虑了动态约束，并且在轨迹实时生成的同时还有应对运动限制的能力，同时解决基于超图的非线性优化问题。在差速驱动的移动机器人上利用图优化框架对该算法进行实现。然后，针对在正常导航的情况下，每个代价地图中的障碍物被视为一系列孤立的点，如果地图的分辨率很高，这会导致大量的障碍，并且可能在计算轨迹时引入更长的计算时间或不稳定性。另一方面，障碍物的转换也需要时间。但是，转换时间很大程度上取决于所选的算法，并且可以在单独的线程中执行。可使用地图转换的方式对局部代价地图层的障碍物点进行处理，使得可行性检查步骤的距离计算的运算量大大减少。

1 时间弹性带算法设计

1.1 移动机器人导航框架

在机器人操作系统(robot operating system，ROS)中导航节点(move_base)扮演了导航框架中管理者的角色，对每个导航节点输出的信息进行管理。move_base 是 ROS里实现路径规划的载体，move_base框架中有一个全局路径规划器和一个本地实时规划器，move_base架构如图1所示。

图1 Move_base架构Fig.1 Move_base architecture

全局路径规划是一种基于地图的静态路径规划方法，利用已建立的环境地图信息和位置信息，确定自身在地图中所处位置并接收导航目标点，在开始移动前完成路径的搜寻。基于该路径在环境无动态障碍增加的条件下，应满足所搜寻路径无限接近理想的最短路径，且具有较高的搜索效率[11]。

局部规划器接收来自全局规划器的路径信息，规划出一条沿着全局路径且能避开障碍物的局部路径，并结合自身周围环境计算速度和航向角并发送给底盘控制器。

1.2 时间弹性带算法原理

时间弹性带(timed elastic band,TEB)算法是将要规划的路径视作一条连接起始和目标点的橡皮筋，尝试将外部约束作为外力让这条橡皮筋形变从而找到合适的导航路径。

设由n个机器人组成的位姿序列记为

Q={si}，i=0,1,…,n,n∈N

(1)

si=[xi,yi,βi]T∈R2×S1

(2)

式中：si为机器人的位姿；N为自然数集合；xi、yi为机器人位置；βi为全局中机器人的方向；R为实数集合；S为方向角集合。

设两个连续位姿的时间间隔记为ΔTi，则可得一个由n-1个时间间隔组成的时间序列τ：

τ={ΔTi}i=0,1,…,n-1

(3)

位姿和时间差示意图如图2所示，每个时间间隔表示机器人需要从当前位姿转换到序列Q中的下一个位姿所需要的时间。将时间序列和机器人位姿序列合并到一起，可得一个新的元组：

图2 位姿和时间差Fig.2 Pose and time difference

B∶=(Q,τ)

(4)

式(4)中：Q为位姿序列。

TEB的关键是利用加权和模型对元组B进行优化和调整。设优化的多目标函数为

(5)

(6)

式中：k为优化的目标函数的个数；γk为单个目标函数的权重；fk(B)为单个目标函数；B*为优化后的元组。

根据式(5)和式(6)，TEB被定义为一个标量化的多目标优化问题。大多数需要的目标函数依赖的参数取决于邻近位姿的子集。

简单超图如图3所示，超图显示位姿s0、s1，时间间隔ΔT1和障碍物O1速度目标函数把距离上界设置在s0～s1，使机器人在ΔT1时间内可达。障碍物与其最近位姿之间的距离fob是由一个最小的间隔距离来限定的，这样可以保证一个不发生碰撞的路径，障碍物的位置是根据机器人感知层提供的传感器数据推断出来的。对应的障碍物不受图优化的影响。

fv为机器人的速度图3 简单超图Fig.3 Simple hypergraph

更大的超图如图4所示，用机器人的速度(fv)、加速度约束(fa)描述机器人的动态行为，依照每个目标函数的权重，把每个多边中的目标函数加权表示为整体目标函数。超图中固定的节点有障碍物节点O1、给定配置的路径点p1和机器人起始状态s0。在应用程序中，循环使用规划器，TEB实时调整原始机器人的轨迹，并且通过改变原始轨迹远离障碍来避免与障碍物发生碰撞。

fp为给定的路径点约束；fob为障碍物约束；初始状态s0 由机器人定位当前位姿给出图4 超图Fig.4 Hypergraph

TEB控制流程图如图5所示，在初始阶段，添加机器人运动学约束，接受机器人定位和全局状态信息，在每一个轨迹修正步骤中，动态的添加新的位姿，移除旧的位姿，并不断裁剪全局路径，将时间和空间分辨率调整到剩余轨迹长度和规划范围之内。

图5 TEB控制流程图Fig.5 TEB control flow chart

在优化阶段，将感知到的动态障碍物和地图中的静态物体还有机器人位姿、时间间隔以及机器人自身的运动学约束放到一起，构建超图之后用图优化的方法求解导航路径。

2 移动机器人导航中的地图转换

2.1 基于噪声的密度聚类算法(DBSCAN)

引入DBSCAN聚类算法将占据栅格转换成凸多边形。DBSCAN是一种聚类算法，它基于数据推测聚类的数目，在邻域参数的基础上对空间中数据进行分割。假设有数据集D={x1,x2,…,xn}，定义如下几个概念[12]。

(1)Eps邻域：对于任意对象xi∈D，该对象半径ε内的区域称为xi的Eps邻域。

(2)核心对象; 对于任意对象xi∈D，若xi的Eps邻域内的样本个数大于等于MinPts，那么判定该对象为核心对象。MinPts表示一定数量的样本点个数，该参数可根据算法实际应用场景设定。

(3)密度直达：若xi是核心对象，且有xi的Eps邻域中包含xj，那么判定xj由该对象密度直达。

(4)密度可达：对xi、xj∈D，假设存在样本序列p1,p2,…,pn，其中p1=xi,pn=xj且pi由pi-1密度直达，那么判定xj由xi密度可达。

(5)密度相连：假设存在xk∈D，xk到xi、xj均是密度可达，那么xi、xj密度相连。

如图6所示，假设MinPts=3 虚线表示Eps邻域，x1是核心对象，x2由x1密度直达，x3由x1密度可达，x3与x4密度相连。

图6 DBSCAN聚类算法示意图Fig.6 Schematic diagram of DBSCAN clustering algorithm

基于上述概念，将簇定义为由密度可达关系导出的最大的密度相连的样本集合。给定参数(ε,MinPts)，簇C是满足以下性质的非空样本子集：①连接性，xi∈C,xj∈C⟹xi与xj密度相连；②最大性，xi∈C，xj由xi密度可达⟹xj∈C。

由以上分析可知，由核心对象密度可达的所有样本组成的集合为满足连接性与最大性的簇[13]。

算法伪代码如表1所示，DBSCAN算法先根据给定的邻域参数(ε,MinPts)遍历整个数据集，找到所有核心对象，然后取任意一个核心对象进行扩充，找出所有和该点密度相连的数据点，形成一个簇，如此反复循环直到所有对象都被归入某个簇或标记为噪声为止。

表1 DBSCAN算法伪代码Table 1 DBSCAN algorithm pseudo code

2.2 聚类处理

在对地图中栅格聚类完成之后，将生成的各聚类簇转换基本几何单元。该转换只影响局部地图，不影响全局静态地图。

把包含离散点集的最小凸集称为“凸包”。凸包计算是最早的复杂几何算法之一，并且有很多变种。该算法还适用于外形与其顶点集合相同的多边形或一组线段。凸包的应用有很多：避免碰撞，确定隐藏对象和进行形状分析等。二维的凸包称为凸多边形，几何对象(如点集或多边形)的凸包是包含该对象的最小凸集。定义凸集S为：集合S是凸集当且仅当任取P、Q∈S，整个线段PQ也在集合S内。定义凸包P为：凸包是包含集合S的每个点的最小凸多边形，当且仅当对于P内的任意两个点A和B，线段AB都在P内时，P为凸包。

使用单调链算法对2D凸包进行求解，单调链算法基于堆栈进行实现，并且有较高的排序效率[14]。

首先对点集S={P0,P1,…,Pn-1}按照x和y递增的顺序进行排列(先x然后y)，设xmin、xmax分别为点集上x坐标的最小值和最大值。显然有，P0x=xmin，也许其他点也有同样的最小值。设Pmin min表示点的x坐标等于xmin时，y坐标取最小值的点，同理，Pmin max表示点的x坐标等于xmin时，y坐标取最大值的点。若点集S上存在多个不同点的x坐标都是xmin，那么Pmin min和Pmin max表示不同的点，否则表示相同的点，对于点Pmax min和Pmax max也是类似的。如图7所示，连接Pmin min、Pmax min和Pmin max、Pmax max分别用Lmin和Lmax表示。在直线Lmax上方的称为上点集Ωmin构成凸包的上凸链，在直线Lmin下方的称为下点集Ωmax构成凸包的下凸链，点集S的凸包Ω为Ω=Ωmin∪Ωmax。

之后用堆栈算法将凸包Ω构造为单调链，对于下凸链，从堆栈上的Pmin min开始，依次序处理点集S中在直线Lmin下方的点，假设在任何阶段，堆栈上的点都是Lmin下方已经处理过的点的凸包。现在考虑Lmin下方的一个点Pk，如果堆栈仅包含一个点Pmin min，则将Pk放入堆栈顶部，然后进入下一阶段。否则确定Pk是否严格位于堆栈顶部两点之间的线的左边。如果是，则将Pk放入堆栈顶部并继续。如果不是，将堆栈顶部的点弹出，然后再次针对堆栈测试Pk，继续直到Pk被推入堆栈。处理完所有点后，将Pmax min推入堆栈以完成下凸链。上凸链以类似的方式构造，但是以降序{Pn-1,…,P1,P0}的方式处理点集S，从Pmax max开始，并且仅考虑高于Lmax的点，上凸链和下凸链连接时要避免连接重复的端点。单调链算法示意图如图7所示，算法伪代码如表2所示。

图7 单调链算法示意图Fig.7 Schematic diagram of monotonic chain algorithm

表2 单调链算法伪代码Table 2 Monotonic chain algorithm pseudo code

地图转换方法PolygonsDBSMCCH采用了单调链算法，算法流程图如图8所示。

3 移动机器人平台仿真与试验

3.1 移动机器人导航仿真试验

Stage是加拿大VirtualPrototypes(VPI)公司推出的用于国防和航空航天领域的灵活且实时的仿真与训练环境，它是一个与ROS有较好的兼容性并且运行效率很高的轻量级仿真平台。在Stage仿真平台下搭建了一个堆放杂物的小屋环境，设置红色障碍物位置为[8,4]，如图9所示。

图9 Stage仿真环境Fig.9 Stage simulation environment

在图9的仿真场景下，进行了加地图转换和不加地图转换的对比试验，试验结果如图10、图11所示。设定同一导航目标点[14,7],导航起始点[2,2]，可以明显看到，不加地图转换的TEB算法局部代价地图层出现了大量扫描出的地图点。而加了地图转换之后的TEB算法，局部代价地图层只有清晰的多边形和少量地图点。

图10 无地图转换仿真示意图Fig.10 Schematic diagram of simulation without map conversion

图11 PolygonsDBSMCCH仿真示意图Fig.11 PolygonsDBSMCCH simulation diagram

实验数据结果如表3所示，在相同条件下，可以看出不进行地图转换的导航需要74.60 s。而使用PolygonsDBSMCCH方法需要67.90 s，可以看出在精度上相差不大，横纵坐标误差都在0.1以内。在时间上，总体来说使用地图转换的导航时间均低于不使用地图转换的导航时间。可以得出结论，地图转换聚类能有效提升导航的快速性，本文研究对于算法改进是有效的。

表3 地图转换仿真实验对比Table 3 Map conversion simulation experiment comparison

3.2 移动机器人导航试验

在实验室中进行了对比试验，使用的移动机器人平台如图12所示，turtlebot2是Willow Garage公司研发的第二代产品，其上搭载有kobuki移动底盘、hokuyo激光测距仪、主控机为Legion Y7000P，配置为因特尔酷睿i5,运行内存8 G。

图12 Turtlebot2移动机器人Fig.12 Turtlebot2 mobile robot

采用Gmapping建图方法在大连海事大学科学会馆214室建立实验室环境地图如图13所示。

图13 实验室环境地图Fig.13 Laboratory environment map

将加入地图转换前后的局部代价地图层进行对比，验证地图转换的有效性。局部代价地图层如图14、图15所示。

红色代表传感器检测到孤立的障碍物点图14 地图转换示意图(无聚类)Fig.14 Map conversion diagram (no clustering)

红色代表传感器检测到孤立的障碍物点图15 地图转换示意图(PolygonsDBSMCCH)Fig.15 Map conversion diagram (PolygonsDBSMCCH)

对比图14、图15可以看到，代价地图层发生了明显的变化。在加入PolygonsDBSMCCH之后，障碍物点变成了规则的多边形障碍物模型。设定相同的起点和终点，起点坐标为[1.88,0.02]，终点坐标为[2.95,-3.48]，最大导航速度设为0.3 m/s，导航数据如表4所示。

表4 地图转换实验对比Table 4 Map conversion experiment comparison

从表4中可以看出，在相同条件下，不进行地图转换的导航需要53.92 s，而优化后的导航使用PolygonsDBSMCCH花费46.11 s，相比于不使用聚类优化的空白对照，导航效果有所改善。精度上，空白对照横坐标相差0.16，纵坐标相差0.12，PolygonsDBSMCCH横坐标相差0.1，纵坐标相差0.02，可见，使用地图转换之后导航精度略有提高，证明了对导航算法的改进的有效性。

4 结论

通过将时间弹性带算法引入局部路径规划，对移动机器人局部路径规划算法进行了优化，并基于时间弹性带算法，提出了改进导航性能的方案。

(1)通过施加导致最短路径的内部收缩力和从障碍物辐射的外部排斥力使得规划的路径变形，从而获得无碰撞路径。

(2)将基于DBSCAN聚类算法的地图转换插件加入局部路径规划，配合TEB算法提升了移动机器人导航的快速性和稳定性，将传感器扫描到的障碍物的原始数据转化为基本几何图形(点、线、多边形)，提高了局部代价地图层的构建效率，使得代价地图层的计算量大大减少，也减少了障碍物约束的计算量。对于移动机器人的仿真试验和现场实验都验证了本文研究中基于时间弹性带的局部路径优化方法的有效性。