3、全局路径表示：路径点序列、样条曲线插值（三次样条、B样条）、路径平滑处理、路径坐标系转换

各位同学，咱们今天聊聊全局路径表示。说实话，这是纯追踪算法落地时最容易被忽视的一环。很多人以为路径就是一堆点连起来，结果上车一跑，方向盘抖得像筛糠。嗯，我当年也踩过这个坑。

3.1 路径点序列——最朴素的表达方式

路径点序列，说白了就是一堆离散的坐标点。比如你从高精地图上采了一串GPS坐标，或者从规划模块拿到一串(x, y)点，这就是最原始的路径表示。

我个人习惯把路径点序列看作「骨架」。它定义了车辆大致要走的方向，但细节需要靠插值来填充。举个例子：

// 路径点序列示例（单位：米）
path_points = [
  {x: 0.0, y: 0.0},
  {x: 1.0, y: 0.5},
  {x: 2.0, y: 1.8},
  {x: 3.0, y: 3.2},
  {x: 4.0, y: 4.0}
]

我在项目中遇到过一个问题：路径点太稀疏时，车辆会在点之间走直线，拐弯处出现明显的「折线感」。你想想看，如果路径点间距超过5米，纯追踪算法在弯道里基本就废了。

3.2 样条曲线插值——让路径变「顺」

为什么要插值？因为离散点之间没有连续的一阶导数（曲率）。纯追踪算法需要知道路径上任意一点的曲率，否则前轮转角算出来会跳变。

3.2.1 三次样条插值

三次样条插值，我理解就是「分段三次多项式」。每两个相邻路径点之间用一个三次多项式连接，保证在连接点处一阶导数和二阶导数连续。

数学上长这样：

S_i(x) = a_i + b_i(x - x_i) + c_i(x - x_i)² + d_i(x - x_i)³

其中 i 表示第 i 段路径。系数 a_i, b_i, c_i, d_i 通过边界条件求解。

我曾经用三次样条做过一个项目，效果不错。但有个坑：如果路径点有噪声，三次样条会「过拟合」，产生不必要的振荡。说白了就是路径点稍微抖一下，插值出来的曲线就跟着抖。

3.2.2 B样条插值

B样条比三次样条更「温和」。它不强制通过所有路径点，而是用控制点来「逼近」路径。这样做的好处是：局部调整不影响全局。

B样条的数学表达式：

C(u) = Σ N_{i,p}(u) * P_i

其中 N_{i,p}(u) 是B样条基函数，P_i 是控制点，p 是次数（一般用三次）。

我个人更推荐B样条。为什么？因为实际路径规划中，路径点往往带有噪声或人为误差。B样条天然具有平滑性，不会因为一个点异常就导致整条路径变形。

3.3 路径平滑处理——别让车「跳舞」

路径平滑，说白了就是去掉路径上的「毛刺」。我见过最夸张的情况：路径点每隔几米就有一个0.5米的横向偏移，车辆跑起来像在走蛇形。

常用的平滑方法：

• 移动平均滤波：简单粗暴，对每个点取前后N个点的平均值。适合噪声较小的情况。
• Savitzky-Golay滤波：用多项式拟合局部窗口。能保留路径的曲率特征，我比较常用。
• 曲率约束平滑：限制路径的最大曲率变化率。适合有物理约束的场景。

举个例子，移动平均滤波的代码：

def smooth_path(points, window_size=5):
    smoothed = []
    half = window_size // 2
    for i in range(len(points)):
        start = max(0, i - half)
        end = min(len(points), i + half + 1)
        avg_x = sum(p.x for p in points[start:end]) / (end - start)
        avg_y = sum(p.y for p in points[start:end]) / (end - start)
        smoothed.append((avg_x, avg_y))
    return smoothed

3.4 路径坐标系转换——从全局到局部

纯追踪算法通常工作在车辆坐标系下。但全局路径是大地坐标系（比如UTM坐标）。所以必须做坐标系转换。

转换步骤：

1. 平移：将全局坐标原点移到车辆当前位置。
2. 旋转：将全局坐标轴旋转到车辆航向角方向。

数学公式：

x_local = (x_global - x_vehicle) * cos(θ) + (y_global - y_vehicle) * sin(θ)
y_local = -(x_global - x_vehicle) * sin(θ) + (y_global - y_vehicle) * cos(θ)

其中 θ 是车辆的航向角。

我记得有一次调试，发现纯追踪算法总是算不对前轮转角。查了半天，原来是坐标系转换时把正负号搞反了。嗯，这种低级错误最容易出在凌晨三点。

3.5 本章知识体系

下面这张图总结了全局路径表示的核心逻辑：

这张图展示了全局路径表示的四个核心模块。路径点序列是基础，经过样条插值变成连续曲线，再通过平滑处理去除噪声，最后转换到车辆坐标系供纯追踪算法使用。每个环节都缺一不可。

---

公众号:蓝海资料掘金营，微信deep3321