CN113985875B - 基于碰撞预测模型的人工势场无人车动态路径规划方法 - Google Patents

Abstract

本发明涉及一种基于碰撞预测模型的人工势场无人车动态路径规划方法。针对动态环境中，人工势场法应用于无人车路径规划所存在的反应不及时情况，由雷达传感器实时收集无人车与动态障碍物位置和运动状态信息，建立碰撞预测模型来判定环境是否安全，随之决定是否添加速度势场使得无人车提前获取需要转向的信息；通过建立虚拟势场来解决动态障碍物诱导无人车运动方向问题。本发明较传统人工势场在动态环境中的表现具备更强的避障能力与安全性，应用在实际无人车上同样达到了预期效果。

Description

基于碰撞预测模型的人工势场无人车动态路径规划方法

技术领域

本发明涉及无人车实时避障的路径规划和无人车应用领域，包括引入碰撞预测模型和速度 势场、建立虚拟势场，具体涉及一种基于碰撞预测模型的人工势场无人车动态路径规划方法。

背景技术

无人车路径规划就是在综合考虑行驶时间、速度和安全等因素的前提下，为无人车规划出 最优的行驶轨迹。无人车为了完成在复杂道路上的安全行驶，其自主移动能力是其基础，其中 路径规划和导航是保证无人车自主移动能力的核心算法，是体现车辆智慧水平的关键所在。

人工势场法因其原理简单﹑结构简洁和生成路径平滑等特点，在无人车避障和路径规划中 有着广泛的应用。该方法无需对全局路径进行搜索，规划时间短，执行效率高，非常适合于对 路径生成实时性和安全性要求较高的规划任务﹐它得到的规划路径虽然不一定最短，但却是最平滑和最安全的。从规划环境是否随时间变化方面看，可以分为静态路径规划和动态路径规划。 当前人工势场法的典型应用大多是在静态路径规划方面，而在动态环境中，实际无人车的避障 效果仍存在不足；同时在动态环境下，人工势场法自身还存在动态障碍物诱导无人车运动方向 问题。因此，本发明引入碰撞预测模型和速度势场来提高人工势场法在动态环境下反应能力，建立虚拟势场来解决动态障碍物诱导无人车运动方向问题，实现将人工势场法在应用于动态环 境路径规划时更具备实际应用价值。

发明内容

本发明的目的在于提供一种基于碰撞预测模型的人工势场无人车动态路径规划方法，在动 态环境下能有效提高人工势场法在无人车路径规划应用上的避障能力，并且解决动态障碍物诱 导无人车运动方向问题。

为实现上述目的，本发明的技术方案是：一种基于碰撞预测模型的人工势场无人车动态路 径规划方法，包括如下步骤：

步骤S1、获取障碍物坐标信息、无人车运动模型和初始状态信息；

步骤S2、判断当前时刻无人车所遇障碍物是否为动态障碍物；

步骤S3、识别危险度区域；

步骤S4、选择是否采用碰撞预测模型，并决定是否添加速度势场；

步骤S5、判断是否遇到障碍物诱导方向问题；

步骤S6、计算无人车引力、斥力函数；

步骤S7、输出无人车当前时刻运动步长及方向；

步骤S8、重复步骤S1-S7，直至到达终点完成全局的路径追踪，并描点连线实现无人车运 动轨迹可视化。

在本发明一实施例中，所述步骤S4的具体实现过程如下：

步骤S4.1、建立具备自适应能力的速度势场；

步骤S4.2、建立碰撞预测模型，评价指标包括极限躲避时间t_c和剩余躲避时间t_o；

步骤S4.3、制定动态环境安全判定条件，决定是否添加速度势场。

在本发明一实施例中，所述步骤S4.1的具体实现过程如下：

所建立的速度势场函数：

上式中，α为无人车距离障碍物的最小距离，α₂为速度势场有效范围，V_o为动态障碍物运动 速度，V_v为无人车运动速度，ξ为增幅系数，其计算公式为：

上式中，Φ_v为无人车至障碍物位置所在线段与障碍物运动方向的夹角。

在本发明一实施例中，所述步骤S4.2的具体实现过程如下：

将过障碍物中心且与障碍物运动方向重合的直线记为k，平行直线k且过障碍物两端最外 端点P_o1、P_o2的直线，分别记为k₁、k₂，即得到无人车与直线k₁、k₂的距离D_c1、D_c2；若点P_o1、P_o2处于无人车运动方向两侧，取无人车具有转向趋势一侧的距离即D_c1或D_c2为D_c，点P_c为驶出直 线k₁、k₂所围区域并与其产生的交点；若点P_o1、P_o2处于无人车运动方向同侧，取D_cm徨x＝ m徨x(D_c1,D_c2)、D_cmin＝min(D_c1,D_c2)，点P_c1、P_c2分别为无人车驶入和驶出直线k₁、k₂所围区 域并与其产生的交点；

在极限情形即无人车转向角从开始就达到最大值下，从无人车受到动态障碍物斥力作用起， 到安全躲避障碍物的用时，记为极限躲避时间t_c；从无人车受到动态障碍物斥力作用起，此时 障碍物斥力作用距离为最远距离，记为α₀，到无人车进入障碍物对车速产生强大阻碍效果的距 离即α<0.35α₀时所用时间，记为即剩余躲避时间t_o；

在无人车与障碍物相遇场景中，极限躲避时间t_c的计算公式为：

上式中，R_min为最小转弯半径，以最小转为半径的圆心O’建立坐标系iO′j，A为段距 离，即无人车坐标在坐标系iO′j上j轴的投影距离，Φ为相对运动夹角，Φ_l为极限转向角，D_lim为障碍物运动方向过极限转向点与无人车的距离，/>为无人车后半车身长度；

剩余躲避时间的计算公式为：

上式中，α₀为障碍物对无人车的斥力影响范围，Φ_v为碰撞预测夹角，即无人车至障碍物位 置所在线段与障碍物运动方向的夹角。

在本发明一实施例中，所述步骤S4.3的具体实现过程如下：

当点P_c1、P_c2处于无人车运动方向同侧时，将D_cmin与D_cm徨x分别代入式(3)可得t_c1和t_c2， 即无人车到达点P_c1、P_c2的用时；将D_cmin与D_cm徨x代入式(4)可得t_o1和t_o2，即障碍物到达点P_c1、 P_c2的用时；

动态环境安全判定条件：当P_c1、P_c2处于同侧时，若(t_c1+t_p<t_o1)且(t_c2+t_p<t_o2)，或t_c1>t_o1+t_p；当P_c1、P_c2不处于同侧，若(t_c+t_p<t_o)或t_c>t_o+t_p，则无人车一定拥有在和 障碍物发生碰撞前进行躲避的能力，故不必添加速度势场，其中t_p为保护时间；在其它条件下， 两者较大概率发生碰撞，故均为其添加速度势场。

在本发明一实施例中，所述步骤S5的具体实现过程如下：

在无人车行驶过程中，当满足以下条件时将出现障碍物误导方向问题：动态障碍物出现在 连接无人车与目标点的线段上并且无人车在其斥力势场范围内，V_v∈(0.7～1.3)V_o，且此时通过建立虚拟势场来进行解决，其实现方式：在无人车上取与X轴即 第一象限内夹角大小为Φ的方向，即障碍物运动方向，离无人车(0.2～0.35)α₀的位置上添加虚 拟障碍物来对无人车进行正确引导，使无人车从障碍物后方绕过，避免得到无效甚至错误的规 划路径。

相较于现有技术，本发明具有以下有益效果：本发明方法能够提高人工势场法在动态环境 下反应能力，解决动态障碍物诱导无人车运动方向问题，将人工势场法在应用于动态环境路径 规划时更具备实际应用价值。

附图说明

图1为碰撞预测模型分析过程的示意图。

图2为碰撞预测模型的举例图。

图3为本发明基于碰撞预测模型的人工势场无人车动态路径规划算法流程图。

具体实施方式

下面结合附图1-3，对本发明的技术方案进行具体说明。

本发明一种基于碰撞预测模型的人工势场无人车动态路径规划方法，具体包括如下步骤：

步骤S1、获取障碍物坐标信息、无人车运动模型和初始状态信息；

获取障碍物坐标信息、无人车运动模型和初始状态信息通过无人车自身携带的摄像头、激 光雷达、GPS、惯导、里程计以及线加速度和角加速度等传感器的数据融合进行建模得到。

步骤S2、判断当前时刻无人车所遇障碍物是否为动态障碍物；

将雷达实时采样的地图信息保存为栅格地图，以地图全局坐标系为参考坐标系将相邻两个 时刻的地图进行对比，可得障碍物坐标位置是否发生变化，以此判断障碍物是否为动态障碍物。

步骤S3、识别危险度区域；

由步骤S2得知障碍物若为动态障碍物时，根据无人车与动态障碍物相遇情况划分危险度区 域，随后通过动态障碍物和无人车的运动状态和危险度区域计算在两者直至区域危险度到达安 全程度前是否会发生碰撞。障碍物向180°方向运动，与无人车形成相对运动方向夹角Φ为180°。 相对位置夹角Φ₀为无人车对于障碍物而言所在的角度，α₁为障碍物速度势场最大范围。

在人工势场路径规划的动态避障过程中，根据危险程度可将动态障碍物周围环境划分为三 种区域：以障碍物前进方向±15°范围为危险区域，两侧共计150°范围为警告区域，后方180°范 围为安全区域。

步骤S4、选择是否采用碰撞预测模型，并决定是否添加速度势场；

由步骤S3得出区域的危险度后，若危险度区域处于安全区域，无人车无需采用碰撞预测模 型便具备避障能力；若处于警告区域或危险区域，无人车计算动态环境安全评价指标，即极限 躲避时间t_c和剩余躲避时间t_o，随后根据动态环境安全判定条件决定是否添加速度势场函数。

其速度势场函数为：

上式中，α为无人车距离障碍物的最小距离，α₂为速度势场有效范围，V_o为动态障碍物运 动速度，V_v为无人车运动速度，ξ为增幅系数，其计算公式为：

图1中，过障碍物中心且与障碍物运动方向重合的直线记为k，平行直线k且过障碍物两 端最外端点P_o1、P_o2的直线，分别记为k₁、k₂，即得到无人车与直线k₁、k₂的距离D_c1、D_c2。记 直线k₁、k₂、无人车分别与直线k的距离为D_o1、D_o2、D_co。若点P₀₁、P_o2处于同侧，取D_cm徨x＝ m徨x(D_c1,D_c2)、D_cmin＝min(D_c1,D_c2)。点P_c1、P_c2分别为无人车驶入和驶出直线k₁、k₂所围区 域并与其产生的交点；若点P_o1、P_o2处于无人车运动方向两侧，取无人车具有转向趋势一侧的距离(即D_c1或D_c2)为D_c，点P_c驶出直线k₁、k₂所围区域并与其产生的交点。

在极限情形(无人车转向角δ从开始就达到最大值)下，将无人车受到动态障碍物斥力作用 (α＝α₀)起，到安全躲避障碍物的用时记为t_c，即极限躲避时间。希望无人车转向更加有效， 设定极限转向角Φ_l用以避免无人车转向方向受过大斥力作用而摆向障碍物运动方向，即无人车 不会过度转向。图1中的点P_l为达到极限转向角Φ_l对应时刻的位置。极限转向角Φ_l计算公式为：

当α≤0.3α₀时，由于斥力函数中的指数部分的影响，无人车速度大小V_v约从0.9V_vm徨x起开 始迅速下降，所以在计算无人车成功躲避障碍物的条件中，保留(0.25～0.35)α₀的安全距离且 代入计算的车速大小为(0.8～0.9)V_vm徨x。

将无人车的躲避动态障碍物的行驶过程分为两部分：第一阶段为当前位置以最小转弯半径 R_min驶至点P_l，第二阶段为无人车从点P_l直行驶出点P_c。图2为例，仅存在第一阶段，以O′为原 点建立坐标系iO′j，直线k₂交i轴于点I，延长D_c且过点O”所在直线交j轴于点J，同时交直线k₂于点H。设直线k过点P_l与无人车的距离为D_lim，其计算公式为：

(1)当点P_c1、P_c2不处于同侧，D_c≤D_lim时，无人车极限躲避时间t_c(即当前位置至驶过点P_c所需时长)为：

上式中，记为A，可知A＝R_minsin|Φ-π|。

(2)当点P_c1、P_c2处于同侧，D_c>D_lim时，无人车极限躲避时间t_c包括两阶段，如图1所示，第一阶段无人车驶过路径为弧即达到极限转向点P_l的过程，第二阶段无人车驶过路 径为/>由此可知极限躲避时间t_c为：

综上，在无人车与障碍物相遇场景中，极限躲避时间t_c的计算公式为：

上式中，为无人车后半车身长度。

将无人车受到动态障碍物斥力作用(α＝α₀)起，到无人车进入障碍物对车速产生强大阻 碍效果的距离(即α<0.35α₀)时所用时间，记为即剩余躲避时间t_o其计算公式为：

动态环境安全判定条件：当P_c1、P_c2处于同侧时，若(t_c1+t_p<t_o1)且(t_c2+t_p<t_o2)，或t_c1>t_o1+t_p；当P_c1、P_c2不处于同侧，若(t_c+t_p<t_o)或t_c>t_o+t_p，则无人车一定拥有在和 障碍物发生碰撞前进行躲避的能力，故不必添加速度势场，其中t_p为保护时间。在其它条件下， 两者较大概率发生碰撞，故均为其添加速度势场。无人车的碰撞预测速度势场函数为：

上式中，U_vr为动态障碍物的斥力势场和速度势场之和，而速度势场有效范围α₂为：

步骤S5、是否遇到动态障碍物诱导无人车方向问题；

由于人工势场法在路径规划中的本质是建立势场，使无人车往势场较低处运动，当动态障 碍物满足一定条件时，无人车周围某一方向会持续出现最低势场，从而使无人车失去路径规划 能力。障碍物误导方向问题满足条件：当动态障碍物出现在连接无人车与目标点的线段上并且无人车在其斥力势场范围内，V_v∈(0.7～1.3)V_o，且时，因为无人车和障碍 物的相对位置在一定时间内变化不大，使无人车合力方向始终指向障碍物这一侧，由于无人车 存在运动约束，只能被迫偏离路径。直至两者因速度差而积累一定的距离，无人车离开障碍物斥力影响范围，才能逃脱障碍物的错误诱导，这会使无人车产生多余的路径和能耗。

通过建立虚拟势场避免此类情况，虚拟势场实现方式：在无人车上取与X轴(第一象限内) 夹角大小为Φ的方向，即障碍物运动方向，离无人车(0.25～0.35)α₀的位置上添加虚拟障碍物 来对无人车进行正确引导，使无人车从障碍物后方绕过，避免得到无效甚至错误的规划路径。

步骤S6、计算无人车引力、斥力函数；

人工势场法在进行路径规划时，将无人车所处环境转化成变化的势场环境。首先给无人车 施加一个由自身指向目标点的吸引力，使无人车不断朝目标点前进，再让环境中的障碍物对无 人车产生一个排斥力，最后无人车在合力作用下，不断改变前行方向，避开障碍物，到达目标点。

引力函数为：

F_a＝-λ_a(P-P_g) (11)

上式中，λ_a为引力势场常量，P_g为目标点位置向量，P为无人车位置向量。

斥力函数为：

上式中，λ_r为斥力势场常量，α为无人车距离障碍物的最小距离，α₀为障碍物对无人车的 斥力影响范围。

步骤S7、输出无人车当前时刻运动步长及方向；

由步骤S6中计算所得无人车所受合力矢量，合力矢量的模长为步长，将两次合力矢量方向 变化可得无人车转向角度。并将其发送至无人车的执行机构，通过电信号控制电机的转速和方 向盘转角，控制无人车沿着最优避障路径行驶。

步骤S8、重复以上循环，直至到达终点完成全局的路径追踪，并描点连线对无人车运动轨 迹可视化。

以上是本发明的较佳实施例，凡依本发明技术方案所作的改变，所产生的功能作用未超出 本发明技术方案的范围时，均属于本发明的保护范围。

Claims (3)

1.一种基于碰撞预测模型的人工势场无人车动态路径规划方法，其特征在于，包括如下步骤：

步骤S1、获取障碍物坐标信息、无人车运动模型和初始状态信息；

步骤S2、判断当前时刻无人车所遇障碍物是否为动态障碍物；

步骤S3、识别危险度区域；

步骤S4、选择是否采用碰撞预测模型，并决定是否添加速度势场；

步骤S5、判断是否遇到障碍物诱导方向问题；

步骤S6、计算无人车引力、斥力函数；

步骤S7、输出无人车当前时刻运动步长及方向；

步骤S8、重复步骤S1-S7，直至到达终点完成全局的路径追踪，并描点连线实现无人车运动轨迹可视化；

所述步骤S4的具体实现过程如下：

步骤S4.1、建立具备自适应能力的速度势场；

步骤S4.2、建立碰撞预测模型，评价指标包括极限躲避时间t_c和剩余躲避时间t_o；

步骤S4.3、制定动态环境安全判定条件，决定是否添加速度势场；

所述步骤S4.1的具体实现过程如下：

所建立的速度势场函数：

上式中，α为无人车距离障碍物的最小距离，α₂为速度势场有效范围，V_o为动态障碍物运动速度，V_v为无人车运动速度，ξ为增幅系数，其计算公式为：

上式中，Φ_v为无人车至障碍物位置所在线段与障碍物运动方向的夹角；

所述步骤S4.2的具体实现过程如下：

将过障碍物中心且与障碍物运动方向重合的直线记为k，平行直线k且过障碍物两端最外端点P_o1、P_o2的直线，分别记为k₁、k₂，即得到无人车与直线k₁、k₂的距离D_c1、D_c2；若点P_o1、P_o2处于无人车运动方向两侧，取无人车具有转向趋势一侧的距离即D_c1或D_c2为D_c，点P_c为驶出直线k₁、k₂所围区域并与其产生的交点；若点P_o1、P_o2处于无人车运动方向同侧，取D_cmax＝max(D_c1，D_c2)、D_cmin＝min(D_c1，D_c2)，点P_c1、P_c2分别为无人车驶入和驶出直线k₁、k₂所围区域并与其产生的交点；

在极限情形即无人车转向角从开始就达到最大值下，从无人车受到动态障碍物斥力作用起，到安全躲避障碍物的用时，记为极限躲避时间t_c；从无人车受到动态障碍物斥力作用起，此时障碍物斥力作用距离为最远距离，记为α₀，到无人车进入障碍物对车速产生强大阻碍效果的距离即α＜0.35α₀时所用时间，记为即剩余躲避时间t_o；

在无人车与障碍物相遇场景中，极限躲避时间t_c的计算公式为：

上式中，R_min为最小转弯半径，以最小转为半径的圆心O’建立坐标系iO′j，A为段距离，即无人车坐标在坐标系iO′j上j轴的投影距离，Φ为相对运动夹角，Φ_l为极限转向角，D_lim为障碍物运动方向过极限转向点与无人车的距离，/>为无人车后半车身长度；

剩余躲避时间的计算公式为：

上式中，α₀为障碍物对无人车的斥力影响范围，Φ_v为碰撞预测夹角，即无人车至障碍物位置所在线段与障碍物运动方向的夹角。

2.根据权利要求1所述的基于碰撞预测模型的人工势场无人车动态路径规划方法，其特征在于，所述步骤S4.3的具体实现过程如下：

当点P_c1、P_c2处于无人车运动方向同侧时，将D_cmin与D_cmax分别代入式(3)可得t_c1和t_c2，即无人车到达点P_c1、P_c2的用时；将D_cmin与D_cmax代入式(4)可得t_o1和t_o2，即障碍物到达点P_c1、P_c2的用时；

动态环境安全判定条件：当P_c1、P_c2处于同侧时，若(t_c1+t_p＜t_o1)且(t_c2+t_p＜t_o2)，或t_c1＞t_o1+t_p；当P_c1、P_c2不处于同侧，若(t_c+t_p＜t_o)或t_c＞t_o+t_p，则无人车一定拥有在和障碍物发生碰撞前进行躲避的能力，故不必添加速度势场，其中t_p为保护时间；在其它条件下，两者较大概率发生碰撞，故均为其添加速度势场。

3.根据权利要求2所述的基于碰撞预测模型的人工势场无人车动态路径规划方法，其特征在于，所述步骤S5的具体实现过程如下：

在无人车行驶过程中，当满足以下条件时将出现障碍物误导方向问题：动态障碍物出现在连接无人车与目标点的线段上并且无人车在其斥力势场范围内，V_v∈(0.7～1.3)V_o，且此时通过建立虚拟势场来进行解决，其实现方式：在无人车上取与X轴即第一象限内夹角大小为Φ的方向，即障碍物运动方向，离无人车(0.2～0.35)α₀的位置上添加虚拟障碍物来对无人车进行正确引导，使无人车从障碍物后方绕过，避免得到无效甚至错误的规划路径。