基于模型预测（MPC）的无人驾驶汽车轨迹跟踪

  先推荐下自己的公众号——Joe学习笔记，公众号上会不定期更新一些文章，主要是自己平时学到的知识，内容包括自动驾驶、计算机视觉、人工智能和机器人技术。我会第一时间把文章更新在公众号上，欢迎大家订阅和分享！文章是从公众号搬过来的。

  新出了一个关于机器人路径规划和轨迹优化的视频课程，介绍常用的路径规划算法和轨迹优化算法，感兴趣的可以点击b站链接观看。更新中。。。 https://www.bilibili.com/video/BV1yT4y1T7Eb

  最近，自动驾驶技术挺火的，前半学期上现代控制原理我们小组做了个相关的大作业，第二篇文章就分享一下基于模型预测（MPC）的自动驾驶汽车轨迹追踪。
  自动驾驶汽车一般分为感知、决策和控制三部分。感知部分有很多内容，以后可能会写一些文章简单介绍，通俗的讲就是感知汽车周围的环境。决策即为根据感知的环境信息，产生遵守交通规则的驾驶过程，其中很大一部分为路径规划。路径规划有很多方法，第一篇文章介绍了基于遗传算法的路径规划方法，感兴趣的朋友可以参阅上一篇文章。本文的重点为怎么通过控制汽车的方向盘、油门和刹车对已经规划的路径进行跟踪。常用的控制方法就是经典的PID控制，网上有一篇挺详细的关于自动驾驶汽车的PID控制文章，感兴趣的朋友可以点击此链接。
http://www.360doc.com/content/18/0801/20/48488381_774988784.shtml
  PID控制存在积分环节，控制时具有滞后性，而自动驾驶技术对实时性的要求非常高，因此PID控制不太适用于自动驾驶技术。本文介绍的是一种基于模型预测的自动驾驶汽车轨迹追踪方法，不存在滞后性且不需要像PID控制那样调节参数。本文使用matlab/simulink进行仿真试验，首先介绍车辆运动学建模。
  在车辆运动学建模时先简化汽车，仅保留汽车的一半，简单的用前后轮和车轴表示，即为自行车模型。车轮的转角用delta表示（由于不知道希腊字母怎么打，都用英文表示），车轴长用L表示，汽车转弯可以近似为后轮绕某一点做圆周运动。转弯半径R通过直角三角形关系推导出。车辆航向变化的角度为车辆行走的距离（即圆弧长度）除以转弯半径，其对时间的导数即为航向的角速度，基本的推导如下图所示。

  以汽车后轮为原点建立直角坐标系，其纵向速度、横向速度和航向角速度如下图所示，并用状态方程的形式表示。

  当输入汽车的速度v和车轮转角delta，通过上述的公式，再积分后即可求得汽车的位置x、y和航向角fai，simulink仿真如下图所示，这个在后面有用。

  刚得到的状态方程有sin，cos和乘法操是一个非线性的方程，因此要先进行线性化，线性化使用的是泰勒展开的方法。下图为生成的路径上的某一点，取生成的路径上的某一点进行泰勒展开，取第一阶即可完成线性化的过程。后面的计算过程是在离散化后操作的，因此还需要离散化操作，线性化和离散化的结果如下所示。

线性化后的结果：

离散化后的结果：

  下面正式开始介绍控制器的设计。此控制器的名字为基于模型预测的方法，重点为预测二字，因此首先根据汽车的初始状态，利用上式离散状态方程预测接下来一段时间汽车的路径。下图中黑色的虚线表示规划的轨迹，红色的虚线为预测的轨迹。

  进行轨迹跟踪时希望规划的轨迹和预测的轨迹重合，因此需要设计一个目标函数，也可以叫代价函数，函数值越大表示轨迹重合度越低，因此最终的目标是最小化目标函数。目标函数较复杂，下图给出了较形象的解释，也标出了参考的来源，感兴趣的读者可以看具体的论文。

  根据实际汽车的物理条件限制目标函数的未知量存在约束，下图是北京理工大学出版社出版的《无人驾驶车辆模型预测控制》中实际车辆的参数（本文的方法也是参考于此书），并根据参数求得的约束。


  由于控制器预测轨迹，所以不存在延时效应，符合自动驾驶技术的实时性要求。整个控制器的框图如下图所示。

  有了程序框图接下来就是整体的simulink的仿真，具体的仿真图如下所示，其中的carModel即为上面提到的车辆的运动学simulink仿真。其中的MPC控制器参考书中的代码使用s-Function功能编写的，记得当时和同学抄代码都抄错了好多。

  最后就是仿真结果，视频在公众号的文章中——🔗。视频中绿色的线为给定的轨迹，汽车从原点出发，红色为汽车行驶路径。这次代码参考来源于书本，simulink仿真都给出了图片就不放源代码了，想实现的读者可以自己试着抄书上的代码。部分内容参考于互联网，不知道会不会侵权，如果侵权了就删了。