本文详细介绍分段五次多项式插值的数学原理、公式推导及代码实现。该方法广泛应用于自动驾驶路径规划，能够生成 $C^2$ 连续（位置、航向连续；可实现曲率连续）的平滑轨迹。

一、问题背景

1.1 为什么需要多项式插值？

在自动驾驶路径规划中，经常需要将离散的路径点（如车道中心线、参考轨迹）转换为连续可导的曲线，以满足：

1. 舒适性要求：曲率连续，避免方向盘突变
2. 控制可行性：车辆运动学约束要求轨迹 $C^2$ 连续
3. 优化便利性：多项式形式便于求导、积分和优化

1.2 为什么选择"五次"？

核心思路：边界条件数量决定多项式次数。

要实现 $C^2$ 连续，每段曲线需要满足起点和终点的约束：

• 起点：位置 + 速度 + 加速度 → 3个方程
• 终点：位置 + 速度 + 加速度 → 3个方程
• 共计：6个方程

n 次多项式有 n+1 个待定系数。要解 6 个方程，需要 6 个未知数，因此选择 5次多项式（系数 a₀~a₅ 共6个）。

二、曲线连续性的数学定义

2.1 C⁰、C¹、C²、C³ 连续性

对于参数曲线 $\mathbf{r}(s) = (x(s), y(s))$，其中 $s$ 为参数：

$C^0$ 连续（位置连续）

两段曲线 $\mathbf{r}_1(s)$ 和 $\mathbf{r}_2(s)$ 在连接点处位置相同：

$$\mathbf{r}_1(s_{end}) = \mathbf{r}_2(s_{start})$$

即：

$$x_1(s_{end}) = x_2(s_{start}), \quad y_1(s_{end}) = y_2(s_{start})$$

物理意义：轨迹没有"断点"，车辆可以从一段平滑过渡到下一段。

$C^1$ 连续（切线连续）

在 $C^0$ 连续的基础上，一阶导数（切向量）也连续。

$$\mathbf{r}'_1(s_{end}) = \mathbf{r}'_2(s_{start})$$

即：

$$x'_1(s_{end}) = x'_2(s_{start}), \quad y'_1(s_{end}) = y'_2(s_{start})$$

推导：一阶导数定义切线方向

$$\mathbf{r}'(s) = \frac{d\mathbf{r}}{ds} = \left(\frac{dx}{ds}, \frac{dy}{ds}\right) = (x', y')$$

物理意义：航向角 θ 连续，车辆经过连接点时不需要瞬时转向。

$C^2$ 连续（二阶导数连续）

在 $C^1$ 连续的基础上，二阶导数也连续。

$$\mathbf{r}''_1(s_{end}) = \mathbf{r}''_2(s_{start})$$

即：

$$x''_1(s_{end}) = x''_2(s_{start}), \quad y''_1(s_{end}) = y''_2(s_{start})$$

推导：二阶导数与曲率的关系

曲率公式（详见 §4.2）：

$$\kappa = \frac{x'y'' - y'x''}{\left((x')^2 + (y')^2\right)^{3/2}}$$

当 $x', y'$ 连续（$C^1$）且 $x'', y''$ 连续（$C^2$），且 $|\mathbf{r}'| \neq 0$ 时，曲率 $\kappa$ 连续。

物理意义：曲率连续，车辆经过连接点时方向盘转角不需要瞬时变化，乘客感觉舒适。

$C^3$ 连续（Jerk 连续）

在 $C^2$ 连续的基础上，三阶导数也连续。

$$\mathbf{r}'''_1(s_{end}) = \mathbf{r}'''_2(s_{start})$$

物理意义：曲率变化率连续，方向盘转动速度连续,更适合高速场景。

2.2 连续性与多项式次数的关系

结论：要实现 $C^2$ 连续,每段需要 6 个边界条件,因此选择五次多项式。

三、五次多项式数学模型

3.1 参数化表示

对于路径的一段,使用参数 s ∈ [0, 1]（归一化弧长），分别对 x 和 y 坐标建立五次多项式：

$$x(s) = a_0 + a_1 s + a_2 s^{2} + a_3 s^{3} + a_4 s^{4} + a_5 s^{5}$$ $$y(s) = b_0 + b_1 s + b_2 s^{2} + b_3 s^{3} + b_4 s^{4} + b_5 s^{5}$$

简记为：

$$x(s) = \sum_{i=0}^{5} a_i s^{i}, \quad y(s) = \sum_{i=0}^{5} b_i s^{i}$$

3.2 关于参数 s 的说明

为什么用 s ∈ [0, 1] 而不是 s ∈ [0, L]？

使用归一化参数有以下优点：

1. 数值稳定性：系数 $a_i$ 的量级更均匀
2. 通用性：不同长度的段使用相同的公式
3. 边界条件统一：起点 s=0，终点 s=1

3.3 L（段长度）的详细解释

L 是本段路径的实际物理长度（单位：米）。

为什么需要 L？

1. 速度归一化：将物理速度转换为参数域速度
2. 加速度归一化：将物理加速度转换为参数域加速度
3. 曲率计算：曲率公式中的分母涉及切向量长度

物理量与参数量的转换

边界条件中 L 的作用

设切向量长度为 L（即参数域中 $|\mathbf{r}'| = L$），则：

$$\begin{aligned} x' &= L \cos\theta \\ y' &= L \sin\theta \\ x'' &= -L^{2} \kappa \sin\theta \\ y'' &= L^{2} \kappa \cos\theta \end{aligned}$$

推导过程见 §4.1 和 §4.2。

3.4 边界条件

五次多项式有 6 个待定系数，需要 6 个边界条件：

起点 (s = 0)：

终点 (s = 1)：

四、边界条件的物理意义与公式推导

4.1 一阶导数与航向角的关系

推导过程

参数曲线 $\mathbf{r}(s) = (x(s), y(s))$ 的切向量为：

$$\mathbf{r}'(s) = \left(\frac{dx}{ds}, \frac{dy}{ds}\right) = (x', y')$$

切线方向（航向角 θ)满足：

$$\tan\theta = \frac{dy/ds}{dx/ds} = \frac{y'}{x'}$$

由反正切函数得到：

$$\theta = \arctan\left(\frac{y'}{x'}\right)$$

但 arctan 只能处理一三象限， 范围为 $(-\frac{\pi}{2}, \frac{\pi}{2})$。

在路径规划中，切向量可能指向任何方向，因此使用 双参数反正切函数 arctan2：

$$\theta = \arctan2(y', x')$$

arctan2 的优势：能正确处理所有四个象限，返回范围 $(-\pi, \pi]$。

引入段长度 L

设切向量的模长为 L（段长度），即：

$$|\mathbf{r}'| = \sqrt{(x')^2 + (y')^2} = L$$

根据三角函数定义：

$$\cos\theta = \frac{x'}{|\mathbf{r}'|} = \frac{x'}{L}, \quad \sin\theta = \frac{y'}{|\mathbf{r}'|} = \frac{y'}{L}$$

反解得到：

$$\boxed{x' = L \cos\theta, \quad y' = L \sin\theta}$$

代码实现

1
2
3
4
5

# 一阶导数边界条件
xp0 = L * np.cos(theta0)  # 起点x方向"速度"
yp0 = L * np.sin(theta0)  # 起点y方向"速度"
xp1 = L * np.cos(theta1)  # 终点x方向"速度"
yp1 = L * np.sin(theta1)  # 终点y方向"速度"

4.2 二阶导数与曲率的关系

曲率公式的推导

曲率的几何定义：曲率是切线方向对弧长的变化率。

$$\kappa = \frac{d\theta}{d\ell}$$

其中 $\ell$ 是弧长，$\theta$ 是切线角。
参数曲线的曲率公式推导：
由 $\theta = \arctan(y'/x')$，对弧长 $\ell$ 求导：

$$\frac{d\theta}{d\ell} = \frac{d}{d\ell}\arctan\left(\frac{y'}{x'}\right)$$

使用链式法则和商法则：

$$\frac{d\theta}{d\ell} = \frac{1}{1+(y'/x')^{2}} \cdot \frac{x'y'' - y'x''}{(x')^2} \cdot \frac{1}{|\mathbf{r}'|}$$

其中 $|\mathbf{r}'| = \sqrt{(x')^2 + (y')^2}$ 是切向量长度。
简化：

$$\kappa = \frac{x'y'' - y'x''}{\left((x')^2 + (y')^2\right)^{3/2}}$$ $$\boxed{\kappa = \frac{x'y'' - y'x''}{|\mathbf{r}'|^{3}}}$$

从曲率反推二阶导数

已知曲率 $\kappa$ 和航向角 $\theta$，切向量长度 $|\mathbf{r}'| = L$，求 $x'', y''$。
由曲率公式：

$$\kappa = \frac{x'y'' - y'x''}{L^{3}}$$

代入 $x' = L\cos\theta$, $y' = L\sin\theta$：

$$\kappa L^{3} = (L\cos\theta) y'' - (L\sin\theta) x''$$ $$\kappa L^{2} = \cos\theta \cdot y'' - \sin\theta \cdot x''$$

关键观察：$x'', y''$ 可以看作切向加速度和法向加速度的分量。
在曲线运动中：

• 切向加速度：$a_t = \frac{d|\mathbf{v}|}{dt}$（速度大小变化）
• 法向加速度：$a_n = \frac{v^{2}}{R} = v^{2}\kappa$（向心加速度）
  当速度恒定时（$a_t = 0$），加速度完全指向曲率中心（法向）：

$$\mathbf{a} = a_n \cdot \mathbf{n} = v^{2}\kappa \cdot \mathbf{n}$$

其中 $\mathbf{n} = (-\sin\theta, \cos\theta)$ 是法向量（指向曲率中心）。
因此在参数域（$v = L$）：

$$\begin{aligned} x'' &= -L^{2} \kappa \sin\theta \\ y'' &= L^{2} \kappa \cos\theta \end{aligned}$$

验证：代入曲率公式

$$\kappa = \frac{(L\cos\theta)(L^{2}\kappa\cos\theta) - (L\sin\theta)(-L^{2}\kappa\sin\theta)}{L^{3}} = \frac{L^{3}\kappa(\cos^{2}\theta + \sin^{2}\theta)}{L^{3}} = \kappa \checkmark$$ $$\boxed{x'' = -L^{2} \kappa \sin\theta, \quad y'' = L^{2} \kappa \cos\theta}$$

代码实现

1
2
3
4
5

# 二阶导数边界条件（由曲率反推）
xpp0 = -L**2 * kappa0 * np.sin(theta0)  # 起点x加速度
ypp0 =  L**2 * kappa0 * np.cos(theta0)  # 起点y加速度
xpp1 = -L**2 * kappa1 * np.sin(theta1)  # 终点x加速度
ypp1 =  L**2 * kappa1 * np.cos(theta1)  # 终点y加速度

五、系数求解与线性方程组推导

5.1 从边界条件到线性方程组

设五次多项式：

$$x(s) = a_0 + a_1 s + a_2 s^{2} + a_3 s^{3} + a_4 s^{4} + a_5 s^{5}$$

各阶导数：

$$\begin{aligned} x'(s) &= a_1 + 2a_2 s + 3a_3 s^{2} + 4a_4 s^{3} + 5a_5 s^{4} \\ x''(s) &= 2a_2 + 6a_3 s + 12a_4 s^{2} + 20a_5 s^{3} \end{aligned}$$

起点 (s = 0) 的边界条件：

$$\begin{aligned} x(0) &= a_0 = x_0 \\ x'(0) &= a_1 = x'_0 \\ x''(0) &= 2a_2 = x''_0 \Rightarrow a_2 = \frac{x''_0}{2} \end{aligned}$$

终点 (s = 1) 的边界条件：

$$\begin{aligned} x(1) &= a_0 + a_1 + a_2 + a_3 + a_4 + a_5 = x_1 \\ x'(1) &= a_1 + 2a_2 + 3a_3 + 4a_4 + 5a_5 = x'_1 \\ x''(1) &= 2a_2 + 6a_3 + 12a_4 + 20a_5 = x''_1 \end{aligned}$$

得到方程组（未知量 $a_3, a_4, a_5$）：

$$\begin{cases} a_3 + a_4 + a_5 = x_1 - a_0 - a_1 - a_2 = \Delta x - a_1 - a_2 \\ 3a_3 + 4a_4 + 5a_5 = x'_1 - a_1 - 2a_2 \\ 6a_3 + 12a_4 + 20a_5 = x''_1 - 2a_2 \end{cases}$$

5.2 矩阵形式

完整的六元线性方程组矩阵形式：

$$\begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 1 & 1 & 1 & 1 & 1 & 1 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 2 & 3 & 4 & 5 \\ 0 & 0 & 2 & 0 & 0 & 0 \\ 0 & 0 & 2 & 6 & 12 & 20 \end{bmatrix} \begin{bmatrix} a_0 \\ a_1 \\ a_2 \\ a_3 \\ a_4 \\ a_5 \end{bmatrix} = \begin{bmatrix} x_0 \\ x_1 \\ x'_0 \\ x'_1 \\ x''_0 \\ x''_1 \end{bmatrix}$$

矩阵元素来源：

• 第1行：$x(0) = a_0$，只有 $a_0$ 系数为 1
• 第2行：$x(1) = a_0 + a_1 + a_2 + a_3 + a_4 + a_5$，所有系数为 1
• 第3行：$x'(0) = a_1$，只有 $a_1$ 系数为 1
• 第4行：$x'(1) = a_1 + 2a_2 + 3a_3 + 4a_4 + 5a_5$，系数为 1,2,3,4,5
• 第5行：$x''(0) = 2a_2$，只有 $a_2$ 系数为 2
• 第6行：$x''(1) = 2a_2 + 6a_3 + 12a_4 + 20a_5$，系数为 2,6,12,20

5.3 解析解推导

由前三个方程直接得到：

$$a_0 = x_0, \quad a_1 = x'_0, \quad a_2 = \frac{x''_0}{2}$$

代入后三个方程，令 $\Delta x = x_1 - x_0$，得到关于 $a_3, a_4, a_5$ 的方程组：

$$\begin{bmatrix} 1 & 1 & 1 \\ 3 & 4 & 5 \\ 6 & 12 & 20 \end{bmatrix} \begin{bmatrix} a_3 \\ a_4 \\ a_5 \end{bmatrix} = \begin{bmatrix} \Delta x - x'_0 - \frac{x''_0}{2} \\ x'_1 - x'_0 - x''_0 \\ x''_1 - x''_0 \end{bmatrix}$$

使用 Cramer 法则或 Gauss 消元法求解，得到：

$$\boxed{\begin{aligned} a_0 &= x_0 \\ a_1 &= x'_0 \\ a_2 &= \frac{x''_0}{2} \\ a_3 &= 10\Delta x - 6x'_0 - 4x'_1 - \frac{3}{2}x''_0 + \frac{1}{2}x''_1 \\ a_4 &= -15\Delta x + 8x'_0 + 7x'_1 + \frac{3}{2}x''_0 - x''_1 \\ a_5 &= 6\Delta x - 3x'_0 - 3x'_1 - \frac{1}{2}x''_0 + \frac{1}{2}x''_1 \end{aligned}}$$

5.4 代码实现

1
2
3
4
5
6
7
8

self.ax = [
    x0,                                                    # a₀
    xp0,                                                   # a₁
    xpp0 / 2,                                              # a₂
    10*(x1-x0) - 6*xp0 - 4*xp1 - 1.5*xpp0 + 0.5*xpp1,      # a₃
    -15*(x1-x0) + 8*xp0 + 7*xp1 + 1.5*xpp0 - xpp1,         # a₄
    6*(x1-x0) - 3*xp0 - 3*xp1 - 0.5*xpp0 + 0.5*xpp1        # a₅
]

y 方向系数 ay 的求解完全类似。

六、轨迹评估公式推导

6.1 位置

直接由多项式定义：

$$x(s) = \sum_{i=0}^{5} a_i s^{i}, \quad y(s) = \sum_{i=0}^{5} b_i s^{i}$$

6.2 一阶导数（速度）

推导：对多项式求导

$$\frac{d}{ds}\left(\sum_{i=0}^{5} a_i s^{i}\right) = \sum_{i=1}^{5} i \cdot a_i s^{i-1}$$

展开：

$$x'(s) = a_1 + 2a_2 s + 3a_3 s^{2} + 4a_4 s^{3} + 5a_5 s^{4}$$

6.3 二阶导数（加速度）

推导：再次求导

$$\frac{d^{2}}{ds^{2}}\left(\sum_{i=0}^{5} a_i s^{i}\right) = \sum_{i=2}^{5} i(i-1) \cdot a_i s^{i-2}$$

展开：

$$x''(s) = 2a_2 + 6a_3 s + 12a_4 s^{2} + 20a_5 s^{3}$$

6.4 航向角

由一阶导数的几何意义（见 §4.1）：

$$\theta(s) = \arctan2(y'(s), x'(s))$$

6.5 曲率

由曲率公式（见 §4.2）：

$$\kappa(s) = \frac{x'(s) y''(s) - y'(s) x''(s)}{\left[(x'(s))^2 + (y'(s))^2\right]^{3/2}}$$

6.6 代码实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

def evaluate(self, s: np.ndarray) -> Dict[str, np.ndarray]:
    """评估参数 s ∈ [0, 1]"""
    s = np.atleast_1d(s).astype(float)
    # 位置
    x = sum(self.ax[i] * s**i for i in range(6))
    y = sum(self.ay[i] * s**i for i in range(6))
    # 一阶导数
    dx = sum(i * self.ax[i] * s**(i-1) for i in range(1, 6))
    dy = sum(i * self.ay[i] * s**(i-1) for i in range(1, 6))
    # 二阶导数
    ddx = sum(i * (i-1) * self.ax[i] * s**(i-2) for i in range(2, 6))
    ddy = sum(i * (i-1) * self.ay[i] * s**(i-2) for i in range(2, 6))
    # 航向角
    theta = np.arctan2(dy, dx)
    # 曲率
    kappa = (dx * ddy - dy * ddx) / (dx**2 + dy**2)**1.5
    return {'x': x, 'y': y, 'theta': theta, 'kappa': kappa,
            'dx': dx, 'dy': dy, 'ddx': ddx, 'ddy': ddy}

七、分段策略

7.1 单段多项式的局限

用单段五次多项式拟合大转角圆弧时：

• 转角越大，曲率偏差越大
• 无法精确匹配圆弧的恒定曲率特性
• 边界条件过于刚性

7.2 分段拼接原理

将大转角圆弧分解为多个小段：

$$\text{分段数 } n = \lceil \frac{\Delta\theta}{\theta_{seg}} \rceil$$

每段独立拟合，在连接点处保证 $C^2$ 连续。

7.3 曲率边界条件设计

关键技巧：合理设置各段的曲率边界条件

1
2
3
4
5
6
7
8
9
10

# 曲率边界条件
if i == 0:
    start_kappa = 0.0      # 起点：曲率归零（便于与直线衔接）
else:
    start_kappa = 0.5 / R  # 中间段起点
if i == self.num_segments - 1:
    end_kappa = 0.0        # 终点：曲率归零
else:
    end_kappa = 0.5 / R    # 中间段终点

设计原理：

• 起点/终点 κ=0：便于与直线段平滑过渡
• 中间段 κ=0.5/R：半圆弧曲率作为"缓冲"，避免曲率突变

八、运动学分析

给定车速 v，可计算：

8.1 横向加速度

推导：向心加速度公式
车辆以速度 v 沿曲率 $\kappa$ 的曲线行驶，向心加速度为：

$$a_n = \frac{v^{2}}{R} = v^{2} \kappa$$

8.2 Jerk（加加速度）

推导：加速度对时间的变化率

$$j = \frac{da_n}{dt} = \frac{d(v^{2}\kappa)}{dt} = v^{2} \frac{d\kappa}{dt} = v^{2} \frac{d\kappa}{ds} \cdot \frac{ds}{dt} = v^{3} \frac{d\kappa}{ds}$$

数值近似：

$$j \approx v^{3} \frac{\Delta\kappa}{\Delta s}$$

8.3 代码实现

1
2
3
4
5
6
7
8
9

# 横向加速度
a_n = v**2 * path['kappa']
# Jerk
jerk = np.zeros_like(path['kappa'])
for i in range(1, len(path['s'])):
    ds = path['s'][i] - path['s'][i-1]
    if ds > 0:
        jerk[i] = v**3 * (path['kappa'][i] - path['kappa'][i-1]) / ds

九、完整代码实现

9.1 数据结构定义

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

from dataclasses import dataclass
from typing import Tuple, List, Dict
import numpy as np
@dataclass
class ArcInfo:
    """圆弧信息"""
    radius: float           # 半径 (米)
    start_angle: float      # 起始角度 (弧度)
    end_angle: float        # 结束角度 (弧度)
    center: Tuple[float, float] = (0, 0)  # 圆心位置
    @property
    def total_angle(self) -> float:
        """总转角（弧度）"""
        return self.end_angle - self.start_angle
    @property
    def total_angle_deg(self) -> float:
        """总转角（度）"""
        return np.degrees(self.total_angle)
    @property
    def arc_length(self) -> float:
        """弧长（米）"""
        return self.radius * abs(self.total_angle)
@dataclass
class Node:
    """路径节点"""
    x: float
    y: float
    theta: float  # 航向角（弧度）
    kappa: float  # 曲率
    s: float      # 弧长位置

9.2 五次多项式段类

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

class QuinticPolynomialSegment:
    """五次多项式段"""
    def __init__(self, start_state: Tuple, end_state: Tuple, L: float):
        """
        start_state: (x0, y0, theta0, kappa0)
        end_state: (x1, y1, theta1, kappa1)
        L: 段长度（米）
        """
        self.L = L
        x0, y0, theta0, kappa0 = start_state
        x1, y1, theta1, kappa1 = end_state
        # 一阶导数边界条件（由航向角计算）
        xp0 = L * np.cos(theta0)
        xp1 = L * np.cos(theta1)
        yp0 = L * np.sin(theta0)
        yp1 = L * np.sin(theta1)
        # 二阶导数边界条件（由曲率计算）
        xpp0 = -L**2 * kappa0 * np.sin(theta0)
        xpp1 = -L**2 * kappa1 * np.sin(theta1)
        ypp0 = L**2 * kappa0 * np.cos(theta0)
        ypp1 = L**2 * kappa1 * np.cos(theta1)
        # X 系数（解析解）
        self.ax = [
            x0,
            xp0,
            xpp0 / 2,
            10*(x1-x0) - 6*xp0 - 4*xp1 - 1.5*xpp0 + 0.5*xpp1,
            -15*(x1-x0) + 8*xp0 + 7*xp1 + 1.5*xpp0 - xpp1,
            6*(x1-x0) - 3*xp0 - 3*xp1 - 0.5*xpp0 + 0.5*xpp1
        ]
        # Y 系数（同理）
        self.ay = [
            y0,
            yp0,
            ypp0 / 2,
            10*(y1-y0) - 6*yp0 - 4*yp1 - 1.5*ypp0 + 0.5*ypp1,
            -15*(y1-y0) + 8*yp0 + 7*yp1 + 1.5*ypp0 - ypp1,
            6*(y1-y0) - 3*yp0 - 3*yp1 - 0.5*ypp0 + 0.5*ypp1
        ]
    def evaluate(self, s: np.ndarray) -> Dict[str, np.ndarray]:
        """评估参数 s ∈ [0, 1]"""
        s = np.atleast_1d(s).astype(float)
        x = sum(self.ax[i] * s**i for i in range(6))
        y = sum(self.ay[i] * s**i for i in range(6))
        dx = sum(i * self.ax[i] * s**(i-1) for i in range(1, 6))
        dy = sum(i * self.ay[i] * s**(i-1) for i in range(1, 6))
        ddx = sum(i * (i-1) * self.ax[i] * s**(i-2) for i in range(2, 6))
        ddy = sum(i * (i-1) * self.ay[i] * s**(i-2) for i in range(2, 6))
        theta = np.arctan2(dy, dx)
        kappa = (dx * ddy - dy * ddx) / (dx**2 + dy**2)**1.5
        return {'x': x, 'y': y, 'theta': theta, 'kappa': kappa,
                'dx': dx, 'dy': dy, 'ddx': ddx, 'ddy': ddy}

9.3 分段插值器

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84

class SegmentedQuinticInterpolator:
    """分段五次多项式插值器"""
    def __init__(self, arc_info: ArcInfo, segment_angle_deg: float,
                 points_per_segment: int = 50):
        self.arc_info = arc_info
        self.segment_angle_deg = segment_angle_deg
        self.segment_angle_rad = np.radians(segment_angle_deg)
        self.points_per_segment = points_per_segment
        self.num_segments = max(1, int(np.ceil(
            abs(arc_info.total_angle_deg) / segment_angle_deg
        )))
        self.quintic_segments: List[QuinticPolynomialSegment] = []
        self.all_nodes: List[Node] = []
        self._build()
    def _get_arc_point(self, angle: float) -> Tuple[float, float, float]:
        """获取圆弧上某角度处的点"""
        R = self.arc_info.radius
        cx, cy = self.arc_info.center
        # 圆心在原点时的参数化
        x = cx + R * np.sin(angle)
        y = cy + R * (1 - np.cos(angle))
        theta = angle
        return x, y, theta
    def _build(self):
        """构建所有分段"""
        R = self.arc_info.radius
        total_angle = self.arc_info.total_angle
        actual_segment_angle = total_angle / self.num_segments
        current_s = 0.0
        current_angle = self.arc_info.start_angle
        for i in range(self.num_segments):
            end_angle = current_angle + actual_segment_angle
            segment_length = R * abs(actual_segment_angle)  # L
            start_x, start_y, start_theta = self._get_arc_point(current_angle)
            end_x, end_y, end_theta = self._get_arc_point(end_angle)
            # 曲率边界条件
            start_kappa = 0.0 if i == 0 else 0.5 / R
            end_kappa = 0.0 if i == self.num_segments - 1 else 0.5 / R
            qp = QuinticPolynomialSegment(
                (start_x, start_y, start_theta, start_kappa),
                (end_x, end_y, end_theta, end_kappa),
                segment_length
            )
            self.quintic_segments.append(qp)
            # 采样并创建节点
            s_vals = np.linspace(0, 1, self.points_per_segment)
            result = qp.evaluate(s_vals)
            for j, s in enumerate(s_vals):
                if i > 0 and j == 0:
                    continue  # 避免重复添加连接点
                node = Node(
                    x=result['x'][j], y=result['y'][j],
                    theta=result['theta'][j], kappa=result['kappa'][j],
                    s=current_s + s * segment_length
                )
                self.all_nodes.append(node)
            current_angle = end_angle
            current_s += segment_length
    def get_path_data(self) -> Dict[str, np.ndarray]:
        """获取完整路径数据"""
        return {
            'x': np.array([n.x for n in self.all_nodes]),
            'y': np.array([n.y for n in self.all_nodes]),
            'theta': np.array([n.theta for n in self.all_nodes]),
            'kappa': np.array([n.kappa for n in self.all_nodes]),
            's': np.array([n.s for n in self.all_nodes])
        }

十、可视化结果

10.1 示例 1：R=15m, 90° 转角, 20° 分段

统计信息：

• 分段数：5
• 每段长度 L ≈ 5.24 m
• 曲率比：约 1.0x（接近圆弧曲率）
• 最大偏差：约 0.6 cm
• 平均偏差：约 0.3 cm

10.2 示例 2：R=10m, 180° 转角, 30° 分段

统计信息：

• 分段数：6
• 每段长度 L ≈ 5.24 m
• 曲率比：约 1.0x
• 最大偏差：约 1.4 cm
• 平均偏差：约 0.6 cm

十一、总结

11.1 方法优势

11.2 适用场景

• 自动驾驶轨迹规划
• 机器人路径平滑
• CNC 加工路径生成
• 无人机航线优化

11.3 关键公式速查

参考资料

• Polynomial Trajectory Planning for Autonomous Vehicles
• Optimal Trajectory Generation for Dynamic Street Scenarios
• Path Planning for Autonomous Vehicles

文章链接：
https://www.zywvvd.com/notes/study/control/quintic-polynomial-interpolation/quintic-polynomial-interpolation/