别再只会调PID了！聊聊MPC和LQR在自动驾驶小车里的实战选择
别再只会调PID了！聊聊MPC和LQR在自动驾驶小车里的实战选择
当你第一次尝试让自制的小车沿着白线自动行驶时，大概率会从PID控制器开始。转动电位器调整Kp、Ki、Kd参数的过程，就像在玩一个神秘的电子游戏——参数太小，小车反应迟钝；参数太大，它又会像醉汉一样左右摇摆。但当你开始挑战更复杂的场景，比如S形弯道或动态避障时，单纯调整这三个参数就会遇到天花板。这时候，是时候了解MPC（模型预测控制）和LQR（线性二次调节器）这两个工业级控制算法了。
1. 从PID到现代控制：为什么需要升级？
在树莓派上跑PID控制小车直线行驶，就像用自行车代步——简单够用。但当你需要处理复杂路况时，就相当于要用自行车完成山地越野，这时候就需要更专业的"装备"。
PID的三大局限在复杂场景中尤为明显：
- 预见性缺失：就像闭着眼睛走路，只能靠碰到墙壁后的痛感来调整方向
- 耦合处理困难：转向和速度控制各自为政，实际转弯时却需要协同工作
- 约束无视：电机最大转速、转向角度等物理限制无法内置到控制逻辑中
我在早期项目中就遇到过这样的困境：小车在90度直角弯前总是冲出赛道。增加D参数可以缓解，但遇到路面不平整时又会剧烈抖动。这就是典型的PID调节死胡同——改善一个指标会恶化另一个。
2. 控制器三剑客核心特性对比
让我们用具体数据说话。下表对比了三种控制器在STM32F4（168MHz）和Jetson Nano上的表现：
| 特性 | PID | LQR | MPC |
|---|---|---|---|
| 计算复杂度 | O(1) | O(n³) | O(n³)每步 |
| 内存占用(KB) | <1 | ~10 | ~50 |
| 响应延迟(ms) | 0.1 | 1-2 | 5-20 |
| 支持约束 | 否 | 需后处理 | 原生支持 |
| 多变量协同 | 需独立调节 | 自动优化 | 自动优化 |
| 适合场景 | 直线巡航 | 中高速弯道 | 复杂动态环境 |
实测提示：在STM32上运行MPC时，预测时域超过3步就会导致控制周期>50ms，这对高速行驶的小车来说已经会产生明显延迟。
具体到代码实现复杂度：
# PID实现示例（Python伪代码） def pid_update(error): integral += error * dt derivative = (error - last_error) / dt output = Kp*error + Ki*integral + Kd*derivative last_error = error return output # LQR需要先解Riccati方程 import numpy as np def lqr(A,B,Q,R): P = np.matrix(scipy.linalg.solve_continuous_are(A, B, Q, R)) K = np.matrix(scipy.linalg.inv(R)*(B.T*P)) return K3. 硬件适配：从STM32到Jetson的算法选择
3.1 资源受限平台（STM32系列）
在Cortex-M4内核的STM32上，LQR是性价比最高的选择。以我的智能车大赛项目为例：
预处理阶段：
- 离线计算反馈矩阵K（在PC上完成）
- 将K值硬编码到单片机
const float K[4] = {0.82, -1.15, 0.43, 0.27}; // 状态反馈矩阵实时控制：
- 仅需做矩阵乘法运算
void control_update() { float u = -K[0]*x1 - K[1]*x2 - K[2]*x3 - K[3]*x4; set_motor(u); }
3.2 高性能平台（Jetson Nano/Raspberry Pi 4）
当使用带Linux系统的主控时，MPC开始变得可行。ROS+ACADO工具链是个不错的组合：
# 安装ACADO git clone https://github.com/acado/acado.git mkdir build && cd build cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local make install典型MPC工作流程：
- 建立车辆动力学模型
- 定义优化目标和约束
- 实时求解优化问题
// 简化的MPC设置代码 Controller controller; DifferentialEquation model; OCP ocp(0.0, 1.0, 20); // 1秒预测时域，分20步 model << dot(x) == v*cos(theta); model << dot(y) == v*sin(theta); model << dot(theta) == (v/L)*tan(phi); ocp.minimizeLSQ(Q, h); // 二次型代价函数 ocp.subjectTo( -0.5 <= phi <= 0.5 ); // 转向角约束4. 场景化选型指南
4.1 校园物流小车（低速复杂环境）
需求特征：
- 平均速度<1m/s
- 频繁启停
- 需要避让行人
方案选择：
- 使用MPC处理动态障碍物
- 预测时域设置为2秒
- 重点优化计算效率：
# 使用OSQP求解器 solver = osqp.OSQP() solver.setup(P, q, A, l, u, verbose=False)
4.2 智能车竞速（中高速循迹）
实测数据：
| 控制器 | 平均圈速 | 最大横向误差 |
|---|---|---|
| PID | 23.5s | 15cm |
| LQR | 21.8s | 8cm |
| MPC | 22.1s | 6cm |
优化技巧：
- 预计算不同速度下的LQR参数
- 使用串级控制：
- 外层LQR处理路径跟踪
- 内层PID控制转向舵机
4.3 室内服务机器人
特殊挑战：
- 需要精确停靠（误差<2cm）
- 地面可能存在滑动
混合方案：
- 全局使用MPC规划
- 末端切换为PID进行微调
- 加入滑动补偿：
% 滑动检测算法 if abs(encoders_velocity - odom_velocity) > threshold adjust_friction_coefficient(); end
5. 避坑实践：那些教程里不会告诉你的细节
模型失配处理：
- 在车辆模型中加入滑动参数估计
def adaptive_model(): while True: actual = get_actual_trajectory() predicted = model.predict() adjust_model(actual - predicted)实时性保障技巧：
- 固定迭代次数的QP求解
- 热启动优化（复用上一步的解）
- 降采样视觉输入
调试可视化工具：
# ROS工具链 rqt_plot /control/error rviz -d car_model.rviz硬件级优化：
- 在STM32上使用ARM的DSP库加速矩阵运算
- 为Jetson编写CUDA核函数处理QP求解
第一次尝试MPC时，我完全低估了模型精度的重要性。用理想自行车模型控制的小车，在实际测试中遇到地板接缝就会偏离轨迹。后来加入轮胎滑动补偿后，控制精度立即提升了60%。这印证了控制领域的那句老话：垃圾模型进，垃圾控制出。