参考路径表示方法：离散点、多项式、样条与Frenet坐标系

路径表示，说白了就是怎么把一条路“告诉”给车。你想想看，车要沿着弯道走，总得有个数学描述吧？我这些年做项目，见过不少新手在这上面栽跟头。今天咱们就把四种主流方法掰开揉碎讲清楚。

离散点路径：最朴素的表达

这是最直接的方式。把路径上的点一个个列出来，就像用坐标纸画点一样。

// 离散点路径示例
struct PathPoint {
    double x;      // 米
    double y;      // 米
    double yaw;    // 弧度
    double kappa;  // 曲率
};

std::vector<PathPoint> discrete_path = {
    {0.0, 0.0, 0.0, 0.0},
    {1.0, 0.1, 0.2, 0.05},
    {2.0, 0.4, 0.4, 0.08},
    // ... 更多点
};

优点：简单粗暴，传感器直接输出就是这种格式。我在做高精地图解析时，原始数据就是一堆离散点。

缺点：点太密占内存，点太疏精度不够。而且你没法直接求导——曲率、航向角都得靠差分近似，噪声一大就完蛋。

⚠️ 注意：离散点路径的曲率计算对噪声极其敏感。我曾经遇到过，点间距0.1米时曲率抖动得像心电图，后来改成0.5米采样才稳定下来。

多项式拟合路径：平滑但脆弱

用多项式曲线去拟合那些离散点。最常见的是三次多项式或五次多项式。

// 三次多项式：y = a0 + a1*x + a2*x^2 + a3*x^3
struct CubicPolynomial {
    double a0, a1, a2, a3;
    
    double evaluate(double x) {
        return a0 + a1*x + a2*x*x + a3*x*x*x;
    }
    
    double derivative(double x) {
        return a1 + 2*a2*x + 3*a3*x*x;
    }
};

我个人习惯用五次多项式做换道路径规划。为什么？因为可以同时约束位置、速度和加速度的边界条件。你想想看，三次多项式只能约束位置和速度，加速度跳变会让人晕车。

💡 小技巧：多项式拟合时，别忘了做坐标变换。把路径转到局部坐标系下再拟合，数值稳定性会好很多。我早期直接在世界坐标系下拟合，结果曲率计算经常溢出。

但有个坑：多项式是全局的。一个点的噪声会污染整条曲线。你试试用10阶多项式拟合100个点，两端肯定翘到天上去——这就是著名的龙格现象。

样条曲线路径：工程界的首选

样条曲线把路径分成一段段，每段用低阶多项式拼接。既保证了平滑性，又避免了全局多项式的问题。

// 三次样条插值示例
class CubicSpline {
private:
    std::vector<double> x_, y_;
    std::vector<double> a_, b_, c_, d_;  // 每段系数
    
public:
    double evaluate(double x) {
        // 找到x所在的区间
        int i = findSegment(x);
        double dx = x - x_[i];
        // 三次样条公式
        return a_[i] + b_[i]*dx + c_[i]*dx*dx + d_[i]*dx*dx*dx;
    }
};

嗯，这里要注意：样条曲线的边界条件选择很关键。自然样条（二阶导为零）和固定边界（指定一阶导）差别很大。我做车道线跟踪时，发现自然样条在弯道入口处会有明显的“拖尾”现象，后来改成固定边界才解决。

样条类型	连续性	适用场景
三次样条	C2连续	一般路径跟踪
B样条	可调	需要局部修改
NURBS	可调	精确圆锥曲线

Frenet坐标系下的路径表示

这是最优雅的方式。把路径分解成纵向（s方向）和横向（d方向）。说白了，就是沿着路走，而不是沿着X轴走。

// Frenet坐标系下的路径点
struct FrenetPoint {
    double s;      // 纵向距离，沿参考线
    double s_dot;  // 纵向速度
    double s_ddot; // 纵向加速度
    double d;      // 横向偏移
    double d_dot;  // 横向速度
    double d_ddot; // 横向加速度
};

为什么Frenet坐标系这么香？因为大部分控制约束都是沿着道路方向的。比如“不要压线”就是d的约束，“保持安全距离”就是s的约束。在笛卡尔坐标系下，这些约束会变成复杂的非线性不等式。

🌟 核心思想：Frenet坐标系把二维路径跟踪问题，解耦成两个一维问题——纵向控制和横向控制。这是我见过最优雅的工程简化。

我曾经在高速弯道项目中吃过亏。直接用笛卡尔坐标做MPC，结果弯道曲率变化大时，预测模型完全不准。换成Frenet坐标系后，同样的MPC算法，跟踪精度提升了40%。

转换公式其实不复杂：

// 笛卡尔到Frenet转换（简化版）
FrenetPoint cartesianToFrenet(double x, double y, 
                               const std::vector<PathPoint>& ref_path) {
    // 1. 找到最近点
    int nearest = findNearestPoint(x, y, ref_path);
    // 2. 计算s（沿参考线的累积距离）
    double s = ref_path[nearest].s;
    // 3. 计算d（横向偏移，带符号）
    double d = signedDistance(x, y, ref_path[nearest]);
    return {s, 0, 0, d, 0, 0};
}

⚠️ 注意：Frenet坐标系在参考线曲率突变处会有奇异点。我记得有一次在S形弯道，曲率过零点附近控制量突然发散，排查了两天才发现是Frenet转换时的数值问题。

四种方法怎么选？

我个人的经验法则：

离散点：适合传感器原始数据、快速原型验证
多项式：适合短距离、简单场景（换道、泊车）
样条曲线：适合长距离、复杂道路（高速、城市道路）
Frenet坐标系：适合需要精确横向控制的场景（车道保持、避障）

实际项目中，往往是组合使用。比如用样条曲线生成参考线，然后在Frenet坐标系下做控制。这样既保证了路径平滑，又简化了控制问题。

最后说一句：别纠结哪种方法最好。我见过用离散点+插值跑得飞快的项目，也见过用NURBS样条但控制抖成筛子的。关键是你对方法的理解深度，以及和实际场景的匹配度。

可可精品资料库

📚 课程目录

参考路径表示方法：离散点、多项式、样条与Frenet坐标系

离散点路径：最朴素的表达

多项式拟合路径：平滑但脆弱

样条曲线路径：工程界的首选

Frenet坐标系下的路径表示

四种方法怎么选？