在得到候选的轨迹后，为了寻求更优质更平滑，体感更好的路径，需要基于样条的车辆轨迹进行二次规划方法搜索。约束条件有：曲率和曲率连续性、贴近中心线、避免碰撞。
假设我们的上层（行为规划层）要求当前车辆在 到
期间完成一次变道，即车辆在横向上需要完成一个
以及纵向上完成一个
的移动，则可以将s和l分别表示：s(t)和l(t)，使用
和
来分别表示
和
方向的多项式系数。
通过这种转换原来的规划问题被分割成了两个独立的优化问题，即对横向的轨迹优化和对纵向的速度优化，选取损失函数Cost最小的作为最终规划序列。
横向的轨迹规划：
以 方向的轨迹优化为例，
~
时间段内：
分别将 和
代入上式中，可以确定候选多项式曲线的系数。
其中，目标点 时刻的状态为
，对于横向轨迹优化问题，我们实际上希望车辆最终沿着参考线平行的方向行驶，令
那么目标配置只涉及 和
两个变量的组合，而这两个变量在无人驾驶的应用场景中实际上是受限的，我们可以通过定义
和
来约束目标配置的取值范围，通过
和
来限制采样密度，从而在每一个时间周期获得一个有限的横向备选轨迹集合：
需要在上图绿色轨迹的基础上设计成本函数，对于不同的场景，成本函数也不相同。
在高速场景下，成本函数为：
其仅适用于相对高速度的场景，在极端低速的情况下，车辆的制动能力是不完整的，我们不再将 表示为关于时间
的五次多项式，损失函数也会略有不同，但是这种基于有限采样轨迹，通过优化损失函数搜索最优轨迹。
其中， ~
时间段内累积的横向偏移量为：
接着主要是使得 最小，现将其转换成QP公式：
纵向的轨迹优化：
在高速场景中， 通过
和
来限制采样密度，从而在每一个时间周期获得一个有限的纵向备选速度集合：
成本函数为：
五次多项式
根据车辆当前点和目标点的的速度和加速度来配置自动驾驶车辆所需要的行驶参数，即速度曲线可以用五次多项式的时间函数表示：
1、规划的5次位移曲线如下：
2、规划的4次速度曲线如下：
3、规划的3次加速度曲线如下：
4、规划的2次加加速度曲线如下：
- 约束条件
进行速度规划的同时，需要判断是否符合车辆运动和道路等限制，判断内容包括：
（a）s 方向上的车辆位移必须单调递增；
（b）s 方向上的速度是否超过设定的最大限速；
（c）s 方向的加速度是否超过设定的最大加速度限制；
参考
开发者说 | Apollo 6.0 QP（二次规划）算法解析
评论（0）
您还未登录，请登录后发表或查看评论