CN117445923A - 基于可达性分析的网联车辆换道轨迹规划方法 - Google Patents

Abstract

本发明公开基于可达性分析的网联车辆换道轨迹规划方法，属于路径规划技术领域，包括建立车辆换道轨迹函数，给车辆换道轨迹函数增加约束条件，约束条件包括最小安全距离约束、安全性约束和舒适性约束，障碍物的可达性分析；进行车辆换道轨迹规划。本发明使用五次和六次的混合多项式以及换道临界碰撞模型，规划了满足安全性和舒适性约束的换道轨迹；考虑到前车的紧急变道所引发的安全问题，牺牲部分舒适性，规划了安全避让轨迹。降低了紧急情况下网联车的反应时间；结合搜索到的安全位置，求解出避让的轨迹函数，将两段轨迹拼接成一条轨迹，可以实现换道轨迹与避让轨迹的合理衔接。

Description

基于可达性分析的网联车辆换道轨迹规划方法

技术领域

本发明公开基于可达性分析的网联车辆换道轨迹规划方法，属于路径规划技术领域。

背景技术

换道是车辆行驶中最基本的一种驾驶行为，网联自主车辆CAV的主动换道需要感知***、轨迹规划以及轨迹跟踪控制等技术协同作用。换道轨迹规划首先满足车辆动力学与运动学约束以及换道安全性,完成从当前车道过渡到目标车道的任务。目前常见的换道轨迹规划方法主要有基于搜索的算法、基于采样的算法、曲线拟合算法、机器学习算法和人工势场法。现有技术对避障的研究仅限于单一被动的避障轨迹，缺乏对紧急变道问题提前规划避障轨迹的研究。

发明内容

本发明的目的在于提供基于可达性分析的网联车辆换道轨迹规划方法，以解决现有技术中，CAV车辆换道避障未考虑可达性规划的问题。

基于可达性分析的网联车辆换道轨迹规划方法，包括：

S1.建立车辆换道轨迹函数；

S2.给车辆换道轨迹函数增加约束条件，约束条件包括最小安全距离约束、安全性约束和舒适性约束；

S3.障碍物的可达性分析；

S4.进行车辆换道轨迹规划。

S1包括：

车辆换道轨迹函数为：

式中，x(t)，y(t)分别是车辆在换道过程中的纵向和横向位移；

A＝(a₆a₅ a₄ a₃ a₂ a₁ a₀)^T、B＝(b₅ b₄ b₃ b₂ b₁ b₀)^T是多项式的待定系数向量，t是时间。

最小安全距离约束包括：车辆M与原车道行驶的前车L0初始时刻最小安全距离MSS_M，L0为：

式中，α是换道车辆横摆角，t_c是临界碰撞时间，V_M、a_L0、a_M、a_L0是自车和前车的速度和加速度，L为车长，W为车宽，t₀是初始时间，τ是中间变量。

安全性约束包括：车辆的横向位移约束以及安全距离约束；

式中，y(t)是车辆的横向位移，H＝3.75是车道宽度，t₁是换道结束时间；

根据最小安全距离模型计算出的最小初始距离，加上车辆行驶时与周围车辆保持的最小纵向距离d₀，作为安全距离D_(M，L0)：

D_(M，L0)＞MSS_(M，L0)+d₀。

舒适性约束包括：

式中，a_x为纵向加速度，a_y为横向加速度，j_x为纵向加加速度，j_y为横向加加速度。

S2包括：

临界碰撞时刻为t_c，临界碰撞时刻CAV车辆的右前点的坐标为：

(X_m(t_c)-diga cos(θ+α)+L cosα，Y_m(tc)-diga sin(θ+α)+L sinα)；

式中，diga为CAV车辆中心点至CAV车辆的右前点的距离，X_m(t_c)为临界碰撞时刻CAV车辆中心点横坐标，θ为diga所在直线与x轴的锐角夹角，α为车辆横摆角速度，Y_m(t_c)为临界碰撞时刻CAV车辆中心点纵坐标；

临界碰撞时刻HV车辆左后点的坐标分为：

X_L0(t_c)为临界碰撞时刻HV车辆中心点横坐标；

换道过程中车辆横摆角速度α＜＜θ，α，考虑换道临界碰撞点两车碰撞处点的坐标相等，得到以下临界碰撞约束方程：

将临界碰撞约束方程和车辆换道轨迹函数联立，结合已知的换道初始时刻和换到结束时刻车辆状态求出换道横纵向轨迹函数。

S3包括：

动力***的模型定义为M＝(f，X₀，U)，f，X₀，U分别表示动力***、动力***状态和输入的边界约束，在t＝r时刻的可达集R(M，r)为：

式中，x(t)是状态，u(t)是输入，t是时间，f(x(t)，u(t))表示动力***，x(0)表示初始状态，x(0)∈X₀，

时间区间t∈[0，r]内的可达集是：

R＝(M，[0，r])＝∪_t∈[0，r]R(M，t)；

用proj(x)＝[x₁，x₂，x₃]^T表示车辆的占用集x₁和x₂表示车辆的横纵向位置，x₃表示车辆的方向，proj(R(M，t))＝{proj(x)|x∈R(M，t)}表示车辆位置和方向的集合，计算其他车辆可能的占用区域；CAV车辆的动力***的模型为M₀和M_i，其中M_i是模型M₀的过近似模型，M_i的占用集包含M₀的占用集，则表示为：

S3包括：

占用集用一个圆心为c(t)，半径为r(t)的圆代替，设纵向位移s_x＝0，横向位移s_y＝1，纵向速度v_x＝v以及横向速度v_y＝0，用曲线拟合每一步圆形占用集的区域边界得到占据边界的二维函数[b_x(t)，b_y(t)]^T，其中b_x(t)和b_y(t)分别是：

式中，v₀为初始时刻的速度，a_max为最大加速度。

S3包括：

车辆是长为L宽为W的矩形，占用率O₁用多边形q表示，占用区域各点的坐标为：

式中，q₁、q₂、q₃、q₄、q₅、q₆为用多边形q的顶点，c_x(t_k)，r(t_k)，c_y(t_k)，b_x(t_k)，c_y(t_k+1)，r(t_k+1)，c_x(t)、c_y(t)、r(t)表示占用集圆心横纵坐标和半径，t_k、t_k+1表示预测的时间步，b_x(t_k)，b_y(t_k)表示两步圆形占用集拟合出的横向边界坐标用p表示车辆和其附近所有车辆的区域，p₁、p₂、p₃、p₄、p₅、p₆表示p的顶点。

相对比现有技术，本发明具有以下有益效果：本发明使用五次和六次的混合多项式以及换道临界碰撞模型，规划了满足安全性和舒适性约束的换道轨迹；考虑到前车的紧急变道所引发的安全问题，使用基于集的预测工具SPOT搜索安全区域，牺牲部分舒适性，规划了安全避让轨迹。降低了紧急情况下网联车的反应时间；通过设计的仿真交通场景，使用换道轨迹函数求解出网联车换道3S时的各项参数。结合搜索到的安全位置，求解出避让的轨迹函数，将两段轨迹拼接成一条轨迹，可以实现换道轨迹与避让轨迹的合理衔接；换道轨迹规划方面的研究，尚未深入考虑关于轨迹跟踪控制对车辆实际轨迹的影响；在今后的研究中将进一步研究轨迹跟踪控制对车辆行驶轨迹的影响，以此构建更加安全可靠的换道规控模型。

附图说明

图1是本发明占用集图；

图2是不考虑车辆尺寸的占用集图；

图3是考虑车辆尺寸的占用集图；

图4是换道场景及换道轨迹图；

图5是纵向位移图；

图6是曲率图；

图7是纵向速度图；

图8是横向速度图；

图9是纵向加速度图；

图10是横向加速度图；

图11是纵向加加速图；

图12是横向加加速度图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

基于可达性分析的网联车辆换道轨迹规划方法，包括：

S1.建立车辆换道轨迹函数；

S2.给车辆换道轨迹函数增加约束条件，约束条件包括最小安全距离约束、安全性约束和舒适性约束；

S3.障碍物的可达性分析；

S4.进行车辆换道轨迹规划。

S1包括：

车辆换道轨迹函数为：

式中，x(t)，y(t)分别是车辆在换道过程中的纵向和横向位移；

A＝(a₆a₅ a₄ a₃ a₂ a₁ a₀)^T、B＝b₅ b₄ b₃ b₂ b₁ b₀)^T是多项式的待定系数向量，t是时间。

最小安全距离约束包括：车辆M与原车道行驶的前车L0初始时刻最小安全距离MSS_M，L0为：

式中，α是换道车辆横摆角，t_c是临界碰撞时间，V_M、V_L0、a_M、a_L0是自车和前车的速度和加速度，L为车长，W为车宽，t₀是初始时间，τ是中间变量。

安全性约束包括：车辆的横向位移约束以及安全距离约束；

式中，y(t)是车辆的横向位移，H＝3.75是车道宽度，t₁是换道结束时间；

根据最小安全距离模型计算出的最小初始距离，加上车辆行驶时与周围车辆保持的最小纵向距离d₀，作为安全距离D_(M，L0)：

D_(M，L0)＞MSS_(M，L0)+d₀。

舒适性约束包括：

式中，a_x为纵向加速度，a_y为横向加速度，j_x为纵向加加速度，j_y为横向加加速度。

S2包括：

临界碰撞时刻为t_c，临界碰撞时刻CAV车辆的右前点的坐标为：

(X_m(t_c)-diga cos(θ+α)+L cosα，Y_m(t_c)-diga sin(θ+α)+L sinα)；

式中，diga为CAV车辆中心点至CAV车辆的右前点的距离，X_m(t_c)为临界碰撞时刻CAV车辆中心点横坐标，θ为diga所在直线与x轴的锐角夹角，α为车辆横摆角速度，Y_m(t_c)为临界碰撞时刻CAV车辆中心点纵坐标；

临界碰撞时刻HV车辆左后点的坐标分为：

X_L0(t_c)为临界碰撞时刻HV车辆中心点横坐标；

换道过程中车辆横摆角速度α＜＜θ，α，考虑换道临界碰撞点两车碰撞处点的坐标相等，得到以下临界碰撞约束方程：

将临界碰撞约束方程和车辆换道轨迹函数联立，结合已知的换道初始时刻和换到结束时刻车辆状态求出换道横纵向轨迹函数。

S3包括：

动力***的模型定义为M＝(f，X₀，U)，f，X₀，U分别表示动力***、动力***状态和输入的边界约束，在t＝r时刻的可达集R(M，r)为：

式中，x(t)是状态，u(t)是输入，t是时间，f(x(t)，u(t))表示动力***，x(0)表示初始状态，x(0)∈X₀，时间区间t∈[0，r]内的可达集是：

R＝(M，[0，r])＝∪_t∈[0，r]R(M，t)；

用proj(x)＝[x₁，x₂，x₃]^T表示车辆的占用集x₁和x₂表示车辆的横纵向位置，x₃表示车辆的方向，proj(R(M，t))＝{proj(x)|x∈R(M，t)}表示车辆位置和方向的集合，计算其他车辆可能的占用区域；CAV车辆的动力***的模型为M₀和M_i，其中M_i是模型M₀的过近似模型，M_i的占用集包含M₀的占用集，则表示为：

S3包括：

占用集用一个圆心为c(t)，半径为r(t)的圆代替，设纵向位移s_x＝0，横向位移s_y＝0，纵向速度v_x＝v以及横向速度v_y＝0，用曲线拟合每一步圆形占用集的区域边界得到占据边界的二维函数[b_x(t)，b_y(t)]^T，其中b_x(t)和b_y(t)分别是：

式中，v₀为初始时刻的速度，a_max为最大加速度。

S3包括：

车辆是长为L宽为W的矩形，占用率O₁用多边形q表示，占用区域各点的坐标为：

式中，q₁、q₂、q₃、q₄、q₅、q₆为用多边形q的顶点，c_x(t_k)，r(t_k)，c_y(t_k)，b_x(t_k)，c_y(t_k+1)，r(t_k+1)，c_x(t_k)、c_y(t_k)、r(t)表示占用集圆心横纵坐标和半径，t_k、t_k+1表示预测的时间步，b_x(t_k)，b_y(t_k)表示两步圆形占用集拟合出的横向边界坐标用p表示车辆和其附近所有车辆的区域，p₁、p₂、p₃、p₄、p₅、p₆表示p的顶点。

考虑换道轨迹本身的安全性、舒适性以及换道效率基础上，进一步考虑前车向目标车道紧急变道。通过可达性分析***出前车可能会占用的道路区域，这种情况可以分为三种，第一种是前车变道后，两车的车间距足够自车回到原车道；第二种是在两车间据不满足换道要求，可以通过减速让行甚至刹停来规避前车；第三种是车间距不满足减速避让的要求，这时就要通过转向加减速的策略来实现避让。轨迹规划流程如图1所示，规划换道轨迹时提前搜索避障安全区域。这样在紧急情况下，可以降低网联车的反应时间，采取最适合的策略来处理交通安全问题。

现有技术中，双车道的交通场景下，假设只有一辆障碍车，网联车辆换道时，前车紧急变道时两车间距较小，网联车选择转向加刹车的策略规避前车。多项式规划换道轨迹，用可达性分析搜索到安全区域，再用多项式规划的安全避让轨迹，本发明是使用前车紧急变道时车间距满足网联车回到原车道，使用多项式再规划出一条换道轨迹。就可以在保证安全的前提下保持道路畅通。

建立换道轨迹的车辆模型，可以准确的模拟换道过程，还可以简化计算过程。目前比较常用的车辆模型有椭圆车辆模型、动态圆车辆模型以及矩形车辆模型。其中矩形模型与实际车辆相似度最高，反应车辆运动的误差最小。

车辆在换道时一般会与周围车辆发生追尾、角碰、侧碰等三种的碰撞事故。建立最小安全距离模型MSS，同时根据最小安全距离模型建立换道约束，可以确保换道过程中的安全。

网联车与原车道前车发生碰撞的临界点是，CAV车辆换道时左前点与前车的左后点发生碰撞。MSS_M，L0是CAV车辆与前车碰撞的临界最小安全距离，X_M是在碰撞发生时CAV车辆的纵向位移，X_L0是碰撞发生前HV车辆的纵向位移。

CAV车辆在驶出原车道时自车左前点与前车左后点发生角碰撞，假设该临界时刻为t_c。该临界时刻CAV车辆左前点的横向位移为：Y(t_c)＝W cosα。

为了实现网联车辆的自主换道，首先需要规划出一条符合车辆动力学及运动学约束的换道轨迹。由于等速偏移模型和Dubins曲线模型在换道过程中运动方向发生突变不满足曲率连续的要求，而正余弦换道模型和贝塞尔曲线模型在换道的起点和终点存在极大值，导致车辆行驶至并道点后方向盘并未归正，车辆行驶会偏离车道中心线。多项式模型则没有这些缺点，所以选择多项式模型来构建换道轨迹和安全轨迹。

考虑到换道过程中纵向位移远大于横向位移，因此为了提高纵向方向上换道轨迹的精确性和连续性，增加纵向位移多项式的次数，来描述复杂动态交通场景下的车辆换道轨迹。

将时间矩阵记为T_A、T_B：

到换道起点和终点的横纵向方程的矩阵表达式：

该模型一共有13个未知量。并且在起点t＝t₀和终点t＝t₁的位置、速度、加速度已知，在t₀和t₁时刻的横向和纵向一共可以得到12个方程，并不能解出该方程，需要建立临界约束条件，联立求解。

基于轨迹函数和最小安全距离模型，车辆换道过程中的运动学和动力学约束。需要考虑到道路带来的约束，为防止车辆发生侧翻，需要限制最大速度、加速度。最后为满足舒适性的要求，需要限制最大加加速度。

汽车在行驶过程中，最基本的约束就是车道线和车道边界以及安全碰撞约束：

为了保证换道过程中乘客的舒适性，规划的换道轨迹横纵向的加速度以及加加速度要在一个合理的范围内，并且加加速度对乘客的舒适性影响更大。最大横向加速度为2m/s²，最大纵向加速度为2.5m/s²，最大横向加加速度为10m/s²。以最短换道时间为衡量换道效率指标，通常情况下车辆换道时间越短，对原车道和目标车道上游交通流的延误越少，引发周围车辆拥堵的可能性越小，换道效率越高。一般换道时间在2s到6s之间。无人驾驶车辆换道从开始到结束，与前车可能发生角碰撞，只要在某一时刻不发生角碰撞，那么整个换道过程将不会发生碰撞。

网联车拥有优越的计算能力，并且不会犯随机错误，这一点在预测和防止碰撞方面的能力远超人类驾驶员。从这一观点出发引发了许多关于安全性验证的讨论，例如使用可达性分析规划或验证安全轨迹，其原则是当且仅当可以证明轨迹与任何动态或静态障碍物可能占用的时空区域不相交时轨迹就是安全的。从这一点出发开发了基于集的预测工具SPOT。该程序运用可达性分析的方法，计算出场景中车辆未来的占用集。

从SPOT工具预测的前车占用集中搜索得到安全区域，进而得到安全轨迹最终的安全位置。基于可达性分析工具SPOT设计的交通场景。并用MATLAB获取了该场景下的换道轨迹和安全轨迹数据，设计的换道状态向量如表1和表2所示。

假设前车初始时刻在自车前方10m处，以7.0m/s(25.2km/h)的速度匀速行驶，设置车辆换道时间为4.0s，根据临界碰撞模型式求出临界碰撞时间t_c＝1.76S。在t＝3s时前车向目标车道紧急变道，根据交通参与者占用集预测搜索到的安全区域，选择(x,y)＝(62,3.0)作为安全位置。

表1换道初始时刻状态值

轴向	位移X/m	速度V/ms-1	加速度a/ms-2
				X	0.0	10.0	0.0
Y	0.0	0.0	0.0

。

表2换道结束时刻状态值

轴向	位移X/m	速度V/ms-1	加速度a/ms-2
				X	45.0	10.0	0.0
Y	1.875	0.0	0.0

。

最终换道轨迹函数和安全轨迹函数为：

本发明，占用集如图1所示，不考虑车辆尺寸的占用集如图2所示，考虑车辆尺寸的占用集如图3所示。建立的场景以及换道过程中的换道场景及换道轨迹如图4，纵向位移如图5，曲率如图6，纵向速度如图7，横向速度如图8，纵向加速度如图9，横向加速度如图10，纵向加加速如图11，横向加加速度如图12。实线是换道轨迹，虚线是安全避让轨迹。从图中可以看出规划的换道轨迹曲线连续平滑，曲率没有发生突变。换道初始时刻和终止时刻横向速度、加速度、纵向速度、加速度均为0，与实际情况相符。横纵向加速度、加加速度变化范围满足舒适性要求。横纵向速度满足安全性要求。安全避让轨迹的横纵向速度、加速度也满足安全性要求，并且符合实际情况。由图可知在规定时间内车辆能够到达轨迹规划指定的位置。综上所述，规划的换道轨迹和安全轨迹能够实现安全高效的换道避障。

以上实施例仅用于说明本发明的技术方案，而非对其限制，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换，而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (9)

1.基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，包括：

S1.建立车辆换道轨迹函数；

S2.给车辆换道轨迹函数增加约束条件，约束条件包括最小安全距离约束、安全性约束和舒适性约束；

S3.障碍物的可达性分析；

S4.进行车辆换道轨迹规划。

2.根据权利要求1所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，S1包括：

车辆换道轨迹函数为：

式中，x(t)，y(t)分别是车辆在换道过程中的纵向和横向位移；

A＝(a₆a₅ a₄ a₃ a₂ a₁ a₀)^T、B＝(b₅ b₄ b₃ b₂ b₁ b₀)^T是多项式的待定系数向量，t是时间。

3.根据权利要求2所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，最小安全距离约束包括：车辆M与原车道行驶的前车L0初始时刻最小安全距离MSS_M，L0为：

式中，α是换道车辆横摆角，t_c是临界碰撞时间，V_M、V_L0、a_M、a_L0是自车和前车的速度和加速度，L为车长，W为车宽，t₀是初始时间，τ是中间变量。

4.根据权利要求3所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，安全性约束包括：车辆的横向位移约束以及安全距离约束；

式中，y(t)是车辆的横向位移，H＝3.75是车道宽度，t₁是换道结束时间；

根据最小安全距离模型计算出的最小初始距离，加上车辆行驶时与周围车辆保持的最小纵向距离d₀，作为安全距离D_(M，L0)：

D_(M,L0)＞MSS_(M,L0)+d₀。

5.根据权利要求4所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，舒适性约束包括：

式中，a_x为纵向加速度，a_y为横向加速度，j_x为纵向加加速度，j_y为横向加加速度。

6.根据权利要求5所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，S2包括：

临界碰撞时刻为t_c，临界碰撞时刻CAV车辆的右前点的坐标为：

(X_m(t_c)-diga cos(θ+α)+L cosα，Y_m(t_c)-diga sin(θ+α)+L sinα)；

式中，diga为CAV车辆中心点至CAV车辆的右前点的距离，X_m(t_c)为临界碰撞时刻CAV车辆中心点横坐标，θ为diga所在直线与x轴的锐角夹角，α为车辆横摆角速度，Y_m(t_c)为临界碰撞时刻CAV车辆中心点纵坐标；

临界碰撞时刻HV车辆左后点的坐标分为：

X_L0(t_c)为临界碰撞时刻HV车辆中心点横坐标；

换道过程中车辆横摆角速度α＜＜θ，α，考虑换道临界碰撞点两车碰撞处点的坐标相等，得到以下临界碰撞约束方程：

将临界碰撞约束方程和车辆换道轨迹函数联立，结合已知的换道初始时刻和换到结束时刻车辆状态求出换道横纵向轨迹函数。

7.根据权利要求6所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，S3包括：

动力***的模型定义为M＝(f，X₀，U)，f，X₀，U分别表示动力***、动力***状态和输入的边界约束，在t＝r时刻的可达集R(M，r)为：

式中，x(t)是状态，u(t)是输入，t是时间，f(x(t)，u(t))表示动力***，x(0)表示初始状态，

时间区间t∈[0，r]内的可达集是：

R＝(M，[0，r])＝U_t∈[0，r]R(M，t)；

用proj(x)＝[x₁，x₂，x₃]^T表示车辆的占用集x₁和x₂表示车辆的横纵向位置，x₃表示车辆的方向，proj(R(M，t))＝{proj(x)|x∈R(M，t)}表示车辆位置和方向的集合，计算其他车辆可能的占用区域；CAV车辆的动力***的模型为M₀和M_i，其中M_i是模型M₀的过近似模型，M_i的占用集包含M₀的占用集，则表示为：

8.根据权利要求7所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，S3包括：

占用集用一个圆心为c(t)，半径为r(t)的圆代替，设纵向位移s_x＝0，横向位移s_y＝0，纵向速度v_x＝v以及横向速度v_y＝0，用曲线拟合每一步圆形占用集的区域边界得到占据边界的二维函数[b_x(t)，b_y(t)]^T，其中b_x(t)和b_y(t)分别是：

式中，v₀为初始时刻的速度，a_max为最大加速度。

9.根据权利要求8所述的基于可达性分析的网联车辆换道轨迹规划方法，其特征在于，S3包括：

车辆是长为L宽为W的矩形，占用率O₁用多边形q表示，占用区域各点的坐标为：

式中，q₁、q₂、q₃、q₄、q₅、q₆为用多边形q的顶点，c_x(t_k)，r(t_k)，c_y(t_k)，b_x(t_k)，c_y(t_k+1)，r(t_k+1)，c_x(t)、c_y(t)、r(t)表示占用集圆心横纵坐标和半径，t_k、t_k+1表示预测的时间步，b_x(t_k)，b_y(t_k)表示两步圆形占用集拟合出的横向边界坐标用p表示车辆和其附近所有车辆的区域，p₁、p₂、p₃、p₄、p₅、p₆表示p的顶点。