知我者，谓我心忧. 不知我者，谓我何求。— 佚名 黍离

🏰代码及环境配置：请参考 环境配置和代码运行!

---

PiecewiseJerkOptimizer是Apollo中planning模块生成Path/Speed曲线的优化方法. 基于Frenet坐标系, 生成平滑, 安全的目标曲线. 本节将讲解基于PiecewiseJerk的路径优化方法.

5.5.1 问题定义

SL坐标系下, 给定路径总长$length$, 我们将路径均匀的离散成$n$个点(上图中的蓝点), 其中$\Delta s=length/n$. 我们定义$3n$个优化变量, 每个点有3个优化变量:

• $l_i$:第$i$个点的横向坐标,
• $l_i^{\prime }$:第$i$个点的横向偏差的1阶导数, 代表横向的速度
• $l_i^{\prime \prime }$:第$i$个点的横向偏差的2阶导数, 代表横向的加速度

5.5.2 目标函数

目标函数定义如下:

$$\begin{array}{r}mincost=w_l\sum _{i=1}^n(l_i-r_i{)}^2+w_{l^{\prime }}\sum _{i=1}^n{l}_i^{\prime 2}+w_{l^{\prime \prime }}\sum _{i=1}^n{l}_i^{\prime \prime 2}+w_{l^{\prime \prime \prime }}\sum _{i=1}^{n-1}{l}_{i\to i+1}^{\prime \prime \prime }{}^2\end{array}$$

• $r_i$:代表第i个点的引导点的横向坐标
• $(l_i-r_i{)}^2$:代表偏离引导线的惩罚
• $l_i^{\prime 2},l_i^{\prime \prime 2}$:代表横向速度和加速度的惩罚
• $l_i^{\prime \prime \prime }$:横向的3阶导数(jerk), 只计算粗糙值:二阶导数插值得到. $l_i^{\prime \prime \prime }=\frac{l_{i+1}^{\prime \prime }-l_i^{\prime \prime }}{\Delta s}$
• $w_l,w_l^{\prime },w_l^{\prime \prime },w_l^{\prime \prime \prime }$:常数, 不同cost的权重

5.5.3 约束条件

5.5.3.1 连接约束

我们决策变量只有$l_i,l_i^{\prime },l_i^{\prime \prime }$, 那么如何保证点与点之间的连接是平滑的呢?

可以利用简单的积分来约束$l_i,l_i^{\prime },l_i^{\prime \prime }$ 和 $l_{i+1},l_{i+1}^{\prime },l_{i+1}^{\prime \prime }$的关系:

$$\begin{array}{rl}{l}_{i+1}^{\prime \prime }& =l_i^{\prime \prime }+{\int }_0^{\Delta s}{l}_{i\to i+1}^{\prime \prime \prime }ds=l_i^{\prime \prime }+l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s\\ l_{i+1}^{\prime }& =l_i^{\prime }+{\int }_0^{\Delta s}{l}^{\prime \prime }(s)ds=l_i^{\prime }+l_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^2\\ l_{i+1}& =l_i+{\int }_0^{\Delta s}{l}^{\prime }(s)ds\\ & =l_i+l_i^{\prime }\ast \Delta s+\frac{1}{2}\ast l_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}\ast l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^3\end{array}$$

本质上, 上面的积分就是泰勒展开式的过程:

$$f(x)=f(a)+f^{\prime }(a)(x-a)+\frac{f^{\prime \prime }(a)}{2!}(x-a{)}^2+\frac{f^{\prime \prime \prime }(a)}{3!}(x-a{)}^3+\cdots +\frac{f^{(n)}(a)}{n!}(x-a{)}^n+\cdots$$

泰勒展开式是一种重要的函数近似方法, 精度取决于泰勒展开的最高阶数. 如果我们用泰勒展开式近似点和点之间的曲线, 舍弃掉泰勒展开式的误差项, 可以得到3阶泰勒展开:

$$l_{i+1}=l_i+l_i^{\prime }\ast \Delta s+\frac{1}{2}{l}_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}{l}_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^3$$

5.5.3.2 边界约束

边界约束较为简单, 主要是

$$\begin{array}{rl}{l}_{lower}\le & l_i\le l_{upper}\\ l_{min}^{\prime }\le & l_i^{\prime }\le l_{max}^{\prime }\\ l_{min}^{\prime \prime }\le & l_i^{\prime \prime }\le l_{max}^{\prime \prime }\end{array}$$

5.5.4 完整问题

最终, 我们得到了完整的PiecewiseJerkPath优化问题, 显然这是一个QP优化问题.

$$\begin{array}{rl}min& w_l\sum _{i=1}^n(l_i-r_i{)}^2+w_{l^{\prime }}\sum _{i=1}^n{l}_i^{\prime 2}+w_{l^{\prime \prime }}\sum _{i=1}^n{l}_i^{\prime \prime 2}+w_{l^{\prime \prime \prime }}\sum _{i=1}^{n-1}{l}_{i\to i+1}^{\prime \prime \prime }{}^2\\ subject& to:\\ & l_{lower}\le l_i\le l_{upper}\\ & l_{min}^{\prime }\le l_i^{\prime }\le l_{max}^{\prime }\\ & l_{min}^{\prime \prime }\le l_i^{\prime \prime }\le l_{max}^{\prime \prime }\\ l_{i+1}^{\prime }& =l_i^{\prime }+l_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^2\\ l_{i+1}& =l_i+l_i^{\prime }\ast \Delta s+\frac{1}{2}\ast l_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}\ast l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^3\end{array}$$

参考链接:

• Zhang, Yajia, et al. “Optimal vehicle path planning using quadratic optimization for baidu apollo open platform.” 2020 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2020.

---

🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn