CN116429144A - 基于改进Astar和DWA融合算法的自主车辆路径规划方法 - Google Patents

Abstract

本发明公开了一种基于改进Astar和DWA融合算法的自主车辆路径规划方法，包括步骤S1：将车辆待经过的平面区域采用栅格法建立栅格地图，并在所述栅格地图中设定出发位置与终点位置；步骤S2：通过基于跳点改进的Astar算法对所述栅格地图进行全局路径的规划；所述全局路径为从所述出发位置至终点位置的全局最优路径；步骤S3：采用B样条曲线对所述全局路径进行平滑处理，获得优化全局路径；步骤S4：基于改进DWA算法对所述优化全局路径进行局部避障路径规划，获取最终规划的全局路径，改进Astar和DWA融合算法可以使局部路径与全局路径贴合得很好，减少了自主车辆从起始节点到目标节点的时间；同时改进Astar和DWA融合算法在贴合全局路径的同时仍然可以实时避障。

Description

基于改进Astar和DWA融合算法的自主车辆路径规划方法

技术领域

本发明涉及自主车辆路径规划技术领域，尤其涉及一种基于改进Astar和DWA融合算法的自主车辆路径规划方法。

背景技术

自主车辆想要完成自动运输任务，需要有自主定位导航功能。实现自主定位导航功能的一个关键技术是路径规划。路径规划是自主车辆根据所执行的任务要求、所在的场景信息以及自身的位姿和目标点等信息，规划出一条最优无碰撞的路径。路径规划可以分为全局路径规划和局部路径规划。

全局路径规划假设环境信息全部静态已知，通过规划出整个路径来实现从起点到终点的导航。Astar算法为常用的全局路径规划，在环境较小时，具有快速响应环境变化、路径直接且具有较高的性价比等优点，但Astar算法也存在不足之处。例如路径折线段多且拐角时角度较大，而且在环境较为复杂时，Astar算法的实时性和搜索效率将受到影响。Astar算法在全局路径中通常是有效且准确的，但是在对Astar算法不断研究学习的过程中发现它还存在一定的问题：

(1)Astar算法需要大量的内存来存储关于环境地图中已经探索过的节点的信息。如果自主车辆所处环境较为复杂，规划的全局路径较长，那么探索过的节点会很多，使得Astar算法的计算量巨大，导致路径规划实时性差，效率低下。(2)在启发式函数不太适合的情况下，Astar算法易于缓慢收敛。在这种情况下，算法可能会在找到最优路径之前探索许多不必要的节点。欧几里得度量法作为启发函数简单、灵活，但是在某些情况下可能并不是最好的启发函数。(3)Astar算法假设环境是静态的，并且在搜索过程中不会改变。这使得它不适用于解决动态环境中的问题，在动态环境中，环境的状态会随时间而变化。在这种情况下，Astar算法通常无法避开障碍物。(4)Astar算法所规划的路径往往是折线段且转角多，在转弯处角度很大或者违反自主车辆运动规律，不利于自主车辆在实际运行中的行驶。

而局部路径规划环境并非全部已知，可能存在动态障碍物等未知环境，需要在行进过程中不断重新规划路径，以适应变化的环境。目前常用的局部路径算法大多没有考虑自主车辆自身的约束，因此可能规划出违反自主车辆运动规律的路径。

发明内容

本发明提供一种基于改进Astar和DWA融合算法的自主车辆路径规划方法，以克服上述技术问题。

为了实现上述目的，本发明的技术方案是：

一种基于改进Astar和DWA融合算法的自主车辆路径规划方法，包括以下步骤：

步骤S1：将车辆待经过的平面区域采用栅格法建立栅格地图，并在所述栅格地图中设定出发位置与终点位置；

步骤S2：通过基于跳点改进的Astar算法对所述栅格地图进行全局路径的规划；所述全局路径为从所述出发位置至终点位置的全局最优路径；

步骤S3：采用B样条曲线对所述全局路径进行平滑处理，获得优化全局路径；

步骤S4：基于改进DWA算法对所述优化全局路径进行局部避障路径规划，获取最终规划的全局路径。

进一步的，步骤S2中所述基于跳点改进的Astar算法对所述栅格地图进行全局路径的规划，具体为

步骤S2.1：设置Openlist与Closelist，所述Openlist用于存放栅格地图未进行代价估计的节点，所述Closelist用于存放构成最终规划路径的节点；

步骤S2.2：根据栅格地图设置起点、目标点以及障碍物节点，并将所述起点加入到Openlist中定义为初始节点；此时所述初始节点为是Openlist中的唯一节点，所述Closelist此时为空表；

步骤S2.3：根据代价估计函数F(n)计算当前Openlist中节点的代价估计函数值，并选择所述节点作为当前节点；

所述代价估计函数F(n)计算公式为

F(n)＝G(n)+H(n)(2.1)

其中，H(n)表示估计当前节点n到目标节点最小代价的欧几里得距离函数；G(n)表示初始节点到当前节点n的实际代价；F(n)表示从初始节点经过节点n到达目标节点的最小代价估计的代价函数；(x₁,y₁)与(x₂,y₂)表示节点坐标；

步骤S2.4：判断当前节点n是否为目标节点，若是，则把当前节点加入至Closelist中输出从初始节点至目标点的全局最优路径；

若不是，则基于跳点搜索算法对当前节点n进行节点扩展，并将扩展后的节点加入至所述Openlist中，基于所述代价估计函数F(n)计算所述Openlist中所有节点的代价估计值，并根据所述代价估计值进行跳点选择；

步骤S2.5：:将选择后的跳点作为最新的当前节点，并从所述Openlist中取出加入所述Closelist中，重复步骤S2.4。

进一步的，所述跳点选择的策略为

步骤S2.3.1：基于跳点搜索算法对栅格地图中当前节点n进行节点搜索扩展获取当前节点的邻域节点，所述节点搜索扩展的扩展方向包括水平方向、竖直方向以及对角线方向；

所述邻域节点包括自然邻居点与强迫邻居点；

且所述自然邻居点的选择规则为：将根据舍弃规则筛选所述邻域节点后，剩下的邻域节点作为自然邻居节点；

所述舍弃规则：对于水平方向与竖直方向搜索扩展筛选公式为

len(＜P,…,N＞/X)≤len(＜P,X,N＞)(2.3)

式中：P表示当前节点X的父节点；＜P,…,N＞/X表示父节点P不经过当前节点X到达节点N的路线，＜P,X,N＞表示父节点P经过当前节点X到达节点N的路线，len()表示路线的距离大小；

对于对角线方向搜索扩展筛选公式为

len(＜P,…,N＞/X)＜len(＜P,X,N＞)(2.4)

所述强迫邻居点的选择规则为：计算栅格地图中当前节点x的父节点到节点x的自然邻居节点的距离时，若当前节点x的其他邻域节点中有障碍物栅格，

且搜索到的邻居节点符合强制点判定规则则将该节点作为强迫邻居点，所述强制点判定规则为

len(＜P,…,M＞/X)＞len(＜P,X,M＞)(2.5)

式中：M表示当前节点x的强迫邻居点；

步骤S2.3.2：所述强迫邻居点基于跳点识别规则寻找跳点，并将跳点放进Openlist中；

所述跳点识别规则可表示为：

式中：x表示当前节点；

表示当前节点x周围节点的拓展方向；k表示地图栅格的数量；y表示朝/>

方向移动k个栅格后拓展至的目标节点；其中移动最小k的目标节点y作为当前节点x的跳点；

且满足以下条件：y是目标节点；且y至少有一个强制邻居点；若选定目标节点y向周围节点的拓展方向为对角线方向

则/>

且

是与/>

为45°的水平或者垂直的方向，则节点z表示为目标节点y的跳点，节点y表示为当前节点x为跳点；

步骤S2.3.3：将所述当前节点的各跳点加入Openlist中，利用代价估计函数F(n)计算当前节点的所有跳点的代价估价值并进行比较，选择代价估价值最小的跳点作为路径选择的下一节点；

将所述代价估价值最小的跳点从Openlist中删除，并加入Closelist中；

步骤S2.3.4：将所述代价估价值最小的跳点作为最新当前节点，重复步骤S2.3.1至步骤S2.3.2，获得最新当前节点的各跳点；

判断所述最新当前节点的各跳点是否已都存在所述Openlist中，若不存在，则将不存在所述Openlist中的所述最新当前节点的跳点加入Openlist中，重复执行步骤S2.3至步骤S2.5；

若存在，则比较最新当前节点的跳点的实际代价值与拥有不同父节点的已经存储在Openlist中的最新当前节点的跳点的实际代价值的大小；

定义所述最新当前节点的跳点的实际代价值为第一代价值，且所述最新当前节点的跳点的父节点定义为第一父节点；

定义所述拥有不同父节点的已经存储在Openlist中的最新当前节点的跳点的实际代价值为第二代价值，且将已经存储在Openlist中的最新当前节点的跳点的父节点为第二父节点；

若所述第一实际代价值大于等于第二实际代价值，则保留所述第一实际代价值与所述第一父节点，重复执行步骤S2.3至步骤S2.5；

若所述第一实际代价值小于第二实际代价值，则将已经存储在Openlist中的最新当前节点的跳点更新为最新当前节点，并更新所述最新当前节点的实际代价值为所述第二实际代价值，重复执行步骤S2.3至步骤S2.5。

进一步的，步骤S3中所述B样条曲线的计算公式为

其中，P_i(i＝0,1,…,n)为需要平滑的点，N_i,k(i＝0,1,…,n)称为k阶B样条基函数；

所述k阶B样条基函数N_i,k(t)，i＝0,1,…,n的递推公式为

其中，t_i表示第i个需要进行平滑的节点的值；t_i+1表示第i+1个需要进行平滑的节点的值；N_i,0(t)表示0阶B样条基函数；N_i,k-1(t)表示k-1阶B样条基函数；N_i+1,k-1(t)表示第i+1个节点的k-1阶B样条基函数。

进一步的，步骤S4中所述获取最终规划的全局路径，具体为

步骤S4.1：建立自主车辆运动学模型，并判断所述自主车辆运动学模型的模拟点是否到达格栅地图中优化全局路径目标终点；

若是，则结束自主车辆运动学模型运动；若否，则基于优化全局路径提取局部规划子目标点，执行步骤S4.2；

步骤S4.2:基于所述局部规划子目标点对所述车辆运动学模型前进的速度空间进行速度动态窗口的计算，获取在自主车辆自身速度约束、加速度约束以及安全速度约束条件约束下的车辆有效速度；

且所述车辆有效速度包括若干线速度与角速度的组合；

步骤S4.3：根据车辆有效速度V_r遍历自主车辆所有的线速度v与角速度ω，并获取在速度空间中每组线速度v与角速度ω在前向模拟时间内的运动轨迹；

步骤S4.4:将所述每组线速度v与角速度ω根据改进Astar和DWA融合算法的轨迹评价函数进行打分，选取自主车辆的最优线速度v与角速度ω；

步骤S4.5:将获取最优的线速度v与角速度ω发送给所述自主车辆，重复执行步骤S4.1至步骤S4.4，直至到达目标终点获取最终规划的全局路径。

进一步的，步骤S4.1中所述车辆运动学模型具体为

基于栅格地图建立X_G-Y_G位置直角坐标系，且设定自主车辆在t时刻的位置为{x(t),y(t)}，所述自主车辆运动方向X_c与X_G轴正方向的夹角为θ(t)；

设置所述自主车辆的后轮轴中点为(x_r,y_r)、前轮轴中点为(x_f,y_f)；且设定所述自主车辆的后轮轴中点(x_r,y_r)为车体坐标系的原点O_c，可得公式：

y′_rcosθ_r-x′_rsinθ_r＝0 (2.9)

式中，y′_r表示自主车辆的后轮轴中点纵坐标y_r的一阶导；x′_r表示自主车辆的后轮轴中点横坐标x_r的一阶导；θ_r表示自主车辆运动方向X_c与X_G轴正方向的夹角；

所述自主车辆的后轮轴中点轨迹可由前后轮坐标之间的关系得到，计算公式为：

式中，l表示自主车辆的长度；

把公式(2.10)代入公式(2.9)中再进行求导，可得公式：

x′_fsinθ_r-y′_fcosθ_r+θ′_rl＝0 (2.11)

式中，y′_f表示自主车辆的前轮轴中点纵坐标y_f的一阶导；x′_f表示自主车辆的前轮轴中点横坐标x_f的一阶导；θ′_r表示θ_r的一阶导；

所述自主车辆的前轮轴中点在x和y方向的分速度如公式(2.12)，将公式(2.12)代入公式(2.11)中，用三角函数展开，得到公式(2.13)：

式中，v表示自主车辆的运动速度；α表示自主车辆的运动速度与自主车辆运动方向X_c之间的夹角；

对公式(2.13)进行积分，得到自主车辆的方向角θ_r，如公式(2.14)；将公式(2.10)、公式(2.12)以及公式(2.14)联立，并采用三角函数展开，得到后轮轴中点在x和y方向的分速度，如公式(2.15)：

将θ_r代入公式(2.15)得到所述自主车辆的后轮轴中点的轨迹方程，如公式(2.16)：

其中，A，B为常数，且A＝x_r-Rsinθ_r，B＝y_r+Rcosθ_r；则自主车辆的后轮轴中心的轨迹方程是一个圆，R＝lcotα；角速度公式ω＝v_r/R；t表示时间。

进一步的，步骤S4.2中所述车辆有效速度V_r的表达式为

V_r＝V_s∩V_d∩V_a(2.17)

式中，V_s表示自主车辆自身的速度约束；V_d表示自主车辆的加速度约束；V_a表示自主车辆的安全速度约束；

所述自主车辆自身的速度约束V_s的表达式为

V_s＝{(v,ω)|v∈[v_min,v_max],ω∈[ω_min,ω_max]} (2.18)

式中，(v,ω)表示自主车辆的线速度和角速度的速度对；v_min,v_max分别表示自主车辆的最小与最大线速度；ω_min,ω_max分别表示自主车辆的最小与最大角速度；

所述自主车辆的加速度约束V_d的表达式为

V_d＝{(v,ω)|v∈[v_c-v′·t,v_c+v′·t],ω∈[ω_c-ω′·t,ω_c+ω′·t]} (2.19)

式中，(v_c,ω_c)表示当前自主车辆的实际线速度和实际角速度的速度对；t表示时间间隔；v′表示v的一阶导；ω′表示ω的一阶导；

所述自主车辆的安全速度约束V_a的表达式为

式中，(v_c,ω_c)表示自主车辆当前的实际速度对；(v′_b,ω′_b)表示自主车辆最大减速度对；dist(v,ω)表示速度对(v,ω)的轨迹上距离最近的障碍物和自主车辆间的距离。

进一步的，步骤S4.4中所述改进Astar和DWA融合算法的轨迹评价函数的公式为

式中，Score(v,ω)表示轨迹评价函数；heading(v,ω)表示自主车辆航向角的评价子函数，dist(v,ω)表示自主车辆与障碍物间距离的安全评价子函数，velocity(v,ω)表示速度评价子函数，α，β，γ均表示为评价子函数的权重系数；η表示全局代价权重系数；G(v,ω)表示自主车辆全局路线代价估计子函数；n表示自主车辆的当前节点；f(n)表示改进Astar算法的代价估计函数。

有益效果：本发明提供了一种基于改进Astar和DWA融合算法的自主车辆路径规划方法，传统Astar算法随着地图环境的变得更大和更加复杂，实时性和搜索效率都会降低，本发明采用跳点搜索的思想进行改进，使得在大地图和复杂环境下比传统Astar算法运行搜索效率高，并且采用B样条曲线算法进行全局路径优化，使优化后的路径转折点角度变小，路径更加平滑、曲率更加平缓；同时，为了克服DWA算法在路径规划中的局限性，构造改进Astar和DWA融合算法的轨迹评价函数，在避开障碍物的同时尽可能贴合全局路径规划。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明基于改进Astar和DWA融合算法的自主车辆路径规划方法流程框图；

图2为本实施例中改进后的Astar算法流程图；

图3为本实施例中改进Astar和DWA融合局部路径规划流程图；

图4为本实施例中自主车辆的运动学模型图；

图5为本实施例中自然邻居点示意图；

图6为本实施例中强迫邻居点示意图；

图7为本实施例中跳点选择示意图；

图8为本实施例中B样条曲线基函数示意图；

图9为本实施例中20*20地图下Astar算法与改进后的Astar算法路径仿真对比图；

图10为本实施例中60*60地图下Astar算法路径仿真图；

图11为本实施例中60*60地图下改进后的Astar算法路径仿真图；

图12为本实施例中20*20地图下改进后的Astar算法与路径平滑后的改进Astar算法路径仿真对比图；

图13为本实施例中60*60地图下改进后的Astar算法路径仿真图；

图14为本实施例中60*60地图下路径平滑后的改进Astar算法路径仿真图；

图15为本实施例中改进Astar算法与路径平滑后的改进Astar算法的局部放大仿真对比图；

图16为本实施例中传统DWA算法路径仿真图；

图17为本实施例中改进Astar和DWA融合算法的路径仿真图；

图18为本实施例中动态环境下传统DWA算法路径仿真图；

图19为本实施例中动态环境下改进Astar和DWA融合算法的路径仿真图。

具体实施方式

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

本实施例提供了一种基于改进Astar和DWA融合算法的自主车辆路径规划方法，如图1所示，包括以下步骤：

步骤S1：将车辆待经过的平面区域采用栅格法建立栅格地图，并在所述栅格地图中设定出发位置与终点位置；步骤S2：通过基于跳点改进的Astar算法对所述栅格地图进行全局路径的规划；所述全局路径为从所述出发位置至终点位置的全局最优路径；步骤S3：采用B样条曲线对所述全局路径进行平滑处理，获得优化全局路径；步骤S4：基于改进DWA算法对所述优化全局路径进行局部避障路径规划，获取最终规划的全局路径。

传统Astar(A-Star)算法随着地图环境的变得更大和更加复杂，算法的实时性和搜索效率都会降低；针对这个问题，采用跳点搜索的思想进行改进，使改进算法在大地图和复杂环境下比传统Astar算法运行搜索效率高，转弯次数少的同时，还可以找到最优路径。针对Astar算法路径折线段多且拐角时角度较大的问题，采用三次B样条曲线算法优化路径，优化后的路径转折点角度变小，路径更加平滑、曲率更加平缓。然后对阿克曼结构的运动学特性进行了分析，为了克服DWA(Dynamic Window Approach)算法在路径规划中的局限性，引入全局路径规划的指导，对其评价函数进行了改进。在评价函数中增加了全局代价子函数，在避开障碍物的同时尽可能贴合全局路径规划。

在具体实施例中，如图2所示，步骤S2中所述基于跳点改进的Astar算法对所述栅格地图进行全局路径的规划，具体为步骤S2.1：设置Openlist与Closelist，所述Openlist用于存放栅格地图未进行代价估计的节点，所述Closelist用于存放构成最终规划路径的节点；步骤S2.2：根据栅格地图设置起点、目标点以及障碍物节点，并将所述起点加入到Openlist中定义为初始节点；此时所述初始节点为是Openlist中的唯一节点，所述Closelist此时为空表；步骤S2.3：根据代价估计函数F(n)计算当前Openlist中节点的代价估计函数值，并选择所述节点作为当前节点；所述代价估计函数F(n)计算公式为

F(n)＝G(n)+H(n)(2.1)

在Astar算法搜索路径中，启发函数H(n)的挑选十分重要；有以下三种情况：

(1)真实H(n)小于估计H(n)，此时搜索节点数量会增多，搜索范围大，但是效率会较低，不过可以保证得到最优路径；

(2)真实H(n)等于估计H(n)，此时搜索效率最高；

(3)真实H(n)大于估计H(n)，此时搜索节点数量减少，搜索范围小，但是搜索效率会增大，但是不能保证得到最优路径。

在Astar算法中，通常使用欧几里得距离作为启发函数H(n)；

其中，H(n)表示估计当前节点n到目标节点最小代价的欧几里得距离函数；G(n)表示初始节点到当前节点n的实际代价；F(n)表示从初始节点经过节点n到达目标节点的最小代价估计的代价函数；(x₁,y₁)与(x₂,y₂)表示节点坐标；

步骤S2.4：判断当前节点n是否为目标节点，若是，则把当前节点加入至Closelist中输出从初始节点至目标点的全局最优路径；若不是，则基于跳点搜索算法对当前节点n进行节点扩展，并将扩展后的节点加入至所述Openlist中，基于所述代价估计函数F(n)计算所述Openlist中所有节点的代价估计值，并根据所述代价估计值进行跳点选择；跳点搜索算法(JPS)背后的思想是利用搜索空间的一些特性来修剪搜索树。它通过识别某些节点(称为跳点)来工作，这些节点可以在搜索过程中跳过。跳点是已知对于某些路径来说是最佳的节点，这意味着所有通过它们的路径必须长于到该节点的最短路径。因此使用跳点搜索策略对Astar算法的搜索树进行修剪，可以减少搜索过程中探索的节点数量。通过识别跳点，能够跳过Astar算法原来将要探索的许多节点。这可以显著减少执行搜索所需的时间和内存量；步骤S2.5：:将选择后的跳点作为最新的当前节点，并从所述Openlist中取出加入所述Closelist中，重复步骤S2.4。

在具体实施例中，所述跳点选择的策略为：步骤S2.3.1：基于跳点搜索算法对栅格地图中当前节点n进行节点搜索扩展获取当前节点的邻域节点，所述节点搜索扩展的扩展方向包括水平方向、竖直方向以及对角线方向；所述邻域节点包括自然邻居点与强迫邻居点；且所述自然邻居点的选择规则为：将根据舍弃规则筛选所述邻域节点后，剩下的邻域节点作为自然邻居节点；

图5是自然邻居点在水平和对角线方向的示意图。其中，箭头表示节点前进方向，X表示当前节点，P表示父节点，定义直线运动代价为10，对角线运动代价为14。图5(a)为水平向右的自然邻居点搜索情况。假设要从当前节点X前往节点8，那么从父节点P到节点8的最短路径有两条，最短距离为24。一条可以不经过节点X就到达节点8，即可走4-7-8的虚线箭头路线，另外一条需要经过节点X到节点8，即可以走4-X-8的实线箭头路线。如果从父节点P到节点n存在一条路径，该路径不需要经过当前节点X，且路径长度小于或等于需要经过节点X的路径长度，那么节点n是当前节点X的可舍弃邻居点，否则，节点n当前节点X的自然邻居点。因此，在图5(a)中只有节点5是当前节点X的自然邻居点。在进行水平方向搜索扩展时，对于水平方向与竖直方向搜索扩展筛选公式为

len(＜P,…,N＞/X)≤len(＜P,X,N＞)(2.3)

式中：P表示当前节点X的父节点；＜P,…,N＞/X表示父节点P不经过当前节点X到达节点N的路线，＜P,X,N＞表示父节点P经过当前节点X到达节点N的路线，len()表示路线的距离大小；

对于对角线方向搜索扩展筛选公式为

len(＜P,…,N＞/X)＜len(＜P,X,N＞)(2.4)

图5(b)为对角线的自然邻居点搜索情况。假设要从当前节点X前往节点8，那么从父节点P到节点8的最短路径是虚线箭头路线，最短距离为20，并且不经过当前节点X，即可走6-7-8的路线。如果从父节点P到节点n存在一条路径，该路径不需要经过当前节点X，且路径长度小于需要经过节点X的路径长度，那么节点n是当前节点X的可舍弃邻居点。因此，在图5(b)中节点2、3和5，是当前节点X的自然邻居点。

且搜索到的邻居节点符合强制点判定规则则将该节点作为强迫邻居点，所述强迫邻居点的选择规则为：计算栅格地图中当前节点x的父节点到节点x的自然邻居节点的距离时，若当前节点x的其他邻域节点中有障碍物栅格，图6是强迫邻居点在水平方向和对角线方向的示意图，在图6(a)中的黑色方格2和图6(b)中的黑色方格4表示障碍物；如果当前节点的邻节点中有障碍物，并且节点M满足公式(2.5)，则称此节点为当前节点X的强制邻居点；

len(＜P,…,M＞/X)＞len(＜P,X,M＞)(2.5)

式中：M表示当前节点x的强迫邻居点；

因此图6(a)中节点3为当前节点X的强迫邻居点，图6(b)中节点1为当前节点X的强迫邻居点；

步骤S2.3.2：所述强迫邻居点基于跳点识别规则寻找跳点，并将跳点放进Openlist中；所述跳点识别规则可表示为：

式中：x表示当前节点；

表示当前节点x周围节点的拓展方向；k表示地图栅格的数量；y表示朝/>

方向移动k个栅格后拓展至的目标节点；其中移动最小k的目标节点y作为当前节点x的跳点；

且满足以下条件：y是目标节点；且y至少有一个强制邻居点；若选定目标节点y向周围节点的拓展方向为对角线方向

则/>

且

是与/>

为45°的水平或者垂直的方向，则节点z表示为目标节点y的跳点，节点y表示为当前节点x为跳点，如图7所示；

步骤S2.3.3：将所述当前节点的各跳点加入Openlist中，利用代价估计函数F(n)计算当前节点的所有跳点的代价估价值并进行比较，选择代价估价值最小的跳点作为路径选择的下一节点；将所述代价估价值最小的跳点从Openlist中删除，并加入Closelist中；

步骤S2.3.4：将所述代价估价值最小的跳点作为最新当前节点，重复步骤S2.3.1至步骤S2.3.2，获得最新当前节点的各跳点；

判断所述最新当前节点的各跳点是否已都存在所述Openlist中，若不存在，则将不存在所述Openlist中的所述最新当前节点的跳点加入Openlist中，重复执行步骤S2.3至步骤S2.5；

若存在，则比较最新当前节点的跳点的实际代价值与拥有不同父节点的已经存储在Openlist中的最新当前节点的跳点的实际代价值的大小；

定义所述最新当前节点的跳点的实际代价值为第一代价值，且所述最新当前节点的跳点的父节点定义为第一父节点；

定义所述拥有不同父节点的已经存储在Openlist中的最新当前节点的跳点的实际代价值为第二代价值，且将已经存储在Openlist中的最新当前节点的跳点的父节点为第二父节点；

若所述第一实际代价值大于等于第二实际代价值，则保留所述第一实际代价值与所述第一父节点，重复执行步骤S2.3至步骤S2.5；

若所述第一实际代价值小于第二实际代价值，则将已经存储在Openlist中的最新当前节点的跳点更新为最新当前节点，并更新所述最新当前节点的实际代价值为所述第二实际代价值，重复执行步骤S2.3至步骤S2.5。用跳点思想改进Astar算法是在节点扩展前先进行寻找跳点操作，和传统Astar算法每次探索节点时都需要将八邻域节点都计算一遍并加入到Openlist中相比，跳点改进Astar算法只需要将搜索到的跳点加入到Openlist中，大大降低了Openlist计算和维护需要的时间，从而提高搜索路径的效率，有效降低内存占用。

在具体实施例中，B样条曲线是一种广泛应用于计算机图形学、计算机辅助设计(CAD)等领域的曲线；它是一种局部控制曲线，具有高度的灵活性和良好的数学性质，B样条曲线的构造基于多项式基函数和控制点，其中多项式基函数决定了曲线的形状，控制点决定了曲线的位置，且k阶B样条曲线的表达式为

其中，P_i(i＝0,1,…,n)为需要平滑的点，N_i,k(i＝0,1,…,n)称为k阶B样条基函数；

所述k阶B样条基函数N_i,k(t)，i＝0,1,…,n的递推公式为

其中，t_i表示第i个需要进行平滑的节点的值；t_i+1表示第i+1个需要进行平滑的节点的值；N_i,0(t)表示0阶B样条基函数；N_i,k-1(t)表示k-1阶B样条基函数；N_i+1,k-1(t)表示第i+1个节点的k-1阶B样条基函数。

B样条曲线具有以下性质：(1)凸包性：B样条曲线位于由其控制顶点组成的凸包内部；(2)几何不变性：B样条曲线的形状和位置与坐标系的选择无关；(3)局部支撑性：更改控制顶点会影响曲线的局部形状；(4)非负性：B样条函数均为正值。传统Astar算法规划的路径折线段多且拐角时角度较大，导致自主车辆转向困难，这使得自主车辆的运动效率降低；改进Astar算法虽然提高了寻路效率，但生成的路径转弯时拐角度数依然很大；因此，对生成的路径进行平滑处理是必要的。公式(2.8)是一个递推公式，它可以用低阶的B样条基函数的线性组合表示高阶B样条基函数；具体来说，对于任意正整数k，N_i,k(t)可以表示为(k-1)次B样条基函数N_i,k-1(t)和N_i+1,k-1(t)的线性组合；图8中的(a),(b),(c)中的黑色实线分别表示1、2和3次的B样条基函数，蓝色虚线分别表示构成对应函数的两个较低次的B样条基函数。例如，在图8(c)中，三次B样条基函数N_i,3由两个较低次B样条基函数N_i,2和N_i+1,2线性组合而成。

在具体实施例中，如图3所示，步骤S4中所述获取最终规划的全局路径，具体为：步骤S4.1：建立自主车辆运动学模型，并判断所述自主车辆运动学模型的模拟点是否到达格栅地图中优化全局路径目标终点；若是，则结束自主车辆运动学模型运动；若否，则基于优化全局路径提取局部规划子目标点，执行步骤S4.2；步骤S4.2:基于所述局部规划子目标点对所述车辆运动学模型前进的速度空间进行速度动态窗口的计算，获取在自主车辆自身速度约束、加速度约束以及安全速度约束条件约束下的车辆有效速度；且所述车辆有效速度包括若干线速度与角速度的组合；步骤S4.3：根据车辆有效速度V_r遍历自主车辆所有的线速度v与角速度ω，并获取在速度空间中每组线速度v与角速度ω在前向模拟时间内的运动轨迹；步骤S4.4:将所述每组线速度v与角速度ω根据改进Astar和DWA融合算法的轨迹评价函数进行打分，选取自主车辆的最优线速度v与角速度ω；步骤S4.5:将获取最优的线速度v与角速度ω发送给所述自主车辆，重复执行步骤S4.1至步骤S4.4，直至到达目标终点获取最终规划的全局路径。

在具体实施例中，大部分自主车辆采用阿克曼结构，其在转向时受到最小转弯半径的限制；此外，由于自主车辆自身具有速度、加速度等运动学限制，因此需要对其进行运动学建模，步骤S4.1中所述车辆运动学模型具体为：基于栅格地图建立X_G-Y_G位置直角坐标系，且设定自主车辆在t时刻的位置为{x(t),y(t)}，所述自主车辆运动方向X_c与X_G轴正方向的夹角为θ(t)；如图4所示，设置所述自主车辆的后轮轴中点为(x_r,y_r)、前轮轴中点为(x_f,y_f)；且设定所述自主车辆的后轮轴中点(x_r,y_r)为车体坐标系的原点O_c，把运动学模型看成一个刚体，那么垂直方向速度分量的和为0，可得公式：

y′_rcosθ_r-x′_rsinθ_r＝0 (2.9)

式中，y′_r表示自主车辆的后轮轴中点纵坐标y_r的一阶导；x′_r表示自主车辆的后轮轴中点横坐标x_r的一阶导；θ_r表示自主车辆运动方向X_c与X_G轴正方向的夹角；所述自主车辆的后轮轴中点轨迹可由前后轮坐标之间的关系得到，计算公式为：

式中，l表示自主车辆的长度；

把公式(2.10)代入公式(2.9)中再进行求导，可得公式：

x′_fsinθ_r-y′_fcosθ_r+θ′_rl＝0 (2.11)

式中，y′_f表示自主车辆的前轮轴中点纵坐标y_f的一阶导；x′_f表示自主车辆的前轮轴中点横坐标x_f的一阶导；θ′_r表示θ_r的一阶导；所述自主车辆的前轮轴中点在x和y方向的分速度如公式(2.12)，将公式(2.12)代入公式(2.11)中，用三角函数展开，得到公式(2.13)：

式中，v表示自主车辆的运动速度；α表示自主车辆的运动速度与自主车辆运动方向X_c之间的夹角；对公式(2.13)进行积分，得到自主车辆的方向角θ_r，如公式(2.14)；将公式(2.10)、公式(2.12)以及公式(2.14)联立，并采用三角函数展开，得到后轮轴中点在x和y方向的分速度，如公式(2.15)：

将θ_r代入公式(2.15)得到所述自主车辆的后轮轴中点的轨迹方程，如公式(2.16)：

其中，A，B为常数；t表示时间；且A＝x_r-Rsinθ_r，B＝y_r+Rcosθ_r；因此后轮轴中心的轨迹方程是一个圆，即R＝lcotα；又有角速度公式ω＝v_r/R，如果角速度为0，那么自主车辆的运动轨迹是直线，假设自主车辆的线速度和角速度在未来的一小段时间恒定不变，则自主车辆在这段时间内的轨迹可以用公式(2.16)进行预测。

在具体实施例中，在DWA算法中，为了保证自主车辆能够在动态环境中安全运行，需要对自主车辆的速度进行约束，DWA算法有三种速度空间限制，步骤S4.2中所述车辆有效速度V_r的表达式为

V_r＝V_s∩V_d∩V_a (2.17)

式中，V_s表示自主车辆自身的速度约束；V_d表示自主车辆的加速度约束；V_a表示自主车辆的安全速度约束；

首先，自主车辆的线速度和角速度在某一时刻都存在限制，设V_s为自主车辆自身速度约束，则所述自主车辆自身的速度约束V_s的表达式为

V_s＝{(v,ω)|v∈[v_min,v_max],ω∈[ω_min,ω_max]} (2.18)

式中，(v,ω)表示自主车辆的线速度和角速度的速度对；v_min,v_max分别表示自主车辆的最小与最大线速度；ω_min,ω_max分别表示自主车辆的最小与最大角速度；其次，自主车辆的加速度受到电机性能的限制；因此，存在一个动态窗口，表示自主车辆在下一个模拟周期中实际能够达到的速度范围；设t为时间间隔，(v_c,ω_c)分别表示当前自主车辆的实际线速度和角速度，所述自主车辆的加速度约束V_d的表达式为

V_d＝{(v,ω)|v∈[v_c-v′·t,v_c+v′·t],ω∈[ω_c-ω′·t,ω_c+ω′·t]} (2.19)

式中，(v_c,ω_c)表示当前自主车辆的实际线速度和实际角速度的速度对；t表示时间间隔；v′表示v的一阶导；ω′表示ω的一阶导；最后，自主车辆运动过程中必须避开障碍物，因此在DWA算法中还要考虑障碍物对自主车辆运动的影响。具体来说，自主车辆的速度必须使得自主车辆能够在规定时间内安全避开障碍物，因此需要根据障碍物的位置等因素，对速度采样空间进行进一步的限制；所述自主车辆的安全速度约束V_a的表达式为

式中，(v_c,ω_c)表示自主车辆当前的实际速度对；(v′_b,ω′_b)表示自主车辆最大减速度对；dist(v,ω)表示速度对(v,ω)的轨迹上距离最近的障碍物和自主车辆间的距离。

在具体实施例中，改进后的Astar算法没有考虑未知环境局部避障的情况，同时也没有考虑自主车辆本身的动力学特征，所以如果自主车辆按照此规划路径进行运行，可能会出现与障碍物相碰撞或者运动不连贯的情况。而DWA算法能够在运行过程中实时动态规划路径，从而弥补了改进Astar算法在局部避障上的问题。然而，传统的DWA算法没有全局路径规划的指引，因此在实际路径规划时往往不能得到最优路径，为了解决这一问题，可以提取改进后Astar算法生成的全局路径节点，并将其融合到DWA算法的评价函数中，获得改进Astar和DWA融合算法的轨迹评价函数，步骤S4.4中所述改进Astar和DWA融合算法的轨迹评价函数的公式为

式中，Score(v,ω)表示轨迹评价函数；heading(v,ω)表示自主车辆航向角的评价子函数，dist(v,ω)表示自主车辆与障碍物间距离的安全评价子函数，velocity(v,ω)表示速度评价子函数，α，β，γ均表示为评价子函数的权重系数；η表示全局代价权重系数；G(v,ω)表示自主车辆全局路线代价估计子函数；n表示自主车辆的当前节点；f(n)表示改进Astar算法的代价估计函数。

heading(v,ω)的计算方式为heading(v,ω)＝180-θ，这是一个评估自主车辆当前行驶方向与目标方向一致程度的指标。θ是自主车辆在预测位置处的行驶方向与目标点方向之间的夹角。当该夹角越小时，表示自主车辆的行驶方向越接近目标点方向，所得的得分就越高，反之则越低。dist(v,ω)用来评估轨迹的安全性。可以采用两种方式进行表达：第一种是判断轨迹与障碍物的相交情况。如果相交，则直接舍弃该轨迹；第二种形式是计算模拟轨迹上自主车辆与最近障碍物之间的距离。velocity(v,ω)是在确保正确的方向和安全轨迹的前提下，尽可能提高自主车辆的速度，以便提高效率。为了平衡三个评价子函数的权重，通常需要对它们进行归一化处理，以便使不同轨迹之间的评分差别不会过大，从而使得轨迹更加平滑。鉴于改进Astar算法可以规划出全局最优路径，因此把全局的代价函数融入到DWA算法的评价函数中，以在评价函数中提供全局指导作用。

仿真结果对比如下：改进Astar算法仿真对比：在不同的栅格地图下，对Astar算法和跳点改进Astar算法进行对比仿真实验，实验的主要性能指标有：转弯次数，节点数量，路径长度，搜索时间，占用内存。仿真所使用的计算机参数为：R5-4600H处理器，16G内存，Windows10运行系统，使用的仿真环境为MATLAB R2020a。

如图9至图11所示分别是20*20和60*60栅格地图下Astar算法和跳点改进Astar算法的路径规划结果。其中黑色矩形区域代表不可通行的障碍物，空白矩形区域代表可通过区域，矩形a表示起始节点，矩形b表示目标节点。矩形a与矩形b之间的连线代表算法搜索出的最优路径，算法路径允许沿着直线和对角线方向移动。

从图9至图11的对比中可以看出在不同的地图大小中，改进Astar算法规划出来的路径拐弯次数明显减少。两种算法都能从起始点搜索出前往目标点的路径。为了更加直观形象的观察上述算法的路径规划特性，本文通过表格的方式将算法在三种不同规格下的地图中规划的路径长度、转弯次数、搜索时间、节点个数等进行了对比，具体如表3.1所示。

表3.1不同栅格地图下两种算法仿真结果对比

由表3.1可以看出，在相同的栅格地图环境下，两种路径算法都能够搜索到最短路径，只是在较小的栅格地图中，改进算法的转弯次数和节点个数相对较少。随着栅格地图的不断增大，与Astar算法相比，改进算法大大减少了搜索中的节点个数，降低了搜索时间，从而有效减少内存资源的消耗，并且转弯次数大大减少。转弯次数减少表明在实际运行中自主车辆可以更好的进行路径规划与跟踪控制而降低撞到障碍物的概率。综上所述，改进Astar算法要优于传统Astar算法。

路径平滑仿真对比：如图12和图14分别为20*20和60*60的栅格地图下对改进Astar算法用准均匀三次B样条曲线进行仿真对比实验。将图中拐点较多的地方进行放大处理，得到局部对比图15所示。

从图12和图14可以看出，在不同的栅格地图下，使用三次B样条曲线后，路径都变得更加连续平滑。而从图15可以看出，经过路径平滑处理后，自主车辆从起始点避开静态障碍物到达目标点的同时，消除了路径平滑前节点(31,22)，(31,26)，(33,28)，(34,28)，(38,22)，(41,32)等处的折线大拐角。这样在实际移动过程中自主车辆的转向移动会更加自然，提高了实际运动效率和可行性。

改进Astar和DWA融合仿真对比：为了验证融合算法的有效性，进行了两组仿真实验的对比。第一组是在所有障碍物都已知的静态环境下进行。第二组实验的地图参数与静态环境相同，不过在自主车辆的运动过程中会加入未知障碍物，验证融合算法的动态避障能力。设置栅格地图的大小为20*20，对两种算法的权重系数进行相同的设置，α,β和γ分别为0.05,0.2,0.3。而融合算法的全局代价的权重系数η取值为0.1时效果较好。此外，传统DWA算法和融合算法的其他基本参数设置也相同，如表3.2所示。

表3.2DWA算法基本参数

(1)基于静态环境中的仿真

按照设置好的参数，对传统DWA算法和融合算法进行仿真。仿真结果如图16至图17所示。其中黑色矩形区域表示障碍物，空白矩形区域表示可通行区域，矩形a表示自主车辆的起始节点(2,2)，矩形b表示自主车辆的目标节点(19,19)。矩形a与矩形b之间的虚线为改进Astar算法的全局规划路径，图16中矩形a与矩形b之间的实线为传统DWA算法的规划路径，图17中矩形a与矩形b之间的实线为融合算法的规划路径。从图16可以看出，传统的DWA算法只能考虑自身附近的局部障碍物信息，规划出来的路径转弯次数多且转弯幅度较大，与全局路径存在较大偏差；相比之下，融合算法利用全局路径的规划点来引导DWA算法，并在评价函数中增加了全局代价子函数，通过综合考虑多个评价因素，使其规划结果能够较好的贴合全局路径，进一步提高了规划结果的质量和可靠性。

为了进一步对比传统DWA算法和融合算法，选取路径长度和行驶时间进行对比；如表3.3所示。

表3.3算法仿真数据对比

从表3.3可以看出和传统DWA算法相比，融合算法的路径更短，从27.06m缩短到24.74m，并且路径转弯次数减少且转弯幅度小，同时更加贴合全局路径规划，最终行驶时间也由84.24s缩短到78.18s，时间节省了7.2％，加快了自主车辆的行驶速度。

(2)基于动态环境中的仿真

在静态环境的基础上加入未知静态障碍物和动态障碍物。如图18至图19所示，其中矩形c表示未知静态障碍物，有三个分别为(12,14)，(14,12)和(15,12)；矩形d表示动态障碍物，其路径由实线x表示，起始节点为(5,9)，终止节点为(5,7)，速度大小为0.26m/s。从图18至图19可以看出改进Astar算法由于使用的是预先提供的地图信息，因此无法有效地应对出现的未知静态障碍物和动态障碍物。而传统DWA算法和融合算法都可以规避未知静态障碍物和动态障碍物。但是可以看出传统DWA算法与全局路径规划的路径相差甚远，而融合算法则是在避开所有障碍物的同时还能很好的贴合全局规划。

与静态环境下相同，为了进一步对比传统DWA算法和融合算法，选取路径长度和行驶时间进行对比，如表3.4所示。

表3.4动态环境下算法仿真数据对比

从表3.4可以看出在动态环境下和传统DWA算法相比，融合算法把路径长度从27.27m缩短到25.16m，路径长度变短的同时也很好的避开了未知静态障碍物和动态障碍物，同时更加贴合全局路径规划，最终行驶时间也由87.86s缩短到79.89s，时间节省了9.7％，加快了自主车辆的行驶速度。

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

Claims (8)

1.一种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，包括以下步骤：

步骤S1：将车辆待经过的平面区域采用栅格法建立栅格地图，并在所述栅格地图中设定出发位置与终点位置；

步骤S2：通过基于跳点改进的Astar算法对所述栅格地图进行全局路径的规划；所述全局路径为从所述出发位置至终点位置的全局最优路径；

步骤S3：采用B样条曲线对所述全局路径进行平滑处理，获得优化全局路径；

步骤S4：基于改进DWA算法对所述优化全局路径进行局部避障路径规划，获取最终规划的全局路径。

2.根据权利要求1所述的种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，步骤S2中所述基于跳点改进的Astar算法对所述栅格地图进行全局路径的规划，具体为

步骤S2.1：设置Openlist与Closelist，所述Openlist用于存放栅格地图未进行代价估计的节点，所述Closelist用于存放构成最终规划路径的节点；

步骤S2.2：根据栅格地图设置起点、目标点以及障碍物节点，并将所述起点加入到Openlist中定义为初始节点；此时所述初始节点为是Openlist中的唯一节点，所述Closelist此时为空表；

步骤S2.3：根据代价估计函数F(n)计算当前Openlist中节点的代价估计函数值，并选择所述节点作为当前节点；

所述代价估计函数F(n)计算公式为

F(n)＝G(n)+H(n)(2.1)

其中，H(n)表示估计当前节点n到目标节点最小代价的欧几里得距离函数；G(n)表示初始节点到当前节点n的实际代价；F(n)表示从初始节点经过节点n到达目标节点的最小代价估计的代价函数；(x₁,y₁)与(x₂,y₂)表示节点坐标；

步骤S2.4：判断当前节点n是否为目标节点，若是，则把当前节点加入至Closelist中输出从初始节点至目标点的全局最优路径；

若不是，则基于跳点搜索算法对当前节点n进行节点扩展，并将扩展后的节点加入至所述Openlist中，基于所述代价估计函数F(n)计算所述Openlist中所有节点的代价估计值，并根据所述代价估计值进行跳点选择；

步骤S2.5：:将选择后的跳点作为最新的当前节点，并从所述Openlist中取出加入所述Closelist中，重复步骤S2.4。

3.根据权利要求2所述的种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，所述跳点选择的策略为

步骤S2.3.1：基于跳点搜索算法对栅格地图中当前节点n进行节点搜索扩展获取当前节点的邻域节点，所述节点搜索扩展的扩展方向包括水平方向、竖直方向以及对角线方向；

所述邻域节点包括自然邻居点与强迫邻居点；

且所述自然邻居点的选择规则为：将根据舍弃规则筛选所述邻域节点后，剩下的邻域节点作为自然邻居节点；

所述舍弃规则：对于水平方向与竖直方向搜索扩展筛选公式为

len(＜P,…,N＞/X)≤len(＜P,X,N＞)(2.3)式中：P表示当前节点X的父节点；＜P,…,N＞/X表示父节点P不经过当前节点X到达节点N的路线，＜P,X,N＞表示父节点P经过当前节点X到达节点N的路线，len()表示路线的距离大小；

对于对角线方向搜索扩展筛选公式为len(＜P,…,N＞/X)＜len(＜P,X,N＞)(2.4)所述强迫邻居点的选择规则为：计算栅格地图中当前节点x的父节点到节点x的自然邻居节点的距离时，若当前节点x的其他邻域节点中有障碍物栅格，且搜索到的邻居节点符合强制点判定规则则将该节点作为强迫邻居点，所述强制点判定规则为len(＜P,…,M＞/X)＞len(＜P,X,M＞)(2.5)式中：M表示当前节点x的强迫邻居点；

步骤S2.3.2：所述强迫邻居点基于跳点识别规则寻找跳点，并将跳点放进Openlist中；

所述跳点识别规则可表示为：

式中：x表示当前节点；

表示当前节点x向周围节点的拓展方向；k表示地图栅格的数量；y表示朝/>

方向移动k个栅格后拓展至的目标节点；其中移动最小k的目标节点y作为当前节点x的跳点；

且满足以下条件：y是目标节点；且y至少有一个强制邻居点；若选定目标节点y向周围节点的拓展方向为对角线方向

则/>

且

是与/>

为45°的水平或者垂直的方向，则节点z表示为目标节点y的跳点，节点y表示为当前节点x为跳点；

步骤S2.3.3：将所述当前节点的各跳点加入Openlist中，利用代价估计函数F(n)计算当前节点的所有跳点的代价估价值并进行比较，选择代价估价值最小的跳点作为路径选择的下一节点；

将所述代价估价值最小的跳点从Openlist中删除，并加入Closelist中；

步骤S2.3.4：将所述代价估价值最小的跳点作为最新当前节点，重复步骤S2.3.1至步骤S2.3.2，获得最新当前节点的各跳点；

判断所述最新当前节点的各跳点是否已都存在所述Openlist中，若不存在，则将不存在所述Openlist中的所述最新当前节点的跳点加入Openlist中，重复执行步骤S2.3至步骤S2.5；

若存在，则比较最新当前节点的跳点的实际代价值与拥有不同父节点的已经存储在Openlist中的最新当前节点的跳点的实际代价值的大小；

定义所述最新当前节点的跳点的实际代价值为第一代价值，且所述最新当前节点的跳点的父节点定义为第一父节点；

定义所述拥有不同父节点的已经存储在Openlist中的最新当前节点的跳点的实际代价值为第二代价值，且将已经存储在Openlist中的最新当前节点的跳点的父节点为第二父节点；

若所述第一实际代价值大于等于第二实际代价值，则保留所述第一实际代价值与所述第一父节点，重复执行步骤S2.3至步骤S2.5；

若所述第一实际代价值小于第二实际代价值，则将已经存储在Openlist中的最新当前节点的跳点更新为最新当前节点，并更新所述最新当前节点的实际代价值为所述第二实际代价值，重复执行步骤S2.3至步骤S2.5。

4.根据权利要求1所述的种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，步骤S3中所述B样条曲线的计算公式为

其中，P_i(i＝0,1,…,n)为需要平滑的点，N_i,k(i＝0,1,…,n)称为k阶B样条基函数；

所述k阶B样条基函数N_i,k(t)，i＝0,1,…,n的递推公式为

其中，t_i表示第i个需要进行平滑的节点的值；t_i+1表示第i+1个需要进行平滑的节点的值；N_i,0(t)表示0阶B样条基函数；N_i,k-1(t)表示k-1阶B样条基函数；N_i+1,k-1(t)表示第i+1个节点的k-1阶B样条基函数。

5.根据权利要求1所述的种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，步骤S4中所述获取最终规划的全局路径，具体为

步骤S4.1：建立自主车辆运动学模型，并判断所述自主车辆运动学模型的模拟点是否到达格栅地图中优化全局路径目标终点；

若是，则结束自主车辆运动学模型运动；若否，则基于优化全局路径提取局部规划子目标点，执行步骤S4.2；

步骤S4.2:基于所述局部规划子目标点对所述车辆运动学模型前进的速度空间进行速度动态窗口的计算，获取在自主车辆自身速度约束、加速度约束以及安全速度约束条件约束下的车辆有效速度；

且所述车辆有效速度包括若干线速度与角速度的组合；

步骤S4.3：根据车辆有效速度V_r遍历自主车辆所有的线速度v与角速度ω，并获取在速度空间中每组线速度v与角速度ω在前向模拟时间内的运动轨迹；

步骤S4.4:将所述每组线速度v与角速度ω根据改进Astar和DWA融合算法的轨迹评价函数进行打分，选取自主车辆的最优线速度v与角速度ω；

步骤S4.5:将获取最优的线速度v与角速度ω发送给所述自主车辆，重复执行步骤S4.1至步骤S4.4，直至到达目标终点获取最终规划的全局路径。

6.根据权利要求5所述的种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，步骤S4.1中所述车辆运动学模型具体为

基于栅格地图建立X_G-Y_G位置直角坐标系，且设定自主车辆在t时刻的位置为{x(t),y(t)}，所述自主车辆运动方向X_c与X_G轴正方向的夹角为θ(t)；

设置所述自主车辆的后轮轴中点为(x_r,y_r)、前轮轴中点为(x_f,y_f)；且设定所述自主车辆的后轮轴中点(x_r,y_r)为车体坐标系的原点O_c，可得公式：

y′_rcosθ_r-x′_rsinθ_r＝0 (2.9)

式中，y′_r表示自主车辆的后轮轴中点纵坐标y_r的一阶导；x′_r表示自主车辆的后轮轴中点横坐标x_r的一阶导；θ_r表示自主车辆运动方向X_c与X_G轴正方向的夹角；

所述自主车辆的后轮轴中点轨迹可由前后轮坐标之间的关系得到，计算公式为：

式中，l表示自主车辆的长度；

把公式(2.10)代入公式(2.9)中再进行求导，可得公式：

x′_fsinθ_r-y′_fcosθ_r+θ′_rl＝0 (2.11)

式中，y′_f表示自主车辆的前轮轴中点纵坐标y_f的一阶导；x′_f表示自主车辆的前轮轴中点横坐标x_f的一阶导；θ′_r表示θ_r的一阶导；

所述自主车辆的前轮轴中点在x和y方向的分速度如公式(2.12)，将公式(2.12)代入公式(2.11)中，用三角函数展开，得到公式(2.13)：

式中，v表示自主车辆的运动速度；α表示自主车辆的运动速度与自主车辆运动方向X_c之间的夹角；

对公式(2.13)进行积分，得到自主车辆的方向角θ_r，如公式(2.14)；将公式(2.10)、公式(2.12)以及公式(2.14)联立，并采用三角函数展开，得到后轮轴中点在x和y方向的分速度，如公式(2.15)：

将θ_r代入公式(2.15)得到所述自主车辆的后轮轴中点的轨迹方程，如公式(2.16)：

其中，A，B为常数，且A＝x_r-Rsinθ_r，B＝y_r+Rcosθ_r；则自主车辆的后轮轴中心的轨迹方程是一个圆，R＝lcotα；角速度公式ω＝v_r/R；t表示时间。

7.根据权利要求5所述的种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，步骤S4.2中所述车辆有效速度V_r的表达式为

V_r＝V_s∩V_d∩V_a (2.17)

式中，V_s表示自主车辆自身的速度约束；V_d表示自主车辆的加速度约束；V_a表示自主车辆的安全速度约束；

所述自主车辆自身的速度约束V_s的表达式为

V_s＝{(v,ω)|v∈[v_min,v_max],ω∈[ω_min,ω_max]} (2.18)

式中，(v,ω)表示自主车辆的线速度和角速度的速度对；v_min,v_max分别表示自主车辆的最小与最大线速度；ω_min,ω_max分别表示自主车辆的最小与最大角速度；

所述自主车辆的加速度约束V_d的表达式为

V_d＝{(v,ω)|v∈[v_c-v′·t,v_c+v′·t],ω∈[ω_c-ω′·t,ω_c+ω′·t]} (2.19)

式中，(v_c,ω_c)表示当前自主车辆的实际线速度和实际角速度的速度对；t表示时间间隔；v′表示v的一阶导；ω′表示ω的一阶导；

所述自主车辆的安全速度约束V_a的表达式为

式中，(v_c,ω_c)表示自主车辆当前的实际速度对；(v′_b,ω′_b)表示自主车辆最大减速度对；dist(v,ω)表示速度对(v,ω)的轨迹上距离最近的障碍物和自主车辆间的距离。

8.根据权利要求5所述的种基于改进Astar和DWA融合算法的自主车辆路径规划方法，其特征在于，步骤S4.4中所述改进Astar和DWA融合算法的轨迹评价函数的公式为

式中，Score(v,ω)表示轨迹评价函数；heading(v,ω)表示自主车辆航向角的评价子函数，dist(v,ω)表示自主车辆与障碍物间距离的安全评价子函数，velocity(v,ω)表示速度评价子函数，α，β，γ均表示为评价子函数的权重系数；η表示全局代价权重系数；G(v,ω)表示自主车辆全局路线代价估计子函数；n表示自主车辆的当前节点；f(n)表示改进Astar算法的代价估计函数。

Images