CN117288213A

CN117288213A - 一种自适应的动态窗口局部路径规划方法

Info

Publication number: CN117288213A
Application number: CN202311106817.2A
Authority: CN
Inventors: 范佳亮; 华均; 熊志勇; 谢红志
Original assignee: Suzhou Dachengyunhe Intelligent Technology Co ltd
Current assignee: Suzhou Dachengyunhe Intelligent Technology Co ltd
Priority date: 2023-08-30
Filing date: 2023-08-30
Publication date: 2023-12-26

Abstract

本发明公开了一种自适应的动态窗口局部路径规划方法，包括S1，通过约束条件确定速度采样动态窗口；S2，确定速度采样模拟轨迹；S3，设计模糊控制器，得到代价函数权重系数；S4，确定动态窗口法的轨迹代价函数值J；S5，从S4得到的数据中选择所述轨迹代价函数值J最大的轨迹作为最优轨迹，无人车辆按所述最优轨迹行驶；S6，判断无人车辆的定位与终点的距离，如到达终点，则停止路径规划，如未到达终点，则执行S7；S7，重复步骤S1‑S6，规划下一采样时间内无人车辆的行驶轨迹。该方法能更好地适应环境变化，在复杂多变的环境中能快速规划出最优轨迹。

Description

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

技术领域

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

背景技术

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

发明内容

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

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

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

S1，通过约束条件确定速度采样动态窗口V_r的大小；

S2，确定速度采样模拟轨迹：

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

x(t+1)＝x(t)+v(t)·Δt·cos(θ(t))

y(t+1)＝y(t)+v(t)·Δt·sin(θ(t))

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

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

S31，确定无人车辆航向与目标点航向偏差其中，θ_d为目标点的航向角；

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

其中，[x,y]是无人车辆的位置，[x_i,y_i]是障碍物i的位置，再根据|r_i|确定距离无人车辆最近的障碍物的距离e_d；

S32，确定模糊控制器规则，

所述航向偏差的模糊集为{S,M,B}，分别代表小、中、大，所述障碍物的距离e_d的模糊集为{N,M,F}，分别代表近、中、远；

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

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

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

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

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

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

S4，根据下式确定动态窗口法的轨迹代价函数值J，

J＝α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)，

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

S5，从S4得到的数据中选择所述轨迹代价函数值J最大的轨迹作为最优轨迹，无人车辆按所述最优轨迹行驶；

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

S7，重复步骤S1-S6，规划下一采样时间内无人车辆的行驶轨迹。

步骤S1中所述约束条件包括：

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

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

其中，v_min为最小无人车辆线速度，v_max为最大无人车辆线速度，ω_min最小无人车辆角速度，ω_max最大无人车辆角速度；

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

其中，a_min为无人车辆最大减速度，a_max无人车辆线速度最大加速度，为无人车辆角速度最大减速度，/>为无人车辆角速度最大加速度；

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

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

结合上述三个约束条件，获得V_r＝V_s∩V_d∩V_k，用于确定最终速度采样动态窗口V_r的大小。

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

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

heading(v,ω)＝180°-Δθ

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

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

模拟的无人车辆轨迹点与障碍物的距离为其中，[x_i,y_i]是第i个障碍物的位置，[x_obs,y_obs]是模拟的车辆轨迹点，dist为一常数。

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

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

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

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

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

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

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

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

附图说明

图1为本发明的自适应的动态窗口局部路径规划方法的流程图；

图2为本发明中态窗口示意图；

图3为本发明中航向误差计算示意图；

图4为本发明中障碍物距离计算示意图；

图5为本发明中模拟轨迹点与障碍物距离示意图。

具体实施方式

以下结合附图和实施例对本发明的技术方案作进一步详细说明。

动态窗口法的轨迹代价函数J＝α·heading(v,w)+β·dist(v,w)+γ·vel(v,w)，其中α,β,γ为权重系数，heading(v,w)为航向误差，dist(v,w)为与障碍物距离代价，vel(v,w)为速度代价，选择代价函数值最大的作为最优路径。从代价函数可以看出，权重系数对于路径评判具有重要的作用，增大其中一个系数，相当于减小其中另外两个系数，说明在路径选择的时候比较看重权重系数较大的那个指标。在不同环境中对轨迹的选择有不同的考量，例如：在航向误差不大、距离障碍物较远时，希望规划出来的较快的车速；在距离障碍物较近时，应牺牲掉航向误差和速度的代价，规划出一条低速安全的路径规避障碍物。为了达到这个目的，本发明设计了一个模糊控制器，通过模糊控制器响应不同的场景，动态输出代价函数的权重系数，所提出的自适应DWA可以适应不同的环境。

在模糊控制器设计过程中，以航向角偏差障碍物距离e_d为输入。航向偏差的论域为[-10°,10°]，其模糊集为{S,M,B}，分别代表小、中、大。障碍物距离的论域为[0,4]，其模糊集为{N,M,F}，分别代表近、中、远。以评价函数系数权重为输出α、β、γ，其论域为[0,1]，模糊集{S,M,B}分别代表小、中、大。输入变量的隶属度函数采用高斯型隶属度函数，输出变量采用三角形隶属度函数。

利用迷糊控制器可以感知环境的变化，不同的环境产生不同的权重系数。模糊规则制定如下：

(1)当航向偏差很小，距离障碍物很远时，为了尽快到达目的地，应该选择较快的轨迹。

(2)当航向偏差稍大，距离障碍物很远时，为了减小航向偏差，应选择与目标点航向近似一致的轨迹。

(3)当航向偏差稍大，距离障碍较近时，为了避开障碍物，牺牲航向误差的代价，选择距离障碍较远的轨迹。

(4)当航向偏差很大，且距离障碍物又较近时，选择速度较慢的轨迹。

自适应DWA核心有三个：(1)根据车辆自身运动特性以及周边环境生成速度采样空间；(2)计算自车航向与目标点航向的偏差以及自车距离最近障碍物的距离e_d，将e_d、/>作为模糊控制器的输入并输出轨迹评价函数的权重系数α、β、γ；(3)根据评价函数对各组采样速度生成的模拟轨迹评分，选择评价函数值最大的那条轨迹并执行。

如图1所示，本发明的自适应动态窗口局部路径规划方法包括以下步骤：

S1，通过约束条件确定速度采样动态窗口V_r的大小。所述约束条件包括：

(1)根据无人车辆所处环境以及本身的机械结构确定车辆所能行驶的最大速度：

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

(2)无人车辆的动力***提供的最大加速度和最大减速度有限，那么在单位时间dt内车辆所能达到的速度也会受到限制：

(3)为了生成安全的速度区间，需要车辆按照当前采样速度行驶，以最大减速度停车，不能与最近障碍物发生碰撞：

上面三式中，v为无人车线速度，ω为无人车辆角速度，v_min为无人车辆最小线速度，v_max为无人车辆最大线速度，ω_min为无人车辆最小角速度，ω_max为无人车辆最大角速度，a_min为无人车辆最大减速度，a_max无人车辆线速度最大加速度，为无人车辆角速度最大减速度，/>为无人车辆角速度最大加速度，dist(v,ω)表示模拟轨迹距离障碍物最近的距离，V_s为无人车辆所能行驶的最大速度，V_d为无人车辆在单位时间dt内所能达到的速度，V_k为安全停止速度。

结合上述三个约束条件，可以确定最终速度采样动态窗口的大小：

V_r＝V_s∩V_d∩V_k (4)

通过式(4)确定最终的动态窗口如图2所示，其中I号区域表示无人车辆由于自身结构限制所不能达到的速度，II号区域表示无人车辆可以到达的速度。但是由于障碍物对速度的限制，实际采样速度只能在III号区域采样。

S2，确定速度采样模拟轨迹：

动态窗口法将无人车的位置约束问题转换为速度约束问题，在动态窗口中采样一组速度[v,ω]，假设在单位采样时间Δt内无人车辆位移较小，轨迹为直线。x，y为无人车辆坐标，利用无人车辆运动学模型计算出每一个采样时刻的轨迹点，所有轨迹点连接成一条完整的轨迹。其中，无人车辆运动学模型为：

x(t+1)＝x(t)+v(t)·Δt·cos(θ(t))

y(t+1)＝y(t)+v(t)·Δt·sin(θ(t))

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

如图3、图4所示，模糊控制器的输入为无人车辆航向与目标点航向偏差无人车辆距离最近障碍物的距离e_d；输出的是轨迹评价函数的权重系数α、β、γ。如图3所示，θ为无人车辆的航向角，θ_d为目标点的航向，航向误差/>找到距离无人车辆最近的障碍物，距离无人车辆障碍物最近的距离/>其中[x,y]是无人车辆的位置，[x_i,y_i]是第i个障碍物的位置，再根据|r_i|确定距离无人车辆最近的障碍物的距离e_d。

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

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

S4，确定动态窗口法的轨迹代价函数值J：

为了选择出一条最优轨迹，需要建立一个评价函数。评价函数通常由航向角、与障碍物最短距离、采样速度组成，定义的一般形式如下：

J＝α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω) (6)

(1)航向角评价函数heading(v,ω)：

heading(v,ω)＝180°-Δθ (7)

式中，Δθ表示模拟轨迹终点与目标点的航向偏差，误差越小评价函数的值越大。

(2)障碍物距离评价函数dist(v,ω)：

如图5所示，首先设置一个固定距离dist，每一组采样速度会产生一系列模拟轨迹点。遍历每一个轨迹点，计算该轨迹点距离障碍物的距离其中，[x_i,y_i]是第i个障碍物的位置，[x_obs,y_obs]是模拟的轨迹点。最终模拟轨迹点距离障碍物最近距离由(8)式得到：

(3)速度评价函数velocity(v,ω)：

velocity(v,ω)＝abs(v) (9)

为了确保各项评价标准在最终的评价结果中具有相对的权重，需对评价函数进行归一化处理，其目的是使评价结果更加综合且更加平滑，从而更好地选择出最优轨迹。

式中，j代表每个采样速度得出的轨迹。

S5，最优轨迹：

由评价函数的到每一条模拟轨迹的权值，选择权值最大的轨迹作为最优轨迹，无人车辆按所述最优轨迹行驶。

Claims

1.一种自适应的动态窗口局部路径规划方法，其特征在于，包括以下步骤：

S1，通过约束条件确定速度采样动态窗口V_r的大小；

S2，确定速度采样模拟轨迹：

x(t+1)＝x(t)+v(t)·Δt·cos(θ(t))

y(t+1)＝y(t)+v(t)·Δt·sin(θ(t))

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

S32，确定模糊控制器规则，

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

S4，根据下式确定动态窗口法的轨迹代价函数值J，

J＝α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)，

2.根据权利要求1所述的自适应的动态窗口局部路径规划方法，其特征在于：S1中所述约束条件包括：

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

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

V_d＝{(v,ω),v∈[v-a_min·dt,v+a_max·dt]∧

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

3.根据权利要求1或2所述的自适应的动态窗口局部路径规划方法，其特征在于：S3中所述的航向偏差的论域为[-10°,10°]，所述障碍物距离/>的论域为[0,4]。

4.根据权利要求3所述的自适应的动态窗口局部路径规划方法，其特征在于，S4中所述的航向角评价函数heading(v,ω)由下式获得：

heading(v,ω)＝180°-Δθ

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

5.根据权利要求3所述的自适应的动态窗口局部路径规划方法，其特征在于，S4中所述的障碍物距离评价函数dist(v,ω)由下式获得：

6.根据权利要求3所述的自适应的动态窗口局部路径规划方法，其特征在于，S4中所述的速度评价函数velocity(v,ω)为所述无人车辆的线速度绝对值。

7.根据权利要求3所述的自适应的动态窗口局部路径规划方法，其特征在于：

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