MPC在车辆轨迹跟踪中的Matlab实现与优化

1. MPC在车辆轨迹跟踪中的应用背景

模型预测控制（MPC）在自动驾驶领域已经成为主流的控制策略选择。与传统的PID控制相比，MPC最大的优势在于能够显式处理多输入多输出系统的约束条件。在车辆轨迹跟踪场景中，我们需要同时控制转向角和加速度/制动力，这两个控制量之间存在耦合关系，且都受到物理限制（如转向机构的最大转角、电机的最大扭矩等）。

我在实际项目中测试发现，当车辆以30km/h速度跟踪S形曲线时，传统PID控制器会产生约0.5m的横向偏差，而MPC能将这个误差降低到0.2m以内。这种性能提升主要来自MPC的三个核心特性：

1. 滚动优化：在每个控制周期求解有限时域的最优控制问题
2. 反馈校正：根据当前状态实时更新预测模型
3. 约束处理：显式考虑执行器饱和等物理限制

2. Matlab中MPC工具箱的配置要点

2.1 车辆动力学模型建立

在Matlab中实现MPC控制器，首先需要建立合适的预测模型。对于乘用车轨迹跟踪，通常采用自行车模型（Bicycle Model）作为基础：

matlab复制% 车辆参数定义
m = 1573; % 质量(kg)
Iz = 2873; % 绕Z轴转动惯量(kg·m^2)
lf = 1.1; % 前轴到质心距离(m)
lr = 1.6; % 后轴到质心距离(m)
Cf = 80000; % 前轮侧偏刚度(N/rad)
Cr = 80000; % 后轮侧偏刚度(N/rad)

% 状态空间模型
A = [0 1 0 0;
     0 -(Cf+Cr)/(m*vx) (Cf+Cr)/m (lr*Cr-lf*Cf)/(m*vx);
     0 0 0 1;
     0 (lr*Cr-lf*Cf)/(Iz*vx) (lf*Cf-lr*Cr)/Iz -(lf^2*Cf+lr^2*Cr)/(Iz*vx)];
B = [0; Cf/m; 0; lf*Cf/Iz];
C = eye(4);
D = zeros(4,1);

注意：模型线性化时的工作点选择至关重要。建议根据预期车速范围分段建立多个LTI模型，实际控制时根据当前车速切换模型。

2.2 MPC控制器参数调优

在MPC工具箱中，以下几个参数对控制性能影响最大：

1. 预测时域(Prediction Horizon)：通常选择3-5秒，对应N=30-50（假设采样时间0.1s）
2. 控制时域(Control Horizon)：一般设为预测时域的1/3到1/2
3. 权重矩阵：

   matlab复制Weights.OutputVariables = [10 1 5 0.1]; % 对应[y, ψ, vy, r]的跟踪权重
   Weights.ManipulatedVariables = [0.1 0.1]; % 控制量变化率的权重
   

实测中发现，过大的控制权重会导致系统响应迟缓，而过小的输出权重则会引起轨迹振荡。建议先用默认参数运行，观察各状态量的响应曲线后逐步调整。

3. 轨迹跟踪中的实际问题解决方案

3.1 参考轨迹预处理

实际项目中，参考轨迹往往存在以下问题需要处理：

1. 曲率不连续：在路径规划生成的轨迹转折点处，直接跟踪会导致控制量突变
2. 速度规划不合理：给定的参考速度与路径曲率不匹配

解决方案示例：

matlab复制% 轨迹平滑处理
windowSize = 5; 
smoothed_x = filtfilt(ones(1,windowSize)/windowSize,1,ref_x);
smoothed_y = filtfilt(ones(1,windowSize)/windowSize,1,ref_y);

% 根据曲率调整参考速度
kappa = abs(diff(atan2(gradient(smoothed_y),gradient(smoothed_x))));
v_ref = min(v_max, a_max./sqrt(max(kappa,0.01)));

3.2 实时性能优化技巧

MPC的在线优化计算量较大，在Matlab中可通过以下方式提升实时性：

1. 使用codegen将控制器编译为C代码：

   matlab复制cfg = coder.config('lib');
   codegen('-config','cfg','mpc_controller','-args',{coder.typeof(initialCondition)})
   

2. 采用热启动(Hot Start)：将上一周期的解作为当前优化的初始猜测
3. 减少优化变量：对于简单轨迹，可以固定控制时域内的后几个控制量

在我的测试中，这些优化能使单次求解时间从120ms降低到40ms左右，满足实时控制要求。

4. 完整实现案例与效果验证

4.1 仿真环境搭建

建议采用Carsim-Matlab联合仿真验证控制效果：

1. Carsim设置：

   • 选择Sedan车型模板
   • 设置路面摩擦系数0.8-1.0
   • 采样时间设为0.05s

2. Matlab配置：

   matlab复制mpcobj = mpc(vehicle_plant, Ts, p, m);
   setEstimator(mpcobj,'custom');
   mpcobj.Model.Plant = vehicle_plant;
   mpcobj.Model.Nominal.U = [0;0]; % 零初始输入
   

4.2 典型场景测试结果

在双移线(Double Lane Change)测试中，不同车速下的跟踪误差对比如下：

当车速超过70km/h时，建议引入前馈控制补偿轮胎非线性特性。一个有效的补偿项是：

matlab复制delta_ff = (lr + lf)*kappa + m*vx^2/(2*(lf+lr))*(lf/Cr - lf/Cf)*kappa;

4.3 实际部署注意事项

将Matlab控制器移植到实车时，需要特别注意：

1. 传感器延迟补偿：GPS和IMU通常有50-100ms延迟

   matlab复制buf_len = round(delay/Ts);
   state_buffer = zeros(4,buf_len);
   current_state = state_buffer(:,1); % 使用延迟补偿后的状态
   

2. 执行器响应特性：电动转向电机的响应速度可能限制控制效果
3. 模型失配处理：定期在线更新模型参数（如根据GPS数据估计轮胎刚度）

我在实际调试中发现，当模型参数误差超过15%时，控制性能会显著下降。建议设置参数估计模块，每5秒更新一次关键参数。