CN115309163B - 基于改进方向评价函数dwa算法的局部路径规划方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

DWA算法由Fox D等人提出，用于进行机器人局部避障。相比于其他的机器人路径规划算法，DWA算法是在加以约束的速度矢量空间对线速度和角速度进行采样，利用机器人的运动学方程得出多组待评价的轨迹，随后对这些待评价轨迹给出的得分，选取得分最高的最优轨迹，完成局部路径规划，保证了对机器人发布速度指令的可行性。

但对于DWA算法来说，没有考虑完成局部路径规划后的朝向问题，对于朝向仅是简单的将到达目标点半径的机器人进行原地转向，这对于一些室内应用场景是可行的，但对于时刻有着行人和车辆的动态环境，这种行为显然具有一定的危险性。因此对于移动平台在结构化道路的局部路径规划，更需要像自动驾驶汽车一样，在完成避障或者变道后，继续朝向前方，而不是原地进行转向。对于DWA算法在最恶劣的情况，也即目标点始终被占据或始终有障碍物出现在侧边的情况也没有较好的处理。

发明内容

本发明的目的在于提供一种基于改进方向评价函数DWA算法的局部路径规划方法，以解决现有技术中DWA算法没有考虑完成局部路径规划后的朝向、目标点始终被占据或始终有障碍物出现在侧边的情况。

为实现上述目的，本发明提供以下的技术方案：第一方面，本发明提供一种基于改进方向评价函数DWA算法的局部路径规划方法，包括：

通过无人车SLAM功能对环境进行同步定位与建图，生成栅格地图和代价地图，发布二维导航目标；

采用DWA算法生成采样轨迹，用改进的方向评价函数做采样轨迹代价计算；

判断最小代价路径是否可达，若选取位置半径长度内出现占用概率高于阈值的点，判定此点不可达，缩短半径到可达点的位置，作为下一路径点方向；

判断无人车对路径点和目标点的夹角，若大于阈值则重新发布二维导航目标，处于阈值范围内则驶向二维导航目标。

第二方面，本发明提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现第一方面所述的方法的步骤。

第三方面，本发明提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面所述的方法的步骤。

第四方面，本发明提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现第一方面所述的方法的步骤。

与现有技术相比，本发明的有益效果为：本发明结构将无人车发布二维导航点后的朝向加入了考量范畴，解决现有技术中DWA算法没有考虑完成局部路径规划后的朝向问题；对二维导航目标是否可达做优先判断，有效避免进行局部路径规划过程中出现的障碍物占据目标点的情况；对移动平台对下一个路径点和目标点的夹角做独立判断，对始终有障碍物出现在侧边的情况时传统DAW算法产生错误预测的问题。

附图说明

图1为本发明的整体流程示意图。

图2为二维点发布与无人车朝向示意图。

图3为采样点与道路预测示意图。

图4为前方障碍遮挡目标示意图。

图5为侧面障碍跟随运动与夹角示意图。

具体实施方式

结合附图，一种基于改进方向评价函数的DWA算法的局部路径规划方法，包括如下步骤：

步骤1、如图1所示，启动移动平台的进行同步定位与建图功能，通过此功能对激光雷达坐标系、基坐标系和里程计坐标系之间进行变换，发布地图原始数据、栅格地图数据和机器人姿态分布熵数据，利用外接电脑生成栅格地图和代价地图。启动移动平台进行导航的navigation功能包，发布二维导航目标(2D Nav Goal)指定移动平台需要前往位置的位姿，同时获取移动平台当前的朝向，将朝向作为评价标准之一纳入考量，如图2所示，计算二维导航目标需要指向的角度，并在移动平台所在栅格地图距离自身半径R处指向下一个路径点的方向发布一个的二维导航目标，在进行路径规划的同时解决了获取无人车朝向的问题。可选的，通过订阅mavros/global_position/compass_hdg话题获取移动平台当前的朝向。

步骤2、定义V_s为移动平台线速度与角速度的集合，也即DWA算法对局部路径进行搜索求解的最大范围：

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

定义V_a为移动平台运动时与障碍物无碰撞的线速度和角速度，定义和/>为移动平台的最大线减速度和最大角减速度，定义Distance(v,ω)为速度(v,ω)对应的轨迹上距离障碍物最近的距离，则有：

考虑到移动平台电机存在转矩的限制，因此在一个控制周期T_s内存在移动平台最大和最小可达的速度变化范围，因此需要对动态窗口进行缩小，定义v_c和ω_c为移动平台当前的线速度和角速度，定义和/>为移动平台最大的线加速度和角加速度，则有：

综合对上述移动平台最大速度约束，与障碍物的无碰撞约束以及移动平台自身电机转矩约束，得到动态窗口集合：

V＝V_s∩V_a∩V_b

在速度矢量空间V中，如图3所示，根据线速度和角速度的采样点数量，可以将连续的速度矢量空间V离散化，得到离散的采样点(v,ω)，对于每个采样点可以根据移动平台的运动学方程给出移动平台的运动轨迹，如图3所示；

本发明提出在评价函数中引入方向评价子函数，去除了局部路径对全局最优路径的跟随，方向评价子函数如下：

其中ψ_GPS为移动平台所需要驶向的下一个路径点的方向，ψ_i为规划出待选择路径终点指向的方向，判断此时移动平台的朝向是否指向下一个路径点所在的方向，若接近与此方向，代价就较低，若远离此方向，代价就较高，改进后的整体评价函数如下：

Cost(v,ω)＝αObs(v,ω)+βDir(v,ω)+γGdist(v,ω)

其中α、β、γ为系数，Obs(v,ω)为轨迹经过总栅格的总代价，对有障碍的轨迹直接抛弃，Gdist(v,ω)为轨迹终点到目标点的距离，结合本发明提出的方向评价子函数，对三个子函数进行加权运算即可作为对最优局部路径的评判标准，选取一个得分最高的路径即为当前移动平台局部最优路径，将路径方向作为全新的考核标准纳入局部路径规划流程中。

步骤3、选定最优路径后，移动平台在沿规划路径行驶的过程中，继续判断障碍物始是否终占据着二维导航目标，如图4所示，移动平台此时由于到达目标点的代价较高，虽然不会撞向目标点所在的障碍物，但规划出的路径始终围绕着障碍物，移动平台就会始终围绕着障碍物运动，此时需要提前设置判断阈值，若取到的位置半径长度内出现了占用概率高于此阈值，则认为此点为不可达点，缩短半径直到为可达点的位置，作为此时移动平台前往下一个路径点方向对移动平台二维导航目标不可达的情况进行判定，若此时二维导航目标不可达，则重新在距离自身半径R处指向下一个路径点的方向发布一个的二维导航目标，来摆脱障碍物对二维导航目标的占据，该步骤可以有效避免进行局部路径规划过程中出现的障碍物占据目标点的情况。

步骤4、如图5所示，判断侧边是否存在障碍与移动平台一起前行，阻挡其行驶到目标点的情况，为防止移动平台逆向行驶或陷入局部最优不再前进，判断移动平台到下一个路径点和移动平台到目标点的夹角，将夹角的判断的阈值设置为一个较大的锐角，夹角小于等于阈值则继续前行，夹角为钝角大于设定的阈值，此时重新在远端发布指向下一个路径点的二维导航目标，重复进行第二步价值判断，直至顺利通过阈值检测，局部路径规划结束。该步骤有效规避了传统DWA算法移动平台与车道线存在较大夹角而产生错误预测的问题。

本实施例基于上海硅步科学仪器有限公司生产的RIA-R100移动平台完成设计算法的验证，RIA-R100移动平台搭载了Velodyne VLP-16激光雷达，Intel RealSense D435深度相机，U-Blox 7GPS模块，HC-SR04超声波雷达和思岚RPLIDAR-A2激光雷达。外接笔记本电脑使用了主频2.60GHz英特尔i7-9750H处理器，拥有16GB内存和英伟达RTX2060显卡，操作***为Win 10。

基于上述，改进了方向评价函数DWA算法用于局部路径规划时，可以有效解决现有技术中DWA算法没有考虑完成局部路径规划后的朝向、目标点始终被占据或始终有障碍物出现在侧边的情况。

以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (3)

1.一种基于改进方向评价函数DWA算法的局部路径规划方法，其特征在于，包括：

通过无人车SLAM功能对环境进行同步定位与建图，生成栅格地图和代价地图，发布二维导航目标，具体如下：

启动移动平台的进行同步定位与建图功能，通过此功能对激光雷达坐标系、基坐标系和里程计坐标系之间进行变换，发布地图原始数据、栅格地图数据和机器人姿态分布熵数据，生成栅格地图和代价地图；

启动移动平台进行导航的navigation功能包，发布二维导航目标指定移动平台需要前往位置的位姿，同时获取移动平台当前的朝向，计算二维导航目标需要指向的角度，并在移动平台所在栅格地图距离自身半径R处指向下一个路径点的方向发布一个的二维导航目标；

采用DWA算法生成采样轨迹，用改进的方向评价函数做采样轨迹代价计算，具体如下：

定义V_s为移动平台线速度与角速度的集合，也即DWA算法对局部路径进行搜索求解的最大范围：

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

定义V_a为移动平台运动时与障碍物无碰撞的线速度和角速度，定义和/>为移动平台的最大线减速度和最大角减速度，定义Distance(v,ω)为速度(v,ω)对应的轨迹上距离障碍物最近的距离，则有：

对动态窗口进行缩小，定义v_c和ω_c为移动平台当前的线速度和角速度，定义和/>为移动平台最大的线加速度和角加速度，则有：

T_s为控制周期；

综合对上述移动平台最大速度约束，与障碍物的无碰撞约束以及移动平台自身电机转矩约束，得到动态窗口集合：

V＝V_s⌒V_a⌒V_b

在速度矢量空间V中，根据线速度和角速度的采样点数量，将连续的速度矢量空间V离散化，得到离散的采样点(v,ω)，对于每个采样点根据移动平台的运动学方程给出移动平台的运动轨迹，引入方向评价子函数，方向评价子函数如下：

其中ψ_GPS为移动平台所需要驶向的下一个路径点的方向，ψ_i为规划出待选择路径终点指向的方向，判断此时移动平台的朝向是否指向下一个路径点所在的方向，若接近于此方向，代价就较低，若远离此方向，代价就较高，改进后的整体评价函数如下：

Cost(v,ω)＝αObs(v，ω)+βDir(v，ω)+γGdist(v,ω)

其中α、β、γ为系数，Obs(v,ω)为轨迹经过总栅格的总代价，对有障碍的轨迹直接抛弃，Gdist(v,ω)为轨迹终点到目标点的距离，结合方向评价子函数，对三个子函数进行加权运算即可作为对最优局部路径的评判标准，选取一个得分最高的路径即为当前移动平台局部最优路径；

判断最小代价路径是否可达，若选取位置半径长度内出现占用概率高于阈值的点，判定此点不可达，缩短半径到可达点的位置，作为下一路径点方向，具体为：

选定最优路径后，移动平台在沿规划路径行驶的过程中，继续判断障碍物始是否终占据着二维导航目标，设置判断阈值，若取到的位置半径长度内出现了占用概率高于判断阈值，则认为此点为不可达点，缩短半径直到为可达点的位置，作为此时移动平台前往下一个路径点方向对移动平台二维导航目标不可达的情况进行判定，若此时二维导航目标不可达，则重新在距离自身半径R处指向下一个路径点的方向发布一个的二维导航目标，来摆脱障碍物对二维导航目标的占据；

判断无人车对路径点和目标点的夹角，若大于阈值则重新发布二维导航目标，处于阈值范围内则驶向二维导航目标，具体如下：

判断侧边是否存在障碍与移动平台一起前行，阻挡其行驶到目标点的情况，判断移动平台到下一个路径点和移动平台到目标点的夹角，夹角小于等于阈值则继续前行，夹角大于设定的阈值，此时重新在远端发布指向下一个路径点的二维导航目标，重复进行第二步价值判断，直至顺利通过阈值检测。

2.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1所述的方法的步骤。

3.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1所述的方法的步骤。