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

你可能感兴趣的:(自动驾驶)