CN109764886B - 一种路径规划方法 - Google Patents

Abstract

本发明提供一种路径规划方法，该方法通过对车辆建模得到车辆的约束，同时在估价函数中加入方向代价并带入A*算法，使得所规划路径更加符合车辆的运动学模型,转折次数少，路线更平滑。将本发明方法应用于学校校园路径规划，结果表明：在规划相同路径上面，改进的A*算法规划的路径相比传统A*所规划的路径，长度降低约2.22％，累计路径转折点降低约65.28％，累计转折角度降低约49.03％，所规划出的路径能够更好满足车辆运动学模型。

Description

一种路径规划方法

技术领域

本发明涉及控制工程技术领域，尤其涉及一种路径规划方法。

背景技术

随着无人驾驶的快速发展，无人车运用在日常生活中的可能性也越来越大。而无人车的安全性、可靠性和舒适性尤为重要，这就对无人车运动路线的优化提出了更高的要求。因此，路径规划一直是无人车研究中的一个热点，对于其规划的好坏常常用规划的路径长度、代价和时间等因素来评价。目前，学者们对路径规划方法进行了大量研究，主要包括：基于采样的Voronoi图方法、快速搜索随机树等方法；基于节点的Dijkstra、A*算法、D*算法等；基于模型的人工势场法、动态窗口法等；基于生物启发式的神经网络、遗传算法、蚁群算法等。其中，A*算法便于理解和实现，是目前使用最广泛的规划法之一。

由于A*算法能够有效的求解出最优路径，同时利用启发函数能够简单的评估地图上任意点到终点的距离，从而能够有效的减少搜索空间，提高搜索效率。所以被广泛的运用在移动机器人领域。但是，在应用中，经典的A*算法仍存在一些问题，如：所规划的路径距离障碍物太近，路线不够平滑，转折个数比较多等，导致规划出来的路线可能紧贴障碍物。王维，裴东等人通过对当前结点及父节点的代价进行指数衰减，使得A*算法在离目标较远时能够较快的靠近目标。王红卫,马勇等通过将延长线路的中间节点删除,提出平滑A*算法，使得A*算法的结果更加平滑。Saska和吴宪祥等提出基于三次样条曲线的参数优化问题，使得路径利于机器人的运动控制。单伟和孟正大提出使用极多项式和使用分段多项式函数是A*规划的路径来满足移动机器人的动力学特性。但上述改进算法考虑的是两轮车的运动学特性，不适用于无人驾驶汽车的路径规划。

由于在车辆的实际运动中需要路线是平滑的，具有连续的曲率的和不能有较大的转折的。而传统A*算法所规划的路径则存在这些问题。

发明内容

本发明的目的在于解决上述现有技术存在的缺陷，提供一种路径规划方法，该方法对车辆进行建模分析，得到最小的转弯半径和最大的曲率。同时，提出方向代价解决车辆在起始时刻方向盘打死问题。然后，对提取出来的转折点进行带有阿克曼转向约束的路径优化，使A*算法所规划出的路径更加符合车辆的运动学特性。

一种路径规划方法，包括以下步骤：

步骤1：初始化起点、终点的方向；

步骤2：判断起点和终点之间是否有障碍物，如果存在障碍物，则进入步骤3；如果没有障碍物，再进一步判断车身朝向与当前点和目标点构成的直线之间的夹角

是否大于

如果

则用y＝k*x+b估计移动代价；如果

则用y＝a*x²+b*x+c估计移动路径代价；

步骤3：将起始位置放到开启列表，将关闭列表初始化为空。执行A^*算法的扩展过程,即搜寻规划节点，所述开启列表表示待搜索节点列表；

步骤4：判断开启列表是否为空，如果为空，则寻找路径失败，退出程序；如果不为空，则进入步骤5；

步骤5：计算节点与起始点的方向代价o(n)＝a(n)+d(n)和路沿惩罚代价c(n)；其中，a(n)为起始点的朝向到当前位置的角度的代价，d(n)是起始点到当前结点n的直线代价；

步骤6：利用公式(4)计算开启列表中估价函数f(n)；

f(n)＝g(n)+h(n)+o(n)+c(n) (4)

其中，g(n)是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的估计代价；

步骤7：将开启列表中具有最小f(n)所对应的节点，作为扩展节点，并将该节点加入关闭列表中,同时，取扩展节点的邻近节点，判断其是否在关闭列表中，如果不是，则将该邻近节点加入开启列表中，并更新开启列表中其余节点的代价函数f(n)；

步骤8：判断步骤7中的扩展节点是否为终点，如果不是，则返回步骤5；如果该扩展节点为终点，则将关闭节点中的节点保存为规划路径，即Path1。

进一步地，如上所述的方法，在步骤8之后，还包括以下步骤：

步骤9：获取所述路径path1；

步骤:10：利用直线拟合算法删除冗余点得到转折点；

步骤11：依次从path1中取pose[i-2],pose[i-1],pose[i]:3个转折点进行拟合y(x)¹＝a*x²+b*x+c，同时，判断提取出来的转折点中，是否包含终点，如果含有终点，则进入步骤13,否则执行下一步；

步骤12：对y(x)¹＝a*x²+b*x+c求一阶导和二阶导，带入公式(10)中，判断是否满足公式(11)，如果满足，则进入步骤13；否则移动转折点，判断移动次数是否小于3，如果小于3，更新最小的曲率并重新执行步骤12，如果大于3，则保留曲率最小的转折点，然后进入步骤13；

所述更新最小的曲率包括：移动转折点pose[i-1]，首先求出y(x)¹的对称轴

如公式(12)所示，判断所提取的出来的转折点是否大于

同时，通过判断a的正负号确定y(x)¹的开口朝向，如a＞0且

则将pose[i-1]向左上角移动一个栅格，如果a＞0且

则将pose[i-1]向右上角移动一个栅格，如果a＜0且

则将pose[i-1]向左下角移动一个栅格，如果a＜0且

则将pose[i-1]向右下角移动一个栅格；

步骤13：拟合转折点，进行插值；

步骤14：结束规划，输出路径Path；

K＜K_max (11)

index_new是移动后的节点的索引,_index是此刻节点的索引，nx表示地图的宽度。

有益效果：

本发明通过建立车辆模型的约束，提出改进A*算法克服了传统的A*算法转折点多，路线不平滑和曲率不完整等问题。同时，考虑车辆的初始航向提出方向代价，能够较好的解决初始航向过大，方向盘打死的问题。通过在实验证明，与传统A*算法相比，改进的A*算法具有规划路径光滑、转折次数大幅减少，规划路径较短等优点，且避免了方向打死的问题，更适合于无人车的运动学特性。

附图说明

图1为本发明野马E70电动汽车的简化的车辆模型；

图2为采用传统A*算法的曼哈顿距离所规划的局部路径；

图3为采用本发明改进启发函数所规划的局部路径；

图4为当

时，改进h(n)所规划的路径；

图5为未加方向代价的路径点；

图6为加入方向代价的路径点；

图7为未加入到路沿惩罚代价的路径点

图8为加入到路沿惩罚代价的路径点

图9为改进A*算法流程图；

图10为转折点移动算法流程图；

图11为某学校的校园地图；

图12(a)为利用改进A*算法在图11中区域一测试的结果；

图12(b)为利用传统A*算法在图11中区域一测试的结果；

图13(a)为利用改进A*算法在图11中区域二测试的结果；

图13(b)为利用传统A*算法在图11中区域二测试的结果。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

车辆的运动学模型

由于汽车是一种典型的含有非完整性约束系统，本发明以平台野马E70电动汽车的简化车辆模型为例，如图1所示，在惯性坐标系XY下，

为车体的横摆角(航向角)，L是轴距，R是转弯半径，θ为车辆纵轴与x轴之间的夹角。由于车辆只能用前轮转向，所以需要满足阿克曼转向条件

只考虑车辆在二维平面上运动，不考虑俯仰和侧倾带来的影响，则该车辆的动力学模型为

dx*sin(θ)-dy*cos(θ)＝0 (1)

其中，K_max车辆最大转弯曲率，R_min车辆最小转弯半径.

A*算法是一种在静态路网中求解最短路径最有效的直接搜索方法，也是解决许多其他搜索问题的有效算法。算法中的距离估算值与实际值越接近，扩展的节点数越少，搜索速度越快。A*算法的估价函数的一般形式为

f(n)＝g(n)+h(n) (3)

其中f(n)是从初始状态经由状态n到目标状态的代价估计，g(n)是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的估计代价。而传统的A*算法的h(n)多数采用Manhattan距离和Euclidean距离对目标状态进行估计。因此传统A*所规划出来的路径往往不具有连续的曲率且转折次数过多等不满足车辆运动学模型问题。

为了解决传统A*算法所规划的路线不适用于车辆的问题，本发明基于传统A*算法使用连续函数来估计h(n)，提出加入方向代价，将车辆初始方向按照一定的权重加入代价函数中，同时对车辆运动学进行建模得到车辆的约束，将约束带入启发函数和对路径的优化中。

改进A*算法中的代价函数定义为

f(n)＝g(n)+h(n)+o(n)+c(n) (4)

o(n)＝a(n)+d(n) (5)

c(n)＝C*d_n*e^-(n1-nc) (7)

其中，o(n)是由起始点的朝向到当前位置的角度的代价a(n)和起始点到当前结点n直线代价d(n)所构成,c(n)是当前节点n到路沿的惩罚代价。A是起始点的朝向到当前位置的角度的代价的系数，n_angle是起始点的朝向到当前位置的角度绝对值。B是起始点的朝向到当前位置的直线的代价的系数，n_distance是起始点的朝向到当前位置距离。由于加入方向代价o(n)主要是解决车辆初始时刻方向盘打死的问题，所以使用单调递减的函数，e是自然常数，在公式(6)中表示指数函数的底数，C表示当前栅在地图上的代价值，n1表示当前栅格在某个方向所扩展的栅格数量，nc表示当前栅格在某个方向所扩展的第几个栅格(n1＞nc)，d_n表示nc到起点或终点的最大距离，c(n)表示在n1范围内所计算的最大代价值。

1、对函数h(n)进行改进

因为A*算法是一种启发式搜索算法，而启发函数h(n)直接影响A*算法的路径规划性能。假设当前节点到目标节点的实际代价值为H(n)，当h(n)<H(n)时，搜索节点多，运算效率低，但可搜索到最优路径；当h(n)＝H(n)时，是理想情况，沿最短路径进行搜索，且效率最高；当h(n)>H(n)时，搜索节点少，运算效率高，但通常难以搜索到最优路径。而传统的A*算法的h(n)所使用Manhattan距离和Euclidean距离对代价值进行估计，会造成在栅格地图中所规划出的路线的曲率不连续，不能较为准确的对当前节点代价值进行估计。所以，本发明提出使用曲率连续的变函数进行对当前节点的代价值进行较为合理的估值。

首先，通过连接起始点和目标点来判断路线上是否有障碍物，生成预判函数y_h＝k*x+b(y_h中x取值从起始点到终点)。如果没有障碍物，则说明直线运动的代价最小，所以使用y＝k*x+b作为启发函数h(n)用于计算路径点的代价值。否则，则根据起始点和目标点计算启发函数h(n)的路径代价值，则使用二次函数y＝a*x²+b*x+c作为启发函数h(n)用于计算路径点。例如令扩展点为p(n)＝(x₁,y₁)，终点为q＝(x₂,y₂)，则：

其中，y_h为预判函数。

当预判函数y_h在起始点和终点间没有障碍物且车辆的朝向和直线的夹角

时，最优的路经应该是一条曲线，就需要对启发函数y＝k*x+b进行修改使用二次函数对节点的代价值进行评价。因为在规划之前只知道起始点和终点的坐标不能拟合二次函数，所以通过设置两个点

来进行拟合。设起始点s＝(x₁’,y₁’)，终点是p＝(x₂,y₂)。(将起始点s设为坐标原点，便于后续处理)

其中n<N(本文N＝3)，n从0开始,n是拟合函数y(x)上的遇到障碍物的次数的迭代次数。N是最多迭代的次数，当

的坐标与起始点s＝(x₁’,y₁’)，终点是p＝(x₂,y₂)的坐标的x或者y相同，或n>N，则使用y_n(n)＝k*x₁+b来评估当前节点到目标节点的估计值。如果所预置的点并不在障碍物中则使用起始点s＝(x₁’,y₁’)，

和终点p＝(x₂,y₂)，拟合一条曲线y¹(x)＝a¹*x²+b¹*x+c¹，将其替换原本的y_h(n)＝k*x₁+b。

图2和图3分别是采用传统A*算法的曼哈顿距离和采用本文的改进启发函数所规划的局部路径。从图中可看出，与传统的A*算法相比，改进的启发函数所规划的路径更加平滑，初始转角较小，转折次数少，路径更短。

当预判函数y_h与车身初始航向的夹角大于

且在y_h中x取值从起始点到终点之间无障碍时，根据车辆的运动学特性可知最优路线应为一条曲率连续的弧线。因此，用二次曲线y¹(x)代替y_h(n)来对当前节点进行估值。所规划的路径如图4所示能够很好满足车辆的运动学特性。

方向代价o(n)

通过对图1中车辆的建模分析，发现车辆在运动过程中存在着最大前轮转角。而在传统的A*算法中未考虑车辆的最大前轮转角和车辆的朝向，因此规划出的路径在起始的时刻存在着转弯过大，会出现车辆方向盘会打死的问题。所以本发明在传统A*算法上面提出的方向代价，将车辆朝向和车辆最大的前轮转角考虑到代价函数中，使得A*算法更加适用于车辆的运动学模型。(见公式4)

a)首先获得车辆的实际朝向(本发明设正东方向为零度，逆时针为正)。

b)扩展每个节点。节点和起始点的夹角小于

则加a(n)(公式5和6)。否则，继续扩展每个节点。

通过对比图5和图6可以看出加入方向代价后，可以看出加入方向代价后，能够较好的解决初始时候规划不合理的问题。同时，能够减少50％以上的扩展节点数。

路沿惩罚代价c(n)

由于定位和地图存在着一定的误差，所以在规划的路线必须保证与障碍物之间的距离保持大于车身宽度。而传统的A*算法在规划中未能考虑于障碍物的距离。本发明通过计算每个栅格的惩罚代价来限制路径与障碍物的距离。设起始点s＝(x₁’,y₁’)，扩展点为p(n)＝(x₁,y₁),终点是q＝(x₂,y₂)。

a)判断在n1*n1栅格范围内，当扩展节点p(n)四周的地图的代价值是否为0。如果代价值为零，说明扩展节点时安全的。如果代价值不为零，则说明周围存在着路沿，执行b)。

b)获取n1*n1范围内最大的栅格代价带入公式(7)中。通过对比图7和图8中，传统A*算法和加入到路沿的惩罚代价的A*算法能够较好的说明加入惩罚代价的A*所规划的路径与障碍物保持一定的距离，使得车辆在行驶过程中更加安全。

实施例1：

本发明提供的基于改进A*算法的无人车路径规划方法，如图9所示，包括以下步骤：

步骤1：初始化起点、终点的方向；

步骤2：判断起点和终点之间是否有障碍物，如果存在障碍物，则进入步骤3；如果没有障碍物，再进一步判断车身朝向与当前点和目标点构成的直线之间的夹角

是否大于

如果

则用y＝k*x+b估计移动代价；如果

则用y＝a*x²+b*x+c估计移动路径代价；

步骤3：将起始位置放到开启列表，将关闭列表初始化为空。执行A^*算法的扩展过程,即搜寻规划节点；所述开启列表表示待搜索节点列表；

步骤4：判断开启列表是否为空，如果为空，则寻找路径失败，退出程序；如果不为空，则进入步骤5；

步骤5：计算节点与起始点的方向代价o(n)＝a(n)+d(n)和路沿惩罚代价c(n)；

步骤6：利用公式(4)计算开启列表中估价函数f(n)；

具体地，开启列表和关闭列表均属于A*算法是术语。开启列表中存放当前待搜索的候选节点。关闭列表中的节点序列表示到目前已经搜寻得到的扩展(最优)节点集合。

f(n)＝g(n)+h(n)+o(n)+c(n) (4)

步骤7：将开启列表中具有最小f(n)所对应的节点，作为扩展节点，并将该节点加入关闭列表中,同时，取扩展节点的邻近节点，判断其是否在关闭列表中，如果不是，则将该邻近节点加入开启列表中，并更新开启列表中其余节点的代价函数f(n)；

步骤8：判断步骤7中的扩展节点是否为终点，如果不是，则返回步骤5；如果该扩展节点为终点，则将关闭节点中的节点保存为规划路径，即Path1。

实施例2：路径优化

由于车辆转向存在着最小转弯半径，不能像麦克纳姆轮全向移动。而传统的A*算法所规划出的路径转折数目太多，并且不够平滑，使得车辆在贴合路径时，摇摆不定。所以必须对规划出来的路径进行一步优化，使得路径更加适用于阿克曼转向的车辆模型。优化过程即为转折点移动算法，算法流程如图10所示,该算法在实施例1的基础上包括以下步骤：

步骤9：获取所述路径path1；

步骤:10：利用直线拟合算法删除冗余点得到转折点；

步骤11：依次从path1中取pose[i-2],pose[i-1],pose[i]:3个转折点进行拟合y(x)¹＝a*x²+b*x+c，同时，判断提取出来的转折点中，是否包含终点，如果含有终点，则进入步骤13,否则执行下一步；

步骤12：对y(x)¹＝a*x²+b*x+c求一阶导和二阶导，带入公式(10)中，判断是否满足公式(11)，如果满足，则进入步骤13；否则移动转折点，判断移动次数是否小于3，如果小于3，更新最小的曲率并重新执行步骤12，如果大于3，则保留曲率最小的转折点，然后进入步骤13；

所述更新最小的曲率包括：移动转折点pose[i-1]，首先求出y(x)¹的对称轴

如公式(12)所示，判断所提取的出来的转折点是否大于

同时，通过判断a的正负号确定y(x)¹的开口朝向，如a＞0且

则将pose[i-1]向左上角移动一个栅格，如果a＞0且

则将pose[i-1]向右上角移动一个栅格，如果a＜0且

则将pose[i-1]向左下角移动一个栅格，如果a＜0且

则将pose[i-1]向右下角移动一个栅格；

步骤13：拟合转折点，进行插值；

步骤14：结束规划，输出路径Path；

K＜K_max (11)

index_new是移动后的节点的索引,index是此刻节点的索引，nx表示地图的宽度。

实验例：

为了验证改进A*算法的有效性，本发明分别将A*算法和改进的A*的算法应用于实验地点的栅格地图(4846×2816)上如图11)，栅格大小为0.298米/pixel，选择不同的地点进行实现。同时，基于ROS(开源机器人操作系统)平台，并使用Rviz进行显示，计算机配置为:ubuntu14.04LTS，处理器inteli5-6500，主频为3.2Ghz，运行内存为8G。

本次实验通过在地图上选择起始点和终点，使用改进A*算法和传统A*进行路径的规划。通过判断规划路径的长度，路线是否平滑和转折数量(

则认为是转折)等方式来进行判断。(传统A*使用的Manhattan距离)，对区域a和区域b进行路径规划，规划结果见图12，图13。表1给出了不同区域规划的指标对比。

表1改进A*算法和传统A*算法测试对比

从图12和13可以看出，改进的A*算法所规划出来的路径较为平滑，具有连续的曲率并且能够较好的满足车辆运动学模型。同时，在区域一中能够看出传统A*算法对起始时刻路线不合理和规划路径贴合膨胀层等问题，在加入惩罚代价和方向代价后能够如图b较好解决此问题。这些都说明改进的A*算法所规划的路径能够较好的解决传统A*算法所规划的路线不适用于车辆运动学模型的问题。

通过表1能够看到虽然改进A*算法运行的时间比传统A*算法略长，但是所增加的时间也在可以接受的范围内。同时，通过牺牲时间获得了更加平滑，更加安全，路径长度平均降低约2.225％且转折次数和转折角度都有较大的减少的路径。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (1)

1.一种路径规划方法，其特征在于，包括以下步骤：

步骤1：初始化起点、终点的方向；

步骤2：判断起点和终点之间是否有障碍物，如果存在障碍物，则进入步骤3；如果没有障碍物，再进一步判断车身朝向与当前点和目标点构成的直线之间的夹角

是否大于

如果

则用y＝k*x+b估计移动代价；如果

则用y＝a*x²+b*x+c估计移动路径代价，然后进入步骤3；

步骤3：将起始位置放到开启列表，将关闭列表初始化为空；执行A^*算法的扩展过程,即搜寻规划节点，所述开启列表表示待搜索节点列表；

步骤4：判断开启列表是否为空，如果为空，则寻找路径失败，退出程序；如果不为空，则进入步骤5；

步骤5：计算节点与起始点的方向代价o(n)＝a(n)+d(n)和路沿惩罚代价c(n)；其中，a(n)为起始点的朝向到当前位置的角度的代价，d(n)是起始点到当前结点n的直线代价；

步骤6：利用公式(4)计算开启列表中估价函数f(n)；

f(n)＝g(n)+h(n)+o(n)+c(n) (4)

其中，g(n)是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的估计代价；

步骤7：将开启列表中具有最小f(n)所对应的节点，作为扩展节点，并将该节点加入关闭列表中,同时，取扩展节点的邻近节点，判断其是否在关闭列表中，如果不是，则将该邻近节点加入开启列表中，并更新开启列表中其余节点的代价函数f(n)；

步骤8：判断步骤7中的扩展节点是否为终点，如果不是，则返回步骤5；如果该扩展节点为终点，则将关闭节点中的节点保存为规划路径，即Path1；

在步骤8之后，还包括以下步骤：

步骤9：获取所述路径path1；

步骤10：利用直线拟合算法删除冗余点得到转折点；

步骤11：依次从path1中取pose[i-2],pose[i-1],pose[i]3个转折点进行拟合y(x)¹＝a*x²+b*x+c，同时，判断提取出来的转折点中，是否包含终点，如果含有终点，则进入步骤13,否则执行下一步；

步骤12：对y(x)¹＝a*x²+b*x+c求一阶导和二阶导，带入公式(10)中，判断是否满足公式(11)，如果满足，则进入步骤13；否则移动转折点，判断移动次数是否小于3，如果小于3，更新最小的曲率并重新执行步骤12，如果大于3，则保留曲率最小的转折点，然后进入步骤13；

所述更新最小的曲率包括：移动转折点pose[i-1]，首先求出y(x)¹的对称轴

如公式(12)所示，判断所提取的出来的转折点是否大于

同时，通过判断a的正负号确定y(x)¹的开口朝向，如a＞0且

则将pose[i-1]向左上角移动一个栅格，如果a＞0且

则将pose[i-1]向右上角移动一个栅格，如果a＜0且

则将pose[i-1]向左下角移动一个栅格，如果a＜0且

则将pose[i-1]向右下角移动一个栅格；

步骤13：拟合转折点，进行插值；

步骤14：结束规划，输出路径Path；

K＜K_max (11)

index_new是移动后的节点的索引,index是此刻节点的索引，nx表示地图的宽度。

Images