CN114610016A - 基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法 - Google Patents

Abstract

本发明提供了一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法，属于智能驾驶领域。包括以下步骤：利用车载传感器及V2X技术对车辆周边环境进行信息获取；建立障碍物虚拟膨胀的椭圆模型，确定障碍物虚拟膨胀模型的动态参数；采用人工势场法对虚拟膨胀后的障碍物、道路、局部目标点进行建模，利用梯度下降算法寻找综合势能场中沿梯度方向下降最快的路径点，并通过五次多项式对路径点进行拟合得到平滑的路径。与现有技术相比，本发明为障碍物提供了动态虚拟膨胀模型，量化了障碍物膨胀的形状与尺寸，并结合人工势场法和梯度下降算法生成避碰，保障了智能车辆规划路径的安全性、可跟踪性。

Description

基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法

技术领域

本发明属于智能驾驶领域的路径规划技术，具体涉及一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法。

背景技术

随着人工智能技术的应用和发展，以智能车辆为研究对象的路径规划问题越来越受到重视，其中避障路径规划是智能驾驶的关键部分。避障路径规划，即考虑本车和障碍物之间的几何关系寻找出一条避免与障碍物发生碰撞的路径，是智能车辆的重要功能模块之一。

智能车辆需要安全实时地避开障碍物，且具有较高的乘坐舒适性，提高出行效率，而可靠的路径规划算法是保证智能车辆成功避碰的关键。目前常用的避碰路径规划算法有人工势场法、模拟退火法、模糊逻辑法、神经网络法、动态窗口法、强化学习法及基于行为的路径规划方法。人工势场法是一种虚拟力法，它的基本思想是把车辆在周围环境中的运动视为车辆在人工建立的虚拟力场中的运动。目标点产生引力，引导车辆向目标点运动。障碍物产生斥力，避免车辆与障碍物发生碰撞，根据引力和斥力的合力来规划车辆的运动路径。应用人工势场法规划出来的路径一般比较平滑且安全，算法简明，实时性良好，适合智能车辆领域。

目前智能车辆在路径规划阶段，通常使用点质量模型，可以有效地降低路径规划的计算量。为了使路径规划的结果实际可行，需要根据车身尺寸对障碍物进行膨胀处理。

参考文献1：“Plessen MG，Bernardini D，Esen H，et al.Spatial-BasedPredictive Control and Geometric Corridor Planning for Adaptive CruiseControl Coupled With Obstacle Avoidance[J].IEEE Transactions on ControlSystems Technology，2017：1-13.(Plessen M G，Bernardini D和Esen H等，基于空间预测控制和几何廊道规划的自适应巡航避障控制，IEEE控制系统技术杂志，2017年，第1页到1027页)”提出安全裕度被添加到障碍物的轮廓中以反映车辆尺寸，形成一个矩形膨胀区域。障碍物的矩形膨胀会导致障碍物占地面积过大，且矩形的轮廓会导致规划的路径不平滑，不利于车辆的跟踪控制。

参考文献2：“Benjamin，Gutjahr，Lutz，et al.Lateral Vehicle TrajectoryOptimization Using Constrained Linear Time-Varying MPC[J].IEEE Transactionson Intelligent Transportation Systems，2017.(Benjamin，Gutjahr和Lutz等，基于约束线性时变MPC的车辆横向轨迹优化，IEEE智能交通系统杂志，2017年)”以及发明专利申请文献《基于模型预测的无人驾驶汽车局部路径规划方法》(CN111338340A)提出将智能车辆及障碍物建模成矩形，并用若干个沿车辆纵轴等距分布的特定半径的圆来近似智能车辆及障碍物。在路径规划过程中利用多圆实现智能车辆及障碍物的近似会造成计算量的增大，不利于车辆路径规划的实时性。

参考文献3：“Rosolia U，Bruyne S D，Alleyne A G.Autonomous VehicleControl：A Nonconvex Approach for Obstacle Avoidance[J].IEEE Transactions onControl Systems Technology，2017，25(2)：469-484.(Rosolia U，Bruyne S D和AlleyneA G.自动车辆控制：一种非凸的避障方法，IEEE控制系统技术杂志，2017年第25卷第2期，第469页到484页)”提出障碍物采用椭圆表示，椭圆长、短半轴分别代表障碍物的长和宽，分别将车辆的半长半宽加倍来增加安全裕度，不能适用于所有交通环境中。

综上所述，现有技术存在以下问题：

1、参考文献1报道的避碰路径规划算法采用增加矩形安全边际对障碍物进行膨胀的方法来确保规划路径的安全性，但矩形膨胀区域会不仅会使膨胀区域过大，而且矩形轮廓可能会导致规划的路径不平滑。

2、文献2将障碍物分解为沿车辆纵轴等距分布的特定半径的圆，会导致路径规划的计算量增大，不利于路径规划的实时性；

3、文献3提出将障碍物用椭圆形表示，并将因此障碍物的长宽加倍来实现障碍物的膨胀处理，但是该方法不能根据智能车辆周边的环境信息实时调整椭圆的膨胀尺寸，应用场景受限。

障碍物膨胀的形状及尺寸对避碰路径的规划有着至关重要的作用。为了使路径的规划实时性好，障碍物的膨胀形状应当简单，路径规划计算量小；障碍物的膨胀尺寸应该随着交通环境动态变化，在不同的交通环境中，障碍物的膨胀尺寸越小，其占据道路的范围越小，可通行路径范围的增大使智能车辆规划的路径平滑性更好。本发明提出的基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法不仅对智能车辆路径规划的研究具有明确的理论意义，而且对提高交通出行效率、降低事故发生率具有很强的现实意义。

发明内容

本发明要解决的技术问题就是克服上述各种方案的局限性，提出一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法。该方法不仅实现障碍物膨胀尺寸根据相对速度、相对加速度和和偏航角度的动态膨胀，还利用人工势场法对膨胀后的障碍物和道路进行建模，并设立虚拟目标点，推动车辆向前运动，结合梯度下降算法和五次多项式拟合进行路径规划及优化，完成了车辆避让静态障碍物和动态障碍物路径的规划。

本发明的目的是这样实现的，本发明提供了一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法，所述规划方法包括以下步骤：

步骤1，智能车辆状态信息及周边环境信息的获取

通过智能车辆的车载传感器、定位系统和V2X系统获取智能车辆状态信息及周边环境信息；

步骤2，参考坐标系的设定

以智能车辆几何中心点为原点，以交通流方向为x轴、垂直于x轴的方向为y轴，设定车辆坐标系；

设智能车辆开始换道时刻为t₀，以t₀时刻智能车辆所在道路的最右侧边界为u轴，垂直于u轴的方向为v轴，设定地球坐标系；

将智能车辆在t₀时刻所在的车道记为原始车道，原始车道的中心线在地球坐标系中的v轴坐标记为v_cr；并将原始车道左侧相邻车道记为左侧车道，左侧车道的中心线在地球坐标系中的v轴坐标为v_cl；

将智能车辆沿着地球坐标系u轴方向行驶的速度为纵向速度，智能车辆沿着地球坐标系v轴方向行驶的速度为横向速度；

步骤3，障碍物动态虚拟膨胀

将智能车辆前方静止物体称为静态障碍物，纵向速度小于智能车辆纵向速度的运动物体称为动态障碍物，静态障碍物和动态障碍物统称为障碍物，进行障碍物动态虚拟膨胀；

步骤3.1，将智能车辆的车身形状简化为矩形，并将该矩形的四个角分别记为智能车辆左前角P_LF、智能车辆右前角P_RF、智能车辆左后角P_LR和智能车辆右后角P_RR，在t₀时刻智能车辆左前角P_LF、智能车辆右前角P_RF、智能车辆左后角P_LR和智能车辆右后角P_RR在车辆坐标系中的坐标分别如下：

其中，θ表示智能车辆的初始航向角，L_ego为智能车辆车身长度，W_ego为智能车辆车身宽度；

步骤3.2，设智能车辆沿着地球坐标系u轴方向行驶的距离为纵向距离，智能车辆沿着地球坐标系v轴方向行驶的距离为横向距离；

设智能车辆前方有障碍物，智能车辆在t₀时刻开始向左侧车道换道，且经过t时间后智能车辆右前角P_RF与障碍物发生碰撞，将t₀时刻智能车辆几何中心点与障碍物几何中心点的纵向距离记为换道初始纵向安全距离

其计算式如下：

其中，L_obs为障碍物的长度，v_obs为障碍物的纵向速度，a_obs为障碍物的纵向加速度，v_ego为智能车辆的纵向速度，a_ego为智能车辆的纵向加速度；

将智能车辆右前角P_RF与障碍物发生碰撞时，智能车辆几何中心点与障碍物几何中心点的纵向距离记为临界纵向距离

临界纵向距离

的计算公式如下：

步骤3.3，设t₀时刻障碍物几何中心点在车辆坐标系中的坐标记为(x₀，y₀)，将障碍物的形状简化为矩形，使用该矩形的外接椭圆对障碍物进行虚拟膨胀，该外接椭圆划定的区域即为虚拟膨胀后的障碍物所占用的道路范围，并记为危险区域A_danger，其中，外接椭圆的长轴σ₁和外接椭圆的短轴σ₂的计算式分别为：

其中，W_obs表示障碍物的宽度；

步骤4，环境建模

步骤4.1，设在t₀时刻，障碍物几何中心点在地球坐标系中的坐标为(u_obs(t₀)，v_obs(t₀))；

将车辆换道避碰过程分为两个阶段，分别记为阶段S₁和阶段S₂，定义阶段S₁为智能车辆从t₀时刻开始，换道至左侧车道中心线后继续行驶，直到智能车辆几何中心点超过障碍物几何中心点的纵向距离等于临界纵向距离

的位置结束，定义阶段S₂为从左侧车道上智能车辆几何中心点超过障碍物几何中心点的纵向距离等于临界纵向距离

的位置开始，到障碍物前方的原始车道中心线位置结束；

设立局部目标点，并计算局部目标点势能场的场强，计算过程如下：

步骤4.1.1，设任意时刻智能车辆的几何中心点的位置为路径点P_k，路径点P_k在地球坐标系中的坐标为(u_k，v_k)，判断路径点P_k是否处于阶段S₁；

如果处于阶段S₁，则进入步骤4.1.2；

如果不处于阶段S₁，则进入步骤4.1.3；

步骤4.1.2，设在阶段S₁中任意一个障碍物的几何中心点位置为第一动态位置P_L1，第一动态位置P_L1在地球坐标系中的坐标为(u_L1，v_L1)，在左侧车道中心线上选取第一局部目标点P_g1，第一局部目标点P_g1在地球坐标系上的坐标为(u_g1，v_g1)，若障碍物为静止障碍物时，

v_g1＝v_cl，若障碍物为动态障碍物时，

v_g1＝v_cl，进入步骤4.1.4；

步骤4.1.3，设在阶段S₂中任意一个障碍物的几何中心点位置为第二动态位置P_L2，第二动态位置P_L2在地球坐标系中的坐标为(u_L2，v_L2)，取消第一局部目标点P_g1，在原始车道中心线上设立第二局部目标点P_g2，第二局部目标点P_g2在地球坐标系中的坐标记为(u_g2，v_g2)，若障碍物为静止障碍物时，

v_g2＝v_cr，若障碍物为动态障碍物时，

v_g2＝v_cr；

步骤4.1.4，引入局部目标点P_gi，i或为1，或为2，计算局部目标点P_gi的势能场在路径点P_k处的场强U_goali，i或为1，或为2，计算公式如下：

U_goali＝A_goal[(u_k-u_gi)²+(v_k-v_gi)²]

其中，A_goal为局部目标点势能场的场强系数；

步骤4.2，对步骤3得到的危险区域A_danger计算势能场场强，危险区域A_danger产生的势能场在路径点P_k处的场强U_obstacle的计算公式如下：

0＜SD＜SD₀

其中，A_obstacle为危险区域势能场系数，SD₀为危险区域的影响距离，SD为智能车辆几何中心点到危险区域A_danger边界的最短距离；

步骤4.3，将智能车辆周边的道路车道线分成不可跨越车道线和可跨越车道线，且假设车道线是直线，道路车道线产生的势能场在路径点P_k处的场强U_road的计算公式如下：

其中，A_road为道路势能场系数，L_w为车道宽度，m₁为不可跨越障碍物车道线的系数，m₂为可跨越障碍物车道线的系数；

步骤4.4，将与局部目标点P_gi对应的在路径点P_k处的智能车辆周边环境综合势能场记为综合势能场，并将综合势能场的场强记为综合场强U_i，i或为1，或为2，其计算公式如下：

U_i＝U_obstacle+U_road+U_goali

步骤5，避碰路径的生成

定义避碰路径如下：以智能车辆从原始车道开始向左侧车道换道时的几何中心点为起点、智能车辆几何中心点到达第二局部目标点P_g2的位置为终点，由N个避碰路径点组成的避碰路径；

步骤5.1，计算综合场强U_i的负梯度

i或为1，或为2，计算公式如下：

其中，

表示路径点P_k处综合场强U_i沿地球坐标系中u轴的变化率，

表示路径点P_k处综合场强U_i沿地球坐标系中v轴的变化率，i或为1，或为2；

将路径点P_k处的最快下降速度记为v_ki，i或为1，或为2，并令最快下降速度v_ki等于综合场强U_i的负梯度

其计算公式如下：

将最快下降速度v_ki单位化，得到单位最快下降速度

i或为1，或为2，计算公式如下：

将路径点P_k的下一个相邻路径点记为路径点P_k+1，用步长λ乘以单位最快下降速度

得出每次迭代前进的距离，并与路径点P_k在地球坐标系中的坐标(u_k，v_k)相加，迭代得到路径点P_k+1在地球坐标系中的坐标(u_k+1，v_k+1)，计算公式如下：

步骤5.2，以智能车辆在避碰过程中到达第一局部目标点P_g1将避碰路径分成两段，分别记为避碰路径1和避碰路径2；将避碰路径1中任意一个路径点记为路径点P_1α，与路径点P_1α对应的障碍物位置记为障碍物位置P_1m，路径点P_1α处在与第一局部目标点P_g1对应的综合场强U₁的综合势能场中，将避碰路径2中任意一个路径点记为路径点P_2β，与路径点P_2β对应的障碍物位置记为障碍物位置P_2n，路径点P_2β处在与第二局部目标点P_g2对应的综合场强U₂的综合势能场中，其中α为避碰路径1中的路径点的序号，α＝1，2，…，N1，m为避碰路径1中障碍物位置的序列号，m＝1，2，…，N1，β为避碰路径2中的路径点的序号，β＝1，2，…，N2，n为避碰路径2中障碍物位置的序列号，n＝1，2，…，N2，N1+N2＝N；

步骤5.3，避碰路径中N个路径点的计算

步骤5.3.1，避碰路径1中N1个路径点的计算

设路径点P_1α在地球坐标系中的坐标为(u_1α，v_1α)，障碍物位置P_1m在地球坐标系中的坐标为(u_1m，v_1m)，将路径点P_1α的下一个相邻路径点记为路径点P_1α+1，将路径点P_1α+1对应的障碍物位置记为障碍物位置P_1m+1，路径点P_1a+1在地球坐标系中的坐标为(u_1α+1，v_1α+1)，按照步骤5.1的方法计算得到坐标(u_1α+1，v_1α+1)，u_1α+1和v_1α+1计算公式如下：

设智能车辆避碰路径规划周期为T，障碍物位置P_1m+1在地球坐标系中的坐标为(u_1m+1，v_1m+1)，u_1m+1和v_1m+1的计算公式如下：

v_1m+1＝v_cl

记录路径点P_1α+1，并计算路径点P_1α+1与第一局部目标点P_g1之间的第一距离误差ΔS₁，计算公式如下：

ΔS₁＝||P_1α+1-P_g1||

若第一距离误差ΔS₁小于或等于阈值ΔS_min，进入步骤5.3.2；

若第一距离误差ΔS₁大于阈值ΔS_min，令P_1α+1＝P_k，P_1m+1＝P_L1并返回步骤4.1.1；

步骤5.3.2，避碰路径2中N2个路径点的计算

设路径点P_2β在地球坐标系中的坐标为(u_2β，v_2β)，障碍物位置P_2n在地球坐标系中的坐标为(u_2n，v_2n)，将路径点P_2β的下一个相邻路径点记为路径点P_2β+1，将与路径点P_2β+1相对应的障碍物位置记为障碍物位置P_2n+1，路径点P_2β+1在地球坐标系中的坐标为(u_2β+1，v_2β+1)，按照步骤5.1的方法计算得到坐标(u_2β+1，v_2β+1)，u_2β+1和v_2β+1的计算公式如下：

设智能车辆避碰路径规划周期为T，障碍物位置P_2n+1在地球坐标系中的坐标为(u_2n+1，v_2n+1)，u_2n+1和v_2n+1的计算公式如下：

v_2n+1＝v_cr

记录路径点P_2β+1，并计算路径点P_2β+1与第二局部目标点P_g2之间的第二距离误差ΔS₂，计算公式如下：

ΔS₂＝||P_2β+1-P_g2||

若第二距离误差ΔS₂小于或等于阈值ΔS_min，进入步骤5.4；

若第二距离误差ΔS₂大于阈值ΔS_min，令P_2β+1＝P_k，P_2n+1＝P_L2，并返回步骤4.1.1；

步骤5.4，避碰队列设定

按照步骤5.3.1的方式，从避碰路径1的起点P₁₁开始，计算得到避碰路径1中N1个路径点的位置，并将N1个路径点组成避碰路径1的避碰队列[P₁₁，P₁₂，…，P_1α，，P_1α+1…，P_1N1]，其中避碰路径1的起点P₁₁为智能车辆在t₀时刻的位置；

按照步骤5.3.2的方式，从避碰路径2的起点P₂₁开始，计算得到避碰路径2中N2个路径点的位置，并将N2个路径点组成避碰路径2的避碰队列[P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]，其中避碰路径2的起点P₂₁为步骤4.1.2中第一局部目标点P_g1的位置；

将避碰路径1的避碰队列[P₁₁，P₁₂，…，P_1α，，P_1α+1，…，P_1N1]和避碰路径2的避碰队列[P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]合并，得到一个完整的避碰队列P，P＝[P₁₁，P₁₂，…，P_1α，，P_1α+1，…，P_1N1，P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]；

步骤5.5，采用五次多项式对完整的避碰队列P中N个路径点进行拟合，得到五次多项式的系数和常数：包括五次多项式的五次项系数c₀、五次多项式的四次项系数c₁、五次多项式的三次项系数c₂、五次多项式的二次项系数c₃、五次多项式的一次项系数c₄和五次多项式的常数c₅；将智能车辆避碰路径中的任意一个点记为P_poly，P_poly在地球坐标系中坐标记为(u_poly，v_poly)，(u_poly，v_poly)满足下式：

优选地，步骤所述智能车辆状态信息包括智能车辆的位置、智能车辆的纵向速度、智能车辆的纵向加速度、智能车辆的横向速度、智能车辆的航向角、智能车辆的长宽信息；智能车辆周边环境信息包括车道宽度、车道线类型、静态障碍物的位置、静态障碍物的长宽、动态障碍物的位置、动态障碍物的纵向速度、动态障碍物的纵向加速度、动态障碍物的长宽信息；

优选地，所述时间t按照以下公式确定：

其中，W_ego为障碍物的宽度，u_obs为智能车辆的横向速度；

优选地，所述最短距离SD按照以下方法确定：

在危险区域边界上取任意一点记为危险边界点P_s，画危险边界点P_s与障碍物几何中心点的连线，且将该连线与车辆坐标系x轴正方向之间的夹角记为危险夹角

则危险边界点P_s的坐标为

最短距离SD的计算式如下：

与现有技术相比，本发明的有益效果是：

1、与障碍物矩形膨胀模型相比，本发明提出的障碍物椭圆膨胀区域所占车道空间更少，为车辆避碰路径的规划提供更多空间，更有利于实现路径的规划。

2、与参考文献3中的障碍物椭圆形虚拟膨胀方法相比，本发明提出了障碍物动态虚拟膨胀椭圆模型，并依据换道避碰纵向安全距离，计算膨胀椭圆模型的参数，量化了障碍物膨胀的形状与尺寸。该模型的参数根据车辆相对速度、航向角动态变化，能够更好的适应不同交通环境。

3、将障碍物虚拟膨胀模型与人工势场法结合，并利用五次多项式对路径点进行拟合优化，路径规划不仅实时性良好，且规划出的路径平滑且安全。

附图说明

图1是本实施例中基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法整体流程图；

图2是本实施例中两个坐标系的示意图；

图3是本实施例中智能车辆向左侧换道避碰过程示意图；

图4是本实施例中障碍物动态虚拟膨胀模型的示意图；

图5是本实施例中智能车辆几何中心与危险区域A_danger边界的最短距离示意图；

图6是本实施例中不同类型的车道线示意图；

图7是本实施例中智能车辆局部目标点的选取示意图；

图8是本实施例中智能车辆前方静态障碍物避碰场景示意图；

图9是本实施例中智能车辆在静态障碍物场景中规划的避碰路径；

图10是本实施例中智能车辆前方动态障碍物避碰场景示意图；

图11是本实施例中智能车辆在动态障碍物场景中规划的避碰路径；

图12是本实施例中智能车辆配套的智能车辆避碰路径规划系统框图。

具体实施方式

下面结合附图和实施例，对本发明一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法进行详细说明。

图1是本实施例的基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法的流程图，由该图可见，本发明避碰路径规划方法的步骤如下：

步骤1，智能车辆状态信息及周边环境信息的获取

通过智能车辆的车载传感器、定位系统和V2X系统获取智能车辆状态信息及周边环境信息；

所述智能车辆状态信息包括智能车辆的位置、智能车辆的纵向速度、智能车辆的纵向加速度、智能车辆的横向速度、智能车辆的航向角、智能车辆的长宽信息；智能车辆周边环境信息包括车道宽度、车道线类型、静态障碍物的位置、静态障碍物的长宽、动态障碍物的位置、动态障碍物的纵向速度、动态障碍物的纵向加速度、动态障碍物的长宽信息。

步骤2，参考坐标系的设定

以智能车辆几何中心点为原点，以交通流方向为x轴、垂直于x轴的方向为y轴，设定车辆坐标系；

设智能车辆开始换道时刻为t₀，以t₀时刻智能车辆所在道路的最右侧边界为u轴，垂直于u轴的方向为v轴，设定地球坐标系；

将智能车辆在t₀时刻所在的车道记为原始车道，原始车道的中心线在地球坐标系中的v轴坐标记为v_cr；并将原始车道左侧相邻车道记为左侧车道，左侧车道的中心线在地球坐标系中的v轴坐标为v_cl；

将智能车辆沿着地球坐标系u轴方向行驶的速度为纵向速度，智能车辆沿着地球坐标系v轴方向行驶的速度为横向速度。

图2是本实施例中两个坐标系的示意图，由图2可见，车辆坐标系和地球坐标系的关系如下：

u＝xsinθ+ycosθ

v＝xsinθ+ycosθ

其中，θ表示智能车辆的初始航向角。

步骤3，障碍物动态虚拟膨胀

将智能车辆前方静止物体称为静态障碍物，纵向速度小于智能车辆纵向速度的运动物体称为动态障碍物，静态障碍物和动态障碍物统称为障碍物，进行障碍物动态虚拟膨胀。

步骤3.1，将智能车辆的车身形状简化为矩形，并将该矩形的四个角分别记为智能车辆左前角P_LF、智能车辆右前角P_RF、智能车辆左后角P_LR和智能车辆右后角P_RR，在t₀时刻智能车辆左前角P_LF、智能车辆右前角P_RF、智能车辆左后角P_LR和智能车辆右后角P_RR在车辆坐标系中的坐标分别如下：

其中，θ表示智能车辆的初始航向角，L_ego为智能车辆车身长度，W_ego为智能车辆车身宽度。

步骤3.2，设智能车辆沿着地球坐标系u轴方向行驶的距离为纵向距离，智能车辆沿着地球坐标系v轴方向行驶的距离为横向距离。

设智能车辆前方有障碍物，智能车辆在t₀时刻开始向左侧车道换道，且经过t时间后智能车辆右前角P_RF与障碍物发生碰撞，将t₀时刻智能车辆几何中心点与障碍物几何中心点的纵向距离记为换道初始纵向安全距离

其计算式如下：

其中，L_obs为障碍物的长度，v_obs为障碍物的纵向速度，a_obs为障碍物的纵向加速度，v_ego为智能车辆的纵向速度，a_ego为智能车辆的纵向加速度。

所述时间t按照以下公式确定：

其中，W_ego为障碍物的宽度，u_obs为智能车辆的横向速度。

将智能车辆右前角P_RF与障碍物发生碰撞时，智能车辆几何中心点与障碍物几何中心点的纵向距离记为临界纵向距离

临界纵向距离

的计算公式如下：

步骤3.3，设t₀时刻障碍物几何中心点在车辆坐标系中的坐标记为(x₀，y₀)，将障碍物的形状简化为矩形，使用该矩形的外接椭圆对障碍物进行虚拟膨胀，该外接椭圆划定的区域即为虚拟膨胀后的障碍物所占用的道路范围，并记为危险区域A_danger，其中，外接椭圆的长轴σ₁和外接椭圆的短轴σ₂的计算式分别为：

其中，W_obs表示障碍物的宽度。

图3是本实施例中智能车辆向左侧换道避碰过程示意图，图4是本实施例中障碍物动态虚拟膨胀模型的示意图。在本实施例中，θ＝15度，x₀＝15，y₀＝2，σ₁＝15，σ₂＝2，L_obs＝3米，W_obs＝2米，L_ego＝3米，W_ego＝2米。

步骤4，环境建模

步骤4.1，设在t₀时刻，障碍物几何中心点在地球坐标系中的坐标为(u_obs(t₀)，v_obs(t₀))。

将车辆换道避碰过程分为两个阶段，分别记为阶段S₁和阶段S₂，定义阶段S₁为智能车辆从t₀时刻开始，换道至左侧车道中心线后继续行驶，直到智能车辆几何中心点超过障碍物几何中心点的纵向距离等于临界纵向距离

的位置结束，定义阶段S₂为从左侧车道上智能车辆几何中心点超过障碍物几何中心点的纵向距离等于临界纵向距离

的位置开始，到障碍物前方的原始车道中心线位置结束。

设立局部目标点，并计算局部目标点势能场的场强，计算过程如下：

步骤4.1.1，设任意时刻智能车辆的几何中心点的位置为路径点P_k，路径点P_k在地球坐标系中的坐标为(u_k，v_k)，判断路径点P_k是否处于阶段S₁；

如果处于阶段S₁，则进入步骤4.1.2；

如果不处于阶段S₁，则进入步骤4.1.3。

步骤4.1.2，设在阶段S₁中任意一个障碍物的几何中心点位置为第一动态位置P_L1，第一动态位置P_L1在地球坐标系中的坐标为(u_L1，v_L1)，在左侧车道中心线上选取第一局部目标点P_g1，第一局部目标点P_g1在地球坐标系上的坐标为(u_g1，v_g1)，若障碍物为静止障碍物时，

v_g1＝v_cl，若障碍物为动态障碍物时，

v_g1＝v_cl，进入步骤4.1.4。

步骤4.1.3，设在阶段S₂中任意一个障碍物的几何中心点位置为第二动态位置P_L2，第二动态位置P_L2在地球坐标系中的坐标为(u_L2，v_L2)，取消第一局部目标点P_g1，在原始车道中心线上设立第二局部目标点P_g2，第二局部目标点P_g2在地球坐标系中的坐标记为(u_g2，v_g2)，若障碍物为静止障碍物时，

v_g2＝v_cr，若障碍物为动态障碍物时，

v_g2＝v_cr。

步骤4.1.4，引入局部目标点P_gi，i或为1，或为2，计算局部目标点P_gi的势能场在路径点P_k处的场强U_goali，i或为1，或为2，计算公式如下：

U_goali＝A_goal[(u_k-u_gi)²+(v_k-v_gi)²]

其中，A_goal为局部目标点势能场的场强系数。

步骤4.2，对步骤3得到的危险区域A_danger计算势能场场强，危险区域A_danger产生的势能场在路径点P_k处的场强U_obstacle的计算公式如下：

0＜SD＜SD₀

其中，A_obstacle为危险区域势能场系数，SD₀为危险区域的影响距离；SD为智能车辆几何中心点到危险区域A_danger边界的最短距离。

所述最短距离SD按照以下方法确定：

在危险区域A_danger边界上取任意一点记为危险边界点P_s，画危险边界点P_s与障碍物几何中心点的连线，且将该连线与车辆坐标系x轴正方向之间的夹角记为危险夹角

危险夹角

的取值范围是

则危险边界点P_s的坐标为

最短距离SD的计算式如下：

步骤4.3，将智能车辆周边的道路车道线分成不可跨越车道线和可跨越车道线，且假设车道线是直线，道路车道线产生的势能场在路径点P_k处的场强U_road的计算公式如下：

其中，A_road为道路势能场系数，L_w为车道宽度，m₁为不可跨越障碍物车道线的系数，m₂为可跨越障碍物车道线的系数。

步骤4.4，将与局部目标点P_gi对应的在路径点P_k处的智能车辆周边环境综合势能场记为综合势能场，并将综合势能场的场强记为综合场强U_i，i或为1，或为2，其计算公式如下：

U_i＝U_obstacle+U_road+U_goali

图5是本实施例中智能车辆几何与危险区域A_danger边界的最短距离示意图，展示了通过上述步骤将障碍物进行虚拟膨胀后的效果图，椭圆形的膨胀区域尺寸随着智能车辆的航向角、智能车辆与障碍物的相对速度、智能车辆的尺寸以及障碍物的尺寸确定，其膨胀区域范围是动态变化的。

图6是本实施例不同类型的车道线示意图，展示了道路中实线和虚线两种不同的车道线类型，实线代表不可跨越车道线，虚线表示可跨越车道线。

图7是本实施例中智能车辆局部目标点的选取示意图。

本实施例中，m₁＝0.8，m₂＝0.2，A_obstacle＝12，SD₀＝0.02，L_w＝4米，A_road＝2，

步骤5，避碰路径的生成

定义避碰路径如下：以智能车辆从原始车道开始向左侧车道换道时的几何中心点为起点、智能车辆几何中心点到达第二局部目标点P_g2的位置为终点，由N个避碰路径点组成的避碰路径。

步骤5.1，计算综合场强U_i的负梯度

i或为1，或为2，计算公式如下：

其中，

表示路径点P_k处综合场强U_i沿地球坐标系中u轴的变化率，

表示路径点P_k处综合场强U_i沿地球坐标系中v轴的变化率，i或为1，或为2。

将路径点P_k处的最快下降速度记为v_ki，i或为1，或为2，并令最快下降速度v_ki等于综合场强U_i的负梯度

其计算公式如下：

将最快下降速度v_ki单位化，得到单位最快下降速度

i或为1，或为2，计算公式如下：

将路径点P_k的下一个相邻路径点记为路径点P_k+1，用步长λ乘以单位最快下降速度

得出每次迭代前进的距离，并与路径点P_k在地球坐标系中的坐标(u_k，v_k)相加，迭代得到路径点P_k+1在地球坐标系中的坐标(u_k+1，v_k+1)，计算公式如下：

在本实施例中，步长λ＝0.1。

步骤5.2，以智能车辆在避碰过程中到达第一局部目标点P_g1将避碰路径分成两段，分别记为避碰路径1和避碰路径2；将避碰路径1中任意一个路径点记为路径点P_1α，与路径点P_1α对应的障碍物位置记为障碍物位置P_1m，路径点P_1α处在与第一局部目标点P_g1对应的综合场强U₁的综合势能场中，将避碰路径2中任意一个路径点记为路径点P_2β，与路径点P_2β对应的障碍物位置记为障碍物位置P_2n，路径点P_2β处在与第二局部目标点P_g2对应的综合场强U₂的综合势能场中，其中α为避碰路径1中的路径点的序号，α＝1，2，…，N1，m为避碰路径1中障碍物位置的序列号，m＝1，2，…，N1，β为避碰路径2中的路径点的序号，β＝1，2，…，N2，n为避碰路径2中障碍物位置的序列号，n＝1，2，…，N2，N1+N2＝N。

步骤5.3，避碰路径中N个路径点的计算

步骤5.3.1，避碰路径1中N1个路径点的计算

设路径点P_1α在地球坐标系中的坐标为(u_1α，v_1α)，障碍物位置P_1m在地球坐标系中的坐标为(u_1m，v_1m)，将路径点P_1α的下一个相邻路径点记为路径点P_1α+1，将路径点P_1α+1对应的障碍物位置记为障碍物位置P_1m+1，路径点P_1α+1在地球坐标系中的坐标为(u_1α+1，v_1a+1)，按照步骤5.1的方法计算得到坐标(u_1α+1，v_1α+1)，u_1α+1和v_1α+1计算公式如下：

设智能车辆避碰路径规划周期为T，障碍物位置P_1m+1在地球坐标系中的坐标为(u_1m+1，v_1m+1)，u_1m+1和v_1m+1的计算公式如下：

v_1m+1＝v_cl

记录路径点P_1α+1，并计算路径点P_la+1与第一局部目标点P_g1之间的第一距离误差ΔS₁，计算公式如下：

ΔS₁＝||P_1α+1-P_g1||

若第一距离误差ΔS₁小于或等于阈值ΔS_min，进入步骤5.3.2；

若第一距离误差ΔS₁大于阈值ΔS_min，令P_1α+1＝P_k，P_1m+1＝P_L1并返回步骤4.1.1。

在本实施例中，阈值ΔS_min＝0.05。

步骤5.3.2，避碰路径2中N2个路径点的计算

设路径点P_2β在地球坐标系中的坐标为(u_2β，v_2β)，障碍物位置P_2n在地球坐标系中的坐标为(u_2n，v_2n)，将路径点P_2β的下一个相邻路径点记为路径点P_2β+1，将与路径点P_2β+1相对应的障碍物位置记为障碍物位置P_2n+1，路径点P_2β+1在地球坐标系中的坐标为(u_2β+1，v_2β+1)，按照步骤5.1的方法计算得到坐标(u_2β+1，v_2β+1)，u_2β+1和v_2β+1的计算公式如下：

设智能车辆避碰路径规划周期为T，障碍物位置P_2n+1在地球坐标系中的坐标为(u_2n+1，v_2n+1)，u_2n+1和v_2n+1的计算公式如下：

v_2n+1＝v_cr

记录路径点P_2β+1，并计算路径点P_2β+1与第二局部目标点P_g2之间的第二距离误差ΔS₂，计算公式如下：

ΔS₂＝||P_2β+1-P_g2||

若第二距离误差ΔS₂小于或等于阈值ΔS_min，进入步骤5.4；

若第二距离误差ΔS₂大于阈值ΔS_min，令P_2β+1＝P_k，P_2n+1＝P_L2，并返回步骤4.1.1。

步骤5.4，避碰队列设定

按照步骤5.3.1的方式，从避碰路径1的起点P₁₁开始，计算得到避碰路径1中N1个路径点的位置，并将N1个路径点组成避碰路径1的避碰队列[P₁₁，P₁₂，…，P_1α，，P_1α+1，…，P_1N1]，其中避碰路径1的起点P₁₁为智能车辆在t₀时刻的位置。

按照步骤5.3.2的方式，从避碰路径2的起点P₂₁开始，计算得到避碰路径2中N2个路径点的位置，并将N2个路径点组成避碰路径2的避碰队列[P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]，其中避碰路径2的起点P₂₁为步骤4.1.2中第一局部目标点P_g1的位置。

将避碰路径1的避碰队列[P₁₁，P₁₂，…，P_1α，，P_1α+1，…，P_1N1]和避碰路径2的避碰队列[P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]合并，得到一个完整的避碰队列P，P＝[P₁₁，P₁₂，…，P_1α，，P_1α+1，…，P_1N1，P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]。

步骤5.5，采用五次多项式对完整的避碰队列P中N个路径点进行拟合，得到五次多项式的系数和常数：包括五次多项式的五次项系数c₀、五次多项式的四次项系数c₁、五次多项式的三次项系数c₂、五次多项式的二次项系数c₃、五次多项式的一次项系数c₄和五次多项式的常数c₅；将智能车辆避碰路径中的任意一个点记为P_poly，P_poly在地球坐标系中坐标记为(u_poly，v_poly)，(u_poly，v_poly)满足下式：

至此，基于障碍物动态虚拟膨胀的智能车辆避碰路径规划结束。

为了实现该方法，还配套在智能车辆上安装了一个智能车辆避碰路径规划系统，具体见图12。由图12可见，该系统包括环境感知模块和路径规划模块。所述环境感知模块对车载传感器、定位系统和V2X系统接收的信息进行处理，获取智能车辆周边环境信息和智能车辆自身的状态信息，周边环境信息包括车道宽度、车道线类型、静态障碍物的位置、静态障碍物的长宽、动态障碍物的位置、动态障碍物的纵向速度、动态障碍物的纵向加速度、动态障碍物的长宽信息，智能车辆自身的状态信息包括智能车辆的位置、智能车辆的纵向速度、智能车辆的纵向加速度、智能车辆的横向速度、智能车辆的航向角、智能车辆的长宽，并将处理结果发送给路径规划模块；所述路径规划模块包括坐标系设定、障碍物动态虚拟膨胀、环境建模和路径生成策略，其中，步骤1在环境感知模块完成的，步骤2、步骤3、步骤4和步骤5在路径规划模块完成。

为了验证本发明的基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法的有效性，在Matlab环境下搭建了静态障碍物和动态障碍物两种场景进行验证。

一、静态障碍物场景

图8给出了本实施例中智能车辆前方静态障碍物避碰场景，在该静态障碍物避碰场景中，道路是直的，并由可跨越车道线和不可跨越车道线组成。智能车辆以10m/s的速度沿着车流方向行驶，智能车辆前方15m处出现了一个静止障碍物，椭圆形区域为虚拟膨胀后的障碍物所占道路范围，属于危险区域。图9给出了本实施例中智能车辆在静态障碍物场景中规划的避碰路径，从图9中可以看出，智能车辆在静态场景中采用本发明的方法进行路径规划，能够成功避开障碍物。

二、动态障碍物场景

图10给出了本实施例智能车辆前方动态障碍物避碰场景，在该动态障碍物避碰场景中，智能车辆以10m/s的速度沿着车流方向行驶，障碍物以5m/s的速度沿着原车道向前行驶，智能车辆换道初始时刻与动态障碍物相距15m，椭圆形区域为虚拟膨胀后的障碍物危险区域，该危险区域随着障碍物一起向前移动。图11给出了本实施例中智能车辆在动态障碍物场景中的避碰路径，从该图可见，智能车辆在动态场景中采用本发明的方法进行路径规划，能够成功避开障碍物。

Claims (4)

1.一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法，其特征在于，所述规划方法包括以下步骤：

步骤1，智能车辆状态信息及周边环境信息的获取

通过智能车辆的车载传感器、定位系统和V2X系统获取智能车辆状态信息及周边环境信息；

步骤2，参考坐标系的设定

以智能车辆几何中心点为原点，以交通流方向为x轴、垂直于x轴的方向为y轴，设定车辆坐标系；

设智能车辆开始换道时刻为t₀，以t₀时刻智能车辆所在道路的最右侧边界为u轴，垂直于u轴的方向为v轴，设定地球坐标系；

将智能车辆在t₀时刻所在的车道记为原始车道，原始车道的中心线在地球坐标系中的v轴坐标记为v_cr；并将原始车道左侧相邻车道记为左侧车道，左侧车道的中心线在地球坐标系中的v轴坐标为v_cl；

将智能车辆沿着地球坐标系u轴方向行驶的速度为纵向速度，智能车辆沿着地球坐标系v轴方向行驶的速度为横向速度；

步骤3，障碍物动态虚拟膨胀

将智能车辆前方静止物体称为静态障碍物，纵向速度小于智能车辆纵向速度的运动物体称为动态障碍物，静态障碍物和动态障碍物统称为障碍物，进行障碍物动态虚拟膨胀；

步骤3.1，将智能车辆的车身形状简化为矩形，并将该矩形的四个角分别记为智能车辆左前角P_LF、智能车辆右前角P_RF、智能车辆左后角P_LR和智能车辆右后角P_RR，在t₀时刻智能车辆左前角P_LF、智能车辆右前角P_RF、智能车辆左后角P_LR和智能车辆右后角P_RR在车辆坐标系中的坐标分别如下：

P_LF：

P_RF：

P_LR：

P_RR：

其中，θ表示智能车辆的初始航向角，L_ego为智能车辆车身长度，W_ego为智能车辆车身宽度；

步骤3.2，设智能车辆沿着地球坐标系u轴方向行驶的距离为纵向距离，智能车辆沿着地球坐标系v轴方向行驶的距离为横向距离；

设智能车辆前方有障碍物，智能车辆在t₀时刻开始向左侧车道换道，且经过t时间后智能车辆右前角P_RF与障碍物发生碰撞，将t₀时刻智能车辆几何中心点与障碍物几何中心点的纵向距离记为换道初始纵向安全距离

其计算式如下：

其中，L_obs为障碍物的长度，v_obs为障碍物的纵向速度，a_obs为障碍物的纵向加速度，v_ego为智能车辆的纵向速度，a_ego为智能车辆的纵向加速度；

将智能车辆右前角P_RF与障碍物发生碰撞时，智能车辆几何中心点与障碍物几何中心点的纵向距离记为临界纵向距离

临界纵向距离

的计算公式如下：

步骤3.3，设t₀时刻障碍物几何中心点在车辆坐标系中的坐标记为(x₀，y₀)，将障碍物的形状简化为矩形，使用该矩形的外接椭圆对障碍物进行虚拟膨胀，该外接椭圆划定的区域即为虚拟膨胀后的障碍物所占用的道路范围，并记为危险区域A_danger，其中，外接椭圆的长轴σ₁和外接椭圆的短轴σ₂的计算式分别为：

其中，W_obs表示障碍物的宽度；

步骤4，环境建模

步骤4.1，设在t₀时刻，障碍物几何中心点在地球坐标系中的坐标为(u_obs(t₀)，v_obs(t₀))；

将车辆换道避碰过程分为两个阶段，分别记为阶段S₁和阶段S₂，定义阶段S₁为智能车辆从t₀时刻开始，换道至左侧车道中心线后继续行驶，直到智能车辆几何中心点超过障碍物几何中心点的纵向距离等于临界纵向距离

的位置结束，定义阶段S₂为从左侧车道上智能车辆几何中心点超过障碍物几何中心点的纵向距离等于临界纵向距离

的位置开始，到障碍物前方的原始车道中心线位置结束；

设立局部目标点，并计算局部目标点势能场的场强，计算过程如下：

步骤4.1.1，设任意时刻智能车辆的几何中心点的位置为路径点P_k，路径点P_k在地球坐标系中的坐标为(u_k，v_k)，判断路径点P_k是否处于阶段S₁；

如果处于阶段S₁，则进入步骤4.1.2；

如果不处于阶段S₁，则进入步骤4.1.3；

步骤4.1.2，设在阶段S₁中任意一个障碍物的几何中心点位置为第一动态位置P_L1，第一动态位置P_L1在地球坐标系中的坐标为(u_L1，v_L1)，在左侧车道中心线上选取第一局部目标点P_g1，第一局部目标点P_g1在地球坐标系上的坐标为(u_g1，v_g1)，若障碍物为静止障碍物时，

若障碍物为动态障碍物时，

v_g1＝v_cl，进入步骤4.1.4；

步骤4.1.3，设在阶段S₂中任意一个障碍物的几何中心点位置为第二动态位置P_L2，第二动态位置P_L2在地球坐标系中的坐标为(u_L2，v_L2)，取消第一局部目标点P_g1，在原始车道中心线上设立第二局部目标点P_g2，第二局部目标点P_g2在地球坐标系中的坐标记为(u_g2，v_g2)，若障碍物为静止障碍物时，

v_g2＝v_cr，若障碍物为动态障碍物时，

v_g2＝v_cr；

步骤4.1.4，引入局部目标点P_gi，i或为1，或为2，计算局部目标点P_gi的势能场在路径点P_k处的场强U_goali，i或为1，或为2，计算公式如下：

U_goali＝A_goal[(u_k-u_gi)²+(v_k-v_gi)²]

其中，A_goal为局部目标点势能场的场强系数；

步骤4.2，对步骤3得到的危险区域A_danger计算势能场场强，危险区域A_danger产生的势能场在路径点P_k处的场强U_obstacle的计算公式如下：

其中，A_obstacle为危险区域势能场系数，SD₀为危险区域的影响距离，SD为智能车辆几何中心点到危险区域A_danger边界的最短距离；

步骤4.3，将智能车辆周边的道路车道线分成不可跨越车道线和可跨越车道线，且假设车道线是直线，道路车道线产生的势能场在路径点P_k处的场强U_road的计算公式如下：

其中，A_road为道路势能场系数，L_w为车道宽度，m₁为不可跨越障碍物车道线的系数，m₂为可跨越障碍物车道线的系数；

步骤4.4，将与局部目标点P_gi对应的在路径点P_k处的智能车辆周边环境综合势能场记为综合势能场，并将综合势能场的场强记为综合场强U_i，i或为1，或为2，其计算公式如下：

U_i＝U_obstacle+U_road+U_goali

步骤5，避碰路径的生成

定义避碰路径如下：以智能车辆从原始车道开始向左侧车道换道时的几何中心点为起点、智能车辆几何中心点到达第二局部目标点P_g2的位置为终点，由N个避碰路径点组成的避碰路径；

步骤5.1，计算综合场强U_i的负梯度

i或为1，或为2，计算公式如下：

其中，

表示路径点P_k处综合场强U_i沿地球坐标系中u轴的变化率，

表示路径点P_k处综合场强U_i沿地球坐标系中v轴的变化率，i或为1，或为2；

将路径点P_k处的最快下降速度记为v_ki，i或为1，或为2，并令最快下降速度v_ki等于综合场强U_i的负梯度

其计算公式如下：

将最快下降速度v_ki单位化，得到单位最快下降速度

i或为1，或为2，计算公式如下：

将路径点P_k的下一个相邻路径点记为路径点P_k+1，用步长λ乘以单位最快下降速度

得出每次迭代前进的距离，并与路径点P_k在地球坐标系中的坐标(u_k，v_k)相加，迭代得到路径点P_k+1在地球坐标系中的坐标(u_k+1，v_k+1)，计算公式如下：

步骤5.2，以智能车辆在避碰过程中到达第一局部目标点P_g1将避碰路径分成两段，分别记为避碰路径1和避碰路径2；将避碰路径1中任意一个路径点记为路径点P_1α，与路径点P_1α对应的障碍物位置记为障碍物位置P_1m，路径点P_1α处在与第一局部目标点P_g1对应的综合场强U₁的综合势能场中，将避碰路径2中任意一个路径点记为路径点P_2β，与路径点P_2β对应的障碍物位置记为障碍物位置P_2n，路径点P_2β处在与第二局部目标点P_g2对应的综合场强U₂的综合势能场中，其中α为避碰路径1中的路径点的序号，α＝1，2，…，N1，m为避碰路径1中障碍物位置的序列号，m＝1，2，…，N1，β为避碰路径2中的路径点的序号，β＝1，2，…，N2，n为避碰路径2中障碍物位置的序列号，n＝1，2，…，N2，N1+N2＝N；

步骤5.3，避碰路径中N个路径点的计算

步骤5.3.1，避碰路径1中N1个路径点的计算

设路径点P_1α在地球坐标系中的坐标为(u_1α，v_1α)，障碍物位置P_1m在地球坐标系中的坐标为(u_1m，v_1m)，将路径点P_1α的下一个相邻路径点记为路径点P_1α+1，将路径点P_1α+1对应的障碍物位置记为障碍物位置P_1m+1，路径点P_1α+1在地球坐标系中的坐标为(u_1a+1，v_1α+1)，按照步骤5.1的方法计算得到坐标(u_1α+1，v_1α+1)，u_1a+1和v_1α+1计算公式如下：

设智能车辆路避碰路径规划周期为T，障碍物位置P_1m+1在地球坐标系中的坐标为(u_1m+1，v_1m+1)，u_1m+1和v_1m+1的计算公式如下：

v_1m+1＝v_cl

记录路径点P_1α+1，并计算路径点P_1α+1与第一局部目标点P_g1之间的第一距离误差ΔS₁，计算公式如下：

ΔS₁＝||P_1α+1-P_g1||

若第一距离误差ΔS₁小于或等于阈值ΔS_min，进入步骤5.3.2；

若第一距离误差ΔS₁大于阈值ΔS_min，令P_1α+1＝P_k，P_1m+1＝P_L1并返回步骤4.1.1；

步骤5.3.2，避碰路径2中N2个路径点的计算

设路径点P_2β在地球坐标系中的坐标为(u_2β，v_2β)，障碍物位置P_2n在地球坐标系中的坐标为(u_2n，v_2n)，将路径点P_2β的下一个相邻路径点记为路径点P_2β+1，将与路径点P_2β+1相对应的障碍物位置记为障碍物位置P_2n+1，路径点P_2β+1在地球坐标系中的坐标为(u_2β+1，v_2β+1)，按照步骤5.1的方法计算得到坐标(u_2β+1，v_2β+1)，u_2β+1和v_2β+1的计算公式如下：

设智能车辆避碰路径规划周期为T，障碍物位置P_2n+1在地球坐标系中的坐标为(u_2n+1，v_2n+1)，u_2n+1和v_2n+1的计算公式如下：

v_2n+1＝v_cr

记录路径点P_2β+1，并计算路径点P_2β+1与第二局部目标点P_g2之间的第二距离误差ΔS₂，计算公式如下：

ΔS₂＝||P_2β+1-P_g2||

若第二距离误差ΔS₂小于或等于阈值ΔS_min，进入步骤5.4；

若第二距离误差ΔS₂大于阈值ΔS_min，令P_2β+1＝P_k，P_2n+1＝P_L2，并返回步骤4.1.1：

步骤5.4，避碰队列设定

按照步骤5.3.1的方式，从避碰路径1的起点P₁₁开始，计算得到避碰路径1中N1个路径点的位置，并将N1个路径点组成避碰路径1的避碰队列[P₁₁，P₁₂，…，P_1α，P_1α+1，…，P_1N1]，其中避碰路径1的起点P₁₁为智能车辆在t₀时刻的位置；

按照步骤5.3.2的方式，从避碰路径2的起点P₂₁开始，计算得到避碰路径2中N2个路径点的位置，并将N2个路径点组成避碰路径2的避碰队列[P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]，其中避碰路径2的起点P₂₁为步骤4.1.2中第一局部目标点P_g1的位置；

将避碰路径1的避碰队列[P₁₁，P₁₂，…，P_1α，P_1α+1，…，P_1N1]和避碰路径2的避碰队列[P₂₁，P₂₂，…，P_2β，P_2β+1，…，P_2N2]合并，得到一个完整的避碰队列P，P＝[P₁₁，P₁₂，…，P_1α，P_1α+1，…，P_1N1，P₂₁，P₂₂，…，P_2β，，P_2β+1，…，P_2N2]；

步骤5.5，采用五次多项式对完整的避碰队列P中N个路径点进行拟合，得到五次多项式的系数和常数：包括五次多项式的五次项系数c₀、五次多项式的四次项系数c₁、五次多项式的三次项系数c₂、五次多项式的二次项系数c₃、五次多项式的一次项系数c₄和五次多项式的常数c₅；将智能车辆避碰路径中的任意一个点记为P_poly，P_poly在地球坐标系中坐标记为(u_poly，v_poly)，(u_poly，v_poly)满足下式：

2.根据权利要求1所述的一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法，其特征在于，所述智能车辆状态信息包括智能车辆的位置、智能车辆的纵向速度、智能车辆的纵向加速度、智能车辆的横向速度、智能车辆的航向角、智能车辆的长宽信息；智能车辆周边环境信息包括车道宽度、车道线类型、静态障碍物的位置、静态障碍物的长宽、动态障碍物的位置、动态障碍物的纵向速度、动态障碍物的纵向加速度、动态障碍物的长宽信息。

3.根据权利要求1所述的一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法，其特征在于，所述时间t按照以下公式确定：

其中，W_ego为障碍物的宽度，u_obs为智能车辆的横向速度。

4.根据权利要求1所述的一种基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法，其特征在于，所述最短距离SD按照以下方法确定：

在危险区域A_danger边界上取任意一点记为危险边界点P_s，画危险边界点P_s与障碍物几何中心点的连线，且将该连线与车辆坐标系x轴正方向之间的夹角记为危险夹角

危险夹角

的取值范围是

则危险边界点P_s的坐标为

最短距离SD的计算式如下：

Images