📚 课程目录第3章参考线生成故障

3. 参考线生成故障

各位同学，咱们今天聊一个非常“磨人”的问题——参考线生成故障。

说实话，在我刚入行那会儿，觉得参考线嘛，不就是把全局路径拿过来，稍微平滑一下就行了吗？结果第一次上路测试，车在弯道里直接“画龙”，吓得我一身冷汗。后来我才明白，参考线质量直接决定了规划控制的上限。你想想看，如果参考线本身就有问题，后面的轨迹优化、控制跟踪，那都是“在烂泥地里盖高楼”。

这一节，我们就来拆解参考线生成的三大典型故障：平滑度不足、曲率突变、与全局路径偏离。我会结合我踩过的坑，告诉你如何诊断，又如何修复。

一、平滑度不足：车为什么“抖”起来了？

平滑度不足，说白了就是参考线不够“顺”。车在上面跑，就像走在搓板路上，方向盘来回修正，乘客体验极差。

1.1 现象诊断

怎么判断平滑度不足？我一般看两个指标：

横向加速度抖动：如果车辆在直道上频繁微调方向，大概率是参考线有高频噪声。
曲率一阶导数不连续：曲率变化率（即曲率对弧长的导数）如果出现跳变，说明平滑度不够。

诊断工具：我习惯把参考线的曲率曲线画出来。如果曲率像锯齿一样上下跳动，那基本可以判定平滑度不足。

1.2 常见原因

原始路径点太稀疏：全局路径可能每隔5米才一个点，直接拿来用，插值后必然不平滑。
平滑算法参数不当：比如二次规划中的权重没调好，或者样条曲线的阶数太低。
传感器噪声传导：如果参考线是基于实时感知生成的，那感知的抖动会直接传导到参考线上。

1.3 修复方案

我个人最常用的方法是分段三次样条插值 + 代价函数优化。这里给一个简单的示例：

import numpy as np
from scipy.interpolate import CubicSpline

# 原始路径点（x, y）
x_raw = np.array([0, 5, 10, 15, 20])
y_raw = np.array([0, 0.1, 0.2, -0.1, 0])

# 使用三次样条插值，增加采样密度
cs = CubicSpline(x_raw, y_raw, bc_type='natural')
x_dense = np.linspace(0, 20, 100)
y_dense = cs(x_dense)

# 再对y_dense做一次低通滤波，去除残余高频噪声
from scipy.signal import savgol_filter
y_smooth = savgol_filter(y_dense, window_length=11, polyorder=3)

我的经验：savgol_filter的窗口长度和多项式阶数需要根据实际路径长度调整。我一般先画图看效果，再微调参数。别一上来就套默认值，容易出问题。

二、曲率突变：弯道里的“惊魂一刻”

曲率突变比平滑度不足更危险。它意味着参考线在某一点突然变“弯”，车辆需要瞬间打很大的方向盘。我记得有一次在高速匝道上，曲率从0.01突然跳到0.08，车辆差点冲出车道。

2.1 现象诊断

曲率绝对值超过车辆最小转弯半径对应的曲率：比如车辆最小转弯半径是5米，那曲率不能超过0.2。如果参考线上某点曲率到了0.3，那肯定有问题。
曲率变化率过大：即使曲率绝对值在范围内，但如果变化太快（比如从0.01到0.05只用了0.5米），车辆转向系统也跟不上。

2.2 常见原因

全局路径本身有尖角：比如在高精地图上，路口转弯处可能是一个直角，没有做圆角处理。
平滑算法过度约束：有些算法为了追求路径贴合原始路径，牺牲了曲率连续性。
坐标转换误差：从经纬度转到笛卡尔坐标系时，如果投影方式不对，也会引入曲率突变。

2.3 修复方案

我建议采用曲率约束的二次规划平滑。核心思想是：在优化参考线位置的同时，显式约束曲率及其变化率。

# 伪代码示例：曲率约束优化
# 目标：最小化位置偏差 + 平滑项
# 约束：曲率 <= max_curvature, 曲率变化率 <= max_curvature_rate

def optimize_reference_line(points, max_curv, max_curv_rate):
    # 构建优化问题
    # 变量：每个点的x, y坐标
    # 目标函数：sum(||p_i - p_i_raw||^2) + w1 * sum(||p_i - 2*p_{i-1} + p_{i-2}||^2)
    # 约束：曲率(p_i) <= max_curv
    #       曲率变化率(p_i) <= max_curv_rate
    # 使用OSQP或类似求解器
    pass

注意：曲率约束是非线性的，直接求解比较困难。我通常的做法是：先用线性化近似，然后迭代求解。或者用分段线性约束来逼近。别想着一步到位，工程上都是“先跑通，再优化”。

三、与全局路径偏离：参考线“跑偏”了

这个问题最让人头疼。参考线如果偏离了全局路径，车辆就会“自作主张”走一条新路。轻则偏离导航路线，重则撞上障碍物。

3.1 现象诊断

横向偏差过大：参考线上的点与全局路径对应点的距离超过阈值（比如0.5米）。
路径形状不一致：比如全局路径是左转，参考线却变成了直行。

3.2 常见原因

平滑权重设置失衡：为了追求平滑，把位置偏差项的权重设得太低，导致参考线“飘”走了。
障碍物避让导致偏离：在动态避障中，参考线被障碍物“挤”到了旁边，但避障结束后没有回到全局路径上。
地图更新不及时：全局路径变了，但参考线还是基于旧地图生成的。

3.3 修复方案

我的做法是引入“软约束”+“硬边界”：

软约束：在优化目标中，加大位置偏差项的权重。我一般设权重为100以上，确保参考线不会跑太远。
硬边界：设置一个最大偏离距离（比如0.3米），如果优化结果超出这个范围，直接截断或重新优化。

# 修复示例：增加位置偏差权重
# 原始目标函数
cost = w_smooth * smoothness + w_dev * deviation

# 修复后：提高w_dev
w_dev = 150.0  # 原来是50.0
cost = w_smooth * smoothness + w_dev * deviation

# 同时添加硬约束
for i in range(len(points)):
    if distance(points[i], raw_points[i]) > 0.3:
        points[i] = raw_points[i] + 0.3 * normalize(points[i] - raw_points[i])

避坑指南：我曾经遇到过一个问题——权重调得太大，参考线虽然贴合全局路径，但平滑度变得很差。后来我用了“自适应权重”：在直道上降低位置权重，在弯道上提高位置权重。效果好了很多。

四、知识体系总览

下面这张图是我自己总结的参考线生成故障诊断与修复的完整流程。你可以把它当作一个“检查清单”，遇到问题时按图索骥。

五、总结与实战建议

好了，关于参考线生成的三大故障，我们就聊到这里。最后给你几个实战建议：

先诊断，后修复：别一上来就调参数。先把曲率曲线、横向偏差画出来，找到问题根源。
参数调优要“小步快跑”：每次只改一个参数，观察效果。我见过有人一次改三个参数，结果问题更复杂了。
建立你的“故障库”：每次遇到问题，把现象、原因、修复方案记录下来。时间长了，你就能一眼看出问题所在。

核心一句话：参考线是规划控制的“地基”。地基不稳，上层建筑再漂亮也没用。花时间把参考线做好，后面的工作会轻松很多。

希望今天的内容对你有帮助。如果你在实际项目中遇到其他奇怪的问题，欢迎随时交流。