LQR控制在自动驾驶避障中的实践与优化

1. 自动驾驶避障场景的技术挑战

当车辆以60km/h行驶时，突然出现的障碍物留给系统的反应时间往往不足0.5秒。这个看似简单的避障动作，背后涉及感知、决策、控制三大技术模块的精密配合。传统PID控制器在低速场景表现尚可，但面对高速动态避障时容易出现超调震荡，就像新手司机猛打方向导致的"画龙"现象。

LQR（线性二次调节器）作为现代控制理论的经典算法，通过状态空间建模和代价函数优化，能够实现多变量系统的最优控制。在车辆动力学模型中，横向位移、偏航角、速度等状态变量相互耦合，LQR恰好擅长处理这类多输入多输出系统的控制问题。

2. 车辆动力学建模要点

2.1 自行车模型简化

采用经典的二自由度自行车模型进行建模：

code复制状态方程：dx/dt = Ax + Bu
其中：
x = [横向误差, 横向误差率, 航向误差, 航向误差率]^T
u = 前轮转角
A矩阵包含车辆质量、轴距、轮胎侧偏刚度等参数
B矩阵反映转向输入对状态的影响

注意：轮胎侧偏刚度会随路面条件变化，干燥沥青路面典型值为80-100kN/rad，湿滑路面可能下降30%

2.2 离散化处理

控制器需在10ms周期内完成计算，需将连续模型离散化：

cpp复制MatrixXd discretize(const MatrixXd& A, const MatrixXd& B, double dt) {
    MatrixXd Ad = MatrixXd::Identity(A.rows(), A.cols()) + A * dt;
    MatrixXd Bd = B * dt;
    return {Ad, Bd};
}

3. LQR控制器实现细节

3.1 代价函数设计

Q矩阵对角元素对应状态变量权重：

cpp复制Q.diagonal() << 1.0, 0.1, 0.5, 0.05;  // 横向误差权重最高
R << 0.01;  // 控制量权重

3.2 代数Riccati方程求解

采用迭代法求解反馈矩阵K：

cpp复制MatrixXd solveRiccati(const MatrixXd& A, const MatrixXd& B, 
                     const MatrixXd& Q, const MatrixXd& R) {
    MatrixXd P = Q;
    for (int i = 0; i < max_iter; ++i) {
        MatrixXd K = (B.transpose() * P * B + R).inverse() * B.transpose() * P * A;
        MatrixXd P_new = Q + A.transpose() * P * A - A.transpose() * P * B * K;
        if ((P_new - P).norm() < tolerance) break;
        P = P_new;
    }
    return P;
}

4. 实际调参经验分享

4.1 权重调整策略

• 高速场景（>80km/h）：增大航向误差权重防止甩尾
• 低附着路面：降低控制量权重避免轮胎饱和
• 紧急避障：临时提高横向误差权重系数3-5倍

4.2 实时性优化技巧

1. 预计算K矩阵：在车辆启动时预先计算典型速度对应的K矩阵
2. 查表法：建立速度-最优K矩阵的查找表
3. 降阶处理：当计算资源紧张时，可忽略横向误差率项

5. 典型问题排查指南

实测数据显示，优化后的LQR控制器在干燥路面可实现：

• 60km/h下避障反应时间：0.12s
• 横向加速度峰值：0.3g
• 轨迹偏离量：<0.5m

在冰雪路面需要将最大转向角速度限制在15°/s以内，同时Q矩阵中的误差权重应降低40%以适应低摩擦条件。这个参数调整过程就像老司机根据路况本能地调整方向盘力度，既需要理论支撑，也需要实际路测的反复验证。