徐小疼1. MPC轨迹跟踪控制概述
在无人驾驶车辆控制领域，轨迹跟踪是核心挑战之一。传统PID控制器虽然简单易用，但其"只看当前"的特性在面对车辆动力学非线性、时变特性时往往力不从心。模型预测控制(MPC)凭借其预测未来、滚动优化的特点，成为解决这一问题的利器。
MPC的核心思想可以类比为下棋时的"走一步看三步"：在每个控制周期，控制器基于当前状态和预测模型，计算未来有限时域内的最优控制序列，但只执行第一步控制量。下一周期重复这一过程，实现"边走边看"的闭环控制。这种机制特别适合处理以下场景：
- 存在明显延迟的动力学系统
- 多变量耦合的复杂系统
- 需要兼顾多种约束条件的控制问题
在车辆控制中，MPC能够同时处理：
- 路径跟踪精度要求
- 车辆动力学约束（如轮胎力饱和）
- 乘坐舒适性指标（如加速度限制）
- 执行器物理限制（如转向角速率）
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 车辆动力学建模
2.1 自行车模型构建
车辆动力学模型是MPC预测的基础。我们采用经典的自行车模型(bicycle model)，它在保证足够精度的同时大幅降低了计算复杂度。该模型做了以下合理假设：
- 忽略悬架动力学
- 将左右侧轮胎合并为一个等效轮胎
- 假设纵向速度变化缓慢
- 小角度近似处理转向几何
模型涉及的关键参数如下：
matlab复制m = 1450; % 质量(kg)
lf = 1.3; % 前轴到质心距离(m)
lr = 1.5; % 后轴到质心距离(m)
Iz = 2740; % 绕Z轴转动惯量(kg·m²)
Caf = 80000;% 前轮侧偏刚度(N/rad)
Car = 80000;% 后轮侧偏刚度(N/rad)
2.2 状态方程推导
车辆状态选择为：
code复制x = [y; ψ; vx; vy; ω]
其中：
y - 横向位置误差
ψ - 航向角误差
vx - 纵向速度
vy - 横向速度
ω - 横摆角速度
控制输入为：
code复制u = [δ; a]
其中：
δ - 前轮转向角
a - 纵向加速度
基于牛顿-欧拉方程推导的连续时间状态方程为：
matlab复制function dx = vehicle_model(x, u)
delta = u(1);
a = u(2);
% 轮胎侧偏角计算
alpha_f = delta - atan((x(4)+lf*x(5))/x(3));
alpha_r = -atan((x(4)-lr*x(5))/x(3));
dx = zeros(5,1);
dx(1) = x(3)*sin(x(2)) + x(4)*cos(x(2)); % dy/dt
dx(2) = x(5); % dψ/dt
dx(3) = a - x(4)*x(5); % dvx/dt
dx(4) = (2*(Caf*alpha_f + Car*alpha_r)/m) - x(3)*x(5); % dvy/dt
dx(5) = (2*(lf*Caf*alpha_f - lr*Car*alpha_r))/Iz; % dω/dt
end
2.3 模型离散化处理
由于MPC在数字控制器上实现，需将连续模型离散化。采用一阶欧拉方法：
code复制x_{k+1} = x_k + T_s * f(x_k, u_k)
其中T_s为采样时间，经测试0.05s在精度和计算量间取得良好平衡。高阶方法如Runge-Kutta虽精度更高，但计算成本增加显著。
注意：离散化步长选择需考虑两个因素：
- 必须小于系统最快动态的时间常数
- 需要为QP求解留出足够计算时间
3. MPC控制器设计
3.1 目标函数构建
MPC通过优化目标函数来求取最优控制序列。我们采用二次型代价函数：
code复制J = Σ(x'Qx + u'Ru) + ρε²
其中：
- Q：状态权重矩阵，体现对跟踪误差的重视程度
- R：控制权重矩阵，抑制过大控制量
- ρ：松弛变量权重，保证优化问题可行性
实际调参经验：
matlab复制Q = diag([10, 5, 2, 1]); % 横向误差>航向角误差>速度误差>侧向速度
R = diag([0.1, 0.05]); % 转向角权重>加速度权重
rho = 100; % 松弛因子权重
权重选择遵循以下原则：
- 物理量纲统一化（如角度转为弧度，距离归一化）
- 关键状态（如横向误差）赋予更高权重
- 控制量权重需防止执行器饱和
3.2 约束条件处理
实际车辆控制必须考虑多种约束：
- 执行器物理限制：
matlab复制u_min = [-0.5, -3]'; % 最小转向角(rad), 最小加速度(m/s²)
u_max = [0.5, 2]'; % 最大转向角，最大加速度
- 稳定性约束（防侧翻）：
matlab复制a_lat_max = 3; % 最大侧向加速度(m/s²)
for k = 1:N
constraints = [constraints,
-a_lat_max <= (vy_pred(k)+lf*omega_pred(k))/vx_pred(k) <= a_lat_max];
end
- 控制增量约束（保证舒适性）：
matlab复制delta_u_max = [0.1, 1]'; % 转向角变化率限制(rad/s), 加速度变化率(m/s³)
3.3 预测时域选择
预测时域N与采样时间T_s共同决定预测时长T_h = N×T_s。经验法则：
- 至少覆盖系统主要动态响应时间
- 一般车辆控制选择1-2秒
- 本文取N=10，T_s=0.05s，即T_h=0.5s
过长的预测时域会导致：
- 计算负担急剧增加
- 预测精度下降（模型误差累积）
- 对突发障碍反应迟钝
4. MATLAB实现细节
4.1 求解器配置
使用MATLAB的quadprog求解QP问题，关键配置：
matlab复制options = optimoptions('quadprog',...
'Algorithm','active-set',...
'MaxIterations',100,...
'ConstraintTolerance',1e-6,...
'OptimalityTolerance',1e-6);
实测对比：
- active-set算法：适合中小规模问题，热启动效果好
- interior-point：适合大规模问题，但迭代次数不稳定
4.2 代码优化技巧
- 雅可比矩阵稀疏化：
matlab复制H = sparse(H); % 大幅减少内存占用
- 热启动技术：
matlab复制u_init = [prev_delta; prev_a]; % 用上一时刻解初始化
- 矩阵预计算：
离线计算不变的部分（如Q、R矩阵），减少在线计算量
4.3 实时性保障
关键时间节点（i7-1185G7 @3.0GHz）：
- 模型预测：0.8ms
- QP问题构建：1.2ms
- 求解器计算：3.5ms
- 总周期：<6ms (<0.05s采样时间)
实测发现：当问题不可行时，求解时间可能突增到15ms以上，因此必须引入松弛变量
5. 调试与性能优化
5.1 典型问题排查
- 求解器无解：
- 检查约束是否冲突（如速度过低时无法满足侧向加速度约束）
- 添加松弛变量：
cost = cost + rho*sum(slack.^2)
- 控制抖动：
- 增加控制量权重R
- 添加控制增量约束
- 低通滤波控制输出
- 跟踪滞后：
- 检查预测模型精度
- 适当减小采样时间
- 增加预测时域
5.2 参数整定流程
系统化调参步骤：
- 先调Q矩阵，确保基本跟踪性能
- 再调R矩阵，平滑控制量
- 最后调约束边界，保证安全性
- 反复微调，使用灵敏度分析工具
5.3 实测效果评估
在双移线工况下的性能指标：
| 指标 | 数值 | 单位 |
|---|---|---|
| 最大横向误差 | 0.12 | m |
| RMS横向误差 | 0.05 | m |
| 最大转向角速率 | 0.08 | rad/s |
| 计算时间标准差 | 0.4 | ms |
6. 进阶改进方向
6.1 延迟补偿策略
实际系统存在：
- 执行器延迟（转向电机响应）
- 传感器延迟（视觉处理）
- 通信延迟（CAN总线）
补偿方法：
- 状态预测器：基于当前控制量预测延迟后状态
- 时间戳对齐：统一各传感器时间基准
- 缓冲区管理：维护历史控制序列
6.2 参数自适应
应对车辆参数变化：
- 质量变化（载客/货物）
- 轮胎特性（磨损、路面）
- 动力学参数（电池消耗）
实现方案：
- 在线参数估计（递推最小二乘）
- 多模型MPC（根据工况切换）
- LPV-MPC（线性变参数）
6.3 非线性MPC
当工况极端（如低μ路面）时：
- 自行车模型线性假设失效
- 需考虑轮胎力非线性
- 改用NMPC+序列二次规划(SQP)
计算加速方案：
- 显式MPC（离线计算查表）
- 代码生成（MATLAB Coder)
- GPU并行计算
在实际工程应用中，MPC参数需要根据具体车辆平台反复调试。一个实用的建议是先在仿真中完成80%的调参工作，再逐步迁移到实车测试。每次修改参数后，建议进行完整的闭环仿真测试，包括正常工况和极端工况，确保控制器的鲁棒性。