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

孙浩 杜煜 卜德旭 刘浩栋

摘   要：针对汽车轨迹跟踪模型预测控制求解中存在的规模较大、求解效率较低的问题，提出一种基于时域分解的加速计算方法提高求解效率. 首先引入全局一致性变量，将模型预测控制中邻接控制周期的时域耦合约束转化为全局一致性约束，实现时域解耦;随后在交叉方向乘子法框架下推导了时域分解后优化问题的分块更新方法，并设计了分块更新数值求解问题的停止准则，从而将大规模优化问题转化为小规模子问题;最后搭建了Simulink-CarSim平台进行了算法的仿真验证. 仿真结果表明，在求解精度不变的情况下，求解耗时下降24.21%，从而实现模型预测控制问题的加速求解.

关键词：模型预测控制;自动驾驶汽车;轨迹跟踪;交叉方向乘子法;时域分解

中图分类号：U461.99                            文献标志码：A

Accelerated Solution Method for Vehicle Trajectory

Tracking Based on Model Predictive Control

SUN Hao1，2，DU Yu1，2?，BU Dexu3，LIU Haodong1，2

（1. Beijing Key Laboratory of Information Service Engineering，Beijing Union University，Beijing 100101，China;

2. College of Robotics，Beijing Union University，Beijing 100027，China;

3. College of Traffic and Transportation Engineering，Central South University，Changsha 410076，China）

Abstract：To solve the problems of large scale and low efficiency in the solution of intelligent vehicle trajectory tracking algorithm based on model predictive control，this paper proposes a time-domain splitting method to accelerate the calculation. First，global consistency variables are introduced to transform the time-domain coupling constraints of the adjacent control cycles into global consistency constraints，thus to achieve time-domain decoupling. Then，under the framework of Alternating Direction Method of Multipliers，the block updating method of optimization problem after time domain splitting is derived，and the stop criterion of block updating numerical solution is designed. Therefore，the large-scale optimization problem is converted into several small-scale sub-problems. Finally，the Simulink-CarSim platform is set up and the algorithm is verified by simulation. The simulation results show that，under the same accuracy of the solution，the efficiency of solution is improved by 24.21% on average with the proposed method，achieving accelerated solution for vehicle trajectory tracking based on model predictive control.

Key words：model predictive control; autonomous vehicles; path tracking; alternating direction method of multipliers; time splitting

自動驾驶汽车是提高交通系统安全、效率、经济等性能的有效途径，而轨迹跟踪是实现智能驾驶的核心技术之一[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）所示.

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

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

x（k+1） = Ad x（k） + Bd u（k）      （3）

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

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

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

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

1.2   时域分解

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

首先，以控制周期为单位对状态变量、控制变量进行分块表达. 对于所关注预测时域内的N个控制周期，按照次序给予编号1到N. 对于每个控制周期，该周期初始状态量、输入状态量和输出状态量可分别用x（t）

t表达，其中上角标（t）表示为变量所属的分块编号，且块内变量之间的关系由汽车动力学特性决定，如式（7）所示：

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

为解耦式（8）所示的时域耦合约束，引入全局一致性变量[z][～] = （z0，z1，…，zN）T  ，并使x与z之间满足如式（9）所示的关系，其中：（9a）表示分块（t）的输入变量与zt-1的一致性约束;（9b）表示分块（t）的输出变量与zt的一致性约束.

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

2   分块求解方法

2.1   问题求解

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

为了方便地将式（10）转换为ADMM标准构型，引入块变量：[x][～]t = （x（t）

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

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

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

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

更新块变量[x][～]：

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

2.2   停止准则

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

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

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

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

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求解器可以返回迭代次数，便于开展结果分析.

3.2   仿真结果及分析

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

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

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

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

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

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

最后，在计算平台相同的情况下（CPU：Intel i5

8 600 k;内存16 G），对比分析传统OMPC与TSMPC的求解效率，计算结果如图9所示，TSMPC的总运算量明显下降. 经计算可得OMPC的平均求解耗时为12.02 ms，TSMPC的平均耗时为9.11 ms，平均耗时下降24.21%.

4   结   论

针对传统研究中汽车轨迹跟踪的MPC求解效率低的问题，本文提出一种基于ADMM的时域分解加速求解方法，包括：1）基于汽车二自由度动力学模型的问题建模，并通过引入全局一致性变量将时域耦合约束转变为全局一致性约束; 2）推导了时域分

解问题的分块数值求解方法，建立了分块数值求解的停止准则，将大规模优化问题转化为小规模子问题;3）搭建Simulink-Carsim仿真平台，通过仿真对比证明本文所提出的算法可以降低24.21%的计算耗时.

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

参考文献

[1]    孙浩. 考虑交通车辆运动不确定性的轨迹规划方法研究[D]. 长春： 吉林大学汽车工程学院，2017：1—2.

SUN H. Research on vehicle trajectory planning considering traffic uncertainties [D]. Changchun：College of Automotive，Jilin University，2017：1—2. （In Chinese）