第1章：路径表示方法——路径点序列、多项式曲线、参数化路径与平滑插值

各位同学好，我是老张。今天咱们聊聊路径表示方法。说实话，这是整个路径跟踪的基石。你想想看，连路径怎么描述都没搞清楚，后面谈什么跟踪控制？

我在2018年做AGV项目时，就吃过这个亏。当时图省事，直接用离散点序列当路径，结果小车跑起来一顿一顿的，像得了帕金森。后来才明白——路径表示方法选不对，后面全是坑。

1.1 路径点序列——最朴素但最常用的方式

路径点序列，说白了就是一堆坐标点。比如：

// 路径点序列示例
struct Waypoint {
    double x;
    double y;
    double yaw;      // 可选，航向角
    double velocity; // 可选，期望速度
};

Waypoint path[] = {
    {0.0, 0.0, 0.0, 1.0},
    {1.0, 2.0, 0.3, 1.2},
    {3.0, 4.0, 0.5, 1.5},
    {5.0, 5.0, 0.7, 1.0}
};

这种方式简单直观，但有个致命问题——点与点之间怎么走？

所以，路径点序列通常需要配合插值算法使用。我个人习惯的做法是：

• 路径点间距控制在0.5-1.0米（视车速而定）
• 转弯处加密点，直线段稀疏点
• 每个点附带期望速度信息

1.2 多项式曲线——让路径更平滑

为什么要用多项式曲线？因为路径点序列太「硬」了。你想想看，机器人运动需要连续的位置、速度、甚至加速度。多项式曲线正好能提供这种连续性。

1.2.1 三次样条插值

三次样条是我在项目中用得最多的方法。它保证了一阶和二阶导数连续，也就是速度和加速度连续。

// 三次样条插值核心逻辑
// 输入：路径点序列 (x_i, y_i), i=0,...,n
// 输出：每段的三次多项式系数

// 每段形式：S_i(t) = a_i + b_i*t + c_i*t^2 + d_i*t^3
// 约束条件：
// 1. 通过所有路径点
// 2. 一阶导数连续
// 3. 二阶导数连续
// 4. 边界条件（自然样条或固定端点条件）

1.2.2 贝塞尔曲线

贝塞尔曲线在图形学里用得很多，但在机器人路径规划中也有独特优势。它的特点是：

• 通过首尾控制点，但不一定通过中间控制点
• 凸包性质——曲线一定在控制点构成的凸包内
• 适合做局部路径调整

我记得在做一个泊车辅助系统时，就用贝塞尔曲线来生成倒车路径。因为只需要调整两个控制点，就能改变整个曲线的形状，调试起来特别方便。

// 三阶贝塞尔曲线公式
// P(t) = (1-t)^3*P0 + 3*(1-t)^2*t*P1 + 3*(1-t)*t^2*P2 + t^3*P3
// 其中 t ∈ [0, 1]

double bezierPoint(double p0, double p1, double p2, double p3, double t) {
    double u = 1.0 - t;
    return u*u*u*p0 + 3*u*u*t*p1 + 3*u*t*t*p2 + t*t*t*p3;
}

1.3 参数化路径——让路径与时间解耦

嗯，这里要注意。前面讲的路径表示，本质上都是「几何路径」。但机器人运动需要知道「什么时候到什么地方」。这就引出了参数化路径。

参数化路径的核心思想是：用参数s（通常是弧长）来描述路径，然后单独用时间t来描述运动。

常见的参数化方式：

参数类型	特点	适用场景
弧长参数	均匀采样，计算曲率方便	大多数路径跟踪算法
时间参数	直接对应运动时间	轨迹规划（含时间约束）
归一化参数	t ∈ [0,1]，简单易用	贝塞尔曲线、B样条

1.4 路径平滑与插值——从离散到连续

为什么需要平滑？因为真实世界的机器人有物理限制。最大加速度、最大转向角速度，这些都是硬约束。

我常用的平滑方法：

1. 移动平均滤波——简单粗暴，适合噪声大的路径点
2. B样条拟合——比三次样条更灵活，支持局部调整
3. 曲率约束平滑——保证路径曲率不超过车辆最小转弯半径

// 曲率约束平滑伪代码
// 输入：原始路径点序列
// 输出：满足曲率约束的平滑路径

for each point in path:
    // 计算当前曲率
    curvature = calculateCurvature(point, prev_point, next_point)
    
    // 如果曲率超过限制
    if abs(curvature) > max_curvature:
        // 调整点位置，降低曲率
        adjustPointPosition(point, max_curvature)
        
    // 迭代直到所有点满足约束

知识体系总览

下面这张图总结了本章的核心内容，我建议你保存下来，后面学完整个课程再回来看，会有更深的理解。

这张图把四种路径表示方法的关系理清楚了。你注意看，路径点序列是最底层的数据来源，多项式曲线和参数化路径是两种不同的「加工方式」，而平滑与插值则是贯穿始终的「精加工」步骤。

好了，这一章就讲到这里。路径表示方法看似基础，但真的决定了你后面所有工作的成败。下一章我们会讲路径跟踪的经典算法——纯跟踪和Stanley方法，到时候你会更深刻地理解为什么路径表示这么重要。

---

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