CN110466516B - 一种基于非线性规划的曲线道路自动车换道轨迹规划方法 - Google Patents

Abstract

本发明涉及智能车的轨迹规划领域，具体涉及一种基于非线性规划的曲线道路自动车换道轨迹规划方法。首先利用高精度地图、传感器和GPS设备采集曲线道路信息并实时拟合道路方程；其次将换道总时间等分，构造含有未知参数的换道轨迹多项式曲线方程；然后建立车辆模型，通过计算换道车辆与周边车辆的实时距离来检测是否可能发生碰撞；最后基于换道行为起始点和终点的已知状态以及驾驶规范，设置合理的目标函数和约束条件，将轨迹规划问题转化为非线性优化求解问题，采用序列二次规划算法(SQP)求出换道车辆在不同时间点的位置、速度和加速度，以此规划出高效且安全的换道轨迹。

Description

一种基于非线性规划的曲线道路自动车换道轨迹规划方法

技术领域

本发明涉及智能车的轨迹规划领域，具体涉及一种基于非线性规划的曲线道路自动车换道轨迹规划方法。

背景技术

智能驾驶是智能交通系统的重要组成部分，在降低交通延误、减少交通事故和提高道路通行能力等方面具有巨大潜在研究价值。智能微观交通流行为包括换道行为、跟驰行为、超车行为等，近些年有很多针对跟驰行为的研究，而换道行为因需兼顾车辆的横向和纵向控制，更具挑战性。

轨迹规划是智能驾驶车辆换道行为的基础，其涉及换道过程中车辆间的拥堵疏解、轨迹安全性、舒适性和可跟踪性等问题，因此规划出合适的换道轨迹对实现智能辅助驾驶具有重要意义。

现有研究中，换道轨迹规划领域仍存在一些技术问题：1)现有研究通常只是简单考虑直线道路上的换道行为，缺少对曲线道路换道轨迹规划的讨论，提出的模型不具有普适性；2)没有考虑换道结束时，车辆由换道行为切换至跟驰行为的加速度变化约束；3)对换道结束时的交通状态假定已知，不符合实际道路换道情况，过于理想化。

发明内容

针对上述技术问题，本发明提出了一种基于非线性规划的曲线道路自动车换道轨迹规划方法，在换道总时长和目标位置都未知的情况下，采用依赖于时间的多项式方程建立曲线道路自动车换道轨迹规划模型，以此保证轨迹的导函数(速度、加速度、加加速度)连续且光滑。该曲线道路自动车换道轨迹规划模型可以在不同曲线线型道路上实时规划出满足安全性、舒适性和高效性的换道轨迹，避免因人为驾驶失误造成的交通事故。

本发明是通过以下技术方案实现的：

一种基于非线性规划的曲线道路自动车换道轨迹规划方法，包括如下步骤：

(1)采集道路信息：利用高精度地图、传感器和GPS设备采集曲线道路信息，并基于道路位置点回归拟合出表示道路线型的多项式方程；

(2)表征换道轨迹曲线方程：将换道总时间等分为I个样本点，采用依赖于时间的六次多项式表征横向和纵向的换道轨迹，在换道行为起始点和终点部分交通状态已知的基础上，计算换道轨迹曲线多项式方程的未知参数；

(3)防碰撞检测：建立合适的车辆动力学模型，通过计算换道车辆与周边车辆的实时距离来检测是否可能发生碰撞；

(4)将轨迹规划问题转化为求解未知参数的非线性优化求解问题：考虑换道总时长、换道车辆速度和加速度对换道效率产生的影响，建立目标函数；考虑车辆跟驰行为、防碰撞检测以及驾驶规范，建立一系列约束条件；

(5)对非线性优化求解问题采用序列二次规划算法(SQP)，根据优化计算结果，选取出安全、高效且舒适的最优换道轨迹。

进一步地，步骤(1)中，采集道路信息是通过高精度地图、传感器和GPS设备得到曲线道路上一系列参考样本点的x,y轴坐标及相邻两个参考点之间的长度s，将数据离线存储；在轨迹规划时直接依据车辆换道初始位姿，回归拟合出表示道路线型的多项式方程，具体形式如下所示：

y(x)＝A₀+A₁x+A₂x²+A₃x³+A₄x⁴+A₅x⁵+A₆x⁶

x(s)＝B₀+B₁s+B₂s²+B₃s³+B₄s⁴+B₅s⁵+B₆s⁶

s(x)＝C₀+C₁x+C₂x²+C₃x³+C₄x⁴+C₅x⁵+C₆x⁶

其中，y(x)表示车道曲线的横纵坐标关系；x(s)表示以s为自变量的车道曲线长度与横坐标关系；s(x)表示以x为自变量的车道横坐标与车道曲线长度的关系，A₀,…,A₆、B₀,…,B₆、C₀,…,C₆表示换道轨迹曲线多项式方程的参数，为常数。

进一步地，步骤(2)表征换道轨迹曲线方程的过程中，将换道总时间等分为I个样本点，具体方法为：

平分换道总时长T，并用i＝0,1,…,I对样本时间点进行标号，初始时间点t₀＝0，换道完成时间点t_I＝T；t_i表示第i个时间点，t_i的具体表达式如下：

进一步地，步骤(2)中，换道轨迹曲线方程是基于时间变量的横向和纵向位姿方程，采用两个六次多项式来表示空间轨迹，以此来保证换道车辆在换道期间速度、加速度和加加速度连续且光滑的，具体形式如下：

x_0,i(t_i)＝β₀+β₁t_i+β₂t_i ²+β₃t_i ³+β₄t_i ⁴+β₅t_i ⁵+β₆t_i ⁶

y_0,i(t_i)＝α₀+α₁t_i+α₂t_i ²+α₃t_i ³+α₄t_i ⁴+α₅t_i ⁵+α₆t_i ⁶

其中，t_i表示第i个时间点，x_0,i(t_i)、y_0,i(t_i)分别表示换道车辆在不同样本时间点的纵向和横向坐标位置，α₀,…,α₆和β₀,…,β₆表示换道轨迹曲线方程的待求解参数。

进一步地，已知的换道行为起始点和终点交通状态为:

t₀表示换道初始时刻；

分别表示横向轨迹方程在从初始时刻的一阶导数和二阶导数；

分别表示纵向轨迹方程在从初始时刻的一阶导数和二阶导数；

x_0,0，y_0,0分别表示换道车辆初始时刻纵向和横向的位置；

分别表示换道车辆初始时刻纵向和横向的速度；

分别表示换道车辆初始时刻纵向和横向的加速度；

根据已知的换道行为起始点和终点交通状态，代入横向和纵向位姿方程x_0,i(t_i)、y_0,i(t_i)，计算多项式换道轨迹曲线方程中α₀,α₁,α₂和β₀,β₁,β₂的数值。

进一步地，步骤(3)的防碰撞检测具体过程为：

根据车辆动力学理论，用相互交叉的5个圆对机动车进行简单的建模，其中每个圆的直径为m，在换道过程中通过实时计算换道车辆0与周边车辆的距离来检测是否可能发生碰撞，包括：

(1)换道车辆0与本车道前车1的避免碰撞条件：

k表示相互交叉的5个圆的编号；i为样本时间点；

分别表示在不同时间样本点上包裹换道车辆0的五个圆圆心的横纵坐标；

分别表示在不同时间样本点上包裹本车道前车1的第一个圆圆心的横纵坐标，为避免碰撞，要求在换道过程中两辆车的欧式距离大于圆的直径m；

(2)换道车辆0与目标车道前车3及目标车道后车2的避免碰撞条件：

r²≤(x_0,i-x_2,i)²+(y_0,i-y_2,i)² i＝1,…,I

r²≤(x_0,i-x_3,i)²+(y_0,i-y_3,i)² i＝1,…,I

x_0,i,y_0,i分别表示换道车辆0中心点的横纵坐标；x_2,i,y_2,i分别表示换道目标车道后车2中心点的横纵坐标；x_3,i,y_3,i分别表示目标车道前车3中心点的横纵坐标，为避免碰撞，要求在换道过程中换道车辆0与目标车道前车3及目标车道后车2的欧式距离大于车辆的对角线r。

进一步地，步骤(4)，将轨迹规划问题转化为求解未知参数的非线性优化求解问题，具体为：

(1)构造换道目标函数J_min：

其中，

分别表示换道车辆0在不同时间点纵向和横向的加速度，

分别表示换道车辆0在不同时间点纵向和横向的加加速度；T表示换道总时长，j₂表示目标车道后车2的加加速度；ρ₀,…,ρ₅是目标函数中的权重参数；

(2)考虑换道车辆0的跟驰行为约束：

g_0,I＝c(v_0,I,v_3,I,Δs)

g_min≤g_2,I≤c(v_2,I,v_0,I,Δd)

g_0,I,g_2,I分别表示换道车辆0和目标车道后车2在换道结束状态时的瞬时加速度，v_0,I,v_2,I,v_3,I分别表示车辆0、车辆2和车辆3在时刻T的瞬时速度，Δs,Δd分别表示车辆0与车辆3、车辆2与车辆0在时刻T的距离；

为避免车辆突然从换道行为转变为跟驰行为时加速度变化太大，造成乘客不适性，考虑换道行为结束时，换道车辆0的瞬时加速度等于跟驰模型安全加速度；目标车道后车2的瞬时加速度大于最小加速度，小于等于跟驰模型安全加速度；

(3)考虑车辆驾驶规范准则约束：

分别表示换道车辆0在第i个时间点纵向和横向的速度；

分别表示换道车辆0在第i个时间点纵向和横向的加速度；

分别表示换道车辆0在第i个时间点纵向和横向的加加速度；

分别表示换道车辆0在换道过程中的纵向和横向速度的上限；

分别表示换道车辆0在换道过程中的纵向加速度的上下限；

分别表示换道车辆0在换道过程中的横向加速度的上下限；

分别表示换道车辆0在换道过程中的纵向加加速度的上下限；

分别表示换道车辆0在换道过程中的横向加加速度的上下限；

T_max表示换道车辆0在换道过程中换道总时长的上限。

进一步地，步骤(2)考虑换道车辆0的跟驰行为约束过程中：

g_0,I,v_0,I,v_3,I,Δs,v_2,I,Δd按如下公式计算得到：

计算换道车辆0在T时刻的加速度：

计算换道车辆0在T时刻的速度：

计算目标车道前车3在T时刻的速度：

计算车辆3与车辆0在T时刻的距离：

Δs＝s_3,I-s(x_0,I)

计算目标车道后车2在T时刻的速度：

计算车辆2与车辆0在T时刻的距离：

Δd＝s(x_0,I)-s_2,I

其中，

是轨迹规划过程中需要优化求解的变量，s_3,0，v_3,0,a_3,0,j_3,0分别表示初始时刻车辆3的道路位置、速度、加速度和加加速度，s(x_0,I)表示T时刻车辆0的道路位置，s_2,0,v_2,0,a_2,0分别表示初始时刻车辆2的道路位置、速度和加速度，j₂表示车辆2的在换道过程中的加加速度。

进一步地，步骤(2)考虑换道车辆0的跟驰行为约束过程中：跟驰模型安全加速度计算公式按如下：

c(v_0,I,v_3,I,Δs)＝κ{V₁+V₂tanh[C₁(Δs-s_c)-C₂]-v_0,I}+λ(v_3,I-v_0,I)

c(v_2,I,v_0,I,Δd)＝κ{V₁+V₂tanh[C₁(Δd-s_c)-C₂]-v_2,I}+λ(v_0,I-v_2,I)

其中，κ,λ,s_c,C₁,C₂,V₁,V₂为跟驰模型中预先设定的参数，为常量。

进一步地，步骤(5)中采用序列二次规划算法对建立的非线性规划模型进行优化求解，在满足约束条件基础下快速得出换道车辆在不同时间样本点上的位置、速度和加速度，由此规划出相应的最优换道轨迹。

本发明的有益技术效果在于：

本发明所述方法利用高精度地图离线存储好道路平面信息，在轨迹规划时直接依据车辆换道初始位姿回归拟合得到当前车道以及目标车道平面曲线的函数表达式，从而让轨迹规划模型在任意道路平面线型上都可以有效实施。

本发明所述方法在换道总时长和目标位置都未知的情况下，采用依赖于时间的多项式方程建立曲线道路自动车换道轨迹规划模型，以此保证轨迹的导函数(速度、加速度、加加速度)连续且光滑。并且，考虑当换道车辆到达目标位置时，车辆的交通状态会受前车的位置、速度影响，为了符合实际换道情形，假定换道结束时的交通状态未知，同时在设置约束条件时将跟驰行为融入至换道轨迹规划的非线性模型中，避免车辆行驶过程中由于加速度过大引起的不适感。通过设置合理的目标函数和约束条件，规划出高效、安全、舒适的换道轨迹。

本发明所述方法能应用于交通流微观仿真系统中，增加仿真的真实度，提高智能交通系统的效率。

附图说明

图1为本发明实施例中轨迹规划结构框图；

图2为本发明实施例中曲线道路示意图；

图3为本发明实施例中换道过程周边车辆相对位置示意图；

图4为本发明实施例中车辆模型示意图；

图5为本发明实施例中换道轨迹图；

图6a表示换道车辆在x轴方向速度与时间关系图；

图6b表示换道车辆在y轴方向速度与时间关系图；

图6c表示换道车辆在x轴方向加速度与时间关系图；

图6d表示换道车辆在y轴方向加速度与时间关系图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。

如图1所示，本发明实施例提供一种基于非线性规划的曲线道路自动车换道轨迹规划方法，包括：采集道路平面信息、表征换道轨迹曲线方程、将轨迹规划问题转化为求解未知参数的非线性优化求解问题、规划出最优换道轨迹。

步骤1，采集道路平面信息，利用高精度地图、传感器和GPS设备采集曲线道路信息，得出实施例中应用的曲线路段的平面样本点横纵坐标x、y以及相邻样本点之间的曲线长度s，利用切比雪夫插值法拟合出当前车道以及目标车道平面曲线的函数表达式，具体线性如图2所示，具体方程式如下所示：

(1)当前车道

y(x)_current＝47.2452+69.1670x+29.7104x²+6.6436x³+0.8775x⁴+0.1829x⁵-0.0180x⁶

x(s)_current＝460.7306+253.6940s-9.7353s²-3.5600s³-0.5740s⁴+0.1848s⁵+0.1410s⁶

s(x)_current＝459.5630+265.7837x+9.7107x²+4.4295x³+1.4385x⁴+0.2706x⁵-0.0268x⁶

(2)目标车道

y(x)_target＝51.6349+70.1708x+30.1549x²+6.8124x³+0.9559x⁴+0.2486x⁵-0.0566x⁶

x(s)_target＝460.3115+253.4286s-9.8285s²-3.5641s³-0.5819s⁴+0.1755s⁵+0.1440s⁶

s(x)_target＝460.0521+266.1802x+9.9108x²+4.4943x³+1.4737x⁴+0.2915x⁵-0.0202x⁶

图3为换道过程周边车辆相对位置示意图，换道车辆标号为车辆0，当前车道前车标号为车辆1，目标车道后车标号为车辆2，目标车道前车标号为车辆3，本发明默认换道过程中当前车道后车在获知车辆0换道意图后会主动避让，因此换道场景设置中不考虑当前车辆后车。

第二步，用变量T表示换道总时长，并用i＝0,1,…,I对样本时间点进行标号，其中t_i表示第i个时间点，该实施例选取中I＝20。本发明认为初始时间点t₀＝0，换道完成时间点t_I＝T。t_i的具体表达式如下：

基于上述时间点的设定，采用两个六次多项式来表示换道曲线轨迹，以此来保证车辆在换道期间的速度、加速度和加加速度是连续且光滑的，具体形式如下：

x_0,i(t_i)＝β₀+β₁t_i+β₂t_i ²+β₃t_i ³+β₄t_i ⁴+β₅t_i ⁵+β₆t_i ⁶

y_0,i(t_i)＝α₀+α₁t_i+α₂t_i ²+α₃t_i ³+α₄t_i ⁴+α₅t_i ⁵+α₆t_i ⁶

其中，α₀,…,α₆和β₀,…,β₆表示多项式轨迹方程的参数，x_0,i(t_i)、y_0,i(t_i)分别表示换道车辆在不同样本时间点的横纵坐标位置。

根据车载传感器探测出的换道车辆初始交通状态，此处，设定车辆0初始时刻交通状态分别为：y_0,0＝0,

x_0,0＝360,

可以计算出所述轨迹方程中α₀,α₁,α₂和β₀,β₁,β₂的数值：

α₀＝0,α₁＝0,α₂＝0,β₁＝360,β₁＝15,β₂＝0

第三步，对换道车辆0进行防碰撞检测，如图4所示，将车辆简化为由5个直径为m的圆(k＝0,1,…,4)交叉包裹的多边形，在换道过程中分别检测换道车辆与本车道前车和、目标车道前后车的距离是否可以避免碰撞。

具体约束条件如下所示：

分别表示在不同时间样本点上包裹换道车辆的五个圆的横纵坐标，

分别表示在不同时间样本点上包裹本车道前车1距离换道车辆0最近的圆的圆心横纵坐标，要求在换道过程中两辆车的实时距离大于圆的直径m＝3.6m。

r²≤(x_0,i-x_2,i)²+(y_0,i-y_2,i)² i＝1,…,20

r²≤(x_0,i-x_3,i)²+(y_0,i-y_3,i)² i＝1,…,20

x_0,i,y_0,i,x_2,i,y_2,i,x_3,i,y_3,i分别表示换道车辆0、目标车道后车2、目标车道前车3的车辆中心横纵坐标，要求在换道过程中两辆车的实时距离大于车辆的对角线r＝5.12m。

第四步，将换道轨迹规划问题转化为非线性优化问题，构造目标函数J_min：

其中

分别表示换道车辆0的纵向和横向的加速度和加加速度，T表示换道总时长，j₂表示目标车辆后车的加加速度。ρ₀,ρ₁,ρ₂,ρ₃,ρ₄,ρ₅是权重参数。

其次考虑防碰撞约束、跟驰约束、交通规范约束条件。

(1)防碰撞约束：

r²≤(x_0,i-x_2,i)²+(y_0,i-y_2,i)² i＝1,…,20

r²≤(x_0,i-x_3,i)²+(y_0,i-y_3,i)² i＝1,…,20

(2)跟驰行为约束：

g_0,I＝c(v_0,I,v_3,I,Δs)

g_min≤g_2,I≤c(v_2,I,v_0,I,Δd)

g_0,I,g_2,I分别表示换道车辆和目标车道后车2在换道结束状态时的瞬时加速度，为避免发生车辆从换道行为转变为跟驰行为，加速度变化太大，造成不适性，本发明认为换道车辆在I时刻的瞬时加速度等于与前车计算出的跟驰模型安全加速度；认为目标车道后车1在I时刻的瞬时加速度小于等于与前车计算出的跟驰模型安全加速度。

其中，跟驰模型计算公式如下所示：

c(v_0,I,v_3,I,Δs)＝κ{V₁+V₂tanh[C₁(Δs-s_c)-C₂]-v_0,I}+λ(v_3,I-v_0,I)

其中κ＝0.4,λ＝0.5,S_c＝4.8,C₁＝0.13,C₂＝1.57,V₁＝6.75,V₂＝7.91，v_0,I表示换道车辆在时刻T的速度，v_3,I表示目标车道前车在时刻T的速度，Δs表示目标车道前车与换道车辆在时刻T的距离差。

(3)交通规范准则约束：

0≤T≤10

上述约束条件表示在换道过程中换道车辆的横纵向速度、加速度、加加速度和换道总时长的上下限范围。

第五步，采用序列二次规划算法SQP对建立的非线性规划模型进行优化求解，在满足约束条件下得出换道车辆的速度、加速度、加加速度在不同时间样本点的最优数值,由此规划出相应的最优换道轨迹。

图5为该实施例中在曲线道路上规划出的换道轨迹图，从图中可以看出规划出的曲线轨迹过渡平滑、无尖点、无突变点。图6a-d分别为换道车辆x方向和y方向速度、加速度随时间变化趋势图，可以看出两个方向的交通状态变化范围都比较小，说明车辆跟踪该规划换道轨迹的舒适性。

综上所述，本发明提出了一种基于非线性的曲线道路自动车换道轨迹方法，在换道总时长和目标位置都未知的情况下，采用依赖于时间的多项式方程来表示换道车辆的时空轨迹，通过输入回归得到的路段平面信息，建立可应用于曲线道路场景的换道轨迹规划模型。

以上对本发明提供的一种基于非线性规划的曲线道路自动车换道轨迹规划方法进行了详细介绍，以上所述仅为本发明的一个具体实施例，仅用于说明本发明的设计思想和特点，并不用于限制本发明，凡在本发明技术思想下所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，包括如下步骤：

(1)采集道路信息：利用高精度地图、传感器和GPS设备采集曲线道路信息，并基于道路位置点回归拟合出表示道路线型的多项式方程；

(2)表征换道轨迹曲线方程：将换道总时间等分为I个样本点，采用依赖于时间的六次多项式表征横向和纵向的换道轨迹，在换道行为起始点和终点部分交通状态已知的基础上，计算换道轨迹曲线多项式方程的未知参数；

(3)防碰撞检测：建立车辆动力学模型，通过计算换道车辆与周边车辆的实时距离来检测是否可能发生碰撞；

(4)将轨迹规划问题转化为求解未知参数的非线性优化求解问题：考虑换道总时长、换道车辆速度和加速度对换道效率产生的影响，建立目标函数；考虑车辆跟驰行为、防碰撞检测以及驾驶规范，建立一系列约束条件；

(5)对非线性优化求解问题采用序列二次规划算法，根据优化计算结果，选取出安全、高效且舒适的最优换道轨迹。

2.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(1)中，采集道路信息是通过高精度地图、传感器和GPS设备得到曲线道路上一系列参考样本点的x，y轴坐标及相邻两个参考点之间的长度s，将数据离线存储；在轨迹规划时直接依据车辆换道初始位姿，回归拟合出表示道路线型的多项式方程，具体形式如下所示：

y(x)＝A₀+A₁x+A₂x²+A₃x³+A₄x⁴+A₅x⁵+A₆x⁶

x(s)＝B₀+B₁s+B₂s²+B₃s³+B₄s⁴+B₅s⁵+B₆s⁶

s(x)＝C₀+C₁x+C₂x²+C₃x³+C₄x⁴+C₅x⁵+C₆x⁶

其中，y(x)表示车道曲线的横纵坐标关系；x(s)表示以s为自变量的车道曲线长度与横坐标关系；s(x)表示以x为自变量的车道横坐标与车道曲线长度的关系，A₀，...，A₆、B₀，...，B₆、C₀，...，C₆表示车道曲线多项式方程的参数，为常数。

3.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)表征换道轨迹曲线方程的过程中，将换道总时间等分为I个样本点，具体方法为：

平分换道总时长T，并用i＝0，1，...，I对样本时间点进行标号，初始时间点t₀＝0，换道完成时间点t_I＝T；t_i表示第i个时间点，t_i的具体表达式如下：

4.根据权利要求3所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)中，换道轨迹曲线方程是基于时间变量的横向和纵向位姿方程，采用两个六次多项式来表示空间轨迹，以此来保证换道车辆在换道期间速度、加速度和加加速度连续且光滑的，具体形式如下：

x_0，i(t_i)＝β₀+β₁t_i+β₂t_i ²+β₃t_i ³+β₄t_i ⁴+β₅t_i ⁵+β₆t_i ⁶

y_0，i(t_i)＝α₀+α₁t_i+α₂t_i ²+α₃t_i ³+α₄t_i ⁴+α₅t_i ⁵+α₆t_i ⁶

其中，t_i表示第i个时间点，x_0，i(t_i)、y_0，i(t_i)分别表示换道车辆在不同样本时间点的纵向和横向坐标位置，α₀，...，α₆和β₀，...，β₆表示换道轨迹曲线方程的待求解参数。

5.根据权利要求4所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，已知的换道行为起始点和终点交通状态为：

y_0，i(t₀)＝y_0，0，

x_0，i(t₀)＝x_0，0，

t₀表示换道初始时刻；

分别表示横向轨迹方程在从初始时刻的一阶导数和二阶导数；

分别表示纵向轨迹方程在从初始时刻的一阶导数和二阶导数；

x_0，0，y_0，0分别表示换道车辆初始时刻纵向和横向的位置；

分别表示换道车辆初始时刻纵向和横向的速度；

分别表示换道车辆初始时刻纵向和横向的加速度；

根据已知的换道行为起始点和终点交通状态，代入横向和纵向位姿方程x_0，i(t_i)、y_0，i(t_i)，计算多项式换道轨迹曲线方程中α₀，α₁，α₂和β₀，β₁，β₂的数值。

6.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(3)的防碰撞检测具体过程为：

根据车辆动力学理论，用相互交叉的5个圆对机动车进行简单的建模，其中每个圆的直径为m，在换道过程中通过实时计算换道车辆0与周边车辆的距离来检测是否可能发生碰撞，包括：

(1)换道车辆0与本车道前车1的避免碰撞条件：

k＝0，...，4且i＝1，...，I

k表示相互交叉的5个圆的编号；i为样本时间点；

分别表示在不同时间样本点上包裹换道车辆0的五个圆圆心的横纵坐标；

分别表示在不同时间样本点上包裹本车道前车1的第一个圆圆心的横纵坐标，为避免碰撞，要求在换道过程中两辆车的欧式距离大于圆的直径m；

(2)换道车辆0与目标车道前车3及目标车道后车2的避免碰撞条件：

r²≤(x_0，i-x_2，i)²+(y_0，i-y_2，i)² i＝1，...，I

r²≤(x_0，i-x_3，i)²+(y_0，i-y_3，i)² i＝1，...，I

x_0，i，y_0，i分别表示换道车辆0中心点的横纵坐标；x_2，i，y_2，i分别表示换道目标车道后车2中心点的横纵坐标；x_3，i，y_3，i分别表示目标车道前车3中心点的横纵坐标，为避免碰撞，要求在换道过程中换道车辆0与目标车道前车3及目标车道后车2的欧式距离大于车辆的对角线r；所述车辆的对角线r是换道车辆0、目标车道后车2以及目标车道前车3中的车辆对角线最大的值。

7.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(4)，将轨迹规划问题转化为求解未知参数的非线性优化求解问题，具体为：

(1)构造换道目标函数J_min：

其中，

分别表示换道车辆0在不同时间点纵向和横向的加速度，

分别表示换道车辆0在不同时间点纵向和横向的加加速度；T表示换道总时长，j₂表示目标车道后车2的加加速度；ρ₀，...，ρ₅是目标函数中的权重参数；

(2)考虑换道车辆0的跟驰行为约束：

g_0，I＝c(v_0，I，v_3，I，Δs)

g_min≤g_2，I≤c(v_2，I，v_0，I，Δd)

g_0，I，g_2，I分别表示换道车辆0和目标车道后车2在换道结束状态时的瞬时加速度，v_0，I，v_2，I，v_3，I分别表示车辆0、车辆2和车辆3在时刻T的瞬时速度，Δs，Δd分别表示车辆0与车辆3、车辆2与车辆0在时刻T的距离；

为避免车辆突然从换道行为转变为跟驰行为时加速度变化太大，造成乘客不适性，考虑换道行为结束时，换道车辆0的瞬时加速度等于跟驰模型安全加速度；目标车道后车2的瞬时加速度大于最小加速度，小于等于跟驰模型安全加速度；

(3)考虑车辆驾驶规范准则约束：

分别表示换道车辆0在第i个时间点纵向和横向的速度；

分别表示换道车辆0在第i个时间点纵向和横向的加速度；

分别表示换道车辆0在第i个时间点纵向和横向的加加速度；

分别表示换道车辆0在换道过程中的纵向和横向速度的上限；

分别表示换道车辆0在换道过程中的纵向加速度的上下限；

分别表示换道车辆0在换道过程中的横向加速度的上下限；

分别表示换道车辆0在换道过程中的纵向加加速度的上下限；

分别表示换道车辆0在换道过程中的横向加加速度的上下限；

T_max表示换道车辆0在换道过程中换道总时长的上限。

8.根据权利要求7所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)考虑换道车辆0的跟驰行为约束过程中：

g_0，I，v_0，I，v_3，I，Δs，v_2，I，Δd按如下公式计算得到：

计算换道车辆0在T时刻的加速度：

计算换道车辆0在T时刻的速度：

计算目标车道前车3在T时刻的速度：

计算车辆3与车辆0在T时刻的距离：

Δs＝s_3，I-s(x_0，I)

计算目标车道后车2在T时刻的速度：

计算车辆2与车辆0在T时刻的距离：

Δd＝s(x_0，I)-s_2，I

其中，

是轨迹规划过程中需要优化求解的变量，s_3，0，v_3，0，a_3，0，j_3，0分别表示初始时刻车辆3的道路位置、速度、加速度和加加速度，s(x_0，I)表示T时刻车辆0的道路位置，s_2，0，v_2，0，a_2，0分别表示初始时刻车辆2的道路位置、速度和加速度，j₂表示车辆2的在换道过程中的加加速度。

9.根据权利要求8所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(2)考虑换道车辆0的跟驰行为约束过程中：跟驰模型安全加速度计算公式按如下：

c(v_0，I，v_3，I，Δs)＝κ{V₁+V₂tanh[C₁(Δs-s_c)-C₂]-v_0，I}+λ(v_3，I-v_0，I)

c(v_2，I，v_0，I，Δd)＝κ{V₁+V₂tanh[C₁(Δd-s_c)-C₂]-v_2，I}+λ(v_0，I-v_2，I)

其中，κ，λ，s_c，C₁，C₂，V₁，V₂为跟驰模型中预先设定的参数，为常量。

10.根据权利要求1所述一种基于非线性规划的曲线道路自动车换道轨迹规划方法，其特征在于，步骤(5)中采用序列二次规划算法对建立的非线性规划模型进行优化求解，在满足约束条件基础下快速得出换道车辆在不同时间样本点上的位置、速度和加速度，由此规划出相应的最优换道轨迹。

Images