基于改进人工势场法的无人车局部实时避障路径规划方法

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


背景技术：

2.路径规划作为无人驾驶车辆避障功能的重要环节，其性能直接关系到车辆在 复杂场景下选择路径的质量和行驶的平稳度。当前用于无人车避障的现有技术中 所常用的人工势场法由于其模型结构简单，不需要很大的计算量就可以实时避障 并完成规划任务，具有结构简单、实时性好、生成的路径圆滑等优点，有利于在 实时避障与平滑轨迹控制等动态规划方面加以应用。然而，现有人工势场法在应 用中仍然存在一些缺点与不足，常发生局部极小值、目标不可达、动态规划效果 差等情况。虽然本领域中有个别方案使用了改进的人工势场法，但依然存在静态 避障效率低、未考虑车辆运动学与动力学影响因素等弊端，导致了处理动态避障 工况时的无解、无法跟踪、避障工况单一等问题。


技术实现要素：

3.有鉴于此，针对上述本领域中存在的技术问题，本发明提供了一种基于改进 人工势场法的无人车局部实时避障路径规划方法，具体包括以下步骤：
4.步骤一、针对履带式无人车所在的局部避障行驶环境，对车辆传感器检测到 的障碍物分别进行筛选，基于障碍物与车辆之间的距离去除对车辆轨迹规划无影 响的障碍物，然后评估剩余的静态和动态障碍物以及目标行驶目标点对轨迹规划 的影响风险用于后续建立行车势场环境；
5.步骤二、针对筛选的静态障碍物建立其对车辆的斥力势场函数模型，考虑车 速与车辆—障碍物相对位置的夹角α对斥力势场进行改进，得到以下改进后的水 滴形斥力势场函数模型u
rep
：
[0006][0007][0008]
式中，k
rep
为斥力势场正比例系数，d
obs
为车辆与静态障碍物距离，d0为障碍物 对车辆产生斥力的最大影响范围，kd为调节因子，md为常数；
[0009]
步骤三、针对筛选的动态障碍物建立其对车辆的斥力势场函数模型，考虑其 与车辆的相对位置、相对速度以及相对加速度对斥力场进行改进，得到以下改进 后的动态障碍物斥力势场函数u
rep
：
[0010][0011]
式中，v
eo
为动态障碍物相对于车辆的相对速度，kv为相对速度比例系数，a
eo
为动态障碍物相对于车辆的相对加速度，ka为相对加速度比例系数，β为动态障碍 物和车辆的相对速度与相对位置的夹角，γ为动态障碍物和自主车辆的相对加速度 与相对位置的夹角；
[0012]
步骤四、针对行驶目标点建立其对车辆的引力势场函数模型；
[0013]
步骤五、利用所建立的斥力势场函数模型和引力势场函数模型计算车辆所受 的引力与斥力，并由所述引力与斥力构建行车环境风险场势力函数模型；结合障 碍物运动速度和方位设置动态路径规划周期、安全距离阈值、碰撞风险约束条件， 以进行避障路径规划并实时滚动获取车辆位置与速度控制量；
[0014]
步骤六、进行避障工况试验，包括静态障碍物的换道避障、侧向动态障碍物 的减速避障工况；利用贝塞尔曲线对所规划的避障路径中的车辆位置进行平滑处 理，从而得到完整的无人车局部避障规划路径。
[0015]
进一步地，步骤一中对车辆传感器检测到的障碍物分别进行筛选具体包括以 下过程：
[0016]
首先建立一条自我车辆到目标点的虚拟直线，记为l
st
,其表达式如下：
[0017]
其中，为直线l
st
的斜率，为 直线l
st
的截距，(x
ego
,y
ego
)、(x
target
,y
target
)分别为车辆以及目标点的位置坐标；
[0018]
设障碍物到车辆的距离为d
obs-ego
，障碍物到直线l
st
的距离为d
obs-st
，d
obs-ego
与 d
obs-st
的表达式如下：
[0019][0020][0021]
其中，d
obs-ego
和d
obs-st
对应的安全阈值分别为d
obs-ego*
和d
obs-st*
。
[0022]
进一步地，步骤四针对行驶目标点建立其对无人车的引力势场函数模型具体 包括以下过程：
[0023]
设车辆在二维坐标系中的当前位置坐标为(x,y)，目标点坐标为(xg,yg),则该引 力势场函数u
att
的表达式为：
[0024]
[0025]
式中，k
att
为为引力势场正比例系数，d
goal
为车辆当前位置坐标与目标点坐标 之间的距离矢量，方向为从车辆指向目标点。
[0026]
进一步地，步骤五中车辆所受的引力与斥力计算过程具体包括：
[0027]
对水滴形斥力势场函数模型u
rep
求导得到镜头障碍物对车辆的斥力函数：
[0028][0029]
对于动态障碍物的斥力势场，将其看作由相对位置势场函数u
rep-d
、相对速度 势场函数u
rep-v
、相对加速度势场函数u
rep-a
三部分构成，分别为：
[0030][0031][0032][0033]
对相对位置势场函数u
rep-d
求导得到相对位置斥力函数为：
[0034][0035]
将车辆当前位置坐标为(x,y)由原坐标系转换到以动态障碍物位置坐标为原 点，以障碍物和车辆相对速度方向为x轴的坐标系下，得到坐标(m,n)，转换关系 为：
[0036][0037]
则相对速度势场函数u
rep-v
分别对位置和速度求导得到相对速度斥力函数为：
[0038][0039][0040]
其中，相对速度斥力f
rep-v1
的大小与车辆和障碍物距离d
obs
有关，方向与车辆 和障
碍物的连线垂直；相对速度斥力f
rep-v2
的大小与相对速度有关，方向在车辆和 障碍物的连线上；
▽
表示相应势场的梯度；
[0041]
相对速度势场函数u
rep-a
分别对位置和速度求导得到相对速度斥力函数为：
[0042][0043][0044]
其中，相对加速度斥力f
rep-a1
的大小与车辆和障碍物距离d
obs
有关，方向与车 辆和障碍物的连线垂直；相对加速度斥力f
rep-a2
的大小与相对加速度有关，方向在 车辆和障碍物的连线上；
[0045]
得到动态障碍物对车辆的总斥力为：
[0046][0047]
将行驶目标对车辆的引力势场函数模型u
att
求导得到引力函数：
[0048]fatt
＝
‑▽uatt
＝-k
attdgoal
。
[0049]
进一步地，步骤五中由目标点引力势场、镜头障碍物和动态障碍物斥力势场 共同作用构成行车环境风险势场函数：
[0050]
u＝u
att
∑u
rep
[0051]
对其求导得到行车环境风险场势力函数模型：
[0052]
f＝
‑▽
u＝f
att
∑f
rep
。
[0053]
进一步地，步骤五中采用以下形式的贝塞尔曲线对所规划的避障路径中的车 辆位置进行平滑处理：
[0054][0055]
其中，p(s)为控制点，s为变量，p(i)代表位置点坐标,b
i,n
(s)为n次伯恩斯 坦多项式，可表示为：
[0056]
[0057]
式中，n表示贝塞尔曲线的阶数，位置点个数为n 1，控制点个数为n-1。
[0058]
上述本发明所提供的基于改进人工势场法的无人车局部实时避障路径规划方 法，其相对于现有技术至少具有以下有益效果：
[0059]
(1).本发明通过改进人工势场法静态障碍物的斥力势场函数，加入改进的 距离调节因子，构建了水滴形斥力势场，可显著提高路径规划的精度与效率；
[0060]
(2).本发明在路径规划过程中，考虑了车辆运动学与动力学影响因素，在 动态障碍物势场函数中加入了相对速度函数、相对加速度函数，解决了传统势场 法对于动态避障工况无解的问题，规划的路径实时性和平顺性较好；
[0061]
(3).本发明通过对道路环境中存在复杂动态障碍物的工况进行分析，分别 针对静态障碍物进行换道避障，针对侧向动态障碍物进行减速避障，规划算法静 态规划效率高，且在动态规划中也有较高的适用性和有效性。
附图说明
[0062]
图1为本发明所提供方法的流程框图；
[0063]
图2为本发明所提供方法中障碍物斥力与车辆速度、位置关系示意图；
[0064]
图3为本发明所提供方法中改进后的水滴型斥力势场；
[0065]
图4为本发明所提供方法中改进障碍物斥力场与现有技术斥力场的二维对比图；
[0066]
图5为本发明所提供方法中计算动态障碍物斥力所执行的坐标转换示意图；
[0067]
图6为本发明所提供方法中的静态障碍物的换道避障试验场景示意图；
[0068]
图7为本发明所提供方法中的静态避障试验结果航向角对比图；
[0069]
图8为本发明所提供方法中的动态障碍物的减速避障试验场景示意图；
[0070]
图9为本发明所提供方法中的动态避障试验结果对比图。
具体实施方式
[0071]
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述 的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例， 本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都 属于本发明保护的范围。
[0072]
本发明所提供的一种基于改进人工势场法的无人车局部实时避障路径规划方 法，如图1所示，具体包括：
[0073]
在步骤一中，针对履带式无人车所在的局部避障行驶环境，对车辆传感器检 测到的障碍物分别进行筛选，基于障碍物与车辆之间的距离去除对车辆轨迹规划 无影响的障碍物，然后评估剩余的静态和动态障碍物以及目标行驶目标点对轨迹 规划的影响风险用于后续建立行车势场环境；
[0074]
在步骤二中，针对筛选的静态障碍物建立其对车辆的斥力势场函数模型，考 虑车速与车辆—障碍物相对位置的夹角α对斥力势场进行改进，如图2所示，设 车辆速度与车辆和障碍物相对位置的夹角为α，当车辆朝向障碍物运动时(黑车)， α＝0，根据当前运动趋势车辆会与障碍物发生碰撞，此时障碍物斥力最大；当车 辆速度与车辆和障碍物相对位置的夹角逐渐增大时(红车)，二者碰撞几率减小， 应当减小障碍物斥力，此时0＜α＜90
°
。当车
辆速度与车辆和障碍物相对位置的夹 角继续增大时(蓝车)，二者碰撞几率更小，此时α＞90
°
。由此根据上述对障碍物 斥力和车辆位置、速度的分析，得到以下改进后的水滴形斥力势场函数模型u
rep
：
[0075][0076][0077]
式中，k
rep
为斥力势场正比例系数，d
obs
为车辆与静态障碍物距离，d0为障碍物 对车辆产生斥力的最大影响范围，kd为调节因子，md为常数。传统势场法中md等 价于0，本发明为了使避障规划更加高效、安全，取md为0.6；
[0078]
通过对改进的障碍物斥力势场进行建模，其三维模型如图3所示，可以看出 改进后的斥力势场相比于传统势场形状不再是一个圆柱样，而是随静态障碍物和 车辆的距离、方位的不同呈现水滴样。为了更直观的展示本文提出的改进势场法 的合理性与创新性，将本发明提出的改进障碍物斥力场与传统势场法的障碍物斥 力场以及其他研究者提出的障碍物斥力场进行对比，障碍物斥力场二维对比分析 图如图4所示。图中蓝色圆形为传统斥力场的斥力分布，黑色不完整圆是根据其 他研究者提出的斥力势场函数生成的，红色水滴形是本文提出的改进斥力场的斥 力分布。图中数值代表车辆速度与车辆相对静态障碍物位置的夹角α，范围是 0-180
°
。
[0079]
当环境中存在动态障碍物时，随着时间、障碍物位置的变化，动态障碍物速 度大小、方向和加速度变化对势场环境也存在影响。在路径规划过程中，动态障 碍物的斥力势场应实时反馈障碍物的速度大小、加速度大小和运动方向。若障碍 物朝向车辆运动，二者的相对速度和相对加速度就会增加，车辆受到障碍物的斥 力也应随之增加。
[0080]
为此在步骤三中，针对筛选的动态障碍物建立其对车辆的斥力势场函数模型， 考虑其与车辆的相对位置、相对速度以及相对加速度对斥力场进行改进，得到以 下改进后的动态障碍物斥力势场函数u
rep
：
[0081][0082]
式中，v
eo
为动态障碍物相对于车辆的相对速度，kv为相对速度比例系数，a
eo
为动态障碍物相对于车辆的相对加速度，ka为相对加速度比例系数，β为动态障碍 物和车辆的相对速度与相对位置的夹角，γ为动态障碍物和自主车辆的相对加速度 与相对位置的夹角；
[0083]
通过车载传感器可获得动态障碍物运动信息，当cosβ《0时，动态障碍物和自 主车辆的相对速度与相对位置的夹角大于90
°
，此时动态障碍物驶离自主车辆， 对应的障碍物
斥力势场也相应减弱。通过在步骤三改进后的静态障碍物斥力势场 函数的基础上加入引入相对速度和相对加速度，动态障碍物的斥力势场更加符合 实际。
[0084]
在步骤四中，针对行驶目标点建立其对车辆的引力势场函数模型；
[0085]
在步骤五中，利用所建立的斥力势场函数模型和引力势场函数模型计算车辆 所受的引力与斥力，并由所述引力与斥力构建行车环境风险场势力函数模型；结 合障碍物运动速度和方位设置动态路径规划周期、安全距离阈值、碰撞风险约束 条件，以进行避障路径规划并实时滚动获取车辆位置与速度控制量；
[0086]
在步骤六中，进行避障工况试验，包括静态障碍物的换道避障、侧向动态障 碍物的减速避障工况；利用贝塞尔曲线对所规划的避障路径中的车辆位置进行平 滑处理，从而得到完整的无人车局部避障规划路径。
[0087]
在本发明的一个优选实施方式中，步骤一中对车辆传感器检测到的障碍物分 别进行筛选具体包括以下过程：
[0088]
首先建立一条自我车辆到目标点的虚拟直线，记为l
st
,其表达式如下：
[0089]
其中，为直线l
st
的斜率，为 直线l
st
的截距，(x
ego
,y
ego
)、(x
target
,y
target
)分别为车辆以及目标点的位置坐标；
[0090]
设障碍物到车辆的距离为d
obs-ego
，障碍物到直线l
st
的距离为d
obs-st
，当d
obs-ego
或d
obs-st
任意一项小于各自的安全阈值时，就判定其对应的障碍物会对自我车辆后 续的路径规划造成影响，从而将这些可能对路径规划有影响的障碍物筛选出来。 这种障碍物预筛选方法忽略了距离较远的障碍物对自身期望轨迹的影响，可大大 提高路径规划的效率，因此d
obs-ego
与d
obs-st
的表达式如下：
[0091][0092][0093]
其中，d
obs-ego
和d
obs-st
对应的安全阈值分别为d
obs-ego*
和d
obs-st*
。通过大量仿真 实验证明：当d
obs-ego
*和d
obs-st
*分别取60m和30m时，自主车辆可在最短时间内 规划出有效的避障路径。当d
obs-ego
＜d
obs-st
*或者d
obs-st
＜d
obs-st*
时，其对应的障碍 物便能够参与到后续的规划算法计算。
[0094]
在本发明的一个优选实施方式中，步骤四针对行驶目标点建立其对无人车的 引力势场函数模型具体包括以下过程：
[0095]
在势场法路径规划中，由目标点产生的引力势场与车辆到目标点的距离相关， 车辆与目标点的距离越短，其引力势能的值就越小。设车辆在二维坐标系中的当 前位置坐标为(x,y)，目标点坐标为(xg,yg),则该引力势场函数u
att
的表达式为：
[0096][0097]
式中，k
att
为为引力势场正比例系数，d
goal
为车辆当前位置坐标与目标点坐标 之间的距离矢量，方向为从车辆指向目标点。
[0098]
在本发明的一个优选实施方式中，步骤五中车辆所受的引力与斥力计算过程 具体包括：
[0099]
对水滴形斥力势场函数模型u
rep
求导得到镜头障碍物对车辆的斥力函数：
[0100][0101]
对于动态障碍物的斥力势场，将其看作由相对位置势场函数u
rep-d
、相对速度 势场函数u
rep-v
、相对加速度势场函数u
rep-a
三部分构成，分别为：
[0102][0103][0104][0105]
对相对位置势场函数u
rep-d
求导得到相对位置斥力函数为：
[0106][0107]
为了确定相对速度、相对加速度和相对位置的关系，对原坐标系xoy进行变 换，变换后的坐标系为xoy以动态障碍物位置坐标为原点，以障碍物和自主车辆相 对速度方向为x轴，那么自主车辆相对障碍物的位置就是自主车辆在xoy中的坐标， 自主车辆与障碍物的连线与x轴的夹角就是与相对速度的夹角β。坐标转换关系如 图5所示。
[0108]
根据坐标系的平移和旋转原理，可得到xoy坐标系中车辆的位置坐标(m,n)与 原坐标系中坐标(x,y)的关系为：
[0109][0110]
式中，θ为相对速度方向和原坐标系x轴方向的夹角。
[0111]
故可根据几何知识得到坐标(m,n)，转换关系为：
[0112][0113]
则相对速度势场函数u
rep-v
分别对位置和速度求导得到相对速度斥力函数为：
[0114][0115][0116]
其中，相对速度斥力f
rep-v1
的大小与车辆和障碍物距离d
obs
有关，方向与车辆 和障碍物的连线垂直；相对速度斥力f
rep-v2
的大小与相对速度有关，方向在车辆和 障碍物的连线上；
▽
表示相应势场的梯度；
[0117]
相对速度势场函数u
rep-a
分别对位置和速度求导得到相对速度斥力函数为：
[0118][0119][0120]
其中，相对加速度斥力f
rep-a1
的大小与车辆和障碍物距离d
obs
有关，方向与车 辆和障碍物的连线垂直；相对加速度斥力f
rep-a2
的大小与相对加速度有关，方向在 车辆和障碍物的连线上；
[0121]
得到动态障碍物对车辆的总斥力为：
[0122][0123]
将行驶目标对车辆的引力势场函数模型u
att
求导得到引力函数：
[0124][0125]
在本发明的一个优选实施方式中，步骤五中由目标点引力势场、镜头障碍物 和动态障碍物斥力势场共同作用构成行车环境风险势场函数：
[0126][0127]
对其求导得到行车环境风险场势力函数模型：
[0128]
f＝
‑▽
u＝f
att
∑f
rep
。
[0129]
在本发明的一个优选实施方式中，步骤五中采用以下形式的贝塞尔曲线对所 规划的避障路径中的车辆位置进行平滑处理：
[0130][0131]
其中，p(s)为控制点，s为变量，p(i)代表位置点坐标,b
i,n
(s)为n次伯恩斯 坦多项式，可表示为：
[0132][0133]
式中，n表示贝塞尔曲线的阶数，位置点个数为n 1，控制点个数为n-1。
[0134]
如图6在prescan中搭建的静态障碍物的换道避障场景，静态障碍物车辆放 置在自主车辆行进方向上，自主车辆以18km/h的速度沿着主路行驶。图7给出了 本发明的方法的运行结果。车辆在前进过程中检测到前方的障碍物后，开始进行 转向避障，避障过程中航向角幅值变化不大，规划路径很快回正，证明了基于水 滴型势场函数的静态避障规划算法的合理性。
[0135]
如图8在prescan中搭建的动态障碍物的减速避障场景，障碍物车辆以10.8km/h的 速度从自主车辆的左前方向自主车辆行驶过来，自主车辆以18km/h的速度沿着主路行驶， 二者在交叉口处相遇，自主车辆进行避障。图9给出了本发明的方法的运行结果。在改 进动态障碍物势场函数后，与传统人工势场法相比，由(a)、(b)可知，避障路径更加平 缓。由(c)、(d)可知改进后车辆和障碍物之间的最小距离为2.5m，比改进前的距离大了 1m左右，也就是说车辆在避障的同时提高了与动态障碍物的距离，保证了车辆行驶安全 性。由(e)、(f)可知，与传统势场法车辆航向角的变化量减少了84％。综上可得，改进动 态障碍物势场环境的构建后，避障路径相较之前的路径更加平滑，路径更短，航向角变 化几乎为零，车辆没有大幅度转弯，提高了车辆的行驶效率，同时保证了车辆和障碍物 的安全距离，验证了所提改进算法的有效性和可靠性。
[0136]
应理解，本发明实施例中各步骤的序号的大小并不意味着执行顺序的先后， 各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过 程构成任何限定。
[0137]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言， 可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变 化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。