基于动态障碍物碰撞评估窗口缩减的动态窗口法

1.本发明涉及无人驾驶路径规划和自主导航、自动驾驶汽车和移动智能体局部避障路径规划技术领域，具体涉及一种基于动态障碍物碰撞评估窗口缩减的动态窗口法。特别是一种针对阿克曼智能车利用动态窗口法(dwa)进行局部路径规划时，面对动态障碍物通过性差的解决方法。


背景技术：

2.智能汽车是集环境感知、动态决策与规划、行为控制与执行等多种功能于一体的综合系统。它可根据预先给定的任务按照己知地图作出目标规划，并在行进过程中不断感知周围环境的信息实时的进行避障、停车及道路跟踪沿着正确的路径前进。而路径规划直接关系到了智能车的行驶轨迹与行驶距离，对智能车的研究意义重大。所谓的路径规划就是在给定起点终点的环境状态下通过一定的算法避开静态障碍物，寻找一条最优路径。路径规划又可以分为局部路径规划和全局路径规划。
3.动态窗口法是一种局部路径规划方法，是ros机器人的内置路径规划算法。
4.动态窗口法(dynamic window approach，dwa)中，起决策作用的为其评价函数，包括朝向角函数、障碍物函数、速度函数等三部分，评价函数完全依靠当前时刻的信息。当面对静态障碍物时，由于障碍物自身位置不会发生改变，算法所计算出来的速度-角速度驱动指令具有较好的通过性；而当面对动态障碍物时，dwa算法评价函数只依靠当前时刻信息，而不具备未来状态预测这一缺陷，会使其动态避障能力较差，导致动态障碍物通过性较低。


技术实现要素：

5.本发明目的在于针对现有技术存在的缺陷和不足，针对现有dwa算法路径规划算法，缺乏有效规避动态障碍的能力这一缺陷，提供一种基于融合智能车、动态障碍物运动状态信息的碰撞预测策略，能够有效规避动态障碍物的dwa算法。即一种基于动态障碍物碰撞评估窗口缩减的动态窗口法。
6.本发明针对阿克曼转向模型，首先将最小转弯半径约束条件融合至dwa算法的速度-角速度约束中；之后基于速度障碍法，当智能车与动态障碍物之间距离小于危险距离，进行危险状态的评定。若智能车被判定处于危险状态，进行基于模拟轨迹、智能车与障碍物的速度信息的未来碰撞预测评估，该过程会剔除碰撞风险较大的速度-角速度组合，缩小原dwa算法所确定的速度角速度窗口，使得智能车最快脱离危险状态，从而达到安全躲避动态障碍物目的。最后通过python3平台仿真，验证算法的有效性。
7.为实现上述目的，本发明采取如下技术方案：
8.一种基于动态障碍物碰撞评估窗口缩减的动态窗口法，其特征在于：
9.步骤s1：根据阿克曼转向模型的最小转弯半径约束条件，改进dwa算法的速度-角速度约束；随机初始化地图环境，初始化智能车速度v0、角速度w0、朝向角θ0为0，将智能车置于起点p0，终点设置为g；
10.步骤s2：考虑极限速度-角速度、极限线加速度-角加速度、制动距离限制、最小转弯半径限制的速度-角速度约束得到t时刻的速度-角速度窗口；
11.步骤s3：由智能车位置p0、动态障碍物位置m0信息计算出此时智能车与动态障碍物之间的欧式距离|p1m1|；若|p1m1|小于安全距离ds且当前时刻不为初始时刻0，跳至步骤s6；否则进入步骤s4；
12.步骤s4：根据步骤s2得到的t时刻的速度-角速度窗口，或步骤s7得到的t时刻的改进速度-角速度窗口，进行速度-角速度组合的采样，在微小间隔δ
t
内的运动模型假设下，计算出t时刻模拟轨迹集τ
t
；
13.步骤s5：利用评价函数完成对步骤s4得到的模拟轨迹集τ
t
的评分，选取出分数最高的一组速度-角速度组合(v
t
，w
t
)，以(v
t
，w
t
)驱动智能车前行时长δ
t
，智能车到达位置p
t+1
；若位置p
t+1
不为终点g，则进入t+1时刻，并重复执行步骤s1-s5；若位置p
t+1
为终点g，则终止循环；
14.步骤s6：进行速度障碍法分析，若智能车被判定为危险状态，则进入步骤s7；否则进入步骤s4；
15.步骤s7：当智能车处于危险状态时，进行基于模拟轨迹、智能车与障碍物的速度信息的未来碰撞预测评估；该过程对步骤s2中，所述t时刻的速度-角速度窗口进行缩减改进，最终得到t时刻的改进速度-角速度窗口，并回到步骤s4。
16.进一步地，步骤s1中所述阿克曼转向模型的最小转弯半径约束具体为：
17.在阿克曼智能车简化的单车模型中，设四个车轮绕着同一个瞬时旋转中心icr转动，根据几何关系得：
[0018][0019][0020]
式中l为轴距，r为转弯半径，α为转向角，当转弯半径r取到最小值r
min
，转向角α达到极限值α
lim
；
[0021][0022]
在已知线速度v条件下所能达到的极限角速度绝对值w
lim
为：
[0023][0024]
故考虑最小转弯半径的角速度约束表示为下式：
[0025][0026]
综上，将速度-角速度窗口vv缩减为：
[0027]vv
＝vm∩vd∩wa。
[0028]
进一步地，步骤s2中所述极限速度-角速度、加速度限制的速度-角速度、制动距离限制速度-角速度约束具体为：
[0029]
(1)极限速度-角速度
[0030]vm
＝{(v，w)|v∈[v
min
，v
max
]∧w∈[w
min
，w
max
]}
[0031]
式中[v
min
，v
max
]为极限线速度范围，[w
min
，w
max
]为极限角速度范围；具智能车能到达的速度-角速度必须在其极限范围内；
[0032]
(2)加速度限制的速度-角速度：智能车下一个δ
t
时间内能到达的速度、角速度分别受加速度、角加速度约束：
[0033][0034]
vc、wc为当前线速度、角速度，为线加速度范围，为角加速度范围；
[0035]
(3)制动距离限制的速度-角速度：最大减加速度下，智能车应能够在与障碍物碰撞前减速至0：
[0036][0037]
dist(v，w)为速度-角速度组合(v，w)对应轨迹离障碍物的最近距离。
[0038]
进一步地，步骤s3中所述安全距离ds具体为：
[0039]ds
＝mδ
tvmax
[0040]
式中m为模拟轨迹的采样步数，δ
t
为模拟轨迹的采样时间间隔，v
max
为智能车的最大线速度。
[0041]
进一步地，步骤s4中所述速度-角速度组合的采样具体为：
[0042]
在t时刻的速度-角速度窗口v
vt
所指的速度范围vc，角速度范围wc下，按照速度分辨率δv与角速度δw将速度范围vc与角速度范围wc离散化，形成速度集合vc与角速度集合wc，之后在这两个集合内按照一一对应的原则进行组合形成速度-角速度组合(v
t
，w
t
)。
[0043]
进一步地，步骤s4中所述微小间隔δ
t
内的运动模型假设具体为：
[0044]
当运动时间间隔足够小时，将相邻两点之间的运动轨迹近匀速直线运动，具体公式如下：
[0045][0046]
x
t
、y
t
表示t时刻智能车的横纵坐标，表示t时刻的航向角，v
t
、w
t
表示t时刻的速度-角速度组合。
[0047]
进一步地，步骤s5中所述评价函数具体为：
[0048]
g(v,w)＝σ(αheading(v,w)+ζdist(v,w)+γvel(v,w))
[0049]
式中σ()为平滑函数，表示智能车航向角，δ为智能车位置与目标点连线与x轴正方向夹角，dist(v,w)为模拟轨迹到最近障碍物的欧氏距离，vel(v,w)表示智能车的线速度大小，α、ζ、γ为三个权重系数。
[0050]
进一步地，步骤s6中所述速度障碍法分析具体为：
[0051]
设某时刻智能车a的速度为自身大小半径为r，位置为(x
ot
,y
ot
)，动态障碍物b
速度为自身大小半径为r1，位置为(x
bt
,y
bt
)，以障碍物b为参考系，智能车a相对障碍物b的速度为障碍物b考虑智能车体积后将半径膨胀为r1+r，过a点作膨胀后b的圆的切线ot1,ot2，若：
[0052][0053]
说明，若智能车与障碍物运动状态保持不变，则将发生碰撞，判定该时刻智能车处于危险状态。
[0054]
进一步地，步骤s7中所述未来碰撞预测评估与窗口进行缩减改进具体为：
[0055]
设进入危险状态时智能车的速度、角速度分别为v0、w0，根据已知条件：线加速度范围为角加速度范围为线速度范围为[v
min
,v
max
]，最小转弯半径r
min
，极限转角为α
lim
，动态窗口的速度与角速度的分辨率分别为δv、δw，获得此时智能车线速度窗口的所有线速度取值集合v
t
为：
[0056][0057]
在阿克曼智能车模型中，角速度取值集合w
t
受到其速度大小的影响，即极限角速度不应超过以极限线速度、最小转弯半径行驶时的角速度大小，故线速度速度集合中任意速度vr所对应的角速度窗口的所有角速度取值集合w
t
为：
[0058][0059]
将视为危险上界，视为危险下界，取线速度集合中的任意速度进行如下分析：由于智能车速度大小不发生改变，则需要通过改变当前速度方向以脱离危险状态，存在两种脱离危险状态方式：
[0060]
1)改变当前速度使合速度与危险上界重叠，需转过角度λ；
[0061]
2)改变当前速度使合速度与危险下界重叠，需转过角度β；
[0062]
设圆m半径为圆m与向量相交于c1、e1，合速度存在两种分布情况：1)λ《β，此时改变方向使与重叠更易使得智能车脱离危险状态；2)λ≥β，此时改变方向使与重叠更易使智能车脱离危险状态.危险状态时，若保持当前合速度那么在
[0063][0064]
后将发生碰撞，若要保证智能车能在td时间内脱离危险状态，需逆时针转过λ角度或顺时针转过β角度：
[0065][0066][0067]
满足条件的wr如下式所示，式中k为角速度安全系数，取(0,1)之间的常数：
[0068][0069]
对线速度取值集合v
t
中的所有线速度进行同样分析，最终求得缩减后的角速度集合wr如下：
[0070]
wr＝{w1,w2…
wr}
[0071]
r为线速度取值集合中的元素个数.速度-角速度窗口vv更新为下式：
[0072]vv
＝vm∩vd∩wa∩wr。
[0073]
相比于现有技术，本发明及其优选方案针对阿克曼转向模型，首先将最小转弯半径约束条件融合至dwa算法的速度-角速度约束中；之后基于速度障碍法，当智能车与动态障碍物之间距离小于危险距离，进行危险状态的评定。若智能车被判定处于危险状态，进行基于模拟轨迹、智能车与障碍物的速度信息的未来碰撞预测评估，该过程会剔除碰撞风险较大的速度-角速度组合，缩小原dwa算法所确定的速度角速度窗口，使得智能车最快脱离危险状态，从而达到安全躲避动态障碍物目的。通过python3平台仿真，验证了算法的有效性。
附图说明
[0074]
图1为本发明实施例阿克曼转向原理简化模型示意图；
[0075]
图2为本发明实施例速度障碍法分析图；
[0076]
图3为本发明实施例动态碰撞评估分析图；
[0077]
图4为本发明实施例仿真环境示意图；
[0078]
图5为原dwa算法仿真结果示意图；
[0079]
图6为本发明实施例改进dwa算法仿真结果示意图；
[0080]
图7为本发明实施例实施流程示意图。
具体实施方式
[0081]
为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：
[0082]
应该指出，以下详细说明都是例示性的，旨在对本技术提供进一步的说明。除非另有指明，本说明书使用的所有技术和科学术语具有与本技术所属技术领域的普通技术人员通常理解的相同含义。
[0083]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本技术的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包
括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0084]
如图7所示，本实施例提供方案具体包括以下实施步骤：
[0085]
步骤s1：根据阿克曼转向模型的最小转弯半径约束条件，改进dwa算法的速度-角速度约束。随机初始化地图环境，初始化智能车速度v0、角速度w0、朝向角θ0为0，将智能车置于起点p0，终点设置为g。
[0086]
步骤s2：考虑极限速度-角速度、极限线加速度-角加速度、制动距离限制、最小转弯半径限制的速度-角速度约束得到t时刻的速度-角速度窗口。
[0087]
步骤s2中所述极限速度-角速度、加速度限制的速度-角速度、制动距离限制速度-角速度约束具体为：
[0088]
(1)极限速度-角速度
[0089]vm
＝{(v，w)|v∈[v
min
，v
max
]∧w∈[w
min
，w
max
]}
[0090]
式中[v
min
，v
max
]为极限线速度范围，[w
min
，w
max
]为极限角速度范围。具体意义为智能车能到达的速度-角速度必须在其极限范围内。
[0091]
(2)加速度限制的速度-角速度：智能车下一个δ
t
时间内能到达的速度、角速度分别受加速度、角加速度约束：
[0092][0093]
vc、wc为当前线速度、角速度，为线加速度范围，为角加速度范围。
[0094]
(3)制动距离限制的速度-角速度：最大减加速度下，智能车应能够在与障碍物碰撞前减速至0：
[0095][0096]
dist(v，w)为速度-角速度组合(v，w)对应轨迹离障碍物的最近距离。
[0097]
综上，t时刻的速度-角速度vv窗口表示为：
[0098]vv
＝vm∩vd∩va。
[0099]
步骤s1中所述阿克曼转向模型的最小转弯半径约束具体为：
[0100]
阿克曼智能车简化的单车模型如图1所示，四个车轮绕着同一个瞬时旋转中心(icr)转动，根据几何关系可得
[0101][0102][0103]
式中l为轴距，r为转弯半径，α为转向角，当转弯半径r取到最小值r
min
，转向角α达到极限值α
lim
；
[0104][0105]
可知，在已知线速度v条件下所能达到的极限角速度绝对值w
lim
为：
[0106][0107]
故考虑最小转弯半径的角速度约束表示为下式：
[0108][0109]
综上，将速度-角速度窗口vv缩减为：
[0110]vv
＝vm∩vd∩wa。
[0111]
步骤s3：由智能车位置p0、动态障碍物位置m0信息计算出此时智能车与动态障碍物之间的欧式距离|p1m1|。若|p1m1|小于安全距离ds且当前时刻不为初始时刻0，跳至步骤s6；否则进入步骤s4。
[0112]
步骤s3中所述安全距离ds具体为：
[0113]ds
＝mδ
tvmax
[0114]
式中m为模拟轨迹的采样步数，δ
t
为模拟轨迹的采样时间间隔，v
max
为智能车的最大线速度。
[0115]
步骤s4：根据步骤s2得到的t时刻的速度-角速度窗口v
vt
(或步骤s7得到的t时刻的改进速度-角速度窗口)，进行速度-角速度组合的采样，在微小间隔δ
t
内的运动模型假设下，计算出t时刻模拟轨迹集τ
t
。
[0116]
步骤s4中所述速度-角速度组合的采样具体为：
[0117]
在t时刻的速度-角速度窗口v
vt
所指的速度范围vc，角速度范围wc下，按照速度分辨率δv与角速度δw将速度范围vc与角速度范围wc离散化，形成速度集合vc与角速度集合wc，之后在这两个集合内按照一一对应的原则进行组合形成速度-角速度组合(v
t
，w
t
)。
[0118]
步骤s4中所述微小间隔δ
t
内的运动模型假设具体为：
[0119]
当运动时间间隔足够小时，可将相邻两点之间的运动轨迹近匀速直线运动，具体公式如下：
[0120][0121]
x
t
、y
t
表示t时刻智能车的横纵坐标，表示t时刻的航向角，v
t
、w
t
表示t时刻的速度-角速度组合。
[0122]
步骤s5：利用评价函数完成对步骤s4得到的模拟轨迹集τ
t
的评分，选取出分数最高的一组速度-角速度组合(v
t
，w
t
)，以(v
t
，w
t
)驱动智能车前行时长δ
t
，智能车到达位置p
t+1
。若位置p
t+1
不为终点g，则进入t+1时刻，并重复执行步骤s1-s5；若位置p
t+1
为终点g，则终止循环。
[0123]
步骤s5中所述评价函数具体为：
[0124]
g(v,w)＝σ(αheading(v,w)+ζdist(v,w)+γvel(v,w))
[0125]
式中σ()为平滑函数，表示智能车航向角，δ为智能车位置与目标点连线与x轴正方向夹角，dist(v,w)为模拟轨迹到最近障碍物的欧氏距离，vel(v,w)表示智能车的线速度大小，α、ζ、γ为三个权重系数。
[0126]
步骤s6：进行速度障碍法分析，若智能车被判定为危险状态，则进入步骤s7；否则进入步骤s4。
[0127]
步骤s6中所述速度障碍法分析具体为：
[0128]
如图2所示，设该时刻智能车a的速度为自身大小半径为r，位置为(x
ot
,y
ot
)，动态障碍物b速度为自身大小半径为r1，位置为(x
bt
,y
bt
)，以障碍物b为参考系，智能车a相对障碍物b的速度为障碍物b考虑智能车体积后将半径膨胀为r1+r，过a点作膨胀后b的圆的切线ot1,ot2，若
[0129][0130]
说明，若智能车与障碍物运动状态保持不变，则将发生碰撞，判定该时刻智能车处于危险状态。
[0131]
步骤s7：当智能车处于危险状态时，进行基于模拟轨迹、智能车与障碍物的速度信息的未来碰撞预测评估。该过程会对步骤s2所述t时刻的速度-角速度窗口进行缩减改进，最终得到t时刻的改进速度-角速度窗口，并回到步骤s4。
[0132]
步骤s7中所述未来碰撞预测评估与窗口进行缩减改进具体为：
[0133]
假设进入危险状态时智能车的速度、角速度分别为v0、w0.根据已知条件：线加速度范围为角加速度范围为线速度范围为[v
min
,v
max
]，最小转弯半径r
min
，极限转角为α
lim
，动态窗口的速度与角速度的分辨率分别为δv、δw，可得此时智能车线速度窗口的所有线速度取值集合v
t
为：
[0134][0135]
由于研究对象为阿克曼智能车，其角速度取值集合w
t
受到其速度大小的影响，即极限角速度不应超过以极限线速度、最小转弯半径行驶时的角速度大小，故线速度速度集合中任意速度vr所对应的角速度窗口的所有角速度取值集合w
t
为：
[0136][0137]
如图3所示，视为危险上界，视为危险下界，取线速度集合中的任意速度进行如下分析：由于智能车速度大小不发生改变，则需要通过改变当前速度方向以脱离危险状态，存在两种脱离危险状态方式：
[0138]
3)改变当前速度使合速度与危险上界重叠，需转过角度λ；
[0139]
4)改变当前速度使合速度与危险下界重叠，需转过角度β；
[0140]
设圆m半径为圆m与向量相交于c1、e1，合速度存在两种分布情
况：1)λ《β，此时改变方向使与重叠更易使得智能车脱离危险状态；2)λ≥β，此时改变方向使与重叠更易使智能车脱离危险状态.危险状态时，若保持当前合速度那么在
[0141][0142]
后将发生碰撞，若要保证智能车能在td时间内脱离危险状态，需逆时针转过λ角度或顺时针转过β角度：
[0143][0144][0145]
满足条件的wr如下式所示，式中k为角速度安全系数，取(0,1)之间的常数：
[0146][0147]
对线速度取值集合v
t
中的所有线速度进行同样分析，最终求得缩减后的角速度集合wr如下：
[0148]
wr＝{w1,w2…
wr}
[0149]
r为线速度取值集合中的元素个数.速度-角速度窗口vv更新为下式：
[0150]vv
＝vm∩vd∩wa∩wr。
[0151]
为了验证本发明提出算法的有效性，将基于python3语言平台，随机初始化复杂的包含动态障碍物地图环境具体如图1所示，智能车起点设置为(-25，25)，终点设置为(30，30)，黑色圆圈为随机初始化的障碍物，障碍物自身半径为0.5m，绿色圆圈为动态障碍物，如图中a、b、c所示，其运动轨迹及方向如图所示。之后将本发明所提出算法与原dwa算法的实验对比如下
[0152]
表2对仿真实验表
[0153][0154]
对比实验总共进行6000次，改进dwa算法到达率提升108.83％，而路径效率、时间效率降低1.87％，1.31％.后两项对比只统计抵达终点的数据，由于地图随机生成，故路径长度与时间效率上的对比存在地图复杂性偏差，原dwa算法到达率低，平均路径长度、平均时间的数据大量来自简单地图，故到达率的提升具有更显著的意义。图5为圆dwa算法仿真结果，与障碍物b发生碰撞；图6为改进dwa算法的仿真结果，成功躲避三处动态障碍物并抵
达终点。
[0155]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。
[0156]
本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的基于动态障碍物碰撞评估窗口缩减的动态窗口法，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。