自动驾驶路径规划：CRV项目MPCLocalPlanner架构与算法详解

1. CRV项目路径规划模块架构解析

在自动驾驶系统中，路径规划模块承担着将全局导航指令转化为可执行轨迹的关键任务。CRV项目中的MPCLocalPlanner类采用分层处理架构，其核心设计理念可概括为"全局粗导引，局部精修整"。该模块通过四个处理阶段实现这一目标：

1. 输入预处理阶段：对车辆状态和全局路径进行坐标转换和有效性校验
2. 路径平滑阶段：使用B样条算法生成几何连续的可行驶路径
3. 速度规划阶段：结合运动学约束和曲率限制生成速度剖面
4. 安全边界生成阶段：计算轨迹两侧的动态可行驶区域

实际工程中发现，输入路径点的质量直接影响规划效果。建议在调用set_global_plan()前，对原始路径进行至少10米的Douglas-Peucker算法压缩，保留关键特征点即可。

2. 核心算法实现细节

2.1 B样条路径平滑技术

模块采用三次均匀B样条（k=3）进行路径插值，其数学表达式为：

code复制C(u) = Σ[Ni,p(u)·Pi] (i=0 to n)

其中控制点Pi通过scipy.interpolate.splprep自动计算得出。关键参数配置如下表所示：

在实测中发现，当原始路径点间距大于3米时，直接插值会导致曲率突变。解决方法是在调用splprep前，先对输入路径进行线性插值加密，确保点间距在0.5-1米范围内。

2.2 曲率限速模型

基于车辆动力学原理，弯道限速公式为：

code复制v_max = √(a_lat_max / |κ|)

其中κ通过中心差分法计算：

python复制def calculate_curvature(x, y):
    dx = np.gradient(x)
    dy = np.gradient(y)
    ddx = np.gradient(dx)
    ddy = np.gradient(dy)
    return (dx*ddy - dy*ddx) / (dx**2 + dy**2)**1.5

实际应用中需要考虑以下修正因素：

1. 增加10%的安全余量（乘以0.9系数）
2. 对曲率进行3点移动平均滤波，避免高频抖动
3. 设置0.1m^-1的曲率阈值，小于该值视为直线路段

3. 运动学速度规划实现

3.1 双向平滑算法

_apply_physical_smoothing函数采用前向-后向双通道处理：

1. 前向通道（保证加速度限制）：

   python复制for i in range(1, len(speed)):
       dist = math.hypot(x[i]-x[i-1], y[i]-y[i-1])
       speed[i] = min(speed[i], 
                     math.sqrt(speed[i-1]**2 + 2*a_max*dist))
   

2. 后向通道（保证减速度限制）：

   python复制for i in range(len(speed)-2, -1, -1):
       dist = math.hypot(x[i+1]-x[i], y[i+1]-y[i])
       speed[i] = min(speed[i],
                     math.sqrt(speed[i+1]**2 + 2*d_max*dist))
   

3.2 时间维度平滑

采用指数移动平均（EMA）滤波器处理速度跳变：

python复制alpha = 0.3  # 平滑因子
smoothed_speed = [speed[0]]
for v in speed[1:]:
    smoothed_speed.append(alpha*v + (1-alpha)*smoothed_speed[-1])

4. 特殊场景处理策略

4.1 终点接近策略

当检测到_is_near_end()时，切换为直线逼近模式：

1. 计算终点方向向量：

   python复制dx = end_x - current_x
   dy = end_y - current_y
   dist = math.hypot(dx, dy)
   yaw = math.atan2(dy, dx)
   

2. 生成线性递减速度剖面：

   python复制for i in range(num_points):
       ratio = i / num_points
       speed.append(max_speed * (1 - ratio**2))
   

4.2 换道行为优化

换道时（RoadOption 5/6）采取以下措施：

1. 速度降低20%（乘以0.8系数）
2. 增大安全走廊宽度（默认值的1.5倍）
3. 延长前视距离（从20米增至30米）

5. 工程实践中的关键问题

5.1 实时性优化技巧

1. 路径点采样优化：

   • 只处理车辆前方50米内的路径段
   • 使用numpy向量化运算替代循环

2. 缓存机制：

   • 对未更新的全局路径复用上次插值结果
   • 曲率计算结果缓存3帧

3. 算法降级：

   • 当计算超时（>50ms）时：
     • 使用上一次有效轨迹
     • 线性外推未来5个点

5.2 典型故障排查

6. 参数调试方法论

建议按以下顺序进行参数整定：

1. 几何参数：

   • spline平滑因子（s）
   • 路径分辨率（建议0.3m）

2. 动力学参数：

   • 最大横向加速度（0.3-0.5g）
   • 最大纵向加速度（0.2-0.3g）

3. 行为参数：

   • 标准前视距离（20-30m）
   • 终点减速距离（10-15m）

调试时推荐使用"速度-曲率"关联图进行可视化验证，确保各弯道点的速度值均位于理论限速曲线下方。