自动驾驶速度平滑教程：从运动约束到解析 Jerk 受限速度规划（2）

前面内容：自动驾驶速度平滑教程：从运动约束到解析 Jerk 受限速度规划（1）

16. 减速目标：哪些点必须认真对待

先给出一句定义：

$$\text{减速目标 = 速度上限曲线中需要车辆提前减速才能到达的低速约束点}$$

这里的“目标”不是行为规划里的任务目标，也不是车辆最终目的地，而是速度规划内部的一个局部目标。它由一个轨迹位置和一个目标速度组成：

$$(s_{T,k},v_{T,k})$$

其中 $s_{T,k}$ 表示第 $k$ 个减速目标在轨迹上的弧长位置，$v_{T,k}$ 表示车辆到达这个位置时希望满足的速度上限。

为什么要单独找这些点？因为前向 jerk 滤波只会从当前状态向前推，它天然更擅长“加速或跟随参考速度”，但它不会主动提前知道“前面某个点必须低速通过”。所以解析平滑器会先从速度上限曲线中找出几个关键低速点，然后对每个低速点做后向减速检查：从哪里开始减速，才能在 jerk 和加速度约束下到达这个低速点。

减速目标来自已经处理过的参考速度上限曲线。也就是说，先有一条综合速度上限：

$$v_{\mathrm{ref}}(s)$$

它已经叠加了外部限速、停车接近速度、曲率限速、转向角速度限速、终点速度等约束。减速目标就是从这条 $v_{\mathrm{ref}}(s)$ 上找出来的局部低速关键点。

最典型的减速目标是“速度谷底”。离散轨迹中，设第 $i$ 个点的参考速度为 $v_i$。如果进入点 $i$ 之前速度明显下降：

$$v_i-v_{i-1}<-ϵ$$

并且点 $i$ 之后速度不再继续明显下降：

$$v_{i+1}-v_i>-ϵ$$

那么点 $i$ 就可以被视为一个减速目标：

$$(s_{T,k},v_{T,k})=(s_i,v_i)$$

这里 $ϵ$ 是一个很小的阈值，用来避免浮点误差或极小速度抖动造成误判。第二个条件不是严格要求 $v_{i+1}>v_i$，而是要求后面没有继续明显下降；因此谷底后面可以是回升，也可以是近似持平。

直观来看，速度序列如果像这样：

$$12,10,8,6,6,8,10$$

那么 $6$ 附近就是减速目标，因为车辆必须提前规划，才能从较高速度平顺降到 $6m/s$。

常见来源包括：

• 停车点，目标速度为 $0$。
• 弯道最低限速点。
• 外部限速开始生效的位置。
• 路径终点。

最后一个轨迹点也有特殊处理。如果轨迹末端相对前一个点仍在下降：

$$v_{N-1}-v_{N-2}<-ϵ$$

那么末端也会被视为减速目标：

$$(s_{T,k},v_{T,k})=(s_{N-1},v_{N-1})$$

因为路径终点通常也需要满足终端速度，很多情况下终点速度会被设为 $0$。

还要注意：不是所有速度较低的点都会成为减速目标。算法关心的是“需要提前减速才能到达的关键低速点”，而不是整段低速区域中的每一个点。对于一段连续下降或连续低速区域，只选代表性的谷底点即可。

例如：

$$12,10,8,6,5.8,5.7,6,8$$

真正重要的是最低附近的 $5.7$，而不是 $6,5.8,5.7$ 都分别作为目标。

因此，多个很近的低速目标会被合并或筛选。如果两个候选低速点索引距离很近，并且后一个速度更低：

$$i_{k+1}-i_k<N_{\mathrm{merge}}$$

$$v_{i_{k+1}}<v_{i_k}$$

则前一个较浅的低速点会被忽略，保留后一个更深的谷底。这样可以避免对同一个减速区域重复做后向减速规划。

用更完整的流程描述，减速目标搜索可以理解为：

1. 从当前车辆最近点之后开始扫描速度上限曲线。
2. 找到“前面在下降，后面不再明显下降”的速度谷底。
3. 如果末端仍在下降，也把末端作为目标。
4. 对距离很近的多个候选目标做筛选，保留更有代表性的低速谷底。
5. 得到按轨迹顺序排列的减速目标序列：

$$(s_{T,0},v_{T,0}),(s_{T,1},v_{T,1}),\cdots ,(s_{T,m},v_{T,m})$$

有了这个序列之后，“当前减速目标”就是正在处理的第 $k$ 个目标：

$$(s_{T,k},v_{T,k})$$

“前一个减速目标”就是它在同一条轨迹上按顺序排在前面的目标：

$$(s_{T,k-1},v_{T,k-1})$$

所以，前一个减速目标不是上一帧的目标，也不是任意历史点，而是当前这次速度规划里已经识别出来、并且已经先于当前目标被处理过的低速关键点。

17. 后向减速滤波：提前判断能不能刹得住

对于某个减速目标，设目标位置为：

$$s_T$$

目标速度为：

$$v_T$$

候选减速起点为：

$$s_0$$

起点状态为：

$$v_0,a_0$$

这里的“候选减速起点”不是随便选一个点，也不是固定等于当前自车点。解析平滑器会先根据减速目标之间的关系，确定一个可以开始检查的范围，再在这个范围内判断距离是否足够。

第一步，确定后向检查的边界。假设当前处理的是第 $m$ 个减速目标，它前面可能还有若干个减速目标：

$$(s_{T,0},v_{T,0}),(s_{T,1},v_{T,1}),\cdots ,(s_{T,m},v_{T,m})$$

如果当前目标是第一个减速目标，那么最早只能从当前车辆最近点开始检查：

$$s_{\mathrm{base}}=s_{\mathrm{ego}}$$

如果当前目标前面存在一个速度更低的减速目标：

$$v_{T,r}<v_{T,m},r<m$$

则说明从目标 $r$ 到目标 $m$ 之间可能存在“重新加速再减速”的过程。此时不应该让当前目标的减速过程跨过前一个更低速目标，否则会破坏前一个目标已经规划好的低速区域。因此会把这个前一个低速目标作为后向检查边界：

$$s_{\mathrm{base}}=s_{T,r}$$

如果一直找不到这样的前序低速目标，就退回到当前车辆最近点：

$$s_{\mathrm{base}}=s_{\mathrm{ego}}$$

第二步，形成候选起点集合。当前目标对应的前向滤波起点通常是上一个减速目标，记为：

$$s_{\mathrm{fwd}}$$

于是候选集合可以理解为：

C = { { s b a s e , s f w d } , s b a s e ≠ s f w d { s b a s e } , s b a s e = s f w d \mathcal{C}= \begin{cases} \{s_{\mathrm{base}},s_{\mathrm{fwd}}\}, & s_{\mathrm{base}}\ne s_{\mathrm{fwd}}\\ \{s_{\mathrm{base}}\}, & s_{\mathrm{base}}=s_{\mathrm{fwd}} \end{cases} C={{sbase​,sfwd​},{sbase​},​sbase​=sfwd​sbase​=sfwd​​

为什么可能有两个候选点？因为多个低速目标连续出现时，当前目标既可能需要从更早的低速区间之后就开始考虑，也可能只需要从本轮前向滤波的起点之后再处理。两个候选点都检查一遍，可以避免过早减速，也避免起点太晚导致刹不住。

第三步，候选点还会被向后修正到“速度真正需要减速的位置”。如果从某个候选点到目标点之间，有一段速度本来就低于目标速度 $v_T$，那一段不需要为了当前目标减速。真正有意义的检查起点，是从候选范围内找到第一个满足：

$$v(s_i)\ge v_T$$

的轨迹点。这个点才会作为该候选方案的实际检查起点。

可用距离是：

$$d_{\mathrm{allow}}=s_T-s_0$$

解析规划会计算：在给定负 jerk、正 jerk 和最小加速度限制下，从 $(v_0,a_0)$ 平滑降到 $(v_T,0)$ 需要多少距离：

$$d_{\mathrm{stop}}$$

若：

$$0\le d_{\mathrm{stop}}\le d_{\mathrm{allow}}$$

则说明从这个起点开始减速来得及；否则需要更早开始减速，或者使用更强的负 jerk、更大的减速度。

因此，减速起点的选择标准可以总结为三个条件：

1. 起点不能跨过已经存在的前序低速目标，尤其不能破坏前一个更低速度目标。
2. 起点处速度应当已经不低于当前目标速度，否则没有必要从那里开始减速。
3. 从该起点到目标点的剩余距离必须足够完成 jerk 受限减速，即 $d_{\mathrm{stop}}\le d_{\mathrm{allow}}$。

后向滤波的一个关键策略是：优先尝试温和 jerk。例如从：

$$j_{\mathrm{start}}=-0.1$$

逐步降低到：

$$j_{\min }=-1.5$$

每次变化一个步长：

$$\Delta j<0$$

能用温和 jerk 解决，就不使用更激烈的 jerk。只有距离不够时，才逐步增加减速强度。

如果多个候选起点都满足距离条件，则优先选择所需 jerk 更温和的方案。因为 $j_{\mathrm{dec}}<0$，所谓“更温和”就是数值更接近 $0$：

$$j=-0.1\text{比}j=-1.0\text{更温和}$$

如果使用最温和的 jerk 已经能刹住，说明减速余量比较充足，此时还会进一步把起点往目标点方向推，寻找“最晚仍然刹得住”的起点：

s s t a r t = max ⁡ { s i ∣ d s t o p ( s i ) ≤ s T − s i } s_{\mathrm{start}}= \max\left\{ s_i\mid d_{\mathrm{stop}}(s_i)\le s_T-s_i \right\} sstart​=max{si​∣dstop​(si​)≤sT​−si​}

这样做的效果是：能晚一点减速就晚一点减速，避免车辆过早降速；但一旦再晚就刹不住，就把起点固定在当前这个位置。

这体现了速度平滑的舒适性优先级。

18. Jerk 受限减速距离的解析推导

这是解析平滑器最重要的数学部分。

目标是从初始状态：

$$(v_0,a_0)$$

到达：

$$(v_T,0)$$

也就是速度降到目标速度，并且终点加速度回到 $0$，避免到达目标点后仍处于继续减速状态。

允许的减速曲线由三类组成。

18.1 Type 1：梯形加速度曲线

当需要较大的速度下降时，加速度会先从 $a_0$ 下降到最小允许加速度 $a_{\min }$，保持一段时间，再恢复到 $0$。

三个阶段是：

1. 使用负 jerk $j_{\mathrm{dec}}<0$，让加速度从 $a_0$ 到 $a_{\min }$。
2. 使用 $j=0$，保持加速度 $a_{\min }$。
3. 使用正 jerk $j_{\mathrm{acc}}>0$，让加速度从 $a_{\min }$ 回到 $0$。

阶段 1 时间：

$$t_1=\frac{a_{\min }-a_0}{j_{\mathrm{dec}}}$$

阶段 3 时间：

$$t_3=\frac{0-a_{\min }}{j_{\mathrm{acc}}}$$

阶段 1 的速度变化可以看成加速度-时间图下面积。这里要用从 $a_0$ 到 $a_{\min }$ 的一般形式，不应该默认 $a_0$ 一定大于 $0$。阶段 1 的时间是：

$$t_1=\frac{a_{\min }-a_0}{j_{\mathrm{dec}}}$$

由于加速度在常 jerk 下线性变化，所以速度变化量等于梯形面积：

$$\Delta v_1=\frac{a_0+a_{\min }}{2}{t}_1$$

代入 $t_1$：

$$\Delta v_1=\frac{a_0+a_{\min }}{2}\cdot \frac{a_{\min }-a_0}{j_{\mathrm{dec}}}=\frac{a_{\min }^2-a_0^2}{2j_{\mathrm{dec}}}$$

源码中常见的写法会把它等价地展开成两项：

$$\Delta v_1=\frac{1}{2}\frac{0-a_0}{j_{\mathrm{dec}}}{a}_0+\frac{1}{2}\frac{a_{\min }-0}{j_{\mathrm{dec}}}{a}_{\min }$$

但这只是代数等价写法，因为：

$$\frac{1}{2}\frac{0-a_0}{j_{\mathrm{dec}}}{a}_0+\frac{1}{2}\frac{a_{\min }-0}{j_{\mathrm{dec}}}{a}_{\min }=-\frac{a_0^2}{2j_{\mathrm{dec}}}+\frac{a_{\min }^2}{2j_{\mathrm{dec}}}=\frac{a_{\min }^2-a_0^2}{2j_{\mathrm{dec}}}$$

它不表示真实加速度过程一定会经过 $0$。如果 $a_0>0$，阶段 1 确实会从正加速度经过 $0$ 再到 $a_{\min }$；但如果 $a_0<0$，阶段 1 只是从一个负加速度继续变到更负的 $a_{\min }$，不会先回到 $0$。

阶段 3 的速度变化：

$$\Delta v_3=\frac{a_{\min }+0}{2}{t}_3=\frac{1}{2}\frac{0-a_{\min }}{j_{\mathrm{acc}}}{a}_{\min }$$

设阶段 2 时间为 $t_2$，则总速度变化满足：

$$v_T-v_0=\Delta v_1+a_{\min }{t}_2+\Delta v_3$$

所以：

$$t_2=\frac{v_T-v_0-\Delta v_1-\Delta v_3}{a_{\min }}$$

代入一般形式：

$$t_2=\frac{v_T-v_0-\frac{a_{\min }^2-a_0^2}{2j_{\mathrm{dec}}}+\frac{a_{\min }^2}{2j_{\mathrm{acc}}}}{a_{\min }}$$

如果：

$$t_2>0$$

说明加速度确实会到达 $a_{\min }$ 并保持一段时间，这就是梯形加速度曲线。

对应距离可以分段积分：

阶段 1：

$$x_1=v_0{t}_1+\frac{1}{2}{a}_0{t}_1^2+\frac{1}{6}{j}_{\mathrm{dec}}{t}_1^3$$

$$v_1=v_0+a_0{t}_1+\frac{1}{2}{j}_{\mathrm{dec}}{t}_1^2$$

$$a_1=a_0+j_{\mathrm{dec}}{t}_1$$

阶段 2：

$$x_2=x_1+v_1{t}_2+\frac{1}{2}{a}_1{t}_2^2$$

$$v_2=v_1+a_1{t}_2$$

阶段 3：

$$x_3=x_2+v_2{t}_3+\frac{1}{2}{a}_1{t}_3^2+\frac{1}{6}{j}_{\mathrm{acc}}{t}_3^3$$

最终减速距离为：

$$d_{\mathrm{stop}}=x_3$$

18.2 Type 2：三角形加速度曲线

如果 $t_2\le 0$，说明不需要保持 $a_{\min }$。此时加速度只需要下降到某个负峰值 $a_1$，再恢复到 $0$，形成三角形加速度曲线。

两个阶段是：

1. 使用负 jerk $j_{\mathrm{dec}}$，从 $a_0$ 到 $a_1$。
2. 使用正 jerk $j_{\mathrm{acc}}$，从 $a_1$ 到 $0$。

需要求出未知的负峰值 $a_1$。

速度变化满足：

$$v_T-v_0=\Delta v_{a_0\to a_1}+\Delta v_{a_1\to 0}$$

其中第一段不能默认经过 $0$。无论 $a_0>0$ 还是 $a_0<0$，都应该直接使用从 $a_0$ 到 $a_1$ 的一般形式：

$$\Delta v_{a_0\to a_1}=\frac{a_0+a_1}{2}\cdot \frac{a_1-a_0}{j_{\mathrm{dec}}}=\frac{a_1^2-a_0^2}{2j_{\mathrm{dec}}}$$

第二段 $a_1\to 0$ 的速度变化是：

$$\Delta v_{a_1\to 0}=\frac{1}{2}\frac{0-a_1}{j_{\mathrm{acc}}}{a}_1=-\frac{a_1^2}{2j_{\mathrm{acc}}}$$

代回总速度变化方程：

$$v_T-v_0=\frac{a_1^2-a_0^2}{2j_{\mathrm{dec}}}-\frac{a_1^2}{2j_{\mathrm{acc}}}$$

移项：

$$v_T-v_0+\frac{a_0^2}{2j_{\mathrm{dec}}}=\frac{1}{2}{a}_1^2\left(\frac{1}{j_{\mathrm{dec}}}-\frac{1}{j_{\mathrm{acc}}}\right)$$

所以：

$$a_1^2=\left(v_T-v_0+\frac{a_0^2}{2j_{\mathrm{dec}}}\right)\frac{2j_{\mathrm{acc}}{j}_{\mathrm{dec}}}{j_{\mathrm{acc}}-j_{\mathrm{dec}}}$$

这与常见的源码写法等价，因为：

$$\frac{1}{2}\frac{0-a_0}{j_{\mathrm{dec}}}{a}_0=-\frac{a_0^2}{2j_{\mathrm{dec}}}$$

所以也可以写成：

$$a_1^2=\left(v_T-v_0-\frac{1}{2}\frac{0-a_0}{j_{\mathrm{dec}}}{a}_0\right)\frac{2j_{\mathrm{acc}}{j}_{\mathrm{dec}}}{j_{\mathrm{acc}}-j_{\mathrm{dec}}}$$

因为这是减速峰值，所以取负根：

$$a_1=-\sqrt{a_1^2}$$

两个阶段时间为：

$$t_1=\frac{a_1-a_0}{j_{\mathrm{dec}}}$$

$$t_2=\frac{0-a_1}{j_{\mathrm{acc}}}$$

再用常 jerk 积分公式求距离：

$$d_{\mathrm{stop}}=\left[v_0{t}_1+\frac{1}{2}{a}_0{t}_1^2+\frac{1}{6}{j}_{\mathrm{dec}}{t}_1^3\right]+\left[v_1{t}_2+\frac{1}{2}{a}_1{t}_2^2+\frac{1}{6}{j}_{\mathrm{acc}}{t}_2^3\right]$$

其中：

$$v_1=v_0+a_0{t}_1+\frac{1}{2}{j}_{\mathrm{dec}}{t}_1^2$$

18.3 Type 3：仅恢复加速度曲线

还有一种情况：车辆当前已经在减速，也就是 $a_0\le 0$，而且只要把加速度平滑恢复到 $0$，速度就能接近目标速度，不需要再施加负 jerk。

此时只使用正 jerk：

$$j=j_{\mathrm{acc}}$$

时间为：

$$t_1=\frac{0-a_0}{j_{\mathrm{acc}}}$$

距离为：

$$d_{\mathrm{stop}}=v_0{t}_1+\frac{1}{2}{a}_0{t}_1^2+\frac{1}{6}{j}_{\mathrm{acc}}{t}_1^3$$

终点速度为：

$$v_1=v_0+a_0{t}_1+\frac{1}{2}{j}_{\mathrm{acc}}{t}_1^2$$

终点加速度为：

$$a_1=a_0+j_{\mathrm{acc}}{t}_1=0$$

如果 $v_1$ 在允许误差内接近 $v_T$，则 Type 3 成立。

19. 如何选择三种减速曲线

解析规划先尝试判断是否需要恒定最小加速度阶段。

如果根据 Type 1 推导得到：

$$t_2>0$$

使用梯形加速度曲线。

如果：

$$t_2\le 0$$

说明不需要保持 $a_{\min }$，再判断仅恢复加速度是否足够。

仅恢复加速度造成的速度变化是：

$$\Delta v_{\mathrm{recover}}=\frac{1}{2}\frac{0-a_0}{j_{\mathrm{acc}}}{a}_0$$

目标速度变化为：

$$\Delta v_{\mathrm{target}}=v_T-v_0$$

若仅恢复还不够减速，或者当前加速度 $a_0>0$ 表示车辆还在加速，则需要 Type 2：

$$\Delta v_{\mathrm{recover}}-\Delta v_{\mathrm{target}}>0\text{or}{a}_0>0$$

否则使用 Type 3。

这三个类型覆盖了常见的 jerk 受限减速情形：

• Type 1：距离较长、需要明显减速，出现恒定减速度平台。
• Type 2：中等减速，不触及最小加速度。
• Type 3：当前已经在减速，只需平滑收尾。

20. 把连续解析曲线写回离散轨迹

计算出减速类型和阶段时间后，可以得到连续曲线：

$$x(t),v(t),a(t),j(t)$$

但输出轨迹是离散点，因此要把连续曲线映射回轨迹点。

做法是先按时间采样解析曲线，得到一组表：

{ t k , x k , v k , a k , j k } \{t_k,x_k,v_k,a_k,j_k\} {tk​,xk​,vk​,ak​,jk​}

然后计算轨迹点从减速起点开始的累计距离：

$$d_i=s_i-s_{\mathrm{start}}$$

再以 $x$ 为自变量，对 $v$ 和 $a$ 做线性插值：

$$v_i=\mathrm{lerp}(x_k,v_k,d_i)$$

$$a_i=\mathrm{lerp}(x_k,a_k,d_i)$$

减速曲线结束之后的点，速度固定为目标速度：

$$v_i=v_T$$

加速度固定为：

$$a_i=0$$

这样就能把解析求得的连续减速过程落到实际轨迹点上。

21. 多个减速目标如何组合

真实轨迹中可能有多个低速区域。例如先经过一个弯道，再遇到停车点；或者连续两个限速区。

解析平滑器按减速目标从近到远处理。每处理一个目标，大致进行：

1. 从上一个有效起点向前做前向 jerk 滤波。
2. 对当前减速目标做后向减速检查。
3. 如果距离足够，写入解析减速曲线。
4. 如果距离不足，降低目标区间速度上限，再重新前向滤波。

处理后一个目标时，会考虑前一个目标是否速度更低。如果前一个目标速度低，而当前目标速度高，中间可能包含重新加速过程；如果当前目标更低，则可能需要从更早位置开始连续减速。

这使得解析方法虽然不是全局最优优化器，但能以局部目标逐步修正的方式生成合理速度曲线。

22. 停车点的强制后处理

速度规划可能因为约束冲突、采样误差或距离不足，无法刚好在输入停车点速度为 $0$。但是停车点具有安全优先级。

因此最终会对停车点之后的速度进行强制限制：

$$v(s)=0,s\ge s_{\mathrm{stop}}$$

如果优化或解析输出在停车点仍明显大于 $0$，系统还应记录告警，因为这表示：

$$v_{\mathrm{planned}}(s_{\mathrm{stop}})>{ϵ}_{\mathrm{stop}}$$

它不一定代表算法错误，也可能是当前速度太高、停车点太近、约束过于温和导致物理上无法及时停下。但无论如何，输出给下游控制器的停车点之后速度必须被安全裁剪。

23. 最大速度与后方轨迹处理

最终速度还需要满足全局最大速度限制：

$$v(s)\le v_{\mathrm{global},\max }$$

对于车辆后方的轨迹点，速度规划器通常不会重新优化，因为车辆已经经过那些点。但输出轨迹为了保持结构完整，仍需要给后方点填入合理速度。

常见策略有两种：

• 如果是首次规划、起步、偏差重规划，就让后方点使用当前最近点的速度和加速度，避免历史不可靠。
• 正常情况下，从上一帧输出轨迹中投影得到后方点速度和加速度，使输出跨帧连续。

这部分不是车辆未来行为的核心，但对控制器输入一致性和调试可视化很重要。

24. 后重采样：让输出适合下游模块

速度平滑内部使用的轨迹点间距，不一定适合下游控制器。优化器可能希望点少一些，停车附近又需要点密一些；而控制器可能有自己的采样间距和轨迹长度要求。

因此速度规划完成后，还会做一次输出重采样。它的作用是把内部速度曲线转换成下游模块更容易消费的轨迹格式。

重采样时速度一般通过插值得到：

$$v_{\mathrm{out}}(s)=\mathrm{interp}(v_{\mathrm{smooth}}(s))$$

轨迹末端仍要保证速度为 $0$：

$$v_{\mathrm{out}}(s_{\mathrm{end}})=0$$

25. 解析平滑器的参数直觉

解析平滑器中的参数可以按功能分组理解。

重采样参数决定轨迹离散程度。点越密，曲线表达越细，但计算更多；点越疏，计算更快，但停车点和弯道细节可能不够。

横向加速度参数决定过弯速度。更小的横向加速度上限会更舒适、更保守；更大的上限会更激进。

前向滤波参数决定正常跟随参考速度时的响应：

$$a_{\max },a_{\min },j_{\max },j_{\min },k_p$$

$k_p$ 越大，越积极追参考速度；但过大可能带来加速度变化更频繁。

后向滤波参数决定遇到减速目标时的策略：

$$j_{\mathrm{start}},j_{\min },a_{\min },\Delta j$$

$j_{\mathrm{start}}$ 越接近 $0$，初始尝试越温和；$j_{\min }$ 越负，极限减速能力越强；$\Delta j$ 控制从温和到激烈搜索的粒度。

还有一组 mild stop 参数，用于区分“温和停车”和“强制停车”。当温和 jerk 就能满足距离时，使用较舒适的最小减速度；当需要更强 jerk 时，才使用更强的最小减速度。

26. 其他平滑器的基本逻辑

速度平滑模块通常不只有解析平滑器，还会提供基于优化的平滑器。它们和解析方法的差别在于：解析方法用运动学公式和滤波规则构造速度曲线；优化方法把速度规划写成数学规划问题，由求解器寻找最优解。

26.1 JerkFiltered 平滑器

JerkFiltered 的思路可以分成两层。

第一层先做前后向 jerk 滤波。前向滤波限制加速能力，得到“从当前状态出发最多能多快”；后向滤波从终点或停车点反推，得到“为了停得住最多能多快”。然后取两者中更小的速度：

$$v_{\mathrm{filtered}}(s)=\min (v_{\mathrm{forward}}(s),v_{\mathrm{backward}}(s))$$

第二层再做优化，使速度尽量高，同时惩罚 jerk、超速、超加速度、超 jerk。

它的优化变量通常包括：

$$b_i=v_i^2,a_i$$

以及用于软约束的松弛变量。核心约束包括：

速度限制：

$$0\le b_i\le v_{\max ,i}^2$$

加速度限制：

$$a_{\min }\le a_i\le a_{\max }$$

空间动力学关系：

$$\frac{b_{i+1}-b_i}{\Delta s_i}=2a_i$$

近似 jerk：

$$j_i\approx \frac{a_{i+1}-a_i}{\Delta s_i}{v}_{\mathrm{ref},i}$$

目标函数可以理解为：

$$\min \sum _i\left(-w_v{b}_i+w_j{j}_i^2+w_{\Delta v}\Delta v_i^2+w_{\Delta a}\Delta a_i^2+w_{\Delta j}\Delta j_i^2\right)$$

其中负的 $b_i$ 项表示希望速度尽量大；其他项表示不希望违反限制或变化太剧烈。

JerkFiltered 的优势是表达能力强，舒适性好；代价是计算量比解析方法大。

26.2 L2 Pseudo Jerk 平滑器

L2 平滑器同样使用：

$$b_i=v_i^2$$

并保持：

$$\frac{b_{i+1}-b_i}{\Delta s_i}=2a_i$$

它主要惩罚 pseudo jerk，也就是加速度对弧长的变化率：

$$\frac{da}{ds}\approx \frac{a_{i+1}-a_i}{\Delta s_i}$$

目标函数可以理解为：

$$\min \sum _i\left(-b_i+w_p{\left(\frac{a_{i+1}-a_i}{\Delta s_i}\right)}^2+w_v\Delta v_i^2+w_a\Delta a_i^2\right)$$

注意 pseudo jerk 不完全等于真实 jerk。真实 jerk 是：

$$j=\frac{da}{dt}=\frac{da}{ds}\frac{ds}{dt}=v\frac{da}{ds}$$

所以 L2 惩罚的是空间上的加速度变化平滑性，而不是严格的时间 jerk。它简单、稳定、求解较直接。

26.3 Linf Pseudo Jerk 平滑器

Linf 平滑器和 L2 类似，但它不是最小化所有 pseudo jerk 的平方和，而是最小化最大 pseudo jerk。

引入变量 $\psi$：

$$\left|\frac{a_{i+1}-a_i}{\Delta s_i}\right|\le \psi$$

目标中最小化：

$$\min w_{\psi }\psi$$

这相当于压低最坏位置的加速度空间变化率。L2 更关注整体平滑，Linf 更关注不要出现局部尖峰。

26.4 四类平滑器的直觉对比

平滑器	核心思想	优点	代价
Analytical	解析积分 + 前向跟踪 + 后向减速	快、确定性强、易理解停车距离	不追求全局最优
JerkFiltered	前后向 jerk 预滤波 + 优化	约束表达丰富、舒适性好	计算量较大
L2	最小化 pseudo jerk 平方和	简洁、整体平滑	对局部峰值不如 Linf 敏感
Linf	最小化最大 pseudo jerk	抑制局部尖峰	求解约束更多

如果实时性要求高，解析方法很有吸引力；如果希望在复杂约束下得到更全局的折中，优化方法更灵活。

27. 如何从零理解完整速度平滑问题

现在可以把整套速度平滑重新串起来。

首先，路径给出车辆要走的几何轨迹，并附带原始速度上限：

$$v_{\mathrm{ref}}(s)$$

然后，外部限速、停车接近速度、横向加速度、转向角速度会继续压低这条上限，得到综合速度上限：

$$v_{\max }(s)=\min \left(v_{\mathrm{ref}}(s),v_{\mathrm{external}}(s),v_{\mathrm{approach}}(s),v_{\mathrm{lat}}(s),v_{\mathrm{steer}}(s),v_{\mathrm{global}}\right)$$

接着，根据当前状态确定初值：

$$v(0)=v_0$$

$$a(0)=a_0$$

速度平滑器求解：

$$v(s),a(s)$$

并希望满足：

$$0\le v(s)\le v_{\max }(s)$$

$$a_{\min }\le a(s)\le a_{\max }$$

$$j_{\min }\le j(t)\le j_{\max }$$

解析平滑器通过分段常 jerk 曲线实现这些约束；优化平滑器通过变量、约束和目标函数实现这些约束。

最后，对停车点、轨迹末端、后方轨迹、输出采样进行后处理，得到下游控制器可用的最终速度轨迹。

28. 一个小例子：为什么不能只按目标速度裁剪

假设当前车辆：

$$v_0=10m/s,a_0=0$$

前方 $20\mathrm{m}$ 有停车点：

$$v_T=0$$

如果只把停车点速度设为 $0$，中间速度不处理，控制器会在接近停车点时突然面对巨大速度误差。

若允许最大减速度：

$$a_{\min }=-2m/{\mathrm{s}}^2$$

不考虑 jerk 的理想匀减速停车距离为：

$$d=\frac{v_0^2-v_T^2}{-2a_{\min }}=\frac{10^2}{4}=25\mathrm{m}$$

这已经超过 $20\mathrm{m}$。如果再考虑 jerk 限制，加速度不能瞬间到 $-2m/{\mathrm{s}}^2$，实际需要距离会更长。

所以速度平滑必须提前计算减速距离。如果物理上来不及，就应尽早减速并发出风险信号，而不是在停车点附近才处理。

29. 调参时最应该关注什么

速度平滑调参不应只看一条速度曲线漂不漂亮，而要同时看几类量。

首先看速度是否违反关键上限：

$$v(s)\le v_{\max }(s)$$

尤其是停车点、外部限速点、弯道最低速度点。

其次看加速度是否符合舒适性和车辆能力：

$$a_{\min }\le a(s)\le a_{\max }$$

如果车辆经常急刹，可能是 $a_{\min }$ 太激进，或减速触发太晚。

然后看 jerk：

$$j_{\min }\le j(t)\le j_{\max }$$

如果乘坐感有顿挫，通常应检查 jerk 限制、前向跟踪增益、减速目标距离和重采样密度。

再看横向指标：

$$a_{\mathrm{lat}}=v^2|\kappa |$$

以及转向角速度：

$$\dot{\delta }=v\frac{d\delta }{ds}$$

如果弯道中过快，优先检查横向加速度限制；如果方向盘动作太急，检查转向角速度限制和曲率计算距离。

最后看初始状态连续性。若每一帧速度、加速度跳动明显，问题可能不是平滑器本身，而是初始状态选择、上一帧轨迹投影或当前车辆状态噪声。

30. 总结

速度平滑的本质，是把“路径上的速度上限”转化为“车辆物理上能连续执行的速度曲线”。它同时考虑效率、安全、舒适性和实时性。

解析 jerk 受限平滑器的核心优势在于清晰的运动学结构。它从常 jerk 积分公式出发，推导出三种减速曲线：梯形加速度、三角形加速度、仅恢复加速度。然后通过前向滤波跟随参考速度，通过后向滤波保证前方低速目标可达。它不是黑盒优化器，而是一套可解释的速度生成逻辑。

其他优化型平滑器则把问题写成二次规划：使用 $b=v^2$ 让速度和加速度关系线性化，用 pseudo jerk 或 jerk 代价控制平滑性，再通过软约束处理速度、加速度、jerk 限制。它们更灵活，但计算成本更高。