一种自适应的动态窗口局部路径规划方法与流程

本发明涉及一种无人车驾驶领域，尤其涉及一种自适应的动态窗口局部路径规划方法。

背景技术：

1、目前，动态窗口法(dwa)是无人车应用较为广泛的局部路径规划方法之一。传统dwa利用固定权重系数的代价函数择出最优轨迹，在一些简单的场景中能够较为快速地找到一条最优的避障路径。但是，在障碍物比较密集的场景中，利用传统dwa规划路径时，极易陷入局部最优、难以到达指定目标点。

技术实现思路

1、为了使路径规划算法能够响应环境快速规划出最优路径，本发明提供一种自适应的dwa，能通过模糊控制器做出规划，在面对不同场景时，能够动态调整评价函数的权重系数，适应各种环境。

2、为此，本发明采用以下技术方案：

3、一种自适应的动态窗口局部路径规划方法，包括以下步骤：

4、s1，通过约束条件确定速度采样动态窗口vr的大小；

5、s2，确定速度采样模拟轨迹：

6、在s1中得到的动态窗口中采样一组速度[v,ω]，通过下面的无人车辆运动学模型表达式计算出单位采样时间δt内每个采样时刻的轨迹点，将若干轨迹点连接成完整的无人车辆运动轨迹，

7、x(t+1)＝x(t)+v(t)·δt·cos(θ(t))

8、y(t+1)＝y(t)+v(t)·δt·sin(θ(t))

9、

10、其中，v为无人车辆线速度，ω为无人车辆角速度，δt为单位采样时间；θ为无人车辆的航向角；x，y为无人车辆坐标，

11、s3，设计模糊控制器，得到代价函数权重系数：

12、s31，确定无人车辆航向与目标点航向偏差其中，θd为目标点的航向角；

13、确定无人车辆距离最近障碍物的距离ed，其中先根据下式确定第i个障碍物距离无人车辆的距离|ri|，

14、

15、其中，[x,y]是无人车辆的位置，[xi,yi]是障碍物i的位置，再根据|ri|确定距离无人车辆最近的障碍物的距离ed；

16、s32，确定模糊控制器规则，

17、所述航向偏差的模糊集为{s,m,b}，分别代表小、中、大，所述障碍物的距离ed的模糊集为{n,m,f}，分别代表近、中、远；

18、当航向偏差在模糊集中为s，且距离在模糊集中为f时，模糊控制器输出较大的速度权重系数γ，倾向于选择一条速度较快的轨迹；

19、当航向偏差在模糊集中为b，且距离在模糊集中为f时，模糊控制器输出较大的航向偏差权重系数α，倾向于选择近似目标点航向的轨迹；

20、当航向偏差在模糊集中为m，且距离在模糊集中为n时，模糊控制器输出较大的障碍物距离权重系数β，倾向于选择距离障碍物较远的轨迹；

21、当航向偏差在模糊集中为b，且距离在模糊集中为n时，模糊控制器输出较小的速度权重系数γ，倾向于选择一条速度较慢的轨迹；

22、s33，通过模糊控制器计算代价函数权重系数：

23、模糊控制器输入变量的隶属度函数采用高斯型隶属度函数，输出变量采用三角形隶属度函数，将所述和ed输入模糊控制器，输出权重系数α、β、γ；

24、s4，根据下式确定动态窗口法的轨迹代价函数值j，

25、j＝α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)，

26、其中，heading(v,ω)为航向角评价函数，dist(v,ω)为障碍物距离评价函数，velocity(v,ω)为速度评价函数；

27、s5，从s4得到的数据中选择所述轨迹代价函数值j最大的轨迹作为最优轨迹，无人车辆按所述最优轨迹行驶；

28、s6，判断无人车辆的定位与终点的距离，如到达终点，则停止路径规划，如未到达终点，则执行s7；

29、s7，重复步骤s1-s6，规划下一采样时间内无人车辆的行驶轨迹。

30、步骤s1中所述约束条件包括：

31、(1)根据下式确定无人车辆所能行驶的最大速度vs：

32、vs＝{(v,ω),v∈[vmin,vmax]∧ω∈[ωmin,ωmax]}，

33、其中，vmin为最小无人车辆线速度，vmax为最大无人车辆线速度，ωmin最小无人车辆角速度，ωmax最大无人车辆角速度；

34、(2)根据下式确定无人车辆在单位时间dt内所能达到的速度vd：

35、

36、其中，amin为无人车辆最大减速度，amax无人车辆线速度最大加速度，为无人车辆角速度最大减速度，为无人车辆角速度最大加速度；

37、(3)根据下式确定安全停止速度vk，无人车辆按照所述安全停止速度行驶时，以最大减速度停车不会碰撞障碍物，

38、

39、其中，dist(v,ω)表示模拟轨迹距离障碍物最近的距离；

40、结合上述三个约束条件，获得vr＝vs∩vd∩vk，用于确定最终速度采样动态窗口vr的大小。

41、步骤s3中所述的航向偏差的论域为[-10°,10°]，所述障碍物距离的论域为[0,4]。

42、步骤s4中所述的航向角评价函数heading(v,ω)由下式获得：

43、heading(v,ω)＝180°-δθ

44、其中，δθ表示模拟无人车辆轨迹终点与目标点的航向偏差。

45、步骤s4中所述的障碍物距离评价函数dist(v,ω)由下式获得：

46、

47、模拟的无人车辆轨迹点与障碍物的距离为其中，[xi,yi]是第i个障碍物的位置，[xobs,yobs]是模拟的车辆轨迹点，dist为一常数。

48、s4中所述的速度评价函数velocity(v,ω)为所述无人车辆的线速度绝对值。

49、s4中，采用下式的方法对所述三种评价函数进行归一化处理，使评价结果更加综合且更加平滑：

50、

51、

52、

53、无人车辆往往工作在复杂多变的场景中，为了使路径规划算法能够响应环境快速规划出最优路径，本发明提出了一种自适应的dwa：设计一个模糊控制器，输入为模拟轨迹终点与目标点的航向误差和以及车辆距离障碍物最短距离，输出的是轨迹评价函数的权重系数。模糊控制器可以响应环境，在面对不同场景时，能够动态调整评价函数的权重系数，可以适应各种环境。

54、与现有技术相比，本发明具有以下有益效果：

55、1.本发明提出的自适应dwa，在多变的场景中，可以根据权重系数适应不同的环境。

56、2.传统dwa在复杂场景下规划路径时，可能会发生找不到合适路径的情况，而本发明的参数自适应方法能够有效减少这种情况；

57、3.相较于传统dwa，本发明的自适应dwa在障碍物密集的场景中能够快速规划出一条最优路径；

58、4.本发明的自适应dwa会优先选择航向与终点航向相近的轨迹，减小车辆在循迹过程中的航向偏差。