Autoware Elastic Band 路径平滑算法

参考来源：ChatGPT、autoware

Autoware Elastic Band 路径平滑算法

1. 先理解它要解决什么问题

自动驾驶规划链路里，上游模块通常已经给出了一条可以行驶的参考路径。可是这条路径未必足够光顺：它可能来自车道中心线、避障模块、路径拼接、离散采样或历史轨迹复用，因此局部会出现轻微锯齿、折角、航向跳变。

这些小的几何不连续看起来不严重，但会放大到后续模块里。尤其是基于 Frenet 坐标系或模型预测轨迹优化的方法，会频繁使用参考路径的切向、法向、曲率和横向误差。如果参考路径本身抖动，Frenet 投影、航向误差、曲率估计和转向输入都会变得不稳定。

Elastic Band 平滑器的目标不是重新规划一条全新的路径，而是在原始路径附近做一次局部几何修正：

在不大幅偏离原始路径的前提下，让离散路径尽量光顺。 \text{在不大幅偏离原始路径的前提下，让离散路径尽量光顺。} 在不大幅偏离原始路径的前提下，让离散路径尽量光顺。

这里有两个关键词。

第一，局部。它只处理自车附近一段有限长度的路径，而不是整条全局路径。

第二，小幅修正。它不会允许路径点随意移动，而是限制每个点只能沿参考路径的横向方向移动一小段距离。

所以 Autoware 里的 Elastic Band 可以先记成一句话：

Autoware Elastic Band 是一个带横向位移约束的离散路径平滑 QP。 \text{Autoware Elastic Band 是一个带横向位移约束的离散路径平滑 QP。} Autoware Elastic Band 是一个带横向位移约束的离散路径平滑 QP。

它名字里有 Elastic Band，但它并不是传统机器人规划里那种"弹簧内力 + 障碍物斥力"的物理迭代法。Autoware 版本更像是把路径点当成一串受限的控制点，通过二次规划求出每个点的横向偏移量。

2. 路径为什么可以被看成一串离散点

一条连续路径可以写成平面曲线：

p(s)= $x(s)y(s)$ \mathbf{p}(s)=\begin{bmatrix} x(s) \\ y(s) \end{bmatrix} p(s)= $x(s)y(s)$

其中 sss 是沿路径的弧长。实际工程里不直接处理连续函数，而是按一定弧长间隔采样，得到 NNN 个离散点：

r0,r1,...,rN−1 \mathbf{r}_0,\mathbf{r}1,\dots,\mathbf{r}{N-1} r0,r1,...,rN−1

第 iii 个参考点写作：

ri= $xiyi$ \mathbf{r}_i=\begin{bmatrix} x_i \\ y_i \end{bmatrix} ri= $xiyi$

每个点还带有一个参考航向角 θi\theta_iθi。由航向角可以构造路径局部坐标系：

ti= $cosθisinθi$ ,ni= $-sinθicosθi$ \mathbf{t}_i=\begin{bmatrix} \cos\theta_i \\ \sin\theta_i \end{bmatrix}, \qquad \mathbf{n}_i=\begin{bmatrix} -\sin\theta_i \\ \cos\theta_i \end{bmatrix} ti= $cosθisinθi$ ,ni= $-sinθicosθi$

ti\mathbf{t}_iti 是切向量，表示路径前进方向；ni\mathbf{n}_ini 是左法向量，表示横向方向。

Autoware Elastic Band 的关键限制是：

每个点的纵向移动为 0,只允许沿法向横向移动。 \text{每个点的纵向移动为 }0,\quad \text{只允许沿法向横向移动。} 每个点的纵向移动为 0,只允许沿法向横向移动。

因此优化后的点不是任意二维变量，而是：

pi=ri+lini \mathbf{p}_i = \mathbf{r}_i+l_i\mathbf{n}_i pi=ri+lini

其中 lil_ili 是第 iii 个点的横向偏移量。把所有横向偏移量收集起来：

l= $l0l1\dotslN-1$ T∈RN \mathbf{l}=\begin{bmatrix} l_0 & l_1 & \cdots & l_{N-1} \end{bmatrix}^{T}\in\mathbb{R}^{N} l= $l0l1\dotslN-1$ T∈RN

这就是 Elastic Band 真正优化的变量。

这个设计非常重要。如果直接优化每个点的 xi,yix_i,y_ixi,yi，变量维度是 2N2N2N，路径点可能沿前后方向乱滑，原始路径的采样顺序和局部结构也更容易被破坏。改成只优化 lil_ili 后，问题维度变成 NNN，每个点只能在一条很短的横向线段上移动，路径拓扑和前后顺序更稳定。

3. 什么叫"路径更平滑"

对离散路径来说，平滑性可以从相邻线段的变化来理解。

第 kkk 个点前后的两段离散切向量近似为：

pk−pk−1,pk+1−pk \mathbf{p}k-\mathbf{p}{k-1}, \qquad \mathbf{p}_{k+1}-\mathbf{p}_k pk−pk−1,pk+1−pk

如果路径局部很平顺，这两个向量应该接近；如果路径出现折角，它们差得就会比较大。因此可以用下面这个量衡量局部折角：

(pk+1−pk)−(pk−pk−1) (\mathbf{p}_{k+1}-\mathbf{p}_k)-(\mathbf{p}k-\mathbf{p}{k-1}) (pk+1−pk)−(pk−pk−1)

整理后得到：

pk+1−2pk+pk−1 \mathbf{p}_{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} pk+1−2pk+pk−1

这就是离散二阶差分。它对应连续曲线里的二阶导数或曲率趋势：一阶差分描述"方向"，二阶差分描述"方向变化"。

于是路径平滑的核心代价可以写成：

Jsmooth=∑k=1N−2∥pk+1−2pk+pk−1∥2 J_{\text{smooth}} = \sum_{k=1}^{N-2} \left\| \mathbf{p}_{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} \right\|^2 Jsmooth=k=1∑N−2∥pk+1−2pk+pk−1∥2

如果某一段路径近似为直线，相邻线段方向几乎不变，二阶差分就接近 000。如果路径有尖角，二阶差分会变大。

这也解释了 Autoware 文档里提到的"菱形对角线"直觉：由前一点、当前点、后一点形成的局部几何关系中，二阶差分向量正是在描述前后两条边的差异。让这个向量短，就等价于让局部折角小。

展开到坐标形式，令：

pk= $xkyk$ \mathbf{p}_k=\begin{bmatrix} x_k \\ y_k \end{bmatrix} pk= $xkyk$

则：

∥pk+1−2pk+pk−1∥2=(xk+1−2xk+xk−1)2+(yk+1−2yk+yk−1)2 \left\| \mathbf{p}{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} \right\|^2 = (x{k+1}-2x_k+x_{k-1})^2 + (y_{k+1}-2y_k+y_{k-1})^2 ∥pk+1−2pk+pk−1∥2=(xk+1−2xk+xk−1)2+(yk+1−2yk+yk−1)2

所以总代价是：

Jsmooth=∑k=1N−2 $(xk+1-2xk+xk-1)2+(yk+1-2yk+yk-1)2$ J_{\text{smooth}} = \sum_{k=1}^{N-2} \left $(x_{k+1}-2x_k+x_{k-1})\^2 + (y_{k+1}-2y_k+y_{k-1})\^2 \\right$ Jsmooth=k=1∑N−2 $(xk+1-2xk+xk-1)2+(yk+1-2yk+yk-1)2$

这只是一个非常经典的离散曲线平滑目标。

4. 二阶差分如何变成矩阵

为了把问题交给 QP 求解器，需要把上面的平方和写成矩阵二次型。

先考虑一维点列：

x= $x0x1\dotsxN-1$ T \mathbf{x}=\begin{bmatrix} x_0 & x_1 & \cdots & x_{N-1} \end{bmatrix}^{T} x= $x0x1\dotsxN-1$ T

定义二阶差分矩阵 D∈R(N−2)×ND\in\mathbb{R}^{(N-2)\times N}D∈R(N−2)×N：

D= $1-210\dots001-21\dots0⋮⋱⋮0\dots01-21$ D=\begin{bmatrix} 1 & -2 & 1 & 0 & \cdots & 0 \\ 0 & 1 & -2 & 1 & \cdots & 0 \\ \vdots & & & & \ddots & \vdots \\ 0 & \cdots & 0 & 1 & -2 & 1 \end{bmatrix} D= 10⋮0−21⋯1−20011⋯⋯⋱−200⋮1

于是：

Dx= $x0-2x1+x2x1-2x2+x3⋮xN-3-2xN-2+xN-1$ D\mathbf{x} = \begin{bmatrix} x_0-2x_1+x_2 \\ x_1-2x_2+x_3 \\ \vdots \\ x_{N-3}-2x_{N-2}+x_{N-1} \end{bmatrix} Dx= x0−2x1+x2x1−2x2+x3⋮xN−3−2xN−2+xN−1

一维二阶差分平方和就是：

∥Dx∥2=(Dx)T(Dx)=xTDTDx \|D\mathbf{x}\|^2 = (D\mathbf{x})^T(D\mathbf{x}) = \mathbf{x}^TD^TD\mathbf{x} ∥Dx∥2=(Dx)T(Dx)=xTDTDx

记：

PD=DTD P_D=D^TD PD=DTD

那么 PDP_DPD 是一个对称半正定矩阵。以 N=5N=5N=5 为例：

D= $1-210001-210001-21$ D=\begin{bmatrix} 1 & -2 & 1 & 0 & 0 \\ 0 & 1 & -2 & 1 & 0 \\ 0 & 0 & 1 & -2 & 1 \end{bmatrix} D= 100−2101−2101−2001

于是：

DTD= $1-2100-25-4101-46-4101-45-2001-21$ D^TD=\begin{bmatrix} 1 & -2 & 1 & 0 & 0 \\ -2 & 5 & -4 & 1 & 0 \\ 1 & -4 & 6 & -4 & 1 \\ 0 & 1 & -4 & 5 & -2 \\ 0 & 0 & 1 & -2 & 1 \end{bmatrix} DTD= 1−2100−25−4101−46−4101−45−2001−21

中间行出现了典型的五点模板：

1−46−41\] \\begin{bmatrix} 1 \& -4 \& 6 \& -4 \& 1 \\end{bmatrix} \[1−46−41

这不是偶然。因为一个内点 pj\mathbf{p}jpj 会同时影响它附近多个二阶差分项：包含 pj−2\mathbf{p}{j-2}pj−2、pj−1\mathbf{p}{j-1}pj−1、pj\mathbf{p}jpj、pj+1\mathbf{p}{j+1}pj+1、pj+2\mathbf{p}{j+2}pj+2 的局部关系都会参与求导，所以 Hessian 自然是五对角带状结构。

二维路径有 xxx 和 yyy 两个坐标。把它们堆叠成：

z= $x0x1⋮xN-1y0y1⋮yN-1$ ∈R2N \mathbf{z}=\begin{bmatrix} x_0 \\ x_1 \\ \vdots \\ x_{N-1} \\ y_0 \\ y_1 \\ \vdots \\ y_{N-1} \end{bmatrix}\in\mathbb{R}^{2N} z= x0x1⋮xN−1y0y1⋮yN−1 ∈R2N

那么二维平滑矩阵可以写成块对角形式：

P0= $DTD00DTD$ P_0=\begin{bmatrix} D^TD & 0 \\ 0 & D^TD \end{bmatrix} P0= $DTD00DTD$

于是：

Jsmooth=zTP0z J_{\text{smooth}} = \mathbf{z}^TP_0\mathbf{z} Jsmooth=zTP0z

在标准 QP 中，目标函数常写成：

min⁡v12vTHv+fTv \min_{\mathbf{v}}\quad \frac{1}{2}\mathbf{v}^TH\mathbf{v} + \mathbf{f}^T\mathbf{v} vmin21vTHv+fTv

因此实际使用时可以把常数因子吸收到 HHH 或权重里。对算法理解来说，关键不是 12\frac{1}{2}21 的位置，而是二阶差分平方和会产生一个稀疏、对称、带状的二次型矩阵。

5. 为什么不能只最小化二阶差分

如果只优化：

min⁡∑k=1N−2∥pk+1−2pk+pk−1∥2 \min \sum_{k=1}^{N-2} \left\| \mathbf{p}_{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} \right\|^2 mink=1∑N−2∥pk+1−2pk+pk−1∥2

最优结果会倾向于把路径拉成尽可能直的线。但自动驾驶里的参考路径不是随便给的，它可能已经避开了障碍物，符合车道结构，包含停车点、目标点和行为规划意图。

所以 Autoware Elastic Band 同时做两件事：

一方面减少二阶差分，另一方面限制每个点不能离原路径太远。 \text{一方面减少二阶差分，另一方面限制每个点不能离原路径太远。} 一方面减少二阶差分，另一方面限制每个点不能离原路径太远。

限制有两层。

第一层是硬约束：每个点最多只能沿法向移动一定距离。

第二层是软代价：横向偏移越大，代价越大。

这使得最终轨迹不是"最直"的路径，而是"在允许范围内更平滑"的路径。

6. Autoware 的变量：从二维坐标压缩成横向偏移

回到第 iii 个参考点：

ri= $xiyi$ \mathbf{r}_i=\begin{bmatrix} x_i \\ y_i \end{bmatrix} ri= $xiyi$

由航向角 θi\theta_iθi 定义法向量：

ni= $-sinθicosθi$ \mathbf{n}_i=\begin{bmatrix} -\sin\theta_i \\ \cos\theta_i \end{bmatrix} ni= $-sinθicosθi$

优化后的点为：

pi(li)=ri+lini \mathbf{p}_i(l_i)=\mathbf{r}_i+l_i\mathbf{n}_i pi(li)=ri+lini

展开为坐标：

xi′(li)=xi−sin⁡θi li x_i'(l_i)=x_i-\sin\theta_i\,l_i xi′(li)=xi−sinθili

yi′(li)=yi+cos⁡θi li y_i'(l_i)=y_i+\cos\theta_i\,l_i yi′(li)=yi+cosθili

这说明每个点只有一个自由度 lil_ili。li>0l_i>0li>0 表示沿左法向偏移，li<0l_i<0li<0 表示沿右法向偏移。

把参考路径坐标堆叠为：

x= $x0⋮xN-1y0⋮yN-1$ ∈R2N \mathbf{x}=\begin{bmatrix} x_0 \\ \vdots \\ x_{N-1} \\ y_0 \\ \vdots \\ y_{N-1} \end{bmatrix}\in\mathbb{R}^{2N} x= x0⋮xN−1y0⋮yN−1 ∈R2N

再定义法向映射矩阵 T∈RN×2NT\in\mathbb{R}^{N\times 2N}T∈RN×2N。它的第 iii 行只有两个非零元素：

Ti,i=−sin⁡θi,Ti,i+N=cos⁡θi T_{i,i}=-\sin\theta_i, \qquad T_{i,i+N}=\cos\theta_i Ti,i=−sinθi,Ti,i+N=cosθi

这个矩阵可以理解为"全局坐标与局部横向方向之间的桥"。

如果给定一个全局位移向量：

Δz= $Δx0⋮ΔxN-1Δy0⋮ΔyN-1$ \Delta\mathbf{z}=\begin{bmatrix} \Delta x_0 \\ \vdots \\ \Delta x_{N-1} \\ \Delta y_0 \\ \vdots \\ \Delta y_{N-1} \end{bmatrix} Δz= Δx0⋮ΔxN−1Δy0⋮ΔyN−1

那么：

(TΔz)i=−sin⁡θi Δxi+cos⁡θi Δyi (T\Delta\mathbf{z})_i = -\sin\theta_i\,\Delta x_i + \cos\theta_i\,\Delta y_i (TΔz)i=−sinθiΔxi+cosθiΔyi

这正是第 iii 个点的全局位移在法向量 ni\mathbf{n}_ini 上的投影。

反过来，TTlT^T\mathbf{l}TTl 会把横向偏移量映射回全局 x/yx/yx/y 坐标增量：

TTl= $-sinθ0 l0⋮-sinθN-1 lN-1cosθ0 l0⋮cosθN-1 lN-1$ T^T\mathbf{l} = \begin{bmatrix} -\sin\theta_0\,l_0 \\ \vdots \\ -\sin\theta_{N-1}\,l_{N-1} \\ \cos\theta_0\,l_0 \\ \vdots \\ \cos\theta_{N-1}\,l_{N-1} \end{bmatrix} TTl= −sinθ0l0⋮−sinθN−1lN−1cosθ0l0⋮cosθN−1lN−1

所以所有优化后的点可以统一写成：

z(l)=x+TTl \mathbf{z}(\mathbf{l}) = \mathbf{x}+T^T\mathbf{l} z(l)=x+TTl

这一步是理解 Autoware Elastic Band 的核心。原本平滑代价定义在全局坐标 z\mathbf{z}z 上，但真正的优化变量是横向偏移 l\mathbf{l}l。只要把 z(l)\mathbf{z}(\mathbf{l})z(l) 代入平滑代价，就能得到关于 l\mathbf{l}l 的 QP。

7. 把平滑代价投影到横向偏移变量上

带权重的全局平滑矩阵记为：

R=wsP0 R=w_sP_0 R=wsP0

其中 wsw_sws 是平滑权重，P0P_0P0 是二阶差分矩阵构成的块对角平滑矩阵。

优化后的全局坐标是：

z(l)=x+TTl \mathbf{z}(\mathbf{l})=\mathbf{x}+T^T\mathbf{l} z(l)=x+TTl

因此平滑代价可以写成：

Jsmooth(l)=12(x+TTl)TR(x+TTl) J_{\text{smooth}}(\mathbf{l}) = \frac{1}{2} \left( \mathbf{x}+T^T\mathbf{l} \right)^T R \left( \mathbf{x}+T^T\mathbf{l} \right) Jsmooth(l)=21(x+TTl)TR(x+TTl)

展开：

Jsmooth(l)=12xTRx+12xTRTTl+12lTTRx+12lTTRTTl \begin{aligned} J_{\text{smooth}}(\mathbf{l}) = \frac{1}{2} \mathbf{x}^TR\mathbf{x} + \frac{1}{2} \mathbf{x}^TRT^T\mathbf{l} + \frac{1}{2} \mathbf{l}^TTR\mathbf{x} + \frac{1}{2} \mathbf{l}^TTRT^T\mathbf{l} \end{aligned} Jsmooth(l)=21xTRx+21xTRTTl+21lTTRx+21lTTRTTl

因为 RRR 是对称矩阵，所以：

xTRTTl=lTTRx \mathbf{x}^TRT^T\mathbf{l} = \mathbf{l}^TTR\mathbf{x} xTRTTl=lTTRx

于是：

Jsmooth(l)=12lTTRTTl+lTTRx+12xTRx J_{\text{smooth}}(\mathbf{l}) = \frac{1}{2} \mathbf{l}^TTRT^T\mathbf{l} + \mathbf{l}^TTR\mathbf{x} + \frac{1}{2}\mathbf{x}^TR\mathbf{x} Jsmooth(l)=21lTTRTTl+lTTRx+21xTRx

最后一项：

12xTRx \frac{1}{2}\mathbf{x}^TR\mathbf{x} 21xTRx

只和原始参考路径有关，不含优化变量 l\mathbf{l}l，因此不会影响最优解，可以丢掉。

所以平滑项在 QP 中贡献：

Psmooth=TRTT P_{\text{smooth}}=TRT^T Psmooth=TRTT

qsmooth=TRx \mathbf{q}_{\text{smooth}}=TR\mathbf{x} qsmooth=TRx

也就是说：

Jsmooth(l)=12lTPsmoothl+qsmoothTl+const J_{\text{smooth}}(\mathbf{l}) = \frac{1}{2} \mathbf{l}^T P_{\text{smooth}} \mathbf{l} + \mathbf{q}_{\text{smooth}}^T\mathbf{l} + \text{const} Jsmooth(l)=21lTPsmoothl+qsmoothTl+const

这里的线性项很值得注意。它来自原始路径 x\mathbf{x}x。如果原始路径本身已经很光顺，TRxTR\mathbf{x}TRx 对偏移的驱动力会比较弱；如果原始路径有明显折角，线性项会推动某些 lil_ili 取非零值，从而降低局部二阶差分。

8. 横向误差正则项

如果只用平滑项，路径可能会在允许区间内尽量漂向更直的位置。为了让结果仍然贴近原始路径，需要惩罚横向偏移本身：

Jlat(l)=12wl∥l∥2 J_{\text{lat}}(\mathbf{l}) = \frac{1}{2}w_l\|\mathbf{l}\|^2 Jlat(l)=21wl∥l∥2

写成矩阵形式：

Jlat(l)=12lT(wlI)l J_{\text{lat}}(\mathbf{l}) = \frac{1}{2} \mathbf{l}^T (w_lI) \mathbf{l} Jlat(l)=21lT(wlI)l

其中 wlw_lwl 是横向误差权重。

它的作用有三个：

抑制路径偏离原始参考线。
在平滑和保形之间建立权衡。
改善 Hessian 的条件数，让 QP 更稳定。

如果 wlw_lwl 很大，优化结果会更接近原始路径，但平滑效果变弱。如果 wlw_lwl 很小，路径更容易被拉顺，但也更可能偏离原始参考形状。

9. 最终 QP 模型

把平滑项和横向误差项相加，得到：

J(l)=12lT(TRTT+wlI)l+(TRx)Tl J(\mathbf{l}) = \frac{1}{2} \mathbf{l}^T \left( TRT^T+w_lI \right) \mathbf{l} + \left( TR\mathbf{x} \right)^T \mathbf{l} J(l)=21lT(TRTT+wlI)l+(TRx)Tl

由于：

R=wsP0 R=w_sP_0 R=wsP0

最终可以写成：

min⁡l∈RN12lT(wsTP0TT+wlI)l+(wsTP0x)Tl \min_{\mathbf{l}\in\mathbb{R}^{N}}\quad \frac{1}{2}\mathbf{l}^T\left(w_sTP_0T^T+w_lI\right)\mathbf{l} + \left(w_sTP_0\mathbf{x}\right)^T\mathbf{l} l∈RNmin21lT(wsTP0TT+wlI)l+(wsTP0x)Tl

这就是 Autoware Elastic Band 平滑的核心目标函数。

令：

P=wsTP0TT+wlI P=w_sTP_0T^T+w_lI P=wsTP0TT+wlI

q=wsTP0x \mathbf{q}=w_sTP_0\mathbf{x} q=wsTP0x

则标准 QP 形式为：

min⁡l12lTPl+qTl \min_{\mathbf{l}}\quad \frac{1}{2}\mathbf{l}^TP\mathbf{l} + \mathbf{q}^T\mathbf{l} lmin21lTPl+qTl

接下来只需要加上每个横向偏移量的上下界约束。

10. 横向约束：每个点只能在一条短线段上移动

Autoware Elastic Band 不允许路径点在平面中任意移动。第 iii 个点只能在法向线段上运动：

pi(li)=ri+lini \mathbf{p}_i(l_i)=\mathbf{r}_i+l_i\mathbf{n}_i pi(li)=ri+lini

并满足：

−ci≤li≤ci -c_i\le l_i\le c_i −ci≤li≤ci

其中 cic_ici 是第 iii 个点允许的最大横向偏移距离。

从几何上看，第 iii 个点的可行区间端点是：

piu=ri+cini \mathbf{p}_i^{u} = \mathbf{r}_i+c_i\mathbf{n}_i piu=ri+cini

pil=ri−cini \mathbf{p}_i^{l} = \mathbf{r}_i-c_i\mathbf{n}_i pil=ri−cini

也就是：

pi(li)∈ $ri-cini, ri+cini$ \mathbf{p}_i(l_i)\in\left $\\mathbf{r}_i-c_i\\mathbf{n}_i,\\, \\mathbf{r}_i+c_i\\mathbf{n}_i \\right$ pi(li)∈ $ri-cini,ri+cini$

把全部约束写成矩阵形式：

lmin⁡≤Al≤lmax⁡ \mathbf{l}{\min}\le A\mathbf{l}\le \mathbf{l}{\max} lmin≤Al≤lmax

在这个问题里：

A=I A=I A=I

因此约束就是最简单的 box constraint：

$-c0-c1⋮-cN-1$ ≤ $l0l1⋮lN-1$ ≤ $c0c1⋮cN-1$ \begin{bmatrix}-c_0 \\ -c_1 \\ \vdots \\ -c_{N-1}\end{bmatrix}\le \begin{bmatrix}l_0 \\ l_1 \\ \vdots \\ l_{N-1}\end{bmatrix}\le \begin{bmatrix}c_0 \\ c_1 \\ \vdots \\ c_{N-1}\end{bmatrix} −c0−c1⋮−cN−1 ≤ l0l1⋮lN−1 ≤ c0c1⋮cN−1

如果某个 ci=0c_i=0ci=0，则：

li=0 l_i=0 li=0

这个点被硬固定，完全不能横向移动。

11. fix、joint、smooth 三类移动范围

不同位置的路径点不应该拥有同样的自由度。Autoware Elastic Band 把横向移动范围大致分成三类：

ci={cfix,固定区域cjoint,连接过渡区域csmooth,主要平滑区域 c_i=\begin{cases} c_{\text{fix}}, & \text{固定区域} \\ c_{\text{joint}}, & \text{连接过渡区域} \\ c_{\text{smooth}}, & \text{主要平滑区域} \end{cases} ci=⎩ ⎨ ⎧cfix,cjoint,csmooth,固定区域连接过渡区域主要平滑区域

fix 区域

fix 区域用于强约束关键点。典型位置包括：

路径起点。
局部路径包含目标点时的终点附近。
需要与上一周期结果保持连续的前端区域。

如果：

cfix=0 c_{\text{fix}}=0 cfix=0

那么该区域点满足：

li=0 l_i=0 li=0

也就是完全固定。

起点固定很好理解：自车附近的轨迹不能突然横向跳动，否则会造成转向突变。终点固定也很重要，特别是当局部平滑片段已经包含目标点时，不能让平滑器改变目标位置；只固定最后一个点有时还不足以稳定末端方向，因此终点前一个真实点也常被收紧，从而稳定末端姿态。

joint 区域

joint 区域是固定段和平滑段之间的过渡段。

如果从"完全固定"直接切换到"较自由平滑"，边界处可能出现新的折角。joint 区域使用较小的横向可动范围，让自由度逐渐释放：

0≤cfix≤cjoint≤csmooth 0\le c_{\text{fix}}\le c_{\text{joint}}\le c_{\text{smooth}} 0≤cfix≤cjoint≤csmooth

这不是数学上必须的硬规则，但它符合常见调参直觉。

smooth 区域

smooth 区域是主要被优化的部分。这里允许更大的横向微调，让二阶差分代价有足够空间下降：

∣li∣≤csmooth |l_i|\le c_{\text{smooth}} ∣li∣≤csmooth

如果 csmoothc_{\text{smooth}}csmooth 太小，路径几乎无法被改变，平滑效果有限。如果太大，路径可能偏离上游规划意图。

12. 完整问题的一句话形式

现在可以把整个问题写成最简洁的形式：

pi=ri+lini \mathbf{p}_i = \mathbf{r}_i+l_i\mathbf{n}_i pi=ri+lini

min⁡lws∑k=1N−2∥pk+1−2pk+pk−1∥2+wl∑i=0N−1li2 \min_{\mathbf{l}}\quad w_s\sum_{k=1}^{N-2} \left\| \mathbf{p}{k+1} -2\mathbf{p}k + \mathbf{p}{k-1} \right\|^2 + w_l\sum{i=0}^{N-1}l_i^2 lminwsk=1∑N−2∥pk+1−2pk+pk−1∥2+wli=0∑N−1li2

subject to：

−ci≤li≤ci,i=0,...,N−1 -c_i\le l_i\le c_i, \qquad i=0,\dots,N-1 −ci≤li≤ci,i=0,...,N−1

再写成 QP：

min⁡l12lTPl+qTl \min_{\mathbf{l}}\quad \frac{1}{2}\mathbf{l}^TP\mathbf{l} + \mathbf{q}^T\mathbf{l} lmin21lTPl+qTl

subject to：

lmin⁡≤l≤lmax⁡ \mathbf{l}{\min}\le \mathbf{l}\le \mathbf{l}{\max} lmin≤l≤lmax

其中：

P=wsTP0TT+wlI P=w_sTP_0T^T+w_lI P=wsTP0TT+wlI

q=wsTP0x \mathbf{q}=w_sTP_0\mathbf{x} q=wsTP0x

这就是 Autoware Elastic Band 平滑器最核心的数学模型。

13. 为什么它一定是 QP

QP 的基本形式是：

min⁡v12vTHv+fTv \min_{\mathbf{v}}\quad \frac{1}{2}\mathbf{v}^TH\mathbf{v} + \mathbf{f}^T\mathbf{v} vmin21vTHv+fTv

subject to：

blower≤Cv≤bupper \mathbf{b}{\text{lower}}\le C\mathbf{v}\le \mathbf{b}{\text{upper}} blower≤Cv≤bupper

Autoware Elastic Band 满足这个结构：

变量 l\mathbf{l}l 是连续向量。
优化后的点 pi=ri+lini\mathbf{p}_i=\mathbf{r}_i+l_i\mathbf{n}_ipi=ri+lini 对 l\mathbf{l}l 是线性的。
二阶差分对 l\mathbf{l}l 仍然是线性的。
线性表达的平方和就是二次目标。
横向移动范围是线性不等式。
固定点可以看成上下界相同的 box constraint。

所以这个问题不是非线性规划，而是标准二次规划。

此外，P0=DTDP_0=D^TDP0=DTD 是半正定的，wsTP0TTw_sTP_0T^TwsTP0TT 也是半正定的。如果 wl>0w_l>0wl>0，那么 wlIw_lIwlI 会让 Hessian 更稳定：

P=wsTP0TT+wlI P=w_sTP_0T^T+w_lI P=wsTP0TT+wlI

通常会比纯平滑项更适合数值求解。

14. 为什么稀疏结构很适合在线规划

二阶差分只连接相邻三点，因此 DDD 很稀疏。DTDD^TDDTD 是五对角带状矩阵。这样的矩阵有两个工程优势：

存储代价低。
求解器可以利用稀疏结构快速迭代。

而且变量只有 NNN 个横向偏移量，而不是 2N2N2N 个全局坐标变量。对于在线规划来说，这个降维非常关键。

Elastic Band 因此可以在较高频率的规划循环中作为轻量几何平滑器使用。它不追求解决完整的动态避障和车辆动力学问题，而是先把参考路径变得更适合后续优化。

15. 在线运行时为什么要裁剪、重采样和补齐

虽然核心数学模型很短，但在线系统还需要一些工程步骤来保证问题稳定。

裁剪局部路径

平滑器通常只优化自车附近的一段路径。前向优化长度大致由点数和采样间隔决定：

Lforward=NΔs L_{\text{forward}} = N\Delta s Lforward=NΔs

其中 NNN 是优化点数量，Δs\Delta sΔs 是优化点弧长间隔。

自车后方也会保留一小段路径，用来保证输出轨迹和自车附近状态连续。这样做的目的不是改变历史路径，而是避免输出轨迹在自车附近突然断开。

插入固定点

在线规划是周期性运行的。如果每一帧都完全重新优化，轨迹前端可能在相邻周期之间轻微跳动。为了减少这种跳变，可以把上一周期已经优化过的前端轨迹作为当前周期的稳定锚点。

这相当于告诉优化器：

车前很近的轨迹不要随意改变。 \text{车前很近的轨迹不要随意改变。} 车前很近的轨迹不要随意改变。

这对方向盘稳定性尤其重要。

等弧长重采样

二阶差分默认相邻点间隔比较均匀。如果路径点间距忽大忽小，同样的二阶差分数值就不再代表同样的几何弯折程度。

因此 Elastic Band 会倾向于使用固定弧长间隔：

Δs=constant \Delta s=\text{constant} Δs=constant

这样二阶差分代价的物理含义更一致，QP 数值也更稳定。

补齐点数

在线优化器希望问题维度固定。若某次局部路径比较短，例如已经接近目标点，实际点数不足 NNN，可以用末尾点补齐到固定数量。

这样做的好处是：

优化变量维度保持不变。
Hessian 和约束矩阵维度保持稳定。
更容易复用上一轮求解状态，也就是 warm start。

补齐点不代表真实路径变长，它只是为了保持优化问题形状一致。最终输出时只使用真实路径对应的部分。

16. 求解后如何回到路径

QP 求出来的是：

l∗= $l0*l1*\dotslN-1*$ T \mathbf{l}^{*} = \begin{bmatrix} l_0^{*} & l_1^{*} & \cdots & l_{N-1}^{*} \end{bmatrix}^{T} l∗= $l0*l1*\dotslN-1*$ T

每个点的平滑后位置为：

pi∗=ri+li∗ni \mathbf{p}_i^{*} = \mathbf{r}_i+l_i^{*}\mathbf{n}_i pi∗=ri+li∗ni

也就是：

xi∗=xi−sin⁡θi li∗ x_i^{*}=x_i-\sin\theta_i\,l_i^{*} xi∗=xi−sinθili∗

yi∗=yi+cos⁡θi li∗ y_i^{*}=y_i+\cos\theta_i\,l_i^{*} yi∗=yi+cosθili∗

得到新的点列后，还需要根据几何形状重新计算路径朝向。因为点的位置已经改变，旧的航向角未必仍然与新路径切向一致。

Elastic Band 本身只处理几何形状。速度不是它优化的对象。输出轨迹的速度通常从输入路径继承或重新映射，因此可以理解为：

Elastic Band 改形状，不重新设计速度曲线。 \text{Elastic Band 改形状，不重新设计速度曲线。} Elastic Band 改形状，不重新设计速度曲线。

17. 结果验证和失败回退

因为这是在线自动驾驶系统，平滑结果不能只看求解器是否返回一个数值，还需要判断结果是否可信。

常见验证方式是检查最大横向偏移：

max⁡i∣li∗∣≤lvalid \max_i |l_i^{*}|\le l_{\text{valid}} imax∣li∗∣≤lvalid

其中 lvalidl_{\text{valid}}lvalid 是允许的验证阈值。

如果优化失败、结果包含异常数值，或横向偏移超过验证阈值，就不应该发布这条新轨迹。更稳妥的方式是回退到上一轮成功结果；如果没有上一轮结果，再退回原始输入路径。

这体现了在线规划里的一个基本原则：

宁可输出保守且连续的轨迹，也不要输出突兀的新解。 \text{宁可输出保守且连续的轨迹，也不要输出突兀的新解。} 宁可输出保守且连续的轨迹，也不要输出突兀的新解。

18. 参数如何影响结果

Elastic Band 的参数可以按功能分成五组。

离散化参数

num_points 决定优化点数量 NNN。

delta_arc_length 决定优化点间隔 Δs\Delta sΔs。

前向优化长度近似为：

Lforward=NΔs L_{\text{forward}}=N\Delta s Lforward=NΔs

NNN 太小，优化范围短，平滑效果有限；NNN 太大，计算量增加。Δs\Delta sΔs 太大，路径细节表达粗糙；Δs\Delta sΔs 太小，变量更多，且二阶差分对噪声更敏感。

权重参数

smooth_weight 对应 wsw_sws：

ws∑k=1N−2∥pk+1−2pk+pk−1∥2 w_s \sum_{k=1}^{N-2} \left\| \mathbf{p}_{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} \right\|^2 wsk=1∑N−2∥pk+1−2pk+pk−1∥2

wsw_sws 越大，路径越倾向于降低折角和锯齿。

lat_error_weight 对应 wlw_lwl：

wl∑i=0N−1li2 w_l\sum_{i=0}^{N-1}l_i^2 wli=0∑N−1li2

wlw_lwl 越大，路径越贴近原始输入。

两者的关系可以理解为：

wswl 越大，越追求平滑；wlws 越大，越保守贴原路径。 \frac{w_s}{w_l} \text{ 越大，越追求平滑；} \qquad \frac{w_l}{w_s} \text{ 越大，越保守贴原路径。} wlws 越大，越追求平滑；wswl 越大，越保守贴原路径。

clearance 参数

clearance_for_fix 对应 cfixc_{\text{fix}}cfix。

clearance_for_joint 对应 cjointc_{\text{joint}}cjoint。

clearance_for_smooth 对应 csmoothc_{\text{smooth}}csmooth。

它们本质上定义变量边界：

−ci≤li≤ci -c_i\le l_i\le c_i −ci≤li≤ci

如果 clearance 太小，优化器没有足够空间消除折角。如果 clearance 太大，结果可能偏离上游路径意图。一般来说，固定区最小，连接区适中，平滑区最大。

num_joint_points 决定固定区和平滑区之间有多少过渡点。过渡点越多，前端连接越柔和，但真正自由平滑的区域会相对变短。

求解器参数

max_iteration 控制 QP 最大迭代次数。

eps_abs 和 eps_rel 控制绝对和相对收敛精度。

精度越严格，结果可能更精细，但计算时间增加。在线规划里通常需要在稳定性和实时性之间折中。

在线稳定参数

enable_warm_start 表示是否使用上一轮优化结果辅助当前求解。由于相邻规划周期的问题通常很相似，warm start 可以减少迭代次数，并提高轨迹连续性。

enable_optimization_validation 和 validation.max_error 用于检查优化结果是否超过允许横向误差。它们不是改变目标函数，而是在输出前做安全过滤。

重规划相关参数用于判断是否需要重新执行平滑。如果输入路径、自车位置或目标变化很小，可以复用上一轮结果；如果变化超过阈值，就需要重新优化。

19. 它和传统 Elastic Band 的区别

经典机器人 Elastic Band 通常会把路径看成一条受力的弹性带：

pi(t+1)=pi(t)+α(fiinternal+fiobstacle) \mathbf{p}_i^{(t+1)} = \mathbf{p}_i^{(t)} + \alpha \left( \mathbf{f}^{\text{internal}}_i + \mathbf{f}^{\text{obstacle}}_i \right) pi(t+1)=pi(t)+α(fiinternal+fiobstacle)

其中 finternal\mathbf{f}^{\text{internal}}finternal 表示弹性内力，让路径更短更平滑；fobstacle\mathbf{f}^{\text{obstacle}}fobstacle 表示障碍物斥力，让路径远离障碍物。

Autoware 的 Elastic Band 明显不同：

它不是力迭代，而是 QP。
它不显式构造障碍物斥力。
它不负责碰撞检查。
它限制点只能沿局部法向移动。
它的主要目标是让后续基于 Frenet 的优化更稳定。

所以不要把 Autoware Elastic Band 理解成完整避障规划器。更准确的说法是：

它是路径优化前的几何平滑预处理器。 \text{它是路径优化前的几何平滑预处理器。} 它是路径优化前的几何平滑预处理器。

20. 为什么它不做碰撞检查

这点非常容易误解。

Elastic Band 的输出路径可能比输入路径更光顺，但它不保证一定在可行驶区域内，也不保证不碰撞障碍物。原因是它的约束只描述了每个点的横向可移动范围：

−ci≤li≤ci -c_i\le l_i\le c_i −ci≤li≤ci

这里没有障碍物距离约束，也没有车辆 footprint 与道路边界的碰撞约束。

这种设计是有意的。Autoware 中不同模块有不同职责：

Elastic Band 负责轻量几何平滑。
后续轨迹优化或可行驶区域检查负责可行性和碰撞相关问题。
速度规划负责速度、停车点、减速度和舒适性。

职责拆开后，Elastic Band 可以保持简单、稳定、快速。

21. 它为什么能让后续 Frenet / MPT 更稳定

Frenet 坐标系依赖参考路径。给定参考路径后，车辆状态会被描述成：

纵向位置 s,横向误差 l,航向误差 eθ \text{纵向位置 }s,\quad \text{横向误差 }l,\quad \text{航向误差 }e_{\theta} 纵向位置 s,横向误差 l,航向误差 eθ

如果参考路径有局部锯齿，则切向和法向会跳变：

航向角 θi\theta_iθi 一旦抖动，横向误差、航向误差和曲率估计都会跟着抖动。后续优化器会看到一个不稳定的参考系，求解出的转向角也可能变得不稳定。

Elastic Band 通过降低二阶差分：

pk+1−2pk+pk−1 \mathbf{p}_{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} pk+1−2pk+pk−1

让路径局部方向变化更平顺，从而让 Frenet frame 更稳定。它不是直接优化车辆动力学，但它改善了后续动力学优化的输入条件。

22. 一个从零理解的学习路线

如果你完全不了解 Autoware，可以按下面这条路线学习 Elastic Band。

第一步，理解路径是离散点列：

r0,r1,...,rN−1 \mathbf{r}_0,\mathbf{r}1,\dots,\mathbf{r}{N-1} r0,r1,...,rN−1

第二步，理解平滑就是减少相邻线段方向变化：

pk+1−2pk+pk−1 \mathbf{p}_{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} pk+1−2pk+pk−1

第三步，理解二阶差分平方和可以写成矩阵二次型：

∥Dx∥2=xTDTDx \|D\mathbf{x}\|^2=\mathbf{x}^TD^TD\mathbf{x} ∥Dx∥2=xTDTDx

第四步，理解 Autoware 不直接优化 x,yx,yx,y，而是优化横向偏移：

pi=ri+lini \mathbf{p}_i=\mathbf{r}_i+l_i\mathbf{n}_i pi=ri+lini

第五步，把全局平滑代价投影到横向偏移变量上：

z(l)=x+TTl \mathbf{z}(\mathbf{l})=\mathbf{x}+T^T\mathbf{l} z(l)=x+TTl

第六步，展开得到 QP：

min⁡l12lT(wsTP0TT+wlI)l+(wsTP0x)Tl \min_{\mathbf{l}}\quad \frac{1}{2}\mathbf{l}^T\left(w_sTP_0T^T+w_lI\right)\mathbf{l} + \left(w_sTP_0\mathbf{x}\right)^T\mathbf{l} lmin21lT(wsTP0TT+wlI)l+(wsTP0x)Tl

第七步，加上横向 box constraint：

−ci≤li≤ci -c_i\le l_i\le c_i −ci≤li≤ci

理解完这七步，就已经抓住了 Autoware Elastic Band 的主干。

23. 优点和局限

优点

它很稳定，因为目标是二次函数，约束是线性边界。

它很快，因为 Hessian 来自二阶差分，具有稀疏带状结构。

它很保守，因为点只能沿法向小幅移动，不会彻底改变路径拓扑。

它很适合在线系统，因为可以固定优化维度、使用 warm start，并在失败时回退到上一轮结果。

局限

它不负责避障。

它不保证输出路径一定 collision-free。

它不能处理大尺度拓扑变化。

它依赖上游参考路径质量。如果原始路径很差，或者横向可动范围很小，平滑效果会受限。

它不是动力学优化。车辆转向角、转向角速度、轮胎约束、道路边界和障碍物约束需要交给后续模块处理。

24. 最后记住三个公式

第一个，平滑目标：

Jsmooth=∑k=1N−2∥pk+1−2pk+pk−1∥2 J_{\text{smooth}} = \sum_{k=1}^{N-2} \left\| \mathbf{p}_{k+1}-2\mathbf{p}k+\mathbf{p}{k-1} \right\|^2 Jsmooth=k=1∑N−2∥pk+1−2pk+pk−1∥2

第二个，Autoware 的横向偏移参数化：

pi=ri+lini \mathbf{p}_i = \mathbf{r}_i+l_i\mathbf{n}_i pi=ri+lini

第三个，最终 QP：

subject to：

−ci≤li≤ci -c_i\le l_i\le c_i −ci≤li≤ci

如果只能用一句话总结：

Autoware Elastic Band 通过优化每个路径点的法向横向偏移，在固定横向走廊内最小化离散二阶差分，从而得到更平顺但不负责避障的参考路径。 \text{Autoware Elastic Band 通过优化每个路径点的法向横向偏移，在固定横向走廊内最小化离散二阶差分，从而得到更平顺但不负责避障的参考路径。} Autoware Elastic Band 通过优化每个路径点的法向横向偏移，在固定横向走廊内最小化离散二阶差分，从而得到更平顺但不负责避障的参考路径。