自动驾驶控制算法实战：基于b站老王开源框架的轨迹跟踪优化方案

共计 1570 个字符，预计需要花费 4 分钟才能阅读完成。

背景痛点

传统 PID 控制在自动驾驶轨迹跟踪中存在明显局限性。我在 MATLAB/Simulink 中搭建了双移线测试场景，当车速超过 60km/ h 时：

• 急转弯场景：PID 的固定参数无法适应曲率变化，最大横向误差达 35cm
• 湿滑路面：积分项累积导致车辆出现持续振荡，需手动重置积分器
• 参数敏感性：同一组 PID 参数在干燥 / 湿滑路面表现差异达 300%

通过仿真数据发现，误差累积主要发生在路径曲率突变点（如下公式所示的转向角计算滞后）：

$$
\delta_{PID} = K_p e + K_i \int e\,dt + K_d \frac{de}{dt}
$$

技术选型

测试了 b 站老王框架中的三种核心算法：

最终选择 MPC 方案，因其具备：

• 显式处理系统约束的能力
• 前馈 + 反馈的复合控制结构
• 可扩展的状态空间模型

核心实现

动态权重调整器

// 使用 Eigen 进行矩阵运算优化
MatrixXd WeightAdapter::updateWeights(const VehicleState &state) {
    // 根据曲率变化率调整权重
    double curvature_rate = fabs(state.curvature - last_curvature_) / dt_;
    double q_ratio = 1.0 + curvature_rate * Q_SCALE_FACTOR; // Q_SCALE_FACTOR=2.5

    // 构造对角权重矩阵
    MatrixXd Q = MatrixXd::Zero(MPC_STATE_DIM, MPC_STATE_DIM);
    Q.diagonal() << q_ratio * BASE_Q_LAT, BASE_Q_HEADING, BASE_Q_STEER;
    return Q;
}

QP 求解器配置

关键参数设置原则：

1. 预测时域选择 8 -12 步（对应 3 - 4 秒）
2. 控制时域设为预测时域的 1 /3
3. 松弛因子 ε 初始值建议 0.1

OSQPSolver::configure() {
    settings_->alpha = 1.0;  // 过松弛参数
    settings_->eps_abs = 1e-4; // 绝对收敛容差
    settings_->max_iter = 1000; // 迭代次数
}

Apollo 接口适配

通过 ControlCommand 消息桥接：

message ControlCommand {optional double steering_target = 1;  // 归一化[-1,1]
    optional double acceleration = 2;     // m/s²
    optional int32 gear_position = 3;
}

避坑指南

多线程安全实现

• 使用 std::mutex 保护 QP 求解器实例
• 避免在回调中直接调用solve()
• 推荐采用任务队列模式

滤波参数调优

// 二阶低通滤波器参数
constexpr double CUTOFF_FREQ = 5.0; // Hz
constexpr double DAMPING_RATIO = 0.7;

CAN 延迟补偿

采用 Smith 预估器补偿 200ms 通信延迟：

$$
u(t) = K_p e(t) + K_i \int e(t+\tau)\,dt
$$

验证数据

CARLA 仿真结果

计算耗时（树莓派 4B）

• 矩阵构建：4.2ms
• QP 求解：9.8ms
• 总耗时：14.0ms

延伸思考

1. 如何融合视觉感知的预测信息到 MPC 的状态空间模型？
2. 在算力受限的嵌入式平台，有哪些 MPC 降阶方法？
3. 动态权重调整策略能否通过强化学习在线优化？

整个方案已开源在 GitHub（遵守 BSD 协议），欢迎社区开发者共同改进。在实际工程中，建议先进行 HIL 测试再实车部署，特别注意不同路面附着系数对控制效果的影响。