基于MPC的无人驾驶车辆轨迹跟踪控制实践

1. 项目背景与核心问题

轨迹跟踪是无人驾驶车辆实现自主导航的基础能力之一。在实际道路环境中，车辆需要实时处理复杂的动力学约束、环境干扰和不确定性因素，这对控制算法的鲁棒性和实时性提出了极高要求。传统PID控制方法在面对非线性、强耦合的车辆动力学系统时往往表现不佳，而模型预测控制（MPC）因其显式处理约束和滚动优化的特性，成为解决这一问题的理想选择。

我在参与某园区无人接驳车项目时，曾遇到传统控制方法在弯道跟踪中产生较大横向偏差的问题。当车速超过20km/h时，PID控制器在曲率半径小于30m的弯道上会出现明显的振荡现象。这促使我们转向研究基于MPC的解决方案，最终实现了在40km/h速度下，对曲率半径15m弯道的稳定跟踪（横向误差<0.3m）。

2. 技术方案设计

2.1 车辆动力学建模

采用经典的自行车模型作为基础框架，在MATLAB/Simulink中建立了包含以下关键参数的非线性模型：

% 车辆参数定义 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)

通过小角度假设对模型进行线性化处理，得到状态空间表达式：

ẋ = Ax + Bu y = Cx

其中状态变量x=[v_y, γ, e, Δψ]分别代表横向速度、横摆角速度、横向误差和航向误差。

实际建模中发现：当转向角超过8°时，线性化模型误差会显著增大。因此我们在MPC中设置了转向角约束|δ|≤0.14rad(≈8°)，并通过迭代线性化来处理大转角工况。

2.2 MPC控制器设计

采用二次规划(QP)形式构建优化问题：

min J = Σ( x(k)'Qx(k) + u(k)'Ru(k) ) + x(N)'Px(N) s.t. x(k+1) = Ax(k) + Bu(k) |u(k)| ≤ u_max |Δu(k)| ≤ Δu_max

关键参数设计要点：

• 预测时域N=20，控制时域Nu=5（经测试在采样时间0.1s时能平衡实时性与预测效果）
• 权重矩阵Q=diag([10,1,50,10])，突出横向误差e的惩罚
• 输入权重R=0.1*eye(2)，避免转向和加速度突变

% MPC参数配置示例 mpcobj = mpc(linmodel,Ts,N,Nu); mpcobj.Weights.OutputVariables = [10 1 50 10]; mpcobj.Weights.ManipulatedVariables = [0.1 0.1]; mpcobj.Weights.ManipulatedVariablesRate = [0.01 0.01];

3. 实现细节与优化

3.1 参考轨迹处理

实际工程中采用三次样条插值生成平滑参考路径。关键处理步骤包括：

1. 对原始路径点进行等弧长重采样
2. 计算曲率并标记特殊路段（如直角弯）
3. 根据当前车速动态调整前视距离：

lookahead_dist = min(max(0.3*vx, 5), 30); % 限制在5-30m范围内

3.2 实时性优化

通过以下方法将单步计算时间控制在50ms内：

• 使用QP求解器的热启动(warm start)技术
• 将雅可比矩阵计算移至离线阶段
• 采用显式MPC（eMPC）将在线优化转为查表操作

实测对比数据：

4. 典型问题与解决方案

4.1 曲率突变场景

当遇到急弯时，容易出现前轮饱和导致跟踪失效。我们采用的应对策略：

1. 在路径预处理阶段检测曲率变化率dκ/ds
2. 当dκ/ds超过阈值时，提前触发速度规划模块降速
3. 在MPC成本函数中增加曲率变化率惩罚项

4.2 低附着路面

通过轮胎力观测器实时估计路面摩擦系数μ，并动态调整：

• 最大转向角约束：δ_max = 0.8atan(μlr/(vx^2))
• Q矩阵中的横向误差权重：Q(3,3)=50*μ

5. 完整实现流程

1. 环境搭建：

addpath('casadi') % 最优控制求解器 init_vehicle_params(); % 加载车辆参数

2. 路径加载与预处理：

refPath = load_ref_path('track01.csv'); refPath = smooth_path(refPath, 0.5); % 平滑处理

3. MPC控制器初始化：

mpc_controller = init_mpc_controller(... 'PredictionHorizon',20,... 'ControlHorizon',5,... 'SampleTime',0.1);

4. 主控制循环：

while ~isempty(refPath) [u, opt_info] = mpc_controller.step(x0, refPath); apply_control(u); update_vehicle_state(); refPath = update_lookahead(refPath); end

6. 实测效果验证

在某封闭测试场进行的双移线测试结果：

实际部署中发现：当车速超过60km/h时，需要将预测时域延长至25步以上才能保证稳定性，但这会增加约35%的计算负荷。因此在实际系统中建议根据车速动态调整预测时域。

7. 工程实践建议

1. 参数调试技巧：

• 先固定R矩阵调试Q，找到跟踪精度与舒适度的平衡点
• 在直线段适当降低横向误差权重（Q(3,3)减小30%）
• 对加速度约束采用不对称限幅（制动-3m/s²，加速2m/s²）

2. 实时性保障：

• 使用MATLAB Coder生成C代码可提升3-5倍速度
• 对于x86平台，启用AVX指令集优化
• 将QP求解失败时的备选策略设为上一时刻最优解的线性外推

3. 特殊场景处理：

if detect_obstacle() % 切换至紧急避障模式 mpc_controller.Weights.OutputVariables = [1 1 200 5]; activate_emergency_constraints(); end

在最终部署版本中，我们加入了基于路面识别的参数自适应模块，使系统能在不同附着系数路面上自动调整控制参数。这套方案已成功应用于园区物流车项目，累计运行超过2000公里，轨迹跟踪误差始终保持在0.5m以内。