汽车轨迹跟踪模型预测控制的加速求解方法

孙浩，杜煜†，卜德旭，刘浩栋

（1.北京联合大学北京市信息服务工程重点实验室，北京 100101；2.北京联合大学机器人学院，北京 100027；3.中南大学交通运输工程学院，湖南长沙 410076）

自动驾驶汽车是提高交通系统安全、效率、经济等性能的有效途径，而轨迹跟踪是实现智能驾驶的核心技术之一[1]，其目标是在满足汽车复杂动力学约束、行驶安全性约束前提下，以较小的偏差、较高的舒适性跟踪所规划的轨迹[2].模型预测控制（Model Predictive Control，MPC）最早被应用在成本不敏感的大型工程中（如化工产业），并被认为是当年最具影响力的现代控制算法之一[3].MPC 通过引入参考模型，对约束的处理直接简单，不仅能够有效处理被控模型特性、约束带来影响，还能够通过滚动求解优化问题找到当前的最优控制量[4].基于上述优点，MPC 在汽车轨迹跟踪问题的研究中得到国内外研究者的广泛关注.

求解MPC 问题的主流思路是将其转化为二次规划（Quadratic Programming，QP）问题.其中，无约束MPC 求解相对简单.Xu 等以跟踪误差为性能指标实现AGV 的轨迹跟踪控制[5]，Shen 等则建立了综合考虑跟踪精度和舒适性的目标函数[6]，也有研究将输入量的边界建模为目标函数中代价值，构成“软约束”，但其数学原问题仍是无约束二次规划.无约束二次规划问题可通过梯度下降等方法直接求解，但其局限在于仅考虑车辆的运动及动力学约束，没有考虑运动过程中所受到的其它约束，局限性较强.因此，约束MPC 更具备实际意义.如Bo 等建立了基于动力学的增量模型，考虑了执行机构的动力学特性约束[7]；Cai 等则通过4 自由度车辆模型加入车辆运动的侧翻约束，提高车辆跟踪控制的安全性[8].约束MPC的数学原问题是约束QP 问题（MPC-QP 问题），需要通过数值优化方法计算求解.求解约束二次规划问题主要采用主动集法、内点法等方法，迭代次数较多.Wang 和Boyd 的研究表明，对于状态维数为m、控制输入维数为n、求解时域为T 的MPC 问题，状态转移形式及控制输入形式的MPC-QP 问题求解的时间复杂度分别为O（[T（n+m）]3）和O（[Tn]3）[9].综上，虽然MPC 在轨迹跟踪问题研究中得到广泛关注，但MPC-QP 问题存在规模大、求解所需的浮点运算多、求解效率低的缺点，制约了MPC 的性能.因此，提高轨迹跟踪MPC 问题的求解效率具有重要的意义.

针对上述问题，本文提出一种加速求解方法以提高求解效率.该方法通过交叉方向乘子法（Alternating Direction Method of Multipliers，ADMM）实现汽车轨迹跟踪问题的时域分解，将集中大规模的优化问题转为分块小规模优化问题，从而降低汽车轨迹跟踪MPC 问题的时间复杂度，实现求解加速.

1 模型预测控制问题的时域分解

1.1 问题建模

为了考虑车辆的动力学特性，MPC 控制器依赖如图1 所示的轨迹跟踪模型预测车辆的未来状态.其中，系统输入为前轮转角δ；系统状态量x=[y，θ，ω，β]T（在x 中，y 表示侧向偏差，θ 表示航向角，ω 表示横摆角速度，β 表示质心侧偏角）；系统的输入u=[δ].

图1 轨迹跟踪模型Fig.1 Trajectory tracking model

当汽车轮胎侧偏特性处于线性范围时，基于小角度假设可得到跟踪控制系统的线性连续状态方程，如式（1）所示.

式（1）中各矩阵如式（2）所示.其中，k1、k2分别为二自由度模型中车辆前、后轮等效侧偏刚度；a、b 分别为二自由度模型中车辆直线到前、后轴的距离.

那么，可得线性离散时间系统的状态空间模型，如式（3）所示.

且连续与离散系统矩阵转换关系如式（4）所示，其中Ts为离散系统的采样时间.

不失一般性，假设当前时刻系统状态为x0，根据离散系统的状态方程（3），在预测时域N 内系统的输入与系统状态之间的转移关系如图2 所示，其中x∈Rm，u∈Rn.

图2 预测时域状态转移示意图Fig.2 States transition in prediction horizon

设离散时刻t 下系统的参考状态为rt，那么综合考虑跟踪精度、舒适性的优化目标函数J 可用式（5）表示.式中Q∈Rm×m、R∈Rn×n分别为状态变量、输入变量的权重对角矩阵.

在轨迹跟踪问题中，代价函数越小越好.对式（5）进行展开，轨迹跟踪的MPC 问题可转化为如式（6）所示的约束二次规划问题.在目标函数中，省略了不影响极值求解的常数项.在约束条件中，（6a）表示汽车的动力学约束，（6b）表示不同时刻下状态变量及控制变量的可行域约束.可以看出，虽然目标函数J 在时域上解耦，但由于约束条件（6a）的存在（即t 周期的终止状态为t+1 周期的初始状态），并不能在求解过程中实现问题的分块解耦计算.

1.2 时域分解

为了实现问题的时域解耦，本文引入全局一致性变量，将汽车动力学带来的时域耦合约束转为全局一致性约束，从而实现问题的分块求解.图3 为ADMM 时域分解示意图.

图3 时域分解框图Fig.3 Time domain splitting framework

首先，以控制周期为单位对状态变量、控制变量进行分块表达.对于所关注预测时域内的N 个控制周期，按照次序给予编号1 到N.对于每个控制周期，该周期初始状态量、输入状态量和输出状态量可分别用表达，其中上角标（t）表示为变量所属的分块编号，且块内变量之间的关系由汽车动力学特性决定，如式（7）所示：

而块间关系由时域耦合约束决定，满足式（8）：

因此，联合式（7）～式（9），问题（6）可重写为如式（10）所示形式.此时，邻接块之间不存在直接关系，其耦合约束通过一致性变量间接表达，从而实现控制周期之间时域解耦.

2 分块求解方法

2.1 问题求解

综上所述，本文已经构建了MPC 问题的时域分解范式，本节的工作则是基于ADMM 求解时域分解后的问题.ADMM 的标准构型如式（11）所示.

为了方便地将式（10）转换为ADMM 标准构型，引入块变量对一个控制周期内所有变量统一表达.其中，块变量的下角标表示其所对应块编号，与块内变量的上角标（t）相同.问题（10）可被构建为ADMM 标准构型，如式（12）所示.

（12a）为系统的动力学约束；（12b）为系统的可行域约束；（12c）为一致性约束.式中变量的含义如下所示：

问题（12）的增广拉格朗日函数如式（14）所示，其中yt为拉格朗日对偶变量，ρ 为惩罚因子.

至此，本文已经完成MPC 问题的时域分解，并构建了其ADMM 的标准形式.求解ADMM 问题过程中，每个迭代周期需要依次更新x、z、y 变量.更新公式如式（15）～式（17）所示.

可以看出，块变量及对偶变量均可直接分块更新，且更新过程相对独立.全局一致性变量中的元素更新如式（18）所示，其中G0=[I，0，0]，G1=[0，0，I].全局一致性变量的更新过程仅涉及小规模矩阵的乘加运算，没有迭代，所以更新速度很快.

2.2 停止准则

在数值迭代求解过程中，当原始变量残差和对偶变量残差均足够小时，就可认为求解结果收敛.本文根据Boyd 的经典算法设计停止准则，将问题（12）视为一个整体设计停止准则，设该大问题停止条件所涉及的变量如式（19）所示.

问题的原始残差向量rk+1及对偶残差向量sk+1如式（20）所示：

其中，矩阵Ares、Bres的定义如下：

且Ares，Bres∈R（2m+n）（N+1）×（2m+n）（N+1）.

原始残差边界及对偶残差边界如式（22）所示.其中：m 为系统状态变量维数，n 为控制变量维数，N为预测时域.

联合式（20）（22），可得本文问题的停止准则为：

3 仿真结果与分析

3.1 仿真工况与参数

为了验证所提出方法的有效性，本文搭建了基于Simulink/CarSim 联合仿真平台，其中Simulink 用来构建控制策略，CarSim 用来提供动力学模型.仿真中所使用的动力学模型主要参数数值[10]如表1 所示.

基于相同的动力学模型，开展对原MPC（origin MPC，OMPC）和本文所提出的时域分解MPC（Time Splitting MPC，TSMPC）的仿真对比工作，OMPC 问题构型采用控制输入方式.算法所涉及的相关参数设置如表2 所示.为了保证仿真对比的有效性，本文仿真实验在使用相同的算法参数的同时，也采用相同的二次优化求解器OSQP[11]对OMPC 与TSMPC 进行求解.OSQP 求解器可以返回迭代次数，便于开展结果分析.

表1 动力学模型参数表Tab.1 Dynamic model parameters

表2 仿真参数Tab.2 Simulation parameters

3.2 仿真结果及分析

本文以典型单移线工况为例开展仿真实验，证明相比于传统的OMPC，本文所提出的TSMPC 能够有效降低求解的时间复杂度.

仿真实验中的前轮转角计算结果对比如图4 所示.由于采用了相同的数值求解停止参数，且OMPC与TSMPC 约束二次规划问题是等价的，所以两算法计算结果，即最优前轮转角完全相同，从而说明时域分解方法求解结果的正确性.

图4 计算结果对比Fig.4 Longitudinal motion in the simulation condition

TSMPC 方法的轨迹跟踪效果如图5 所示，其中实线为跟踪控制的车辆运动路径，点划线表示期望单移线路径.由跟踪效果可以看出，TSMPC 可以实现对期望轨迹的有效跟踪.

图5 轨迹跟踪结果Fig.5 Trajectory tracking results

具体地，OMPC 与TSMPC 的跟踪误差如图6 所示.由于仿真工况相同，最优控制输入相同，所以两算法的跟踪控制误差也相同，且最大误差约为0.023 m，方法能够保证较高的跟踪精度.

图6 轨迹跟踪误差Fig.6 Trajectory tracking error

TSMPC 及OMPC 算法仿真过程中车辆其他主要状态量，包括横摆角速度ω、航向角θ 如图7 所示.TSMPC 算法能够保证轨迹跟踪过程状态量处于合理的范围内.

在相同的求解精度下，OMPC 与TSMPC 中迭代次数对比如图8 所示.TSMPC 的迭代次数相对较高，这是因为时域耦合约束转为一致性约束后，相当于给全局一致性变量一个松弛范围.因此，相比于OMPC，TSMPC 的迭代求解需要在松弛范围的收敛过程中额外增加一致性变量，进而增加了迭代次数.

图7 横摆角速度及航向角Fig.7 Yaw rate and yaw angle

图8 迭代次数对比Fig.8 Contrast of iterations numbers

最后，在计算平台相同的情况下（CPU：Intel i5 8 600 k；内存16 G），对比分析传统OMPC 与TSMPC的求解效率，计算结果如图9 所示，TSMPC 的总运算量明显下降.经计算可得OMPC 的平均求解耗时为12.02 ms，TSMPC 的平均耗时为9.11 ms，平均耗时下降24.21%.

图9 时间耗时对比Fig.9 Contrast of time consuming

4 结论

针对传统研究中汽车轨迹跟踪的MPC 求解效率低的问题，本文提出一种基于ADMM 的时域分解加速求解方法，包括：1）基于汽车二自由度动力学模型的问题建模，并通过引入全局一致性变量将时域耦合约束转变为全局一致性约束；2）推导了时域分解问题的分块数值求解方法，建立了分块数值求解的停止准则，将大规模优化问题转化为小规模子问题；3）搭建Simulink-Carsim 仿真平台，通过仿真对比证明本文所提出的算法可以降低24.21%的计算耗时.

本文算法中一致性约束的引入增加了数值求解的迭代次数，未来工作可研究时域分解模块之间信息传递拓扑对收敛速度影响的机理，从而减少迭代次数.