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/chepwavege/article/details/99939522

 

所以物理意义 就是要保证曲率 不能太大，曲率变化率不能太大！

第一次微分： heading angle,  二次：曲率， 三次：曲率变化率！！！！

1.5 将开销（cost）函数转换为QP公式

QP公式:

下面是将开销（cost）函数转换为QP公式的例子：

且

且

然后得到，

 

从聚合函数中提取出常量得到， 6*1  乘以 1*6 的矩阵

最后得到，积分过程

 

请注意我们最后得到一个6介的矩阵 （6X6）来表示5介样条插值的衍生开销。 应用同样的推理方法可以得到2介，3介样条插值的衍生开销。

2 约束条件

2.1 初始点约束

假设第一个点为 (s_0, l_0), (s_0, l'_0) and (s_0, l''_0)，其中l_0 , l'_0 and l''_0表示横向的偏移，并且规划路径的起始点的第一，第二个点的衍生开销可以从f_i(s), f'_i(s), f_i(s)''计算得到。-

将上述约束转换为QP约束等式，使用等式：

下面是转换的具体步骤：第一个点带入曲线方程得到，S-L 下的关系， 同样多阶导数同样适用

且

且

其中，i是包含s_0的样条段的索引值。

2.2 终点约束

和起始点相同，终点(s_e, l_e) 也应当按照起始点的计算方法生成约束条件。

将起始点和终点组合在一起，得出约束等式为：

2.3 平滑节点约束--意义：曲率，曲率变化率 连续！

该约束的目的是使样条的节点更加平滑。假设两个段seg_k 和seg_{k+1}互相连接，且seg_k的累计值s为s_k。计算约束的等式为：

下面是计算的具体步骤：

然后

将s_0 = 0代入等式。

同样地，可以为下述等式计算约束等式：

2.4 点采样边界约束--横向offset. 不能和障碍物相交（Y方向）

在路径上均匀的取样m个点，检查这些点上的障碍物边界。将这些约束转换为QP约束不等式，使用不等式：

首先基于道路宽度和周围的障碍物找到点 的下边界，且。计算约束的不等式为：

 

同样地，对上边界，计算约束的不等式为：