4. 路径规划器(Path Planner)调试：路径生成算法、代价函数调优

路径规划器，说白了就是自动驾驶的「大脑」——它决定了车怎么走。我调试这个模块好几年了，踩过的坑比走过的路还多。今天咱们就聊聊路径生成算法和代价函数调优的那些事。

4.1 路径生成算法的核心逻辑

路径规划不是画一条线那么简单。它要考虑车辆的运动学约束、道路边界、障碍物避让，还得保证路径平滑。我个人习惯把路径生成拆成三步：

1. 采样：在可行空间里生成候选路径
2. 评估：用代价函数给每条路径打分
3. 选择：挑出最优的那条

嗯，这里要注意——采样密度和计算时间是死对头。我在项目中遇到过，采样太密导致规划周期超时，车直接停在路中间。后来我学乖了，动态调整采样步长：直道用粗采样，弯道用细采样。

4.2 代价函数的设计与调优

代价函数是路径规划器的灵魂。你想想看，同样的采样点，不同的权重配比，出来的路径天差地别。我一般把代价函数分成这几类：

代价类型	含义	典型权重范围
横向偏移代价	偏离参考线的惩罚	10.0 - 50.0
曲率代价	路径弯曲程度的惩罚	5.0 - 20.0
障碍物距离代价	靠近障碍物的惩罚	100.0 - 500.0
速度代价	偏离目标速度的惩罚	1.0 - 10.0
加速度代价	加速度变化的惩罚	0.5 - 5.0

调优的时候，我建议先固定障碍物代价，再调其他项。为什么？因为安全永远是第一位的。我曾经把横向偏移代价设得太高，结果车为了贴线行驶，差点蹭到旁边的锥桶。那次之后，我就在代码里加了个保护逻辑：

// 伪代码示例：代价函数计算
double CalculateTotalCost(const Path& path) {
    double cost = 0.0;
    
    // 横向偏移代价
    cost += kLateralOffsetWeight * path.lateral_offset;
    
    // 曲率代价
    cost += kCurvatureWeight * path.curvature;
    
    // 障碍物距离代价 - 这个必须优先
    double obstacle_cost = CalculateObstacleCost(path);
    if (obstacle_cost > kMaxObstacleCost) {
        return std::numeric_limits<double>::max(); // 直接淘汰
    }
    cost += obstacle_cost;
    
    return cost;
}

4.3 路径平滑处理

原始采样路径往往很「毛躁」，直接给控制模块会出问题。我常用的平滑方法有两种：

• B样条插值：适合全局路径，平滑度高但计算量大
• 共轭梯度优化：适合局部路径，收敛快但容易陷入局部最优

我个人更偏爱B样条。虽然慢一点，但胜在稳定。记得有一次，我用共轭梯度优化，结果在U型弯里路径直接「打结」了——车头朝里，车尾朝外，控制模块直接报错。换成B样条后，再也没出过这种问题。

4.4 知识体系流程图

下面这张图是我自己总结的路径规划器调试流程，你照着这个思路走，基本不会跑偏：

4.5 实战调试案例

最后分享一个我印象最深的调试经历。有一次在园区测试，车总是莫名其妙地「画龙」——左右摇摆。我查了三天，最后发现是代价函数里的横向偏移权重设成了0。你想想看，没有横向约束，路径当然乱飘。

从那以后，我养成了一个习惯：每次改完参数，先在日志里打印出各项代价的数值。看看哪项贡献最大，哪项几乎没起作用。说白了，调参不是玄学，是科学——你得让数据说话。