本文主要是介绍自动驾驶（七十六）---------Apollo MPC之代码解析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
最近一直在调LQR上车的效果，初见成效，所以也很好奇Apollo的MPC控制算法，没有项目驱动，只能靠自己的驱动力了，这里结合代码分析理论，详细梳理一遍apollo的MPC算法。
首先MPC的代码在apollo文件地址为：
1. 初始化
- 加载MPC的参数和配置：
，主要包含vehicle_param、速度和转角限制等。 - 车辆运动学模型及其离散化：
；包括矩阵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开源，大大简化了传统车企的自动驾驶开发时间，但并未想百度设想的能统一自动驾驶平台，说到底百度只有软件算法的解决方案，车企找几个刚毕业的大学生也能移植改进，只有软硬结合的一整套解决方案才能真正统一自动驾驶平台。
这篇关于自动驾驶（七十六）---------Apollo MPC之代码解析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
