CN111532283B - 一种基于模型预测控制的半挂汽车列车的路径跟踪方法 - Google Patents

Abstract

本发明公开了一种基于模型预测控制的半挂汽车列车的路径跟踪方法，第一步，建立车辆道路关系模型；第二步，建立半挂车运动学模型和半挂汽车列车三自由度动力学模型；第三步，采用模型预测方法设计半挂汽车列车路径跟踪控制器；第四步，求解约束优化问题并执行；本发明建立了半挂汽车列车路径跟踪控制器，可预测半挂车的运行状态，相对于人为驾驶，不存在由于牵引车和半挂车的铰接结构导致的驾驶员对半挂车运动状态反应迟钝的问题，反应速度更快，提高了半挂汽车列车的安全性；本方法的控制目标中，通过加入侧向位移偏差较小和控制量较小的控制目标，可一定程度避免半挂车的侧向加速度过大，降低半挂汽车列车侧翻的可能性。

Description

一种基于模型预测控制的半挂汽车列车的路径跟踪方法

技术领域

本发明属于自主驾驶技术领域，涉及一种半挂汽车列车的路径跟踪控制方法，具体涉及一种基于模型预测控制的半挂汽车列车的路径跟踪方法。

背景技术

半挂汽车列车由于其高效率和低成本而被广泛应用于全世界的物流运输中。随着无人码头的出现和物流业的迅猛发展，人们提出了对更高效更安全的物流运输的需求。同时，无人驾驶技术的发展为提高半挂汽车列车的运输效率和安全性带来了新的机遇。由于不需要驾驶员而且可以长期不间断工作，无人驾驶半挂汽车列车在提高物流运输效率方面具有很大优势。然而，关于自动驾驶半挂汽车列车的路径跟踪的研究很少。同时，牵引车与半挂车之间复杂的耦合运动也给自动驾驶半挂汽车列车的路径跟踪带来了困难。

发明内容

本发明提供了一种基于模型预测控制的半挂汽车列车的路径跟踪方法，通过控制牵引车前轮转角实现半挂车的路径跟踪保证半挂车不和道路边界碰撞的问题。

本发明是采用如下技术方案实现的：

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

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

建立大地坐标系，原点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₁ 00]^T (13)

矩阵C如式(14)所示：

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

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

式中A_c＝e^ATs,

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)||² (24)

(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*(k)作用于牵引车，控制半挂车沿着大地坐标系中的道路中心线f(x)行驶。

与现有技术相比本发明的有益效果是：

1.本方法在建立车辆道路模型时，考虑了半挂车和道路的形状和大小，降低了半挂车跨越车道线或与车道边界碰撞的可能性。

2.本方法建立了半挂汽车列车路径跟踪控制器，可预测半挂车的运行状态，相对于人为驾驶半挂汽车列车，不存在由于牵引车和半挂车之间的的铰接结构导致的驾驶员对半挂车运动状态反应迟钝的问题，具有更快的反应速度，提高了半挂汽车列车安全性能。

3.本方法的控制目标中，通过加入半挂车侧向位移偏差较小的控制目标和控制量较小的控制目标，可以一定程度避免半挂车的侧向加速度过大，降低半挂汽车列车侧翻的可能性。

附图说明

图1是本发明所述的一种基于模型预测控制的半挂汽车列车的路径跟踪方法的流程简图。

图2是本发明所述的一种基于模型预测控制的半挂汽车列车的路径跟踪方法中建立的车辆道路关系模型的示意图。

图3是本发明所述的一种基于模型预测控制的半挂汽车列车的路径跟踪方法中建立的半挂车运动学模型的示意图。

图4是本发明所述的一种基于模型预测控制的半挂汽车列车的路径跟踪方法中建立的半挂汽车列车三自由度动力学模型的示意图。

具体实施方式

下面结合附图对本发明作详细的描述：

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

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

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

图2为车辆道路关系模型的示意图，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为半挂车运动学模型的示意图，假定半挂车是一个刚体，车轮不会发生形变，根据半挂车运动学方程和半挂车运动的几何关系得半挂车运动学模型如式(3)所示：

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

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

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

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

图4为半挂汽车列车三自由度动力学模型的示意图，半挂汽车列车三自由度动力学模型包含牵引车和半挂车两个刚体，选择牵引车质心侧偏角β₁、牵引车横摆角速度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,

k代表第k个采样时刻；

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

半挂汽车列车的行驶环境是复杂多变的，其间充满着各种不确定性因素，所以不论是半挂汽车列车自动驾驶还是人驾驶汽车，都需要时刻根据周围的环境信息来实时决定下一步的行驶计划，这一点同模型预测控制的思想一致；

同时由于半挂汽车列车路径跟踪的主要控制问题是保证半挂汽车列车路径跟踪的运动学约束如式(2)，是一种带约束的控制问题，而模型预测控制是一种可以显示处理约束的控制方法，所以本方法采用模型预测控制进行控制算法的设计；

把步骤二中建立的半挂汽车列车的离散状态空间模型(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) 行驶。

Claims (1)

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)行驶。

Images