CN111397622A - 基于改进A*算法与Morphin算法的智能汽车局部路径规划方法 - Google Patents

Abstract

本发明公开一种基于改进A*算法与Morphin算法的智能汽车局部路径规划方法，当智能汽车在行驶过程中利用A*算法来进行路径规划时，采用栅格边长可变的方法对传统A*算法进行改进，当监测到智能汽车距离障碍车辆较远时，将栅格边长设置较大一点，缩短计算时间，提升规划速度和效率，更能满足车辆高速行驶时的实时性要求；在改进后的A*算法规划出一条初始路径之后，通过引入Morphin算法，针对A*算法规划出的初始路径进行平滑优化，使其满足车辆的运动学约束，更利于车辆跟踪；通过设置滚动周期来进行滚动规划，每经过一个滚动周期检测一遍规划路径的剩余路径上是否有障碍物，若有障碍物则重新规划路径，使规划出的路径能够适应复杂多变的道路环境。

Description

基于改进A*算法与Morphin算法的智能汽车局部路径规划 方法

技术领域

本发明涉及智能汽车控制领域，具体是智能汽车的局部路径规划方法，是基于改进A* 算法与Morphin算法的智能汽车局部路径规划方法。

背景技术

在众多的交通事故中，因人为因素导致的交通事故占据了极大的比重，而无人驾驶汽车作为减少交通事故的有力手段正吸引着众多学者的研究。无人驾驶汽车在执行的过程中包含有环境感知、行为决策、路径规划和轨迹跟踪等环节，其中路径规划负责将决策层的宏观命令解释为一条可供车辆跟踪的路径曲线，同时要保证规划出的路径能够适应车辆高速运动的实时性以及周围道路环境的复杂性，在整个无人驾驶汽车的执行过程中起到了承前启后的作用。

路径规划的核心就是算法的设计，而A*算法作为一个经典的寻路算法，目前被广泛用于无人驾驶汽车的路径规划领域，如中国专利申请号为201710794646.5的文献中提出的一种基于A*算法的最短寻路方法，该方法首先建立栅格地图，接着设置起点和目标点，然后利用A*算法在栅格地图上进行搜索，该方法得益于A*算法的原理，可以保证搜索到的路径最短，但该方法存在搜索时间长，不能满足车辆高速运动时的实时性要求。又如中国专利申请号为201910494421.7文献中提出的一种基于A*算法的自动驾驶无人车局部路径规划方法，该方法在设置好起始点和目标点以及无人车移动步长后，分别从起点向目标点方向和从目标点向起点方向同时使用A*算法搜索路径，并在无人车移动一个步长之后刷新栅格地图，并将无人车新位置设置为新起点，重新规划，该方法可以缩短规划时间，但是该方法在刷新栅格地图之后没有重新选择目标点，导致无法满足车辆行驶时周围环境的高复杂性，降低了规划路径的安全性，同时该方法所规划出的路径存在很多折点，不能很好的满足车辆的动力学约束，不利于车辆跟踪。

A*算法的原理是在确定局部目标点P_g以后，先将无人驾驶汽车周围的环境栅格化，接着通过评价函数f(n)＝g(n)+h(n)来评价当前节点的周围节点代价值，其中，g(n)为过去成本函数，即所选节点到起点的距离，h(n)为启发函数，即所选节点到目标点的距离，并选取代价值最小的节点作为下一节点，依次寻找，直至到达局部目标点。如图4所示的传统的A*算法的原理图，其中，P_s为起点，P_g为目标点，y为障碍物，首先根据评价函数依次计算出起点P_s周围8个节点的代价值，选取代价值最小的右上角节点作为下一节点，以此往复，直至找寻到目标点P_g,从而得到路径l_c。A*算法的优点和缺点都很明显，优点是对环境反应迅速，搜索路径直接，并能保证搜到的路径为最短路径，缺点是运算量大，随着节点的增多搜索效率下降，不能很好满足车辆高速运动时对路径规划算法的实时性要求。

如图6所示的Morphin算法的原理图，Morphin算法的原理就是在智能汽车前进方向上生成一组离散的弧线路径l_i(i＝1,2,3…n)，采用其中一条安全的路径(l_i)提供给执行器进行跟踪，R为探测半径，行驶路径的生成是根据车辆运动学模型，按照一定的间隔生成一组车辆运动路径。

发明内容

本发明的目的是针对现有智能汽车行驶时路径规划技术的不足，提出一种基于改进A* 算法与Morphin算法的智能汽车局部路径规划方法，以缩短路径规划的时间，并使规划出的路径满足车辆运动学约束，具有良好的可跟踪性，同时还能适应道路环境的复杂性，提高所规划路径的安全性。

本发明基于改进A*算法与Morphin算法的智能汽车局部路径规划方法采用的技术方案是包括以下步骤：

步骤(1)：由决策模块根据从地图数据下载模块接收到的全局路径信息I和全局目标点P_q，以及从计算模块接收到的智能车辆当前位置P_s和车速v_s、障碍车辆位置P_o和车速v_ob、以及智能车辆与障碍车辆之间的最短距离d_ob，结合驾驶规则知识库中提供的车辆驾驶和路面交通方面的先验知识K进行宏观决策得到现场决策指令e，并将决策指令e连同智能车辆当前位置P_s、障碍车辆位置P_o、全局路径信息I、安全距离d_s以及智能车辆与障碍车辆之间的最短距离d_ob一起发送给路径规划模块；

步骤(2)：路径规划模块将智能车辆周围环境地图栅格化，采用A*算法在栅格地图上进行路径规划，得到初始路径l_c并确定出局部目标点P_g；当监测到智能车辆距离障碍车辆较远时，栅格边长设置的较大，反之将栅格边长设置的较小；

步骤(3)：在初始路径l_c的每一个折点处采用Morphin算法，在Morphin算法生成的弧线路径l_i中选择一条最优路径l_g作为新的规划路径重新进行规划，i＝1,2,3...，对初始路径l_c进行平滑优化，得到平滑优化后的最终路径l_z；

步骤(4)：将最终路径l_z发送给车辆控制模块以实时控制智能车辆行驶在最终路径l_z上；

步骤(5)：采用雷达监测模块每经过一个设定的滚动周期Vt检测一遍最终路径l_z上的剩余路径上是否有障碍物，若有，则返回步骤(1)，反之则继续判断智能车辆当前位置P_s是否位于局部目标点P_g处。

进一步地，步骤(2)中：当智能车辆与障碍车辆之间的最短距离d_ob大于2d_s时，将栅格边长设置为60cm，当智能车辆与障碍车辆之间的最短距离d_ob大于d_s且小于2d_s时，将栅格边长设置为40cm，当智能车辆与障碍车辆之间的最短距离d_ob小于安全距离d_s时，将栅格边长设置为20cm。

进一步地，步骤(3)中：Morphin算法的起点p_ki与初始路径l_c上的折点p_zi之间的距离

R为Morphin算法的探测半径。

进一步地，步骤(3)中：当弧线路径l_i上存在障碍物时，Morphin算法评价函数J＝+∞，当弧线路径l_i上不存在障碍物时，Morphin算法评价函数J＝α₁L+α₂θ+α₃k，L为每条弧线路径l_i对应的长度，θ为每条弧线路径l_i终点处的切线l_k与初始路径l_c的夹角，k为每条弧线路径l_i的曲率，α₁,α₂,α₃,α₄为各参数权值。

进一步地，步骤(5)中，若智能车辆当前位置P_s没有位于局部目标点P_g处，则继续控制车辆行驶在最终路径l_z上，若智能车辆当前位置P_s位于局部目标点P_g处，则继续判断智能车辆当前位置P_s是否到达全局目标点P_q处，若智能车辆已到达全局目标点P_q则结束规划，否则返回步骤(1)。

本发明的有益效果是：

1.当智能汽车在行驶过程中利用A*算法来进行路径规划时，本发明采用栅格边长可变的方法对传统A*算法进行改进，当监测到智能汽车距离障碍车辆较远时，将栅格边长设置的较大一点，达到缩短计算时间，提升规划速度和效率的有益效果，更能满足车辆高速行驶时的实时性要求。如图4和图5所示，传统A*算法规划一条从P_s到P_g的路径需要计算 11次，而在改进之后仅需计算7次，大大减少了计算量，提升了规划效率。

2.在改进后的A*算法规划出一条初始路径l_c之后，通过引入Morphin算法，针对A*算法规划出的路径l_c进行平滑优化，使其满足车辆的运动学约束，更利于车辆跟踪。如图 7至图9所示，由A*算法规划出的路径l_c中存在很多折点，不利于车辆跟踪，在经过Morphin算法平滑优化后的最终路径l_z则更加平滑，更利于车辆进行跟踪。

3.本发明通过设置滚动周期Δt来进行滚动规划,每经过一个滚动周期检测一遍规划路径的剩余路径上是否有障碍物，若有障碍物则重新规划路径，使规划出的路径能够适应复杂多变的道路环境，从而能够适应车辆行驶时周围道路环境的复杂性。

附图说明

图1为实现本发明基于改进A*算法与Morphin算法的智能汽车局部路径规划方法的硬件结构框图；

图2为本发明基于改进A*算法与Morphin算法的的智能汽车局部路径规划方法的运行流程图；

图3为图2中本发明在栅格化过程中的栅格大小设置原理图；

图4为传统A*算法的原理示意图；

图5为图2中本发明的改进的A*算法的原理示意图；

图6为Morphin算法的原理示意图；

图7为改进后的A*算法规划出的初始路径l_c经Morphin算法平滑优化前的示意图；

图8为图7中的初始路径l_c经Morphin算法平滑优化过程示意图；

图9为图8中的初始路径l_c经Morphin算法之后的路径平滑优化后的示意图。

具体实施方式

如图1和图2所示，本发明基于改进A*算法与Morphin算法的的智能汽车局部路径规划方法的实现基于图1所示的硬件结构，该硬件结构包括地图数据下载模块、GPS/INS定位测速模块、雷达监测模块、计算模块、驾驶规则知识库、决策模块、路径规划模块和车辆控制模块。其中，地图数据下载模块、GPS/INS定位测速模块和雷达监测模块组成环境感知环节。地图数据下载模块和驾驶规则知识库的输出端连接决策模块的输入端， GPS/INS定位测速模块、雷达监测模块的输出端连接计算模块的输入端，计算模块的输出端连接决策单元的输入端，决策单元的输出端连接路径规划模块的输入端，路径规划模块的输出端连接速车辆控制模块的输入端。

地图数据下载模块用于获取智能车辆起点到目标点的全局路径信息I和全局目标点P_q，并将该信息发送给决策模块。

驾驶规则知识库负责将车辆驾驶和路面交通方面的先验知识K发送给决策模块。

GPS/INS定位测速模块负责采集智能车的当前位置信息P_s和车速信息v_s，并将其发送给计算模块。

雷达监测模块负责采集障碍车辆的位置信息P_o、车速信息v_ob、智能车与障碍车之间的最短距离d_ob，并将其发送给计算模块。

计算模块根据从GPS/INS定位测速模块接收到的智能车辆当前位置信息P_s和车速信息 v_s，计算出当前智能车与障碍车所需保持的安全距离d_s，并将其连同从雷达监测模块接收到的障碍车辆位置信息P_o、车速信息v_ob、智能车与障碍车之间的最短距离d_ob、以及智能车的当前位置信息P_s一并发送给决策模块。

决策模块根据从计算单元接收到的当前智能车与障碍车所需保持的安全距离d_s、智能车与障碍车之间的最短距离d_ob、智能车的当前位置信息P_s、障碍车辆位置信息P_o和车速信息v_ob，以及从地图数据下载模块接收到的全局路径信息I和全局目标点P_q，再参考从驾驶规则知识库中获取的车辆驾驶和路面交通方面的先验知识K进行宏观决策得到现场决策指令e，并将指令e连同接收到的信息一并发送给路径规划模块。

路径规划模块根据智能汽车周围障碍车的位置信息P_o和速度信息v_ob，结合从决策模块接收到的决策指令e和全局路径信息I来选取局部目标点P_g，接着将智能汽车周围的环境地图栅格化，并在栅格化后的地图上，以智能车当前位置P_s为起点，以局部目标点P_g为终点，利用改进的A*算法和Morphin算法进行路径规划，并将规划好的期望路径L_q发送给车辆控制模块。

车辆控制模块在接收到期望路径信息L_q之后，控制车辆行驶在L_q上。

如图2和图3所示，本发明所述的基于改进A*算法与Morphin算法的智能汽车局部轨迹规划方法在进行路径规划时，分为环境感知及宏观决策环节、路径规划及路径平滑环节、车辆控制环节以及滚动规化环节。计算模块计算出智能汽车当前车速下需要保持的安全距离，决策模块进行宏观决策得到决策指令，并将决策指令、安全距离、发送给路径规划模块。路径规划模块根据决策指令确定局部目标点，然后利用改进的A*算法进行路径规划得到初始路径，接着用Morphin算法对初始路径进行平滑优化得到最终路径，并将最终路径发送给车辆控制模块。车辆控制模块控制车辆行驶在最终路径上，同时设立滚动周期进行滚动规划，每经过一个滚动周期检测剩余路径上是否有障碍车辆，若有则返回决策环节重新决策并规划路径，若无则继续行驶在最终路径上。具体步骤如下：

环境感知及宏观决策环节：

步骤1：地图数据下载模块将全局路径信息I和全局目标点P_q发送给决策模块。

步骤2：GPS/INS定位测速模块将智能车辆的当前位置信息P_s和车速信息v_s发送给计算模块。

步骤3：雷达监测模块将智能车辆周围的障碍车辆位置信息P_o、车速信息v_ob以及智能车辆与障碍车辆之间的最短距离d_ob发送给计算模块。

步骤4：计算模块根据从步骤2和步骤3中接收到的信息，计算智能车辆需要与障碍车辆之间保持的安全距离d_s，其中安全距离d_s的计算方法为：

步骤5:计算模块将计算得到的安全距离d_s，连同接收到的P_s,P_o,d_ob,v_ob一并发送给决策模块。

步骤6：决策模块根据从地图数据下载模块接收到的全局路径信息I和全局目标点P_q，以及从计算模块接收到的智能车辆当前位置P_s和车速v_s、障碍车辆位置P_o和车速v_ob、以及智能车辆与障碍车辆之间的最短距离d_ob等信息，结合驾驶规则知识库中提供的车辆驾驶和路面交通方面的先验知识K进行宏观决策得到现场决策指令e，并将决策指令e，连同智能车辆当前位置P_s、障碍车辆位置P_o、全局路径信息I、安全距离d_s、以及智能车辆与障碍车辆之间的最短距离d_ob一起发送给路径规划模块。

路径规划环节：

步骤7：路径规划模块在接收到的智能车辆位置信息P_s和障碍车辆位置信息P_o后，将智能车辆周围环境地图栅格化。当监测到智能车辆距离障碍车辆较远时，栅格化时，将栅格边长设置的较大一点；反之，当监测到智能车辆距离障碍车辆较近时，将栅格边长设置的较小一点。如图3和图5所示，具体的划分方法为：当智能车辆与障碍车辆之间的最短距离d_ob大于2d_s时，将栅格边长设置为60cm，当智能车辆与障碍车辆之间的最短距离d_ob大于d_s且小于2d_s时，将栅格边长设置为40cm，当智能车辆与障碍车辆之间的最短距离d_ob小于安全距离d_s时，将栅格边长设置为20cm。

步骤8：根据智能汽车周围障碍车的位置信息P_o和速度信息v_ob，结合从决策模块接收到的决策指令e和全局路径信息I在步骤7中栅格化好后的已划分好的栅格地图上确定局部目标点P_g，接着利用A*算法在步骤7中划分好的栅格地图上进行路径规划，得到初始路径l_c。

步骤9：如图7至图9所示，在得到初始路径l_c之后，利用Morphin算法对其进行平滑优化，得到最终路径l_z。具体的平滑方法为：在初始路径l_c的每一个折点处采用Morphin 算法重新进行规划，在Morphin算法生成的弧线路径l_i(i＝1,2,3...)中选择一条最优的路径l_g作为新的规划路径，为了使Morphin算法与改进后的A*算法更好的融合，规定Morphin算法的起点p_ki(i＝1,2,3...)与初始路径l_c上的折点p_zi(i＝1,2,3...)之间的距离

(i＝1,2,3...)，其中R为Morphin算法的探测半径，点p_ri(i＝1,2,3...)为l_i(i＝1,2,3...)与l_c的交点。

为了使选择的路径能够安全避开障碍物y的同时，所选路径的长度及以与初始路径l_c之间还能够较好地贴合，这里选择的评价函数J为：

当弧线路径l_i上存在障碍物y时：

J＝+∞，

当弧线路径l_i上不存在障碍物y时：

J＝α₁L+α₂θ+α₃k，

其中，L为每条弧线路径l_i对应的长度，θ为每条弧线路径l_i终点处的切线l_k与初始路径l_c的夹角，k为每条弧线路径l_i的曲率，y为障碍物，α₁,α₂,α₃,α₄为各参数权值。

步骤10：将经过Morphin算法平滑后的最终路径l_z发送给车辆控制模块。

车辆控制环节：

步骤11：车辆控制模块实时控制智能车辆行驶在最终路径l_z上。

滚动规化环节：

步骤12：如图2所示，雷达监测模块每经过一个设定的滚动周期Vt检测一遍最终路径 l_z上的剩余路径上是否有障碍物，若有障碍物，则返回步骤6，反之若无障碍物，则继续判断智能车辆当前位置P_s是否位于局部目标点P_g处，若不是，则说明智能车辆还没有到达局部目标点P_g，则返回步骤11，继续控制车辆行驶在最终路径l_z上；若是，则继续判断智能车辆当前位置P_s是否到达了全局目标点P_q处，若车辆已经到达全局目标点P_q则结束规划，否则返回步骤6重新进行决策，并接着开始规划新的路径。

Claims (6)

1.一种基于改进A*算法与Morphin算法的智能汽车局部路径规划方法，其特征是包括以下步骤：

步骤(1)：由决策模块根据从地图数据下载模块接收到的全局路径信息I和全局目标点P_q，以及从计算模块接收到的智能车辆当前位置P_s和车速v_s、障碍车辆位置P_o和车速v_ob、以及智能车辆与障碍车辆之间的最短距离d_ob，结合驾驶规则知识库中提供的车辆驾驶和路面交通方面的先验知识K进行宏观决策得到现场决策指令e，并将决策指令e连同智能车辆当前位置P_s、障碍车辆位置P_o、全局路径信息I、安全距离d_s以及智能车辆与障碍车辆之间的最短距离d_ob一起发送给路径规划模块；

步骤(2)：路径规划模块将智能车辆周围环境地图栅格化，采用A*算法在栅格地图上进行路径规划，得到初始路径l_c并确定出局部目标点P_g；当监测到智能车辆距离障碍车辆较远时，栅格边长设置的较大，反之将栅格边长设置的较小；

步骤(3)：在初始路径l_c的每一个折点处采用Morphin算法，在Morphin算法生成的弧线路径l_i中选择一条最优路径l_g作为新的规划路径重新进行规划，i＝1,2,3...，对初始路径l_c进行平滑优化，得到平滑优化后的最终路径l_z；

步骤(4)：将最终路径l_z发送给车辆控制模块以实时控制智能车辆行驶在最终路径l_z上；

步骤(5)：采用雷达监测模块每经过一个设定的滚动周期Vt检测一遍最终路径l_z上的剩余路径上是否有障碍物，若有，则返回步骤(1)，反之则继续判断智能车辆当前位置P_s是否位于局部目标点P_g处。

2.根据权利要求1所述的基于改进A*算法与Morphin算法的智能汽车局部路径规划方法，其特征是：步骤(2)中：当智能车辆与障碍车辆之间的最短距离d_ob大于2d_s时，将栅格边长设置为60cm，当智能车辆与障碍车辆之间的最短距离d_ob大于d_s且小于2d_s时，将栅格边长设置为40cm，当智能车辆与障碍车辆之间的最短距离d_ob小于安全距离d_s时，将栅格边长设置为20cm。

3.根据权利要求1所述的基于改进A*算法与Morphin算法的智能汽车局部路径规划方法，其特征是：步骤(3)中：Morphin算法的起点p_ki与初始路径l_c上的折点p_zi之间的距离

R为Morphin算法的探测半径。

4.根据权利要求1所述的基于改进A*算法与Morphin算法的智能汽车局部路径规划方法，其特征是：步骤(3)中：当弧线路径l_i上存在障碍物时，Morphin算法评价函数J＝+∞，当弧线路径l_i上不存在障碍物时，Morphin算法评价函数J＝α₁L+α₂θ+α₃k，L为每条弧线路径l_i对应的长度，θ为每条弧线路径l_i终点处的切线l_k与初始路径l_c的夹角，k为每条弧线路径l_i的曲率，α₁,α₂,α₃,α₄为各参数权值。

5.根据权利要求1所述的基于改进A*算法与Morphin算法的智能汽车局部路径规划方法，其特征是：步骤(5)中，若智能车辆当前位置P_s没有位于局部目标点P_g处，则继续控制车辆行驶在最终路径l_z上，若智能车辆当前位置P_s位于局部目标点P_g处，则继续判断智能车辆当前位置P_s是否到达全局目标点P_q处，若智能车辆已到达全局目标点P_q则结束规划，否则返回步骤(1)。

6.根据权利要求1所述的基于改进A*算法与Morphin算法的智能汽车局部路径规划方法，其特征是：步骤(1)中，地图数据下载模块将全局路径信息I和全局目标点P_q发送给决策模块，GPS/INS定位测速模块将智能车辆的当前位置信息P_s和车速信息v_s发送给计算模块，雷达监测模块将智能车辆周围的障碍车辆位置信息P_o、车速信息v_ob以及智能车辆与障碍车辆之间的最短距离d_ob发送给计算模块，计算模块计算出智能车辆需要与障碍车辆之间保持的安全距离

Images