路上突然蹿出只野猫怎么办？自动驾驶系统得在0.1秒内做出精准转向决策。今天咱们用C++手撸个LQR控制器，看看怎么让车辆像老司机一样丝滑过弯-优快云博客

自动驾驶控制-基于动力学跟踪误差模型LQR算法C++纯代码实现，百度apollo横向控制所用模型。代码注释完整，可以自己看明白，也可以付费提供代码和算法原理讲解服务。通过C++程序实现的路径跟踪仿真，可视化绘图需要安装matplotlibcpp库，已经提前安装好包含在头文件，同时需要安装Eigen库，文件内也含有安装教程。可以自定义路径进行跟踪，只需有路径的X Y坐标即可，替换下图中框框标出来的地方路径就可以了。图片是双移线和一些自定义的路线仿真效果。

先上硬菜——动力学模型。这里用Apollo同款的误差跟踪模型，状态向量选了五个关键参数：

// 状态向量定义 [横向误差, 误差变化率, 航向误差, 航向变化率, 前轮转角]
Eigen::VectorXd x(5);
x << lateral_error, lateral_error_rate, heading_error, heading_error_rate, steer_angle;

构建系统矩阵时要注意轮胎的魔术公式特性，这里简化后的A、B矩阵初始化：

// 系统矩阵初始化（部分核心参数）
A_(0, 1) = 1.0;  // 横向误差微分
A_(1, 1) = -2*(Cf+Cr)/(mass*velocity);  // 横向速度阻尼项
A_(1, 3) = 2*(Cf*lf - Cr*lr)/(mass*velocity) - velocity;  // 横摆力矩耦合项

B_(1, 0) = 2*Cf/(mass*velocity);  // 前轮转角对横向运动影响
B_(3, 0) = 2*Cf*lf/(iz*velocity); // 前轮转角对横摆运动影响

LQR求解器才是重头戏，这里用迭代法解Riccati方程：

void solveLQR() {
    Eigen::MatrixXd P = Q_;
    for (int i = 0; i < max_iteration; ++i) {
        Eigen::MatrixXd P_next = Q_ + A_.transpose()*P*A_ - 
                               A_.transpose()*P*B_*(R_ + B_.transpose()*P*B_).inverse()*B_.transpose()*P*A_;
        if ((P_next - P).cwiseAbs().maxCoeff() < tolerance) break;
        P = P_next;
    }
    K_ = (R_ + B_.transpose()*P*B_).inverse() * B_.transpose()*P*A_;
}

这里有个坑要注意：迭代次数不够会导致控制量震荡，建议设500次迭代保平安。

路径跟踪主循环长这样：

while (!path.empty()) {
    // 获取当前误差（此处简化处理）
    double target_yaw = atan2(next_wp.y() - current_wp.y(), next_wp.x() - current_wp.x());
    double heading_error = target_yaw - current_yaw;
    
    // LQR控制器输出
    Eigen::VectorXd control = controller.computeControl(state);
    
    // 更新车辆状态（用二自由度模型简化）
    updateVehicleState(control(0), dt);
    
    // 记录轨迹用于绘图
    x_hist.push_back(vehicle_x);
    y_hist.push_back(vehicle_y);
}

换路径比换袜子还简单，找到代码里这个位置：

// ############ 在这里替换你的路径 ##############
std::vector<Eigen::Vector2d> ref_path = {
    {0.0, 0.0}, {10.0, 3.0}, {20.0, 0.0},  // 双移线路径
    {30.0, -2.0}, {40.0, 0.0}
};
// #############################################

实测效果：60km/h下跑双移线，横向误差能控制在0.2米内。有个骚操作——把Q矩阵的横向误差权重调大，车辆会像磁铁一样吸着参考线走。

最后说下怎么画图：

// 画参考路径
plt::plot(x_ref, y_ref, "r--");

// 画实际轨迹
plt::plot(x_hist, y_hist, "b-");

// 装饰图表
plt::title("LQR路径跟踪效果");
plt::xlabel("X (m)");
plt::ylabel("Y (m)");
plt::axis("equal");
plt::show();

完整代码里还藏着速度自适应、抗积分饱和等彩蛋功能。需要调参指导或算法原理详解的兄弟，私信扔个红包过来咱们慢慢唠。下期可能整活MPC横向控制，看大伙儿点赞够不够热烈了。