基于改进方向评价函数DWA算法的局部路径规划方法技术

本发明专利技术公开了一种基于改进方向评价函数DWA算法的局部路径规划方法，包括以下步骤：无人车SLAM功能对环境进行同步定位与建图，生成栅格地图和代价地图，发布二维导航目标；DWA算法生成采样轨迹，用改进的方向评价函数做采样轨迹代价计算；判断最小代价路径是否可达，若选取位置半径长度内出现了占用概率高于阈值的点，判定此点不可达，缩短半径到可达点的位置，作为下一路径点方向；判断无人车对路径点和目标点的夹角，大于阈值则重新发布二维导航目标，处于阈值范围内则驶向二维导航目标。该方法避免了传统DWA算法可能陷入局部最优的状况，解决了目标点始终被占据或始终有障碍物在侧边的情况。侧边的情况。侧边的情况。

【技术实现步骤摘要】
基于改进方向评价函数DWA算法的局部路径规划方法


[0001]本专利技术涉及无人驾驶与机器视觉领域，特别涉及一种基于改进方向评价函数DWA算法的局部路径规划方法。

技术介绍

[0002]DWA算法由Fox D等人提出，用于进行机器人局部避障。相比于其他的机器人路径规划算法，DWA算法是在加以约束的速度矢量空间对线速度和角速度进行采样，利用机器人的运动学方程得出多组待评价的轨迹，随后对这些待评价轨迹给出的得分，选取得分最高的最优轨迹，完成局部路径规划，保证了对机器人发布速度指令的可行性。
[0003]但对于DWA算法来说，没有考虑完成局部路径规划后的朝向问题，对于朝向仅是简单的将到达目标点半径的机器人进行原地转向，这对于一些室内应用场景是可行的，但对于时刻有着行人和车辆的动态环境，这种行为显然具有一定的危险性。因此对于移动平台在结构化道路的局部路径规划，更需要像自动驾驶汽车一样，在完成避障或者变道后，继续朝向前方，而不是原地进行转向。对于DWA算法在最恶劣的情况，也即目标点始终被占据或始终有障碍物出现在侧边的情况也没有较好的处理。

技术实现思路

[0004]本专利技术的目的在于提供一种基于改进方向评价函数DWA算法的局部路径规划方法，以解决现有技术中DWA算法没有考虑完成局部路径规划后的朝向、目标点始终被占据或始终有障碍物出现在侧边的情况。
[0005]为实现上述目的，本专利技术提供以下的技术方案：第一方面，本专利技术提供一种基于改进方向评价函数DWA算法的局部路径规划方法，包括：/>[0006]通过无人车SLAM功能对环境进行同步定位与建图，生成栅格地图和代价地图，发布二维导航目标；
[0007]采用DWA算法生成采样轨迹，用改进的方向评价函数做采样轨迹代价计算；
[0008]判断最小代价路径是否可达，若选取位置半径长度内出现占用概率高于阈值的点，判定此点不可达，缩短半径到可达点的位置，作为下一路径点方向；
[0009]判断无人车对路径点和目标点的夹角，若大于阈值则重新发布二维导航目标，处于阈值范围内则驶向二维导航目标。
[0010]第二方面，本专利技术提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现第一方面所述的方法的步骤。
[0011]第三方面，本专利技术提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面所述的方法的步骤。
[0012]第四方面，本专利技术提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现第一方面所述的方法的步骤。
[0013]与现有技术相比，本专利技术的有益效果为：本专利技术结构将无人车发布二维导航点后的朝向加入了考量范畴，解决现有技术中DWA算法没有考虑完成局部路径规划后的朝向问题；对二维导航目标是否可达做优先判断，有效避免进行局部路径规划过程中出现的障碍物占据目标点的情况；对移动平台对下一个路径点和目标点的夹角做独立判断，对始终有障碍物出现在侧边的情况时传统DAW算法产生错误预测的问题。
附图说明
[0014]图1为本专利技术的整体流程示意图。
[0015]图2为二维点发布与无人车朝向示意图。
[0016]图3为采样点与道路预测示意图。
[0017]图4为前方障碍遮挡目标示意图。
[0018]图5为侧面障碍跟随运动与夹角示意图。
具体实施方式
[0019]结合附图，一种基于改进方向评价函数的DWA算法的局部路径规划方法，包括如下步骤：
[0020]步骤1、如图1所示，启动移动平台的进行同步定位与建图功能，通过此功能对激光雷达坐标系、基坐标系和里程计坐标系之间进行变换，发布地图原始数据、栅格地图数据和机器人姿态分布熵数据，利用外接电脑生成栅格地图和代价地图。启动移动平台进行导航的navigation功能包，发布二维导航目标(2D Nav Goal)指定移动平台需要前往位置的位姿，同时获取移动平台当前的朝向，将朝向作为评价标准之一纳入考量，如图2所示，计算二维导航目标需要指向的角度，并在移动平台所在栅格地图距离自身半径R处指向下一个路径点的方向发布一个的二维导航目标，在进行路径规划的同时解决了获取无人车朝向的问题。可选的，通过订阅mavros/global_position/compass_hdg话题获取移动平台当前的朝向。
[0021]步骤2、定义V
s
为移动平台线速度与角速度的集合，也即DWA算法对局部路径进行搜索求解的最大范围：
[0022]V
s
＝{(v,ω)|v∈[v
min
,v
max
],ω∈[ω
min
,ω
max
]}
[0023]定义V
a
为移动平台运动时与障碍物无碰撞的线速度和角速度，定义和为移动平台的最大线减速度和最大角减速度，定义Distance(v,ω)为速度(v,ω)对应的轨迹上距离障碍物最近的距离，则有：
[0024][0025]考虑到移动平台电机存在转矩的限制，因此在一个控制周期T
s
内存在移动平台最大和最小可达的速度变化范围，因此需要对动态窗口进行缩小，定义v
c
和ω
c
为移动平台当前的线速度和角速度，定义和为移动平台最大的线加速度和角加速度，则有：
[0026][0027]综合对上述移动平台最大速度约束，与障碍物的无碰撞约束以及移动平台自身电机转矩约束，得到动态窗口集合：
[0028]V＝V
s
∩V
a
∩V
b
[0029]在速度矢量空间V中，如图3所示，根据线速度和角速度的采样点数量，可以将连续的速度矢量空间V离散化，得到离散的采样点(v,ω)，对于每个采样点可以根据移动平台的运动学方程给出移动平台的运动轨迹，如图3所示；
[0030]本专利技术提出在评价函数中引入方向评价子函数，去除了局部路径对全局最优路径的跟随，方向评价子函数如下：
[0031][0032]其中ψ
GPS
为移动平台所需要驶向的下一个路径点的方向，ψ
i
为规划出待选择路径终点指向的方向，判断此时移动平台的朝向是否指向下一个路径点所在的方向，若接近与此方向，代价就较低，若远离此方向，代价就较高，改进后的整体评价函数如下：
[0033]Cost(v,ω)＝αObs(v,ω)+βDir(v,ω)+γGdist(v,ω)
[0034]其中α、β、γ为系数，Obs(v,ω)为轨迹经过总栅格的总代价，对有障碍的轨迹直接抛弃，Gdist(v,ω)为轨迹终点到目标点的距离，结合本专利技术提出的方向评价子函数，对三个子函数进行加权运算即可作为对最优局部路径的评判标准，选取一个得分最高的路径即为当前移动平台局部最优路径，将路径方向作为全新的考核标准纳入局部路径规划流程中本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于改进方向评价函数DWA算法的局部路径规划方法，其特征在于，包括：通过无人车SLAM功能对环境进行同步定位与建图，生成栅格地图和代价地图，发布二维导航目标；采用DWA算法生成采样轨迹，用改进的方向评价函数做采样轨迹代价计算；判断最小代价路径是否可达，若选取位置半径长度内出现占用概率高于阈值的点，判定此点不可达，缩短半径到可达点的位置，作为下一路径点方向；判断无人车对路径点和目标点的夹角，若大于阈值则重新发布二维导航目标，处于阈值范围内则驶向二维导航目标。2.根据权利要求1所述的基于改进方向评价函数DWA算法的局部路径规划方法，其特征在于，所述的通过无人车SLAM功能对环境进行同步定位与建图，生成栅格地图和代价地图，发布二维导航目标，具体如下：启动移动平台的进行同步定位与建图功能，通过此功能对激光雷达坐标系、基坐标系和里程计坐标系之间进行变换，发布地图原始数据、栅格地图数据和机器人姿态分布熵数据，生成栅格地图和代价地图；启动移动平台进行导航的navigation功能包，发布二维导航目标指定移动平台需要前往位置的位姿，同时获取移动平台当前的朝向，计算二维导航目标需要指向的角度，并在移动平台所在栅格地图距离自身半径R处指向下一个路径点的方向发布一个的二维导航目标。3.根据权利要求1所述的基于改进方向评价函数DWA算法的局部路径规划方法，其特征在于，所述的采用DWA算法生成采样轨迹，用改进的方向评价函数做采样轨迹代价计算，具体如下：定义V
s
为移动平台线速度与角速度的集合，也即DWA算法对局部路径进行搜索求解的最大范围：V
s
＝{(v,ω)|v∈[v
min
,v
max
],ω∈[ω
min
,ω
max
]}定义V
a
为移动平台运动时与障碍物无碰撞的线速度和角速度，定义和为移动平台的最大线减速度和最大角减速度，定义Distance(v,ω)为速度(v,ω)对应的轨迹上距离障碍物最近的距离，则有：对动态窗口进行缩小，定义v
c
和ω
c
为移动平台当前的线速度和角速度，定义和为移动平台最大的线加速度和角加速度，则有：综合对上述移动平台最大速度约束，与障碍物的无碰撞约束以及移动平台自身电机转矩约束，得到动态窗口集合：V＝V
s
∩V
a
∩V
b
在速度矢量空间V中，根据线速度和角速度的采样点数量，将连续的速度矢量空间V离散化，得到离散的采样点(v,ω)，对于每个采样点根据移动平台的运动学方程...

【专利技术属性】
技术研发人员：杨中华，胡泰洋，李汪洋，陈杰，周阳，邵亚东，肖泽龙，薛文，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：