一种基于改进A-Star算法与DWA算法的动态环境路径规划方法及系统

本发明涉及动态路径规划，具体而言，涉及一种基于改进a-star算法与dwa算法的动态环境路径规划方法及系统。

背景技术：

1、动态路径规划指的是在运行过程中对路径进行实时调整和优化，以应对环境变化和任务需求的变化。在仓储、物流等行业中，动态路径规划非常重要，因为它可以帮助机器人智能地适应快速变化的工作环境和任务，提高机器人的效率和灵活性。在实际工作中，仓库或生产线中的环境经常发生变化，例如货物堆放位置、道路阻塞等，需要根据实时情况对路径进行调整和优化，以尽可能避开障碍物和缩短路径。

2、当地图中存在动态障碍物时，地图中障碍物信息不能完全已知，a-star算法为全局路径规划算法，在进行路径规划时依赖已知的地图信息，故无法躲避动态障碍物。dwa算法为局部路径规划算法，在进行路径规划时会不断更新地图模型，避免与动态障碍物发生碰撞，但dwa算法容易陷入局部最小值，此时就会出现找不到路的情况。

技术实现思路

1、本发明要解决的技术问题是：

2、为了解决a-star算法无法躲避动态障碍物，dwa算法容易陷入局部最小值，会出现找不到路的情况，且缺乏二者有效融合的问题。

3、本发明为解决上述技术问题所采用的技术方案：

4、本发明提供了一种基于改进a-star算法与dwa算法的动态环境路径规划方法，包括以下步骤：

5、步骤一、采用改进a-star算法在只考虑静态障碍物的情况下规划出完整路径；

6、步骤二、给出机器人的初始状态c，包括横轴坐标、纵轴坐标、航向角、速度和角速度；

7、步骤三、判断当前轨迹的末端是否为路径终点，若当前轨迹末端未到达路径终点，则重复以下步骤，直至当前轨迹末端到达终点并跳转至步骤四，

8、①通过机器人本身能达到的速度vs，与以当前速度和最大加速度在一定时间内能达到的速度vd，确定速度采样的窗口；

9、②通过窗口中的最大速度、最小速度、最大角速度、最小角速度和速度分辨率、角速度分辨率确定能采样得到的速度；

10、③以采样到的速度生成多条预测轨迹，通过轨迹末端的状态计算航向角偏差、距障碍物的最小距离、制动距离、速度大小及到改进a-star算法所规划路径的最短距离；

11、④根据轨迹末端距障碍物的最小距离确定对其进行评价时速度采用的系数；

12、⑤选择最优轨迹，将③中获取的航向角偏差、距障碍物的最小距离、制动距离、速度大小及到改进a-star算法所规划路径的最短距离和④中的评价时速度采用的系数代入至评价函数中，计算得到评价函数中的评价函数值最大的路径作为最优轨迹，记录其对应的速度与角速度，更新机器人状态；

13、步骤四：将各段轨迹组合为完整的路径。

14、进一步地，在步骤一中，具体包括以下步骤：

15、①、获取并输入起始点、目标节点和地图信息，将起始点添加进开放列表；

16、②、获取路径，包括：

17、从起始点开始，即第一个当前节点为起始点，判断当前节点的相邻节点是否在开放列表中：若在，记录相邻节点中最小g值的节点，将当前节点作为该最小g值相邻节点的父节点；若不在，将该相邻节点加入开放列表并判断该相邻节点是否为目标节点，如果为目标节点，结束当前循环；如果不为目标节点，将当前节点作为该节点的父节点；

18、(2)将(1)中得到的当前节点移入关闭列表；

19、(3)采用改进的评价函数计算相邻节点的f值；

20、(4)选择开放列表中f值最小的节点，作为下一当前节点；

21、当开放列表不为空时重复上述步骤，否则跳到③；

22、③、若目标节点在关闭列表中，则从目标节点开始，沿着每一节点的父节点回溯到起始节点，保存路径；若目标节点不在关闭列表中，则无法找到路径；

23、④、对路径点进行判断，若存在路径点与障碍物之间的最小距离小于最小安全距离则路径无效，否则保存路径，将保存的路径作为初次规划得到路径，最小安全距离定义为机器人中心到其最远边缘的距离；

24、⑤：对初次规划得到路径进行再处理，在保证路径与障碍物之间的距离大于最小安全距离的情况下，去除路径中的第一类和第二类节点，得到最终路径。

25、进一步地，在步骤②(3)中，改进后的评价函数如式(1)所示，

26、f(n)＝g(n)+h(n)+5.1/o(n) (1)

27、o(n)的计算公式如式(2)和(3)所示：

28、o(n)＝min(dist) (2)

29、

30、其中，f(n)代表评价函数；g(n)为起始点到当前节点n的观测评估值；h(n)为当前节点n到终点的估计评估值；o(n)为当前节点n与障碍物之间的最小距离，dist为当前节点与障碍物之间的距离；(xn,yn)为当前节点n的坐标；(xo(i),yo(i))为第i个障碍物的坐标；i为障碍物的数量。

31、进一步地，在步骤三④中，改进后的评价函数的表达式如式(9)所示：

32、

33、其中，α、β和γ为加权系数；σ为归一化参数；heading(v,ω)为当前行驶方向与目标方向之间的夹角；dist(v,ω)为机器人轨迹距障碍物的最小距离；velocity(v,ω)为当前速度的大小；λ为参数；a为常数，表示路径点距改进a-star算法已规划出路径的最远距离；distastar(v,ω)为指定速度、角速度下当前节点与采样点之间的最小值；γ_fast和γ_slow均为评价函数中速度的系数。

34、进一步地，distastar(v,ω)的表达式在式(10)和(11)中给出，

35、

36、distastar(v,ω)＝min(dis)  (11)

37、其中，当前路径点n的坐标为(nx,ny)；采样点sample的坐标为(sampix,sampiy)。

38、进一步地，dist(v,ω)的表达式如式(12)所示，

39、

40、其中第i个障碍物的坐标为(obix,obiy)。

41、进一步地，在步骤一④中，最小安全距离如式(4)所示，

42、o(p)≥机器人中心至最远边缘距离  (4)

43、其中，p为路径点。

44、进一步地，在步骤一⑤中，判断路径点是否为第一类节点的方法是，计算该节点前一段路径和后一段路径的斜率是否相同，设待判定路径点c的坐标为(xc,yc)，其前一个路径点p1的坐标为(xp1,yp1)，后一个路径点p2的坐标为(xp2,yp2)，则这两段路径的斜率k1和k2分别为如式(5)和(6)所示：

45、

46、

47、若k1与k2相同则判定该路径点为第一类节点，将其去除，若k1与k2不同，则保留该节点。

48、进一步地，在步骤一⑤中，判断去除第一类路径点后保留下来的路径点是否为第二类节点的方法为，首先将起始点与目标节点相连得到直线l1n，若该直线未经过障碍物并且直线l1n路径与障碍物的距离大于等于最小安全距离，则判定起始点与目标节点之间的路径点为第二类节点，保留直线l1n作为最终路径；若该直线经过障碍物，则将起始点与倒数第二个路径点相连得到直线l1n-1，若直线l1n-1未经过障碍物且与障碍物的距离大于等于最小安全距离，则判定起始点与倒数第二个路径点之间的路径点为第二类节点，将其去除，再将倒数第二个节点与目标点相连，确定最终路径；若直线l1n-1经过障碍物，则将起始点与倒数第三个路径点相连，以此类推，直到将起始点与目标节点无障碍的连接起来，去除第二类节点后得到的路径为最终路径。

49、一种基于改进a-star算法与dwa算法的动态环境路径规划系统，该系统具有与上述步骤对应的程序模块，运行时执行上述的基于改进a-star算法与dwa算法的动态环境路径规划方法中的步骤。

50、相较于现有技术，本发明的有益效果是：

51、本发明一种基于改进a-star算法与dwa算法的动态环境路径规划方法及系统，以提高机器人的工作效率为目标，提出对机器人行驶路径长度和速度进行优化，针对dwa算法进行了两点改进：首先将路径点与改进a-star算法所规划路径的最小距离引入评价函数，使dwa规划所得路径更加贴近直线路径，缩短路径长度；其次以预测轨迹末端距障碍物的最小距离来对评价函数中速度的系数进行选择，提高机器人在距障碍物较远时的行驶速度；使机器人在远离障碍物时以较快的速度运行，在靠近障碍物时，为保证行驶安全，以较缓慢的速度运行，这样既提高了整体的行驶速度，又保证了机器人的行走安全性。

52、将本发明提出的改进dwa算法与其他现有算法进行单任务模拟下和多任务模拟下的性能比较，仿真结果表明，在路径长度、运行时间及行驶速度方面其性能都有所提升，更适合于高速运转的智能仓储。