📚 课程目录第2章路径平滑处理

2. 路径平滑处理

任务目标

全局路径规划器输出的路径通常由离散路点组成，在拐弯处存在尖锐转角，不符合车辆运动学约束。本案例实现一个轻量级路径平滑器：读取包含尖锐拐点的路径点序列，通过三次B样条曲线拟合生成平滑轨迹，并输出平滑后的路径点供下游控制模块使用。

实现方案

采用均匀三次B样条逼近原始路径点。B样条具有局部支撑性，计算简单且不改变路径整体走向。核心步骤：

将原始路径点作为控制点，构造三次均匀B样条；
以固定弧长间隔采样B样条曲线，得到密集平滑点；
对采样点做等间距重采样，保证输出点间距均匀，便于下游模块使用。

核心代码

// path_smoother.cpp
// 编译: g++ -std=c++17 path_smoother.cpp -o path_smoother
// 运行: ./path_smoother input_path.txt output_path.txt

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <string>

struct Point {
    double x, y;
};

// 三次均匀B样条基函数 (参数t∈[0,1])
Point bspline(const Point& p0, const Point& p1,
              const Point& p2, const Point& p3, double t) {
    double t2 = t * t, t3 = t2 * t;
    double b0 = (1 - t) * (1 - t) * (1 - t) / 6.0;
    double b1 = (3 * t3 - 6 * t2 + 4) / 6.0;
    double b2 = (-3 * t3 + 3 * t2 + 3 * t + 1) / 6.0;
    double b3 = t3 / 6.0;
    return {b0 * p0.x + b1 * p1.x + b2 * p2.x + b3 * p3.x,
            b0 * p0.y + b1 * p1.y + b2 * p2.y + b3 * p3.y};
}

// 计算两点距离
double dist(const Point& a, const Point& b) {
    return std::hypot(a.x - b.x, a.y - b.y);
}

// 路径平滑主函数
std::vector<Point> smoothPath(const std::vector<Point>& raw,
                               double interval = 1.0) {
    if (raw.size() < 4) return raw;  // 点数不足时直接返回

    // 1. 以原始点为控制点，密集采样B样条
    const int SAMPLES_PER_SEG = 20;
    std::vector<Point> dense;
    for (size_t i = 0; i + 3 < raw.size(); ++i) {
        for (int j = 0; j < SAMPLES_PER_SEG; ++j) {
            double t = static_cast<double>(j) / SAMPLES_PER_SEG;
            dense.push_back(bspline(raw[i], raw[i+1], raw[i+2], raw[i+3], t));
        }
    }
    // 补上最后一个控制点
    dense.push_back(raw.back());

    // 2. 等间距重采样
    std::vector<Point> result;
    result.push_back(dense.front());
    double accumulated = 0.0;
    for (size_t i = 1; i < dense.size(); ++i) {
        double d = dist(dense[i-1], dense[i]);
        accumulated += d;
        if (accumulated >= interval) {
            // 线性插值到精确间隔位置
            double excess = accumulated - interval;
            double ratio = 1.0 - excess / d;
            result.push_back({dense[i-1].x + ratio * (dense[i].x - dense[i-1].x),
                              dense[i-1].y + ratio * (dense[i].y - dense[i-1].y)});
            accumulated = excess;
        }
    }
    // 确保终点被包含
    if (dist(result.back(), dense.back()) > 0.1) {
        result.push_back(dense.back());
    }
    return result;
}

int main(int argc, char** argv) {
    if (argc != 3) {
        std::cerr << "用法: " << argv[0] << " input.txt output.txt\n";
        return 1;
    }

    // 读取原始路径
    std::ifstream fin(argv[1]);
    if (!fin) { std::cerr << "无法打开输入文件\n"; return 1; }
    std::vector<Point> raw;
    double x, y;
    while (fin >> x >> y) raw.push_back({x, y});
    fin.close();

    // 执行平滑
    auto smoothed = smoothPath(raw, 1.0);

    // 写入结果
    std::ofstream fout(argv[2]);
    for (const auto& p : smoothed) {
        fout << p.x << " " << p.y << "\n";
    }
    fout.close();

    std::cout << "平滑完成: " << raw.size() << " 点 → "
              << smoothed.size() << " 点\n";
    return 0;
}

运行与验证

输入文件格式：每行两个空格分隔的浮点数，表示路点x、y坐标。示例输入（含90°尖锐拐角）：

运行命令：

g++ -std=c++17 path_smoother.cpp -o path_smoother
./path_smoother input.txt output.txt

验证方法：

程序输出“平滑完成: N 点 → M 点”，M应大于N（采样加密）；
用Python或绘图工具绘制原始点与平滑点，观察拐角处是否变为平滑圆弧；
检查输出点间距约为1.0（等间距重采样生效）；
计算相邻三点夹角，平滑后最大转角应显著小于原始90°拐角。

判定标准：若拐角处路径由折线变为连续圆弧、输出点间距均匀（约1.0m）、且程序无报错退出，则平滑处理通过验证。本算法适用于全局路径的后处理，不改变路径起点与终点，适合作为规划与控制之间的中间层。