GreedyAbyss1. 二自由度MPC轨迹跟踪控制概述
轨迹跟踪控制在自动驾驶和车辆动力学控制领域是个经典问题。不同于简单的PID控制，基于模型预测控制(MPC)的方法能够提前考虑未来多个时间步的系统行为，在处理非线性、强耦合的车辆动力学问题时展现出独特优势。
这次我们要实现的是一个支持自定义参考轨迹的二自由度MPC控制器。所谓二自由度，指的是我们主要控制车辆的横向运动（侧向位置）和横摆运动（航向角），而纵向速度则假设由另一个独立控制器管理。这种解耦处理在工程实践中非常常见，既能保证控制精度，又能降低计算复杂度。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 车辆动力学模型建立
2.1 二自由度自行车模型
二自由度车辆模型是轨迹跟踪控制的基石。这个简化模型将四个车轮等效为前后两个车轮，并假设轮胎侧偏特性处于线性区域。模型的运动方程可以表示为：
code复制% 模型参数
m = 1573; % 质量(kg)
Iz = 2873; % 转动惯量(kg·m²)
lf = 1.34; % 前轴距(m)
lr = 1.43; % 后轴距(m)
Caf = 80000; % 前轮侧偏刚度(N/rad)
Car = 80000; % 后轮侧偏刚度(N/rad)
Ts = 0.1; % 控制周期(s)
vx = 20; % 纵向速度(m/s)
% 状态空间矩阵
A = [1, Ts;
-2*(Caf+Car)/(m*vx)*Ts, 1-2*(lf*Caf-lr*Car)/(Iz*vx)*Ts];
B = [0; 2*Caf/(m*vx)*Ts];
这里特别要注意的是状态矩阵中的速度项vx。在实际应用中，vx会显著影响系统的动态特性。我的经验是：当车速变化超过±5m/s时，最好重新线性化模型，否则控制性能会明显下降。
2.2 模型离散化与预测时域
MPC需要在预测时域内求解优化问题。通常我们会选择3-5秒的预测时域，对应N=30-50个离散时间步（假设Ts=0.1s）。但有个重要发现：预测时域不宜固定不变。
在实测中发现，当参考轨迹曲率较大时（如发夹弯），过长的预测时域会导致：
- 求解器计算负担加重
- 预测轨迹偏离实际可行域
- 控制量出现剧烈振荡
建议的调整策略：
python复制# 根据曲率动态调整预测时域
curvature = abs(ref_traj['kappa'][current_index])
if curvature > 0.05: # 急弯情况
N = 20
elif curvature > 0.02: # 中等弯道
N = 30
else: # 直道或缓弯
N = 50
3. MPC控制器设计与调参
3.1 代价函数设计
MPC的核心是代价函数，通常形式为：
J = Σ(xᵀQx + uᵀRu)
其中Q和R矩阵的调参直接影响控制性能。经过数十次实车测试，我总结出以下规律：
- Q矩阵（状态权重）：
- 增大Q[0,0]（侧向位置误差权重）：跟踪精度提高，但可能导致控制量抖动
- 增大Q[1,1]（横摆角误差权重）：车辆朝向更准确，但可能牺牲部分位置精度
- R矩阵（控制量权重）：
- 增大R值：控制量变化更平滑，但响应变慢
- R值过小：前轮转角会出现高频振荡
3.2 动态权重调整策略
固定参数的MPC在复杂路况下表现不佳。通过大量测试，我发现动态调整Q矩阵能显著提升性能：
python复制# 动态权重调整算法
curvature = abs(ref_traj['kappa'][current_index])
speed_factor = vx / 20 # 归一化速度因子
# Q矩阵动态调整
Q_lat = 1.0 + 15 * curvature - 0.3 * speed_factor
Q_yaw = 0.1 + 2 * curvature + 0.1 * speed_factor
Q = np.diag([Q_lat, Q_yaw])
# R矩阵动态调整
R_scale = 0.01 * (1 + 0.5 * speed_factor)
R = np.array([[R_scale]])
这个调整策略的物理意义是：
- 弯道处（曲率大）加强侧向位置控制
- 高速时适当降低控制灵敏度
- 直道行驶时保持基础控制性能
3.3 约束处理技巧
输出约束对保证车辆稳定性至关重要。常见的错误是设置固定约束值，这会导致：
- 低速时转向不足
- 高速时转向过度
建议采用速度自适应约束：
c++复制// 根据车速动态调整最大前轮转角
double delta_max = 0.6 - 0.15 * fabs(current_speed/40);
if(delta_max < 0.3) delta_max = 0.3;
// 设置MPC约束
mpc.setConstraint(-delta_max, delta_max);
此外，控制量变化率约束也很重要。通常建议：
- 转向角速度不超过50°/s
- 加速度变化率不超过2m/s³
4. 实车调试经验分享
4.1 CarSim联合调试技巧
CarSim是验证控制算法的利器。几个实用技巧：
- 轨迹可视化：通过UDP将预测轨迹发送到CarSim Scene
matlab复制% Simulink UDP发送模块配置
udp_sender = udp('127.0.0.1', 30000);
fopen(udp_sender);
fwrite(udp_sender, traj_data, 'double');
-
参数实时调整：使用CarSim的VS Command功能在线修改MPC参数
-
数据同步记录：确保仿真时钟与控制器时钟严格同步
4.2 典型问题排查指南
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 直道行驶抖动 | Q矩阵权重过高 | 降低Q[0,0]10%-20% |
| 弯道跟踪滞后 | 预测时域过长 | 根据曲率动态缩短N |
| 高速时转向振荡 | R矩阵权重过低 | 增大R值并添加变化率约束 |
| 急弯冲出赛道 | 输出约束过紧 | 适当放宽delta_max约束 |
4.3 鲁棒性测试方案
完整的测试应该包含以下场景：
- 正弦波轨迹（验证周期性跟踪性能）
- 阶跃轨迹（验证瞬态响应）
- 蛇形轨迹（验证连续转向性能）
- 低附着路面（验证鲁棒性）
特别提醒：仿真通过≠实车可用。必须考虑：
- 传感器噪声（给状态估计添加5-10%噪声）
- 执行器延迟（添加100-200ms延迟）
- 路面附着系数变化（μ从0.3到1.0）
5. 进阶调参技巧
5.1 权重非线性处理
偶然发现一个有趣现象：对Q矩阵权重取三次方后，控制鲁棒性明显提升：
python复制Q_lat = (1.0 + 15 * curvature)**(1/3)
Q_yaw = (0.1 + 2 * curvature)**(1/3)
虽然数学原理还不完全清楚，但实测表明：
- 减少了极端曲率下的过调
- 改善了不同速度下的控制一致性
- 增强了抗干扰能力
5.2 多速率控制策略
传统MPC使用固定控制周期，但在复杂场景下可以考虑：
- 高速行驶时延长控制周期（如Ts=0.15s）
- 低速急弯时缩短控制周期（如Ts=0.05s）
实现要点：
- 保持预测时域长度不变（即调整N）
- 模型参数需要重新计算
- 注意控制量插值处理
5.3 前馈补偿设计
单纯反馈控制存在理论滞后。加入前馈控制可以显著提升弯道性能：
matlab复制% 前馈转角计算
delta_ff = (lr + m*vx^2/(2*Caf)*(lr+lf)/R) / (lf + lr);
注意前馈量需要与反馈量合理配合：
- 前馈占比随速度增加而提高
- 曲率估计要准确（建议低通滤波）
- 需要设置输出限幅
6. 工程实现建议
6.1 代码优化技巧
MPC实时性要求高，几个优化点：
- 预计算不变矩阵（如A,B,Q,R）
- 使用热启动（复用上一周期解）
- 选择高效QP求解器（如OSQP）
- 并行化预测计算
6.2 参数整定流程
建议的调参顺序：
- 固定R=0.01，只调Q
- 固定Q，调整R
- 微调预测时域N
- 最后调整约束边界
每次调整后都要测试：
- 阶跃响应
- 正弦跟踪
- 不同速度下的性能
6.3 实测注意事项
实车调试风险高，务必：
- 先在仿真中验证所有极端情况
- 实车测试时准备紧急停止机制
- 记录完整数据（建议100Hz以上）
- 从低速开始逐步验证
记住：没有放之四海皆准的参数组合。真正的工程能力体现在根据具体场景快速调整策略的本事。多积累不同路况下的调试经验，比死磕理论公式更有价值。