融合优化A*算法与动态窗口法的动态路径规划算法研究

姚进鑫,刘丽桑,3,何栋炜,3,陈 健,王 斌,徐 辉,郭江峰,陈 炜,3

(1.福建工程学院 电子电气与物理学院，福州 350118；2.福建工程学院 福建省工业集成自动化行业技术开发基地，福州 350118；3.福建工程学院 电子信息与电气技术国家级实验教学示范中心，福州 350118)

0 引言

路径规划是机器人研究领域必不可少的一项技术,近年来该技术正逐渐应用于智能物流、智能家居、海上搜救、巡航等领域[1-2]。所谓路径规划，就是让机器人在遵循一定的评价指标函数的前提下，在充满障碍的环境中能够顺利避开障碍物，从而规划出一条从起始位置到目标位置的最优路径[3]。随着无人驾驶和人工智能等新兴领域的出现，国内外的学者们研究了许多种不同的路径规划算法。根据规划所针对的目标范围的不同，可以将其分为全局路径规划和局部路径规划。其中A*算法、Dijkstra算法、D*算法等均为较为常见的全局路径规划算法，而局部路径规划算法则有动态窗口法、人工势场法、遗传算法等[4-6]。

由于标准A*算法在运行时需要从起始点开始，不断地对相邻节点进行扩展，并利用定义的代价估计函数来寻找使函数取得最小值的节点，因此该算法的耗时主要在于对相邻节点代价估计函数的计算以及对最小的节点的选择。如此一来，标准A*算法就会存在一些不可避免的缺陷：随着机器人所处的二维空间的增大，算法扩展的无用节点不断增加，使得计算数据量与算法搜索时间不断增加，寻路效率不高。赵晓等[7]将跳点搜索法与A*算法相结合，对A*算法进行优化，通过筛选得到的关键跳点间的直线跳跃到达目标点，得到最终路径。这种方法虽然跳过了A*算法中扩展的大量无用节点，从而减少算法的计算量，提高了路径规划速度，但是所规划出来的路径曲率变化率不连续，转折处不够光滑，且无法实现动态避障。张敬寒等[8]通过扩大其算法的搜索邻域并采用最小二叉堆的方法来优化标准A*算法，进一步缩短和减少了其路径长度和拐点的数量，并通过3次均匀B样条曲线使路径得到进一步平滑，提高了路径规划效率，但没有考虑到其路径规划的实时性和对于动态障碍物的避障性能。而动态窗口法是在采样的多组速度中考虑速度与单位时间内的速度变化量组成的约束条件，并在模拟生成的运动轨迹中找到使评价函数取最大值的轨迹所对应的速度驱动机器人。但在障碍物密集等特殊情况下，存在陷入局部区域而无法到达目标位置的致命问题，这也是局部路径规划算法普遍存在的问题之一。王洪斌等[9]通过引入二次A*算法，并结合目标成本函数对所有目标点进行优先级判定，同时采用自适应圆弧优化与加权障碍物步长调节算法，有效地缩短和减少了路径长度和转折次数，最后通过预瞄偏差角追踪算法改进动态窗口法来对动态障碍物进行捕捉，提升了路径规划效率。程传奇等[10]则在动态窗口法的基础上，根据关键点选取策略筛选出少量转折点设计了一种能够满足全局最优的评价指标，并将该指标作为动态窗口法的评价函数，以此来提高路径的平滑性以及动态避障能力。但由于其关键点选取策略无法应对复杂场景下的实时路径规划，使得该算法存在一定的局限性。

本文首先基于跳点搜索法对A*算法进行优化，然后与动态窗口法相结合，提出了一种将优化A*算法与动态窗口法相融合的混合路径规划算法。一方面由于跳点搜索法的加入，使得优化后的A*算法在静态路径规划的速度上得以提高，另一方面该混合算法在传统动态窗口法的基础上，结合跳点搜索法所获得的跳跃点信息提出了一个新的方位角评价指标，经这一评价指标优化后的评价函数使得机器人在动态环境中也能寻找到一条全局最优路径，克服了原算法易陷入局部最优的弊端。

1 基于跳点搜索的优化A*算法

1.1 A*算法

A*算法是在静态的二维配置空间中用于计算最优路径的启发式搜索算法，它综合了经典的Dijkstra算法与启发式的最佳优先搜索(BFS)算法的优点，通过代价评估函数搜索最优路径节点作为下一个需要遍历的节点,重复这一过程，直到发现目标点，形成最优路径[11]。其中，代价评估函数为：

f(m)=h(m)+g(m)

(1)

式中:f(m)表示当前位置m代价评估函数；g(m)表示机器人从初始位置到达当前位置m的实际代价；h(m)表示机器人从当前位置m到目标位置的估计代价；h(m)的选择直接影响了A*算法的成功率与准确性，其值的大小与实际值越接近，搜索效率就越高。

如图1所示为A*算法规划的路径。

图1 A*算法规划路径

从图1中的绿色网格开始，规划一条到达红色网格的最优路径，浅蓝色网格为最终规划出的最优路径，浅绿色网格为在规划过程中搜索过的节点。可以发现，A*算法存在的某些不足之处[12]：由于A*算法在寻路时，需要不断地对当前节点的8个相邻节点进行搜索，并计算其估计代价，但实际上只有少量节点与最终生成的路径有关，需要进行必要的计算，而大部分的节点与最终生成的路径毫无关联，不必对这些无用节点进行访问。这一缺点使得这些无用节点随着配置空间的增大而增多，不仅消耗内存，也降低了寻路效率。

1.2 跳点搜索法

跳点搜索法(jump point search)是由Daniel Har-abor和Alban Grastien提出的一种新的寻路算法[13]。与A*算法不同的是，该算法中引入了跳点的概念，通过一定的筛选规则选出那些表征方向变化的特定节点后，机器人只按照这些特定节点进行跳跃式前进,这些特定节点也称为跳点。对于跳点的搜索，删除空间网格中的那些无用节点是跳点搜索法的核心，也叫剪枝，陶凯[14]提出了许多剪枝规则。根据节点的8邻域障碍物的有无，可以将其大致分为2种情况进行阐述。

1.2.1节点邻域没有障碍物

当某一节点的周围8邻域节点中不存在障碍物时，则该节点将沿直线和对角线方向搜索，如图2所示。当该节点向直线方向搜索至当前节点x时，如图2(a)所示。图中点p为当前位置节点x的父节点，会发现一个现象：若以p点为起点，直接抵达网格标为灰色的节点(这里假设抵达节点n)，得到路径(图中虚线箭头)所花费的时间要优于同样从p点出发的一条途径当前位置x到达灰色节点的路径长度。如果我们在此过程中去评估这条路径p→x→n则会增加其所带来的耗时成本。因此，当由p点水平向右搜索到当前位置x时，只需考虑y节点即可。A*算法中正是因为评估了大量类似灰色网格这种毫无意义的节点，无形中增加了计算量，降低了搜索效率。所以，假设在当前位置节点x的8邻域内任取一节点n为目标节点,若要规划一条起点为p、目标为n且耗时成本最优的路径，在向水平和垂直方向直线扩展时，显然，只有满足以下约束所生成的路径p→x→y才是最优路径[15-16]。

图2 节点的8邻域没有障碍物

length(p,…,n|x)>length(p,x,n)

(2)

length(p,x,n)衡量的是从出发点p到目标点n且经过x的路径长度；length(p,…,n|x)衡量的同样是从出发点p到目标点n的路径长度，但这条路径绕过了x。

同样，当节点的扩展为对角线方向时，如图2(b)所示。同样，只有满足以下约束所生成的路径p→x→y才是最优路径[7]。

length(p,…,n|x)≥length(p,x,n)

(3)

由此，可得到一条剪枝规则，即当节点领域内不存在障碍物时，应剪去不满足约束条件(2)和(3)的灰色网格节点，而满足约束条件白色网格(如图中的y，y1，y2，y3)则被称作节点x的自然邻节点。

1.2.2节点邻域有障碍物

在算法运行过程中，总会遇到邻域内存在障碍物的节点。如图3(a)和3(b)所示，节点同样可以向直线方向和对角线2个方向进行搜索，且同样需要剪去图中的灰色节点。不同的是，图2中节点n属于非自然邻节点，而图3中节点n却不必执行剪枝操作，需将其标为白色。因为在图3中的节点x上方遇到了障碍物，此时只有以p点为起点，并经过当前位置x到达节点n得到的路径才是所有可行路径中的耗时成本最优路径，因此，将这一节点叫作强制邻节点[15-16]。于是，得到强制邻节点需要满足的条件：

图3 节点的8邻域有障碍物

(4)

由此可得到在遭遇障碍物时的剪枝规则，即当扩展的节点领域内遭遇障碍物时，应剪去不满足约束条件(2)和(3)且除去满足约束条件(4)的强制邻节点n以及障碍物节点以外的剩余灰色节点。满足该条件的节点x就是算法中所要寻找的跳点。也就是说，一个所谓的跳点其实就是一个邻域内含有强制邻节点的网格节点[16]。

1.3 基于跳点搜索的优化A*算法

在剪掉那些无意义的节点后，便得到了一系列的跳跃点。对这些跳跃点执行标准A*算法的操作后，便可快速地得到全局路径。优化的A*算法的流程如图4所示。

图4 优化A*算法流程框图

对于优化A*算法中距离的度量方式，使用较多的度量方式有欧式几何距离或曼哈顿距离。在平面直角坐标系中任取两点(pi,qi)和(pj,qj)，则这两点之间的最短直线距离DEij称为欧式几何距离；两点横坐标之差的绝对值与纵坐标之差的绝对值之和DMij称为曼哈顿距离。即

(5)

DMij=|pi-pj|+|qi-qj|

(6)

为了方便机器人的操控，结合以上2种度量方式,本文为估计代价h(m)设计了一种更接近实际的新的距离函数[10]：

(7)

式中，dx(m)=|pi-pj|,dy(m)=|qi-qj|，(pi,qi)、(pj,qj)分别为起始点和目标点的坐标。

2 动态窗口法

动态窗口法是在由速度和加速度组成的速度矢量空间中对多组速度进行采样操作，并综合考虑速度和加减速性能的约束，用机器人的运动模型模拟这些速度在一段时间间隔内的运动轨迹，并依据评价指标对所获得的轨迹进行评价，最后选取评价最高的轨迹所对应的速度和加速度作为机器人的驱动速度参数。该算法的核心就是将路径规划问题转化成带约束的速度矢量空间的优化问题[11,17]。

2.1 机器人的运动模型

实现动态窗口法的第一步需要获取机器人的运动模型，这也是模拟其运动轨迹的前提条件。假设机器人的运动轨迹由一段微小的圆弧轨迹组成，每段圆弧轨迹都对应着唯一的速度矢量(vt,ωt)。由于每段圆弧轨迹生成的时间间隔Δt很短，可以近似将这段轨迹看成是一段直线轨迹。于是，假设机器人在一段时间间隔Δt内以恒定的速度作直线运动，得到机器人的运动模型为

(8)

式中：xt、yt、θt为机器人在t时刻所处的坐标位置与方位角；xt+1、yt+1、θt+1为机器人在t+1时刻所处的坐标位置与方位角；vt和ωt分别为t时刻机器人的平移速度与旋转角速度。

2.2 机器人的速度采样

为求解上述的机器人运动模型，则需要对机器人的速度进行采样并代入模型公式求解，推算出模拟轨迹。在动态窗口法中定义了3种不同的约束来限制采样速度的范围，第1种是受机器人所处环境以及自身物理结构的限制，机器人所能达到的速度范围为[17-18]：

(9)

式中：vmax和vmin为机器人所能达到的最大、最小的线速度；ωmax和ωmin为机器人所能达到的最大、最小角速度。

第2种约束考虑了遭遇障碍物时的情景。当机器人采用最大减速度vb′、ωb′进行紧急制动时，为了使机器人能在碰撞之前停下来，保证其安全性，必须设定其容许的速度范围：

(10)

式中：dist(v,ω)表示速度矢量(v,ω)模拟的轨迹距最近障碍物的距离。

第3种约束是在一个模拟的时间间隔Δt内受电机所允许的最大加速度va′、ωa′和最大减速度vb′、ωb′的限制，所能达到的速度范围：

(11)

式中：vc、ωc表示机器人当前的线速度和角速度。

2.3 评价函数

在多组采样速度组成的集合下，通过模拟可以得到多组可行的轨迹。要从这些可行轨迹中选择最优轨迹，则需要从多个维度对其进行评估，使得机器人能够沿着最优轨迹安全、快速到达目标位置。在传统的动态窗口法中设计了方位角评价函数head(v,ω)、障碍物间隙评价函数dist(v,ω)和速度评价函数vel(v,ω)3个加权项。其评价函数为：

(12)

式中：head(v,ω)是衡量当前选择的采样速度下所产生的模拟轨迹末端方向与目标位置方向的角度偏差，偏差量越大，head(v,ω)的评分越低；dist(v,ω)是衡量速度矢量(v,ω)模拟的轨迹距最近障碍物的距离，评分越低，机器人与障碍物相撞的几率越大；vel(v,ω)是衡量机器人在当前轨迹下朝目标点的行驶速度；σ、β、γ是这3个评价指标的加权系数。

3 融合算法

优化的A*算法能够快速获得全局路径信息，能够很好地应对仅有静态障碍物的简单环境，但对于复杂环境中出现的动态障碍物却无法进行实时避障。而传统的动态窗口法有着不错的动态避障性能，但因其评价函数指标head(v,ω)中只评估最终目标点这一个方面，容易出现陷入局部区域无法到达目标的致命缺陷，使得它难以根据全局环境信息规划出一条全局最优的路径。针对上述2种算法存在的问题，本文采用上文所提的优化A*算法所获得的全局路径跳跃点信息与传统的动态窗口法相融合，提出新的方位角评价指标JPHead(v,ω)，并对传统动态窗口法的评价函数进行优化，使得优化后的动态窗口法评价函数在动态避障的同时充分考虑其全局最优性。优化后的评价函数表达式为

(13)

JPHead(v,ω)与原方位角评价指标head(v,ω)的区别在于，JPHead(v,ω)衡量的是模拟轨迹末端与距离当前轨迹最近的跳点之间的方位角偏差，可有效避免陷入局部最优，保证系统可到达最终目标点。

该融合算法保证了在进行动态路径规划时能够沿着全局最优路径点规划出一条平滑的全局最优路径。改进的动态窗口法流程如图5所示。

图5 优化的动态窗口法流程框图

4 仿真与实验

4.1 仿真与结果分析

为了验证提出的基于优化的A*算法与动态窗口法的融合算法的有效性与泛化性，本文在Matlab中建立了一个15×15的网格环境，并分别对传统的A*算法和与跳点搜索法相结合的优化A*算法、传统的动态窗口法和本文融合算法进行了4组对比仿真实验。另外，针对融合算法又进行了第5组实验，来验证本文算法在动态环境下的实时避障能力。

第1、2组仿真实验结果如图6、7所示。其中，起始节点用绿色圆圈表示，其坐标分别为(3,14)和(3,3)；目标节点则用红色圆圈表示，其坐标分别为(15,4)和(15,10)；形状大小不一的多个黑色障碍物分别置于图中的不同位置，图6(a)与图7(a)中用黑色圆圈标记的节点表示在传统A*算法搜索过程中所遍历的节点，图6(b)与图7(b)中用黑色十字标记的节点表示在优化A*算法搜索过程中所发现的跳点，蓝色的折线为2种算法在不同环境下所得到的最优路径。表1为2种算法在不同环境下的路径规划性能对比。

图7 在实验环境2下传统A*算法与优化A*算法的仿真结果

通过图6、7与表1可以看出，与传统A*算法相比，经过跳点搜索法优化后的A*算法在不同的实验环境下扩展节点数有显著减少，分别减少了80.90%和82.14%。在路径长度方面，实验环境1下2种算法得到的路径长度相同，而实验环境2下反而还比传统A*算法要稍长一些，即便如此，在2个不同环境下的搜索时间却分别减少了5.41%与18.37%，且在实验环境2下的转折次数也有所减少。综合来看，优化后的A*算法在得到与传统A*算法基本相同的全局路径的基础上，其搜索路径的时间和速度，甚至是转折次数上还要优于传统A*算法。尽管如此，但是优化A*算法始终没有解决路径转折处不够光滑的问题。

表1 传统A*算法与优化A*算法在不同环境下的路径规划性能

图6 在实验环境1下传统A*算法与优化A*算法的仿真结果

第3、4组仿真实验结果如图8、9所示。2种环境下的起始节点和目标节点的设置以及障碍物分布与第1、2组实验相同，起始节点和目标节点同样用绿色圆圈和红色圆圈表示。设置图6(b)中机器人的最大线速度和最大角速度为3.5 m/s和40 rad/s；最大线加速度和最大角加速度为0.35 m/s2和60 rad/s；线速度分辨率为0.01 m/s，角速度分辨率为1 rad/s。评价函数的3个加权系数σ、β、γ设置为0.05、0.2、0.1。表2为2种算法在不同环境下的路径规划性能对比。另外，通过将优化A*算法与融合算法形成的2条路径分别划分成相同间隔相同等份的线段，通过计算各段斜率的平均值即平滑度，来衡量路径的平滑程度，平滑度越小，代表其平滑性越好。如表3所示。

表2 传统动态窗口法与融合算法在不同环境下的路径规划性能

表3 优化A*算法与融合算法在不同环境下的平滑度

从图8、9以及表2、3可以看出，在相差无几的时间内与相同的实验环境1下，融合算法规划的路径长度比传统动态窗口法更短。而在相同的实验环境2下，融合算法很好地解决了传统动态窗口法容易陷入局部最优而无法到达目标点的缺陷，成功地抵达目标点，完成路径规划。不仅如此，融合算法还解决了优化A*算法路径转折处不够光滑的问题，使之不仅与优化A*算法一样具有全局路径规划能力，且其规划的路径比优化A*算法更加平滑。

图8 在实验环境1下传统动态窗口法与融合算法的仿真结果

图9 在实验环境2下传统动态窗口法与融合算法的仿真结果

第5组的动态仿真实验过程如图10所示，图中新增了黑色障碍物与红色障碍物用以模拟动态障碍，它们的半径均为0.3 m，其初始位置分别为(6,13.5)和(11.5,7)。令黑色障碍物以0.15 m/s的速度向右方移动，红色障碍物则以0.25 m/s的速度向上方移动。从图10(b)和10(c)中可以看到，当遇到正在缓慢向右移动的黑色障碍物时，机器人会主动避开它并选择向其移动方向的左侧绕行，避免与障碍物发生碰撞。接着，机器人会继续朝着目标点方向前进，当到达如图10(d)所示的位置时，机器人与红色障碍物交叉相遇，而此时在其移动方向的左侧又存在着一个静态障碍物，且2个障碍物之间的距离十分接近，使得机器人无法安全地向左绕行进行避障，最终，机器人选择向移动方向的右侧绕过这2个障碍物进行避障，如图10(e)所示。从动态仿真实验的过程可以看到，无论是追及黑色障碍物还是与红色障碍物交叉相遇，机器人都能很好地避开障碍物，且其规划的路径依然满足全局最优性。

图10 在动态环境下融合算法的仿真实验结果

4.2 实验与结果分析

为了进一步验证提出的融合算法在实际环境中的实现效果，搭建了一辆搭载有ROS系统的智能小车，同时该智能小车还配备有LeTMC-520深度摄像头，减速比为30的24 V电动机以及RPLIDAR-A2激光雷达传感器等硬件设备，并采用搭载Intel I3处理器的工控机作为小车的主控制器。在所有实验开始之前，需要对智能小车的角速度，线速度和IMU进行校正，并整定电动机控制模块的PID参数，以确保所有实验能够顺利进行。然后，使用储物箱和纸箱作为静态障碍物搭建了一个简单的实验环境模型，再将小车置于该实验环境模型中，通过上位机来控制小车以及其搭载的传感器进行实验环境信息的采集。最后，通过采集到的环境信息数据在上位机的Rviz平台中构建出如图11所示环境地图，并将构建好的环境地图及时保存至工控机中，以方便进行以下实验。

图11 环境地图

实际环境下的实验同样分为静态实验和动态实验两部分。在静态实验中，首先，在地图中指定了一个起始点和一个目标点，分别位于如图12(a)和图12(e)所示的位置。然后，让智能小车根据本文融合算法进行路径规划，并到达指定的目标点。从图12所示的实验过程可以看到，智能小车从图12(a)所示的起始位置开始行驶，并在如图12(b)，12(c)和12(d)所示的整个行驶过程中平稳地避开了所有静态障碍物，最后沿着全局最优路径安全地到达目标点，如图12(e)所示。

图12 本文融合算法在静态实验环境下的实验过程

与静态实验环境不同的是，在动态实验环境中添加了一辆朝着左上角方向移动的蓝色小车作为动态障碍物。实验开始后，智能小车同样从与图12(a)中相同的起始点开始行驶，如图13(a)所示。当在行驶过程中遇到蓝色小车时，智能小车则及时向右转弯并绕过蓝色小车以避免与之发生碰撞，如图13(b)和13(c)所示。随后，智能小车又返回到融合算法规划的最佳路径上并继续行驶，如图13(d)所示。

图13 本文融合算法在动态实验环境下的实验过程

5 结论

本文在跳点搜索的基础上对A*算法进行优化，设计了由曼哈顿和欧氏距离结合的新的距离评估函数获取全局路径信息；将优化的A*算法应用于动态窗口法中评价函数的优化上，提出了新的方位角评价指标JPHead(v,ω)，形成了新的融合算法，避免了原算法易陷入局部最优的缺陷。设计了具有动、静态障碍物的若干不同的环境并进行仿真实验，结果表明：在相同的实验环境下，新的融合算法能在不改变优化A*算法规划的原路径的基础上，增加了规划路径在转折处的平滑程度，使机器人在复杂环境中的移动更顺滑，同时缩短了路径长度；而在不同的实验环境下，融合算法解决了传统动态窗口法陷入局部区域的问题，并以平滑的路径到达目标点。通过一系列实验可以看到，本文融合算法有效地提高了机器人在复杂场景中路径规划能力，但其运行速度还有待提高，这也是下一步的工作重点。