CN112622895A - 一种应用于自动驾驶的轨迹控制的预估控制方法 - Google Patents

Abstract

本发明公开了一种应用于自动驾驶的轨迹控制的预估控制方法，属于基本智能驾驶技术领域。首先域控制器实时接收来自虚拟大脑控制器和整车CAN网络输入的信号，然后对信号中的横向偏差dy和航向角偏差dphi进行滤波后，代入动态误差预测估计模型中，计算得到新的横向偏差dy、航向角偏差dphi、横向偏差变化率和航向角偏差变化率；根据偏差动态变化量的预估模型预估未来H_p个点的横向偏差dy、横向偏差变化率、转向角偏差dphi以及转向角偏差变化率；最后计算转向角，以CAN报文形式输入给转向角控制***，转向控制输出请求要受到主动转向***稳定运行的物理极限限制。本发明提高了自动驾驶的安全性、舒适性，扩大了自动驾驶的适应场景。

Description

一种应用于自动驾驶的轨迹控制的预估控制方法

技术领域

本发明属于基本智能驾驶技术领域，特别涉及一种应用于自动驾驶的轨迹控制的预估控制方法。

背景技术

现有无人驾驶***(SAE L4)自动驾驶的环境感知，路径规划全都交给具有强大计算能力的虚拟大脑来负责。整车控制器负责控制汽车执行执行机构—发动机，转向***，制动***等去使汽车跟踪大脑的轨迹。

传统的控制算法，无法或未充分利用到大脑的未来轨迹信息，同时结合整车的动力学信息，去安全、精准、舒适地驾驶汽车。传统控制算法—PID控制算法存在转向波动大，跟踪误差大，响应慢，鲁棒性低的问题，难以适应于复杂的开放地道路环境。

发明内容

为了解决上述问题，本发明提出一种应用于自动驾驶的轨迹控制的预估控制方法，设计合理，克服了现有技术的不足，具有良好的效果。

为了实现上述目的，本发明采用如下技术方案：

一种应用于自动驾驶的轨迹控制的预估控制方法，包括下列步骤：

步骤1：域控制器实时接收来自虚拟大脑控制器和整车CAN网络输入的信号；

虚拟大脑控制器输入的信号包括规划轨迹的坐标信息、运动信息以及当前车辆位置和参考轨迹的位置误差；

规划轨迹的坐标信号包括：横坐标y、纵坐标x以及航向角phi；

运动信息包括：规划轨迹的速度、加速度、曲率以及曲率变化速率；

当前车辆位置和参考轨迹的位置偏差包括：基于车辆坐标系的横向偏差dy、纵向偏差dx以及航向角偏差dphi；

整车CAN网络输入的信号包括：多信号来源的车速、多信号来源的转向角度、实际曲率、整车状态、转向***状态、轮胎胎压以及整车车重；

步骤2：对横向偏差dy、航向角偏差dphi、横向偏差变化率和航向角偏差变化率滤波，并代入动态误差预估模型中，计算得到预估后的横向偏差dy、航向角偏差dphi、横向偏差变化率和航向角偏差变化率；该计算结果将滤掉横向偏差dy和航向角偏差dphi带来的杂波，同时可以修正滤掉横向偏差dy和航向角偏差dphi的自身的累计误差。

步骤3：以预估后的横向偏差dy和航向角偏差dphi、横向偏差变化率、航向角偏差变化率以及未来H_p个点的曲率请求、未来H_p个点的车速请求、当前转向角作为输入信号，根据偏差动态变化量的预估模型预估出未来H_p个点的横向偏差dy、横向偏差变化率、航向角偏差dphi以及航向角偏差变化率；

步骤4：计算转向角；

转向角等于稳态控制转向角U_stb的第一个值U_stb(1)和动态转向角△U_robuss之和。

步骤5：域控制器以CAN报文形式将计算得到的转向角请求信号传输至转向角控制***，且转向控制输出请求要受到主动转向***稳定运行的物理极限限制。

优选地，在步骤1中，域控制器限制输入信号的速度变化率、曲率变化率、横向偏差变化率、纵向偏差变化率以及航向角偏差变化率的大小，且对多信号源进行优先级处理，以提高***的鲁棒性。

优选地，在步骤2中：

步骤2.1：将横向偏差dy、航向角偏差dphi、实测横向偏差变化率和实测航向角变化率与动态误差预估模型上一次输出的预估横向偏差dy、预估航向角偏差dphi、预估横向偏差变化率和预估航向角偏差变化率作差，得到横向偏差差值、航向角偏差差值、横向偏差变化率差值和航向角偏差变化率差值；

步骤2.2：对上述得到的差值分别进行卡尔曼滤波，得到横向偏差修正值、航向角偏差修正值、横向偏差变化率修正值和航向角偏差变化率修正值；

步骤2.3：将横向偏差修正值、航向角偏差修正值、横向偏差变化率修正值和航向角偏差变化率修正值与动态误差预估模型上一次输出的预估横向偏差、预估航向角偏差、预估横向偏差变化率和预估航向角偏差变化率作和，得到新的横向偏差、航向角偏差、横向偏差变化率和航向角偏差变化率；

步骤2.4：将新的横向偏差、航向角偏差、横向偏差变化率和航向角偏差变化率，以及当前反馈的实际车速和当前反馈的实际转向角，代入到动态误差预估模型中，计算得到此次预估后的横向偏差、航向角偏差、横向偏差变化率和航向角偏差变化率；

优选地，在步骤2.1中，根据控制器请求的曲率ctrl_curv、速度ctrl_vel，以及整车CAN反馈的转向速率yaw_rate、横向加速度latAccl，求出实测横向偏差变化率和实测航向角偏差变化率：

dy_rate＝intgrl(latAccl-ctrl_vel^2*ctrl_curv)；

dphi_rate＝yaw_rate-ctrl_vel*ctrl_curv；

其中，dy_rate为实测横向偏差变化率，dphi_rate为实测航向角偏差变化率，intgrl为积分函数。

优选地，动态误差预估模型是基于车速的线性化的车辆动力学离散模型，不同的车速下对应一个线性的误差动力学模型，误差动力学的连续性模型为：

其中，

u＝δ_f，r＝c_r，

其中，e₁,

e₂,

分别为横向偏差，横向偏差变化率，航向角偏差，航向角偏差变化率；δ_f为前轮转向角，c_r为请求轨迹的曲率；v为车身的质心速度，I_Z为车体转动惯量，m为车体的质量，c_af为前轮胎的转向刚度，c_ar为后轮胎的转向刚度，L_f为从前轴到车辆质心的距离，L_r1为从后轴第一根轴到质心的距离，L_r2为从后轴第二根轴到质心的距离；

将等式①转化为不同车速下的离散模型，离散化的采样周期为Ts,离散化的方法可以是零阶保持方法，一阶保持方法或双线性方法，转换后的模型为：

x_k+1＝A_d(i)x_k+B_d(i)u_k+E_d(i)r_k ②

其中，A_d(i)、B_d(i)、E_d(i)是指不同车速下的离散化模型矩阵；k为数字计算步数，i表示整数化后的车速值，比如车速为12.6m/s，最近值整数化为12m/s；

等式②即为动态误差预估模型，是针对6*4配置的卡车车头所建立的模型。

优选地，基于动态误差预估模型，该动态误差预估模型可以描述为：

其中，k为数字计算步数；x_dyn(k+1)分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率的动态变化量；x_S(k)分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率的稳态变化量；x_k+1分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率；u_dyn是转向***的转向角动态变化量；u_S(k)是转向***的转向角稳态量，u_k是转向***的转向角；

基于等式③，得到一种偏差动态变化量的预估模型：

其中，H_u为预估控制的动态控制量预期步数；H_p为预估控制的预估步数，且每步的步长为Ts；A_d,B_d,E_d为某个特定车速Vavg下动态误差预估模型的矩阵式，Vavg是给定轨迹点的车速在H_p内的平均车速。

优选地，在步骤3中，H_p的范围为3～50，预估时间范围为300ms～5s。当预估时间或点数过小，则预估动态变化不大；当预估时间或点数过大，则预估的动态变化误差太大。

优选地，在步骤4中，包括下列子步骤：

步骤4.1：植入二次优化算法的离线计算公式，计算动态转向角△U_robuss；

预估控制的基本原理是根据设计的罚函数，计算出最优控制值，并把最优控制值的第一个值作为最终的控制值。罚函数包括轨迹的预估的横向偏差加权平方和，横向偏差变化率加权平方和，航向角偏差加权平方和，控制转向角偏差加权平方和的之和的最小化，同时考虑转向角限制。

其中，

其中，k为数字计算步数，k_i为预估的步数，Y是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率，△U是为最优角度补偿值，Q,q,R,r_w是罚函数的权重系数；

得到离线运算公式：

其中，g＝2θ^TQε_k；H＝(θ^TQθ+R)；

在实际***中，由于模型的误差以及来自外界的干扰，使该离线优化算子不一定是最优解，为了保证该解即使不是最优值，依然能够保证轨迹在稳定范围，能够跟踪参考轨迹，一种终值稳定算法表示如下所示，计算出稳定补偿值△u_adpt：

其中，

为最后一个预估值与其边界的差值的表达式，K为调节系数；n为数字计算步长，且n≤H_p；Cv为x_dyn的不变集；

动态转向角△U_robuss为：

△U_robuss＝△U₍₁₎+△u_adpt；

其中，△U₍₁₎为最优角度补偿值△U的第一个值；

步骤4.2：稳态控制转向角U_stb为：

U_stb＝L*ρ；

其中，L是等效轴距，ρ是转向比；

为稳态控制转向角U_stb的第一个值；

步骤4.3：根据步骤1和步骤2计算转向角，转向角等于稳态控制转向角U_stb的第一个值

和动态转向角△U_robuss之和。

本发明所带来的有益技术效果：

1.解决了轨迹跟踪的问题；

2.当大脑发出的反馈横向偏差和航向角偏差信号出现丢失或异常时，提高控制***的鲁棒性，减少停机或性能降低的次数；

3.当转向请求指令可能超越主动转向***线性运行区间或可能使转向***急剧下降时,提高控制***的鲁棒性，减少停机或性能降低的次数；

4.提高了轨迹跟踪性能，减小了横向误差，比如大大减小了小半径转弯，高速变道，倒车等场景的横向误差，从而扩大了轨迹跟踪的应用场景；

5.提高控制***的效率，减少频繁变换转角，增加转向的平滑度，使车辆的乘坐舒适性大大提高。

附图说明

图1为本发明中一种应用于自动驾驶的轨迹控制的预估控制方法的流程图；

图2为本发明中对信号进行滤波处理的流程图；

具体实施方式

为了便于本领域一般技术人员理解与实施本发明，下面结合附图和具体实施例对本发明的具体实施方式做进一步说明：

如图1所示，一种应用于自动驾驶的轨迹控制的预估控制方法，包括下列步骤：

步骤1：域控制器实时接收来自虚拟大脑控制器和整车CAN网络输入的信号；

虚拟大脑控制器输入的信号包括规划轨迹的坐标信息、运动信息以及当前车辆位置和参考轨迹的位置误差；

规划轨迹的坐标信号包括：横坐标y、纵坐标x以及转向角phi；

运动信息包括：规划轨迹的速度、加速度、曲率以及曲率变化速率；

当前车辆位置和参考轨迹的位置偏差包括：基于车辆坐标系的横向偏差dy、纵向偏差dx以及转向角偏差dphi；

整车CAN网络输入的信号包括：多信号来源的车速、多信号来源的转向角度、实际曲率、整车状态、转向***状态、轮胎胎压以及整车车重；

域控制器限制输入信号的速度变化率、曲率变化率、横向偏差变化率、纵向偏差变化率以及转向角偏差变化率的大小，且对多信号源进行优先级处理，以提高***的鲁棒性。

步骤2：对横向偏差dy、转向角偏差dphi、横向偏差变化率和转向角偏差变化率滤波，并代入动态误差预估模型中，计算得到预估后的横向偏差dy、航向角偏差dphi、横向偏差变化率和转向角偏差变化率；该计算结果将滤掉横向偏差dy和航向角偏差dphi带来的杂波，同时可以修正滤掉横向偏差dy和航向角偏差dphi的自身的累计误差。

步骤3：以预估后的横向偏差dy和转向角偏差dphi、横向偏差变化率、航向角偏差变化率以及未来H_p个点的曲率请求、未来H_p个点的车速请求、当前转向角作为输入信号，根据偏差动态变化量的预估模型预估出未来H_p个点的横向偏差dy、横向偏差变化率、转向角偏差dphi以及转向角偏差变化率；H_p的范围为3～50，预估时间范围为300ms～5s。当预估时间或点数过小，则预估动态变化不大；当预估时间或点数过大，则预估的动态变化误差太大。

步骤4：计算转向角；

转向角等于稳态控制转向角U_stb的第一个值

和动态转向角△U_robuss之和。

步骤5：域控制器以CAN报文形式将计算得到的转向角请求信号传输至转向角控制***，且转向控制输出请求要受到主动转向***稳定运行的物理极限限制。

具体地，在步骤2中：

步骤2.1：将横向偏差dy、转向角偏差dphi、实测横向偏差变化率和实测转向角变化率与动态误差预估模型上一次输出的预估横向偏差dy、预估转向角偏差dphi、预估横向偏差变化率和预估转向角偏差变化率作差，得到横向偏差差值、转向角偏差差值、横向偏差变化率差值和转向角偏差变化率差值；

步骤2.2：对上述得到的差值分别进行卡尔曼滤波，得到横向偏差修正值、转向角偏差修正值、横向偏差变化率修正值和转向角偏差变化率修正值；其中图2中增益系数是离线卡尔曼滤波的计算系数；

步骤2.3：将横向偏差修正值、转向角偏差修正值、横向偏差变化率修正值和转向角偏差变化率修正值与动态误差预估模型上一次输出的预估横向偏差、预估转向角偏差、预估横向偏差变化率和预估转向角偏差变化率作和，得到新的横向偏差、转向角偏差、横向偏差变化率和转向角偏差变化率；

步骤2.4：将新的横向误差、转向角误差、横向偏差变化率和转向角偏差变化率，以及当前反馈的实际车速和当前反馈的实际转向角，代入到动态误差预估模型中，计算得到此次预估后的横向偏差、转向角偏差、横向偏差变化率和转向角偏差变化率；

具体地，在步骤2.1中，根据控制器请求的曲率ctrl_curv、速度ctrl_vel，以及整车CAN反馈的转向速率yaw_rate、横向加速度latAccl，求出实测横向偏差变化率和实测航向角偏差变化率：

其中，dy_rate为实测横向偏差变化率，dphi_rate为实测航向角偏差变化率，intgrl为积分函数。

具体地，动态误差预估模型是基于车速的线性化的车辆动力学离散模型，不同的车速下对应一个线性的误差动力学模型，针对6*4配置的卡车车头的误差动力学的连续性模型为：

其中，

u＝δ_f，r＝c_r，

其中，e₁,

e₂,

分别为横向偏差，横向偏差变化率，航向角偏差，航向角偏差变化率；δ_f为前轮转向角，c_r为请求轨迹的曲率；v为车身的质心速度，I_Z为车体转动惯量，m为车体的质量，c_af为前轮胎的转向刚度，c_ar为后轮胎的转向刚度，L_f为从前轴到车辆质心的距离，L_r1为从后轴第一根轴到质心的距离，L_r2为从后轴第二根轴到质心的距离；

将等式①转化为不同车速下的离散模型，离散化的采样周期为Ts,离散化的方法可以是零阶保持方法，一阶保持方法或双线性方法，转换后的模型为：

x_k+1＝A_d(i)x_k+B_d(i)u_k+E_d(i)r_k ②

其中，A_d(i)、B_d(i)、E_d(i)是指不同车速下的离散化模型矩阵；k为数字计算步数，i表示整数化后的车速值，比如车速为12.6m/s，最近值整数化为12m/s；

等式②即为动态误差预估模型。

具体地，基于动态误差预估模型，该动态误差预估模型可以描述为：

其中，k为数字计算步数；x_dyn(k+1)分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率的动态变化量；x_S(k)分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率的稳态变化量；x_k+1分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率；u_dyn是转向***的转向角动态变化量；u_S(k)是转向***的转向角稳态量，u_k是转向***的转向角；

基于等式③，得到一种偏差动态变化量的预估模型：

其中，H_u为预估控制的动态控制量预期步数；H_p为预估控制的预估步数，且每步的步长为Ts；A_d,B_d,E_d为某个特定车速Vavg下动态误差预估模型的矩阵式，Vavg是给定轨迹点的车速在H_p内的平均车速。

具体地，在步骤4中，包括下列子步骤：

步骤4.1：植入二次优化算法的离线计算公式，计算动态转向角△U_robuss；

预估控制的基本原理是根据设计的罚函数，计算出最优控制值，并把最优控制值的第一个值作为最终的控制值。罚函数包括轨迹的预估的横向偏差加权平方和，横向偏差变化率加权平方和，航向角偏差加权平方和，控制转向角偏差加权平方和的之和的最小化，同时考虑转向角限制。

其中，

其中，k为数字计算步数，k_i为预估的步数，Y是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率，△U是为最优角度补偿值，Q,q,R,r_w是罚函数的权重系数；

得到离线运算公式：

其中，g＝2θ^TQε_k；H＝(θ^TQθ+R)；

在实际***中，由于模型的误差以及来自外界的干扰，使该离线优化算子不一定是最优解，为了保证该解即使不是最优值，依然能够保证轨迹在稳定范围，能够跟踪参考轨迹，一种终值稳定算法表示如下，计算出稳定补偿值△u_adpt：

其中，

为最后一个预估值与其边界的差值的表达式，K为调节系数；n为数字计算步长，且n≤H_p；Cv为x_dyn的不变集；

动态转向角△U_robuss为：

△U_robuss＝△U₍₁₎+△u_adpt；

其中，△U₍₁₎为最优角度补偿值△U的第一个值；

步骤4.2：稳态控制转向角U_stb为：

U_stb＝L*ρ；

其中，L是等效轴距，ρ是转向比；

为稳态控制转向角U_stb的第一个值；

步骤4.3：根据步骤1和步骤2计算转向角，转向角等于稳态控制转向角U_stb的第一个值

和动态转向角△U_robuss之和。

以上为本实施例的完整实施过程。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (9)

1.一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，包括下列步骤：

步骤1：域控制器实时接收来自虚拟大脑控制器和整车CAN网络输入的信号；

虚拟大脑控制器输入的信号包括规划轨迹的坐标信息、运动信息以及当前车辆位置和参考轨迹的位置误差；

规划轨迹的坐标信号包括：横坐标y、纵坐标x以及航向角phi；

运动信息包括：规划轨迹的速度、加速度、曲率以及曲率变化速率；

当前车辆位置和参考轨迹的位置偏差包括：基于车辆坐标系的横向偏差dy、纵向偏差dx以及航向角偏差dphi；

整车CAN网络输入的信号包括：多信号来源的车速、多信号来源的转向角度、实际曲率、整车状态、转向***状态、轮胎胎压以及整车车重；

步骤2：对横向偏差dy、航向角偏差dphi、横向偏差变化率和航向角偏差变化率滤波，并代入动态误差预估模型中，计算得到预估后的横向偏差dy、航向角偏差dphi、横向偏差变化率和航向角偏差变化率；

步骤3：以预估后的横向偏差dy和航向角偏差dphi、横向偏差变化率、航向角偏差变化率以及未来H_p个点的曲率请求、未来H_p个点的车速请求、当前转向角作为输入信号，根据偏差动态变化量的预估模型预估出未来H_p个点的横向偏差dy、横向偏差变化率、航向角偏差dphi以及航向角偏差变化率；

步骤4：计算转向角；

转向角等于稳态控制转向角U_stb的第一个值

和动态转向角△U_robuss之和；

步骤5：域控制器以CAN报文形式将计算得到的转向角请求信号传输至转向角控制***，且转向控制输出请求要受到主动转向***稳定运行的物理极限限制。

2.根据权利要求1所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，在步骤1中，域控制器限制输入信号的速度变化率、曲率变化率、横向偏差变化率、纵向偏差变化率以及航向角偏差变化率的大小，且对多信号源进行优先级处理，以提高***的鲁棒性。

3.根据权利要求1所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，在步骤2中：

步骤2.1：将横向偏差dy、航向角偏差dphi、实测横向偏差变化率和实测航向角变化率与动态误差预估模型上一次输出的预估横向偏差dy、预估航向角偏差dphi、预估横向偏差变化率和预估航向角偏差变化率作差，得到横向偏差差值、航向角偏差差值、横向偏差变化率差值和航向角偏差变化率差值；

步骤2.2：对上述得到的差值分别进行卡尔曼滤波，得到横向偏差修正值、航向角偏差修正值、横向偏差变化率修正值和航向角偏差变化率修正值；

步骤2.3：将横向偏差修正值、航向角偏差修正值、横向偏差变化率修正值和航向角偏差变化率修正值与动态误差预估模型上一次输出的预估横向偏差、预估航向角偏差、预估横向偏差变化率和预估航向角偏差变化率作和，得到新的横向偏差、航向角偏差、横向偏差变化率和航向角偏差变化率；

步骤2.4：将新的横向误差、航向角误差、横向偏差变化率和航向角偏差变化率，以及当前反馈的实际车速和当前反馈的实际转向角，代入到动态误差预估模型中，计算得到此次预估后的横向偏差、航向角偏差、横向偏差变化率和航向角偏差变化率。

4.根据权利要求3所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，在步骤2.1中，根据控制器请求的曲率ctrl_curv、速度ctrl_vel，以及整车CAN反馈的转向速率yaw_rate、横向加速度latAccl，求出实测横向偏差变化率和实测航向角偏差变化率：

dy_rate＝intgrl(latAccl-ctrl_vel^2*ctrl_curv)；

dphi_rate＝yaw_rate-ctrl_vel*ctrl_curv；

其中，dy_rate为实测横向偏差变化率，dphi_rate为实测航向角偏差变化率，intgrl为积分函数。

5.根据权利要求3所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，动态误差预估模型是基于车速的线性化的车辆动力学离散模型，不同的车速下对应一个线性的误差动力学模型，误差动力学的连续性模型为：

其中，

u＝δ_f，r＝c_r，

其中，e₁,

e₂,

分别为横向偏差，横向偏差变化率，航向角偏差，航向角偏差变化率；δ_f为前轮转向角，c_r为请求轨迹的曲率；v为车身的质心速度，I_Z为车体转动惯量，m为车体的质量，c_af为前轮胎的转向刚度，c_ar为后轮胎的转向刚度，L_f为从前轴到车辆质心的距离，L_r1为从后轴第一根轴到质心的距离，L_r2为从后轴第二根轴到质心的距离；

将等式①转化为不同车速下的离散模型，离散化的采样周期为Ts，离散化的方法是零阶保持方法，一阶保持方法或双线性方法，转换后的模型为：

x_k+1＝A_d(i)x_k+B_d(i)u_k+E_d(i)r_k ②

其中，A_d(i)、B_d(i)、E_d(i)是指不同车速下的离散化模型矩阵；k为数字计算步数，i表示整数化后的车速值；

等式②即为动态误差预估模型。

6.根据权利要求5所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，动态误差预估模型是针对6*4配置的卡车车头所建立的模型。

7.根据权利要求3所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，基于动态误差预估模型，该动态误差预估模型可以描述为：

其中，k为数字计算步数；x_dyn(k+1)分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率的动态变化量；x_S(k)分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率的稳态变化量；x_k+1分别是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率；u_dyn是转向***的转向角动态变化量；u_S(k)是转向***的转向角稳态量，u_k是转向***的转向角；

基于等式③，得到一种偏差动态变化量的预估模型：

其中，H_u为预估控制的动态控制量预期步数；H_p为预估控制的预估步数，且每步的步长为Ts；A_d,B_d,E_d为某个特定车速Vavg下动态误差预估模型的矩阵式，Vavg是给定轨迹点的车速在H_p内的平均车速。

8.根据权利要求1所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，在步骤3中，H_p的范围为3～50，预估时间范围为300ms～5s。

9.根据权利要求1所述的一种应用于自动驾驶的轨迹控制的预估控制方法，其特征在于，在步骤4中，包括下列子步骤：

步骤4.1：植入二次优化算法的离线计算公式，计算动态转向角△U_robuss；

其中，

其中，k为数字计算步数，k_i为预估的步数，Y是横向偏差、横向偏差变化率、航向角偏差以及航向角偏差变化率，△U是为最优角度补偿值，Q,q,R,r_w是罚函数的权重系数；

得到离线运算公式：

其中，g＝2θ^TQε_k；H＝(θ^TQθ+R)；

在实际***中，由于模型的误差以及来自外界的干扰，使该离线优化算子不一定是最优解，为了保证该解即使不是最优值，依然能够保证轨迹在稳定范围，能够跟踪参考轨迹，一种终值稳定算法表示如下，计算出稳定补偿值△u_adpt：

其中，

为最后一个预估值与其边界的差值的表达式，K为调节系数；n为数字计算步长，且n≤H_p；Cv为x_dyn的不变集；

动态转向角△U_robuss为：

△U_robuss＝△U₍₁₎+△u_adpt；

其中，△U₍₁₎为最优角度补偿值△U的第一个值；

步骤4.2：稳态控制转向角U_stb为：

U_stb＝L*ρ；

其中，L是等效轴距，ρ是转向比；

为稳态控制转向角U_stb的第一个值；

步骤4.3：根据步骤1和步骤2计算转向角，转向角等于稳态控制转向角U_stb的第一个值

和动态转向角△U_robuss之和。

Images