拖挂大载荷特种车辆离散路径跟踪控制

周达，伟利国，袁玉龙，陆一乔

（1.中国农业机械化科学研究院集团有限公司，北京 100083；2.中国中元国际工程有限公司，北京 100089）

目前较为常用的路径追踪方法有：比例积分微分（proportional-integral-derivative，PID）方法、纯追踪方法、模糊控制方法、模型预测控制（model predictive control，MPC）方法等[1-2]。PID方法在控制领域应用广泛，该方法不需要准确的控制对象的数学模型，实现较为简单，且通常具有良好的控制效果。然而，PID方法的调参过程较为繁琐，且对于外部干扰的适应性不足[3]。纯追踪方法是另一种简便有效的路径追踪方法，其核心是连接车辆的后轴中点与预瞄点，并通过几何方法确定航向偏角，进而计算出转向角[4]。该方法在中低速车辆中应用广泛，具有较好的效果。模糊控制方法不需要精确的车辆运动学模型，可以将人类现有的经验转化为模糊规则，进而通过模糊推理计算转向角。对于一些系统模型复杂且已有较为成熟的人工控制经验时，模糊控制方法较为经济高效[5]。MPC方法则考虑了车辆运动学或动力学模型，将转向角作为控制量，根据当前的位姿及对一定时间内的车辆运动趋势进行预测，从而建立车辆实际行走轨迹与目标路径间偏差的数学模型，通过优化的方法求解出最优的一组控制量。当车辆运动学或动力学模型已知时，MPC方法可以获得较好的控制效果[6]。

本文针对拖挂大载荷机组设备的特种车高精度路径跟踪问题，将纯追踪算法和积分控制算法有机结合，提出了一种离散路径跟踪方法。本文控制对象车辆为三轮前驱结构，驱动和转向均由前轮完成，车辆最高时速6 km/h。

1 车辆及荷载模型

1.1 运动学分析

车辆运动学模型是从几何学角度描述车辆的运动规律，并且不考虑车辆所受的力[7]。为了高效方便地进行分析和控制，本文采用运动学方法对车辆进行抽象，并根据以下前提条件建立车辆运动学模型[8]：只考虑车辆在平面的运动；车辆低速行驶且不考虑轮胎的滑移；车身可视为是刚性的。

由于不考虑轮胎滑移，因此各个车轮的速度可认为均沿车轮轴线方向，从而车辆运动模型可被简化为自行车模型，即只有一个前轮，一个后轮，前、后轮之间为刚性连接。根据刚体运动学可以求得车辆的速度瞬时中心，即前轮法线与后轴延长线的交点。在短时间内，可以认为车辆绕行驶速度的瞬时中心做圆周运动（当前轮方向沿车身轴线方向时车辆做直线运动）。图1所示为简化的车辆运动学模型，其中P点为车辆后轮中心，O点为速度瞬时中心，R为转弯半径，δ为前轮转角（逆时针为正，顺时针为负），l为轴距。由几何关系可得：

图1 车辆运动学模型Fig.1 Kinematic model of vehicle

1.2 纯追踪模型

由1.1节可知，当车辆前轮保持固定转角时，P点可以近似为做圆周运动。如果要使P点经过某个目标点T，只需找到合适的前轮转角并使P点所要经过的圆周轨迹穿过T点即可。其中，T点可通过预瞄法查找获得，即以P点为圆心，以给定前视距离s为半径做前视距离圆OP，如果OP与目标路径存在交点，则该交点即为预瞄点T点[9]。图2所示为纯追踪模型示意图，α为车辆航向与后轴中点-预瞄点连线间的夹角。

图2 纯追踪模型示意图Fig.2 Schematic diagram of pure pursuit model

由几何关系可得∠TOP=2α，另根据正弦定理可得：

进行简化可得：

1.3 极限转角

对于式（1）所述几何关系，存在一种特殊情况，即当90°<|α|<180°时，sinα=sin(180°-α)。此时纯追踪算法的输出转角等同于车辆航向角偏差较小时的输出转角。比如，当α=180°时（车辆航向完全背离预瞄点），sinα=0，即纯追踪算法的输出转角δ=0°，显然车辆将向背离预瞄点的方向行驶。图3表示当车辆航向偏差大于90°时的示意图。其中，T为OP与路径的交点，T′为以线段-O--P-为轴线的T点的镜像点。

图3 车辆大航向偏差示意图Fig.3 Schematic diagram of big heading error of vehicle

显而易见，sinα=sinα′。如前所述，当 |α|越接近180°时，输出转角将越接近0°，从而造成车辆趋于失控。根据式（1），当α=±90°时，计算的输出转角达到理论极限值：

式中：δl为逆时针极限值；δr为顺时针极限值。

此外，考虑到车辆前轮有机械转向限位（大约65°），导致车辆前轮有机械转角δth（逆时针方向为正，顺时针方向为负），从而真正可以输出的最大转角应为

式中：δlmax为可以输出的逆时针极限值；δrmax为可以输出的顺时针极限值。

1.4 追踪静差

本文中所研究的特种车辆，在实际使用中的某些场合下拖挂有负载设备。当拖挂作业时，负载设备的前轮被固定在车辆后部。图4为特种车辆拖挂模型示意图。

图4 拖挂模型示意图Fig.4 Schematic diagram of the towing model

对于纯追踪算法，其输出值的大小仅取决于当前的误差大小，在误差较小时可以认为是一个近似的比例控制器。当被拖挂设备的质量很大时，其惯性以及后轮与地面的摩擦力也较大，从而当车辆前轮转向角不足以使车辆和被拖挂设备克服此阻力作用时，车体将偏向路径的一侧[10]。由于拖挂模型受力及转向系统较为复杂，难以准确建立其动力学模型，因此为了消除如前所述的净差，引入积分控制器。当净差不断累积时，积分控制器叠加的输出转角将不断增大以进行校正，从而达到消除净差的目的。

2 算法

2.1 预瞄点的选取

本文中预定路径以离散点的形式给出，点与点的间隔约10 cm。由于路径点间隔相对于路径长度来说较为密集，因此，直接采用线性插补的方法，将路径点用直线段顺次连接，从而形成多条首尾相接的直线段。根据式（3），纯追踪算法输出的转角主要取决于α。而要确定α则需要先确定预瞄点，即确定前视距离圆OP与路径的位置关系[11]。对于路径上某直线段，将其写为向量形式n。将P分别连接Di，Di+1得到向量a和b。根据几何关系：a⋅n>0时，P在投影范围之前；a⋅n≤0且b⋅n>0时，P在投影范围之内；b⋅n<0时，P在投影范围之后。分别对三种情况下前视距离圆OP与的位置关系进行分析。

2.1.1P在线段投影范围之前

将s与||a||，||b||进行比较，可分为三种情况：

1）s<||a||：OP与没有交点。不妨以Di作为预瞄点，此时输出转角达到极限转角，车辆可以较快地追踪入线。

2）||a||≤s≤||b||：OP与有一个交点，此交点即预瞄点。

3）s>||b||：OP与无交点。为了判断OP与路径是否存在交点，从开始继续向后查找。若OP与路径上某一直线段存在交点，则此交点即预瞄点。若遍历完路径上之后的所有直线段仍未找到交点，则以路径的倒数第二个点向路径终点做射线，记为c。由于c的起点在OP之内，所以c必与OP存在交点，以此交点作为预瞄点。

2.1.2P点在线段投影范围之内

此时，P点在直线段上有一个投影点，记为F点，从P点向F点作向量，记为h。将s与||h||，||b||进行比较，可分为三种情况：

1）s<||h||：同2.1.1节中的情况1）。

2）||h||≤s≤ ||b||：同2.1.1节中的情况2）。

2.1.3P点在线段投影范围之后

此时，判断OP与的位置关系没有意义。因此，从开始继续向后查找，直到找到投影范围包络住P点的直线段，记为。从开始，重复2.1.2节的步骤。若找不到此直线段则说明P点已在路径终点之后，结束追踪。

2.2 预瞄点T的坐标计算

如前所述，当OP与某直线段存在交点且P点在投影范围之内或之后时，此交点即预瞄点，其坐标计算过程如下：记n的单位向量为t，则有：

从而有：

分别从Di和P向预瞄点T点作向量，记为f和s，从而根据几何关系有：

将Di和T以向量表示，记为Di和T，则有：

当出现2.1.1节中情况3）中无法在路径上找到预瞄点的情况时，将c作为直线段并结合式（9）即可得到T点的坐标。

香龙血树接种在培养基MS+30 g/L蔗糖+6 g/L琼脂+0.3 g/L活性炭+2.0 mg/L 6-BA+0.20 mg/L NAA上更容易诱导长出丛芽，在培养基MS +30 g/L蔗糖+6 g/L琼脂+0.3 g/L活性炭+5.0 mg/L 6-BA+0.20 mg/L NAA和MS +30 g/L蔗糖+6 g/L琼脂+0.3 g/L活性炭+5.0 mg/L 6-BA+0.50 mg/L NAA上更容易诱导形成愈伤组织，形成的愈伤组织经诱导分化能长出生长健壮的苗。

2.3 积分控制

如1.4节中所述，为了消除追踪静差，引入积分控制器。

2.3.1 偏差方向判定

积分控制器需要对横向偏差进行累加，因此偏差需要有方向。图5为偏差方向示意图。

图5 偏差方向示意图Fig.5 Schematic diagram of error direction

结合式（7）可得横向偏差标量的表达式如下：

式中：h为偏差的标量。

2.3.2 积分控制器

如图6所示，以采样时刻为横坐标，以横向偏差为纵坐标，可得一系列离散点。

图6 离散偏差示意图Fig.6 Schematic diagram of discrete error

为代入积分控制器进行计算，需要将离散偏差转换为连续偏差。通过将离散偏差拟合为偏差曲线并通过数值方法计算曲线积分可得连续偏差。

考虑到车辆行驶速度较低，不妨以图6中的阴影部分的面积作为偏差增量以减少计算量，得到累计偏差值：

式中：sum(k)为当前采样时刻的累计偏差；sum(k-1)为上一采样时刻的累计偏差；h(k)为当前采样时刻的偏差；h(k-1)为上一采样时刻的偏差；dt(k)为当前采样时刻与上一采样时刻之间的时间差；sum(0)为累计偏差初值。

则积分控制器的输出为

式中：δI(k)为当前时刻的积分控制输出；KI为积分常数。

2.3.3 积分抗饱和

由式（11）可知，当sum的绝对值随着偏差的累计而变得非常大时，积分控制器也将输出一个非常大的值，甚至造成系统失控。为避免这种情况，为积分控制器的输出设置上、下饱和边界，积分控制器的输出按下式计算[12]：

式中：δI(k),out为积分控制器的输出；δI,max，-δI,max为饱和区上、下边界。

当δI(k)超出饱和区边界后，进入饱和区。

在某些情况（例如：车辆的初始偏差非常大、被拖挂设备的摩擦阻力非常大，导致车辆长时间无法入线等情况）下，偏差会累计得非常快，并且使积分控制器的输出很快进入饱和区。当车辆在输出转角的控制下渐渐靠近目标路径时，车辆的偏差绝对值越来越小，直至变为零。由于偏差累计的滞后性，此时积分控制器仍然在饱和区内，且需要较长时间才能脱离饱和区。这种由积分控制器的滞后性造成的“积分饱和”现象可能引起车辆的大幅超调，进而造成系统失控[13]。为了消除以上问题，在积分控制器中加入反馈抑制：

式中：Kcomp为抗积分饱和系数，取值大于零。

Kcomp越大，则积分控制器退出饱和区的速度越快，反之越慢。因此，Kcomp需要根据试验标定出合理取值。

综上，得到积分控制器的输出后，与纯追踪控制器的输出进行叠加，得到最终的输出转角：

3 实验结果分析

本实验在水平倾斜度为2°的机场跑道上完成。实验所用车辆的参数如下：轴距2 406 mm；轮距1 254 mm；质量2 t；最大行驶速度6 km/h。实验中，车辆的速度随路径曲率而变化并在曲率为零处达到最大行驶速度。本实验采用RTK差分定位的方式，定位模块为“和芯星通UB482”，经全站仪测试，该模块在静止时的定位误差小于2 cm。RTK差分数据传输采用1.4 GHz无线专用网络，延迟小于20 ms。

实验中，带拖挂负载车辆分别对多条路径进行了追踪。每组实验的流程大致如下：当拖挂负载车辆准备就绪后，由控制后台发送目标路径至控制终端；控制终端收到路径后进行解析并返回准备就绪信息至控制后台，控制后台随即发送开始指令至控制终端；控制终端收到指令后启动并开始路径追踪；当车辆到达路径终点处时停车并通知控制后台路径追踪完成，实验结束。

3.1 路径及轨迹

本文挑选出具备代表性的4组实验，并对实验中记录的车辆实时定位数据进行滤波、坐标转换等处理。

图7所示为直线路径及轨迹，图8所示为双纽线路径及轨迹，图9和图10所示分别为2组自由曲线路径及轨迹。

3.1.1 直线实验结果分析

图7中，车辆的目标路径为一条直线。车辆从坐标点(-9,26)附近出发。车辆在启动后进行了航迹纠偏并迅速完成了入线。横向偏差的平均值为1.2 cm。

图7 直线路径及轨迹Fig.7 Straight line path and trajectory

3.1.2 双纽线实验结果分析

图8中，目标路径为一条双纽线，该路径的表达式为

图8 双纽线路径及轨迹Fig.8 Lemniscate path and trajectory

车辆自坐标点(29,121)附近出发，初始航向向南，最终回到双纽线右侧顶点。车辆在路径上曲率最大处的横向偏差最大，而在曲率较小处则较为精准。横向偏差平均值为6.3 cm。

3.1.3 自由曲线实验结果分析

图9和图10中的曲线为自由曲线。图9中车辆自(-38,62)附近出发，图10中车辆自(-3,136)附近出发。图9中，车辆启动后快速完成了入线，在较大的转弯后，轨迹偏向路径外侧（过弯处向外的方向），其原因在于拖挂设备的惯性较大，在过弯时会对车辆后部施加一个较大的侧向推力，造成车辆偏向路径弯曲处的外侧。图10中，车辆在启动阶段速度较低时出现了“抄近路”的现象[14]，在平直段追踪效果最好，在较大的过弯后，与图9的轨迹类似，都出现了偏向路径弯曲处外侧的现象。图9横向偏差平均值为2.5 cm，图10横向偏差平均值为5.3 cm。

图9 自由曲线路径及轨迹1Fig.9 Arbitrary curve path and trajectory 1

图10 自由曲线路径及轨迹2Fig.10 Arbitrary curve path and trajectory 2

3.2 横向偏差分布

图11为车辆追踪各条路径时的横向偏差。图中，每个箱体的上/下边分别对应上/下四分位数，箱体中的线对应中位数，箱体外的上/下两条线分别对应上/下临界异常值，在临界异常线以外的被视作异常值，箱体内的空心点表示平均值。由图11可以看出，误差分布最集中的实验组为直线，最为分散的为自由曲线2。所有实验组中，横向误差的中位数和均值都不同程度地偏向正方向（路径左侧）。由于路径并非都朝向一个方向，因此，可以初步排除由于拖挂负载摩擦力以及实验场地坡道造成误差的可能。一种可能是车辆的零位标定出现偏差，另一种可能是双天线的中线与车体的中线之间存在偏差。

图11 横向偏差箱线图Fig.11 Box plot of lateral error

3.3 改进方向

3.3.1 路径形式

所有实验组中的路径均为离散点形式，为了实现较为精准的控制效果，路径点之间必须有较小的距离。对于大场地、长路径的情况，如果仍采用离散点的形式则路径将占用较大的内存空间；对于路径中较为平直的区间，大量的离散点并不必要。因此，需要在后续实验中将路径由离散点形式转换为参数化形式[15]。

3.3.2 前视距离

对于3.1.3节中所述的纯追踪算法中车辆在低速过弯时出现的“抄近路”现象，可以通过动态调节前视距离的方法（如自适应控制、神经网络等）进行改善[16]。

3.3.3 车辆模型

本实验中车辆行驶速度较低，在后续实验中将提高车辆的最高行驶速度。随着车速的提高，1.1节中应用车辆运动学模型的前提将不再适用，此时根据车辆运动学做出的分析与实际情况间的误差将增大[17]。此外，在路径曲率较大处，拖挂设备的惯性可能造成车辆转向时向路径外侧偏移[18]。综上，考虑引入动力学模型并通过参数在线辨识的方法获得准确的动力学模型[19]。

4 结论

本文针对特种车辆拖挂大载荷实现离散路径跟踪控制的问题进行了研究。分析了车辆和拖挂荷载的运动模型以及基于离散路径的预瞄点查找方法，并在此基础上设计了纯追踪控制器。随后，针对大载荷引起的追踪静差问题，引入了积分控制算法和积分抗饱和算法。最后，对实验数据进行了处理和分析。结果表明，该控制方法具有良好的控制效果。