CN112947469A - 汽车换道轨迹规划与动态轨迹跟踪控制方法 - Google Patents

Abstract

一种汽车换道轨迹规划与动态轨迹跟踪控制方法，包括以下步骤：S1、规划直线道路或弯曲道路条件下的换道轨迹；S2、设计动态轨迹跟踪控制器跟踪参考轨迹，且构建参考轨迹引力函数和障碍车斥力函数，规避动态障碍物。本发明首先提供了结构化道路上的换道参考轨迹规划方法，基于五次多项式进行直线道路或弯曲道路条件下换道参考轨迹规划；然后考虑在换道过程中前车状态变化情况下车辆的避撞问题，在跟踪换道参考轨迹过程中考虑规避前方障碍车辆，基于模型预测算法设计防避撞轨迹跟踪控制器，并分别设计换道参考轨迹引力函数以及障碍车辆斥力函数作为模型预测的优化函数，通过优化算法获得最优控制参量，实现无碰撞轨迹跟踪目的。

Description

汽车换道轨迹规划与动态轨迹跟踪控制方法

技术领域

本发明汽车控制方法的技术领域，尤其涉及一种汽车换道轨迹规划与动态轨迹跟踪控制方法。

背景技术

汽车主动换道轨迹规划与跟踪控制是实现汽车主动换道的关键技术，也是高级辅助驾驶系统的重要功能。其中换道轨迹规划的目的是根据行车环境得到曲率连续的换道轨迹，满足车辆换道效率、安全和乘坐舒适性要求，同时受到车辆运动学和动力学约束。轨迹跟踪控制的技术要点是设计控制规律对已规划的换道轨迹进行实时跟踪控制。通过结合轨迹规划与跟踪控制实现汽车安全、高效、舒适地换道目的。

汽车主动换道轨迹规划包括直线道路条件下的轨迹规划和弯曲道路条件下的轨迹规划。轨迹规划方法的研究在传统跟驰模型的基础上取得了蓬勃的发展，现在常用的智能汽车换道轨迹规划模型有三角函数曲线、样条曲线、贝塞尔曲线和多项式曲线等。相较于其它模型，多项式曲线具有形式简单、计算速度快、曲率连续等优点，得到广泛应用。

结构化道路行驶条件下，交通参与车辆的行驶工况较为复杂多变。虽然基于传统多项式的换道轨迹具有诸多优点，然而其参数的获取是在换道初始时刻计算得到，对前车状态变化考虑存在不足。在这种情况下，跟踪已经得到的换道轨迹存在不确定的碰撞风险。

发明内容

针对背景技术中的不足，本发明基于多项式的换道参考轨迹，考虑前方障碍车辆影响，针对性地提出动态轨迹跟踪控制方案，保证不确定条件下的换道安全，为此，本发明提出了一种汽车换道轨迹规划与动态轨迹跟踪控制方法，具体方案如下：

一种汽车换道轨迹规划与动态轨迹跟踪控制方法，包括以下步骤：

S1、规划直线道路或弯曲道路条件下的换道轨迹；

S2、设计动态轨迹跟踪控制器跟踪参考轨迹，且构建参考轨迹引力函数和障碍车斥力函数，规避动态障碍物。

本发明的有益效果在于：

(1)本发明提出的一种汽车换道轨迹规划与动态轨迹跟踪控制方法：首先提供了结构化道路上的换道参考轨迹规划方法，基于五次多项式进行直线道路或弯曲道路条件下换道参考轨迹规划；然后考虑在换道过程中前车状态变化情况下车辆的避撞问题，在跟踪换道参考轨迹过程中考虑规避前方障碍车辆，基于模型预测算法设计避撞轨迹跟踪控制器，并分别设计换道参考轨迹引力函数以及障碍车辆斥力函数作为模型预测的优化函数，通过优化算法获得最优控制参量，实现无碰撞轨迹跟踪目的。

(2)构建前车势场函数的目的是描述被控车辆受到的前方障碍车辆的影响，让被控车辆与障碍车辆之间根据速度保持一定的安全距离。对前方障碍车辆的势场函数构建考虑到以下两点：

(a)汽车正常行驶时，纵向速度远大于横向速度，所以建立的前车势场函数在纵向上的作用距离大于横向上作用距离。

(b)由于换道过程中前车保持匀速或加速时，对沿换道参考轨迹行驶的安全性不产生影响；前车减速时，仍沿换道参考轨迹行驶则有可能与前车发生碰撞。所以构建的障碍车辆势场函数，应使得在前方障碍车匀速或加速时不影响被控车辆跟踪换道参考轨迹；当前方障碍车辆减速时，被控车辆能够及时做出调整，进行规避。所以构建的障碍车辆势场函数作用边界在换道开始时，尽量与换道参考轨迹相切。

附图说明

图1为本发明一种汽车换道轨迹规划与动态轨迹跟踪控制方法的流程图。

图2(a)为本发明直线轨迹规划示意图。

图2(b)为本发明弯道轨迹规划示意图。

图3为本发明防碰撞轨迹跟踪控制器架构图。

图4为本发明换道参考轨迹引力势场示意图。

图5为本发明障碍车斥力势场示意图。

具体实施方式

参照图1，本发明提出一种汽车换道轨迹规划与动态轨迹跟踪控制方法，根据道路包括以下步骤：

S1、规划直线道路或弯曲道路条件下的换道轨迹；具体步骤如下：

S11、通过雷达或相机等传感器获得前车距离和车速信息，在进行换道参考轨迹规划时假设前车保持匀速，为了避免换道过程中与前车发生碰撞，确定被控车辆开始换道时与前车之间的最小纵向安全距离S_safe，其公式为：

S_safe＝max{S_f，ε_v}

其中，ε_v为道路法规规定的行车间距，S_f为换道时间内自车行驶的纵向距离；为了保证换道过程中的安全性，同时考虑弱化换道过程中对车内成员造成的危险感，换道结束时在纵向上与原车道前方障碍车辆仍留有一定的安全裕度值D₃，S_f＝(v-v_obs)T₁+D₃；T₁为换道时间，D₃取定值，在该方案中D₃＝2m，v表示自车车速，v_obs表示前方障碍车辆车速；

S12、根据道路曲率判断是直线道路还是曲线道路，当道路是直线道路，跳至步骤S131，当道路是曲线道路，跳至步骤S141；

如图2(a)所示，规划直线道路上的换道轨迹步骤如下：

S131、轨迹规划起始时刻t＝0，换道终止时间T₁，即换道时间为T₁。以汽车换道起始位置车辆质心为原点，汽车行驶方向为x轴，构建平面直角坐标系。假设换道过程中车速不变，则换道的轨迹完全由汽车横向位移决定。采用五次多项式来描述横向位移轨迹y_ref(t)，其公式表示为：

y_ref(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

S132、根据换道起始时行车状态和换道终止时刻目标状态，沿y轴的行车状态为：

其中：H为车道宽度；T₁为换道时间，根据上述公式，得到直线道路条件下多项式轨迹的系数：

a₀＝a₁＝a₂＝0，

如图2(b)所示，规划曲线道路上的换道轨迹步骤如下：

S141、由于结构化道路的曲率较小，设定当前车道和目标车道中心线曲率相等，且当前车道中心线方程已知：Y＝f(x)；以换道起始时刻车辆质心为原点，汽车行驶方向为x轴。在弯曲道路上汽车换道时其质心运动可以分解为从质心向瞬心的直线运动和绕质心的圆弧运动。换道时，沿车道中心相切的位移为：

换道时汽车保持匀速，即l＝vt，得到：

S142、向心运动采用与直线换道相同的轨迹形式，即：

将g(x)代入G(t)即可得：

弯曲道路条件下换道轨迹为：

其中，θ为完成换道时车辆的位移角。

S2、根据道路条件，为跟踪换道参考轨迹同时考虑前车影响，基于MPC设计动态轨迹跟踪控制器，实现防碰撞轨迹跟踪控制。如图3所示，在每次采样后，获取自车和前车行驶状态信息，预测在预测时域内前车位置，并构建其障碍势场函数；同时结合换道参考轨迹，构建参考轨迹引力势场函数；将直线道路上或曲线道路上的换道参考轨迹作为轨迹跟踪控制器的参考输入，预测不同控制输入条件下自车位置，并将预测状态代入势场函数，以此构建优化函数；通过求解带约束优化问题求取最优控制参量，实现无碰撞轨迹跟踪控制。

详细地说，是将直线道路上或曲线道路上的换道参考轨迹作为轨迹跟踪控制器的参考输入，结合自车和障碍车辆的行驶状态以及车辆动力学约束，基于模型预测控制算法设计动态轨迹跟踪控制器。

步骤S2具体包括以下步骤：

S21、搭建模型预测所需的动力学模型和运动微分公式：

其中，C_f为前轮轮胎侧偏刚度之和；C_r为后轮轮胎侧偏刚度之和；l_f、l_r分别为质心到前轴、后轴距离，I_z为汽车绕z轴转动惯量，δ_f为前轮转角，v_x为车辆纵向车速，v_y为车辆横向速度，ω为车辆横摆角速度，

为车辆横摆角加速度，

为车辆横向加速度，m为整车质量；

地面坐标系下车辆运动学方程：

其中，

为车辆在x轴方向的速度，

为车辆在y轴方向的速度，

为车辆横摆角速度。

使用上述模型，可以提高模型预测控制算法实时性。本发明采用线性二自由度汽车动力学模型和地面坐标系下车辆运动学微分方程为基础，设计出的模型能够预测控制算法。

S22、由于车辆运动学微分方程是非线性的，进行非线性预测控制算法设计时很难在线实时求解，所以在当前状态点对非线性系统进行近似线性处理。以

为系统状态量，以前轮转角为控制量u＝δ_f，系统输出量为：Y＝[x，y]^T，可得系统状态微分方程：

对上述非线性系统线性化处理，在当前状态点(X₀,u₀)进行泰勒展开并保留一阶项，整理后得到线性处理后得到线性系统：

S23、对步骤S22中得到的线性系统进行离散化采用离散化周期为T_s，得到离散化系统状态方程：

S24、根据步骤S23中得到的离散化系统状态方程设计模型预测控制器，采用前轮转角增量代替原来的前轮转角值作为系统控制输入，并对系统状态量进行如下转换：

转换后新的系统控制输入为：

Δu(k|t)＝u(k|t)-u(k-1|t)

得到新的离散线性化状态方程：

其中，η(k|t)＝Y(k|t)；

如果控制器预测时域N_p,控制时域N_c，且N_c≤N_p。设定在控制时域外控制增量为零，则在预测时域内系统输出：

Y_p＝A_pζ(k|t)+B_pΔU+D_p

其中，

在预测时域内的输出量可以通过系统当前的状态量ζ(k|t)和控制时域内的控制增量ΔU计算得到，实现了模型预测算法的“预测”功能。为了获得控制时域内最优的控制增量ΔU序列，根据预测输出结果和期望值的偏差构建优化函数进行优化算法求解。

S25、系统控制增量是未知的，通过设定适当的优化目标函数，对其进行优化求解，才能得到控制时域内的控制序列，并将控制序列中第一个元素作为实际控制增量作用于系统，实现车辆横向控制。换道参考轨迹的生成，是在假设前方障碍车辆保持匀速基础上进行的。如果在实际的换道过程中，前方障碍车辆保持匀速或者前车加速，被控车辆跟踪规划好的轨迹仍能够保证不与前车发生碰撞；如果前车减速，被控车辆仍然跟随参考轨迹，则存在与前车发生碰撞的风险。为了解决上述问题，本发明以换道参考轨迹为引力场中心，建立引力场函数；被控车辆实时监测前方及周围障碍车辆的动态，对前车构建斥力场函数。所建参考轨迹引力场函数如图4所示，构建的参考轨迹引力函数J_gra和障碍车斥力函数J_rep(t)分别为：

假设在预测时域T_p内，前方障碍车辆的行驶方向和车速不变，以此来预测时间T_p内障碍车位置：

结合以上两点要求以及前车预测位置，以障碍车尾部中心(x_obs,y_obs)为椭圆圆心，构建障碍车斥力场函数作用边界为：

如图5所示，在障碍车斥力场函数作用边界内，所建障碍车斥力场场函数J_rep(t)其公式为：

为了限制控制量的大幅变化，设置控制增量变化代价函数J_ctr(t)：

基于模型预测控制算法预测在当前状态不同控制输入下被控车辆未来有限时域内的位置，并以预测位置处势场函数值和控制增量平方和为优化目标函数J：

T_pT_pT_pT_p基于模型预测控制算法预测在当前状态不同控制输入下被控车辆未来有限时域内的位置，并以预测位置处势场函数值和控制增量平方和为优化目标函数：

S26、首先考虑对控制量和控制增量的约束，控制量即前轮转角u(t)应被约束在机械结构所允许的最大范围之内：

u_min≤u(t)≤u_max

其中，u_min为最小控制量增量，u_max为最大控制量增量；

控制增量Δu(t)满足以下约束：

Δu_min≤Δu(t)≤Δu_max

其中，Δu_min为最小控制量增量，Δu_max为最大控制量增量；

由于在模型预测中采用了二自由度动力学模型，所以在换道过程中应该满足二自由度动力学模型的假设条件，即轮胎侧偏角较小，处于线性区域内，所以对侧向加速度a_y(t)进行约束，其中a_ymin为最小侧向加速度，a_ymax为最大侧向加速度。

a_ymin≤a_y(t)≤a_ymax

为保证换道过程中被控车辆不驶出目标车道边界，车辆横向位移y(t)应满足：

其中，D为被控车辆宽度，η为安全裕值。

通过求解带约束优化目标函数，即可得到既能较好跟踪换道全局参考轨迹，又能有效规避动态障碍物的控制输入，实现动态轨迹跟踪的功能。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (6)

1.一种汽车换道轨迹规划与动态轨迹跟踪控制方法，其特征在于，包括以下步骤：

S1、规划直线道路或弯曲道路条件下的换道轨迹；

S2、设计动态轨迹跟踪控制器跟踪参考轨迹，且构建参考轨迹引力函数和障碍车斥力函数，规避动态障碍物。

2.根据权利要求1所述的汽车换道轨迹规划与动态轨迹跟踪控制方法，其特征在于，步骤S1的具体步骤如下：

S11、根据自车与前车的距离确定换道起始条件，即当两车车距减小到最小纵向安全距离S_safe后开始换道；

S12、规划出直线道路上的换道轨迹和弯曲道路条件下的换道轨迹。

3.根据权利要求2所述的汽车换道轨迹规划与动态轨迹跟踪控制方法，其特征在于，所述步骤S11具体为：

确定被控车辆开始换道时与前车之间的最小纵向安全距离S_safe，其公式为：

S_safe＝max{S_f，ε_v}

其中，ε_v为道路法规规定的行车间距；S_f为换道时间内自车行驶的纵向距离，S_f＝(v-v_obs)T₁+D₃；T₁为换道时间，D₃为换道结束时在纵向上与原车道前方障碍车辆仍留有一定的安全裕度值，v表示自车车速，v_obs表示前方障碍车辆车速。

4.根据权利要求2所述的汽车换道轨迹规划与动态轨迹跟踪控制方法，其特征在于，规划直线道路上的换道轨迹的具体步骤如下：

S131、采用五次多项式来描述横向位移轨迹y_ref(t)，其公式表示为：

y_ref(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

S132、在直线道路上，根据换道起始时行车状态和换道终止时刻目标状态，沿y轴的行车状态为：

获取多项式系数：

其中：H为车道宽度；T₁为换道时间。

5.根据权利要求2所述的汽车换道轨迹规划与动态轨迹跟踪控制方法，其特征在于，规划曲线道路上的换道轨迹的具体步骤如下：

S141、设定当前车道和目标车道中心线曲率相等，且当前车道中心线方程已知：Y＝f(x)；换道时，沿车道中心相切的位移l为：

换道时汽车保持匀速，即l＝vt，得到：

S142、向心运动采用与直线换道相同的轨迹形式，即：

将g(x)代入G(t)即可得：

弯曲道路条件下换道轨迹为：

其中，θ为完成换道时车辆的位移角。

6.根据权利要求1所述的汽车换道轨迹规划与动态轨迹跟踪控制方法，其特征在于，步骤S2具体包括以下步骤：

S21、搭建模型预测所需的动力学模型和运动微分公式：

其中，C_f为前轮轮胎侧偏刚度之和；C_r为后轮轮胎侧偏刚度之和；l_f、l_r分别为质心到前轴、后轴距离，I_z为汽车绕z轴转动惯量，δ_f为前轮转角，v_y为车辆横向速度，ω为车辆横摆角速度，

为车辆横摆角加速度，

为车辆横向加速度，m为整车质量；

地面坐标系下车辆运动学方程：

S22、以

为系统状态量，以前轮转角为控制量u＝δ_f，系统输出量为：Y＝[x，y]^T，并对上述非线性系统线性化处理得到线性系统：

S23、对步骤S22中得到的线性系统进行离散化采用离散化周期为T_s，得到离散化系统状态方程：

S24、根据S23中得到的离散化系统状态方程设计模型预测控制器，采用前轮转角增量代替原来的前轮转角值作为系统控制输入，并对系统状态量进行如下转换：

转换后新的系统控制输入为：

Δu(k|t)＝u(k|t)-u(k-1|t)

得到新的离散线性化状态方程：

其中，η(k|t)＝Y(k|t)；

预测时域内系统输出：

Y_p＝A_pζ(k|t)+B_pΔU+D_p

其中，

S25、构建的参考轨迹引力函数J_gra和障碍车斥力函数J_rep(t)分别为：

为了限制控制量的大幅变化，设置控制增量变化代价函数J_ctr(t)：

基于模型预测控制算法预测在当前状态不同控制输入下被控车辆未来有限时域内的位置，并以预测位置处势场函数值和控制增量平方和为优化目标函数J：

S26、考虑对控制量和控制增量的约束，控制量即前轮转角u(t)应被约束在机械结构所允许的最大范围之内：

u_min≤u(t)≤u_max

控制增量Δu(t)应满足以下约束：

Δu_min≤Δu(t)≤Δu_max

侧向加速a_y(t)进行约束：

a_ymin≤a_y(t)≤a_ymax

车辆横向位移y(t)约束：

其中，D为被控车辆宽度，77为安全裕值。

Images