[发明专利]一种基于模型预测控制的半挂汽车列车的路径跟踪方法

权利要求书

1.一种基于模型预测控制的半挂汽车列车的路径跟踪方法，半挂汽车列车由牵引车和半挂车组成，牵引车和半挂车通过第五轮铰接，本方法在考虑半挂汽车列车运动学约束和执行机构物理约束的条件下，使半挂车尽可能沿着道路中心线行驶，同时尽可能减小执行机构的控制动作达到节能目的，其特征在于，本方法具体步骤如下：

步骤一、建立车辆道路关系模型：

建立大地坐标系，原点O固结于当前时刻半挂车质心o₂所处位置，X轴指向当前时刻半挂车车身正前方，X轴沿逆时针方向旋转90度的方向为Y轴正方向；

f(x)为大地坐标系中的道路中心线；f_l(x)为大地坐标系中的道路左边界线；f_r(x)为大地坐标系中的道路右边界线；因为半挂车的实际运行状态较复杂，所以简化对半挂车运动状态的描述，忽略半挂车的垂向运动和道路的垂向形状，把半挂车看作半挂车质心o₂，把大地坐标系中的道路左边界线f_l(x)和大地坐标系中的道路右边界线f_r(x)向内缩减半挂车宽度w₂的一半，f_l′(x)为简化后的大地坐标系中的道路左边界线；f′_r(x)为简化后的大地坐标系中的道路右边界线；简化后的大地坐标系中的道路边界线、大地坐标系中的道路中心线和原大地坐标系中的道路边界线的关系如式(1)所示：

式中大地坐标系中的道路中心线f(x)、大地坐标系中的道路左边界线f_l(x)和大地坐标系中的道路右边界线f_r(x)通过车载视觉传感器获得；

由于半挂车长度l₂远大于牵引车长度l₁，半挂汽车列车路径跟踪的主要目的是保证半挂车质心o₂在简化后的大地坐标系中的道路左边界线f_l'(x)和简化后的大地坐标系中的道路右边界线f′_r(x)内跟踪大地坐标系中的道路中心线f(x)；因此，半挂汽车列车路径跟踪的运动学约束如式(2)所示：

f_l'(x)≤y₂≤f′_r(x) (2)

式中y₂是半挂车质心o₂在大地坐标系中的侧向位置，单位m；

步骤二、建立半挂车运动学模型和半挂汽车列车三自由度动力学模型：

建立牵引车坐标系：原点O₁与牵引车质心o₁重合，X₁轴指向牵引车车身正前方，X₁轴沿逆时针方向旋转90度为Y₁轴正方向；

建立半挂车坐标系：原点O₂与半挂车质心o₂重合，X₂轴指向半挂车车身正前方，X₂轴沿逆时针方向旋转90度为Y₂轴正方向；

用半挂车运动学模型表征半挂车的操纵稳定性与半挂车的侧向运动之间的关系，并用半挂汽车列车三自由度动力学模型表征半挂汽车列车的操纵稳定性和半挂汽车列车的横摆运动之间的关系，具体过程如下：

假定半挂车是一个刚体，车轮不会发生形变，根据半挂车运动学方程和半挂车运动的几何关系得半挂车运动学模型如式(3)所示：

式中x₂为半挂车质心o₂在大地坐标系中的纵向位置，单位m；y₂为半挂车质心o₂在大地坐标系中的侧向位置，单位m；u₂为半挂车质心o₂的瞬时速度，单位m/s；ψ₂为半挂车横摆角，单位rad；β₂为半挂车质心侧偏角，单位rad；r₂为半挂车横摆角速度，单位rad/s；

车载视觉传感器能够获得半挂汽车列车前方50m的道路信息，该区域内道路的曲率在多数情况下相对较小，因此认为该区域内半挂车横摆角ψ₂较小，同时半挂车质心侧偏角β₂在实际运动中也较小，所以得近似关系如式(4)所示：

把式(4)带入式(3)，假设半挂车质心o₂的瞬时速度u₂恒定且仅考虑半挂车侧向运动，得简化的半挂车运动学模型如式(5)所示：

根据牛顿第二定律和力矩平衡关系，建立半挂汽车列车三自由度动力学模型：

半挂汽车列车三自由度动力学模型包含牵引车和半挂车两个刚体，选择牵引车质心侧偏角β₁、牵引车横摆角速度r₁和半挂车横摆角速度r₂作为模型的三个自由度，将牵引车前轮转角δ_f作为模型的输入；建立该模型时，做出如下假设：

(1)忽略牵引车和半挂车的俯仰和侧倾运动，忽略空气动力学和道路坡度的影响；

(2)牵引车质心o₁的瞬时速度u₁和半挂车质心o₂的瞬时速度u₂恒定且相等，近似认为u₁≈u₂；

(3)牵引车和半挂车之间的铰接角θ较小，近似认为cos(θ)≈1,sin(θ)≈0；

(4)牵引车前轮转角δ_f较小，近似认为cos(δ_f)≈1,sin(δ_f)≈0；

牵引车的动力学方程如式(6)所示：

式中m₁为牵引车质量，单位kg；u₁为牵引车质心o₁的瞬时速度，单位m/s；β₁为牵引车质心侧偏角，单位rad/s；ψ₁为牵引车横摆角，单位rad；F₁为牵引车前轴侧向力，单位N；F₂为牵引车后轴侧向力，单位N；F₄为简化的牵引车和半挂车之间的作用力，单位N；I_1zz为牵引车绕z轴的转动惯量，单位kg·m²；r₁为牵引车横摆角速度，单位rad/s；a为牵引车质心o₁到牵引车前轴的距离，单位m；b为牵引车质心o₁到牵引车后轴的距离，单位m；c为牵引车质心o₁到第五轮的距离，单位m；

半挂车的动力学方程如式(7)所示：

式中m₂为半挂车质量，单位kg；u₂为半挂车质心o₂的瞬时速度，单位m/s；β₂为半挂车质心侧偏角，单位rad/s；ψ₂为半挂车横摆角，单位rad，且ψ₂＝ψ₁-θ；F₃为半挂车后轴侧向力，单位N；F₄为简化的牵引车和半挂车之间的作用力，单位N；I_2zz为半挂车绕z轴的转动惯量，单位kg·m²；r₂为半挂车横摆角速度，单位rad/s；d为半挂车质心o₂到半挂车后轴的距离，单位m；e为半挂车质心o₂到第五轮的距离，单位m；

假设牵引车和半挂车的轮胎始终工作在线性区域，即轮胎侧向力等于轮胎侧偏刚度和轮胎等效侧偏角之间的乘积，得轮胎侧向力与轮胎等效侧偏角之间关系如式(8)所示：

式中k₁为牵引车前轴的侧偏刚度，单位N/rad；k₂为牵引车后轴的侧偏刚度，单位N/rad；k₃为半挂车后轴的侧偏刚度，单位N/rad；

牵引车和半挂车之间的运动学约束如式(9)所示：

结合式(6)、式(7)、式(8)和式(9)，得半挂汽车列车的连续状态空间模型如式(10)所示：

式中x＝[y₂ ψ₂ β₁ r₁ β₂ r₂]^T；

引入矩阵M如式(11)所示：

矩阵A如式(12)所示：

式中a₃₃＝(c+a)k₁+(c-b)k₂，a₅₅＝(e+d)k₃，

矩阵B如式(13)所示：

B＝M^-1[0 0 -(c+a)k₁ -k₁ 0 0]^T (13)

矩阵C如式(14)所示：

C＝[1 0 0 0 0 0] (14)

将半挂汽车列车的连续状态空间模型(10)离散化，采样时间为T_s，得半挂汽车列车的离散状态空间模型如式(15)所示：

式中A_c＝e^ATs,C_c＝C；k代表第k个采样时刻；

步骤三、采用模型预测方法设计半挂汽车列车路径跟踪控制器：

把步骤二中建立的半挂汽车列车的离散状态空间模型(15)作为预测模型，预测时域为P，控制时域为N，且满足N≤P，假设控制时域之外的控制输出保持不变，即δ_f(k+N-1)＝δ_f(k+N-1)＝…＝δ_f(k+P-1)，并给出如下定义：

定义k时刻的输出预测序列如式(16)所示：

Y(k+1|k)＝[y₂(k+1) y₂(k+2) … y₂(k+P)]^T (16)

定义k时刻的状态预测预测序列如式(17)所示：

X(k+1|k)＝[x(k+1) x(k+2) … x(k+P)]^T (17)

定义k时刻控制序列如式(18)所示：

U(k)＝[δ_f(k) δ_f(k+1) … δ_f(k+N-1)]^T (18)

得输出预测方程如式(19)所示：

矩阵S_x如式(20)所示：

矩阵S_u如式(21)所示：

定义半挂汽车列车路径跟踪控制器的参考输入序列如式(22)所示：

R(k)＝[y_r(k+1) y_r(k+2) … y_r(k+P)]^T (22)

式中y_r(k+i),i＝1,2,…,P是大地坐标系中的道路中心线f(x)离散化后的点，采样时间为u₂·T_s；

优化目标如下：

(1)根据对半挂汽车列车路径跟踪问题的分析，半挂汽车列车路径跟踪的主要目的是保证半挂车质心o₂在简化后的大地坐标系中的道路左边界线f′_l(x)和简化后的大地坐标系中的道路右边界线f′_r(x)内跟踪大地坐标系中的道路中心线f(x)，所以应使半挂车质心o₂尽可能跟踪大地坐标系中的道路中心线f(x)，以减少与其他车辆以及道路边缘或障碍物碰撞的风险，通过最小化如式(23)所示的目标函数实现控制目标：

J₁＝||Y(k+1|k)-R(k)||² (23 )

(2)确保半挂汽车列车路径跟踪控制器的输出较小，即牵引车前轮转角δ_f较小，以避免过多的控制动作，达到节能环保的目的，同时保证半挂汽车列车的转向平滑性较好，通过最小化如式(24)所示目标函数实现控制目标：

J₂＝||U(k)||² (24)

由于优化目标J₁和优化目标J₂之间存在冲突，为了得到合适的优化解，引入权重系数来调和矛盾，最终的优化目标函数如式(25)所示：

J＝||Γ_y(Y(k+1|k)-R(k))||²+||Γ_uU(k)||² (25)

式中Γ_y,Γ_u为权重矩阵；

约束条件如下：

(1)根据对半挂汽车列车路径跟踪问题的分析，半挂汽车列车路径跟踪的主要目的是保证半挂车质心o₂在简化后的大地坐标系中的道路左边界线f_l'(x)和简化后的大地坐标系中的道路右边界线f′_r(x)内跟踪大地坐标系中的道路中心线f(x)，所以应使预测时域P内的半挂车质心o₂在大地坐标系中的侧向位置y₂满足式(2)所示的约束，所以引入如式(26)所示约束：

f′_r(k+i)≤y₂(k+i)≤f_l'(k+i) (26)

式中f′_r(k+i),i＝1,2,…,P为简化后的大地坐标系中的道路右边界线f′_r(x)的离散点，采样时间为u₂·T_s；f_l'(k+i),i＝1,2,…,P是简化后的大地坐标系中的道路左边界线f_l'(x)的离散点，采样时间为u₂·T_s；

(2)为使牵引车前轮转角δ_f及其变化率不大于转向机构的饱和值，引入如式(27)所示约束:

式中δ_f(k+i),i＝0,2,…,N-1为牵引车前轮转角，单位rad；δ_fmax为最大牵引车前轮转角，单位rad；δ_fmin为最小牵引车前轮转角，单位rad；△δ_f(k+i)＝δ_f(k+i)-δ_f(k+i-1),i＝0,2,…,N-1为牵引车前轮转角增量，单位rad；△δ_fmin为最小牵引车前轮转角增量，单位rad；△δ_fmax为最大牵引车前轮转角增量，单位rad；

综上，半挂汽车列车路径跟踪问题可以转化为如下约束优化问题：

步骤四、求解约束优化问题并执行：

通过求解步骤三中约束优化问题(28)，将求得的最优控制序列U^*(k)的第一个元素作用于牵引车，控制半挂车沿着大地坐标系中的道路中心线f(x)行驶。