一种无人车局部动态避障路径规划方法

1.本发明属于无人车自动驾驶技术领域，具体涉及一种无人车局部动态避障路径规划方法。


背景技术：

2.路径规划作为无人车领域关键技术之一，决定了无人车辆能否平稳安全的行驶，其在车辆环境信息感知与车辆智能控制功能之间也起着桥梁作用。目前无人车上采用的路径规划算法主要包括a
*
、d
*
、快速随机树等算法，这些算法虽然已经初步运用到许多实车平台上，但都受限于自身计算量较高的缺点，目前只能用于静态规划。人工势场法由于其模型结构简单，不需要很大的计算量就可以实时避障并完成规划任务，具有结构简单、实时性好、生成的路径圆滑等优点，有利于在实时避障与平滑轨迹控制等动态规划方面加以应用。然而，现有的人工势场法还存在以下几个问题：
3.1、复杂环境中存在目标不可达以及局部最优等问题,导致无人车局部震荡或者止步不前，不满足局部避障规划的要求；
4.2、大部分势场法路径规划未考虑车辆运动学与动力学性能，从而导致规划的路径不满足车辆跟踪要求；
5.3、大部分势场法规划算法适用的场景为静态场景或简单的动态场景，在道路环境中存在复杂动态障碍物时，规划算法不能实时有效的规划出一条避障路径。


技术实现要素：

6.为克服上述本领域中存在的技术问题，本发明提供了一种无人车局部动态避障路径规划方法，具体包括以下步骤：
7.步骤一、针对无人车所在的局部避障行驶环境，分别建立所述环境中行驶目标点和障碍物对无人车的引力势场和斥力势场函数模型，用于反映所述目标点对无人车的引力与无人车位置的关系，以及障碍物对无人车的斥力与无人车位置的关系，并由所述引力与斥力构建行车环境风险场势力函数模型；
8.步骤二、在所构建的行车环境风险场势力函数模型达到局部极小值车辆所受合力使车辆远离目标点时，确定对应的导致无人车无法行驶至目标点的车辆具体受力工况；
9.步骤三、针对不同的所述车辆具体受力工况，分别提供对应的路径规划解决方案：
①
利用设置的虚拟目标点临时改变引力；
②
撤销位于行驶方向上且远于目标点的障碍物斥力；
③
根据障碍物数量改进引力势场函数模型；
④
对斥力进行分解并撤销某方向的斥力分量，使剩余斥力分量与引力的合力为趋近目标点的方向；
10.步骤四、基于步骤三中所具体提供的解决方案，结合障碍物运动速度和方位设置动态路径规划周期、安全距离阈值、碰撞风险约束条件，以进行避障路径规划并实时滚动获取车辆位置与速度控制量；
11.步骤五、利用贝塞尔曲线对所规划的避障路径中的车辆位置进行平滑处理，从而
得到完整的无人车局部避障规划路径。
12.进一步地，步骤一中建立所述行车环境风险势力函数模型，具体包括以下过程：
13.采用如下形式的目标点对无人车的引力势场函数模型：
[0014][0015]
其中，η为引力势场正比例系数，d(x,x
goal
)为车辆当前位置坐标x与目标点位置坐标x
goal
之间的一个距离矢量，方向为从车辆指向目标点；
[0016]
对所述引力势场函数求导得到引力函数f
att
，f
att
的方向为引力势能下降最快的方向，即引力势场的负梯度，其表达式为：
[0017][0018]
其中，表示梯度函数。上式表明，引力值的大小与车辆与目标点的距离成正比，车辆距目标点越远，所受到的引力越大，反之引力越小。可通过引力势场正比例系数η调节引力f
att
的值。
[0019]
所述障碍物对无人车的斥力势场函数模型具体采用以下形式：
[0020][0021]
其中，k为斥力势场正比例系数，d0为障碍物对车辆产生斥力的最大影响范围，仅当车辆运动到d0范围内时，才会受到障碍物产生的斥力场作用，且随着两者之间距离的靠近，斥力势场对车辆的作用力越大；当两者的距离大于d0时，斥力场u
rep
＝0；x
obs
为障碍物的位置坐标，d(x,x
obs
)为车辆与障碍物之间的距离；
[0022]
对斥力势场函数求导可得到斥力函数f
rep
，f
rep
的方向为斥力势能下降最快的方向，即引力势场的负梯度，其表达式为：
[0023][0024]
所述行车环境风险场函数由目标点引力势场函数模型和障碍物斥力势场函数模型叠加形成，其表达式如下：
[0025][0026]
其中，n为对车辆产生斥力影响的障碍物个数；
[0027]
对所述行车环境风险场函数求导即得到所述行车风险场势力函数模型，其表达式如下：
[0028][0029]
进一步地，步骤二中所述导致无人车无法行驶至目标点的车辆具体受力工况具体包括：
[0030]
工况一：障碍物位于车辆与目标点之间，目标点对车辆的引力与障碍物对车辆的
斥力大小相等、方向相反，使车辆受力平衡，无法继续向目标点移动；
[0031]
工况二：障碍物位于行驶方向上但远于目标点，目标点对车辆的引力与障碍物对车辆的斥力大小相等、方向相反，使车辆受力平衡，无法继续向目标点移动；
[0032]
工况三：车辆朝向目标点的方向两侧存在两个以上障碍物，障碍物斥力与目标点引力的合力使车辆受力平衡，无法继续向目标点移动；
[0033]
工况四：车辆朝向目标点的方向两侧存在两个以上障碍物，障碍物斥力与目标点引力的合力使车辆远离目标点。
[0034]
进一步地，步骤三中所提供的路径规划解决方案，针对所述各种工况具体执行以下操作：
[0035]
针对工况一提供解决方案
①
，通过提供一个位于目标点附近的虚拟目标点，使车辆在虚拟目标点的引力下，使所述行车风险场势力函数临时逃出局部极小值陷阱，在适合的时间再将所述虚拟目标点撤销；
[0036]
针对工况二提供解决方案
②
，障碍物位于行驶方向上但远于目标点时直接将该障碍物的斥力撤销，从而使所述行车风险场势力函数逃出局部极小值陷阱；
[0037]
针对工况三，首先判断根据障碍物的影响范围，判断障碍物之间的无影响间距是否满足车辆的侧向安全通过距离s
safe
,若满足车辆则提供解决方案
③
，根据障碍物个数n并利用以下方式对引力势场函数进行改进，以提高引力大小：
[0038][0039]
若不满足所述侧向安全通过距离s
safe
，则提供解决方案
①
使所述行车风险场势力函数临时逃出局部极小值陷阱；
[0040]
针对工况四提供解决方案
④
，对斥力进行分解并撤销某方向的斥力分量，使剩余斥力分量与引力的合力改变为趋近目标点的方向。
[0041]
进一步地，步骤四中具体执行以下过程来进行避障路径规划：
[0042]
为提高规划算法效率和准确性，先对行车局部避障环境中的障碍物进行初筛选。首先建立一条自我车辆到目标点的虚拟直线，其表达式如下：
[0043]
l
st
:y＝k
st
x b
st
，x∈(x
eg
o,x
target
)
[0044]
其中，为直线l
st
的斜率，为直线l
st
的截距,(x
ego
,y
ego
)、(x
target
,y
target
)分别为车辆以及目标点的位置坐标；
[0045]
设障碍物到车辆的距离为d
obs-ego
，障碍物到直线l
st
的距离为d
obs-st
，当d
obs-ego
或d
obs-st
任意一项小于各自的安全阈值时，就判定其对应的障碍物会对车辆后续的路径规划造成影响，从而将这些可能对路径规划有影响的障碍物筛选出来，以便进行后续的规划计算。这种障碍物预筛选方法忽略了距离较远的障碍物对自身期望轨迹的影响，可以有效的缩短路径规划算法的时间，大大提高了路径规划的效率。d
obs-ego
或d
obs-st
的表达式如下：
[0046][0047]
[0048]
其中，d
obs-ego
和d
obs-st
对应的安全阈值分别为d
obs-ego*
和d
obs-st*
；
[0049]
针对侧向动态障碍物，根据其运动方向，建立一条以障碍物当前位置坐标为起点，指向障碍物运动方向的射线，记为l
obs
，l
obs
时刻随障碍物坐标及运动方向的改变而改变，因此可在短时域内对障碍物的运动轨迹进行粗略预测，根据障碍物的运动意图，从而判断其是否存在与自我车辆相碰撞的可能。射线l
obs
的表达式如下：
[0050][0051]
其中，θ
obs
为障碍物的运动方向与水平轴的夹角，是一个动态变量，障碍物的位置坐标(x
obs
,y
obs
)随障碍物的运动而时刻更新；
[0052]
设射线l
obs
与所述直线l
st
的虚拟交点为p(x
p
,y
p
)，联立两直线可求得虚拟交点p(x
p
,y
p
)的位置坐标；其横纵坐标表达式如下：
[0053][0054]
基于所述虚拟碰撞点p，分别计算车辆与障碍物从当前位置到达虚拟碰撞点p所需的时间；设车辆当前车速为v
ego
，加速度为a
ego
，障碍物当前速度为v
obs
，加速度为a
obs
，二者分别以当前运动状态驶向点p耗时分别为t
ego-p
、t
obs-p
，其表达式如下：
[0055][0056][0057][0058][0059]
其中，s
ego-p
为自我车辆当前位置到虚拟交点p的距离，s
obs-p
为障碍物车辆当前位置到虚拟交点p的距离；
[0060]
通过比较t
obs-p
与t
ego-p
的大小并结合车辆与障碍物之间的距离，判断出车辆与侧向障碍物碰撞的风险并维持或调整车辆的行驶状态；
[0061]
针对同向动态障碍物，根据无人车当前速度、障碍物速度以及无人车与障碍物之间的距离，判断出车辆与侧向障碍物碰撞的风险并维持或调整车辆的行驶状态。
[0062]
进一步地，步骤五中采用以下形式的贝塞尔曲线对所规划的避障路径中的车辆位置进行平滑处理：
[0063][0064]
其中，p(s)为控制点，s为变量，p(i)代表位置点坐标,b
i,n
(s)为n次伯恩斯坦多项式，可表示为：
[0065][0066]
式中，n表示贝塞尔曲线的阶数，位置点个数为n 1，控制点个数为n-1。
[0067]
优选取三阶贝塞尔曲线进行上述平滑过程，从而得到完整的无人车局部避障规划路径。
[0068]
上述本发明所提供的无人车局部动态避障路径规划方法，相对于现有技术至少具有以下有益效果：
[0069]
(1).本发明通过改进人工势场法，解决了传统人工势场法目标不可达和部分工况下局部最优的问题,使无人车克服局部震荡，从而能够规划出一条满足避障要求的规划的局部规划路径；
[0070]
(2).本发明在路径规划过程中，加入了车辆运动学与动力学性能要求，设置碰撞安全优先级，保证了规划路径的平滑性，满足下层车辆跟踪器的跟踪要求；
[0071]
(3).本发明通过对道路环境中存在复杂动态障碍物的工况进行分析，分别针对侧向动态障碍物和同向障碍物进行避障规划，并将两种工况结合，使得规划算法满足多动态障碍物工况避障，提高了规划算法的适用性和有效性。
附图说明
[0072]
图1为本发明所提供方法的流程框图；
[0073]
图2为本发明针对局部极小值工况一示意图及解决方案图；
[0074]
图3为本发明针对局部极小值工况二示意图及解决方案图；
[0075]
图4为本发明针对局部极小值工况三示意图及解决方案图；
[0076]
图5为本发明针对目标不可达工况示意图及解决方案图；
[0077]
图6为本发明方案中的侧向动态障碍物避障规划示意图；
[0078]
图7为本发明方案中的同向动态障碍物避障规划示意图。
具体实施方式
[0079]
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0080]
本发明所提供的一种无人车局部动态避障路径规划方法，如图1所示，具体包括以下步骤：
[0081]
步骤一、针对无人车所在的局部避障行驶环境，分别建立所述环境中行驶目标点和障碍物对无人车的引力势场和斥力势场函数模型，用于反映所述目标点对无人车的引力与无人车位置的关系，以及障碍物对无人车的斥力与无人车位置的关系，并由所述引力与斥力构建行车环境风险场势力函数模型；
[0082]
步骤二、在所构建的行车环境风险场势力函数模型达到局部极小值车辆所受合力使车辆远离目标点时，确定对应的导致无人车无法行驶至目标点的车辆具体受力工况；
[0083]
步骤三、针对不同的所述车辆具体受力工况，分别提供对应的路径规划解决方案：
①
利用设置的虚拟目标点临时改变引力；
②
撤销位于行驶方向上且远于目标点的障碍物斥力；
③
根据障碍物数量改进引力势场函数模型；
④
对斥力进行分解并撤销某方向的斥力分量，使剩余斥力分量与引力的合力为趋近目标点的方向；
[0084]
步骤四、基于步骤三中所具体提供的解决方案，结合障碍物运动速度和方位设置动态路径规划周期、安全距离阈值、碰撞风险约束条件，以进行避障路径规划并实时滚动获取车辆位置与速度控制量；
[0085]
步骤五、利用贝塞尔曲线对所规划的避障路径中的车辆位置进行平滑处理，从而得到完整的无人车局部避障规划路径。
[0086]
在本发明的一个优选实施方式中，步骤一中建立所述行车环境风险势力函数模型，具体包括以下过程：
[0087]
采用如下形式的目标点对无人车的引力势场函数模型：
[0088][0089]
其中，η为引力势场正比例系数，d(x,x
goal
)为车辆当前位置坐标x与目标点位置坐标x
goal
之间的一个距离矢量，方向为从车辆指向目标点；
[0090]
对所述引力势场函数求导得到引力函数f
att
，f
att
的方向为引力势能下降最快的方向，即引力势场的负梯度，其表达式为：
[0091][0092]
其中，表示梯度函数。上式表明，引力值的大小与车辆与目标点的距离成正比，车辆距目标点越远，所受到的引力越大，反之引力越小。可通过引力势场正比例系数η调节引力f
att
的值。
[0093]
所述障碍物对无人车的斥力势场函数模型具体采用以下形式：
[0094][0095]
其中，k为斥力势场正比例系数，d0为障碍物对车辆产生斥力的最大影响范围，仅当车辆运动到d0范围内时，才会受到障碍物产生的斥力场作用，且随着两者之间距离的靠近，斥力势场对车辆的作用力越大；当两者的距离大于d0时，斥力场u
rep
＝0；x
obs
为障碍物的位置坐标，d(x,x
obs
)为车辆与障碍物之间的距离；
[0096]
对斥力势场函数求导可得到斥力函数f
rep
，f
rep
的方向为斥力势能下降最快的方向，即引力势场的负梯度，其表达式为：
[0097][0098]
所述行车环境风险场函数由目标点引力势场函数模型和障碍物斥力势场函数模
型叠加形成，其表达式如下：
[0099][0100]
其中，n为对车辆产生斥力影响的障碍物个数；
[0101]
对所述行车环境风险场函数求导即得到所述行车风险场势力函数模型，其表达式如下：
[0102][0103]
在本发明的一个优选实施方式中，步骤二中所述导致无人车无法行驶至目标点的车辆具体受力工况如图2-5所示具体包括：
[0104]
工况一：障碍物位于车辆与目标点之间，如图2(a)所示，目标点对车辆的引力与障碍物对车辆的斥力大小相等、方向相反，使车辆受力平衡，无法继续向目标点移动；
[0105]
工况二：障碍物位于行驶方向上但远于目标点，如图3所示，目标点对车辆的引力与障碍物对车辆的斥力大小相等、方向相反，使车辆受力平衡，无法继续向目标点移动；
[0106]
工况三：车辆朝向目标点的方向两侧存在两个以上障碍物，如图4(a)所示，障碍物斥力与目标点引力的合力使车辆受力平衡，无法继续向目标点移动；
[0107]
工况四：车辆朝向目标点的方向两侧存在两个以上障碍物，如图5(a)所示，障碍物斥力与目标点引力的合力使车辆远离目标点。
[0108]
在本发明的一个优选实施方式中，步骤三中所提供的路径规划解决方案，针对所述各种工况具体执行以下操作：
[0109]
针对工况一提供解决方案
①
，通过提供一个位于目标点附近的虚拟目标点，如图2(b)所示，使车辆在虚拟目标点的引力下，使所述行车风险场势力函数临时逃出局部极小值陷阱，在适合的时间再将所述虚拟目标点撤销；
[0110]
针对工况二提供解决方案
②
，障碍物位于行驶方向上但远于目标点时，如图3所示，直接将该障碍物的斥力撤销，从而使所述行车风险场势力函数逃出局部极小值陷阱；
[0111]
针对工况三，当车辆在多个障碍物和目标点的共同作用下陷入局部极小值时，如图4(b)所示,首先判断根据障碍物的影响范围，判断障碍物之间的无影响间距是否满足车辆的侧向安全通过距离s
safe
,若满足车辆则提供解决方案
③
，根据障碍物个数n并利用以下方式对引力势场函数进行改进，使得车辆继续沿当前行驶方向前进，直至到达目标点，如图3所示。改进的势场函数为：
[0112][0113]
若不满足所述侧向安全通过距离s
safe
，则提供解决方案
①
使所述行车风险场势力函数临时逃出局部极小值陷阱；
[0114]
针对工况四提供解决方案
④
，将斥力分为引力方向和垂直引力方向两个向量，如图5(b)所示，判断两个斥力分力的大小：
[0115]
若沿引力方向的斥力分力绝对值小于引力，则令垂向斥力为零，即：
[0116]frepy
＝0
[0117]f合
＝f
att
f
repx
[0118]
若沿引力方向的斥力分力绝对值大于引力，则令引力方向的斥力分力为零,即：
[0119]frepx
＝0
[0120]f合
＝f
att
f
repy
[0121]
由此可见，经本发明改进的势场法能够消除障碍物对目标点附近车辆的影响，使车辆顺利到达目标点。
[0122]
在本发明的一个优选实施方式中，步骤四中具体执行以下过程来进行避障路径规划：
[0123]
为提高规划算法效率和准确性，先对行车局部避障环境中的障碍物进行初筛选。首先建立一条自我车辆到目标点的虚拟直线，其表达式如下：
[0124]
l
st
:y＝k
st
x b
st
，x∈(x
ego
,x
target
)
[0125]
其中，为直线l
st
的斜率，为直线l
st
的截距,(x
ego
,y
ego
)、(x
target
,y
target
)分别为车辆以及目标点的位置坐标；
[0126]
设障碍物到车辆的距离为d
obs-ego
，障碍物到直线l
st
的距离为d
obs-st
，当d
obs-ego
或d
obs-st
任意一项小于各自的安全阈值时，就判定其对应的障碍物会对车辆后续的路径规划造成影响，从而将这些可能对路径规划有影响的障碍物筛选出来，以便进行后续的规划计算。这种障碍物预筛选方法忽略了距离较远的障碍物对自身期望轨迹的影响，可以有效的缩短路径规划算法的时间，大大提高了路径规划的效率。d
obs-ego
或d
obs-st
的表达式如下：
[0127][0128][0129]
其中，d
obs-ego
和d
obs-st
对应的安全阈值分别为d
obs-ego*
和d
obs-st*
；
[0130]
针对侧向动态障碍物，如图6所示，根据其运动方向，建立一条以障碍物当前位置坐标为起点，指向障碍物运动方向的射线，记为l
obs
，l
obs
时刻随障碍物坐标及运动方向的改变而改变，因此可在短时域内对障碍物的运动轨迹进行粗略预测，根据障碍物的运动意图，从而判断其是否存在与自我车辆相碰撞的可能。射线l
obs
的表达式如下：
[0131][0132]
其中，θ
obs
为障碍物的运动方向与水平轴的夹角，是一个动态变量，障碍物的位置坐标(x
obs
,y
obs
)随障碍物的运动而时刻更新；
[0133]
设射线l
obs
与所述直线l
st
的虚拟交点为p(x
p
,y
p
)，联立两直线可求得虚拟交点p(x
p
,y
p
)的位置坐标；其横纵坐标表达式如下：
[0134][0135]
基于所述虚拟碰撞点p，分别计算车辆与障碍物从当前位置到达虚拟碰撞点p所需的时间；设车辆当前车速为v
ego
，加速度为a
ego
，障碍物当前速度为v
obs
，加速度为a
obs
，二者分别以当前运动状态驶向点p耗时分别为t
ego-p
、t
obs-p
，其表达式如下：
[0136][0137][0138][0139][0140]
其中，s
ego-p
为自我车辆当前位置到虚拟交点p的距离，s
obs-p
为障碍物车辆当前位置到虚拟交点p的距离；
[0141]
对自我车辆和障碍物车辆到点p的时间进行比较，若t
obs-p
《t
ego-p
,说明障碍物车辆会较快到达虚拟交点，这种情况下需要预先判断障碍物车辆的位置是否会对自我车辆产生影响。当障碍物车辆位于交点p某一固定范围内时，若障碍物到自我车辆的距离大于安全距离，那么障碍物对自我车辆的运动无影响，自我车辆在目标点的引力作用下维持原状态行驶。若障碍物到自我车辆的距离小于安全距离，碰撞风险增加，那么此时自我车辆就需提前减速甚至制动，以此来规避碰撞的发生。当障碍物车辆驶离p点后，碰撞风险降低，自我车辆便回归原始车速，在引力势场作用下驶向目标点。
[0142]
若t
obs-p
＝t
ego-p
,说明自我车辆与障碍物车辆会同时到达虚拟交点,由于障碍物的运动状况是不可控的，因此要根据自我车辆的运动学和动力学约束来判断应采取何种措施来规避碰撞。规避碰撞的措施主要包括制动、减速、加速和转向。以保证行车安全性为前提设置碰撞决策优先级，减速、制动工况优先级高于加速和转向工况优先级。根据决策优先级的安排，当存在碰撞风险时，在保证安全距离的前提下，自我车辆首先采取减速措施，若减速后依然未能降低碰撞风险，则采取制动措施来避让障碍物车辆。待障碍物车辆逐渐远离自我车辆，碰撞风险降低后，自我车辆开始缓慢加速直至恢复到给定车速。
[0143]
若t
obs-p
》t
ego-p
,说明自我车辆会较快到达虚拟交点，当自我车辆临近虚拟交点p时，若障碍物到自我车辆的距离小于安全距离，碰撞风险增加，此时自我车辆就要在障碍物斥力作用下偏离初始轨迹，即脱离直线轨迹以躲避动态障碍物；其次，当自我车辆感知到碰撞风险增加后，其在避让的同时会提高车速，以快速脱离障碍物的影响范围，进一步保障路径规划的安全性。当自我车辆临近虚拟交点p时，若障碍物到自我车辆的距离大于安全距离，不存在碰撞风险，此时自我车辆就在势场作用下维持原状态行驶，直至到达目标点。
[0144]
针对同向动态障碍物，如图7所示，无人车需要及时判断出前方车辆的行驶状态并
做出决策：跟车、制动或者转向。
[0145]
跟车工况：无人车与前方车辆行驶速度适中且车间距满足安全距离的要求，则无人车保持当前车速进行跟车行驶。
[0146]
制动工况：无人车车速大于前方车辆且车间距小于安全距离，同时无人车侧方存在障碍物且不满足转向要求，则无人车进行制动操作。
[0147]
转向工况：无人车车速大于前方车辆且车间距满足换道间距，同时无人车侧方道路环境满足换道条件，则无人车进行换道操作。
[0148]
通过分析动态障碍物运动状态，实时选取对应的规划控制算法，实现实时避障规划。
[0149]
在本发明的一个优选实施方式中，步骤五中采用以下形式的贝塞尔曲线对所规划的避障路径中的车辆位置进行平滑处理：
[0150][0151]
其中，p(s)为控制点，s为变量，p(i)代表位置点坐标,b
i,n
(s)为n次伯恩斯坦多项式，可表示为：
[0152][0153]
式中，n表示贝塞尔曲线的阶数，位置点个数为n 1，控制点个数为n-1。
[0154]
本实施例中优选取三阶贝塞尔曲线进行上述平滑过程，从而得到完整的无人车局部避障规划路径。
[0155]
应理解，本发明实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
[0156]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。