融合A*算法和动态窗口法的全局动态路径规划方法

于 洋，杜学历，冯迎宾

(沈阳理工大学 自动化与电气工程学院，沈阳 110159)

机器人路径规划是机器人实现自主导航的核心技术之一。机器人路径规划技术是根据某一个参数指标(如工作代价值最低，选择路径最短，运算时间消耗最短等)，在任务区域选择出一条可从起点连接到终点的最优或次优的避障路径。机器人路径规划环境分成两种：静态环境和动态环境。静态环境是在已知先验地图模型下，在静态障碍物中规划一条全局路径；动态环境是指通过机器人搭载的传感器感知周围环境中的动态障碍物，从而进行安全避障。根据规划环境研究路径规划，当前的机器人路径规划研究多采用全局路径和局部动态路径融合的方法[1-4]。

在静态环境下，最有效的路径规划算法是A*算法，该算法利用估计子函数在节点间快速搜索最优路径[5]。但A*算法规划的路径是通过节点进行连接，存在路径不连续的缺点，路径冗长，无法在动态环境下完成避障规划。程传奇等[6]改进了A*算法，剔除A*算法搜索的冗余节点及转折节点，缩短和平滑了路径，然后与动态窗口法融合进行局部安全避障。王小红等[7]针对机器人平滑及动态避障问题提出一种改进A*算法，将8邻域扩展到24邻域减少转折点，路径更加平滑，局部采用动态窗口法进行动态避障。王洪斌等[8]采用对A*算法搜索的节点上进行二次A*算法搜索，完成全局路径规划，局部采用人工势场法进行动态避障。王永雄等[9]提出参数自适应的动态窗口法，根据机器人与局部障碍物距离及密集度调整参数，兼顾了速度与安全性，但未考虑全局路径。

针对大面积的障碍物对已规划全局路径堵塞问题，以及涉及到机器人动力学时，速度与安全性无法同时满足的问题，本文提出一种融合改进A*算法和改进动态窗口法的全局动态路径规划。该算法集成了两种改进算法的优点，可以大量减少全局路径的冗余节点，缩短路径，遇到障碍物可及时重新规划全局路径，局部能够自适应调节速度，兼顾速度和安全性，路径更加平滑。

1 改进A*路径规划算法

1.1 A*算法

A*算法是基于节点的最佳搜索算法，可以快速搜索出节点路径。A*算法的核心是估计子函数，公式为

f(n)=g(n)+h(n)

(1)

式中：f(n)为第n个节点的估计子函数代价值；g(n)为第n个节点到初始节点的实际代价值；h(n)为第n个节点到目标节点的估计代价值。在机器人中代价值是指两节点之间的欧氏距离。

A*算法实现流程是：(1)在环境地图中，初始节点和目标节点设置，初始化OPEN集合表和CLOSE表；(2)选择OPEN表中最小估计子函数代价值的节点，进行邻域扩充，扩充后，该节点放置CLOSE表中，扩充的节点放置OPEN表中；(3)迭代第二步，直到找到目标节点；(4)在CLOSE表中，反向得到最终的路径节点集合，将节点连接，形成路径。

A*算法进行实验仿真，实验结果如图1所示，小圆圈是搜索节点，黑色实心圆是障碍物的膨胀区域，图中线段是路径。

从图1中可以看出，A*规划的路径中存在大量的折线、冗余节点及冗余转折点，且路径距离方面仍然存在可缩短的空间。

1.2 二次A*算法优化

针对A*算法的缺点进行优化，优化的方法是对路径中的节点进行二次A*算法搜索，剔除A*算法规划路径中节点及转折点，保留关键节点，减少路径距离。

优化算法步骤：首先以第一次A*算法搜索的节点作为新的节点进行搜索；然后在当前节点后的节点进行邻域扩充，扩充标准是中间不存在障碍物；最后根据估计子函数进行评定，不断的迭代，最终得到优化路径。通过对图1的A*算法规划的路径进行优化，得到图2所示的优化路径。

由图2可以看出，原A*算法中11个节点优化为4个节点，路径由12.48优化为11.7，转折点由7个优化为2个，节点、转折点数量大幅度减少，路径缩短。节点和转折点的减少，缩短路径，便于机器人实际运动控制及快速到达目标，所以本文采用二次A*算法优化后的路径作为机器人的全局路径。

2 改进动态窗口法

2.1 动态窗口法

动态窗口法是局部动态路径规划算法，将机器人的位置变化控制转化成速度控制，避障问题转变成空间中的运动约束问题，这样可以根据运动约束条件选择局部最优的路径。

(1)建立机器人运动模型

假设在Δt时间内，机器人可视为做匀速直线运动来模拟机器人运动，以其中一组状态值(vt，wt)进行轨迹推算，其运动模型为

(2)

式中：[xt+Δtyt+Δtθt+Δt]T是t+Δt时刻世界坐标系下的机器人位姿；[xtytθt]T是t时刻世界坐标系下的机器人位姿；[vxΔtvyΔtwtΔt]]T是t时刻机器人坐标系下的位姿变化。

(2)机器人速度的运动约束

机器人在实际运动中，存在自身的限制及传感器检测的周围障碍物距离的限制，速度的范围受到限制。

限制1：机器人自身最大速度、最小速度。

Vm={(v，w)|v∈[vmin，vmax]，w∈[wmin，wmax]}

(3)

式中：Vm为机器人自身最大速度、最小速度的限制下的速度范围集合；vmin、vmax分别为最小、最大线速度；wmin、wmax分别为最小、最大角速度。

限制2：机器人动力加速度约束。

(4)

限制3：安全距离限制，即机器人到障碍物之间，使用最大减速度可以在障碍物前停下来。

(5)

三个限制的交集即为机器人最终速度的范围V为

V=Vm∩Vr∩Va

(6)

(3)速度采样与轨迹推算

在速度空间中进行采样，采样出不同组的速度，然后根据机器人运动模型预测机器人的轨迹，通过评价函数对这些轨迹进行最优选择，其评价函数是

(7)

式中：G(v，w)为机器人在速度(v，w)下的轨迹评价值；heading(v，w)为机器人运动方向与目标点之间的夹角；dist(v，w)为机器人与障碍物最小的距离；v(v，w)为机器人当前的速度值；α、β、γ分别为机器人指向权值参数、安全距离权值参数、速度权值参数。heading(v，w)、dist(v，w)、v(v，w)都是归一化后的数据。

(4)当前最优速度选择

通过对不同组速度推算的轨迹进行评价，取评价函数值最大的(v，w)进行控制机器人运动。

2.2 自适应动态窗口法

在机器人运动中，目标是机器人快速运动。在动态窗口法中，速度控制是由γ参数决定，如果γ设定过大，所需时间短，步数少，但机器人的安全性降低；如果γ设定过小，所需时间长，步数多，但机器人安全性较高。

为在整体上兼顾机器人的安全性和速度，采用一种基于障碍物信息的动态参数调整方法进行机器人速度权值参数γ的调整。整体思想是：根据机器人自身所带有的传感器对周围信息的获取，对机器人与周围障碍物的密集度及距离进行自适应设定权值，这样可以在安全区域有较大的速度；危险区域，降低速度，增加安全性。

(1)检测障碍物安全距离

一般机器人传感器检测角度为扇形，假设当检测的障碍物数量为m时，当m大于阈值，表示进入密集障碍区，本次设定阈值为2。检测该范围中障碍物与机器人距离及角度，然后计算不同障碍物之间的距离Intij，其计算公式为

(8)

式中：Di、Dj是第i、j个障碍物与机器人的距离；θi、θj是机器人与第i、j个障碍物朝向的角度值。

通过两点间的Intij值判断机器人是否能够通过这些障碍物，当Intij值大于两个障碍物膨胀距离的2倍，表示可以通过这两个障碍物。

(2)计算自适应阈值

基于安全考虑，机器人与障碍物最短的距离Dmin作为自适应动态函数的输入，当Dmin大于阈值Ds时，机器人选择最大的权值。根据机器人自身来定义阈值Ds。

(9)

(3)自适应函数计算

动态的速度权值与输入Dmin关系是

(10)

式中：γd是输出的动态速度权值参数；γmin是输出的最小速度权值；γmax是输出的最大速度权值；k、a分别是调整参数。

机器人在Ds大于Dmin时，动态的速度权值与输入Dmin关系是单调递增的。其中γmax、γmin的取值方法是：γmax是安全通过密集障碍物最大的速度权值，γmin是通过狭窄的通道且最安全的速度权值。

(4)调整评价函数

调整后的动态窗口法的评价函数是

(11)

设置速度权值参数γ=2、γ=20、γ=γd进行实验，实验中其他参数不变。实验结果如图3所示，分别得到了总路程、耗时、步数、安全距离的数据。

图3a、3b、3c三个实验的数据分析如表1、表2所示，可以得到速度权值参数γ=2、γ=20、γ=γd的对比结果。

表1 图3a与图3c实验数据分析表

表2 图3b与图3c实验数据分析表

根据表1、表2可知，速度权值参数γ=γd与γ=2实验效果相比，保证机器人安全性的基础上，降低机器人步数及耗时，快速达到目标点；速度权值参数γ=γd与γ=20实验效果相比，保证速度情况下，大幅度提升机器人的安全性。故机器人在局部路径规划时，采用自适应动态窗口法可以兼顾速度与安全。

3 融合算法

使用改进的A*算法进行全局路径规划，在局部规划时采用改进动态窗口法，通过传感器检测周围信息，实时规划最优路径，如果遇到大面积动态障碍物堵塞全局路径规划的路径时，重新规划全局路径，防止堵塞的全局路径对局部实时路径规划造成较大的影响。在大面积动态障碍物环境中，融合算法可以保证机器人在整体规划最优，又可以兼顾机器人速度与安全性。

融合算法的实现步骤是：

(1)在静态离散地图中，采用二次A*算法优化后的关键节点作为全局路径；

(2)将关键点作为机器人运动的局部目标点，当机器人与关键点距离超过0.5m时，设定下一个关键点作为局部目标点；

(3)传感器检测到动态障碍堵塞目标关键点或规划的全局路径时，重新进行步骤(1)规划；

(4)局部的目标点作为自适应动态窗口法的目标输入，输出的局部速度值进行轨迹预测，使用评价函数选择最优轨迹对应的速度运动；

(5)是否到达局部目标点，如果没有到达局部目标点，迭代执行步骤(4)，直到到达局部目标点；

(6)是否到达目标节点，如果没有到达，执行步骤(2)，通过不断地迭代，直到到达目标节点。

融合算法的流程如图4所示。

4 实验

为验证算法的有效性，在Matlab 16.0软件实验环境下，构建一个离散化的地图，构建的面积为12m×12m，其中的障碍物进行0.5m膨胀，障碍物分成静态障碍物和动态障碍物，采用黑色圆代表静态障碍物膨胀后的区域，白色圆代表动态障碍物膨胀后的区域。

实验的起始点、目标点分别为世界坐标系下的(1，1)、(9，9)；动态窗口法初始参数是：位置坐标(1，1)，方向90°，初速度为0，角速度为0，评价函数参数为α=1，β=5，γ参数实验时设定；机器人自身参数为速度(0～1.0)m/s，速度变化率0.02m/s，最大加减速度是0.4m/s2，角速度是(-50～+50)°/s，最大角加速度是80°/s2，角速度变化率1°/s，轨迹采样时间是0.05s，轨迹预测时间是3s，自适应动态窗口法的参数为γmin=2，γmax=20，a=1，k=1，l=0.9。

实验分成两部分进行对比，实验1部分进行无动态障碍物堵塞全局路径和有动态障碍物堵塞路径的实验；实验2部分进行兼顾安全性和速度实验，分别采用固定的γ参数和自适应的γ参数。

实验1部分是分别对本次提出的算法进行无动态障碍物的实验和有大面积动态障碍物的实验。实验中仅仅是实验环境改变，动态窗口法评价函数中各个参数不变，分别是α=1、β=5、γ=γd，具体实验结果如图5所示。

图5a中是无动态障碍物的实验，实验中不存在动态障碍物，所以已经规划的全局路径不会发生改变。图5b是在实时运动过程中，检测到原规划的路径被障碍物堵塞，以当前点为起点进行重新规划最短路径，直到机器人到达目标点。

根据实验1可以看出，出现大面积障碍物时，实时规划全局路径，可以有效解决大面积的动态障碍物问题。

实验2是进行速度与安全的兼顾性实验，实验环境采用与图5b一致的大面积动态障碍物的环境，全局路径一致，障碍物一致，仅速度权值参数γ不同。

动态窗口法评价函数中速度权值参数γ=2、γ=20分别是最安全和速度最快的权值参数值，分别对这两个参数进行实验，具体实验如图6所示。

图6a、6b实验结果分别与已经完成的图5b实验数据进行分析，对比分析如表3、表4所示。

表3 图6a与图5b实验数据分析表

表4 图6b与图5b实验数据分析表

根据实验2中γ=2与γd进行对比分析，该算法保证安全性的基础上，可以减少运动的步数、消耗时间；γ=20与γd进行对比分析，该算法可以大幅度提高机器人的安全性。

由实验1、实验2可知，该算法解决了大面积动态障碍物堵塞全局路径的问题，同时改进局部的动态窗口法，得到一条兼顾速度和安全的路径。

5 结论

针对机器人在规划全局路径后，动态场景中会堵塞全局路径的问题，以及运动中与障碍物的安全距离和运动速度的兼顾性问题，本文设计一种全局动态路径规划方法；实验表明：该全局动态路径规划方法可以解决大面积动态障碍物规划问题，且在通过大面积障碍物时，机器人能够兼顾安全与速度。