当前位置：首页 > news >正文

路径平滑优化算法--Polynomial Spiral（多项式螺旋法）

news 2025/7/23 17:17:21

路径平滑优化算法–Polynomial Spiral（多项式螺旋法）

Polynomial Spiral 是一种在自动驾驶路径规划中广泛使用的曲线平滑技术，特别在 Apollo 系统中用于生成平滑、可行且符合车辆动态约束的路径。它的核心思想是通过定义曲率 $κ\kappa$ 作为弧长 $s$ 的多项式函数，来描述一条平滑的曲线轨迹。以下是详细的说明。

1. Polynomial Spiral 的数学基础

1.1 曲率的定义

在 Polynomial Spiral 中，曲率 $κ(s)\kappa(s)$ 是弧长 $s$ 的函数，通常用多项式表示。对于一个 $n$ 次 Polynomial Spiral，曲率函数可以写为：

$κ(s)=a0+a1s+a2s2+⋯+ansn\kappa(s) = a_0 + a_1 s + a_2 s^2 + \dots + a_n s^n$

其中， $a0,a1,…,ana_0, a_1, \dots, a_n$ 是多项式的系数，具体值由边界条件和优化目标确定。

在 Apollo 中，为了在灵活性和计算稳定性之间取得平衡，通常使用三次 Polynomial Spiral（即 $n = 3$ ）。此时，曲率函数为：

$κ(s)=a+bs+cs2+ds3\kappa(s) = a + b s + c s^2 + d s^3$

这里的 $a, b, c, d$ 是待求解的系数。

这个图是一个双子图（上、下两部分），用于展示多项式螺旋（Polynomial Spiral）路径平滑算法中的核心数学属性——曲率 $κ(s)\kappa(s)$ 和其变化率 $dκds\frac{d\kappa}{ds}$ 如何随弧长 $s$ 变化。上图描绘了曲率函数 $κ(s)=a+bs+cs2+ds3\kappa(s) = a + b s + c s^2 + d s^3$ （使用示例系数如 $a = 0, b = 0.01, c = - 0.001, d = 0.0001$ ），展示曲率从起点（ $κ(0)=0\kappa(0)=0$ ）到终点（ $κ(L)=0.1\kappa(L)=0.1$ ，其中 $L = 12$ m）的平滑变化。绿色阴影区域表示车辆动态约束下的最大曲率阈值（例如 ±0.2 1/m），强调路径必须避免急转弯以符合自动驾驶安全。下图显示曲率变化率，突出算法的目标：最小化曲率突变以确保路径平滑，避免车辆失控或不适。这个图帮助理解算法如何通过多项式确保 G2 连续性（位置、方向和曲率连续），类似于使用修改二次多项式插值的路径平滑方法。

1.2 从曲率到路径

路径的几何形状由曲率 $κ(s)\kappa(s)$ 通过积分逐步推导得出。具体步骤如下：

切线角 $θ(s)\theta(s)$ 的计算
路径在弧长 $s$ 处的切线角（即方向）是曲率的积分： $du+θ0\theta(s) = \int_0^s \kappa(u) \, du + \theta_0$ 其中， $θ0\theta_0$ 是起点的初始方向。对于三次 Polynomial Spiral，代入曲率函数： $du+θ0\theta(s) = \int_0^s (a + b u + c u^2 + d u^3) \, du + \theta_0$ 积分结果为： $θ(s)=θ0+as+b2s2+c3s3+d4s4\theta(s) = \theta_0 + a s + \frac{b}{2} s^2 + \frac{c}{3} s^3 + \frac{d}{4} s^4$
路径坐标 ( $(x (s), y (s))$ ) 的计算
路径的二维坐标通过切线角的三角函数积分得到： $x_0 + \int_0^s \cos(\theta(u)) \, du$ , $y_0 + \int_0^s \sin(\theta(u)) \, du$ 其中， $x_0, y_0)$ 是起点的坐标。由于 $θ(s)\theta(s)$ 是一个四次多项式，其正弦和余弦函数的积分通常无法解析求解，因此在实际中会使用数值方法（稍后详述）。

1.3 边界条件的设定

为了确定系数 $a, b, c, d$ ，需要指定起点和终点的状态（位姿和曲率）。假设路径的弧长从 $s = 0$ 到 $s = L$ ，边界条件通常包括：

起点 ( $s = 0$ )：
- 位置： $x(0), y(0)) = (x_0, y_0)$
- 方向： $θ(0)=θ0\theta(0) = \theta_0$
- 曲率： $κ(0)=κ0\kappa(0) = \kappa_0$
终点 ( $s = L$ )：
- 位置： $x(L), y(L)) = (x_f, y_f)$
- 方向： $θ(L)=θf\theta(L) = \theta_f$
- 曲率： $κ(L)=κf\kappa(L) = \kappa_f$

这些条件提供了 6 个约束（起点和终点各有位置、方向、曲率），但三次多项式只有 4 个自由系数（ $a, b, c, d$ ），因此通常需要通过优化来平衡这些约束（详见优化过程）。

2. Polynomial Spiral 的优化过程

在 Apollo 中，Polynomial Spiral 的系数并不是简单地通过边界条件直接解出，而是通过优化一个代价函数来确定。这种方法能够同时满足平滑性、贴近参考路径以及车辆动态约束等多重目标。

2.1 代价函数的设计

优化问题的代价函数 $J$ 通常由以下几部分组成：

平滑性项 $JsmoothJ_{\text{smooth}}$ 
目标是减少曲率的变化率，确保路径平滑，避免急转弯。

定义为： $Jsmooth=∫0L(dκds)2dsJ_{\text{smooth}} = \int_0^L \left( \frac{d \kappa}{ds} \right)^2 ds$ 对于 $κ(s)=a+bs+cs2+ds3\kappa(s) = a + b s + c s^2 + d s^3$ ，

其一阶导数为： $dκds=b+2cs+3ds2ds\frac{d \kappa}{ds} = b + 2c s + 3d s^2ds$ 代入积分后， $JsmoothJ_{\text{smooth}}$ 是一个关于 $b, c, d$ 的二次函数。
偏差项 $JdeviationJ_{\text{deviation}}$
确保平滑后的路径尽量贴近参考路径（由路径规划器生成的一系列离散点）。

定义为： $Jdeviation=∫0L[(x(s)−xref(s))2+(y(s)−yref(s))2]dsJ_{\text{deviation}} = \int_0^L \left[ (x(s) - x_{\text{ref}}(s))^2 + (y(s) - y_{\text{ref}}(s))^2 \right] ds$ 其中，

$(xref(s),yref(s))(x_{\text{ref}}(s), y_{\text{ref}}(s))$ 是参考路径上的点，通常通过插值得到。
约束项 $JconstraintsJ_{\text{constraints}}$

确保路径满足车辆的物理限制，例如：
- 最大曲率： $κ(s)∣≤κmax\kappa(s)| \leq \kappa_{\text{max}}$ （与最小转弯半径相关）
- 最大曲率变化率： $∣dκds∣≤κ˙maxdsd\left| \frac{d \kappa}{ds} \right| \leq \dot{\kappa}_{\text{max}}dsd$ （与转向速率相关）这些约束可以通过惩罚函数或不等式约束加入优化。

综合代价函数为：

$\alpha J_{\text{smooth}} + \beta J_{\text{deviation}} + \gamma J_{\text{constraints}}$

其中， $α,β,γ\alpha, \beta, \gamma$ 是权重因子，用于调整各部分的相对重要性。

2.2 数值求解

由于 $x (s)$ 和 $y (s)$ 的积分无法解析求解，Apollo 使用数值方法来优化代价函数：

离散化
将弧长 $s$ 划分为离散点 ${s0,s1,…,sN}\{s_0, s_1, \dots, s_N\}$ ，通常均匀分布（例如 $si=i⋅Δss_i = i \cdot \Delta s$ ， $Δs=L/N\Delta s = L/N$ ）。
数值积分
对 $θ(s)\theta(s)$ 、 $x (s)$ 、 $y (s)$ 使用数值积分方法（如梯形公式或辛普森公式）Apollo在integral.h中实现了以下几种数值积分方法：
- Trapezoidal rule（梯形法则）
  通过将积分区间划分为多个小梯形来近似计算积分值。这种方法简单，计算速度快，但精度较低，适合对精度要求不高的场景。
- Simpson’s rule（辛普森法则）
  将积分区间划分为小段，并在每段上用抛物线近似积分值。相比梯形法则，精度更高，适合平滑函数的积分。
- Gauss Legendre（高斯-勒让德积分）
  通过选择特定的积分点（高斯点）和权重，在较少的积分点下实现高精度计算。这种方法特别适合多项式函数的积分，是Apollo的默认选择。
计算： $θ(si)≈θ0+∑j=0i−1κ(sj)Δs\theta(s_i) \approx \theta_0 + \sum_{j=0}^{i-1} \kappa(s_j) \Delta s$ Δsx(s_i) $≈x0+∑j=0i−1cos⁡(θ(sj))Δ\approx x_0 + \sum_{j=0}^{i-1} \cos(\theta(s_j)) \Delta$ $≈y0+∑j=0i−1sin⁡(θ(sj))Δ\approx y_0 + \sum_{j=0}^{i-1} \sin(\theta(s_j)) \Delta$
优化步骤详解
1. 曲线长度的计算与优化
  - 积分 $∫0Lds=L\int_0^L ds = L$ 是线性的，直接等于弧长。
  - 优化时，减小 $L$ 会使路径更短，但需与其他目标（如平滑性）和约束（如位置要求）平衡。
2. 曲率平方和的计算与优化
  - 对于 $κ(s)=a+bs+cs2+ds3\kappa(s) = a + b s + c s^2 + d s^3$ ，计算： $∫0Lκ(s)2ds=∫0L(a+bs+cs2+ds3)2ds\int_0^L \kappa(s)^2 ds = \int_0^L (a + b s + c s^2 + d s^3)^2 ds$
  - 展开后，这是一个关于 $a, b, c, d$ 的高次多项式积分，可以解析求解。例如： $∫0L(a+bs)2ds=a2L+abL2+b2L33\int_0^L (a + b s)^2 ds = a^2 L + a b L^2 + \frac{b^2 L^3}{3}$ 对于更高次项，计算类似但更复杂。
  - 优化目标是减小该积分值，通过调整 $a, b, c, d$ 使曲率尽量小且均匀。
3. 曲率变化率平方和的计算与优化
  - 曲率变化率 $dκds=b+2cs+3ds2ds\frac{d \kappa}{ds} = b + 2c s + 3d s^2ds$ ，计算： $∫0L(dκds)2ds=∫0L(b+2cs+3ds2)2ds\int_0^L \left( \frac{d \kappa}{ds} \right)^2 ds = \int_0^L (b + 2c s + 3d s^2)^2 ds$
  - 同样展开并积分，例如： $∫0L(b+2cs)2ds=b2L+2bcL2+4c2L33\int_0^L (b + 2c s)^2 ds = b^2 L + 2 b c L^2 + \frac{4 c^2 L^3}{3}$
  - 优化目标是减小曲率变化率，使路径更平滑，通常通过减小 $b, c, d$ 的绝对值实现。
4. 数值实现
  - 由于解析积分较复杂，实际中常将弧长 $s$ 离散化为点 ${s0,s1,…,sN}\{s_0, s_1, \dots, s_N\}$ ，使用数值积分（如梯形公式）近似： $∫0Lκ(s)2ds≈∑i=0N−1κ(si)2+κ(si+1)22Δs\int_0^L \kappa(s)^2 ds \approx \sum_{i=0}^{N-1} \frac{\kappa(s_i)^2 + \kappa(s_{i+1})^2}{2} \Delta s$
  - 类似地计算曲率变化率平方和。
  - 这些离散值输入IPOPT求解器，优化变量 $a, b, c, d, L$
非线性特性
- 虽然 $κ(s)\kappa(s)$ 和 $dκds\frac{d \kappa}{ds}$ 的积分可以解析，但路径坐标 $(x (s), y (s))$ 涉及 $cos⁡(θ(s))\cos(\theta(s))$ 和 $sin⁡(θ(s))\sin(\theta(s))$ 的积分（即Fresnel积分），无法解析求解。
- 这引入了额外的非线性，使得目标函数与约束的整体优化需通过NLP处理。

2.3 约束条件

位置 Old Location: 位置平移约束：路径需经过或接近参考点，例如 $\approx (x_f, y_f)$ 。
连续性约束：相邻路径段在连接处的方向、曲率和曲率导数连续。
动态约束： $κ(s)\kappa(s)$ 和 $dκds\frac{d \kappa}{ds}$ 需满足车辆物理限制（如最大曲率和曲率变化率）。

这些约束通过数值方法计算路径坐标和方向，并作为NLP的等式或不等式约束输入求解器。

3. 在 Apollo 中的具体应用

Apollo 的路径规划模块利用 Polynomial Spiral 将离散的参考路径点平滑为一条连续、可执行的轨迹。以下是其实现流程和应用场景：

3.1 路径生成流程

生成参考路径
Apollo 的路径规划器（例如基于 A* 或 RRT 的算法）生成一条由离散点组成的初始路径 ${(xi,yi,θi)}\{(x_i, y_i, \theta_i)\}$ 。
路径分段
将参考路径划分为多个段，每段由两个相邻点定义，例如起点 $(xi,yi,θi,κi)(x_i, y_i, \theta_i, \kappa_i)$ 和终点 $(xi+1,yi+1,θi+1,κi+1)(x_{i+1}, y_{i+1}, \theta_{i+1}, \kappa_{i+1})$ 。
Polynomial Spiral 拟合
对每个段，使用一个三次 Polynomial Spiral 拟合平滑曲线，计算系数 $a, b, c, d$ ，使路径满足边界条件并优化代价函数。
路径拼接
将所有平滑的曲线段连接起来，确保段间曲率连续，形成整体路径。

3.2 应用场景示例

变道
车辆从当前车道切换到相邻车道时，参考路径可能是直线或简单曲线，但可能存在方向或曲率突变。Polynomial Spiral 生成一条曲率连续的路径，确保转向平滑。
转弯
在交叉路口或弯道处，Polynomial Spiral 保证车辆以平滑的曲率完成转弯，避免急转弯导致的不适或失控。
避障
当遇到障碍物时，规划器生成绕行路径，Polynomial Spiral 平滑这条路径，确保绕行过程中曲率变化平缓。

4.Polynomial Spiral 路径平滑：实例计算

本节提供了一个具体的实例，展示了如何优化 Polynomial Spiral 以在两点之间生成平滑路径，包括目标函数的设置、约束条件和基本优化步骤。

4.1 场景描述

一辆车需要从点 ( $A$ ) 行驶到点 ( $B$ )，条件如下：

点 ( A )：位置 ( $x_0, y_0) = (0, 0)$ )，航向 ( $θ0=0∘\theta_0 = 0^\circ$ )，曲率 ( $κ0=0\kappa_0 = 0$ )
点 ( B )：位置 ( $x_f, y_f) = (10, 5)$ )，航向 ( $θf=30∘\theta_f = 30^\circ$ )，曲率 ( $κf=0.1\kappa_f = 0.1$ )

我们使用三次 Polynomial Spiral，曲率定义为 ( $κ(s)=a+bs+cs2+ds3\kappa(s) = a + b s + c s^2 + d s^3$ )，其中 ( $s$ ) 是弧长参数，并优化其系数以平滑连接这两点。在本例中，我们固定弧长 ( $L = 12$ )（在实际应用中，( $L$ ) 也可以是优化变量）。

4.2 优化变量

曲率多项式的系数：( $a, b, c, d$ )
弧长 ( $L = 12$ )（为简化起见，固定不变）

4.3 目标函数

目标是最小化一个代价函数 ( $J$ )，以确保路径平滑，通常由以下部分组成：

( $J_1 = L$ )（路径长度，固定为 12，因此权重设为 0）
( $J2=∫012κ(s)2dsJ_2 = \int_0^{12} \kappa(s)^2 ds$ )（曲率平方的积分，惩罚急转弯）
( $J3=∫012(dκds)2dsJ_3 = \int_0^{12} \left( \frac{d \kappa}{ds} \right)^2 ds$ )（曲率导数平方的积分，惩罚曲率变化过快）

权重 $w_1 = 0 ), ( w_2 = 1 ), ( w_3 = 1 )$ ，目标函数为：
$\int_0^{12} \kappa(s)^2 ds + \int_0^{12} \left( \frac{d \kappa}{ds} \right)^2 ds$

4.4 约束条件

路径必须在起点和终点满足边界条件：

起点 ( $(s = 0)$ )：
- ( $κ(0)=a=0\kappa(0) = a = 0$ )
- ( $θ(0)=0\theta(0) = 0$ )
- ( $(x (0), y (0)) = (0, 0)$ )
终点 ( $(s = 12)$ )：
- ( $κ(12)=a+12b+144c+1728d=0.1\kappa(12) = a + 12b + 144c + 1728d = 0.1$ )
- ( $θ(12)=∫012κ(s)ds=30∘=π6\theta(12) = \int_0^{12} \kappa(s) ds = 30^\circ = \frac{\pi}{6}$ )
- ( $(x (12), y (12)) = (10, 5)$ )

位置坐标 ( $(x (s), y (s))$ ) 通过积分计算：
$\int_0^s \cos(\theta(u)) du, \quad y(s) = \int_0^s \sin(\theta(u)) du, \quad \theta(s) = \int_0^s \kappa(u) du$

4.5 优化过程

由于约束条件涉及积分，我们将路径离散化并使用数值方法近似目标函数和约束条件。

4.5.1 离散化

将弧长 ( $[0, 12]$ ) 划分为 ( $N = 120$ ) 个区间，步长 ( $Δs=0.1\Delta s = 0.1$ )。点为 $s_i = i \cdot \Delta s ), ( i = 0, 1, \dots, 120 )$ 。

4.5.2 数值近似

曲率平方项：
$J2≈∑i=0119κ(si)2+κ(si+1)22⋅ΔsJ_2 \approx \sum_{i=0}^{119} \frac{\kappa(s_i)^2 + \kappa(s_{i+1})^2}{2} \cdot \Delta s$
曲率导数平方项（其中 ( $dκds=b+2cs+3ds2)\frac{d \kappa}{ds} = b + 2c s + 3d s^2)$ ）：
$J3≈∑i=0119(dκds∣∗si)2+(dκds∣∗si+1)22⋅ΔsJ_3 \approx \sum_{i=0}^{119} \frac{\left( \frac{d \kappa}{ds} \big|*{s_i} \right)^2 + \left( \frac{d \kappa}{ds} \big|*{s_{i+1}} \right)^2}{2} \cdot \Delta s$
航向和位置：
- ( $θ(s0)=0\theta(s_0) = 0$ )
- ( $θ(si)=θ(si−1)+κ(si−1)+κ(si)2⋅Δs\theta(s_i) = \theta(s_{i-1}) + \frac{\kappa(s_{i-1}) + \kappa(s_i)}{2} \cdot \Delta s$ )
- ( $x(si)=x(si−1)+cos⁡(θ(si−1))+cos⁡(θ(si))2⋅Δsx(s_i) = x(s_{i-1}) + \frac{\cos(\theta(s_{i-1})) + \cos(\theta(s_i))}{2} \cdot \Delta s$ )
- ( $y(si)=y(si−1)+sin⁡(θ(si−1))+sin⁡(θ(si))2⋅Δsy(s_i) = y(s_{i-1}) + \frac{\sin(\theta(s_{i-1})) + \sin(\theta(s_i))}{2} \cdot \Delta s$ )

4.5.3 优化问题

最小化 ( $J = J_2 + J_3$ )，并满足：

( $κ(0)=a=0\kappa(0) = a = 0$ )
( $κ(12)=a+12b+144c+1728d=0.1\kappa(12) = a + 12b + 144c + 1728d = 0.1$ )
( $θ(12)≈π6\theta(12) \approx \frac{\pi}{6}$ )
( $\approx 10), (y(12) \approx 5$ )

4.5 使用 IPOPT 求解

在实际应用中，这个非线性规划（NLP）问题使用 IPOPT 等求解器来解决，通过调整 $(a, b, c, d) 使 (J)$ 最小化，同时在容许范围内满足约束条件。

示例解

优化后（假设结果）：

( $a = 0$ )
( $b = 0.01$ )
( $c = - 0.001$ )
( $d = 0.0001$ )

曲率函数：
$κ(s)=0.01s−0.001s2+0.0001s3\kappa(s) = 0.01 s - 0.001 s^2 + 0.0001 s^3$

可以通过数值积分验证 $(θ(12)),(x(12)),和(y(12))(\theta(12)), (x(12)), 和 (y(12))$ 是否与目标值匹配。
在这里插入图片描述
这个图是一个2D平面视图，比较多项式螺旋算法优化前后的路径效果。灰色虚线和点表示参考路径（未优化，带有噪声的离散点，从起点 (0,0) 到终点 (10,5)），模拟路径规划器（如 A*）生成的初始粗糙轨迹，可能包含尖锐转弯或不连续曲率。蓝色实线表示优化后的平滑路径，使用三次多项式螺旋计算，确保曲率连续并贴近参考路径，同时绕过红色障碍物（例如矩形区域代表静态障碍）。绿色箭头标注起点和终点方向（从 $θ0=0∘\theta_0=0^\circ$ 到 $θf=30∘\theta_f=30^\circ$ ），突出算法如何平衡平滑性、偏差最小化和碰撞避免。这个图直观展示算法在自动驾驶场景（如 Apollo 系统）中的应用，例如生成碰撞自由的 G2 连续路径，通过多项式插值调整系数以优化代价函数，包括平滑项和偏差项。

4. Polynomial Spiral 的优势与局限性

4.1 优势

曲率连续性
Polynomial Spiral 提供曲率连续的路径（ $κ(s)\kappa(s)$ 是连续函数），这对车辆动态控制至关重要。
灵活性
通过调整多项式次数和系数，可以生成适应不同场景的曲线形状。
计算效率
多项式函数易于计算，优化过程适合实时应用。

4.2 局限性

振荡问题
高次多项式可能导致路径振荡（如不必要的波浪形），因此 Apollo 倾向于使用三次多项式。
数值误差
数值积分可能在长路径或高曲率区域引入误差，需要仔细选择离散化步长。

5. 与其他方法的对比

布洛赫曲线 (Clothoid)
曲率线性变化（ $κ(s)=k0+k1s\kappa(s) = k_0 + k_1 s$ ），适合简单转弯，但灵活性不足。
贝塞尔曲线 (Bézier Curve)
通过控制点定义，设计直观，但曲率可能不连续。
样条插值 (Spline Interpolation)
提供高阶连续性（如 $C^2$ ），但计算复杂且曲率难以精确控制。

Polynomial Spiral 在曲率连续性、灵活性和计算效率之间取得了平衡，因此成为 Apollo 的优选方法。

6. 总结

Polynomial Spiral 是一种强大的路径平滑技术，在 Apollo 自动驾驶系统中用于生成平滑、可行且符合动态约束的路径。它通过多项式曲率函数、优化过程和数值方法，将离散的参考路径转化为连续轨迹，广泛应用于变道、转弯和避障等场景。其优雅的数学基础和高效的实现使其在自动驾驶领域具有重要价值。

查看全文

http://www.dtcms.com/a/293437.html