4、路径表示方法：路径点序列、多项式曲线、贝塞尔曲线、样条曲线、路径平滑性评价

各位工程师朋友，今天我们来聊聊路径表示方法。说实话，这个问题我早年做AGV调度系统时踩过不少坑。你想想看，路径怎么描述，直接决定了你的机器人能不能走得顺、走得准。

路径表示，说白了就是「怎么告诉机器人该往哪儿走」。不同的场景，适合不同的方法。我把它分成五块来讲：路径点序列、多项式曲线、贝塞尔曲线、样条曲线，最后再聊聊怎么评价一条路径好不好。

4.1 路径点序列

这是最朴素的方法。就是把路径上的一系列关键点记下来，机器人照着这些点走。

我在项目中遇到过一种情况：用路径点序列做仓储机器人导航。当时觉得这方法太简单了，结果发现点太密了计算量大，点太疏了路径不平滑。嗯，这里要注意一个原则——点间距要跟机器人的控制周期匹配。

举个例子：

// 路径点序列示例
PathPoint[] waypoints = {
    {x: 0.0, y: 0.0, theta: 0.0},
    {x: 1.0, y: 0.5, theta: 0.2},
    {x: 2.0, y: 1.0, theta: 0.4},
    {x: 3.0, y: 1.5, theta: 0.6}
};

路径点序列的优缺点很明显：

优点	缺点
实现简单，容易理解	路径不平滑，有折角
存储开销小	需要大量点才能逼近曲线
适合离散控制	速度连续性差

4.2 多项式曲线

多项式曲线，就是用多项式函数来拟合路径。最常见的是一次多项式（直线）、二次多项式（抛物线）、三次多项式（S曲线）。

我个人习惯用三次多项式。为什么？因为它能保证位置和速度都连续。你想想看，如果机器人走到某点突然速度跳变，那冲击感多难受。

// 三次多项式曲线示例
// x(t) = a0 + a1*t + a2*t^2 + a3*t^3
// y(t) = b0 + b1*t + b2*t^2 + b3*t^3

double x(double t) {
    return a0 + a1*t + a2*t*t + a3*t*t*t;
}

double y(double t) {
    return b0 + b1*t + b2*t*t + b3*t*t*t;
}

多项式曲线有个坑——阶数高了容易震荡。我曾经试过用五次多项式做路径规划，结果曲线在中间段剧烈抖动，把电机都震得嗡嗡响。后来我学乖了，一般不超过三次。

4.3 贝塞尔曲线

贝塞尔曲线，这玩意儿在图形学里用得很多。它的核心思想是：用控制点来「拉」出一条曲线。

贝塞尔曲线的数学形式是这样的：

// 三次贝塞尔曲线
// B(t) = (1-t)^3*P0 + 3*(1-t)^2*t*P1 + 3*(1-t)*t^2*P2 + t^3*P3
// 其中 P0, P1, P2, P3 是控制点

Point bezier(double t, Point P0, Point P1, Point P2, Point P3) {
    double u = 1 - t;
    double x = u*u*u*P0.x + 3*u*u*t*P1.x + 3*u*t*t*P2.x + t*t*t*P3.x;
    double y = u*u*u*P0.y + 3*u*u*t*P1.y + 3*u*t*t*P2.y + t*t*t*P3.y;
    return Point(x, y);
}

贝塞尔曲线的好处是：控制点直观，调整方便。我在做机器人避障时经常用它——把障碍物周围的点设成控制点，曲线自然就绕过去了。

但贝塞尔曲线也有局限：全局性太强。你改一个控制点，整条曲线都变了。这在局部调整时很麻烦。

4.4 样条曲线

样条曲线，尤其是B样条和三次样条，是我最推荐的方法。它结合了多项式的平滑性和路径点序列的灵活性。

样条曲线的核心思想是：分段拟合，整体平滑。每段用低阶多项式，段与段之间保证连续。

// 三次样条插值示例
// 给定n个点，生成n-1段三次多项式
// 每段形式: Si(x) = ai + bi*(x-xi) + ci*(x-xi)^2 + di*(x-xi)^3

struct SplineSegment {
    double a, b, c, d;  // 多项式系数
    double x_start;      // 起始x坐标
};

// 求解样条系数（三对角矩阵法）
vector<SplineSegment> cubicSpline(vector<Point>& points) {
    // ... 求解过程略
}

我记得有一次做高速搬运机器人，用路径点序列时末端抖动厉害。换成三次样条后，抖动直接降了80%。从那以后，但凡要求高的项目，我都用样条。

4.5 路径平滑性评价

路径好不好，不能光凭感觉。得有量化指标。我一般用这几个维度来评价：

1. 曲率连续性：曲率变化越小越好。突变会导致向心力突变。
2. 曲率变化率：也就是曲率对弧长的导数。这个值越小，路径越「舒服」。
3. 最大曲率：不能超过机器人的最小转弯半径。
4. 路径长度：在满足平滑的前提下，越短越好。

这里给个简单的评价函数：

// 路径平滑性评价函数
double smoothnessScore(vector<Point>& path) {
    double score = 0.0;
    double curvature_sum = 0.0;
    double curvature_change_sum = 0.0;
    
    for (int i = 1; i < path.size() - 1; i++) {
        // 计算曲率
        double kappa = computeCurvature(path[i-1], path[i], path[i+1]);
        curvature_sum += fabs(kappa);
        
        // 计算曲率变化
        if (i > 1) {
            double kappa_prev = computeCurvature(path[i-2], path[i-1], path[i]);
            curvature_change_sum += fabs(kappa - kappa_prev);
        }
    }
    
    // 综合评分（越小越好）
    score = curvature_sum / path.size() + curvature_change_sum / path.size();
    return score;
}

最后，我用一张图来总结这几种路径表示方法的关系：

这张图把四种路径表示方法的关系理清楚了。从最简单的路径点序列，到最复杂的样条曲线，每种方法都有自己的适用场景。而路径平滑性评价，则是检验这些方法好坏的标尺。

我个人建议：新手从路径点序列入手，老手直接上样条曲线。中间的多项式和贝塞尔，可以作为过渡和补充。但不管用哪种方法，最后一定要用平滑性指标验证一遍。这是我从多次翻车经历中总结出来的教训。

可可精品资料库