融合改进A*算法和贝塞尔曲线优化的路径规划算法

谢春丽，高胜寒，孙学志

(东北林业大学交通学院，哈尔滨 150006)

0 引言

路径规划是机器人自动行驶的一个重要组成部分，它的主要目的是在特定的具有障碍地图环境下，在给定起始点以及终点的条件下，在尽可能短的时间内生成一条最短无碰撞路径的过程，由于汽车自动驾驶是在机器人以及自动引导汽车AGV(automated guided vehicle)的自动行驶基础上进行的应用，所以路径规划也是汽车自动驾驶的必不缺少的组成部分。在路径规划领域，许多学者做了大量的算法研究，探索出了很多算法及改进，这些算法各有优劣，常用的路径规划方法主要有以下几种：Dijksta算法、A*算法、蚁群算法、RRT算法、LPA*算法和D*lite算法等。Dijksta算法是贪心算法模式应用于路径规划，优点是产生最优解的路径，但缺点占用内存大，运行时间长[1]；A*算法是现在最成熟、应用最广的算法，通过引用启发性函数，平衡运行速度以及最优解之间的关系，缺点是容易陷入局部最优解[2]；蚁群算法是模仿自然界蚁群寻找食物的过程，添加信息素信息正反馈进而寻找路径，优点是不易产生局部最优，易于找到最优解，缺点是收敛速度慢[3]；RRT算法探索空间能力较好，但容易受到狭窄通道影响而找不到解[4]；LPA*算法与D*lite算法同为增量启发式算法，但前者为正向搜索，而后者为反向搜索，两者一般适用于动态地图环境[5]。

本文主要以A*算法为基础，对AGV的路径规划进行研究，使用A*算法的改进算法跳点搜索算法(jumppointssearch，JPS)进行启发性函数改良，从而减少Open_list以及Close_list中不必要的内存占用，同时大幅度减少了算法的运算时间，然后通过贝塞尔曲线(Bézier curve)进行全局路径平滑性优化，再通过python仿真，验证在给定一些地图参数的情况下，能得出有关最优路径的相关数据，使该算法在应用于自动引导汽车时，可以尽量排除模型理想化的影响。

1 传统A*算法

对于路径规划算法方面，学者们在多年来进行不断研究，提出了许多稳定成熟的算法，这些算法原理简单，而且可以进行大量实际运用，如在Dijksta算法基础上提出的A*算法，其公式如下：

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

(1)

式中：f(n)为起始点到目标点的估计代价；g(n)为起始点到中间点n的实际代价；h(n)为启发函数，同时表示中间点n到目标点的估计代价。A*算法主要是通过确定起始点和目标点，建立Open_list以及Close_list 2个列表，由起点出发，把当前点放入Open_list作为父节点，然后在当前节点的8个附近节点进行遍历寻找子节点，将父节点加入Close_list，新的子节点作为父节点并计算f、g、h3个值，反复进行运算直至Open_list列表中存在目标点，然后进行从目标点到起始点的回溯，并从f值选取最合适的路径，得出最优路径。A*算法的流程如图1所示。

图1 传统A*算法的流程框图

在传统A*算法当中，一般将g值计算值表征为路径所经过栅格地图的节点数，而启发性函数选取节点到目标点距离，一般为欧里几德距离度量移动代价：

(2)

式中：(x1，y1)，(x2，y2)分别为节点n1，n2的坐标。对于估价函数f(n)，当g(n)=0时，f(n)=h(n)，估价函数完全取决于启发函数，A*算法变成使用贪心策略的广度优先搜索，此时计算速度快，但不一定能获得最优解；当h(n)=0时，f(n)=g(n)，此时算法变成Dijksta算法，往往能获得最优解，但此时的计算占用内存大，计算速度慢。

A*算法作为经典路径规划算法，虽然可以较稳定地解决路径寻优问题，但也容易陷入“死循环”，不考虑实际车辆运行情况而产生复杂且繁多转折点以及在存在动态障碍物的地图环境中容易碰撞动态障碍物等问题。赵晓等[6]在 A*算法的基础上，结合跳点搜索算法提出一种改进的A*算法，该算法通过筛选跳点进行扩展，直到生成最终路径，扩展过程中使用跳点代替A*算法中大量可能被添加到Open_list和Close_list的不必要节点，从而减少计算量。陈豪等[7]提出了基于二维栅格地图的改进A*算法，其引入了方向矢量和并行搜索，使智能汽车路径搜索同时具备方向性和并行性。宣仁虎[8]针对A*算法在进行全局路径规划时路径较长，转折角度过大，路径不够平滑的缺点，提出了利用智能蚁群算法对A*算法中的路径进行迭代优化的改进算法。Zhong等[9]提出了一种新的混合路径规划方法，该方法将A*算法与自适应窗口方法相结合，可以在大规模动态环境中为移动智能汽车进行全局路径规划，实时跟踪和避障。张阳伟等[10]提出了一种建立正反向遍历的Open_list和Close_list一共4个列表，然后采取通过变步长双向运算，当找到正方向的Open_list和反方向的Close_list存在公共节点时，算法结束得出最优路径。

综上所述，A*算法主要存在以下不足：

1)冗余节点会进行过多代价运算，会导致算法运行时间过长；

2)由于Open_list访问节点过多，所以在运行过程中，会占用大量内存使用空间。

本文的改进算法与A*算法相比，运行时间会大幅缩短以及评估节点数量会在地图环境较大或高障碍率的条件下大幅减少。

2 融合改进A*算法和贝塞尔曲线优化

2.1 栅格地图模型建立

考虑自动引导汽车(AGV)主要是在平面空间进行运动，可以进行以下表述：设智能汽车为A，所在的平面空间为Q，整个平面空间可以分为b行b列进行均匀分割b2个均匀方块区域，对于各个均匀方块区域，可以进行建立二维坐标系，得到诸多二维坐标，再对每个均匀方块赋予一个特征值，栅格地图由栅格M={Mij|Mij=0，1，2，3}组成，0代表该区域为自动引导汽车所在起始栅格qgoal；1代表栅格地图中自动引导汽车的目的栅格qint，2代表自由区域B，可以进行路径规划；3代表存在障碍物的栅格区域C，该区域无法进行路径规划，在进行仿真模拟时躲避。则在该平面空间Q中的避障空间就可以表示成为Qa={q∈W|A(q)∩C=∅}，其中q为该空间的一点，A(q)表示的是智能汽车在空间所在位置q点。而路径规划的目标即寻找由初始位置qint到目标位置qgoal的最短避障路径。

首先对障碍及其周边区域的栅格进行赋值，从而确定模拟环境边界、障碍以及起始点，这样智能汽车在进行路径规划时，可以通过更准确的信息进行规划，降低AGV撞到障碍的风险。下面为模型的处理。

空间Q的划分：将平面二维空间均匀划分成独立的栅格。

区域边缘的处理：将区域边缘视作障碍区域，在规划路径过程中将区域边缘不予以考虑。

2.2 改进A*算法

2.2.1堆结构优化

当AGV进行JPS算法过程中，要进行大量节点代价运算，同时还需要通过代价值进行Open_list以及Close_list中的节点坐标元组的增加和删除，如果每次都进行坐标的代价比较，将会产生大量的计算，同时会增加较长运行时间。

堆是一种特殊的数据结构，定义为：n个元素的序列{k1，k2，ki，…，kn}当且仅当满足下关系时，即ki≤k2i且ki≤k2i+1或者ki≥k2i且ki≥k2i+1，则序列{k1，k2，ki，…，kn}为堆。图2为最小堆的数据结构的运算过程。二叉树的子节点的数值若是小于父节点的数值，则会进行位置进行置换，以此类推，直至符合所有父节点都小于子节点的数值。

图2 最小堆的数据结构的运算过程

于是堆总是满足以下2个性质：

1)堆中某个结点的值总是不大于或不小于其父结点的值；

2)堆总是一颗完整的二叉树。

于是可以通过最小堆(父节点小于子节点的堆)进行最小代价值的筛选，并快速完成Open_list以及Close_list 2个列表的中节点的筛选。

2.2.2JPS算法

减少A*算法的主要途径就是减少Open_list以及Close_list中冗杂节点的遍历过程，而通过跳点可以减少遍历列表中无关节点的占用内存，马小陆等[11]提出了在当前节点与目标点之间有无障碍物2种情况进行讨论跳点的筛选，本文将对JPS算法对跳点的选择进行讨论。

Case1节点周围不存在障碍物

如图3(a)所示，x，g点为栅格地图上的2个点，xpred为x的父节点，若要由xpred到达g最直接最短路径就是由xpred→x→g路线进行行进，但是还有其他的路径规划方案，例如xpred→a2→a3→g，这种路线明显可知不是最优路径，而且进行了复杂节点的运算从而导致A*算法最常见的周围临节点的重复访问。由图3(a)可知，和第二种的路径规划方案类似的非最优方案都会经过图中的灰色栅格，而要进行跳点的筛选，必须对这些灰色栅格所在的不必要节点进行删除。所以，当节点的周围不存在障碍物节点的时候，对于与当前节点在同一水平线或者竖直线上的节点，存在以下的筛选条件：

L(xprep，…，n|x)≤L(xprep，x，n)

(3)

式中：函数L表示路径的长度；则表示的邻节点；L(xprep，…，n|x)表示的是以xprep为起点；n为目标点但不经过x的路径集合；L(xprep，x，n)则表示的是路径xprep→x→n。

当目标点在非直线栅格方向上如图3(b)所示，同时在当前节点与目标节点之间并未存在障碍物，同理依据式(4)可得此时目标点在当前节点的非直线栅格方向上的跳点筛选条件：

L(xprep，…，n|x)

(4)

由图3可知仅由a2，b2，b3为筛选符合条件的跳点，而剩余的灰色节点将被删除，以免导致计算内存过大和运算繁琐。

图3 跳点筛选示意图

对于这2种情况给出以下条件进行跳点筛选：

1)n不是x的自然邻节点；

2)L(xprep，…，n|x)>L(xprep，x，n)

(5)

此时节点x周围存在障碍物栅格，即图4(a)以及图4(b)中的黑色栅格，障碍物栅格创建了强制邻节点(所有满足筛选条件的x的邻节点n)，即图4(a)以及图4(b)中的红色栅格。这些邻节点被创建，是因为一些栅格由于栅格的替代路径被阻塞而不能被裁剪，图4(a)中的a3和图4(b)中的a1都是强制邻节点，在筛选跳点的时候都放进列表。

图4 强制邻节点筛选的一种情况

起始点中间存在障碍物的栅格地图模型如图5所示，浅蓝色部分为筛选出来的强制性邻节点，即跳点。对于路径最优规划，分别将建立的跳点集合进Open_list，从起点qint以及终点qgoal2个点作为A*算法的起始点，然后通过式(1)，进行A*算法的运算找寻最优路径。

图5 JPS算法跳点的筛选

2.2.3改进启发函数

本文的JPS算法是先进行地图建模然后进行地图中跳点的筛选，然后在跳点的基础上进行A*算法的改进启发性函数代价运算，从而实现最优路径寻找。A*算法一般的h(n)为欧里几德距离(euclidean distance)，或者切比雪夫距离(chebyshev distance)：

d(x，y)=Max{|x1-x2|，|y1-y2|}

(6)

或者Octile距离：

d(x，y)=Max{|x1-x2|，|y1-y2|}+

(7)

或者曼哈顿距离(manhattan distance)：

d(x，y)=|x1-x2|+|y1-y2|

(8)

对于栅格地图，若为四方向移动，曼哈顿距离(manhattan distance)是最合适的启发函数。若八方向(相较于四方向增加对角线方向)移动，可以用切比雪夫距离(chebyshev distance)作为启发性函数。欧里几德距离(euclidean distance)适合方向自由度更高的地图作为其启发函数，但是欧式距离中具有一个开方的过程，计算量会较其他算法更大。而Octile距离主要是用于45°转弯，比较适合本文中的八方向地图环境，而且不存在开方等复杂计算。

在地图环境中的最小估计函数会被地图中的障碍物影响，h(n)不仅会因为跳跃点之间的代价而改变，而且还会因为障碍物的分布而改变[12]，但该文献对障碍率对数化后未绝对值化，导致h(n)一直是负优化。本文为了估计最小路径距离值，设置了障碍率代表地图环境。障碍率即当前地图环境下障碍节点数与整体地图环境节点总数的比，为了减少边界无关区域障碍的影响，此时障碍率P如式(6)所示，也可以称之为有效障碍率，是由于将起始点所形成矩形区域以外的障碍物除去，其中式(9)中起始节点为(xs，ys)，终点为(xg，yg)。同时为了使数据更平滑，对障碍率进行对数化处理，然后再与Octile距离进行结合处理作为h(n)，即式(10)。

(9)

(10)

由上式可知，该启发性函数在障碍率较低时，h(n)将会在f(n)中占比较小，此时搜索速度较快，但寻优性较差，但也符合低障碍率的路径规划情景；该启发性函数在障碍率较高时，h(n)将会在f(n)中占比较大，此时搜索速度较慢，但寻优性较为良好。

由于本文主要是以八方向的移动为主，所以不考虑曼哈顿距离，通过在相同地图模型环境，相同起始点进行JPS算法，h(n)分别采用以上距离进行仿真，具体数据如表1所示。以下数据搜索时间仅为主算法时间，不包括地图环境构建以及可视化运行时间。地图环境为图6，此时有效障碍率38.4%。表1为不同启发性函数在相同地图环境下的运行数据，3种启发性函数中，在较高的障碍率的地图中，所生成路径长度一致，但是切比雪夫距离的评估节点数量较另外2种算法多20.31%，占用内存较大。而改进的启发性函数，在高障碍率地图环境中，与欧里几德距离相比较，评估节点数量和路径长度一致，但是搜索时间上减少38.01%，通过表1仿真数据决定本文采取改进启发性函数作为启发式函数。

图6 测试启发式函数地图环境

表1 同启发性函数在相同地图环境下的运行数据

3 贝塞尔曲线的车辆路径轨迹优化

3.1 贝塞尔曲线相关特性

贝塞尔曲线是由伯恩斯坦多项式(bernstein polynomial)推导而来[13]，贝塞尔曲线一般用来进行折线的平滑化处理，本文运用贝塞尔曲线对改进的算法产生出来的折线路径进行平滑处理。

伯恩斯坦多项式的第n阶项有如下形式：

(11)

(12)

其中，βi叫做伯恩斯坦系数。

由文献[14]中的结论，高阶贝塞尔曲线的数值稳定性较差，为了解决数值稳定性，同时为了满足AGV的曲率约束，采取二阶贝塞尔曲线和三阶贝塞尔曲线相结合进行路径平滑性优化，针对二阶和三阶贝塞尔曲线如下属性进行使用。

1)二阶贝塞尔曲线表达式如式(13)所示，三阶贝塞尔曲线表达式如式(14)所示：

P(t)=P0(1-t)2+2P1(1-t)t+P2t2，

t∈[0，1]

(13)

P(t)=P0(1-t)3+3P1(1-t2)t+

3P2(1-t)t2+P3t3，t∈[0，1]

(14)

2)二阶贝塞尔曲线会经过第1、3个控制点；三阶贝塞尔曲线经过第1、4个控制点。

3)曲线在端点切向量表达式如下：

P′(t)=-2P0(1-t)+2P1(1-2t)+2P2t2，

t∈[0，1]

(15)

P′(t)=-3P0(1-t)2+3P1(3t2-4t+1)-

6P2(1-t)t+3P3t2，t∈[0，1]

(16)

4)贝塞尔曲线上任意一点曲率为：

(17)

5)贝塞尔曲线轨迹起始点曲率为：

(18)

6)仿射变换不变特性

通过式(17)二阶贝塞尔曲线上曲率和三阶贝塞尔曲线上曲率可知，路径轨迹曲线连续条件为x′(t)、y′(t)不同时为0，若x′(t)、y′(t)同时为0，可知κ(t)=0，而这使得路径轨迹无法到达后续控制点，但这与贝塞尔曲线定义相矛盾，所以由二阶贝塞尔曲线确定的轨迹曲线和由三阶贝塞尔曲线确定的轨迹曲线曲率处处连续。

对于AGV，控制其贝塞尔曲线轨迹的每一个控制点的曲率是不太容易的，但对于AGV最大前轮转向角是有一定范围的，通过式(19)，可以知道控制点曲率与最大前轮转向角的关系。

(19)

式中：κ为曲率;γ为曲率半径即转弯半径;α为前轮转向角;L为前后轮轴距，所以存在式(20)，即为曲率有界条件。

Kmin≤κ(τ)≤Kmax

(20)

式中：Kmin和Kmax分别为最小、最大曲率。

贝塞尔曲线具有以下性质：拟合曲线的起始点和终止点与被平滑化处理的折线的起始点和终止点重合；折线点一旦确定，拟合曲线唯一。贝塞尔曲线平滑处理后，使生成路径与折线相比更贴近实际，更适合AGV的实际运行。

3.2 贝塞尔曲线优化问题以及改进

由于高阶贝塞尔曲线数值稳定性较差，所以一般的路径优化都采取二阶和三阶贝塞尔曲线优化[14]。在不同控制点的情况，会存在二阶贝塞尔曲线以及三阶贝塞尔曲线之间的优化选择，如图7所示，对前3个点进行二阶贝塞尔曲线优化，则第4个点的优化会被遗漏，从而导致更适合的三阶贝塞尔曲线优化被二阶贝塞尔曲线优先替代，不能达到最优。

图7 二阶贝塞尔曲线与三阶贝塞尔曲线判定以及优化

图8为常见的二阶贝塞尔曲线以及三阶贝塞尔曲线的优化。至于更多控制点的优化则采取n组二阶贝塞尔曲线以及m组三阶贝塞尔曲线进行组合优化而不采用数值不稳定高阶贝塞尔曲线。

图8 二阶贝塞尔曲线与三阶贝塞尔曲线的判定特定节点及其优化

将JPS算法所生成路径，代入到贝塞尔曲线改进程序中，由于二阶贝塞尔曲线和三阶贝塞尔曲线所需控制点数量不同，应先判断能否进行三阶贝塞尔曲线优化，若能则进行优化，否则进行二阶贝塞尔曲线优化，而对于能否进行三阶贝塞尔曲线优化的判断，通过记录控制点坐标，然后记录方向也即4个控制点的3个方向向量，当存在如下9种情况时，即可进行三阶贝塞尔曲线优化。

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

以上的这些公式中的dxn为第n个横坐标方向变化量，dyn为第n个纵坐标方向变化量。

这样，全局路径规划产生的路径将会被二阶贝塞尔曲线和三阶贝塞尔曲线共同优化，而不会产生三阶贝塞尔曲线优化中控制点不足以及二阶贝塞尔曲线优化中容易遗漏一个控制点而未进行三阶贝塞尔曲线优化这2种情况导致的曲线优化不理想的情况。

4 仿真与分析

4.1 改进算法仿真

本文在Window 10，处理器为i7-6900H的环境下通过python3.9进行仿真，为验证本文的改进JPS算法的可行性，地图环境的障碍物位置均为随机选取，在随机的地图环境能更好地验证算法的有效性以及先进性。仿真分别在20*20、30*30、50*50的有效障碍率为20%的低障碍率的地图环境以及20*20、30*30、50*50的有效障碍率为40%左右的高障碍率地图环境，分别进行传统A*算法、JPS算法以及改进JPS算法仿真。同时为验证改进算法的路径生成效果以及贝塞尔曲线的优化效果，在验证启发性函数的30*30，有效障碍率为38.4%的地图环境下进行相应的仿真运行，仿真结果如图9—12所示。

图9 传统A*算法的路径搜索

4.2 评价指标

为从实验结果中分析算法的有效性，在仿真实验中选取路径规划算法评估节点数量、搜索时间以及产生路径长度作为算法的评价指标。传统A*算法、JPS算法、改进JPS算法在较低障碍率地图环境下测试结果见表2，在高障碍率地图环境下测试结果如表3。数据运行时间只是主算法运行时间，不包括地图建模时间，可视化时间以及曲线优化时间。同时运行时间过短会产生波动较大，所以如下数据为进行5次仿真取的平均值。同时由于不同障碍率的地图环境的障碍分布不同，也会对时间产生一些影响。

图10 JPS算法所筛选的跳点

图11 改进JPS算法的路径搜索

图12 改进贝塞尔曲线所生成的改进JPS算法所生成路径

表2 传统A*算法、JPS算法以及改进JPS算法的路径搜索在较低障碍率地图环境下的数据

表3 传统A*算法、JPS算法以及改进JPS算法的路径搜索在高障碍率地图环境下的数据

由表2可知，在低障碍率地图环境下，在地图环境简单的20*20的条件下，无论是评估节点个数还是搜索时间，改进算法与A*算法以及JPS算法相比会存在劣势，但生成的路径长度所差不大。当地图环境变大时，评估节点数会大于A*算法，但会小于JPS算法，而运行时间相对另2种算法在30*30的地图环境下分别减少99.5%和14.1%，在50*50，20%有效障碍率的地图环境下分别减少99.7%和9.3%。对于后2组数据，体现改进算法在有效障碍率变大时，评估节点数会大幅减少，而且运行时间相较于本身也减少11.2%。

通过表3发现，虽然在路径长度上可能略长于A*算法，但路径长度基本与其他2种算法无异，而且改进算法不论是在评估节点数量与其他2个算法相比，分别减少了14.9%和25%；在搜索时间上，相较于其他2种算法，分别减少99.8%和59.2%。

为了验证评估节点数量对贝塞尔曲线的拟合效果影响，分别进行了对低障碍率的改进算法的20%障碍率的20*20、30*30和50*50的地图环境产生的路径进行贝塞尔曲线优化。优化结果如图13—15所示，其中绿点为起点，粉色点为终点，发现评估节点数量并不影响拟合效果。

图13 在20*20地图环境下的贝塞尔曲线拟合曲线

图14 在30*30地图环境下的贝塞尔曲线拟合曲线

图15 在50*50地图环境下的贝塞尔曲线拟合曲线

5 结论

对AGV的路径规划来说，可行性是前提，快速性是其目标。本文改良JPS算法实现过程中，对轨迹进行了贝塞尔曲线优化，以保证规划的路径满足AGV运动可行性。同时使用了最小堆数据结构对Open_list取最小值进行优化，该方法降低了内存消耗，加快了算法的运算。对比传统的算法，改良JPS算法的改进之处在于启发性函数这一改善。在python编程环境下，进行较低有效障碍率以及高障碍率的20*20、30*30、50*50地图环境下的仿真得出的试验结果表明，该算法在有效障碍率较低时，运算时间短，寻优结果较好；在有效障碍率较高时，运算时间也较对照组相比更短，访问节点数较少，节约了内存空间，搜索时间大幅度缩短。对于仿真生成的路径轨迹，通过改进的贝塞尔曲线优化，满足了曲率连续有界约束，验证了算法的有效性与高效性。