改进D*lite 和时间弹性带法的移动机器人路径规划*

吴 涛谢志军陈科伟

(宁波大学信息科学与工程学院，浙江 宁波 315000)

路径规划对于实现移动机器人自主导航具有重要的意义，其目的是在规定环境中规划出一条由起点到终点的最优路径，评价指标可以是时间、距离或安全性等[1]。 根据对环境的了解程度可分为全局路径规划和局部路径规划[2]。 全局路径规划常见的有迪杰斯特拉算法[3]、A*算法[4]、跳点搜索算法[5]、D*lite 算法[6]、RRT*算法[7]等。 迪杰斯特拉算法、A*算法及跳点搜索算法多用于静态环境下。 迪杰斯特拉算法利用贪心策略得到起始点到地图上全部点的最短距离，因此在大型地图上会消耗较多算力。 A*算法通过比较当前节点的八邻域启发函数值搜索路径，提高了搜索的效率，但是当八邻域中出现相同的最小值时，可能无法求出最优解。而跳点搜索算法是在A*算法的基础上只对跳点进行搜索[8]，进一步提高了搜索效率。 然而上述3 种算法在动态环境下都难以保证效果。 D*lite 算法虽适用于动态环境，但算法效率却有待提高。RRT*算法也可用于动态的路径规划，其优势在于对环境建模要求不高，可在不同的地图模型上进行规划；但是其规划精度和迭代次数有较大关系，更高的精度意味着更加耗时。

局部路径规划算法常见的有动态窗口法[8]、粒子群算法[9]、时间弹性带法[10]等。 动态窗口法可实现实时避障，通过在一个动态的窗口中估计机器人可能的轨迹决定机器人的下一个动作，但在复杂的环境下对于轨迹的评估将变得十分复杂，使得效果下降。 粒子群算法将每一条规划的路径视为一个粒子，通过对粒子群进行演化，选择最优的粒子个体，该算法随机性较大，同样不适合复杂环境。 时间弹性带法通过配置多个优化目标，包括路径长度、规划时长、安全距离以及运动学约束等，利用图优化的方式进行问题的求解，但该算法对于全向机器人的支持较差[11]。

D*lite 算法(以下简称D*lite)是一种增量式启发算法，相比于广泛使用的A*算法，更适用于在未知环境下的路径规划。 Setiawan 等[12]通过对A*算法和D*lite 进行仿真和试验的对比，得出结论:D*lite 在动态环境下，多数情况要好于A*算法。由于D*lite 还存在一些问题，各种改进方法也被提出，Xie 等[13]提出了一种改进的D*lite，通过子节点选择优先级策略，由垂直方向的节点状态决定对角方向的节点是否展开，使得规划的路径与障碍物之间产生了安全距离，但该方法会增加节点数量导致非最优解的出现。 Yao 等人[14]提出了一种将D*lite 和人工势场法相结合的融合算法，并将其应用于复杂环境下的无人船自动驾驶，仿真实验表明该算法规划的路径平滑且安全，但是该算法并未对D*lite 的规划效率进行优化。 Li 等人[15]将D*lite和其提出的人工不可遍历顶点的概念相结合，提出了一种可以快速重规划的多机器人路径规划算法，在检测到碰撞发生前可基于人工不可遍历顶点和D*lite 进行路径的重规划。 黄鲁等[16]提出了一种基于路径优化的D*lite，通过将懒惰视线法和距离变换引入D*lite 中，使得算法可以在任意方向进行拓展，同时和障碍物保持了一定的安全距离，但是这样会降低算法的搜索效率。 刘军等[17]通过观察目标点与障碍物之间的关系，引入关键节点概念来降低搜索空间，让算法只对障碍物附近的关键点进行访问，路径由相互连接的关键点构成，很好地降低了搜索空间，但是产生了路径贴近障碍物的问题。

由于机器人在实际应用中需要考虑的因素更为复杂，单一的全局路径规划常常难以胜任。 全局路径规划和局部路径规划相结合成为一种更为常见的做法。 当前和全局规划算法结合较多的局部规划算法有动态窗口法及时间弹性带法。 庞磊等[18]将A*算法和时间弹性带法相融合，通过激光点云标记非行人动态信息，生成全局静态地图和局部动态地图。 利用A*算法在静态地图中进行全局规划，时间弹性带法在局部地图中进行动态规划实现了行人目标的动态跟踪。 李秀智等[19]提出了基于复合式协同策略的机器人自主探索方法，利用改进的RTT 算法进行机器人自主探索，将A*算法和时间弹性带法相融合进行路径规划。 通过增加约束条件，解决机器人倒退时容易碰撞到障碍物的问题，实现了机器人自主探索及路径规划。

针对以上提到的问题，本文提出一种结合改进D*lite 和时间弹性带法的融合算法，通过全局优化提高搜索效率，减少冗余节点，通过局部优化降低动态物体对算法规划效率的影响，并提高路径安全性。

1 全局路径优化

1.1 D*lite

D*lite 采用一种从目标点向起始点规划的策略，并适用于起始点改变的情况。 算法流程如图1 所示。

图1 D*lite 流程图

D*lite 在初始化地图后，首先预规划出一条最优路径。 当机器人运动后，每运动一次就判断当前位置是否为目标点。 若为否，从优先列表中选出k 值最小的节点作为下一个起点。 当路径出现障碍物后，将路径上受影响的点更新状态后再次置入优先列表中，并重新从目标点开始规划出一条路径。由于采用了先前的节点信息，可以实现较快的重规划。

在生成最优路径这个阶段，D*lite 会在地图上建立一个“路径场”，增量式的靠近目标。 使得算法资源消耗巨大，同时会产生一些冗余拐点。

为了提高规划效率，在D*lite 拓展路径节点前增加一个筛选跳点的过程，减少对一些无关节点的计算和存储。

1.2 跳点筛选策略

传统D*lite 拓展节点时，是在当前节点的八邻域中找到下一个节点，同时将这些点都存储在优先列表中，导致搜索效率下降。 而传统跳点搜索策略也会在对角方向产生冗余，本文通过在对角方向搜索时，增加一个判定过程以消除这种冗余。 具体可分为2 种情况:

情况1 遍历的方向为水平或竖直

①在这两个方向上的节点若为起始节点或者目标点，则认为是关键点。

②若在这两个方向上的某一节点有强迫邻点则认为是关键点。

如图2 所示，节点Y的八邻域中存在障碍物，且节点X经过Y到达节点N的距离代价相比于不经过Y到达N的任意路径的距离代价小，就认为Y有强迫邻点N。

图2 关键点筛选策略

情况2 当遍历方向为对角方向

当沿着对角方向搜索时，若对角方向的节点在水平或者垂直方向上的延伸节点满足情况1，且这些满足情况1 的延伸节点都与出发点间无障碍物，则将这些延伸节点置入优先列表。 如图2 所示，从出发点Y开始进行对角方向的搜索，在T节点的水平方向上有节点Z是个关键点，且节点Z与出发点Y之间无障碍物，则将Z置入优先列表。 值得注意的是，情况2 时，对角节点在水平或竖直方向的符合条件1 的全部点都需与遍历的出发点间没有障碍物，才可舍弃对角方向上的点，否则必须保留对角方向上的点。

1.3 优化搜索步长

在每次搜索关键点时都会从当前节点出发一直遍历到关键点或者边界。 当地图较大时，会导致搜索效率的下降。 可通过计算当前点和起始点的距离，动态地调整搜索的步长，提高搜索效率。 常见的距离算法有欧式距离、对角距离和曼哈顿距离。 为确保最大可能地搜索到关键点，本文选用曼哈顿距离计算搜索步长，公式如下:

式中:d(c，s)表示搜索的步长，c和s分别表示当前点和起始点。 当在搜索步长内未发现关键点时，则将步长依次乘以一个倍数继续进行搜索，L为地图的最大边长。

2 局部路径优化

针对动态物体对D*lite 规划效率影响大、规划路径安全性低的问题。 本文通过改进的时间弹性带法进行局部规划加以解决，当时间弹性带法无法完成局部规划时，再调用改进D*lite 进行重规划。

2.1 时间弹性带法及其约束

时间弹性带法是一种适用于竞速机器人的局部路径规划算法，具有规划速度快、参数配置灵活的特点。 但是对于可以原地旋转的机器人会产生不必要的倒退及转折，增加运动时间，在某些情况下甚至会导致路径规划失败[20]。 本文通过增加转向约束来避免以上情况的发生。

定义机器人在时刻k的位姿为:

式中:xk、yk为机器人在地图坐标系中的坐标，βk∈S1，S1∈[－π，π]为机器人的偏航角。 机器人的位姿序列可表示为:Q＝{Sk}k＝1，…，n。 设相邻的两个位姿间的时间间隔为ΔT，则时间序列可表示为ΔT＝{ΔTk}k＝1，…，n－1，其中ΔTk表示从位姿Sk到位姿Sk＋1所用的时间。 将位姿和时间序列合并可得到时间弹性带b＝{Q，T}。 图3 为三个连续的时间弹性带。

图3 时间弹性带序列

而时间弹性带法的优化模型可以表述为找到在最短时间内让机器人从初始位姿S1转换到目标位姿Sn的一系列路径点，同时需要满足运动学上的约束及保证机器人的安全。 该过程可以转化为求解一个非线性的函数:

它由以下约束限制:

式中:初始位姿S1和最终位姿Sn分别受到Sstart和Sfinal的约束。 机器人的速度及加速度分别由不等式vk(·)及αk(·)约束。 不等式Ok(Sk)约束了机器人和障碍物之间的最小距离。

2.2 速度和加速度约束

机器人的平均线速度vk和平均角速度ωk可以由两个连续位姿Sk＋1，Sk的距离范数、偏航角的差值及对应的时间间隔ΔTk分别计算得到，如式(4)和式(5)所示:

令vmax，ωmax分别表示机器人的最大线速度和最大角速度。 则速度的约束可用式(6)表示:

机器人的平均线加速度ak和平均角加速度表示如下:

令amax，分别表示机器人的最大线加速度和最大角加速度。 则加速度的约束可用式(9)表示:

2.3 躲避障碍物

设机器人在位姿Sk时，局部区域的障碍物集合为O，οi表示集合中的障碍物，i＝1，2，…，R。 与障碍物的最小间距设为ρmin。 则机器人与障碍物之间的约束可表示为:

2.4 转向约束

本文研究的是可原地旋转的全向机器人，利用式11 来提取机器人运动的方向。

式中:qk＝[cosβk，sinβk＋1，0]T，而κ是一个可变的比例因子(本文中取κ＝102)，dk，k＋1表示两个位姿间的距离向量，符号<>计算向量的混合积。γ(Sk，Sk＋1)的取值为{－1，1}。 当取值为－1 时机器人向后运动。 当机器人需要向后运动时，通过建立转向约束，使得机器人进行原地旋转。 约束如下:

式中:β表示全局规划路径上离机器人最近的点和机器人x轴的夹角，β0为机器人当前位姿的偏航角。ωmax为机器人最大的角速度。

3 算法融合

改进D*lite 和时间弹性带法融合后，在拥有动态规划能力的基础上，提高了搜索效率及路径的安全性。 融合步骤为:改进D*lite 规划全局路径，得到全局路径点。 将两个相邻的路径点确定为局部路径规划的起始点和目标点。 之后，进入到局部规划中，若局部规划成功，判断是否为全局的终点；否则调用改进D*lite 进行重规划。 融合算法流程图如图4 所示。

图4 融合算法流程图

4 仿真及实验验证

为了验证改进算法的有效性，对本文提出的改进D*lite(以下简称JD*lite)、改进时间弹性带法及融合算法分别进行仿真实验。 之后，将融合算法应用在真实的物理环境中进行实验验证。 硬件平台为:Intel Core i5 8400 处理器，16 GB 内存；编程语言Python，C＋＋。

4.1 全局仿真实验

在20×20 及200×200 的栅格地图上分别进行随机的障碍物填充，各生成10 份不同的地图。 规定起点为(1，1)，终点分别为(20，20)及(200，200)。分别用D*lite、RRT*及JD*lite 进行仿真实验。并设定RRT *在两种情况下的迭代次数分别为2 000次及15 000 次。 仿真实验结果如图5、图6所示。

图5 20×20 地图下3 种算法数据对比

由图5(a)及图6(a)可以看出，RRT*的路径节点个数在两种情况下都要多于另外两种算法。 这是因为RRT*不是以单个栅格进行子节点的拓展，单个栅格中会出现多个拓展节点，这大大增加了节点的个数。 而JD*lite 在路径节点个数上体现出较大的优势，两种情况下的平均节点个数仅为15个及32个。 远低于RRT*的162个、382个和D*lite 的34个、293个。 这是因为JD*lite 在D*lite 的基础上，引入了跳点搜索策略来搜寻子节点，同时删除路径上的冗余跳点，使得JD*lite 在栅格地图上的路径节点数为三者最优。

图6 200×200 地图下3 种算法数据对比

在规划的路径长度上，观察图5(b)、图6(b)可知，JD*lite 在大多数情况下处于领先。 在两种情况下的仿真实验中，仅有一次路径长度长于RRT*，其他均优于D*lite 及RRT*。 平均优于D*lite规划长度7.57%及RRT*规划长度9.83%。

出现上述现象的原因是D*lite 采用八邻域做路径规划，而JD*lite 可跳出八邻域的限制，实现更加灵活的规划。 同时删除了冗余的拐点使得路径进一步缩短。 而RRT*的规划方式虽然更加灵活，但是需要迭代的次数也大大增加，在本文设定的迭代次数下，效果弱于JD*lite。

在规划时间方面，20×20 地图下D *lite 和JD*lite的规划时长差距不大，平均时间为0.04 s 和0.10 s。 而在200×200 的地图上时，则产生了较大的差距，平均时间分别为0.93 s 和4.55 s。 这是因为JD*lite 遍历地图时仅需要判断该点是否为跳点，无需存储大量非关键点，每次进行子节点拓展时需要根据节点的关键值进行排序操作，而这个过程较为耗时。 减少排序队列的长度可有效降低规划时长。 RRT*的规划时长为3 者最长，原因是RRT*在迭代次数固定的情况下，其规划速度和规划地图的难度有较大的关系，当遇到狭窄的通道时，会明显降低规划速度。

两种情况下各随机选出一份规划路径图进行比较，见图7。 可以看到三种算法都能规划成功，D*lite规划的路径有较多的转折，从而导致路径冗余。 而RRT*生成的路径较为曲折，不适合直接用于机器人的路径导航。 而观察图7 发现D*lite 规划的路径总是贴近障碍物的拐角处，这容易导致机器人的碰撞事故发送，需要结合局部路径规划，进行安全性优化。

图7 3 种算法的规划结果

4.2 改进时间弹性带法有效性仿真实验

为了验证时间弹性带法改进的有效性，本文使用Stage 仿真平台进行仿真实验。 首先，建立如图8所示的仿真环境，并导入配备了激光雷达的Turtlebot2 机器人模型。

图8 未优化前的时间弹性带法

如图8 和图9 所示，图中第1 行为虚拟仿真环境。 下面第2 行为第1 行中虚线区域的地图及规划路径放大图。 从图8 中可以看到，当目标点在机器人正方向的侧面及后面时，机器人会产生倒退的路径。 在图9 中，改进后的时间弹性带法在原地调整自身的位姿后，克服了上述的问题。

图9 优化后的时间弹性带法

4.3 融合算法有效性仿真实验

为验证本文提出的融合算法的有效性，分别对D*lite，JD*lite 及融合算法在环境相同的栅格地图上进行路径规划，并设置动态障碍物。 动态障碍物在水平或者竖直方向做往复的匀速运动。 图10(a)为三种算法未与动态障碍物相遇时的情况，结合表2可知，JD*lite 在规划路径长度和规划速度上处在领先的地位，同时发现JD*lite 规划的路径贴近障碍物，存在安全隐患。 而融合算法由于加入了局部规划，在规划时间上比JD*lite 要多；同时考虑了和障碍物的最小距离，使得路径长度也有所增加，但是依然比原始的D*lite 提升了4.21%、45.58%。 图10(b)为动态障碍物运动到D*lite 预规划好的位置上后重规划生成的路径。 可以看到D*lite 成功地进行了重规划，但是由于动态障碍物的运动导致规划路径变的不稳定，产生了路径的分叉。 观察图10(c)并结合表1 可知，JD*lite 在动态物体的扰动下依然可以规划出路径，同时可以发现规划路径的距离变大了。 这是因为动态物体使搜索的跳点位置发生变动，选择的跳点可能不是真正的最优点。 结合图10(d)及表2 可知，当在已规划好的路径上出现动态障碍物时，融合算法可在局部路径上进行局部动态规划，避开动态障碍物之后，回到原来的路径上，同时规划的路径更加平滑，符合机器人的运动约束。 在保持最优路径的同时，做到安全高效的规划。由表2 可以看到局部路径规划的重规划用时要比JD*lite 的用时要长，但是规划的路径长度更接近原始长度。

表1 静态路径规划性能对比

表2 动态路径规划性能对比

图10 动态路径规划图

4.4 融合实验与分析

为进一步验证本文提出的融合算法在移动机器人路径规划上的可行性，利用机器人操作系统(Robot Operating System，ROS)在真实环境下进行实验。 系统环境为Ubuntu 16.04 操作系统，实验机器人为Turblebot2 机器人，如图11(a)所示。

本文的实验场地为办公室走廊到电梯口的一部分，如图11(b)所示。 通过机器人所配置的激光雷达建立的地图如图11(c)所示。

图11 实验环境及建图

分别运行A*算法、D*lite、JD*lite 及融合算法，得到的路径如图12 所示，图12(a)为A*算法规划的路径，可以发现在规划好的路径上遇到未知障碍物时，A*算法不能绕过未知障碍物，导致路径规划失败。 图12(b)为传统D*lite 算法生成的路径，传统的D*lite 绕过未知障碍物和动态障碍物后到达了目标点，但是从运动过程中可以看到，机器人会与障碍物发生碰撞，造成危险，这是由于D*lite 算法本身并未考虑机器人和障碍物的距离。 图12(c)为JD*lite 规划生成的路径图，其规划的路径比起D*lite 算法减少了一些多余的拐点，路径的转折角度幅度更大。 同时发现规划路径和障碍物的距离较近，虽然最终也绕过了障碍物，但是在这个过程中出现了一些停顿，并且在运动过程中还和障碍物发生了轻微的碰撞。 图12(d)为融合算法生成的路径，可以看到融合算法生成的路径较为平滑，且与障碍物始终可以保持一个最小的距离，在遇到动态障碍物时成功地进行了局部动态规划，避开了正在运动的物体。

图12 对比实验

表3 给出D*lite、JD*lite 及融合算法在图12地图中的性能表现。

表3 三种算法性能

由表3 可知，融合算法相比JD*lite 在时间及长度上虽然有所增加，但是提高了机器人的安全性。相比于D*lite 在规划时长、路径长度、最小距离及运动时长上分别优化了67.10%、11.04%、100%和11.99%。

5 结论

本文通过将改进的跳点搜索引入到D*lite 中进行关键点的筛选，融合时间弹性带法进行局部路径优化，提高了规划效率、提升了动态情况下的规划能力，同时提高了生成路径的安全性。 算法在真实的环境下获得了较为明显的优化效果。

需要指出的是本文算法研究的是以栅格地图为基础的路径规划，规划结果的精度是由栅格的大小决定的。 当选用较小栅格尺寸的模拟地图环境时会导致算法的性能有所下降。 同时，本文的实验环境较为简单，在复杂环境中大量的动态物体对于算法的稳定性要求也会更加严格。 同时由于设置了和障碍物的安全距离约束，致使机器人在狭窄通道中的运动受到一定限制。

在接下来的研究中，将尝试复杂动态场景下的移动机器人路径规划研究，进一步推动移动机器人路径规划的研究与实践。