【自动驾驶轨迹规划之最优控制】_最优控制理论路径规划

欢迎大家关注我的B站：

偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com)

目录

1 车辆动力学约束

1.1 模型的采取

1.2 模型的假设

1.3 运动学模型的微分方程组

1.4 控制变量的选择

2 边值约束

2.1 简单的约束

2.2 较复杂的约束

3 碰撞躲避约束

4 代价函数

5 完整的最优控制问题

---

1 车辆动力学约束

1.1 模型的采取

车辆的运动状态改变源自于轮胎受到的合力，由于轮胎受到的力沿接触面分布，需要用到材料力学的知识，有大量的复杂方程，并且在研究车辆低速情况的轨迹规划时，搭建如此复杂的物理模型并不实用，所以我们采用简洁的车辆二自由度模型。

1.2 模型的假设

（1）车辆行驶于平坦路面，忽略车辆在垂直于地面方向上的运动；

（2）忽略车辆受到的空气阻力以及地面侧向摩擦力；

（3）车辆与地面始终保持良好的滚动摩擦；

（4）车辆为刚体，忽略车身悬架结构的影响。

1.3 运动学模型的微分方程组

二自由度模型忽略左右两轮的转动角速度与角加速度差异，将车辆的两只前轮与后轮以车辆的纵轴方向合并为虚拟单轮，因此二自由度模型也称为 bicycle 模型。运动学微分方程组如下

x(t) 与 y(t) 为车辆后轴中点坐标，v(t) 与 a(t) 表示沿车体纵轴方向的速度及加速度， 代表车辆前轮偏转角， 代表车辆的航向角， 代表车辆前后轴距离。根据理论力学的知识，上述方程易得，若有疑问可参考http://t.csdn.cn/QPoru。

1.4 控制变量的选择

一般来说，可被微分的变量是状态变量，因为未被微分的变量可以通过积分得到那些变量并且可被微分的变量一定连续，未被微分的不一定连续。因此在1.3的微分方程组中，我们选择未被微分的 与  作为控制变量，通过积分来确定剩下的状态变量， 与  体现着掌控车辆动力学 的意味，假如遇到对车辆行驶平稳性要求较高的场景，则应引入变量    ，同时控制变量的选择也发生变化。轨迹规划的本质其实就是找到一组控制变量使得在人为设定的代价函数下这一组控制变量使系统最优。

2 边值约束

2.1 简单的约束

车辆的初始状态由车载传感器实时记录推算着，而车辆的最终状态也就是控制的目标状态有一定限制。例如在自主泊车中，车子必须停下来则车速为零，车子不能有运动趋势则加速度为零（包括角加速度），同时车辆最后车轮应该回正，所以前轮偏转角应该为零。

2.2 较复杂的约束

车辆停到某一个车位，相当于一个矩形落在另一个矩形中首先根据状态变量算出车辆的四个顶点坐标

Lw为前后轴距，Lf为车辆前悬距离，Lr为车辆后悬距离，Lb为车宽。根据车辆后轴中心坐标与车辆航向角即可以计算出车辆的四个顶点，目标泊车区域可以通过四条直线方程来表示，想要泊车目的达到，只需要建立四个点在泊车区域内的点约束方程组即可。

3 碰撞躲避约束

两辆汽车不相撞近似为两个矩形不重叠，也就可以看作第一辆车的四个顶点在第二辆车构成的矩形之外，第二辆车的四个顶点也在第一辆车构成的矩形之外。当点在凸边形外，这个点到凸边形某两个点构成的多个三角形面积之和大于凸边形的面积。

 三角形面积可以通过行列式得到

矩形的面积也就是凸边形的面积

  此外，碰撞躲避约束还可以体现为很多形式，根据自动驾驶的不同场景而改变。

4 代价函数

前面的车辆动力学约束、边值约束、碰撞躲避约束都是必须严格满足的硬性约束，但在满足硬约束的情况下，轨迹还有很多条，所以我们需要设定一个代价函数去人为设定指标得到我们期望的最优轨迹。

（1）如果期待运动尽块完成，

 

（2）如果期待车速变化小，

（3）如果期待转动速度变化小

（4）如果期待适当远离障碍物

n为障碍物的数量，d函数代表车辆与障碍物的距离

除了上述例子，我们可以自行根据轨迹规划的目标自行设定代价函数。再将目标函数汇总，就可以得到完整的代价函数

 为权重系数，意味着这个代价在整个代价中的重要性，我们可以进行灵活调参。

5 完整的最优控制问题

将上述的内容汇总就得到了车辆轨迹规划的最优控制框架

         min   J

s.t. 车辆动力学约束

      边值约束

      碰转躲避约束

下一篇文章将开始讲述路径规划中常见的算法。