复杂环境下基于多目标粒子群的DWA路径规划算法

李薪颖，单 梁，常 路，屈 艺，张 永

(南京理工大学 自动化学院, 江苏 南京 210094)

随着人工智能技术的深入发展，基于自动化技术的无人机、智能机器人等已广泛应用于家居、医疗、安防等多个领域。特别是在新冠肺炎疫情期间，采用无人系统技术的智能防疫机器人能够实现无人化消毒和安全化防疫，具有快速机动、远程遥控的特点。这些无人系统在为人们提供服务的同时都具有良好的避障性能，但是在布局复杂、密集程度高、实时响应需求高的多障碍物环境如危化品仓储环境中，机器人需要根据环境的特殊性自主规划路径，同时兼顾速度和安全性，实现智能化导航。

移动机器人的路径规划是实现自主导航的关键技术之一，根据工作环境的不同可以分为全局路径规划和局部路径规划。全局路径规划是在地图信息完全已知的整体环境中寻找最优路径，主要方法有基于图搜索算法的Dijkstra算法[1]、A*算法[2]和基于采样方法的快速扩展随机树(rapidly-exploring random trees，RRT)算法[3]以及基于智能仿生算法的粒子群算法[4-5]、蚁群算法[6]、遗传算法[7]等。局部路径规划则可以在掌握部分环境信息或对环境信息完全未知的情况下实现避障规划，能更好地适应各种复杂或动态的环境，主要算法有人工势场法、动态窗口法(dynamic window approach，DWA)等。人工势场法[8]利用虚拟力场中目标点的吸引力和障碍物的斥力共同驱动机器人运动，但未考虑到机器人的动力学约束且存在局部最优的问题。动态窗口法[9-11]根据当前速度和加速度设置速度预选窗口，然后由评价函数从航向角、避障、速度三个方面筛选出最优速度，不仅充分考虑了机器人的物理限制和环境约束，而且保证了路径规划的完备性和实时性。

然而在障碍物分布密集的复杂环境中，采用固定权重值的基本DWA算法具有一定的局限性，容易出现机器人避障过度或避障失败的问题，降低了算法的成功率。文献[12]将相互速度障碍法与动态窗口法进行融合，并对复杂环境中的障碍物做自适应膨胀处理，既扩大了可选速度的范围，又保障了机器人运动过程中的安全距离。文献[13]针对密集障碍区的路径规划提出了速度权值自适应性动态窗口法，根据复杂环境中的障碍物信息动态调整速度评价函数的权重值，优化了复杂环境中的避障效果。文献[14]在对评价函数进行修改和扩展的基础上利用强化学习对DWA参数进行自适应性变化，有效地优化了DWA的路径规划效果和避障性能。

基于以上分析可知，机器人运动过程中与目标点和障碍物间距离的动态变化，使得DWA算法对各子评价函数的占比提出了时变的要求，因此需要优化权重组合。本文在分析复杂环境中障碍物的分布信息的基础上，提出了基于多目标粒子群优化算法(multi-objective particle swarm optimization，MOPSO)的改进DWA算法。改进的算法采用多目标优化的思想在评价函数的基础上将安全距离和速度作为目标函数，并以此建立权重系数的多目标优化问题的数学模型，再利用粒子群算法迭代收敛获得权重最优解，实现权重组合的动态变化。这样不仅能确保机器人安全有效地通过复杂障碍物区，而且可以提高算法的环境适应性。

1 基础理论算法

1.1 DWA算法基本原理

基本DWA算法的主要思想是将机器人的位置控制转换为速度控制，从机器人运动学、电机动力学以及安全性三个方面对采样速度进行约束，然后通过式(1)的评价函数确定采样窗口中的最优轨迹[15]：

G(v,ω)=K[α·H(v,ω)+β·D(v,ω)+γ·V(v,ω)]

(1)

以运动轨迹的末端为机器人的参考位置，式中H(v,ω)为目标方位角评价函数，分别计算机器人在参考位置的方向角和参考位置到目标点的夹角，根据两角度之差来衡量轨迹朝向目标点的程度；D(v,ω)为机器人在参考位置与障碍物的最小距离，用来衡量该轨迹远离障碍物的程度；V(v,ω)为当前机器人的前进速度；α、β、γ为三个子评价函数对应的权重系数；K表示归一化。

1.2 多目标优化问题

不失一般性，设具有n维决策变量、m维目标变量的多目标优化问题表示为：

(2)

在该优化问题中当可行解x1、x2满足式(3)，即x1在所有目标上都不差于x2，且至少在一个目标上严格优于x2，则称x1为非支配解，x2为支配解，x1是Pareto占优的。

(3)

多目标优化问题中的Pareto最优解集是由多个非支配解组成的，其对应的目标向量的集合称为Pareto前沿。在搜索最优解的过程中需要令这些解集尽可能地逼近Pareto前沿，且要满足分布性和多样性。

1.3 多目标粒子群算法

Coello等[16-17]将Pareto占优思想与粒子群算法相结合，提出了多目标粒子群优化算法，充分发挥了粒子群算法在多目标优化问题中的并行搜索作用，在保证非劣解多样性的基础上改善了算法的局部搜索能力，加快了算法的收敛速度。

MOPSO将搜索过程中产生的非支配解保存在外部存储器中，并从中选择全局最优位置来引导其他粒子在可行域中搜索，最终通过外部存储器中的最优解不断逼近Pareto最优前沿实现收敛。MOPSO中粒子i的位置更新过程如下：

(4)

式中，I表示迭代次数，w表示惯性权重，pbest表示粒子的个体最优位置，gbest表示当前整个种群的最优位置，c1和c2是学习因子，r1和r2为[0,1]的随机数。

2 改进的DWA算法

传统DWA算法因采用固定权重组合能在简单环境中有效地实现路径规划，但也会因此难以适应障碍物分布密集的复杂环境，容易出现路径规划不合理、安全和速度不能兼顾的问题，而本文提出的改进算法通过动态调整权重系数可以对DWA算法进行根本上的优化。首先建立运动环境中复杂障碍物的环境覆盖模型，并提出一种障碍物密集度的判断方法；接着对DWA中的子评价函数进行修正；然后建立权重系数的多目标优化问题的数学模型，在评价函数的基础上确定安全距离和速度的局部目标函数，同时用粒子群算法获得轨迹空间中的最优轨迹。该改进算法能有效地提高运行过程中的安全距离，优化DWA算法在复杂环境中的规划效果。

2.1 多障碍环境覆盖模型的建立

机器人在向目标位置运动过程中会遇到多种障碍物，包括具有特殊形状的几何形障碍物和不能用简单的语言或数学表达式模拟的不规则障碍物。针对机器人易陷入凹点造成的局部最优问题以及凸点造成的规划路径不平滑、多折点的问题，对已知环境中的障碍物作“膨化”处理，建立多障碍环境覆盖模型。

对于图1中规则的几何形障碍物，如矩形等，在最小外接圆的基础上进行圆形膨化，膨胀尺寸取机器人的运动半径R，得到如图1所示的圆形覆盖模型，半径记为R′。

图1 规则障碍物圆形覆盖模型Fig.1 Circular covering model of regular obstacles

(5)

障碍物上各边界点(xi,yi)(i=1,2,…，n)到质心的距离平方和可以表示为：

(6)

令距离平方和P最小，利用最小二乘法可以求得倾斜角θ(式(7)所示)，代入式(5)即可得到长轴，与之垂直的直线即为短轴。

(7)

根据长短轴直线方程可以确定障碍物在上下左右四个方向距离长短轴最远的边界点，得到最小外接矩形。在此基础上考虑机器人的运动半径R，对外接矩形作“膨化”处理，得到图2中的红色矩形框。为了使膨化后的障碍物更符合实际情况，截取矩形的最大内切椭圆作为障碍物的覆盖模型。因此图2中的蓝色椭圆即为该障碍物的椭圆环境覆盖模型，a、b分别为椭圆的长短半轴。

图2 不规则障碍物椭圆覆盖模型Fig.2 Elliptical covering model of irregular obstacles

2.2 障碍物密集区域判断方法的提出

根据传感器信息，机器人可以获得一定范围内的障碍物分布情况，因此以机器人前进方向一定角度内的扇形区域来判断障碍物的密集程度。在图3(a)所示的多障碍环境中，由2.1节可以得到图3(b)所示的多障碍环境覆盖模型，圆形覆盖模型半径为R′，椭圆覆盖模型长半轴为a。在t时刻，以机器人运动的方位角θ(t)为对称轴构建圆弧角2σ、半径r的扇形区域，设该区域涉及的障碍物个数为no。当相邻两个障碍物之间的距离Dob满足Dob-d1-d2≤R时(若为圆形覆盖模型则d=R′，若为椭圆覆盖模型则d=a，R为机器人的运动半径)，定义这些障碍物组成的区域为密集障碍物区，该区域中障碍物的个数记为M。若no超过阈值M，则判断此时机器人进入障碍物密集区。图中根据机器人当前位置形成的扇形区域中的障碍物有no=4个，而M=3，因此此时机器人进入障碍物密集区。

(a) 原始障碍物环境(a) Original obstacle environment

(b) 多障碍环境覆盖模型(b) Multi obstacle environment coverage model图3 障碍物密集区域的判断示意Fig.3 Schematic diagram of judgment in areas with dense obstacles

2.3 改进DWA子评价函数

为了使预测的路径更符合实际情况，在计算评价函数时对模拟轨迹上参考位置的选择方式进行调整优化。在基本DWA算法中，计算子评价函数H(v,ω)和D(v,ω)时的参考位置都是模拟轨迹的末端，且该位置是由向前模拟时间T(T∈[10Δt～30Δt])[18]确定的，即机器人按当前速度持续运动T时间之后的位置。但实际上Δt后机器人的速度便会改变，即模拟轨迹上只有前方一小段是机器人实际会运动到的位置。所以为了防止参考位置离原位置过远失去参考价值，需要分别确定H(v,ω)和D(v,ω)中的向前模拟时间T。

在H(v,ω)函数中，定义移出距离dh，则按照直线运动可由当前速度v确定时间Th，即Th=fix(dh/v)，其中fix(·)为取整函数。修正后的H′(v,ω)函数将以模拟轨迹上Th个时间步的位置进行计算，计算形式如式(8)所示：

H′(v,ω)=180°-|Htheta(XTh,Ggoal)-Htheta(XTh)|

(8)

式中，Htheta(XTh,Ggoal)表示Th个时间步后机器人所在参考位置XTh与目标点Ggoal之间的夹角，Htheta(XTh)表示机器人在参考位置XTh时的方向角。

同样，在D(v,ω)函数中，定义移出距离dd，时间步Td=fix(dd/v)。由于D(v,ω)函数的目的是为了避障，因此dd的取值要略大于dh，且需要舍弃那些离障碍物最短距离小于安全半径R的轨迹。修正后的D′(v,ω)函数如式(9)所示：

(9)

式中：Ddm(XTd,O)表示Td个时间步后机器人所在的参考位置XTd与障碍物O的最短距离；Dmax为障碍物距离上限，即无须考虑过远的障碍物。

2.4 基于改进MOPSO的DWA算法

2.4.1 改进的MOPSO算法

在多目标优化问题中，粒子群算法的高收敛速度可能会使算法陷入局部最优，收敛到伪Pareto前沿，因此在多目标粒子群算法中引入变异算子。随着迭代次数的增加，粒子产生变异的概率降低，这样可以使粒子在算法初期具备高度探索性。

(10)

改进后的MOPSO的实现步骤如下：

步骤1：初始化粒子的速度和位置。

步骤2：根据目标函数计算初始粒子的适应度值。

步骤3：确定非支配解，将非支配解存入外部存储器。

步骤4：确定全局最优解gbest和个体最优解pbest。

步骤5：按照式(4)更新粒子的速度和位置信息，判断是否执行变异操作并对粒子的位置信息做进一步的更新。

步骤6：重新确定非支配解，更新外部存储器。

步骤7：判断算法是否停止，若满足终止条件，输出外部存储器中的非支配解，否则返回步骤4。

2.4.2 MOPSO-DWA算法

为了满足DWA算法在航向、安全和速度等多方面上的要求，本节利用改进的MOPSO算法获得密集障碍物区中的最优权重系数组合，根据不同的环境信息动态调整各子评价函数比重，实现路径规划在合理性、安全性和高效性上的统一。

在t时刻根据速度采样空间可以得到一组预测轨迹J=(J1,J2,…)，其中第s条轨迹对应的信息记为Js=(Hs,Ds,Vs)。由于DWA算法是利用评价函数计算采样空间中的轨迹在航向、安全和速度上的整体得分，但是在面对障碍物分布集中的复杂环境时，这种综合性计算方法容易忽略路径规划在安全性或速度上的局部要求，所以在建立权重系数的多目标优化问题的数学模型时，以评价函数为基础构建如式(11)所示的目标函数。式中，评价函数G*可以获得评价函数得分高的轨迹，使路径更合理；安全距离函数D*可以使轨迹远离障碍物，使路径更安全；速度函数V*是为了确保机器人能以安全合适的速度通过密集障碍区。

(11)

根据DWA中评价函数的具体要求可以确定如式(12)所示的约束条件。

(12)

利用改进后的粒子群算法对该多目标优化问题进行分析，可以确定种群中存在n个三维粒子，粒子每一维的位置信息对应于权重系数(α,β,γ)，迭代优化后可以得到权重系数Pareto最优解集 (α*,β*,γ*)及其对应的Pareto前沿(G*,D*,V*)。由于机器人在障碍物密集区域内运动范围有限，且移出距离d较小，所以各条预测轨迹的末端与障碍物的最小距离D′(v,ω)相差不大，因此只需要从优化后的轨迹空间中选择评价函数得分高且速度低的轨迹J*，再由运动模型即可确定机器人下一时刻的位置。

3 仿真验证与分析

3.1 算法流程

基于上述改进可以得到如图4所示的优化后的DWA算法流程。与传统DWA算法相比，改进后的DWA算法能明确地判断出复杂环境中的障碍物密集区，并且在经过该区域时能够通过MOPSO算法计算出不同环境下权重系数的最优组合，既能使机器人在离密集障碍物较远处保持高速，又能规划出复杂地形下的安全路径，提高了算法的环境适应能力。

图4 改进的DWA算法流程Fig.4 Flow chart of improved DWA algorithm

3.2 仿真分析

3.2.1 仿真1

仿真1为与基本DWA算法的对比。构建如图5所示的环境地图，设定该环境下的障碍物均为规则的几何形障碍物，在用圆形覆盖模型表示的基础上将原始障碍物抽象为一个质点。由文献[15]可知，基本DWA算法在权重系数α=0.8，β=0.1，γ=0.1时易取得较好的避障效果，据此得到如图5(a)所示的规划路线。在本文提出的改进DWA算法中，取H(v,ω)的移出距离dh=0.6 m，D(v,ω)的移出距离dd=0.8 m，取非障碍物密集区的权重系数为α=0.8，β=0.1，γ=0.1，得到如图5(b)所示的路线，路径长度6.177 m，安全距离0.227 2 m。对比两种结果可知，基本DWA算法中的固定权重组合会令机器人受目标方位角的影响而使避障权重减小，进而在障碍物前停下来，无法绕过障碍物；而改进后的DWA算法使机器人在面对由障碍物2、障碍物4和障碍物3组成的通道时能够降低方位角权重α，增加避障权重β，顺利绕过障碍物完成路径规划。

(a) 基本DWA算法的路径(a) Path of basic DWA algorithm

(b) 改进的DWA算法的路径(b) Path of improved DWA algorithm图5 基本DWA算法和改进DWA算法路径规划对比1Fig.5 The first comparison of path planning between basic DWA algorithm and improved DWA algorithm

在图6所示的环境地图中，存在规则的几何形障碍物和不规则障碍物，分别建立相应的环境覆盖模型，实验参数与图5的仿真参数相同。改进的DWA算法路径长度为6.475 3 m，安全距离为0.082 5 m。在图6(a)中，多障碍环境覆盖模型的建立使得机器人没有陷入凹点造成的局部最优，但因方位角的权重系数α占比过大，无法避开障碍物，避障失败。而在图6(b)中，改进后的DWA算法能动态调整各权值系数的占比，并且适用于多个不规则障碍物分布的密集环境，适用范围广泛，成功率高。

(a) 基本DWA算法的路径(a) Path of basic DWA algorithm

(b) 改进的DWA算法的路径(b) Path of improved DWA algorithm图6 基本DWA算法和改进DWA算法路径规划对比2Fig.6 The second comparison of path planning between basic DWA algorithm and improved DWA algorithm

3.2.2 仿真2

仿真2为与文献[13]的对比。在图7和图8所示的规则障碍物分布密集的环境中，据文献[13]取速度权重自适应DWA的参数为α=0.55，β=0.35，γmin=0.1，γmax=0.8，得到如图7(a)所示的路线，路径长度为7.106 2 m，安全距离为0.036 7 m；在本文提出的改进的DWA算法中，取H(v,ω)的移出距离dh=0.7 m，D(v,ω)的移出距离dd=1 m，非障碍物密集区的权重系数为α=0.55，β=0.35，γ=0.8，得到如图8(a)所示的路线，路径长度为6.956 5 m，安全距离为0.090 3 m。两种算法在非障碍物密集区的权重系数一样，但是当机器人在准备进入障碍物密集区时，文献[13]提出的改进算法无法及时降低速度权值，导致机器人无法以较低的速度通过障碍物1和障碍物2之间的狭窄通道，并且在绕过障碍物3和障碍物4时受目标点的影响高速通过，使得规划路径安全距离变小，路径长度变长；而改进后的DWA算法不仅可以使机器人在离稠密障碍物较远处保持高速，而且可以在准备进入障碍物密集区前迅速地调整各权重系数，降低运行速度，确保机器人能安全高效地实现复杂环境中的路径规划。

(a) 速度权重自适应DWA算法的路径(a) Path of speed weight adaptive DWA algorithm

(b) 运行线速度(b) Running linear speed图7 文献[13]规划的路径及运行线速度Fig.7 Reference [13] planned path and running linear speed

(a) 改进的DWA算法的路径(a) Path of improved DWA algorithm

(b) 运行线速度(b) Running linear speed图8 基于MOPSO改进的DWA算法规划的路径及运行线速度Fig.8 Path planning and running line speed of improved DWA algorithm based on MOPSO

4 结论

针对传统DWA算法因采用固定权重系数难以适应多种复杂环境这一弊端，在修正评价函数的基础上提出了一种基于多目标粒子群的DWA算法。根据DWA算法在穿越稠密障碍物时对航向、避障和速度的多方面要求，建立了权重系数的多目标优化问题的数学模型，并利用改进后的粒子群算法进行迭代收敛，实现了权重系数的自适应变化。实验表明该算法通过动态调整各评价函数的权重使机器人获得了复杂环境中的最佳运动速度和更合理的路径，有效地改善了DWA算法在路径规划中的安全性和合理性。

本文现阶段只是对多静态障碍物的复杂环境进行分析规划，未来将进一步研究多动态障碍物的复杂环境下的路径规划，以及多机协同的路径规划算法等。