低速自动驾驶横向跟踪控制研究

唐 坤,曹志雄

(北京天隼图像技术有限公司,北京 100089)

0 引 言

低速自动驾驶车辆与普通高速自动驾驶车辆相比，存在不同的技术特点：(1)低速自动驾驶车辆的行驶速度远小于高速车辆，因此在障碍物的检测距离、控制周期等方面均有较大不同；(2)低速车辆行驶空间相对较小、车道标识不够明确、障碍物对象较为复杂，因此，其路径跟踪控制的精准度要求较高；(3)低速车辆的线控底盘多为自主研发，给控制增添了许多不确定因素[1]。

针对车辆的横向运动控制，国内外研究机构进行了各种不同的研究。日产公司采用经典的PID控制构建横向前馈-反馈控制器解决横向控制实时性的问题[2]；Eom等[3]提出基于三自由度车辆模型的鲁棒H∞横向控制算法；Netto等[4]提出了基于视觉的智能车辆横向自适应控制器；Falcone等[5]针对智能车辆主动转向控制问题，提出了横向模型预测控制算法；另外，模糊控制、滑模控制、神经网络控制、迭代学习与深度学习等也在车辆横向运动控制研究中有广泛应用[6]。

结合低速自动驾驶车辆行驶环境和自身结构的特点，以及横向控制的特点，本文设计了一种纯跟踪(PP)控制与模型预测控制(MPC)联合的控制器，以此满足横向跟踪控制的快速性、稳定性以及精确度。

1 系统模型的建立

1.1 试验样车介绍

本文的车辆仿真模型建立以及实车测试均以北京天隼图像技术有限公司(简称“天隼”)自主研发的FELIS ACH201小型智能化线控底盘为核心，如图1所示，该底盘架构与乘用车保持一致，采用CAN总线通信，由整车控制器对转向系、驱动系以及制动系进行指令解析和转发，实现整车控制总线化。

图1 FELIS AHC201线控底盘

1.2 转向控制系统模型

FELIS AHC201型线控底盘的转向控制系统是一个由转向电机及其底层控制器、霍尔元件、编码器等模块构成的三闭环控制系统，即位置环、速度环以及电流环，其结构如图2所示。上层控制器输出的转向电机期望位置作为底层控制器输入，霍尔元件及编码器实时检测转向电机的位置、速度以及电流，底层控制器根据期望值和实时反馈值调节转向电机的输出位置，实现对转向电机期望位置的实时动态跟踪。

图2 转向控制系统结构

底层控制器3个闭环均采用增量PID控制，并对转向位置、转向速度以及电机电流进行有效安全约束。通过对控制参数的整定，该底层控制器具有良好的跟踪效果，如图3所示，最大期望位置为8 000°，对应车轮转向角为30°，转向所需时间为1.5 s，此过程最大延时时间为15 ms，最大稳态偏差为15°，误差率为0.18%，满足设计要求。同时设置安全约束条件：位置极限值为±9 130°，转速极限为±1 400°/s，电流极限为±30 A。

图3 位置跟踪测试结果

1.3 车辆动力学模型

因为本文主要侧重研究车辆对目标路径的跟踪能力，所以将车辆模型简化为二自由度单车模型，其几何关系如图4所示。根据牛顿力学关系以及轮胎的侧偏力与侧偏角的线性关系，车辆的横向动力学模型以及车身坐标与大地坐标系的转换关系为

图4 车辆单车模型

(1)

式中：m为整车质量；vx为车辆的纵向速度；vy为车辆的横向速度；ψ为质点横摆角；δf为车辆的前轮转向角；lf、lr为质心到前后轴的距离；Cf、Cr为前后轮胎的侧偏刚度；Iz为车辆绕z轴的转动惯量；X、Y为车辆在大地坐标系OXY下车辆参考点位置。

1.4 车辆运动学模型

由于低速自动驾驶车辆的工作车速较低，其运动学特性比动力学特性更为明显,且动力学特性较稳定。故基于车辆运动学模型设计的路径跟踪控制器具备可靠的控制性能。在此同样选用图4所示单车模型建立车辆的运动学模型[7]。以车辆后轴中心(Xr，Yr)为参考点，在低速工况下，质心侧偏角β极小，可假设为0°，车速v近似等于车辆的纵向速度vx，后轮转向角δr恒为0°，l为轴距。则在大地坐标系OXY下，车辆运动学方程[8]：

(2)

2 控制器设计

2.1 控制器整体结构

针对横向运动控制研究的展开前提是车辆的初始位置处于目标路径上，或处于目标路径的附近。但在实际工况中，需要考虑车辆与目标路径的初始位置存在较大跟踪偏差的情况。

MPC作为一种具有多种模式约束的实时在线控制算法，能够实现较高的控制精度。但当车辆与目标路径跟踪偏差较大时，其调节时间较长且波动性较大，尤其在低速自动驾驶控制中存在失控的可能。而PP控制能够实现对目标路径快速跟踪，但在跟踪精度要求较高的工况下，尤其曲率较大的路段，其控制精度明显弱于MPC。因此，通过对上述控制算法特点的总结，并结合低速自动驾驶车辆工作环境和自身结构的特点，设计了基于MPC与PP控制相结合的联合控制器，其整体结构如图5所示。MPC模式以车辆运动学模型作为预测模型，根据目标路径与车辆当前状态信息，计算输出前轮的期望转向角δ1；PP控制模式根据目标路径、车辆当前位置信息以及由当前车速确定的预瞄距离，计算输出前轮的期望转向角δ2；模式选择器根据目标路径与车辆当前位姿信息(位置与方向)，选择相应的控制模式；稳定控制器根据模式标志mod判断是否发生了模式切换，若发生模式的切换，为了防止控制器的输出产生较大的跃变，造成系统产生剧烈振荡，起动模糊控制器，对2种控制模式输出的前轮期望转向角进行加权计算，约束期望转向角的输出幅值，直至切换结束，从而实现平滑切换；根据阿克曼转向总成的逆模型推导前轮转向角与转向电机位置之间的几何关系，确定转向电机的期望位置。

图5 控制器整体结构框

2.2 基于MPC算法的横向控制

针对低速自动驾驶车辆的行驶特点，本文选用车辆运动学模型作为预测模型，但是非线性的模型预测控制计算量庞大，求解速度相对缓慢，难以满足系统对于实时性的要求，因此需要进行线性化处理，以保证控制的实时性[9-10]。

2.2.1 线性时变模型

由式(2)可知，车辆的非线性运动学模型的一般状态方程可写成：

(3)

式中：状态量ξ=[XrYrψ]T，控制量u=[vδr]。

可以将目标路径看作一组已知的车辆状态信息，进而能够应用运动学方程进行描述，将式(3)在目标路径上的某一点(ξ0，u0)处采用泰勒级数表示并忽略高阶项，结合该点处的状态方程，即可求得线性化的误差模型，即：

(4)

应用一阶差商方法对式(4)进行离散化处理，即：

(5)

式中：Ak,t=I+TA(t);Bk,t=TB(t);T为采样周期；I为单位矩阵。

2.2.2 预测模型

通过上述离散线性误差模型，能够计算车辆未来时刻的状态量与输出量。为了便于对控制增量进行计算，将式(5)进行如下转换，令：

(6)

得到一个新的状态空间方程：

(7)

通过式(7)可知，若是已知系统当前时刻的状态量以及未来一段时域内非零的控制增量，进行迭代计算，能够预测未来这段时域内的状态量和输出量。

2.2.3 优化求解

在实际控制中，系统的控制增量是未知的，可以通过设置目标函数，并对其进行滚动寻优，来获得一段控制时域内的控制增量的序列。本文中目标函数设置为

(8)

式中：ηref为未来第i时刻的目标输出；Np为预测时域；Nc为控制时域；R、Q为权重矩阵；ρ为权重因子；ε为松弛因子。

式(8)中：第一项反映了控制系统对目标路径的跟踪能力；第二项反映了前轮转向过程的平稳性；第三项松弛因子的加入能够保证在未来预测时域内的每一刻均有可行解。因此，在设定预测时域内，该目标函数能够实现对目标路径的精确、平稳跟踪。

为了方便求解，需要将目标函数转化为标准二次规划的问题，将式(7)代入式(8)中，可得：

(9)

在实际控制过程中，为了增强跟踪过程准确性、稳定性，往往需要考虑对控制量以及控制增量设置约束条件。在本文设计中，在控制周期设置为0.05 s的前提下，根据AHC201的纵向跟踪能力测试，设定车速控制的约束条件为

车速偏差：-0.05 m/s

车速增量：-0.025 m/s<Δv<0.025 m/s。

根据AHC201的转向能力测试，设定前轮转向控制的约束条件为

(10)

式中：vt为期望车速；Δ为每个控制周期的增量。

综上所述，模型预测的求解问题可以描述为在满足约束条件的前提下，求解每一个采样周期内使式(9)最小的最优控制增量序列：

将该序列的第一个元素用于横向跟踪系统的实际控制，进而求得期望控制量，即：

(11)

而后进入下一采样周期，求取新的最优控制增量序列，最终实现对目标路径的跟踪控制。

2.3 基于PP算法的横向控制

2.3.1 基于单车模型的PP算法原理

PP控制广泛应用于车辆的路径跟踪控制中。文中PP控制基于单车模型，以车辆后轴为切点，车辆纵向车身为切线，通过控制前轮转向角，控制车辆沿着一条经过预瞄点的圆弧行驶，如图6所示[11-12]。

图6 PP算法原理图

图6中,点(gx，gy)为目标路径上的预瞄点，(cx，cy)为车辆的当前位置，δ为前轮的转向角，α为预瞄角，ld为预瞄距离，L为轴距，R为车辆的转向半径。根据正弦定理，可得：

(12)

根据简化的阿克曼转向几何，前轮转向角与车辆转弯半径存在如下关系：

(13)

联立式(12)与式(13)，可得基于PP控制的前轮期望转向角为

(14)

2.3.2 预瞄距离的确定

预瞄距离的确定是PP控制算法能否实现路径准确跟踪的关键，如果预瞄距离过短，易造成系统产生持续的振荡，系统的稳定性下降；如果预瞄距离过长，则跟踪精度下降，尤其在跟踪曲率较大处时，容易产生“抄近路”现象。经分析，预瞄距离与车辆实时的速度、加速度、初始预瞄距离以及转弯半径等参数存在一定的数学关系，通常用于计算预瞄距离的经验公式为[11]

(15)

式中：v为车辆的行驶速度；amax为车辆最大制动加速度；k为速度系数，Rmin为车辆最小的行驶半径。

式(15)中，第1项表示车辆制动距离；第2项表示车辆的反应行驶距离；AHC201的设计参数确定，其最大制动加速度为0.5 m/s2，k为0.5，Rmin为1.8 m。当车速处于3～8 km/h时，系统对于预瞄距离的变化反应较为明显，因此，本文设定预瞄距离数学关系式为

(16)

2.3.3 预瞄点的确定

预瞄距离确定后，根据预瞄距离寻找目标路径上的预瞄点成为整个算法实现的重要环节。本文采用弦长累加的方法确定目标点。具体预瞄点的确定方法如图7所示。

图7 预瞄点的确认方法

寻找目标路径上距离车辆当前位置最近的点B0，然后将B0作为起点，顺序求取每个路径点与下一路径点的距离并进行累加，此累加距离记为ls，即

(17)

当累加至点Bi时，ls(i)大于预瞄距离ld，终止累加，后比较ls(i)与ls(i-1)，选取与ld误差较小的点，即为预瞄点。

2.4 切换控制

2.4.1 切换逻辑

当应用MPC模式进行跟踪控制时，距离偏差与方向偏差过大，易造成控制系统产生振荡，甚至失去跟踪能力。并且不同车速下，距离偏差与方向偏差的影响不尽相同。

本文通过设置不同车速、初始距离偏差以及方向偏差，运用MPC方式控制AHC201跟踪100 m直线路径的测试方法，测试能够保持系统稳定的最大距离偏差eymax及其对应的方向偏差eψmax，后进行数据拟合获得不同车速v下，eymax与eψmax对应关系如图8所示，由图8可知，同一车速下，随着eψmax增大，eymax逐渐减小；不同车速下，随着车速的增大，eymax与eψmax允许范围越来越小。

图8 车速、位置偏差与方向偏差阈值对应关系

对于图5所示的控制系统，本文选用位姿偏差(距离偏差与方向偏差)作为模式选择的指标，根据车辆的当前车速以及图8的对应关系，计算最大方向偏差eψmax为0时对应的最大距离偏差eymax(当前速度下最大的距离偏差)。再将当前的距离偏差ey与该值进行比较，若ey大于eymax，则选用PP控制模式，模式标志mod设为1；若ey小于eymax，则继续根据图8的对应关系以及当前的距离偏差ey。计算对应最大方向偏差eψmax，并将当前的方向偏差eψ与该值进行比较，若eψ大于eψmax，则选用PP控制模式，模式标志mod设为1；否则，在满足MPC约束条件的情况下，选用MPC模式，模式标志mod设为2。

2.4.2 模式切换控制

在发生模式切换时，为了避免控制量的输出产生跃变，引起系统发生瞬时振荡和扰动，设计模式切换控制器对跟踪控制器的输出进行约束[13]。

在图5的跟踪控制器中，当模式切换控制器根据模式标志mod判定发生模式切换时，跟踪控制器的期望输出δref按照下式进行加权处理：

δref=c1δ1+c2δ2

(18)

式中：δ1为切换前控制模式(记为模式1)输出的车轮期望转向角；δ2为切换后控制模式(记为模式2)输出的车轮期望转向角；c1为模式1期望输出的加权系数，c2为模式2期望输出的加权系数。切换前，由模式1完全控制，即c1为1，c2为0；切换后，由模式2完全控制，即c1为0，c2为1，故c1的变化范围为1～0，c2的变化范围为0～1，在切换过程中，模式切换控制器依据设计的模糊控制器实时的调整c1、c2。

上述模糊控制器以加权系数c1、c2作为输出，以切换前后2种控制模式期望输出量的偏差绝对值|e|及其变化率ec作为输入。根据AHC201的测量参数，模糊控制算法的输入量|e|的基本论域为[0°，35°]，模糊论域为{0，1，2，3}，对应的模糊子集为{ZO，PS，PM，PB}；其变化率ec的基本论域为[-25°，25°]，模糊论域为{-1，0，1}，对应的模糊子集为{N，ZO，P}，隶属度函数采用高斯型。模糊控制算法的输出c1、c2的基本论域为[0，1]，模糊论域为{0，1，2，3}，对应的模糊子集为{ZO，PS，PM，PB}，隶属度函数采用三角形。

该模糊控制对应的规则如表1和表2所示，当切换前后的偏差较大且有继续增大的趋势时，为了保证切换的平滑性，需要侧重继承切换前的控制模式；当切换前后的偏差较小时且有继续减小的趋势时，2种控制模式需要适当配合调整；当切换前后的偏差近似为零且能保持这种状态时，说明切换过程结束，切换前控制模式可以退出，切换后控制模式完全接管控制。

表1 加权系数c1模糊规则表

表2 加权系数c2模糊规则表

3 控制效果测试

3.1 仿真测试

3.1.1 仿真平台的介绍

为验证上述联合控制器在路径跟踪控制中的控制效果，本文采用MATLAB/Simulink与Carsim联合仿真的方法进行仿真测试。以AHC201型线控底盘为参考模型，进行数据的标定，其部分参数如表3所示。

表3 FELIS AHC201整车参数

3.1.2 仿真分析

针对静态障碍或者低速障碍进行避障换道是低速自动驾驶的一种典型工况[14-15]。本文采用一种避障换道路径作为仿真的目标路径，如图9所示。

图9 目标路径

重点检验在存在较大初始位姿偏差和曲率实时变化的路况条件下，控制器的跟踪效果，目标路径的方程为

(19)

设置该路径的附着系数为0.8，初始位置为(0 m，0 m)，初始方向为0°；车辆的初始位置为(-1.5 m，0 m)初始方向为-7°；车速设置为5.4 km/h。比较分析不同控制器作用下，跟踪控制的精确度、稳定性以及快速性。

仿真结果如图10所示，该结果表明，MPC的位姿实时处于不断微调状态，一旦位置或方向存在较大的偏差，其需要花费较长时间进行调整，在本次仿真中，由于初始位姿偏差较大，其位置调整长时间维持在-0.04～3.2 m，调整时间约为50 s，其方向调整长时间维持在0.01°～14.7°之间，调整时间约为30 s。PP控制虽实现了对目标路径的及时平稳的跟踪，但稳态偏差相对较大，尤其在曲率较大处，稳态位置偏差变化范围为0.06～0.12 m。联合控制很好地优化了上述2种控制的不足，在初始位姿偏差较大时，处于PP模式，实现了对目标路径及时平稳跟踪，待满足切换条件后，切换为MPC控制模式，实现对目标路径的精确跟踪，且方向的微调波动减小。通过对仿真结果的分析，联合控制算法相较于单一的MPC，跟踪的及时性、稳定性均得到了较好的改善，相较于单一的PP控制，精确度得到了良好优化。

图10 仿真效果对比

针对模式切换控制器控制效果验证，分别应用加入与未加入模式切换控制器的2种联合控制器进行了对比仿真，其仿真结果如图11所示。仿真结果表明，未加入控制器的跟踪路径在切换点处产生较大波动，其转向车轮横摆角速度与车身的侧向加速度产生了较为激烈的振荡，横摆角速度的波动范围为-7°/s～4.5°/s，侧向加速度的波动范围为-2.8～2.2 m/s2；在加入控制器后，其横摆角速度与侧向加速度的波动范围明显减小，最大波动分别为-3.6°/s～-2°/s 以及-0.4～0.2 m/s2。对比结果表明，该模式切换控制器能够较好的优化模式切换的鲁棒性。

图11 有无切换控制器控制效果对比

3.2 实车测试

为了验证上述横向跟踪控制器的实际控制效果，借助天隼自主研发的FELIS自动驾驶开发平台进行了实车测试，如图12所示。该开发平台以工控机为平台，利用高精度的RTK-GPS导航模块创建全局地图，进行车辆的实时定位与局部路径规划；利用由激光雷达、毫米波雷达以及双目视觉传感器组成的感知模块实时检测路况信息(障碍物、行人及道路边缘等)，完成SLAM建图及局部路径规划；利用车体传感器采集车速、加速度以及转角等车体信息，后通过融合算法将上述模块输出的数据信息进行数据融合，完成运动路径二次规划以及行为决策，并通过CAN总线建立的分布式通信网络发送给底层控制器，由底层控制器完成底层的路径跟踪。

图12 FELIS自动驾驶开发平台

本次实车测试中，以若干静止的车辆作为障碍物，完成图13(a)所示的避障路径的标定，以此作为实车测试的目标路径，测试环境为干燥沥青路面，微风，路宽2.7 m，目标车速设定为5.4 km/h。测试结果如图13所示，在整个测试过程中，车速稳定在4.9～5.8 km/h之间；由于存在较大的初始偏差，所以测试初期，控制器控制转向电机迅速转向，快速跟踪上目标路径，相应的位置偏差与方向偏差迅速减小；待达到稳定跟踪后，位置偏差的波动范围为-0.02～0.11 m，方向偏差的变化范围为-1.4°～2.5°，达到了预期的控制目标。

图13 实车测试结果

4 结 语

本文针对低速自动驾驶车辆横向路径跟踪过程中，位姿偏差较大的情况下，单一的控制算法无法同时满足精确度与稳定性的问题，结果MPC算法与PP控制算法的控制特点，设计了基于MPC算法与PP控制算法的联合控制器。

由于MPC算法模式下，随着车速的不同，对位置偏差和方向偏差容忍程度也不同，在不同车速条件下，横向偏差与方向偏差的数据进行拟合，以此作为模式切换的依据。同时设计了基于模糊控制的模式切换控制器，保证切换过程的稳定平滑。

以FELIS AHC201型线控底盘作为模型，搭建了基于MATLAB/Simulink与Carsim的联合仿真平台，分别应用不同控制算法进行了仿真测试，并对仿真结果进行了对比分析。最后应用FELIS自动驾驶开发平台进行了实车测试。仿真测试以及实车测试的结果均表明：本文所设计的联合控制器兼有MPC算法控制精度高以及PP控制跟踪速度快、稳定性好的特点。