Matlab实现MPC轨迹跟踪：从建模到控制优化-育师

1. 项目背景与核心价值

在自动驾驶和机器人控制领域，轨迹跟踪是一个经典而关键的问题。传统PID控制虽然简单易用，但在处理非线性系统、多约束条件时往往力不从心。这正是模型预测控制（MPC）大显身手的地方——它能够显式处理系统约束，通过滚动优化实现前瞻性控制。

我最近用纯Matlab实现了一个基于运动学模型的MPC轨迹跟踪器，完整复现了从理论推导到代码实现的全部流程。与常见的调用第三方库的方案不同，这个实现完全从底层数学公式出发，特别适合想要深入理解MPC工作原理的开发者。实测在2m/s速度下，跟踪误差能稳定控制在5cm以内。

2. 运动学模型构建

2.1 车辆运动学建模

我们采用经典的自行车模型来描述车辆运动学特性。假设车辆为刚性车身，且忽略轮胎侧偏角，可以得到以下非线性状态方程：

dx/dt = v * cos(θ + β) dy/dt = v * sin(θ + β) dθ/dt = v * sin(β) / L_r β = atan(L_r / (L_f + L_r) * tan(δ))

其中(x,y)为后轴中心坐标，θ为航向角，v为车速，δ为前轮转角，L_f和L_r分别表示前后轴到质心的距离。这个模型虽然简化了动力学因素，但对于低速场景（<5m/s）已经足够精确。

注意：在实际编码时，建议使用小角度近似简化计算。当|δ|<15°时，β≈L_rδ/(L_f+L_r)，可显著降低计算复杂度。

2.2 模型离散化处理

MPC需要在离散时间域求解优化问题。我们采用前向欧拉法对连续模型进行离散化：

x(k+1) = x(k) + T_s * v(k) * cos(θ(k) + β(k)) y(k+1) = y(k) + T_s * v(k) * sin(θ(k) + β(k)) θ(k+1) = θ(k) + T_s * v(k) * sin(β(k)) / L_r

采样周期T_s的选择至关重要——太大会导致离散误差，太小会增加计算负担。经过实测，在车速2m/s时，T_s=0.1s能在精度和效率间取得良好平衡。

3. MPC控制器设计

3.1 优化问题构建

MPC的核心是求解一个有限时域的优化问题。我们需要定义：

状态变量：ξ = [x, y, θ]^T
控制输入：u = [v, δ]^T
代价函数：J = Σ(ξ-ξ_ref)^T Q (ξ-ξ_ref) + u^T R u
约束条件：v_min ≤ v ≤ v_max, |δ| ≤ δ_max

其中Q和R为权重矩阵，需要根据控制优先级调整。我的经验值是：

Q = diag([10, 10, 5]); % 更重视位置误差 R = diag([0.1, 1]); % 限制控制量变化

3.2 线性化与QP求解

非线性MPC问题通常需要迭代求解，计算量较大。我们采用连续线性化方法(Successive Linearization)：

在当前状态点ξ_k处对模型进行一阶泰勒展开
将非线性约束转化为线性不等式
用quadprog求解得到的QP问题

关键实现代码如下：

function [u_opt, cost] = solveMPC(x0, refTraj) % 构造Hessian矩阵 H = blkdiag(kron(eye(N),Q), kron(eye(N),R)); % 构造梯度向量 f = [repmat(-Q*refTraj(:), N, 1); zeros(N*nu,1)]; % 调用QP求解器 options = optimoptions('quadprog','Display','none'); u_opt = quadprog(H,f,Aineq,bineq,[],[],lb,ub,[],options); end

实测发现：预测时域N=10时已经能取得不错效果，继续增大N对性能提升有限但计算量会显著增加。

4. Matlab实现技巧

4.1 实时性优化

纯Matlab实现容易遇到实时性问题，我采用了以下优化手段：

预分配所有数组内存
将for循环改为矩阵运算
使用persistent变量保持求解器状态
启用JIT加速（默认开启）

经过优化后，单次求解时间从35ms降至8ms（i7-11800H @2.3GHz），完全能满足实时控制需求。

4.2 可视化调试

强大的可视化是Matlab的天然优势。我开发了实时动画显示功能，可以直观观察：

车辆实际轨迹与参考路径的偏差
预测时域内的状态变化
控制量的变化曲线

核心绘图代码：

function updatePlot(vehicle, predTraj, refPath) delete(findobj('Tag','MPC_Plot')); plot(refPath(:,1), refPath(:,2), 'b--'); hold on; plot(predTraj(:,1), predTraj(:,2), 'g-'); rectangle('Position',[vehicle.x-1, vehicle.y-0.5, 2, 1],... 'Curvature',[0.5 0.5],'FaceColor','r'); axis equal; grid on; drawnow; end

5. 典型问题与解决方案

5.1 轨迹震荡问题

初期实现中出现车辆沿参考轨迹高频震荡的现象，主要原因是：

权重矩阵Q中角度误差权重过大
控制量变化惩罚R设置过小
采样周期与预测时域不匹配

通过以下调整解决：

降低θ的权重（从5调至2）
增加δ的惩罚项（从1调至5）
使T_s*N ≈ 车辆制动距离/车速

5.2 实时性不足

当路径曲率较大时，求解时间会突然增加。这是因为：

非线性增强导致迭代次数增加
约束条件变得更活跃

改进措施包括：

添加求解时间监控，超时则使用上一时刻解
采用warm-start技术，用上一解作为初始猜测
对于紧急情况，切换为纯追踪算法

6. 进阶扩展方向

基础实现稳定后，可以考虑以下增强功能：

动态障碍物避碰：

% 在代价函数中添加排斥势场项 obstacle_cost = sum(exp(-0.5*dist_to_obstacles.^2/sigma^2)); J = J + obstacle_cost;

参数自适应调节：根据跟踪误差动态调整Q矩阵权重，大误差时增加位置权重，小误差时注重舒适性。
C代码生成：使用Matlab Coder将算法转为C代码，便于部署到嵌入式平台：

cfg = coder.config('lib'); codegen('solveMPC','-config','cfg','-args',{x0, refTraj});

这个项目让我深刻体会到，即使像MPC这样理论复杂的控制器，只要拆解到位，用Matlab也能实现干净优雅的解决方案。最关键的是理解每个参数背后的物理意义，而不是盲目调参。下一步我准备加入轮胎动力学模型，进一步提升高速场景下的控制精度。

Matlab实现MPC轨迹跟踪：从建模到控制优化