CN113830088A - 一种智能半挂牵引车轨迹跟踪预测控制方法与车辆 - Google Patents

Abstract

本发明公开了一种智能半挂牵引车轨迹跟踪预测控制方法与车辆，方法包括：S1、采集半挂牵引车的状态变量，输入改进的状态估计器进行估计，估计后输入MPC控制器；S2、先构建基于跟踪误差的半挂牵引车的侧向动力学模型；S3、对所述基于跟踪误差的半挂牵引车的侧向动力学模型离散化，得到半挂牵引车的预测模型；S4、根据所述预测模型构建半挂牵引车的目标函数，并结合控制与输出的约束条件、状态的约束条件得到最优求解；S5、将所述最优求解作为下一时刻的控制变量，作用于半挂牵引车，返回步骤S1。本发明解决了半挂牵引车轨迹跟踪的偏差较大、对实时跟踪和车辆状态突变的跟踪能力不足的问题。

Description

一种智能半挂牵引车轨迹跟踪预测控制方法与车辆

技术领域

本发明涉及轨迹跟踪控制技术领域，尤其涉及一种智能半挂牵引车轨迹跟踪预测控制方法与车辆。

背景技术

半挂牵引车因为装载量大、效率高、专用化等优点，故在公路运输工具中占据重要地位。但由于其自身重心较高且质量大并且挂车与牵引车相互耦合的特点，故在实现半挂牵引车智能辅助驾驶及稳定性方面难度较大，而车辆的轨迹跟踪控制是实现智能辅助驾驶的重要基础。现有的基于预瞄控制的轨迹跟踪控制算法没有考虑到车辆本身的因素影响，如传感器误差信号噪声干扰，还有车辆外部因素的影响如道路弯曲程度、地面附着条件、道路坡度等，在地面附着情况较差且速度较高的情况下，跟踪偏差较大；现有的基于传统模型预测控制的算法在量测噪声统计特性进行实时跟踪和对汽车状态突变的跟踪能力不足方面有明显缺陷，或者使用惯性导航仪等测量设备虽能提高状态量的精确性，但其成本过高，普及性差。

发明内容

(一)要解决的技术问题

基于上述问题，本发明提供一种智能半挂牵引车轨迹跟踪预测控制方法与车辆，解决半挂牵引车轨迹跟踪的偏差较大、对实时跟踪和车辆状态突变的跟踪能力不足的问题。

(二)技术方案

基于上述的技术问题，本发明提供一种智能半挂牵引车轨迹跟踪预测控制方法，包括以下方法：

S1、监测并采集半挂牵引车的状态变量，将所述状态变量输入改进的状态估计器进行估计，估计后输入MPC控制器；

S2、MPC控制器中，先构建基于跟踪误差的半挂牵引车的侧向动力学模型；

S2.1、构建半挂牵引车的侧向运动偏差模型：

其中，

u₁＝δ，u₂＝δ_ref，e_d表示距离偏差，即车辆后轴中心与其在道路中心线上的投影点之间的距离；

表示航向偏差，即车辆后轴中心线与大地坐标系X轴的夹角与道路中心线切向与大地坐标系X轴的夹角的差值；δ表示牵引车前轮转角；δ_ref表示由参考路径获取的前馈控制量；V_x1表示牵引车纵向车速；L₁表示牵引车轴距；

S2.2、分别构建轮胎的线性化模型、半挂牵引车的侧向动力学方程、牵引车的横摆动力学方程、挂车的横摆动力学方程、牵引车与挂车的耦合关系，再结合所述半挂牵引车的侧向运动偏差模型，构建半挂牵引车的侧向动力学线性偏差方程，转换成的状态空间模型即基于跟踪误差的半挂牵引车的侧向动力学模型；

所述半挂牵引车的侧向动力学线性偏差方程：

其中

u₁为控制变量，u₂为附加输入量；

M矩阵为：

N矩阵为：

n₃₄＝-C_ytL₂

P矩阵为：

Q矩阵为：

所述状态空间模型：

A＝M^-1N，B＝M^-1P，E＝M^-1Q，

其中，

为轮胎的侧偏刚度，α表示轮胎侧偏角，F_y为轮胎侧向力；F_yf为牵引车前轮侧向力，F_yr为牵引车后轴侧向力，F_yt为挂车后轴侧向力，C_yf为牵引车前轮侧偏刚度，C_yr为牵引车后轮侧偏刚度，C_yt为挂车后轮侧偏刚度，V_y1为牵引车横向车速，V_x1为牵引车纵向车速，

为牵引车的横摆角，

为挂车的横摆角，λ为铰接角，a₁为牵引车前轴至其质心距离，a₂为牵引点至挂车质心距离，b₁为牵引车后轴至其质心距离，c₁为牵引点至牵引车质心距离，L₁为牵引车轴距，L₂为挂车后轮到牵引点距离；m₁为牵引车质量，m₂为挂车质量，F_py为牵引点处牵引力侧向力分量，I_Z1为牵引车的转动惯量，I_Z2为挂车的转动惯量，V_y2为挂车横向车速，V_x2为挂车纵向车速；

S3、对所述基于跟踪误差的半挂牵引车的侧向动力学模型离散化，得到半挂牵引车的预测模型，得到未来P个时刻的预测状态X(k)和未来P个时刻的预测输出Y(k)；

S4、根据所述预测模型构建半挂牵引车的目标函数，并结合控制与输出的约束条件得到带约束的二次规划问题，再结合状态的约束条件得到最优求解；

S5、将所述最优求解作为下一时刻的控制变量，作用于半挂牵引车，返回步骤S1，下一时刻等同于下一轮中步骤S1中的当前时刻。

进一步的，步骤S1中所述状态变量包括：半挂牵引车辆的前轮转角δ，纵向加速度，侧向加速度，横摆角速度，牵引车的各车轮转速信号和路面附着系数信号。

进一步的，所述纵向加速度，侧向加速度，横摆角速度，牵引车的各车轮转速信号通过加速度传感器采集。

进一步的，步骤S1所述改进的状态估计器基于改进的SCKF算法对牵引车纵向车速、牵引车横向车速以及质心侧偏角进行估计，以所述前轮转角、牵引车的各车轮转速信号为输入量，纵向加速度、侧向加速度及横摆角速度为量测量，包括以下步骤：

S1.3.1、初始化；设置滤波初始值，包括状态值x_0|0、误差协方差阵P_0|0、过程噪声Q、量测噪声R₀以及渐消因子γ_k等；

S1.3.2、由P_0|0＝S_0|0S^T _0|0得到初始协方差阵P_0|0的平方根S_0|0；

S1.3.3、由

得到状态预测值

S1.3.4、由下式可得含有渐消因子γ_k的预测误差协方差平方根S_k|k-1；

S_k|k-1＝R_d(1：n，：)^T

S1.3.5、由下式得到预测量测值

S1.3.6、由下式得到新息协方差阵平方根S_zz，k1k-1

[Q_zz，d R_zz，d]＝qr([η_k|k-1，S_Q，k]^T)

S_zz，k1k-1＝R_zz，d(1：l，：)^T

S1.3.7、由下式可得互协方差P_xz，k|k-1

P_xz，k|k-1＝χ_k|k-1η^T _k1k-1

S1.3.8、由K_k＝(P_xz，k1k-1/S^T _zz，k|k-1)/S_zz，k|k-1可得增益矩阵K_k；

S1.3.9、由

得到状态估计值

S1.3.10、由下式可得误差协方差平方根S_k|k；

[Q_xz，d R_xz，d]＝qr([χ_k|k-1-K_kη_k|k-1，K_kS_R，k]^T)

S_k|k＝R_xz，d(1：n，：)^T

S1.3.11、由下式可得估计量测噪声均值

及协方差

S1.3.12、判断是否滤波时间等于[k，k+1]，若是，则进入步骤S1.3.13，否则返回重复步骤S3.3.3至步骤S3.3.11；

S1.3.13、所述预测量测值

包含[a_x，a_y]组成的向量，通过牛顿第二定律得到瞬时的牵引车纵向车速V_x1、牵引车横向车速V_y1的估计值，根据

得到质心侧偏角，所述状态估计值

输入所述MPC控制器。a_y为纵向加速度，a_x为横向加速度；

其中，

表示输出容积点，n表示状态维数；S_Q，k-1表示系统噪声序列方差阵Q_k-1的平方根系数，qr()表示正交三角分解法，将

化为正交阵Q_d与上三角阵R_d；ξ_i为基本容积点对应的权值，h()为非线性量测函数，μ_k-1为系统k-1时刻输入量，

为量测噪声均值；S_Q，k为系统噪声序列方差阵Q_k的平方根系数，S_R，K为协方差R_k的平方根系数；Z_2n，k1k-1为积分点为2n的传播容积点，

为量测预测值，l为体积容积点个数l＝2n；χ_k|k-1为加权中心矩阵；

b为记忆衰减因子，zk为非线性离散化后的量测量，ε_k为新息序列，

进一步的，步骤S2中，

所述轮胎的线性化模型：

所述半挂牵引车的侧向动力学方程：

所述牵引车的横摆动力学方程：

所述挂车的横摆动力学方程：

所述牵引车与挂车的耦合关系：

V_x2＝V_x1

进一步的，步骤S3中，所述未来P个时刻的预测状态X(k)和未来P个时刻的预测输出Y(k)为：

令k为当前时刻，控制时域M≤预测时域P，未来M个时刻的控制输入为u(k)，u(k+1)，…，u(k+M-1)，未来P个时刻的预测状态为x(k+1|k)，x(k+2|k)，…，x(k+P|k)，未来P个时刻的预测输出为y(k+1|k)，y(k+2|k)，…，y(k+P|k)，则

X(k)＝F_x x(k)+G_x U₁(k)+D_X U₂(k)

Y(k)＝F_yX(K)+G_yU₁(k)+Q_yU₂(k)

进一步的，步骤S4中，所述目标函数：

R(k+1)＝[r(k+1)…r(k+p)]^T

R(k+1)中的各项为参考轨迹中的参考输出值，t表示滚动优化时刻，N₁＜t＜N₂，N₁为滚动时域的初值，N₂为滚动时域的终值；目标函数第一项为预测输出与参考轨迹值的偏差，Γ_y为预测输出y的权重；目标函数第二项为控制输入，Γ_u1为控制输入u1的权重；目标函数第三项为附加输入，Γ_u2为控制输入u2的权重。

进一步的，步骤S4中，所述控制与输出的约束条件：

y_min(t+k)≤y(t+k)≤y_max(t+k)

其中k＝0，1，......，M-1，M为控制时域，则所述带约束的二次规划问题：

其中，

H₁＝2(G_y ^TΓ_y ^TΓ_yG_y+Γ_u1 ^TΓ_u1)

H₂＝2(Q_y ^TΓ_y ^TΓ_yQ_y+Γ_u2 ^TΓ_u2)

G(k+1|k)＝-2(G_y ^TΓ_y ^TΓ_yE_P(k+1|k))

E_P(k+1|k)＝R(k+1)-F_yX(k)。

进一步的，步骤S4中，所述状态约束条件包括：

S4.3.1、质心侧偏角β的约束：

在良好路面上，质心侧偏角β的极限值为：-10°≤β≤10°；

在湿滑路面上，质心侧偏角β的极限值为：-2°≤β≤2°；

S4.3.2、挂车铰接角λ的约束：λ≤60°；

S4.3.3、车辆附着条件约束：

车辆沿车辆坐标系的x轴匀速行驶时：|a_y|≤μg

a_y,min-ε≤a≤a_y,max+ε

其中，a_y为纵向加速度，a_x为横向加速度，μ表示路面附着系数，ε表示偏差；

S4.3.4、轮胎侧偏角α的约束：-5°≤α≤5°。

本发明也公开了一种采用所述的智能半挂牵引车轨迹跟踪预测控制方法的车辆。

(三)有益效果

本发明的上述技术方案具有如下优点：

(1)本发明针对半挂牵引车，采用考虑道路弯曲程度和跟踪误差的一体化的动力学模型进行轨迹预测，选择了距离偏差、航向偏差、牵引车的横摆角、牵引车横向车速、挂车与牵引车的铰接角及其铰接角速度作为状态变量，以便对预测模型进行约束，有利于提高轨迹跟踪的稳定性；且选项的状态变量考虑了道路曲率对跟踪效果的影响，有利于提高弯道跟踪的能力；

(2)本发明的目标函数考虑附加输入量，在结合控制输入和输出约束条件的基础上，加入挂车铰接角、质心侧偏角、车辆附着条件以及轮胎侧偏角约束的状态约束条件，得到优化的控制变量，有利于提高车辆的轨迹跟踪的稳定性和精度，且控制算法在恶劣工况下具有跟踪优势，对外部环境干扰和车辆内部的因素干扰有着良好的鲁棒性和适应性；

(3)本发明采用改进的平方根积卡尔曼滤波算法对牵引车纵向车速、牵引车横向车速及质心侧偏角等进行估计，提高汽车的稳定性、安全性及控制的精确性；

(4)改进的平方根积卡尔曼滤波算法在常规的平方根积卡尔曼滤波算法中加入Sage-Husa自适应算法，对量测噪声统计特性进行在线估计，以提高整个轨迹跟踪控制器的自适应能力；将时变量测噪声估计器引入渐消因子，以适应汽车在行驶过程中状态突变的情况，增强算法的鲁棒性，进一步提高状态估计器在量测噪声统计特性对车辆状态突变的跟踪能力；

(5)本发明不需要车上安装惯性导航仪等高成本设备，只需要加速度传感器便可得到较高精度的状态数据，提高跟踪效果的同时，节约了设备成本。

附图说明

通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，在附图中：

图1为本发明实施例的一种智能半挂牵引车轨迹跟踪预测控制方法的流程示意图；

图2为本发明实施例的一种智能半挂牵引车轨迹跟踪预测控制方法的原理框图；

图3为本发明实施例的半挂牵引车的轨迹跟踪误差的示意图；

图4为本发明实施例的半挂牵引车的受力分析示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本发明公开了一种智能半挂牵引车轨迹跟踪预测控制方法，如图1和图2所示，包括以下步骤：

S1、监测并采集半挂牵引车的状态变量，将所述状态变量输入改进的状态估计器进行估计，估计后输入MPC控制器；

S1.1、采集半挂牵引车的状态变量，状态变量包括：采集到半挂牵引车辆的前轮转角，用加速度传感器这种较低成本的传感器采集到车辆的纵向加速度，侧向加速度，横摆角速度，牵引车的各车轮转速信号，从路面平均附着系数表查询得到路面附着系数信号；

S1.2、将采集的所述状态变量输入到改进的状态估计器中，在改进的状态估计器中基于改进的SCKF算法对牵引车纵向车速、牵引车横向车速以及质心侧偏角进行估计；

前轮转角角度可以通过方向盘转角与汽车转向系统的传动比的乘积得到，或者也通过车辆配备的方向盘转角传感器得到。在估计算法中，前轮转角及牵引车的各车轮转速为输入量，纵向加速度、侧向加速度及横摆角速度为量测量。

由于传感器引起量测噪声，在车辆行驶过程中，传感器受到外界环境及使用频率的影响，使得量测噪声呈现不确定性和时变性。所以必须增强SCKF算法对时变量测噪声的自适应能力。此外，车辆在应对突变情况时，状态估计的残差会突然增大，故要求SCKF算法对车辆状态突变有强跟踪能力，SCKF算法表示平方根容积卡尔曼滤波算法。

故对时变量测噪声的自适应性和车辆状态突变的强跟踪性两个方面进行改进，提出改进的平方根容积卡尔曼滤波算法，即改进的SCKF。采用sage-Husa自适应算法对量测噪声统计特性进行在线估计，将sage-Husa自适应算法引入到SCKF算法中，得出改进的SCKF常值与时变噪声估计器。

基于sage-Huas的噪声估计器量测协方差为：

采用渐消指数加权的方法来加强新数据对统计结果的影响；所谓渐消指数加权法，即在每项式子前面乘以不同的加权系数；

选择加权系数{λ_i}，使其满足：

通过引入渐消因子γ_k对预测误差协方差进行调整，使得残差序列保持相互正交，达到算法对系统状态突变的及时跟踪，

因此，改进的SCKF算法滤波步骤如下：

S1.3.1、初始化；设置滤波初始值，包括状态值x_0|0、误差协方差阵P_0|0、过程噪声Q、量测噪声R_]以及渐消因子γ_k等；

S1.3.2、由P_0|0＝S_0|0S^T _0|0得到初始协方差阵P_0|0的平方根S_0|0；

S1.3.3、由

得到状态预测值

表示输出容积点，n表示状态维数；

S1.3.4、由下式可得含有渐消因子γ_k的预测误差协方差平方根S_k1k-1；

S_k1k-1＝R_d(1：n，：)^T

S_Q，k-1表示系统噪声序列方差阵Q_k-1的平方根系数，qr()表示正交三角分解法，将

化为正交阵Q_d与上三角阵R_d；

S1.3.5、由下式得到预测量测值

ξ_i为基本容积点对应的权值，h()为非线性量测函数，μ_k-1为系统k-1时刻输入量，

为量测噪声均值；

S1.3.6、由下式得到新息协方差阵平方根S_zz，k1k-1

[Q_zz，d R_zz，d]＝qr([η_k|k-1，S_Q，k]^T)

S_zz，k1k-1＝R_zz，d(1：l，：)^T

S_Q，k为系统噪声序列方差阵Q_k的平方根系数，S_R，K为协方差R_k的平方根系数；Z_2n，k1k-1为积分点为2n的传播容积点，

为量测预测值，l为体积容积点个数l＝2n；

S1.3.7、由下式可得互协方差P_xz，k|k-1

P_xz，k|k-1＝χ_k|k-1η^T _k|k-1

S1.3.8、由K_k＝(P_xz，k1k-1/S^t _zz，k1k-1)/S_zz，k1k-1可得增益矩阵K_k；

S1.3.9、由

得到状态估计值

S1.3.10、由下式可得误差协方差平方根S_k|k；

[Q_xz，d R_xz，d]＝qr([χ_k1k-1-K_kη_k|k-1，K_kS_R，k]^T)

S_k|k＝R_xz，d(1：n，：)^T

χ_k|k-1为加权中心矩阵；

S1.3.11、由下式可得估计量测噪声均值

及协方差

b为记忆衰减因子，zk为非线性离散化后的量测量，ε_k为新息序列，

S1.3.12、判断是否滤波时间等于[k，k+1]，若是，则进入步骤S1.3.13，否则返回重复步骤S3.3.3至步骤S3.3.11；

由于模型预测控制是以当前时刻预测下一时刻的输出，所以状态估计器的滤波估计时间应该与MPC控制器的预测时刻保持一致，以使得MPC控制器的每一个预测时域输出的精确度更高，同时有助于提高信号的平滑程度，因此，滤波时间应该与模型预测的每个单位时间段一致，即每个滤波时间为[k,k+1]。

S1.3.13、所述预测量测值

包含[a_x,a_y]组成的向量，通过牛顿第二定律得到瞬时的牵引车纵向车速V_x1、牵引车横向车速V_y1的估计值，根据

得到质心侧偏角的估计值，所述状态估计值

输入所述MPC控制器。a_y为纵向加速度，a_x为横向加速度。

改进的SCKF算法引入了噪声估计器及渐消因子，能对噪声统计特性和汽车状态突变进行实时跟踪。可更准确地估计牵引车纵向车速，牵引车横向车速及质心侧偏角。

S2、MPC控制器中，先构建基于跟踪误差的半挂牵引车的侧向动力学模型；

S2.1、构建半挂牵引车的侧向运动偏差模型；

为了实现半挂牵引车能够跟踪上期望的轨迹，需要建立一个车辆的侧向动力学模型和车辆与期望轨迹之间的关系。由于牵引车在向前行驶时，挂车对其具有较强的跟随能力且前向运动时铰接角具有自动收敛性，又由于前向行驶时车速较快且道路的曲率一般较小，这种情况下可以认为当牵引车跟踪上参考轨迹时，挂车也跟踪上了参考轨迹。

但是半挂牵引车经过弯道时，如图3所示的车辆跟踪误差示意图，路径跟踪过程中存在的跟踪误差为距离偏差e_d和航向偏差

距离偏差e_d定义为车辆后轴中心与其在道路中心线上的投影点之间的距离，设定车辆在道路左侧时距离偏差为正；航向偏差

表示车辆后轴中心线与大地坐标系X轴的夹角，

表示道路中心线切向与大地坐标系X轴的夹角，设定逆时针为正。车辆后轴中心在道路中心线上的投影点为f，设s为点f与道路中心线上的某参考点f]之间的弧长，K_ref为点f处的曲率，V_x和V_y分别为车辆在后轴中心处的速度在车辆坐标系x轴和y轴上的分量。则距离偏差e_d和航向偏差

有如下关系：

其中，δ_ref表示由参考路径获取的前馈控制量。

令u₁＝tan(δ)≈δ，u₂＝tan(δ_ref)≈δ_ref，

则式(1.1)可以简写为：

其中，

基于跟踪误差设计的车辆模型预测控制器可以方便地对跟踪过程中的航向偏差和距离偏差施加约束，且可以考虑道路曲率对跟踪效果的影响，有利于提高弯道跟踪的能力。

S2.2、分别构建轮胎的线性化模型、半挂牵引车的侧向动力学方程、牵引车的横摆动力学方程、挂车的横摆动力学方程、牵引车与挂车的耦合关系，再结合所述半挂牵引车的侧向运动偏差模型，构建半挂牵引车的侧向动力学线性偏差方程，转换成的状态空间模型即基于跟踪误差的半挂牵引车的侧向动力学模型；

其所要建立的基于跟踪误差的半挂牵引车的侧向动力学模型主要用于模型预测控制器的预测模拟作用，为减少控制算法的计算量，故在较为准确的描述车辆动力学的基础上尽可能地简化，故基于跟踪误差的半挂牵引车的侧向动力学模型构建方法如下：

S2.2.1、轮胎的线性化模型；

当轮胎侧偏角较小时，轮胎侧向力可以近似表示为轮胎侧偏角的线性函数：

为轮胎的侧偏刚度，F_y为轮胎侧向力，此轮胎的线性化模型在侧向加速度a_y≤0.4g、轮胎侧偏角α≤6的情况下对常规轮胎有较高的精度。

根据图4的受力分析，图中字符请详见表1，对动力学模型进行小角度假设(sinδ＝0，cosδ＝1；sinλ＝0，cosλ＝1)和纵向速度恒定假设，所以这里只需要考虑半挂牵引车的的侧向运动、牵引车的横摆运动，挂车的横摆运动。

S2.2.2、半挂牵引车的的侧向动力学方程

半挂牵引车的侧向力为：

S2.2.3、牵引车的横摆动力学方程：

S2.2.4、挂车的横摆动力学方程：

S2.2.5、牵引车与挂车的耦合关系为：

V_x2＝V_x1 (2.9)

S2.2.6、根据所述轮胎的线性化模型、半挂牵引车的侧向动力学方程、牵引车的横摆动力学方程、挂车的横摆动力学方程、牵引车与挂车的耦合关系，与所述半挂牵引车的侧向运动偏差模型相结合，构建半挂牵引车的侧向动力学线性偏差方程：

联立上述等式和将上述公式与侧向运动偏差模型相结合，并将线性轮胎模型代入，得到的新的侧向动力学线性偏差方程为：

其中

M矩阵为：

N矩阵为：

其中：

n₃₄＝-C_ytL₂

P矩阵为：

Q矩阵为：

其中u₁＝δ，u₁为控制变量，u₂＝δ_ref为附加输入量。

S2.2.7、将所述半挂牵引车的侧向动力学线性偏差方程，转换成状态空间模型，即基于跟踪误差的半挂牵引车的侧向动力学模型；

可以将式(2.12)转换为状态空间模型：

A＝M^-1N，B＝M^-1P，E＝M^-1Q，

状态输入x即步骤S1中的所述状态估计值

表1参数名称解释表

S3、对所述基于跟踪误差的半挂牵引车的侧向动力学模型离散化，得到半挂牵引车的预测模型，得到未来P个时刻的预测状态X(k)和未来P个时刻的预测输出Y(k)；

在整个控制时域中，令K时刻为当前时刻。控制器结合当前系统的测量值和预测模型，预测未来一段时域内[K，K+P]系统的状态和输出。

S3.1、将约束后的所述基于跟踪误差的半挂牵引车的侧向动力学模型(2.13)离散化，得到：

S3.2、将离散化的方程用向量形式描述，得到未来P个时刻的预测状态X(k)和预测输出Y(k)；

模型离散后，对系统进行多步预测，基于当前的状态x(k)和控制输入预测系统未来的多步输出。模型预测理论设定了预测时域P，控制时域M≤预测时域P，可以预测未来M个时刻的控制输入u(k)，u(k+1)，…，u(k+M-1)作用下，未来P个时刻的预测状态为x(k+1|k)，x(k+2|k)，…，x(k+P|k)和未来P个时刻的预测输出为y(k+1|k)，y(k+2|k)，…，y(k+P|k)。

用向量形式描述为：

X(k)＝F_xx(k)+G_xU₁(k)+D_XU₂(k) (3.2)

其中：

未来P个时间的输出为Y(k)，可得输出方程为：

Y(k)＝F_yX(K)+G_yU₁(k)+Q_yU₂(k) (3.3)

其中，

其中，A、B、C、E均为所述车辆横摆动力学微分方程的状态空间模型(2.13)中的系数。

S4、根据所述预测模型构建半挂牵引车的目标函数，并结合控制与输出的约束条件得到带约束的二次规划问题，再结合状态的约束条件得到最优求解；

S4.1、建立轨迹跟踪的目标函数；

设计的轨迹跟踪控制器能够良好的跟踪参考轨迹，定义输出的参考序列为：

R(k+1)＝[r(k+1)…r(k+P)]^T

参考输出值为：

其中，

表示参考横摆角，Y_ref表示参考侧向位移；

设计轨迹跟踪的目标函数：

其中，t表示滚动优化时刻，N₁<t<N₂，N₁为滚动时域的初值，N₂为滚动时域的终值；目标函数第一项为预测输出与参考轨迹值的偏差，Г_y为预测输出y的权重；Г_y值越大，跟踪效果越好；目标函数第二项为控制输入，Г_u1为控制输入u1的权重，Г_u1越大，控制动作越小，控制越平稳，第三项为附加输入，Г_u2为控制输入u2的权重，Г_u2越大，附加输入量越小。

S4.2、对目标函数结合控制与输出的约束条件得到带约束的二次规划问题；

目标函数的作用就是使车辆精确的跟踪轨迹且保持稳定性。故要加入控制与输出的约束条件：

y_min(t+k)≤y(t+k)≤y_max(t+k)

其中k＝0,1,……,M-1，M为控制时域。

将上述目标函数经控制与输出约束条件优化为二次规划问题：

其中,

H₁＝2(G_y ^TГ_y ^TГ_yG_y+Г_u1 ^TГ_u1)

H₂＝2(Q_y ^TГ_y ^TГ_yQ_y+Г_u2 ^TГ_u2)

G(k+1|k)＝-2(G_y ^TГ_y ^TГ_yE_P(k+1|k))

E_P(k+1|k)＝R(k+1)-F_yX(k)

S4.3、结合状态约束条件，得到最优求解；

为提高车辆在轨迹跟踪过程中的稳定性，需要对所述基于跟踪误差的半挂牵引车的侧向动力学模型添加约束，约束条件包括：

S4.3.1、质心侧偏角的约束

将质心侧偏角约束在侧向力线性区以内以保持车辆的稳定性。在良好路面上其侧偏角β的极限值为：

-10°≤β≤10°

在湿滑路面上的侧偏角β的极限值为：

-2°≤β≤2°

S4.3.2、挂车铰接角约束

为了防止挂车发生折叠现象以及半挂牵引车在跟随参考轨迹时，使得挂车良好的跟随牵引车运动，需要对挂车的铰接角λ进行约束：

λ≤60°

S4.3.3、车辆附着条件约束

轮胎所受的横纵向力的合力小于轮胎附着力的极限值，故车辆的纵向加速度a_y和横向加速度a_x的极限值：

车辆沿车辆坐标系的x轴匀速行驶时：

|a_y|≤μg

将加速度约束定为软约束，根据每个控制周期的求解情况动态调整该约束条件。

a_y,min-ε≤a≤a_y,max+ε

其中，μ表示路面附着系数，ε表示偏差。

S4.3.4、轮胎侧偏角约束

轮胎侧偏角α要限制在一定范围内才会使侧向力保持在线性区域内：

-5°≤α≤5°；

在求解最优控制量时，即[k,k+1]时刻得出的状态变量要在上述状态约束范围内，其实在预测控制模块中就是加入一个限幅模块，如果得出的状态信号超出这个范围，限幅模块会以规定范围的最大值或最小值输出。

S5、将所述最优求解作为下一时刻的控制变量，作用于半挂牵引车，返回步骤S1，下一时刻等同于下一轮中步骤S1中的当前时刻。

综上可知，通过上述的一种智能半挂牵引车轨迹跟踪预测控制方法与车辆，具有以下有益效果：

(1)本发明针对半挂牵引车，采用考虑道路弯曲程度和跟踪误差的一体化的动力学模型进行轨迹预测，选择了距离偏差、航向偏差、牵引车的横摆角、牵引车横向车速、挂车与牵引车的铰接角及其铰接角速度作为状态变量，以便对预测模型进行约束，有利于提高轨迹跟踪的稳定性；且选项的状态变量考虑了道路曲率对跟踪效果的影响，有利于提高弯道跟踪的能力；

(2)本发明的目标函数考虑附加输入量，在结合控制输入和输出约束条件的基础上，加入挂车铰接角、质心侧偏角、车辆附着条件以及轮胎侧偏角约束的状态约束条件，得到优化的控制变量，有利于提高车辆的轨迹跟踪的稳定性和精度，且控制算法在恶劣工况下具有跟踪优势，对外部环境干扰和车辆内部的因素干扰有着良好的鲁棒性和适应性；

(3)本发明采用改进的平方根积卡尔曼滤波算法对牵引车纵向车速、牵引车横向车速及质心侧偏角等进行估计，提高汽车的稳定性、安全性及控制的精确性；

(4)改进的平方根积卡尔曼滤波算法在常规的平方根积卡尔曼滤波算法中加入Sage-Husa自适应算法，对量测噪声统计特性进行在线估计，以提高整个轨迹跟踪控制器的自适应能力；将时变量测噪声估计器引入渐消因子，以适应汽车在行驶过程中状态突变的情况，增强算法的鲁棒性，进一步提高状态估计器在量测噪声统计特性对车辆状态突变的跟踪能力；

(5)本发明不需要车上安装惯性导航仪等高成本设备，只需要加速度传感器便可得到较高精度的状态数据，提高跟踪效果的同时，节约了设备成本。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；虽然结合附图描述了本发明的实施方式，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims (10)

1.一种智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，包括以下方法：

S1、监测并采集半挂牵引车的状态变量，将所述状态变量输入改进的状态估计器进行估计，估计后输入MPC控制器；

S2、MPC控制器中，先构建基于跟踪误差的半挂牵引车的侧向动力学模型；

S2.1、构建半挂牵引车的侧向运动偏差模型：

其中，

u₁＝δ，u₂＝δ_ref，e_d表示距离偏差，即车辆后轴中心与其在道路中心线上的投影点之间的距离；

表示航向偏差，即车辆后轴中心线与大地坐标系X轴的夹角与道路中心线切向与大地坐标系X轴的夹角的差值；δ表示牵引车前轮转角；δ_ref表示由参考路径获取的前馈控制量；V_x1表示牵引车纵向车速；L₁表示牵引车轴距；

S2.2、分别构建轮胎的线性化模型、半挂牵引车的侧向动力学方程、牵引车的横摆动力学方程、挂车的横摆动力学方程、牵引车与挂车的耦合关系，再结合所述半挂牵引车的侧向运动偏差模型，构建半挂牵引车的侧向动力学线性偏差方程，转换成的状态空间模型即基于跟踪误差的半挂牵引车的侧向动力学模型；

所述半挂牵引车的侧向动力学线性偏差方程：

其中

u₁为控制变量，u₂为附加输入量；

M矩阵为：

N矩阵为：

n₃₄＝-C_ytL₂

P矩阵为：

Q矩阵为：

所述状态空间模型：

A＝M^-1N，B＝M^-1P，E＝M^-1Q，

其中，

为轮胎的侧偏刚度，α表示轮胎侧偏角，F_y为轮胎侧向力；F_yf为牵引车前轮侧向力，F_yr为牵引车后轴侧向力，F_yt为挂车后轴侧向力，C_yf为牵引车前轮侧偏刚度，C_yr为牵引车后轮侧偏刚度，C_yt为挂车后轮侧偏刚度，V_y1为牵引车横向车速，V_x1为牵引车纵向车速，

为牵引车的横摆角，

为挂车的横摆角，λ为铰接角，a₁为牵引车前轴至其质心距离，a₂为牵引点至挂车质心距离，b₁为牵引车后轴至其质心距离，c₁为牵引点至牵引车质心距离，L₁为牵引车轴距，L₂为挂车后轮到牵引点距离；m₁为牵引车质量，m₂为挂车质量，F_py为牵引点处牵引力侧向力分量，I_Z1为牵引车的转动惯量，I_Z2为挂车的转动惯量，V_y2为挂车横向车速，V_x2为挂车纵向车速；

S3、对所述基于跟踪误差的半挂牵引车的侧向动力学模型离散化，得到半挂牵引车的预测模型，得到未来P个时刻的预测状态X(k)和未来P个时刻的预测输出Y(k)；

S4、根据所述预测模型构建半挂牵引车的目标函数，并结合控制与输出的约束条件得到带约束的二次规划问题，再结合状态的约束条件得到最优求解；

S5、将所述最优求解作为下一时刻的控制变量，作用于半挂牵引车，返回步骤S1，下一时刻等同于下一轮中步骤S1中的当前时刻。

2.根据权利要求1所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，步骤S1中所述状态变量包括：半挂牵引车辆的前轮转角δ，纵向加速度，侧向加速度，横摆角速度，牵引车的各车轮转速信号和路面附着系数信号。

3.根据权利要求2所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，所述纵向加速度，侧向加速度，横摆角速度，牵引车的各车轮转速信号通过加速度传感器采集。

4.根据权利要求1所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，步骤S1所述改进的状态估计器基于改进的SCKF算法对牵引车纵向车速、牵引车横向车速以及质心侧偏角进行估计，以所述前轮转角、牵引车的各车轮转速信号为输入量，纵向加速度、侧向加速度及横摆角速度为量测量，包括以下步骤：

S1.3.1、初始化；设置滤波初始值，包括状态值x_0|0、误差协方差阵P_0|0、过程噪声Q、量测噪声R₀以及渐消因子γ_k等；

S1.3.2、由P_0|0＝S_0|0S^T _0|0得到初始协方差阵P_0|0的平方根S_0|0；

S1.3.3、由

得到状态预测值

S1.3.4、由下式可得含有渐消因子γ_k的预测误差协方差平方根S_k|k-1；

S_k|k-1＝R_d(1:n,:)^T

S1.3.5、由下式得到预测量测值

S1.3.6、由下式得到新息协方差阵平方根S_zz,k|k-1

[Q_zz,d R_zz,d]＝qr([η_k|k-1,S_Q,k]^T)

S_zz,k|k-1＝R_zz,d(1:l,:)^T

S1.3.7、由下式可得互协方差P_xz,k|k-1

P_xz,k|k-1＝χ_k|k-1η^T _k|k-1

S1.3.8、由K_k＝(P_xz,k|k-1/S^T _zz,k|k-1)/S_zz,k|k-1可得增益矩阵K_k；

S1.3.9、由

得到状态估计值

S1.3.1]、由下式可得误差协方差平方根S_k|k；

[Q_xz,d R_xz,d]＝qr([χ_k|k-1-K_kη_k|k-1,K_kS_R,k]^T)

S_k|k＝R_xz,d(1:n,:)^T

S1.3.11、由下式可得估计量测噪声均值

及协方差

S1.3.12、判断是否滤波时间等于[k,k+1]，若是，则进入步骤S1.3.13，否则返回重复步骤S3.3.3至步骤S3.3.11；

S1.3.13、所述预测量测值

包含[a_x,a_y]组成的向量，通过牛顿第二定律得到瞬时的牵引车纵向车速V_x1、牵引车横向车速V_y1的估计值，根据

得到质心侧偏角，所述状态估计值

输入所述MPC控制器。a_y为纵向加速度，a_x为横向加速度；

其中，

表示输出容积点，n表示状态维数；S_Q,k-1表示系统噪声序列方差阵Q_k-1的平方根系数，qr()表示正交三角分解法，将

化为正交阵Q_d与上三角阵R_d；ξ_i为基本容积点对应的权值，h()为非线性量测函数，μ_k-1为系统k-1时刻输入量，

为量测噪声均值；S_Q,k为系统噪声序列方差阵Q_k的平方根系数，S_R,K为协方差R_k的平方根系数；Z_2n,k|k-1为积分点为2n的传播容积点，

为量测预测值，l为体积容积点个数l＝2n；χ_k|k-1为加权中心矩阵；

b为记忆衰减因子，z_k为非线性离散化后的量测量，ε_k为新息序列，

5.根据权利要求1所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，步骤S2中，

所述轮胎的线性化模型：

所述半挂牵引车的侧向动力学方程：

所述牵引车的横摆动力学方程：

所述挂车的横摆动力学方程：

所述牵引车与挂车的耦合关系：

V_x2＝V_x1

6.根据权利要求1所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，步骤S3中，所述未来P个时刻的预测状态X(k)和未来P个时刻的预测输出Y(k)为：

令k为当前时刻，控制时域M≤预测时域P，未来M个时刻的控制输入为u(k),u(k+1),···,u(k+M-1)，未来P个时刻的预测状态为x(k+1|k)，x(k+2|k)，···，x(k+P|k)，未来P个时刻的预测输出为y(k+1|k)，y(k+2|k)，···，y(k+P|k)，则

X(k)＝F_xx(k)+G_xU₁(k)+D_XU₂(k)

Y(k)＝F_yX(K)+G_yU₁(k)+Q_yU₂(k)

7.根据权利要求6所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，步骤S4中，所述目标函数：

R(k+1)＝[r(k+1) … r(k+p)]^T

R(k+1)中的各项为参考轨迹中的参考输出值，t表示滚动优化时刻，N₁<t<N₂，N₁为滚动时域的初值，N₂为滚动时域的终值；目标函数第一项为预测输出与参考轨迹值的偏差，Г_y为预测输出y的权重；目标函数第二项为控制输入，Г_u1为控制输入u1的权重；目标函数第三项为附加输入，Г_u2为控制输入u2的权重。

8.根据权利要求7所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，步骤S4中，所述控制与输出的约束条件：

y_min(t+k)≤y(t+k)≤y_max(t+k)

其中k＝0,1,……,M-1，M为控制时域，则所述带约束的二次规划问题：

其中,

H₁＝2(G_y ^TГ_y ^TГ_yG_y+Г_u1 ^TГ_u1)

H₂＝2(Q_y ^TГ_y ^TГ_yQ_y+Г_u2 ^TГ_u2)

G(k+1|k)＝-2(G_y ^TГ_y ^TГ_yE_P(k+1|k))

E_P(k+1|k)＝R(k+1)-F_yX(k)。

9.根据权利要求8所述的智能半挂牵引车轨迹跟踪预测控制方法，其特征在于，步骤S4中，所述状态约束条件包括：

S4.3.1、质心侧偏角β的约束：

在良好路面上，质心侧偏角β的极限值为：-10°≤β≤10°；

在湿滑路面上，质心侧偏角β的极限值为：-2°≤β≤2°；

S4.3.2、挂车铰接角λ的约束：λ≤60°；

S4.3.3、车辆附着条件约束：

车辆沿车辆坐标系的x轴匀速行驶时：|a_y|≤μg

a_y,min-ε≤a≤a_y,max+ε

其中，a_y为纵向加速度，a_x为横向加速度，μ表示路面附着系数，ε表示偏差；

S4.3.4、轮胎侧偏角α的约束：-5°≤α≤5°。

10.一种采用权利要求1-9任一项所述的智能半挂牵引车轨迹跟踪预测控制方法的车辆。

Images