多参数优化MPC的自动驾驶轨迹跟踪控制

李学慧，苏 振，张俊友

（山东科技大学交通学院，山东 青岛 266590）

0 引言

轨迹跟踪控制是实现车辆自动驾驶的关键技术之一［1］，其原理是将车辆当前的实际位姿与目标位姿进行比较，并转换到车身坐标系，得到横向误差，通过最小化横向误差计算得到前轮转角，并作用于执行机构，使车辆沿着目标路径行驶［2］。

在轨迹跟踪控制中使用较多的控制算法有：PID控制、模型预测控制、预瞄控制和线性二次型调节器等［3］。模型预测控制能够结合被控系统的动态模型与系统当前的状态，预测系统未来的输出，通过求解含约束条件的最优控制问题，获得系统的最优控制输入，并且具有处理多约束优化问题的能力［4］，更适用于复杂多变的自动驾驶应用场景。车辆在高速、低附着路面和大曲率路径工况下，传统MPC算法处理能力有限，会出现跟踪误差大、稳定性差等现象。近年来，通过改进MPC算法提高自动驾驶轨迹跟踪精度和稳定性成为国内外研究热点问题［5］。

改进MPC算法主要有参数优化和与其他算法相结合2种方式。通过参数优化改进的MPC算法可以不断调整预测时域、控制时域和调整权重矩阵，从而获得相对最优解，更好地作用于被控系统。李培庆等［6］使用权重矩阵自适应的MPC控制器，目标函数的权重矩阵根据参考路径的曲率变化进行优化调节，提高了轨迹跟踪精度和稳定性，但是没有针对不同速度工况进行优化。李耀华等［7］建立了一种智能车轨迹跟踪性能综合评价指标，制定了不同车速下的预测时域和控制时域参数调节策略，获取最优预测时域与控制时域，结合规划层和控制层解决过度避障问题，减小跟踪误差。王银等［8］使用自适应轨迹跟踪控制策略，预测时域根据车速变化进行自适应优化调节，提高了低附着系数路面工况下车辆变速行驶时的轨迹跟踪精度和稳定性。预测时域的产生是通过三次多项式拟合4组数据得到，由于数据较少，不能得到最优预测时域。Lin等［9］根据横向速度和纵向速度变化，通过模糊控制调节预测时域和控制时域参数，并利用余弦相似度触发状态矩阵更新机制，验证了该策略在变速工况下具有良好的适应性和跟踪精度。Wang等［10］使用Mamdani模糊自适应算法对MPC控制器权重矩阵进行优化调节，与传统MPC算法相比，在跟踪精度和转向平顺性方面具有更好的跟踪性能。李韶华等［11］根据横向误差和横摆角误差使用Takagi-Sugeno模糊变权重MPC控制算法，在线优化MPC权重矩阵，测试了该算法在不同速度下的跟踪效果，但是没有根据不同速度对时域进行优化。

在不同速度工况下，仅调节模型预测控制器的权重矩阵，能够提高轨迹跟踪精度，但受时域的限制，优化幅度有限，如果在此基础上同时调节时域，会进一步提高跟踪精度。根据车辆行驶速度、横向位置误差和横摆角误差作为模糊输入，利用模糊控制对MPC控制器的预测时域、控制时域和权重矩阵等多个参数进行实时优化，通过权重矩阵优化提高跟踪精度，同时进行时域优化，进一步提高车辆在不同速度工况下的适应性。

1 无人车辆系统建模

汽车是一个复杂的非线性多约束系统，在研究车辆运动控制相关的问题时，为了降低系统的复杂程度，提高算法的运算效率，在满足车辆自身的结构约束和控制算法需求的前提下，一般会忽略部分非研究重点的因素［12］，本文中假设：忽略车辆的垂向运动，轮胎受力的横纵向耦合关系，载荷的左右转移以及横纵向空气动力学效应；质心侧偏角和轮胎滑移率较小；车辆各个轮胎受力均处于线性区域。基于上述假设，将车辆简化为图1所示的3自由度动力学模型。

图1 车辆3自由度模型示意图

图1中，a和b分别为车辆质心与前轴和后轴的距离；v表示车辆质心速度；φ表示车辆横摆角；˙φ表示车辆横摆角速度；δf表示车辆的前轮转角；β表示质心侧偏角；x和y为车身坐标系；X和Y表示大地坐标系；αf和αr分别为前后轮胎侧偏角；Fyf表示前轮胎侧向力的合力；Fyr表示后轮胎侧向力的合力；Fxf表示前轮胎纵向力的合力；Fxr表示后轮胎纵向力的合力。

简化后的车辆动力学方程如下：

式中：m表示车辆质量；Iz表示车辆的转动惯量；Clf和Clr分别表示车辆前后轮胎的纵向刚度；Ccf和Ccr分别表示车辆前后轮的侧偏刚度；sf和sr分别表示前后轮滑移率。

由车身坐标系和大地坐标系转换得到：

此时可以将被控系统描述为如下的状态空间表达式：

2 轨迹跟踪控制器的设计

模型预测控制属于过程控制类策略，在每一个控制周期内，对有限时域内开环控制问题进行优化求解，并不断地滚动优化实现反馈控制。将车辆非线性动力学模型线性化，然后离散化，并加入约束条件，转化为二次规划问题求解。

2.1 非线性系统线性化与离散化

对系统的状态方程＝f（ξ，u）在参考值ξ＝ξ0，u＝u0处利用泰勒展开式，对车辆非线性动力学模型线性化处理后，得到线性时变方程：

再利用向前欧拉法进行离散化后可得：

式中：Ak，t＝I＋TAt，0，Bk，t＝TBt，0，Dk，t＝TDt，0，I为单位矩阵，T为采样周期。

令u（k）＝u（k－1）＋Δu（k），Δu（k）为控制增量，定义新的状态变量ξ～（k）＝［ξ（k），u（k－1）］T，将式（5）转化为以控制增量为输入的状态方程并生成新的状态空间表达式：

自动驾驶轨迹跟踪控制的目标是尽量减小车辆实际行驶轨迹与参考轨迹的误差，使车辆平稳行驶。所以目标函数既要能表征跟踪精度，又要能表征跟踪稳定性［13］。采用控制增量作为状态量，并加入松弛因子ε，目标函数如式（7）所示。

式中：η（·｜k）表示k时刻之后一段时间内系统的输出；ηref（·｜k）是η（·｜k）的参考值；Δu为前轮转角增量；ρ为权重系数；ε为松弛因子；Np为预测时域，Nc为控制时域；Q和R为权重矩阵。表达式如下：

在目标函数中还需计算控制系统未来时刻的输出，系统的未来时刻输出形式如下：

2.2 转化为二次规划问题求解

将预测模型和目标函数相结合，构建轨迹跟踪二次规划问题并设置约束条件，利用Matlab中的求解器quadprog进行求解［14］。

式中：x表示返回值向量；H表示二次目标矩阵，是实对称矩阵；f表示线性目标向量；E＝Ψkξ（k）＋ΓkΦ－Yref（k），Yref（k）是Y（k）的参考值。对式（13）进行求解，获得控制输入增量序列Δ＝将该序列中第一个元素取出，当前时刻控制量与该元素相加后作用于被控系统，即u（k＋1）＝u（k）＋Δ。

在设计模型预测控制器时，需要对输出量、控制量和控制量增量进行约束。输出量根据车辆参考轨迹设置；对于控制量和控制量增量即车辆前轮转角和前轮转角增量约束可设置为：

2.3 优化方案

将车辆行驶速度、横向位置误差和横摆角误差作为模糊输入，利用模糊控制对MPC控制器多个参数进行优化，在速度不变时通过调节权重矩阵提高轨迹跟踪精度，在不同速度工况下通过调节预测时域和控制时域对控制器进一步优化，实现MPC控制器多参数调节，轨迹跟踪控制器架构如图2所示。

图2 轨迹跟踪控制器架构

2.3.1 权重矩阵优化

在同一速度工况下采用横向误差ΔY和横摆角误差Δφ作为Mamdani模糊控制［10］输入，输出权重矩阵调节系数rφ、rY和rR，通过式（15）计算改变权重矩阵Q和R，式（15）中设置初始值为：Qφ0＝200 000，QY0＝90 000，Rδ0＝500 000。Mamdani模糊控制输入和输出变量的隶属度如图3和图4所示，Δφ的论域设置为［－15，15］，ΔY的论域设置为［－6，6］，rφ、rY和rR的论域设置为［0，1］。

图3 输入变量隶属度

图4 输出变量隶属度

模糊规则设置如表1—表3所示，模糊输入输出三维图如图5所示。

表1 rφ模糊规则

表2 rY模糊规则

表3 rR模糊规则

图5 Mamdani模糊输入输出三维图

2.3.2 时域优化

MPC控制器中的预测时域与控制时域对轨迹跟踪效果的影响如图6所示。若目标函数中其他参数不变，当预测时域Np过大时，控制器可以预测更远的距离，会考虑到更远位置处的偏差，从而削弱对车辆当前位置偏差的控制，导致过早转向，出现距离车辆当前位置较近处的跟踪误差较大的现象；当预测时域Np过小时，预测的未来车辆状态信息过少，车辆无法及时转向，容易失控。当控制时域Nc增大时，有利于控制灵敏度和精度，但会降低系统稳定性和实时性；当控制时域Nc减小时，情况则相反。

图6 预测时域和控制时域对轨迹跟踪的影响

为了使车辆在不同速度工况下控制器具有最优时域参数，设置预测时域初始值Np0，控制时域初始值Nc0，通过Takagi－Sugeno模糊控制获得系数rNp和rNc，使用式（16）计算Np和Nc。因为系数rNp和rNc的论域设置为［0，1］，Np和Nc的值不超过Np0和Nc0，所以Np0和Nc0的设置不能太小，结合相关文献，并反复仿真实验后选取Np0＝33、Nc0＝16。

Takagi-Sugeno模糊控制的输入为速度v，输出为系数rNp和rNc，速度v论域设置为［0，12］，速度v论域与真实车速v*的数量关系为v＝v*／10，v*单位为km／h，速度v隶属度如图7所示。T-S模糊规则如表4所示，时域调节模糊规则二维关系如图8所示。

表4 时域调节模糊规则

图7 速度隶属度

图8 Takagi-Sugeno模糊输出二维图

3 仿真验证

在自动驾驶车辆行驶过程中，需要将可行性、安全性和稳定性作为优化目标。可行性表现为车辆前轮转角控制在规定范围，安全性表现为车辆行驶过程中轨迹跟踪精度，稳定性表现为车辆行驶过程中横摆角误差的变化。为了检验改进的MPC控制器的跟踪效果，使用Matlab和Carsim进行轨迹跟踪控制联合仿真，在Matlab中搭建MPC控制器，输出控制量作用于Carsim，Carsim将车辆实时状态变量输出作用于Matlab。

3.1 工况选择

参考轨迹选择双移线方程，路径设置如下：

式中：Yref为横向参考坐标；φref为参考横摆角；Xref为参考纵向坐标；dx1、dx2、dy1、dy2均为轨迹参数。

3.2 仿真参数设置

MPC传统算法使用如表5所示的5个固定参数，MPC多参数调节算法对这5个参数进行实时优化，MPC仅时域调节算法只优化Np和Nc，MPC仅权重调节算法只优化Qφ、QY和Rδ。Carsim中均使用B-Class车型，车辆参数如表6所示。

表5 模型预测控制器参数

表6 车辆主要参数

3.3 仿真结果评价

在高附着系数μ＝0.8的路面上进行36、54、72 km／h三种典型车速仿真实验；在低附着系数μ＝0.4的路面进行54 km／h车速仿真实验。将使用改进MPC控制器的车辆与传统MPC控制器的车辆进行仿真对比。

3.3.1 高附着路面仿真分析

1）横向误差对比。不同车速下轨迹如图9所示，在X＝53 m和85 m附近大曲率路径处，采用MPC多参数优化算法的控制器具有更高的跟踪精度，采用其余3种算法的控制器都由于过早转向导致跟踪误差较大。为了更清晰地观察横向轨迹误差大小的变化，设置ΔY＝Yref－Y，如图10所示，可以直观地显示横向误差的变化。

图9 不同车速下轨迹曲线

图10 横向误差变化曲线

在测试的速度工况下，随着速度提高，采用MPC多参数优化算法的控制器在减小横向误差方面优于采用MPC仅时域调节和仅权重调节的控制器，特别是在72 km／h工况下，MPC多参数优化算法优势更加明显。

对横向误差仿真结果进行数据统计，统计结果如表7所示，数据表明：在减小横向误差方面，不同速度工况下，采用MPC多参数优化算法的控制器均表现最优，平均优化比例可达27.4%。

表7 最大横向误差

2）横摆角误差对比。横摆角误差可以反映车辆行驶的稳定性，为了清晰地观察横摆角误差大小及误差变化，设置Δφ＝φref－φ，横摆角误差变化如图11所示。在低速36 km／h时，4种控制器的横摆角误差变化区别不明显；在54 km／h时，采用MPC多参数优化算法控制器的车辆行驶过程中横摆角误差较小；在72 km／h时，4种MPC控制器的车辆横摆角误差曲线都出现明显波动，采用MPC多参数优化算法控制器的车辆行驶过程中横摆角波动幅度较小。

图11 横摆角误差变化

对横摆角误差仿真结果进行数据统计，统计结果如表8所示，在减小最大横摆角误差方面，3种改进算法都比传统算法有优势，采用MPC多参数优化算法的控制器在减小最大横摆角误差方面表现最优，车辆行驶稳定性最好，特别是72 km／h的时候，最大横摆角误差优化明显，误差在1°以内。

表8 最大横摆角误差

3）欧氏距离对比。为了能够评价轨迹跟踪全过程的整体误差，基于欧式距离的相似度算法［15］对采用不同MPC控制器的车辆实际行驶轨迹与参考轨迹进行对比。欧式距离模型如图12所示，计算Ni和Mi两点间的欧氏距离：

图12 欧式距离模型

实际轨迹与参考轨迹的欧式距离可表示为：

欧氏距离综合考虑轨迹的整体误差，用于评价跟踪效果更准确，车辆在双移线路径行驶时，跟踪误差主要分布在纵向位移0～100 m处，在此范围内均匀取点100个，即式（19）中n＝100。对欧氏距离计算结果进行数据统计，统计结果如表9所示，数据表明：采用MPC多参数优化算法控制器的欧氏距离明显低于其余3种控制器，平均优化比例可达16.5%。

表9 欧氏距离

3.3.2 低附着路面仿真分析

为了验证多参数优化MPC控制器在低附着路面工况下的优势，选取附着系数μ＝0.4，车速为54 km／h进行仿真实验。低附着路面仿真结果如图13所示，将仿真结果进行数据统计，统计结果如表10—表12所示。仿真结果表明：与其余3种控制算法相比，采用MPC多参数优化算法控制器的车辆行驶轨迹更接近参考轨迹，且横摆角误差波动较小，在低附着路面工况下具有良好的操纵稳定性；采用仅时域优化算法的MPC控制器由于过度减小最大横向误差，导致轨迹在X＝85 m附近出现明显波动，且横摆角误差在X＝90 m附近出现明显波动，操纵稳定性较差。采用MPC多参数优化算法控制器的车辆行驶轨迹具有更小的欧氏距离，轨迹跟踪整体误差减小。

表10 最大横向误差

表11 最大横摆角误差

表12 欧氏距离

图13 低附着路面仿真结果

4 结论

1）针对自动驾驶车辆横向控制在大曲率路径处跟踪误差较大的问题，提出了多参数优化MPC的自动驾驶轨迹跟踪控制策略，进行了仿真验证。

2）采用MPC多参数优化算法的控制器在高附着路面，能够更好地减小最大横向误差，提高跟踪精度，保持良好的操纵稳定性，在车速72 km／h双移线工况下将最大横摆角误差控制在1°以内。

3）采用MPC多参数优化算法的控制器在低附着路面，能够更好地平衡跟踪精度与操纵稳定性，避免因提高跟踪精度而降低操纵稳定性，在车速为54 km／h工况下，比MPC传统算法的最大横摆角误差减小27.3%。