1. 项目概述
在自动驾驶、机器人导航和工业控制等领域，轨迹跟踪控制一直是个经典而关键的课题。最近我在一个AGV（自动导引车）项目中遇到了轨迹跟踪精度不足的问题，尝试了PID控制效果不理想后，转而研究基于LQR（线性二次调节器）的最优控制算法。这个算法在四自由度动力学模型上的实现效果出乎意料地好，特别是配合实时K值计算模块后，系统响应速度和跟踪精度都有了显著提升。
LQR算法本质上是通过求解Riccati方程得到最优状态反馈矩阵K，使系统在状态误差和控制量之间达到最佳平衡。相比传统PID控制，LQR能够更好地处理多变量耦合系统，而且理论上有明确的稳定性证明。不过实际工程中，直接应用教科书上的LQR会遇到不少挑战——比如模型线性化带来的误差、实时计算资源限制等，这些都需要在实现时特别注意。
2. 核心原理与模型构建
2.1 四自由度动力学模型
我们采用的动力学模型包含x/y平面位移、偏航角以及前轮转向角四个自由度。模型推导过程如下：
-
建立车辆坐标系与全局坐标系的转换关系：
math复制\dot{X} = v \cos(\psi + \beta) \\ \dot{Y} = v \sin(\psi + \beta) \\ \beta = \arctan(\frac{l_r}{l_f + l_r} \tan(\delta_f))其中v为车速，ψ为偏航角，δf为前轮转角，lf/lr为前后轴到质心的距离。
-
考虑横向动力学方程：
math复制m(\dot{v}_y + v_x \dot{\psi}) = F_{yf} + F_{yr} \\ I_z \ddot{\psi} = l_f F_{yf} - l_r F_{yr}这里m为质量，Iz为转动惯量，Fyf/yr为前后轮侧向力。
-
使用小角度假设线性化轮胎模型：
math复制F_{yf} = -C_f \alpha_f \approx -C_f (\delta_f - \frac{v_y + l_f \dot{\psi}}{v_x}) \\ F_{yr} = -C_r \alpha_r \approx -C_r \frac{l_r \dot{\psi} - v_y}{v_x}
最终得到的状态空间方程为：
math复制\dot{x} = A x + B u \\
y = C x
其中状态向量x=[vy, ψ, ψ̇, Y]T，控制输入u=δf。
2.2 LQR控制原理
LQR算法的核心是最小化如下代价函数：
math复制J = \int_0^\infty (x^T Q x + u^T R u) dt
通过求解代数Riccati方程：
math复制A^T P + P A - P B R^{-1} B^T P + Q = 0
得到最优反馈矩阵K=R⁻¹BᵀP，控制律为u=-Kx。
这里Q和R的选择至关重要：
- Q矩阵对角元素对应各状态量的权重
- R矩阵元素反映控制量的代价
通常需要经过多次调试才能找到合适的权重组合。
3. 系统实现细节
3.1 实时K值计算模块
传统LQR实现通常离线计算K矩阵，但在车速变化较大时，固定K值会导致控制性能下降。我们开发了基于Eigen库的实时求解器：
cpp复制#include <Eigen/Dense>
using namespace Eigen;
MatrixXd computeLQRK(const MatrixXd& A,
const MatrixXd& B,
const MatrixXd& Q,
const MatrixXd& R) {
// 解Riccati方程
MatrixXd P = Q;
const int max_iter = 100;
double tolerance = 1e-6;
for(int i=0; i<max_iter; ++i){
MatrixXd P_next = A.transpose()*P + P*A - P*B*R.inverse()*B.transpose()*P + Q;
if((P_next - P).cwiseAbs().maxCoeff() < tolerance){
P = P_next;
break;
}
P = P_next;
}
return R.inverse()*B.transpose()*P;
}
实际应用中需要注意：
- 矩阵条件数检查，避免数值不稳定
- 设置最大迭代次数防止死循环
- 采用增量更新策略（车速变化超过5%才重新计算）
3.2 控制架构设计
系统采用分层控制结构：
code复制[轨迹规划层]
↓ 参考轨迹
[LQR控制器] ← [状态估计器]
↓ 转向指令
[执行器驱动层]
关键数据流：
- 定位模块提供100Hz的状态更新
- 控制器运行频率50Hz
- 执行器响应延迟<10ms
4. 实际调试经验
4.1 权重参数整定
经过大量测试，总结出Q/R矩阵的调试经验：
- 初始设置：
math复制Q = diag([1, 10, 1, 5]) \\
R = [0.1]
- 调整原则：
- 增大Q对角元素 → 对应状态量收敛更快
- 增大R元素 → 控制量更平滑但响应变慢
- 推荐步骤：
- 先调横向误差权重（Q[3]）
- 再调偏航角相关权重（Q[1],Q[2]）
- 最后微调R值平衡平滑性
4.2 常见问题排查
- 系统发散：
- 检查模型参数准确性（特别是轮胎刚度Cf/Cr）
- 验证状态估计数据的可靠性
- 降低控制增益逐步调试
- 高频振荡：
- 增加R矩阵元素
- 检查执行器延迟
- 添加低通滤波器（截止频率建议15-20Hz）
- 稳态误差：
- 确认是否包含积分环节
- 检查速度估计偏差
- 考虑添加前馈补偿
5. 性能优化技巧
- 矩阵运算加速：
- 使用Eigen的Map功能避免内存拷贝
- 利用SSE/AVX指令集
- 固定尺寸矩阵优化（Matrix4d代替MatrixXd）
- 代码级优化：
cpp复制// 不好的实现：临时矩阵多
MatrixXd temp = A.transpose()*P;
MatrixXd result = temp + P*A - ...;
// 优化实现：利用noalias和表达式模板
MatrixXd result;
result.noalias() = A.transpose()*P;
result.noalias() += P*A - ...;
- 实时性保障：
- 采用RT-Preempt内核
- 控制线程绑定CPU核心
- 内存预分配避免动态申请
6. 实测效果对比
在低速AGV（v<2m/s）场景下的测试数据：
| 指标 | PID控制 | LQR固定K | LQR实时K |
|---|---|---|---|
| 最大横向误差 | 0.15m | 0.08m | 0.05m |
| 稳态误差 | 0.03m | 0.02m | 0.01m |
| 转向抖动幅度 | ±3° | ±1.5° | ±0.8° |
| CPU占用率 | 5% | 12% | 18% |
可以看到实时K值方案在精度上有明显优势，但计算开销也相应增加。在实际项目中，我们最终采用了混合策略：低速时用实时K值，高速时切换为固定K值，这样既保证了控制精度，又避免了过大的计算负荷。