基于模型预测控制的无人驾驶履带车辆轨迹跟踪方法研究

胡家铭， 胡宇辉， 陈慧岩， 刘凯

(北京理工大学 机械与车辆学院, 北京 100081)

0 引言

无人驾驶履带车辆，作为未来高机动地面无人战斗系统的一个重要组成部分，具有广泛的应用前景[1]。在无人驾驶履带车辆研究中，轨迹跟踪控制是其最重要的内容之一，特别是在复杂环境下的高精度轨迹跟踪是保证其行驶安全和完成相关任务的基础[2]。然而，在实际轨迹跟踪控制中，特别是在越野环境下，无人驾驶履带车辆仍然面临很多挑战[3]。首先，履带车辆是一个包含复杂非完整性约束的非线性耦合系统，而且履带相对于地面的滑移、滑转以及侧滑运动难以准确测量；其次，无人驾驶履带车辆的轨迹跟踪控制还受到行驶道路的环境约束以及执行机构的饱和约束；最后，无人驾驶履带车辆高精度的轨迹跟踪控制需要充分考虑建模误差、参数扰动等系统不确定性以及外部干扰的影响。因此，研究无人驾驶履带车辆的轨迹跟踪控制具有重要的理论及现实意义。

目前，国内外学者对履带车辆的转向过程做了大量研究。如文献[4-8]建立了履带与地面之间存在剪切作用关系的转向过程模型，并通过实车转向试验验证了所建立模型的准确性。文献[9]建立了基于瞬时转向中心的履带车辆运动学模型，并采用Levenberg-Marquardt算法进行滑动参数的实时估计，用于预测车辆行驶轨迹。文献[10] 运用二次优化理论和状态反馈控制原理搭建车辆动力学模型和控制器。文献[11]提出了基于非线性模型预测控制的滚动预测范围估计方法。上述研究实现了对履带车辆转向过程的建模，并能够为其转向控制提供理论支持。

考虑到路网文件提供的参考路径是一系列不包含曲率信息的离散路点这一情况，前期研究已经证明忽略道路曲率或将其视为定值的路径跟踪方法会产生较大的跟踪误差，并导致系统振荡[12]。因此，多项式曲线、样条曲线和Bezier曲线等参数化的路径拟合方法被广泛地应用于为路径跟踪提供的道路曲率信息[13-14]。然而，参数化的曲线拟合方法存在确定曲线阶次的难题。一方面选择较高的曲线阶次虽然能够实现较好的拟合精度，但是增加了求解的复杂程度，加重了优化计算量，从而影响路径跟踪算法的实时性；另一方面，选择较低的曲线阶次虽然能够降低计算量，但是在拟合复杂路径时容易产生较大的拟合误差，进而增加无人驾驶车辆发生碰撞危险的可能性。在这种情况下，为了降低参考路径的拟合误差，通常将其分成多段进行拟合，但分段过多也会增加拟合的计算量。因此，需要研究一种参考路径分段数目自适应调整的参考路径拟合方法。

针对无人驾驶履带车辆的轨迹跟踪问题，模型预测控制由于具备能够系统地考虑模型非线性，且易于处理多约束优化问题等优势，在无人驾驶车辆的运动规划与控制等领域得到了广泛的应用[15]。然而，由于履带车辆在实际转向时，履带相对于地面的滑移、滑转及侧滑运动受到行驶速度、转弯半径和地面条件等复杂因素的影响，很难准确测量。同时，由于载荷变化、重心转移等造成的模型参数改变以及外部干扰等不利因素增加了轨迹跟踪控制的难度，甚至造成系统的不稳定。

本文研究履带车辆的滑动转向特性，建立了基于瞬时转向中心的履带车辆运动学模型。针对参考路径是离散的路点序列这一特点，提出了一种基于3次Bezier曲线的参考路径自适应拟合方法，在实现路径平滑基础上提供道路的曲率信息。设计了无人驾驶履带车辆的模型预测轨迹跟踪控制器，系统地处理无人驾驶履带车辆建模误差、环境约束以及执行机构约束。同时，在轨迹跟踪控制器中引入了跟踪误差反馈校正因子，有效地抑制模型不确定性和外部干扰带来的影响。

1 基于瞬时中心的履带车辆运动学建模和控制系统问题描述

1.1 履带车辆运动学模型

本文研究的无人驾驶履带车辆由某型装甲车改装而成，是一种典型的差速转向车辆，通过调节左、右两侧主动轮的转速来控制航向和速度。履带车辆左、右两侧主动轮各有一套独立的动力传动系统，由直流电机、两挡位机械自动变速箱、制动系统和减速器等组成，如图1所示。

图1 无人驾驶履带车辆的动力传动系统Fig.1 Power transmission system of unmanned tracked vehicle

图2 履带车辆转向过程的运动学关系Fig.2 Kinematics of tracked vehicle during steering

仅考虑无人驾驶履带车辆在水平地面上的运动，令η=[xo,yo,ψ]T为系统状态量，(xo,yo)表示履带车辆几何中心o在大地坐标系下的位置。考虑到履带车辆滑动转向的特性，即转向过程始终伴随着高速侧履带的滑转与低速侧履带的滑移。针对这一问题，采用文献[9]提出的基于瞬时转向中心的履带车辆运动学模型：

(1)

1.2 控制系统问题描述

ηe(k+1)=η(k)-ηd(k+1)+TsR(ψ)u(k),

(2)

式中：Ts为系统控制周期。

研究目标可以表述为基于瞬时转向中心的履带车辆运动学模型(1)式和误差跟踪模型(2)式设计模型预测的轨迹跟踪控制器，使得无人驾驶履带车辆能够有效地跟踪参考轨迹ηd，且能抑制模型不确定性和外界干扰的影响。

为了充分利用参考路径所提供的信息，得到更准确的参考状态量ηd，提出了一种基于3次Bezier曲线的参考路径自适应拟合方法。同时，结合基于瞬时转向中心的履带车辆运动学模型，设计了基于模型预测的轨迹跟踪控制器。无人驾驶履带车辆控制系统框图如图3所示，本文主要关注基于3次Bezier曲线的自适应路径拟合以及基于模型预测的轨迹跟踪控制器两个模块。

图3 无人驾驶履带车辆控制系统框图Fig.3 Block diagram of control system of unmanned tracked vehicle

2 自适应参考路径拟合

由于Bezier曲线具备控制简便、具有极强的描述能力和易于生成复杂的平滑曲线等优势，因此常被用来进行参考路径的平滑。Bezier曲线可以表示为

(3)

式中：m为Bezier曲线阶次；q(τi)为参数τi处的插值点；Pk为第k个控制点。通过对参数τi在[0, 1]内取值，可以得到在第1个控制点P0和最后1个控制点Pm之间生成任意个插值点。其中，3次Bezier曲线由于具备良好的拟合能力和简单的表达形式等优点，受到广泛关注，其表达式为

(4)

式中：τi∈[0,1]。

为保证后续段曲线和前一段曲线在连接处的光滑(在连接点处切矢量方向相同，大小不等，即G1连续)，必须使得拟合出来的相邻两段曲线在接点处的3个控制点共线，如图4所示。图4以3次Bezier曲线为例，Qk、Pk(k=0, 1, 2, 3)分别为前后两段曲线的控制点，其中，P0、P3为端点，P1、P2为中间点。曲线G1连续要求：

(5)

式中：Gb为方向矢量；s为待定的标量参数。

图4 两段3次Bezier曲线的G1连续Fig.4 G1 continuity of two third-order Bezier curves

由于参考路径的曲线拟合不要求拟合曲线严格地通过各个离散路点，因此选择最小二乘法拟合各段参考路径的中间点。设pi为参考路径给定的离散路点，则最小二次方程可以表示为

(6)

(7)

在此基础上，基于3次Bezier曲线的自适应参考路径拟合流程如图5所示。在路径拟合的初始化阶段将参考路径的起止点作为3次Bezier曲线的第1个和最后1个控制点。在每次曲线拟合的迭代过程中，首先根据(7)式求解各段Bezier曲线中间控制点的位置。然后根据(4)式可以得到与原路径点对应的插值点，并计算插值点与原路径点之间的残差ρ. 若Bezier曲线中间插值点处的残差大于人为设定的阈值β，即ρ≥β，则选择在残差最大的中间点对应的路径点处将参考路径分段。这个过程不断重复，直到拟合得到的各段曲线中间点处残差满足拟合精度ε的要求。

图5 自适应的参考路径曲线拟合流程图Fig.5 Flow chart of adaptive reference path curve fitting

3 基于模型预测的轨迹跟踪控制器

3.1 轨迹跟踪控制器设计

图6 模型预测控制原理示意图Fig.6 Shematic diagram of model predictive control

模型预测控制器的设计思想就是将实际控制问题转化为带约束条件的目标函数，通过对该目标函数进行最优化求解得到未来一段时域内的控制序列，然后将其中第1个元素施加到被控系统上，模型预测控制的基本原理如图6所示。

控制过程中，始终存在一条参考路径，如图6中长虚线所示。图6中，k表示时刻，以时刻k作为当前时刻，ηc和uc分别表示实际执行得到的状态量和控制量与通过车辆模型预测得到的最优状态量和最优控制量之间的误差，ηp和up分别为上一控制周期预测得到的最优状态量和控制量,ηk和um分别为当前控制周期状态量的测量值和控制量实际执行后的测量值，u*表示最优控制序列。控制器结合当前的测量值ηk和um，预测系统未来一段时域内[k,k+Np] 的输出，Np为预测时域，如图6中实线所示。通过求解满足目标函数以及各种约束的优化问题，得到在预测时域内一系列的控制序列u*，如图6中的短虚线所示，将该控制序列的第一个元素u*(0)作为执行控制量。当来到下一个时刻k+1时，重复上述过程，滚动地完成带约束的优化问题，通过优化问题求解工具进行求解[16]。

由于履带车辆的滑转率、滑移率很难测得所导致的建模误差以及外部干扰等因素，在实际应用中实际执行得到的状态量和通过车辆模型预测得到的最优状态量之间存在一定的误差，即

(8)

为了补偿这一误差，将反馈校正因子引入基于模型预测控制的轨迹跟踪控制器，如图7所示。图7中，虚线框表示的是模型预测控制器的主体，主要由履带车辆线性误差模型、系统约束以及目标函数等组成。履带车辆线性误差模型由(2)式得到，控制器设计的主要内容是确定合适的目标函数和系统约束。

图7 带反馈校正的模型预测轨迹跟踪控制器Fig.7 Model predictive trajectory tracking controller with feedback correction

3.2 约束条件

履带车辆由直流电机通过传动系统对左、右两侧的主动轮进行驱动，因此需要考虑轨迹跟踪过程中的执行器饱和约束，约束表达形式为

(9)

式中：umin(k)和umax(k)分别表示控制量的最小约束和最大约束。

另外，考虑参考路径的边界约束，对履带车辆的状态量进行约束

(10)

式中：ηmin(k)和ηmax(k)分别表示状态量的最小约束和最大约束。

3.3 模型预测控制问题

目标函数要能够保证智能车辆快速且平稳的追踪上期望轨迹。因此，需要加入对系统状态量的偏差和控制量的优化，采用如下形式的目标函数：

(11)

4 实车试验

试验主要针对以下两个方面：基于3次Bezier曲线的自适应参考路径拟合算法的拟合效果；本文提出的基于模型预测的轨迹跟踪控制器对参考轨迹跟踪的跟踪能力。

4.1 试验平台

无人驾驶履带车辆平台以及试验路面如图8所示。平台安装的数据采集系统主要包括在主动轮上的轮速传感器、高精度差分全球定位系统(DGPS)、惯性测量单元。其中：轮速传感器测量两侧主动轮转速，经换算可以得到左、右侧履带的线速度；DGPS 系统用来测量车辆的运动轨迹，能测量其在大地坐标系下的坐标值; 惯性测量单元用来测量车辆的航向。履带车辆及基于模型预测的轨迹跟踪控制器参数如表1所示。

表1 履带车辆及模型预测轨迹跟踪控制器的参数

4.2 自适应参考路径拟合算法效果

采集越野环境下的参考路径以及经过基于3次Bezier曲线的自适应参考路径拟合后的路径如图9(a)所示。

图9 越野环境下参考路径的自适应拟合结果Fig.9 Adaptive fitting results of reference paths in off-road environment

由图9(a)可以看出，拟合后得到的路径与给定的参考路径基本重合。图9中+表示Bezier曲线的起点P0和终点P3，·表示Bezier曲线的中间控制点P1和P2. 通过自适应参考路径拟合可以看出，为满足拟合精度阈值要求，该测试道路一共被划分为12段，每段最后一个控制点(即终点)和下一段的起始控制点(即起点)重合。将大量路径点通过少数控制点进行表征，使得该自适应参考路径拟合算法能够以使用较少的控制点实现对参考路径的准确描述。图9(b)显示参考路径的拟合残差保持在0.25 m以下，满足了曲线拟合精度β约束。根据拟合后得到的曲线方程可以计算出参考路径切线方向和参考曲率，为更准确的轨迹跟踪提供了重要信息。

4.3 无人驾驶履带车辆越野道路跟踪测试

将本文所提出的无人驾驶履带车辆轨迹跟踪控制器用于实车越野环境测试，在试验中无人驾驶履带车辆能够适应各种复杂的越野环境，保持可靠的运行。路径跟踪结果如图10所示。

图10 无人驾驶履带车辆越野环境下轨迹跟踪结果Fig.10 Track tracking results of unmanned tracked vehicle in off-road environment

将本文所提出的无人驾驶履带车辆轨迹跟踪控制器用于实车越野环境测试。为了分析引入反馈校正对轨迹跟踪器的影响，分别使用以下两种控制器跟踪图10所示的参考路径：

1)带反馈校正的模型预测轨迹跟踪控制器，记为控制器1；

2)不带反馈校正的模型预测轨迹跟踪控制器，记为控制器2.

在测试过程中，将期望车速设置为10 km/h，结果显示无人驾驶履带车辆能够适应各种复杂的越野环境，保持可靠的运行。

由图10可以看出，本文提出的基于模型预测的轨迹跟踪算法能够很好地跟随参考路径。通过对比跟踪结果，相比于未采用反馈校正的模型预测轨迹跟踪控制器(控制器2)，采用反馈校正的轨迹跟踪控制器(控制器1)能够更好地跟踪参考路径，证明了引入反馈校正能够在一定程度上抑制由建模误差和外部干扰等因素所引起的跟踪误差，提高跟踪精度。

图10和图11给出了无人驾驶履带车辆越野环境下轨迹跟踪结果，从中可以看出通过在模型预测控制器中加入状态反馈，在求解时间无明显提升的状态下可有效地提高跟踪精度。由表2可以看出，跟踪误差平均值在无状态反馈时为0.527 4 m，有状态反馈时为0.442 7 m，通过状态反馈，跟踪误差平均值减少了16%. 从图10(a)可见，尤其在转弯工况下可快速进行路径跟踪。实车试验证实该方法可应用于实车测试，通过状态反馈可有效提高跟踪精度。

图11 轨迹跟踪计算时间Fig.11 Calculational time of trajectory tracking

表2 无人驾驶履带车辆越野环境下轨迹跟踪误差统计

5 结论

1)本文建立了基于瞬时转向中心的履带车辆运动学模型，提出了一种基于3次Bezier曲线的参考路径自适应拟合方法，从而能够充分利用参考路径的曲率信息，实现更精确的轨迹跟踪。

2)考虑履带车辆的环境约束、执行机构饱和约束，设计了基于模型预测的轨迹跟踪控制器，系统地处理车辆模型非线性以及多约束优化问题。针对履带车辆的建模误差及外部干扰，引入了反馈校正因子进行抑制。实车测试证明，本文算法可以实时地在非线性模型和多约束下得到最优运动轨迹和控制序列，并且能有效抑制建模误差及外部干扰的影响。