自动驾驶（七十六）---------Apollo MPC之代码解析

      最近一直在调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开源，大大简化了传统车企的自动驾驶开发时间，但并未想百度设想的能统一自动驾驶平台，说到底百度只有软件算法的解决方案，车企找几个刚毕业的大学生也能移植改进，只有软硬结合的一整套解决方案才能真正统一自动驾驶平台。