参考：ChatGPT、autoware

对应源码文件: autoware_devel/src/core/autoware_core/planning/autoware_velocity_smoother/src/node.cpp

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

1. 为什么自动驾驶需要速度平滑

自动驾驶规划通常会先得到一条几何轨迹，也就是车辆应该沿着哪条路径走。可是“走哪条路”和“用什么速度走”是两个不同问题。即使路径本身没有问题，如果速度变化很突兀，车辆仍然会出现急加速、急刹车、点头、乘客不适，甚至无法满足底盘控制器的物理能力。

速度平滑要解决的问题可以概括为：

$$\text{在给定路径上，生成一条满足速度、加速度、jerk 和横向运动约束的速度曲线}$$

这里的速度曲线不是单个速度值，而是沿路径每个位置的目标速度：

$$v=v(s)$$

其中 $s$ 是沿轨迹的弧长坐标。车辆从当前点出发，沿着 $s$ 增大的方向行驶，速度规划器要为未来一段距离生成目标速度、目标加速度，并尽量保证车辆能安全、舒适地跟踪。

速度平滑的核心矛盾是：

$$\text{速度越高，通行效率越好；变化越平滑，舒适性和可控性越好}$$

所以速度规划不是简单地把速度限制“削低”，而是在多个约束之间寻找一条合理的速度剖面。

2. 纵向运动的三个基本量

理解速度平滑，首先要把纵向运动拆成三层：

速度：

$$v(t)=\frac{dx}{dt}$$

加速度：

$$a(t)=\frac{dv}{dt}$$

Jerk，也叫加加速度：

$$j(t)=\frac{da}{dt}$$

在车辆纵向控制里，加速度影响“快慢变化”，jerk 影响“加速度变化得有多突然”。人的体感通常对 jerk 很敏感。一次刹车如果加速度从 $0$ 瞬间跳到 $-2.5m/{\mathrm{s}}^2$，即使最终减速度并不夸张，也会让乘客觉得突兀。

因此速度平滑一般会限制：

$$0\le v\le v_{\max }$$

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

$$j_{\min }\le j\le j_{\max }$$

其中 $a_{\min }<0$ 表示最大允许减速度，$j_{\min }<0$ 表示允许加速度向更负方向变化的下限。

在一段常 jerk 区间内，运动状态可以直接积分得到：

$$a(t)=a_0+jt$$

$$v(t)=v_0+a_{0}t+\frac{1}{2}j{t}^2$$

$$x(t)=x_0+v_{0}t+\frac{1}{2}{a}_0{t}^2+\frac{1}{6}j{t}^3$$

这三个公式是解析 jerk 受限速度规划的基础。后文所有停车距离、减速距离、速度曲线生成，本质上都是这些式子的分段组合。

3. 为什么速度规划常沿路径弧长建模

车辆实际随时间运动，但规划通常依附于路径点。路径点天然以空间位置排列，因此速度规划常使用弧长 $s$ 作为自变量。

对于连续运动，有：

$$a=\frac{dv}{dt}$$

又因为：

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

所以：

$$a=\frac{dv}{ds}\frac{ds}{dt}=v\frac{dv}{ds}$$

对 $v^2$ 求导：

$$\frac{d(v^2)}{ds}=2v\frac{dv}{ds}=2a$$

令：

$$b(s)=v(s{)}^2$$

则有一个非常重要的关系：

$$\frac{db}{ds}=2a$$

离散到轨迹点 $i$ 和 $i+1$，设两点弧长间隔为 $\Delta s_i$：

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

也就是：

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

这就是许多速度优化器喜欢用 $b=v^2$ 作为变量的原因。它把速度和加速度之间的关系变成了关于 $b$ 和 $a$ 的线性等式，适合构造成二次规划问题。

4. 输入轨迹到底提供了什么

速度平滑器面对的输入不是空白道路，而是一条已经带有若干速度上限的参考轨迹。每个轨迹点通常包含：

• 位姿：车辆应该经过的位置和朝向。
• 参考速度或速度上限：路径、地图、行为决策、停车点等已经给出的速度要求。
• 可能存在的停车点：速度为 $0$ 的点通常表示停止线、障碍物停车点或路径终点。

速度平滑器的任务不是重新决定路线，也不是重新决定所有行为，而是基于已有参考轨迹，生成一个更物理可行、更连续的速度曲线。

如果用数学语言描述，输入提供的是一个空间速度上限：

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

输出希望满足：

$$0\le v(s)\le v_{\mathrm{ref}}(s)$$

但如果距离太短、当前速度太高，可能无法严格满足所有限制。此时速度规划器通常会尽力降低速度，同时通过安全后处理保证停车点之后速度为 $0$。

5. 速度平滑的整体学习路线

一个完整速度平滑流程可以按以下思路理解：

1. 截取当前车辆附近的局部轨迹。
2. 叠加外部限速，例如场景规划给出的临时最大速度。
3. 在停车点附近加入接近停车速度，帮助车辆更稳定地停车。
4. 根据路径曲率限制速度，避免横向加速度过大。
5. 根据转向角速度限制速度，避免方向盘角速度过大。
6. 对轨迹重采样，使轨迹点间距适合速度规划。
7. 根据当前车辆状态和上一帧规划结果确定初始速度、初始加速度。
8. 用某种平滑器生成最终速度曲线。
9. 对停车点、最大速度、后方轨迹、输出采样进行后处理。

这些步骤不是孤立知识点，而是一条自然递进的链路：先得到“不能超过哪里”的速度上限，再考虑车辆“能不能平滑做到”，最后输出控制器能稳定使用的速度曲线。

6. 局部轨迹截取：只规划真正相关的未来

自动驾驶系统里的全局或局部轨迹可能很长，但速度规划没有必要每次处理整条路径。更常见的做法是，以车辆当前位置为中心，保留后方一小段和前方较长一段。

后方轨迹的意义是保证输出轨迹在车辆附近连续，方便控制器和调试工具使用。前方轨迹则用于预测接下来需要加速、减速、停车或过弯的位置。

设车辆当前位置对应弧长为 $s_0$，保留范围可以表示为：

$$s\in [s_0-L_{\mathrm{behind}},s_0+L_{\mathrm{ahead}}]$$

其中 $L_{\mathrm{behind}}$ 通常较小，$L_{\mathrm{ahead}}$ 根据规划视野设置得更长。

局部截取还有一个工程意义：速度优化器的计算量通常随点数增加而增加。只处理必要范围，可以在实时系统里保持稳定计算时间。

7. 外部限速：从“限速值”变成“限速位置”

外部模块可能随时给出一个新的最大速度，例如：

$$v_{\mathrm{limit}}$$

但速度平滑不能简单地从当前位置立刻把速度上限改成这个值。假设车辆当前速度是 $10m/s$，外部限速突然变成 $3m/s$，如果从当前位置硬限制，就意味着车辆必须瞬间降速，这是物理不可能的。

正确思路是：计算在加速度和 jerk 约束下，从当前状态降到目标限速需要多少距离。

给定初始状态：

$$v_0,a_0$$

目标速度：

$$v_{\mathrm{target}}=v_{\mathrm{limit}}$$

约束：

$$a\ge a_{\min },j_{\min }\le j\le j_{\max }$$

速度规划器先计算所需减速距离。令：

$$j_{\mathrm{acc}}=j_{\max }>0,j_{\mathrm{dec}}=j_{\min }<0,a_{\lim }=a_{\min }<0$$

这里 $j_{\mathrm{dec}}$ 用于让加速度变得更负，$j_{\mathrm{acc}}$ 用于让加速度从负值恢复到 $0$，$a_{\lim }$ 是允许达到的最小加速度。减速距离 $d_{\mathrm{decel}}$ 就是从 $(v_0,a_0)$ 平滑过渡到 $(v_{\mathrm{target}},0)$ 所需的位移。

先假设减速曲线会达到最小加速度 $a_{\lim }$，并保持一段恒定减速度。三个阶段分别是：

$$a_0\stackrel{j_{\mathrm{dec}}}{\to }{a}_{\lim },a_{\lim }\stackrel{0}{\to }{a}_{\lim },a_{\lim }\stackrel{j_{\mathrm{acc}}}{\to }0$$

第一段和第三段时间为：

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

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

第一段速度变化为：

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

第三段速度变化为：

$$\Delta v_3=\frac{1}{2}\frac{0-a_{\lim }}{j_{\mathrm{acc}}}{a}_{\lim }$$

中间恒定减速度阶段的时间由总速度变化决定：

$$t_2=\frac{v_{\mathrm{target}}-v_0-\Delta v_1-\Delta v_3}{a_{\lim }}$$

如果 $t_2>0$，说明减速距离采用“三段式”计算。先计算阶段 1 结束状态：

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

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

阶段 2 结束距离为：

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

阶段 2 结束速度为：

$$v_2=v_1+a_{\lim }{t}_2$$

最终减速距离为：

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

如果 $t_2\le 0$，说明不需要保持最小加速度平台，减速曲线会退化成更短的形式。先判断“用最大正 jerk 把当前加速度恢复到 $0$”产生的速度变化量是否足够：

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

这里要特别注意符号：减速时 $v_{\mathrm{target}}<v_0$，所以目标所需速度变化：

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

是一个负数；如果当前已经在减速，即 $a_0<0$，那么 $\Delta v_{\mathrm{recover}}$ 也是负数。因此下面这个判断比较的不是“正的减速量”，而是“带符号的速度变化量”。

若：

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

说明仅恢复加速度还不够，或者初始加速度大于0，说明车辆当前还在加速，需要使用后面的“两段式”三角加速度曲线。这个不等式之所以看起来反直觉，是因为两个量都是负数。把它改写成正的“需要降速量”会更直观。令：

$$R_{\mathrm{recover}}=-\Delta v_{\mathrm{recover}}$$

$$R_{\mathrm{req}}=v_0-v_{\mathrm{target}}$$

那么：

$$\Delta v_{\mathrm{recover}}>v_{\mathrm{target}}-v_0$$

等价于：

$$R_{\mathrm{recover}}<R_{\mathrm{req}}$$

也就是“仅靠恢复加速度带来的降速量，小于实际需要降速量”，所以还需要先继续把加速度压得更负一些。

反之，若车辆已经在减速且不需要额外压低加速度，就使用“一段式”恢复曲线。此时直接反求一个恢复 jerk $j_r$，让终点速度刚好到达 $v_{\mathrm{target}}$ 且终点加速度回到 $0$。由：

$$0=a_0+j_{r}t$$

和：

$$v_{\mathrm{target}}-v_0=a_{0}t+\frac{1}{2}{j}_r{t}^2$$

可得：

$$j_r=-\frac{a_0^2}{2(v_{\mathrm{target}}-v_0)}$$

对应时间为：

$$t=\frac{0-a_0}{j_r}$$

减速距离为：

$$d_{\mathrm{decel}}=v_{0}t+\frac{1}{2}{a}_0{t}^2+\frac{1}{6}{j}_r{t}^3$$

否则使用“两段式”三角加速度曲线。它的含义是：先用负 jerk $j_{\mathrm{dec}}$ 把加速度从 $a_0$ 压到某个负峰值 $a_p$，再用正 jerk $j_{\mathrm{acc}}$ 把加速度从 $a_p$ 恢复到 $0$：

$$a_0\stackrel{j_{\mathrm{dec}}}{\to }{a}_p\stackrel{j_{\mathrm{acc}}}{\to }0$$

这里 $a_p<0$，它就是两段式减速中达到的最大减速度。推导 $a_p$ 的关键，是把速度变化看成加速度-时间图下方的面积：

$$\Delta v=\int a(t)dt$$

两段式曲线的总速度变化必须等于目标速度变化：

$$v_{\mathrm{target}}-v_0=\Delta v_{a_0\to a_p}+\Delta v_{a_p\to 0}$$

不要把第一段默认理解成一定会经过 $0$。如果 $a_0>0$，负 jerk 确实会让加速度先从正值经过 $0$，再继续变成负值；但如果 $a_0<0$，加速度本来就在负区间，第一段只是从一个负加速度继续变成更负的 $a_p$，并不会真实经过 $0$。因此更严谨的推导应该直接使用从 $a_0$ 到 $a_p$ 的一般形式：

$$v_{\mathrm{target}}-v_0=\Delta v_{a_0\to a_p}+\Delta v_{a_p\to 0}$$

对于常 jerk 阶段，从加速度 $a_s$ 变化到 $a_e$ 的时间是：

$$t=\frac{a_e-a_s}{j}$$

因为这段时间内加速度线性变化，速度变化量等于梯形面积：

$$\Delta v=\frac{a_s+a_e}{2}t$$

所以第一段 $a_0\to a_p$ 的速度变化是：

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

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

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

把两项代回总速度变化方程：

$$v_{\mathrm{target}}-v_0=\frac{a_p^2-a_0^2}{2j_{\mathrm{dec}}}-\frac{a_p^2}{2j_{\mathrm{acc}}}$$

把与 $a_p^2$ 有关的项合并：

$$v_{\mathrm{target}}-v_0+\frac{a_0^2}{2j_{\mathrm{dec}}}=\frac{a_p^2}{2}\left(\frac{1}{j_{\mathrm{dec}}}-\frac{1}{j_{\mathrm{acc}}}\right)$$

括号里的项继续通分：

$$\frac{1}{j_{\mathrm{dec}}}-\frac{1}{j_{\mathrm{acc}}}=\frac{j_{\mathrm{acc}}-j_{\mathrm{dec}}}{j_{\mathrm{acc}}{j}_{\mathrm{dec}}}$$

因此：

$$a_p^2=\left(v_{\mathrm{target}}-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_p^2=\left(v_{\mathrm{target}}-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_p$ 表示减速峰值，加速度方向为负，所以取负根：

$$a_p=-\sqrt{\left(v_{\mathrm{target}}-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}}}}$$

对应时间为：

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

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

第一段结束状态：

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

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

最终减速距离为：

$$d_{\mathrm{decel}}=x_1+v_1{t}_2+\frac{1}{2}{a}_p{t}_2^2+\frac{1}{6}{j}_{\mathrm{acc}}{t}_2^3$$

工程上通常还会给这个距离增加一个安全余量 $d_{\mathrm{margin}}$，因此外部限速真正插入的位置距离为：

$$d_{\mathrm{insert}}=d_{\mathrm{decel}}+d_{\mathrm{margin}}$$

然后把外部限速插入到车辆前方约 $d_{\mathrm{insert}}$ 的位置，并从那里开始把速度上限压到 $v_{\mathrm{limit}}$：

$$v_{\max }(s)=\{\begin{array}{ll}\min (v_{\max }(s),v_{\mathrm{peak}}),& s<s_0+d_{\mathrm{insert}}\\ \min (v_{\max }(s),v_{\mathrm{limit}}),& s\ge s_0+d_{\mathrm{insert}}\end{array}$$

这里的 $v_{\mathrm{peak}}$ 用于处理一个常被忽略的细节：如果当前加速度 $a_0>0$，车辆即使立刻施加负 jerk，加速度也不会瞬间降到 $0$，速度还会继续上升一小段。若使用负 jerk $j_{\min }$ 让加速度从 $a_0$ 下降到 $0$，所需时间为：

$$t=\frac{0-a_0}{j_{\min }}$$

这段时间的速度增量是：

$$\Delta v=a_{0}t+\frac{1}{2}{j}_{\min }{t}^2$$

代入 $t$ 可得：

$$\Delta v=-\frac{1}{2}\frac{a_0^2}{j_{\min }}$$

因为 $j_{\min }<0$，所以 $\Delta v>0$。因此未来可能达到的峰值速度是：

$$v_{\mathrm{peak}}=v_0-\frac{1}{2}\frac{a_0^2}{j_{\min }}$$

这解释了为什么外部限速不仅是一个速度数值，还必须结合当前运动状态转化为空间上的限速位置。

8. 停车接近速度：让最后几米更稳定

轨迹中的 $v=0$ 点通常表示停车点。为了提高停车精度，有时会在停车点前一段距离内施加一个较低但非零的接近速度：

$$v_{\mathrm{approach}}$$

设停车点弧长为 $s_{\mathrm{stop}}$，接近区域长度为 $d_{\mathrm{approach}}$，则：

$$s\in [s_{\mathrm{stop}}-d_{\mathrm{approach}},s_{\mathrm{stop}}]$$

在这个范围内限制：

$$v(s)\le v_{\mathrm{approach}}$$

这样车辆不会以较高速度冲到停车点附近再急刹，而是提前进入低速接近状态。对于停止线、障碍物前停车、拥堵跟停，这个策略都能提升最终停止位置的可控性。

如果 $d_{\mathrm{approach}}=0$，则表示不额外启用这个策略。

9. 曲率限速：横向加速度约束

纵向速度规划不能只看纵向。车辆过弯时，速度越高，横向加速度越大。路径曲率为 $\kappa$ 时，横向加速度近似为：

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

为了保证舒适性和稳定性，需要满足：

$$v^2|\kappa |\le a_{\mathrm{lat},\max }$$

所以曲率对应的最大允许速度是：

$$v\le \sqrt{\frac{a_{\mathrm{lat},\max }}{|\kappa |}}$$

当 $\kappa$ 很小，接近直线，允许速度会很大；当 $\kappa$ 很大，弯道很急，允许速度会下降。

实际系统里，横向加速度上限往往随速度区间变化。它不是只有一个固定的 $a_{\mathrm{lat},\max }$，而是由一组速度阈值和一组横向加速度阈值共同定义。设速度阈值为：

$$0<v_1<v_2<\cdots <v_n$$

横向加速度上限为：

$$A_1,A_2,\cdots ,A_n$$

那么可以理解成一组分段约束：

$$a_{\mathrm{lat},\max }=\{\begin{array}{ll}{A}_1,& 0\le v<v_1\\ A_2,& v_1\le v<v_2\\ \cdots & \\ A_n,& v_{n-1}\le v\end{array}$$

这里为了书写简洁，把 $v_{n-1}\le v<v_n$ 和 $v\ge v_n$ 都归到最后一个限制值 $A_n$。实际理解时，可以把 $v_n$ 之后看成最高速度延伸区间，它仍沿用 $A_n$。

速度越高，通常越需要保守的横向加速度限制。

最后一个阈值之后还可以理解成最高速度区间 $[v_n,\infty )$。这个区间仍使用最后一个横向加速度上限 $A_n$。它对应的曲率范围可以写成：

$$0<|\kappa |\le \frac{A_n}{v_n^2}$$

意思是：如果道路曲率小到满足 $|\kappa |\le A_n/v_n^2$，那么车辆即使用不低于 $v_n$ 的速度行驶，横向加速度也不会超过 $A_n$。此时曲率限速计算出来的允许速度会大于等于 $v_n$；如果曲率接近 $0$，允许速度会非常大，实际速度就主要由全局最大速度、前方限速、停车点等其他约束决定，而不是由横向加速度决定。

但真正做限速时，并不是先知道车辆速度再查 $A_i$，而是反过来：已知当前路径曲率 $|\kappa |$，要判断这个曲率允许车辆处在哪个速度区间。因为：

$$v^2|\kappa |\le A_i$$

等价于：

$$|\kappa |\le \frac{A_i}{v^2}$$

所以每个速度区间都会转换成一个“允许曲率区间”。对第 $i$ 个速度区间 $[v_{i-1},v_i]$，其中 $v_0=0$，对应的曲率边界可以写成：

$$K_i^{\mathrm{high}}=\frac{A_i}{v_{i-1}^2+ϵ}$$

$$K_i^{\mathrm{low}}=\frac{A_i}{v_i^2+ϵ}$$

这里 $ϵ$ 是很小的正数，用来避免 $v=0$ 时除零。于是第 $i$ 个速度区间可以理解为：

$$K_i^{\mathrm{low}}<|\kappa |\le K_i^{\mathrm{high}}$$

如果当前曲率 $|\kappa |$ 落在这个范围内，就说明这个速度区间是合适的，并可用下面的公式反推出该区间内允许的最高速度：

$$v_{\kappa }=\sqrt{\frac{A_i}{|\kappa |}}$$

如果 $|\kappa |$ 比某个速度区间的 $K_i^{\mathrm{high}}$ 还大，说明这个弯太急，连这个区间的低速端都无法满足横向加速度限制，需要继续往更低速度区间找。反之，如果 $|\kappa |$ 比最高速度区间的下边界还小，说明这段路几乎不需要因为横向加速度而限速。

举个区间化的例子。假设：

$$v_1=5,v_2=10,v_3=20$$

$$A_1=1.0,A_2=0.8,A_3=0.6$$

则区间 $[5,10]$ 的曲率边界是：

$$K_2^{\mathrm{high}}=\frac{0.8}{5^2}=0.032$$

$$K_2^{\mathrm{low}}=\frac{0.8}{10^2}=0.008$$

如果当前窗口曲率约为 $0.02$，它落在 $(0.008,0.032]$ 内，那么允许速度就是：

$$v_{\kappa }=\sqrt{\frac{0.8}{0.02}}\approx 6.32m/s$$

这个速度位于 $[5,10]$ 区间内，所以它和区间定义是一致的。

曲率限速还有几个重要工程细节。

第一，不能只在曲率最大的一个点限速。车辆需要在入弯前提前减速，也需要在出弯后一小段保持稳定。因此会使用弯道前后距离窗口：

$$[s_i-d_{\mathrm{before}},s_i+d_{\mathrm{after}}]$$

对窗口内的最大曲率进行限速。

离散轨迹中，如果重采样点间距为 $\Delta s$，那么窗口距离会转换成索引范围：

$$N_{\mathrm{before}}=\mathrm{round}\left(\frac{d_{\mathrm{before}}}{\Delta s}\right)$$

$$N_{\mathrm{after}}=\mathrm{round}\left(\frac{d_{\mathrm{after}}}{\Delta s}\right)$$

对某个轨迹点 $i$，会在一个前后索引窗口内取最大曲率：

$${\kappa }_i^{\mathrm{win}}=\underset{j\in {\mathcal{W}}_i}{\max }|{\kappa }_j|$$

其中 ${\mathcal{W}}_i$ 是点 $i$ 附近的弯道影响窗口。这样即使真正的曲率峰值在前方某个点，当前点也会提前受到它影响，从而开始降速；曲率峰值刚过之后，也会保留一段低速，避免刚出弯就立刻加速。

第二，曲率限速不应让速度低于一个最低过弯速度：

$$v_{\mathrm{curve}}\ge v_{\mathrm{curve},\min }$$

这不是说可以无视物理限制，而是避免路径曲率噪声导致车辆在很小的弯曲处被压到极低速度。最终速度限制会取：

$$v_{\mathrm{limit},\mathrm{curve}}=\max \left(v_{\mathrm{curve},\min },\sqrt{\frac{a_{\mathrm{lat},\max }}{|\kappa |}}\right)$$

解析平滑器里还有一种思路：如果一段连续弯道都被横向加速度限制影响，可以把这段弯中速度统一成该段最低限速。这样速度不会在弯中来回抖动，更接近人类驾驶习惯：入弯前减速，弯中稳定，出弯后加速。

这一步也可以理解成“连续限速点合并区间”。假设被曲率限速影响的点索引集合为：

F = { i 1 , i 2 , ⋯   , i m } \mathcal{F}=\{i_1,i_2,\cdots,i_m\} F={i1​,i2​,⋯,im​}

如果相邻两个限速点在空间上的距离小于阈值：

$$\mathrm{dist}(i_k,i_{k+1})<d_{\mathrm{merge}}$$

就把它们视为同一个弯道区间。一个弯道区间可表示为：

$$[i_{\mathrm{start}},i_{\mathrm{end}},v_{\min }]$$

其中：

$$v_{\min }=\underset{i_{\mathrm{start}}\le i\le i_{\mathrm{end}}}{\min }{v}_{\kappa ,i}$$

如果启用“弯中恒速”，则该区间内所有点都使用同一个最低速度：

$$v_i=v_{\min },i_{\mathrm{start}}\le i\le i_{\mathrm{end}}$$

这样能避免弯中速度出现 $3.8\to 3.4\to 3.1\to 3.3\to 3.7$ 这种小幅波动，而是变成更稳定的 $3.1\to 3.1\to 3.1\to 3.1\to 3.1$。

10. 转向角速度限速：不要让方向盘转得太快

横向加速度限制关注的是“车身受到多大横向加速度”，而转向角速度限制关注的是“方向盘或前轮转角变化得多快”。

使用简化自行车模型，前轮转角 $\delta$ 与曲率 $\kappa$ 的关系为：

$$\delta =\arctan (L\kappa )$$

其中 $L$ 是轴距。

沿路径前进时，转角对时间的变化率为：

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

如果允许最大转向角速度为 ${\dot{\delta }}_{\max }$，则需要：

$$v\left|\frac{d\delta }{ds}\right|\le {\dot{\delta }}_{\max }$$

因此对应的速度上限为：

$$v\le \frac{{\dot{\delta }}_{\max }}{\left|\frac{d\delta }{ds}\right|}$$

实际离散轨迹中，可以用相邻点转角差近似：

$$\left|\frac{d\delta }{ds}\right|\approx \frac{|{\delta }_{i+1}-{\delta }_i|}{\Delta s_i}$$

转向角速度限制也有类似的速度区间逻辑。它通常使用同一组速度阈值：

$$0<v_1<v_2<\cdots <v_n$$

设不同速度区间下允许的最大转向角速度为：

$${\Omega }_1,{\Omega }_2,\cdots ,{\Omega }_n$$

其中 ${\Omega }_i$ 通常由角度制参数转换成弧度制。那么最大转向角速度也可以写成分段形式：

$${\dot{\delta }}_{\max }(v)=\{\begin{array}{ll}{\Omega }_1,& 0\le v<v_1\\ {\Omega }_2,& v_1\le v<v_2\\ \cdots & \\ {\Omega }_n,& v_{n-1}\le v\end{array}$$

这里同样把 $v_{n-1}\le v<v_n$ 和 $v\ge v_n$ 合并到了最后一个限制值 ${\Omega }_n$。也就是说，超过最后一个速度阈值后，仍沿用最后一档转向角速度上限。

它的含义和横向加速度分段很像：低速时车辆允许方向盘或前轮转得快一些，因为低速下横摆和侧向风险较小；高速时通常要降低允许转角速度，因为高速快速打方向会带来更强的横向动态冲击，也更容易让控制不稳定。

根据：

$$v\left|\frac{d\delta }{ds}\right|\le {\Omega }_i$$

可得到：

$$\left|\frac{d\delta }{ds}\right|\le \frac{{\Omega }_i}{v}$$

于是第 $i$ 个速度区间 $[v_{i-1},v_i]$ 对应的转角空间变化率边界是：

$$S_i^{\mathrm{high}}=\frac{{\Omega }_i}{v_{i-1}+ϵ}$$

$$S_i^{\mathrm{low}}=\frac{{\Omega }_i}{v_i+ϵ}$$

其中：

$$S=\left|\frac{d\delta }{ds}\right|$$

如果当前路径段的 $S$ 落入某个区间，就可以用：

$$v_{\delta }=\frac{{\Omega }_i}{S}$$

反推出转向角速度约束下的局部速度上限。

同样，最后一个速度阈值之后可以看作最高速度区间 $[v_n,\infty )$。如果 $S$ 足够小，说明即使速度很高，转向角速度也不会超过限制，此时转向角速度约束本身不需要进一步压低速度。

实际处理时还会加几个细节。首先，只在曲率足够大时才触发转向角速度限速：

$$|{\kappa }_i|\ge {\kappa }_{\mathrm{thr}}$$

这样直线或轻微曲率噪声不会触发无意义限速。其次，$S$ 这种差分量容易受离散点噪声影响，所以会对相邻点的 $S$ 做简单均值滤波：

$$S_i\leftarrow \frac{S_{i-1}+S_i+S_{i+1}}{3}$$

最后，转向角变化发生在 $i\to i+1$ 这段 segment 上，因此会用两端点平均速度判断这段是否超限：

$${\bar{v}}_i=\frac{v_i+v_{i+1}}{2}$$

如果：

$${\bar{v}}_i>v_{\delta ,i}$$

就需要降低这两个端点的速度。为了不破坏原有速度曲线形状，通常不是简单把两个点都设成 $v_{\delta ,i}$，而是按比例缩放：

$$v_i^{\mathrm{new}}=\min \left(v_i,\max \left(v_{\mathrm{curve},\min },\min \left(v_{\delta ,i},v_i\frac{v_{\delta ,i}}{{\bar{v}}_i}\right)\right)\right)$$

另一个端点 $v_{i+1}$ 也按同样方式处理。这样既把 segment 的平均速度压到可接受范围附近，又尽量保留原始速度分布。转向角速度限制能防止路径曲率突然变化时车辆需要过快打方向。它和横向加速度限制互补：一个限制横向力，一个限制转向执行变化率。

10.1 这些速度区间和限制值应该怎么设置

横向加速度限制和转向角速度限制里的速度区间不是算法自动推导出来的，而是工程参数。通常会配置三类数组：

$$\text{velocity thresholds}:v_1,v_2,\cdots ,v_n$$

$$\text{lateral acceleration limits}:A_1,A_2,\cdots ,A_n$$

$$\text{steering angle rate limits}:{\Omega }_1,{\Omega }_2,\cdots ,{\Omega }_n$$

第一组 $v_i$ 决定速度轴怎么切段；第二组 $A_i$ 决定每个速度段允许多大的横向加速度；第三组 ${\Omega }_i$ 决定每个速度段允许多快的转向角速度。通常这三组数组长度要一致，并且速度阈值必须单调递增：

$$v_1<v_2<\cdots <v_n$$

设置速度区间时，应该优先围绕车辆控制特性和典型驾驶场景来切分，而不是平均分。常见做法是把速度分成低速、中速、高速几个区域：

$$[0,v_1),[v_1,v_2),\cdots ,[v_{n-1},\infty )$$

低速区间通常覆盖起步、停车、近距离绕行、泊车或拥堵跟车。这个区间可以允许相对更高的转向角速度，因为车速低，车辆对方向变化更容易稳定跟踪。

中速区间通常覆盖城市道路常规行驶。这个区间的横向加速度和转向角速度要在效率与舒适之间折中，既不能让车辆过弯太慢，也不能让乘客明显感到侧向冲击。

高速区间通常覆盖快速路或直线路段。这个区间应该更保守，特别是转向角速度上限一般要降低，因为高速下小角度变化都会产生明显横向动态。

设置横向加速度上限 $A_i$ 时，需要考虑：

• 乘坐舒适性：$A_i$ 越小，过弯越保守，乘客侧向体感越轻。
• 车辆稳定性：高速时不宜给过大的横向加速度上限。
• 路径曲率噪声：如果路径曲率有抖动，过小的 $A_i$ 会导致频繁限速。
• 轮胎、路面、载荷：湿滑路面、重载车辆、低附着场景应更保守。

设置转向角速度上限 ${\Omega }_i$ 时，需要考虑：

• 转向执行器能力：车辆底盘是否真的能稳定跟踪这么快的转角变化。
• 控制器响应：过高的 ${\Omega }_i$ 可能让控制器频繁大幅调整方向。
• 路径平滑程度：路径曲率不连续或噪声较多时，应避免过大的转向角速度上限。
• 高速稳定性：速度越高，${\Omega }_i$ 通常应越小。

还要注意一个容易混淆的点：这些速度区间不是最终速度命令，而是“选择限制值的条件”。算法会先根据当前曲率或转角空间变化率，判断它适合哪个速度区间，再反推出局部速度上限：

$$v_{\kappa }=\sqrt{\frac{A_i}{|\kappa |}}$$

或：

$$v_{\delta }=\frac{{\Omega }_i}{S}$$

最终发布的速度还会继续受到外部限速、停车点、最大速度、纵向加速度和 jerk 平滑的共同约束。因此，区间参数调得越激进，不代表车辆一定会开到那个速度；它只是减少了横向约束对速度的压制。

一个实用的调参顺序是：

1. 先设定保守的 $A_i$ 和 ${\Omega }_i$，保证车辆过弯和变曲率路段稳定。
2. 检查弯道是否过慢，如果过慢，再逐步提高对应速度区间的 $A_i$。
3. 检查方向盘动作是否频繁或突兀，如果突兀，降低对应速度区间的 ${\Omega }_i$，或改善路径平滑。
4. 高速区间优先保守，低速区间可以适度放宽。
5. 每次只调整一个区间，观察该速度段内的横向加速度、转向角速度、目标速度和控制误差。

11. 重采样：让轨迹点适合速度规划

路径点间距对速度规划影响很大。如果点太密，优化或滤波计算量会增加；如果点太疏，停车点、弯道限速、加速度变化可能表达不准。

一种常见策略是根据当前速度进行时间意义上的空间采样。若当前速度为 $v_{\mathrm{ego}}$，密集采样时间间隔为 $\Delta t_{\mathrm{dense}}$，则理想点距为：

$$\Delta s_{\mathrm{dense}}=\max (v_{\mathrm{ego}}\Delta t_{\mathrm{dense}},\Delta s_{\min })$$

在近处或规划关注时间内使用密集采样，在更远处使用稀疏采样：

$$\Delta s_{\mathrm{sparse}}=\max (v_{\mathrm{ego}}\Delta t_{\mathrm{sparse}},\Delta s_{\mathrm{sparse},\min })$$

这背后的直觉很简单：

• 低速、停车、近距离区域需要更高空间分辨率。
• 高速远距离区域不需要过密点，否则计算量浪费。
• 停车点附近需要确保 $v=0$ 点被保留下来。

解析平滑器使用的重采样思路更直接，常按路径段插值生成固定数量的中间点，使后续解析滤波在更均匀的空间点上运行。

12. 初始状态：从当前车还是上一帧规划开始

速度规划每一帧都会重新计算，但车辆运动应该连续。因此初始速度和初始加速度的选择非常重要。

理想情况下，本帧规划应从上一帧已发布轨迹在当前车辆位置处的状态继续：

$$v_0=v_{\mathrm{prev}}(s_{\mathrm{ego}})$$

$$a_0=a_{\mathrm{prev}}(s_{\mathrm{ego}})$$

这样相邻两帧之间不会因为传感器噪声或规划扰动出现速度、加速度跳变。

但在以下情况，应改用车辆当前状态或特殊起步状态：

首次规划时，没有上一帧轨迹可用：

$$v_0=v_{\mathrm{ego}},a_0=0$$

车辆实际速度和上一帧期望速度偏差过大时：

$$|v_{\mathrm{ego}}-v_{\mathrm{prev}}|>{ϵ}_v$$

应从当前车速重新规划，避免继续追随已经不可信的旧速度曲线。

车辆从静止或低速进入自动驾驶起步时，可以使用一个很小的 engage 速度和 engage 加速度：

$$v_0=v_{\mathrm{engage}},a_0=a_{\mathrm{engage}}$$

但如果停车点就在很近处，就不应该再起步，否则会出现刚动又停的体验。

如果车辆还未处于自动驾驶控制状态，为了从人工驾驶平滑切入自动驾驶，也应优先从车辆实际速度和合理裁剪后的加速度开始规划。

13. 倒车轨迹的处理思路

速度平滑算法通常默认速度为非负，沿轨迹弧长正方向规划。如果轨迹表示倒车，速度可能为负。

一种实用做法是：

1. 规划前把负速度取反，转成正向速度问题。
2. 使用同一套速度平滑逻辑。
3. 输出前再把速度符号翻回去。

这样可以复用大部分纵向速度规划算法，同时保留倒车轨迹的语义。

14. 解析 Jerk 受限平滑器的核心思想

现在进入本文重点：基于 AnalyticalJerkConstrainedSmoother 的解析 jerk 受限速度规划。

它的目标不是通过通用优化器求一个全局二次规划，而是使用解析运动学公式和前后向滤波来构造速度曲线。它更像一个规则清晰、计算快速的速度生成器。

它的核心由三部分组成：

1. 前向 jerk 滤波：从当前状态向前积分，尽量跟随参考速度，同时限制加速度和 jerk。
2. 减速目标搜索：找出轨迹上必须降速的局部低速点，例如停车点、弯道低速点、外部限速点。
3. 后向减速滤波：从低速目标向前反推，判断需要从哪里开始减速，才能在 jerk 和加速度约束下刚好到达目标速度。

这套方法的直觉非常像人类驾驶：

• 平时尽量平顺加速或保持速度。
• 看到前方限速、弯道或停车点，就提前算刹车距离。
• 如果距离充足，就用温和 jerk 减速。
• 如果距离不足，就逐步允许更强 jerk 和更大减速度。

15. 前向 jerk 滤波：跟随参考速度但不猛加速

先把这里的“起点”和“目标点”说清楚，因为它和后向减速滤波里的 $s_0\to s_T$ 不完全一样。

前向 jerk 滤波的起点是一个“状态锚点”，也就是从哪个轨迹点开始向前积分。记为：

$$s_{\mathrm{start}}$$

对应状态为：

$$v_{\mathrm{start}},a_{\mathrm{start}}$$

它的选择标准是：

1. 如果当前还没有处理任何减速目标，起点就是当前车辆最近点，也就是裁剪后轨迹的第一个点：

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

此时起点状态来自速度规划的初始状态：

$$v_{\mathrm{start}}=v_{\mathrm{init}},a_{\mathrm{start}}=a_{\mathrm{init}}$$

2. 如果已经处理过前一个减速目标，那么下一次前向滤波会从前一个减速目标点重新开始。这里的“前一个减速目标”不是上一帧规划的目标，而是同一条轨迹上、按弧长顺序排在当前减速目标之前的那个低速目标。比如一条轨迹上依次有三个减速目标：

$$(s_{T,0},v_{T,0}),(s_{T,1},v_{T,1}),(s_{T,2},v_{T,2})$$

当正在处理第 $k$ 个减速目标 $(s_{T,k},v_{T,k})$ 时，前一个减速目标就是：

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

这些目标通常来自速度曲线上的局部低速点，例如前方弯道最低速度点、外部限速点、停车点或终点。前一个减速目标已经被前面的前向/后向滤波处理过，它的速度和加速度已经比较可信，所以后续前向滤波从这里继续：

$$s_{\mathrm{start}}=s_{T,k-1}$$

此时起点状态不是重新取车辆状态，而是使用已经滤波后的结果：

$$v_{\mathrm{start}}=v_{\mathrm{filtered}}(s_{T,k-1})$$

$$a_{\mathrm{start}}=a_{\mathrm{filtered}}(s_{T,k-1})$$

这样做的原因是：前一个低速目标可能已经把速度压低了，从它之后车辆可能需要重新加速、再进入下一个限速区域。前向滤波必须从这个已经确定的低速状态继续向前推，而不能跳回自车点重新算。

3. 如果整条轨迹没有减速目标，那么前向滤波只从当前车辆最近点开始，一直处理到轨迹末端。

4. 如果所有减速目标都处理完了，还会从最后一个减速目标再次向前滤波，补齐最后一个目标之后的剩余轨迹。

因此，前向滤波里的“目标点”不是某一个固定终点。它每推进一步，就把当前要更新的轨迹点 $i$ 当成局部目标点。这个点的参考速度是：

$$v_{\mathrm{ref},i}$$

这里的 $v_{\mathrm{ref},i}$ 已经不是原始地图速度，而是叠加了外部限速、停车接近速度、曲率限速、转向角速度限速之后的速度上限。前向滤波要做的是：从 $s_{\mathrm{start}}$ 的状态出发，沿着轨迹逐点向前传播，让速度尽量接近每个点的 $v_{\mathrm{ref},i}$，但加速度和 jerk 不能超过限制。

所以它的形式更像：

$$(v_{\mathrm{start}},a_{\mathrm{start}})\to v_{\mathrm{ref},start+1}\to v_{\mathrm{ref},start+2}\to \cdots$$

而不是：

$$s_0\to s_T$$

后者是后向减速滤波的思路。

设轨迹点为 $i=0,1,\dots ,N-1$，相邻点距离为：

$$\Delta s_i$$

已知上一点速度和加速度：

$$v_{i-1},a_{i-1}$$

为了从空间点推进到下一个点，需要估计时间步长。解析平滑器使用近似：

$$\Delta t_i=\frac{\Delta s_i}{\max (v_{i-1},v_{\min })}$$

其中 $v_{\min }$ 是防止低速除零的下限。

先用上一点加速度预测当前速度：

$${\hat{v}}_i=\max (v_{i-1}+a_{i-1}\Delta t_i,0)$$

再看参考速度和预测速度的误差：

$$e_i=v_{\mathrm{ref},i}-{\hat{v}}_i$$

用一个比例控制形式生成期望加速度：

$$a_{\mathrm{fb},i}=k_p{e}_i$$

然后对加速度限幅：

$$a_{\lim ,i}=\mathrm{clip}(a_{\mathrm{fb},i},a_{\min },a_{\max })$$

由上一点加速度到目标加速度，对应 jerk 为：

$$j_{\mathrm{fb},i}=\frac{a_{\lim ,i}-a_{i-1}}{\Delta t_i}$$

再对 jerk 限幅：

$$j_i=\mathrm{clip}(j_{\mathrm{fb},i},j_{\min },j_{\max })$$

于是当前点加速度为：

$$a_i=a_{i-1}+j_i\Delta t_i$$

速度则使用前面预测的 ${\hat{v}}_i$。这会生成一条向前传播的速度曲线，具有较好的实时性和连续性。

但前向滤波有一个天然缺陷：它只看当前和过去，不知道前方低速目标是否需要提前很远减速。因此它必须和后向减速滤波配合。

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