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

权利要求书

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)满足下式：