Apollo_ADS_路径规划2- 路径优化 --SL 轨迹 Optimizer

二次规划（QP）+样条插值

 

总结：

1. 转换到 SL frame, 采样到多个点的曲线方程（ L 关于S 的方程）--表示的是横坐标，纵坐标的关系）。每一段用统一的5阶样条表示

2.定义目标函数  曲线方程关于s的一阶、二阶，三阶导数。

3.定义约束

• 初始点，终点约束
• 连续性约束
• 障碍物横向位置的约束（上下）

目标，筛选并获得最优的路径

---

1. 目标函数

1.1 获得路径长度

路径定义在station-lateral坐标系中。s的变化区间为从车辆当前位置点到默认路径的长度 （arc length 弧长）。

1.2 获得样条段

将路径划分为n段，每段路径用一个多项式来表示。

1.3 定义样条段函数

每个样条段 i 都有沿着参考线的累加距离d_i （弧长）。每段的路径默认用5介多项式表示。

l 表示的就是横向的偏离。--重点关注的是横向offset.

1.4 定义每个样条段优化目标函数

关于距离的微分？ 意义？  纯粹为了定义的目的--- 由曲率的定义知道，1,2,3阶导数和曲率相关。(https://baike.baidu.com/item/%E6%9B%B2%E7%8E%87%E5%8D%8A%E5%BE%84/2036643 公式推导部分.)

https://blog.csdn.net/chepwa