⾃动驾驶（七⼗六）---------ApolloMPC之代码解析
最近⼀直在调LQR上车的效果，初见成效，所以也很好奇Apollo的MPC控制算法，没有项⽬驱动，只能靠⾃⼰的驱动⼒了，这⾥结合代码分析理论，详细梳理⼀遍apollo的MPC算法。
⾸先MPC的代码在apollo⽂件地址为：
1. 初始化
1. 加载MPC的参数和配置：，主要包含
vehicle_param、速度和转⾓限制等。
2. 车辆运动学模型及其离散化：
；包括矩阵A，矩阵B，矩阵C，矩阵K，矩阵R，矩阵Q，加载MPC增益调度序列，初始化滤波器。
其中调度增益gain scheduling是⼀种利⽤⼀系列线性控制器，来控制⾮线性系统的⽅法，类似于汽车标定中的map图。
2. 计算控制
计算控制的⼊⼝函数是：
这是MPC的核⼼函数，输⼊：当前车辆状态，⽬标轨迹；输出：⽅向盘转向⾓度，油门，制动。下⾯详细看⼀下这个函数：
计算纵向偏差，实现如下所⽰：
(1). ⾸先到离轨迹最近的点：
(2). 计算到最近点的误差：，包含 横向偏差，横向车速，纵向偏差，纵向车速。
(3). 计算轨迹上的预瞄点：  这⾥主要是⽤到预瞄点的曲率做为前馈。
更新车辆状态： 根据车速、车⾝⾓度计算车辆新的位置信息。重新计算横向偏差、横向偏差变化率、航向⾓偏差、航向⾓偏差变化率、位置偏差、速度偏差。
汽车
更新系数矩阵：  更新矩阵A，并采⽤双线性变换离散法进⾏离散化处理；更新矩阵C，并将其离散化处理；
计算前馈控制量： 通过预瞄点的曲率，结合轴距和速度公式，计算预瞄点对应的⽅向盘转⾓。
如果需要增加对于⾼速转向的增益序列，则需要将Q，R矩阵的相应参数乘以车速。并更新Q，R矩阵。设置预测时序矩阵和参考时序矩阵。
初始化OSQP Solver：
求解优化函数： ，这⾥需要稍加展开：
Apollo MPC使⽤如下优化⽬标函数：，⽽OSQP优化问题的标准形式
是：
在Apollo中系统的状态⽅程：，⽽OSQP中是：需要对MPC优化问题进⾏重新构造，从⽽适配接⼝。
MPC算法求得两个最优值：车轮转⾓，加速度。之后将车轮转⾓转换成⽅向盘转⾓：，同时
求出当前控制量的约束值：
如果允许对MPC控制进⾏前馈补偿，则不受约束的控制量差值=正常控制量-约束控制量，如果该差值⼩于差值限制值，则计算⽅向盘转⾓补偿量。否则补偿量等于0。
⽅向盘转⾓=MPC计算的⽅向盘转⾓+当前车速下的⽅向盘转⾓增加量+⽅向盘转⾓补偿量：
允许设置⽅向盘转⾓限制值，则限制值为最⼤纵向加速度*轴距/速度平⽅转换成⽅向盘转⾓：
输出⾓度滤波：
加速命令=MPC求出的加速反馈+reference_point点的加速度：
如果满⾜停车条件，则加速度为最⼤停车减速度：
根据加速度命令，查标定表得到标定值：
⾄此，整个mpc的计算过程就结束了，本⽂并不是从MPC的算法⾓度出发，单纯从apollo代码的实现⾓度理解MPC，想要升⼊理解MPC的算法可以参见我之前的⽂章。
apollo开源，⼤⼤简化了传统车企的⾃动驾驶开发时间，但并未想百度设想的能统⼀⾃动驾驶平台，说到底百度只有软件算法的解决⽅案，车企⼏个刚毕业的⼤学⽣也能移植改进，只有软硬结合的⼀整套解决⽅案才能真正统⼀⾃动驾驶平台。