多目的地自主移动机器人的路径规划

余文凯，章 政+，金 震，黄卫华

(1.武汉科技大学 机器人与智能系统研究院，湖北 武汉 430081；2.武汉科技大学 信息科学与工程学院，湖北 武汉 430081)

0 引 言

移动机器人的多目的地路径规划是指从起点自主移动到两个或两个以上目的地的无碰可行路径[1-3]。对于多目的地路径规划而言，采用现有的单目的地规划方式，则需要多次规划起点到各目的地的路径，存在多次路径的往返，甚至无效路径等问题，影响机器人的路径搜索效率。因此，在多目的地环境下实现一次性的完整路径规划，对于节约运行时间、提高规划效率具有重要的意义。

多目的地的路径规划问题主要涉及如何快速搜索出各两地点之间的路径、快速排序目的地点和局部动态规划。文献[5]针对公路运输的路径规划问题提出一种求解多目的地运输路线规划问题的方法，并根据不同路段运费及时间等权重，利用哈密顿图/半哈密顿图的构造条件选取适当的路径构造哈密顿通路，使之在运输过程中选取合适的路径不重复地遍历所有目的地，达到最优效果。文献[6]提出了一种基于利用蚁群算法快速收敛的特性改进粒子群算法的路径规划收敛性的方法，该方法将目标地点的选择转化为旅行商问题，并利用蚁群算法进行优化。文献[7]提出一种分级粒子群、遗传和A*算法相结合的遍历多任务路径规划方法，解决了粒子群-遗传算法存在计算成本过高并且单一算法不能解决有障碍物存在的地图上遍历多任务目标点的移动机器人避障行走问题。上述研究工作对多目的地的全局静态路径规划进行了研究，验证了多目的地路径规划比多次单目的地路径规划的效率高。然而，多目的地路径规划算法不仅涉及全局的静态规划，还有机器人在移动中的局部动态规划。

因此，本文结合改进A*算法、模拟退火算法和改进动态窗口算法设计了一个结合静态全局路径规划和局部动态规划的多目的地路径规划算法。实验结果表明，本文所设计的算法具有搜索效率高、适应性强和运动速度平稳性好等特点。

1 多目的地路径规划算法设计

机器人移动到达多个目的地，不仅需要规划两地点之间的路径，还需要规划到各个目的地的先后顺序。如图1所示，其中S为起点，Ti(i=1,2,3,4)为目的地，全局移动路径的方案有多种选择(如图1方案一、方案二)，需要进行全局静态规划，获得全局的最优路径。而在移动中，机器人在按照全局静态最优路径进行移动时，面临行程中出现路径可能会被占据，目的地位置可能发生变换等问题。单一的全局静态规划不能解决移动中的问题。因此，将多目的地路径搜索分成全局静态路径规划和局部动态规划，保证机器人安全、准确抵达每一个目的地。

图1 多目的地路径规划方案

全局静态路径规划是指机器人在移动前规划一条由起点经过所有目的地再回到起点的全局路径。首先，需要多次运算起始地点与目的地(S→T1，S→T2，…)、目的地与目的地(T1→T2，T2→T3，…)两两之间的路径规划，并计算两地点之间规划路径的距离大小；然后，依据此距离大小对多个目的地进行优化排序，选择一条距离最小的全局路径。如图1所示，其路径为(S→T1→T2→T3→T4→S或S→T2→T1→T3→T4→S…)。运算量虽然增加，但规划后的全局路径一次经过所有目的地，移动路径大大减少。

局部动态路径规划是指机器人在移动过程中，面对出现的多个目的地确保机器人可以有效进行局部动态避障的同时，能按照预先规划路径进行移动。设计多目的地局部动态路径规划时，将预先规划的路径节点当作当前移动的目标点，每两个路径节点组成一段路径，通过机器人传感器获取障碍物信息，判断这一段路径内是否有障碍物，存在障碍物进行局部动态避障，否则依据规划的路径进行移动。当目的地位置发生变化时，可将当前节点设置为新的目的地坐标。即保证了全局路径最优，又可及时动态规划。

针对多目的地路径规划的特点，本文设计了一种结合全局静态路径规划与局部动态路径规划的组合路径搜索算法，其结构如图2所示。

图2 多目的地路径规划算法结构

2 基于改进A*算法的多目的地全局静态路径规划

2.1 改进A*算法两地点间的路径规划

A*算法在多目的地全局静态路径规划中需要多次运算进行两地点间的路径规划，是解决静态路网中最短路径的最有效的直接搜索方法。

基于栅格法建立地图模型，采用A*算法规划的路径是由栅格中心节点组成，如图3所示，路径为(S,n1,n2,n3,n4,n5,n6,n7,n8,n9,n10,Ti)；存在许多问题，例如冗余的路径节点，多路径拐点，并且路径节点只能位于网格的中心。若直接将A*算法规划的路径节点代入到动态规划算法中，则导致路径不是最短且平滑度较差。因此，对A*算法规划的路径节点进行删除冗余节点的优化改进。

图3 路径平滑优化

步骤1 判断节点位置是否为一段路径的中间位置，然后删除同一方向上的中间节点，只保留起始的节点、转弯处的节点及目标点。如图3所示，运算后保留的路径为S→n7→n8→n9→Ti。

步骤2 从路径的起始点S方向开始运算，在保留的路径ni、nj之间每隔k步长取一节点m，并与前一段路径节点判断，判断两点之间路径是否存在障碍物。

(1)若有障碍物，则路径节点不变；

(2)若无障碍物，如图4所示，计算障碍物与两点间的连线的距离d。

图4 障碍距离判断

图4中，路径节点S的坐标为(xa,ya)，n12的坐标为(xb,yb)，障碍点C的坐标为(xc,yc)，计算距离d为点C与路径AB之间线段的垂直距离，l为障碍点C到节点S与节点n12之间线段纵轴的距离CD，节点AB之间线段与横轴之间的夹角为∂。由S、n12及C三点坐标可求出障碍点到路径的距离为

d=lcos∂

(1)

判断障碍物与两点间的连线距离d与设置的安全距离D的关系；

(1)若d≤D，则路径节点不变；

(2)若d>D，则节点m为新的路径节点。

如图3所示，处理后的路径为S→n11→Ti。

步骤3 从路径目的地点T的方向，反向取点判断(重复步骤2的判断方式)，输出优化路径Path，算法结束。如图3所示，优化后路径为S→n12→Ti。

2.2 基于模拟退火算法的多目的地排序策略

规划完所有两地点间的路径后，如图1移动顺序有多种情况如S→T1→T2→T3→T4→S或S→T2→T1→T3→T4→S，……。因此，需要对多目的地优化排序，选择出一条经过所有目的地且距离最短的全局路径。

假设在移动机器人多目的地路径规划中含有n个目的地，则路径到达顺序的组合有(n2+n)/2种。为了满足移动机器人运动路径最短要求，机器人需要从(n2+n)/2种组合中选择一条最短路径。模拟退火算法[6]参数少，计算时间短，鲁棒性强，适用于本文高效率的要求，多目的地排序过程采用模拟退火算法优化排序。

基于模拟退火算法的多目的地排序算法步骤如下：

步骤1 确定初始温度Temax、终止温度Temin及降温速率r(0

步骤2 将(S,T1,T2,…,Tn,S)组成n！个排列组合的解空间Ι={(S,T1,T2,…,Tn,S)、(S,T2,T1,…,Tn,S)…}，并随机生成一个初始解Ιo(Ιo∈Ι)；

步骤3 通过二变换法产生新解，任选序号m，k，交换解元素第m个和第k个之间的访问顺序，若交换的前路径解为Ιi=(S,T1,T2,…,Tm,…Tk,…Tn,S)，交换后的路径为新路径，如式(2)所示

Ι′i=(S,T1,…,Tm-1,Tk,Tm+1,…,Tk-1,Tm,Tk+1,…,Tn,S)

(2)

步骤4 设置距离长度为目标函数，再由式(3)、式(4)计算变换前的解Ιo和变换后的解Ι′i的目标函数的差值Δf

(3)

Δf=f(Ι′i)-f(Ιi)

(4)

步骤5 由Metropolis接受准则式(5)计算接收概率p，选出下一代新解；若Δf<0，表示变换后的解代价小于变换前的解，新解为Ι′i；若Δf>0，表示变换后的解代价大于变换前的解，则通过接受当前路径作为新的路径概率p判断新解，概率p越大接受可能性就越大

(5)

步骤6 更新温度Te′，由降温函数式(6)降温

Te′=Te×r

(6)

步骤7 判断降温后的温度Te′是否到达终止温度Temin，满足条件，则输出最优解，算法结束；否则，执行步骤3；

3 基于改进DWA算法的多目的地局部动态路径规划

DWA(dynamic window approach)算法[11]作多目的地的局部动态路径规划算法可以将预先规划的路径节点与局部动态规划有效结合，让机器人按全局规划的最优路径移动的同时，能安全、准确地到达目的地。

移动中DWA算法时刻模拟运动轨迹，选择评价函数最高的一组为下一时刻的运行线速度和角速度进行移动。其评价函数中有表示模拟轨迹与目的地之间方位角偏差的方位角评价函数，可将目的地改成全局规划的路径节点，依据移动位置不断改变路径节点，直到变成目的地。DWA算法可有效结合全局规划的路径，实时性好，动态避障能力强，满足高效率的要求。

3.1 机器人运动模型

基于动态窗口算法的局部动态规划预测机器人的运动轨迹，选择最优路径。首先，建立机器人的运动模型。设机器人的线速度为vt和角速度为wt。当前机器人的位姿表示为(xo,yo,θo)，假设机器人在时间间隔Δt内作匀速直线运动，则下一时刻位姿为(xt,yt,θt)，运动模型如式(7)所示，为

(7)

3.2 机器人速度约束及采样

动态规划中预测轨迹是由机器人可能产生的速度组合(vt,wt)来采样模拟，其速度组的大小范围不仅有机器人自身约束，即机器人速度存在最大、最小速度的约束如式(8)所示，还有紧急刹车的安全距离约束，如式(9)所示。因此，在机器人自身限制及环境因素约束条件下，速度采样范围为V，如式(10)所示

V1={(v,w)|v∈[vmin,vmax]∩w∈[wmin,wmax]}

(8)

(9)

V=V1∩V2

(10)

在速度采样范围V下，可生成多组不同速度组(v,w)的模拟轨迹，如图5所示，选择评价函数最高的一组(v,w)进行移动。

图5 动态窗口采样

3.3 改进采样速度的评价函数

动态规划中，计算生成模拟轨迹每组(v,w)的评价函数，选择最优(v,w)进行移动，传统动态窗口的评价函数为

G(v,w)=α·head(v,w)+β·dist(v,w)+γ·vel(v,w)

(11)

传统DWA算法进行规划时，缺少全局静态规划路径的指引，评价函数dist(v,w)使移动机器人轨迹远离障碍物，易生成局部最优轨迹，增大了全局的移动距离。若直接减少其加权系数β，则对动态障碍物避障时，不能及时避障，导致移动机器人与动态障碍物发生碰撞。因此，本文将静态全局规划的路径节点与DWA算法的评价函数相结合。改进了动态窗口的评价函数，方位角的评价函数和靠近障碍物距离的评价函数，如式(12)所示

G(v,w)=α·heading(v,w)+β·dist_sta(v,w)+
γ·vel(v,w)+λ·dist_dyna(v,w)

(12)

式中：heading(v,w)为当前目标点方位角评价函数，即下一个路径的移动节点作为当前的目标点，在当前速度下，模拟轨迹的位置与当前目标点的方位角偏差；此改进有利于移动机器人沿着全局规划的最优路径移动。dist_sta(v,w)为速度对应轨迹上离全局静态已知障碍物最近距离的评价函数，dist_dyna(v,w)为速度对应轨迹上离局部动态未知障碍物最近距离的评价函数；对障碍物区别后，可分别调节其加权系数β和λ，既可以减少已知障碍物对规划轨迹的干扰，又保证了对未知障碍物及时避障。

3.4 初始位姿状态预处理

多目的地路径规划中，移动机器人初始位姿方向和每到达一个目的地的位姿方向与静态规划路径方向并不相同，其初始位姿与路径目标节点的方位角偏差Δθo越大，生成运动轨迹的转弯幅度越大，导致移动距离增大，如图5移动轨迹所示。

针对上述问题，本文设计了姿态调整函数，在起始点移动前调整机器人姿态的方向角θo，将其姿态与预先规划下一个路径节点的方位角偏差Δθo消除

(13)

4 多目的地路径规划算法流程

本文基于多目的地的路径规划设计的算法结构如图6所示；其算法步骤如下：

步骤1 初始化栅格地图模型，确认n个目的地坐标；

步骤2 生成(n2+n)个规划组；

步骤3 基于A*算法对其所有组进行路径规划，并进行路径节点双向平滑度优化；

步骤4 基于模拟退火算法对所有路径进行优化排序，得到完整最优路径；

步骤5 得到开始移动指令；

步骤6 判断初始位置与下一个路径节点的方向角偏差Δθo，若Δθo为0，则开始移动；否则，姿态调整消除方向角偏差Δθo；

步骤7 获取信息，若目的地位置发生变化，则将当前路径节点设置为目的地坐标；否则，按全局规划路径设置当前路径节点。

步骤8 依据当前路径节点，采用改进后的DWA算法的评价函数实时动态规划，直到到达当前目的地；

步骤9 判断当前目的地是否为出发点，满足条件，则运动结束；否则准备移动下一个目的地点，执行步骤5。

图6 算法流程

5 多目的地仿真实验

为验证本文改进算法的可行性和有效性，设计了多目的地路径规划仿真实验；为方便对比研究，假定移动机器人工作环境栅格边长为单位1 m，地图模型为40×40，其改进A*中的参数取点步长k=0.1,设安全距离D=0.8 m。

(1)多目的地静态全局路径规划仿真实验

为验证多目的地路径规划算法能提高搬运机器人的运输效率，设置了8组仿真实验，每组实验的目的地个数逐渐增加。实验得到移动路径长度如图7所示；其中8个目的地的规划路径对比如图8、图9所示。图中S点表示机器人的搬运地点，Ti点(i=1,2,3,4,5,6,7,8)表示搬运的目的地，黑色栅格区域表示障碍物，实线表示规划的路径。

图7 路径长度对比

图8 8次单目的地路径规划

图9 多目的地路径规划

由图8可知单目的地路径规划导致移动机器人需要多次回到起始点，降低了机器人的运输效率；图9中本文多目的地规划的路径一次规划可到达所有目的地，减少了运算路径的距离。由图7路径长度对比图可知，随着目的地个数增加，单目的地路径规划算法规划的路径长度增长很快，而本文设计的多目的地路径规划算法规划的路径长度增长平缓，与单目的地路径规划算法相比，本文平均长度减少了44.2%的长度，提高了移动机器人的工作效率。

(2)多目的地动态路径规划仿真实验

为验证多目的地局部动态规划的可行性及有效性，本文设计了在基于改进A*算法和姿态调整等其它相同的情况下，对比传统DWA算法和本文改进DWA算法的不同评价函数下局部动态避障能力。其中实验参数设置如下：最大速度为1.0 m/s，最大角速度为20.0°/s，最大线加速度为0.2 m/s2，最大角加速度为50.0°/s2，预测周期为3.0 s;对比评价函数参数α=0.3，β=0.4，γ=0.3，本文改进的评价函数参数α=0.3，β=0.1，γ=0.3，λ=0.4。

在40×40的地图模型中，设置两个目的地，仿真实验结果如图10、图11所示，图中S点表示机器人的搬运地点，Ti点(i=1,2)表示目的地，T’2点表示中途目的地发生变化后的位置；黑色栅格为已知的障碍物，灰色为未知的障碍物；虚线为静态全局规划的路径，实线为移动中局部动态规划的路径，移动顺序为S→T2→T1→S。仿真实验结果如图12、图13所示，图中实线表示线速度随控制节点变化情况，虚线表示角速度随控制节点变化情况。

图10 未改进DWA局部避障规划

图11 改进DWA局部避障规划

由图10和图11移动的路径对比可知，在全局静态规划相同的情况下，实际移动中传统的DWA算法目的地位置发生变化后不能及时重新规划，到达原目的地后才重新规划到达新的目的地，导致移动路径增大；且易受静态障碍物的影响，移动路径转弯幅度大，没有充分利用全局规划的直线路径，增大了移动距离。而本文改进后的DWA算法按照全局规划的路径移动时，移动路径转弯幅度小。且及时动态规划移动到新的目的地，减少了不必要的移动路径。

图12 传统DWA线/角速度变化

图13 改进DWA线/角速度变化

由图12和图13移动中线速度变化情况可知，到达目的地的过程中，传统动态窗口的线速度震荡次数多、最大震幅变化是稳定线速度的71.2%；其角速度一直变化中，其最大值达到了运动限定的最大角速度。而本文改进后的动态窗口法的线速度震荡次数少，且振幅变化的最大值只有稳定线速度20.5%；且角速度变化都在限定的速度内，振幅次数减少；没有产生不必要的路径，机器人的移动时间减少了22.6%。实验结果表明，本文设计的局部动态规划使移动机器人安全、准确的到达了每个目的地。运动过程没有频繁加减速，线速度稳定值高，提高了移动机器人的工作效率。

6 结束语

本文针对多目的地路径规划场景，设计一种基于改进A*、模拟退火策略和改进动态窗口的多目的地路径规划算法。首先，采用A*算法进行两点间的路径规划，并对路径节点进行双向平滑度优化。然后，引用模拟退火策略对所有规划完的路径进行优化排序，得到一条全局最优路径。最后，对动态窗口算法评价函数进行改进，并将优化后的路径节点和目的地位置动态设置为当前路径节点，使移动机器人稳定的按照全局路径移动，同时能及时避碍和准确移动到目的地。仿真结果表明，本文设计的多目的地路径规划算法在全局规划中的路径长度比单目的地路径规划算法平均长度减少了44.2%，提高了移动机器人的工作效率。局部动态规划中比传统动态避障的线速度震荡次数平均减少了50%，运动时间减少了22.6%；有效提高了移动机器人移动速度的平稳性和工作的效率。