1. 项目概述运动学模型与MPC轨迹跟踪的Matlab实践在自动驾驶和机器人控制领域轨迹跟踪是核心问题之一。最近我用Matlab完整实现了一套基于运动学模型的MPC模型预测控制轨迹跟踪方案整个过程从建模到实现全部采用纯Matlab代码完成没有依赖Simulink或其他工具箱。这种方案特别适合学术研究和小型无人系统开发因为Matlab的矩阵运算和优化工具包能极大简化MPC的实现难度。传统PID控制在处理非线性系统和约束条件时表现不佳而MPC通过在线优化未来时间窗口内的控制序列能够显式处理各种约束如速度限制、转向角限制等。我选择自行车模型作为运动学基础因为它在低速场景下5m/s能很好平衡精度和计算复杂度。实测下来这套方案在1kHz的控制频率下单步优化耗时仅2-3ms使用Matlab的quadprog求解器完全能满足实时性要求。2. 运动学模型构建与离散化2.1 自行车模型推导自行车模型是车辆运动学控制的黄金标准它假设前后轮可以合并为单个轮子。模型状态包括(x,y)车辆后轴中心坐标φ航向角v车速δ前轮转向角连续时间模型可以表示为ẋ v*cos(φ) ẏ v*sin(φ) φ̇ (v/L)*tan(δ)其中L是轴距。这个模型有两个输入加速度a和转向角速度ω。注意该模型假设无侧滑当车速较高或地面摩擦系数低时需改用动力学模型2.2 离散化处理MPC需要在离散时间域工作我采用前向欧拉法进行离散化时间步长dt0.01sx(k1) x(k) v(k)*cos(φ(k))*dt y(k1) y(k) v(k)*sin(φ(k))*dt φ(k1) φ(k) (v(k)/L)*tan(δ(k))*dt v(k1) v(k) a(k)*dt δ(k1) δ(k) ω(k)*dt离散化后的状态空间方程可表示为function x_next bicycle_model(x, u, L, dt) % x [px, py, phi, v, delta] % u [a, omega] x_next x [ x(4)*cos(x(3))*dt; x(4)*sin(x(3))*dt; (x(4)/L)*tan(x(5))*dt; u(1)*dt; u(2)*dt; ]; end3. MPC控制器设计3.1 预测模型与优化问题MPC的核心是在每个控制周期求解如下优化问题min J Σ(跟踪误差) Σ(控制量惩罚) Σ(控制变化率惩罚) s.t. 运动学约束 控制量约束 状态约束我设计的代价函数包含三项状态误差惩罚(x-x_ref)Q(x-x_ref)控制量惩罚uRu控制变化率惩罚ΔuRΔΔu对应的Matlab实现Q diag([10, 10, 5, 2, 1]); % 状态权重 R diag([0.1, 1]); % 控制量权重 Rdelta diag([0.01, 0.1]); % 控制变化率权重 H blkdiag(kron(eye(N),Q), kron(eye(Nc),R), kron(eye(Nc),Rdelta)); f zeros(size(H,1),1);3.2 约束处理技巧实际车辆存在物理限制必须处理以下约束速度范围0 ≤ v ≤ 10 m/s转向角范围-30° ≤ δ ≤ 30°加速度范围-3 ≤ a ≤ 3 m/s²转向速度范围-30°/s ≤ ω ≤ 30°/s在Matlab中通过quadprog的A、b矩阵设置这些约束Aineq [...]; % 构建不等式约束矩阵 bineq [...]; % 构建不等式约束边界 options optimoptions(quadprog,Display,off); [u_opt,~,exitflag] quadprog(H,f,Aineq,bineq,[],[],[],[],[],options);实操心得初始调试时可先去掉约束确认基本功能正常后再逐步添加约束便于问题定位4. 轨迹生成与跟踪实现4.1 参考轨迹生成我采用三次样条曲线生成平滑参考轨迹。首先生成路径点然后用spline插值waypoints [0 0; 5 2; 10 -1; 15 3]; t cumsum([0; sqrt(sum(diff(waypoints).^2,2))]); ref_path spline(t, waypoints, linspace(0,t(end),1000));对于每个参考点还需要计算参考速度和航向角dx gradient(ref_path(:,1)); dy gradient(ref_path(:,2)); phi_ref atan2(dy, dx); v_ref 2*ones(size(phi_ref)); % 恒定速度2m/s4.2 闭环控制流程完整的MPC跟踪算法流程如下获取当前状态x(k)从参考轨迹提取N个点的参考序列构建QP问题矩阵H,f,A,b调用quadprog求解最优控制序列应用第一个控制量u(k)等待下一个控制周期核心循环代码结构while k length(ref_time) % 获取当前参考轨迹窗口 ref_window get_reference_window(k, N, ref_path); % 构建QP问题 [H, f, A, b] build_mpc_problem(x_current, ref_window, Q, R, Rdelta); % 求解MPC u_opt solve_mpc(H, f, A, b); % 应用控制量 x_current bicycle_model(x_current, u_opt(1:2), L, dt); % 记录数据 X_history(:,k1) x_current; U_history(:,k) u_opt(1:2); k k 1; end5. 性能优化与调试技巧5.1 计算效率提升纯Matlab实现容易遇到性能瓶颈我采用了以下优化措施预分配所有数组内存将H矩阵构建改为增量更新90%元素不变使用persistent变量保持求解器状态将频繁调用的函数转为pcode优化前后对比操作原始耗时(ms)优化后(ms)H矩阵构建1.80.3QP求解2.11.9状态更新0.050.025.2 调试常见问题求解器失败检查H矩阵是否正定添加小量单位矩阵跟踪振荡增大控制变化率惩罚Rdelta滞后严重减小预测时域或提高速度权重约束冲突检查约束条件是否自相矛盾典型调试命令% 检查H矩阵条件数 cond(H) % 可视化预测序列 plot_prediction(x_current, u_opt) % 验证梯度计算 check_gradient(cost_function, x0)6. 扩展应用与进阶方向这套基础框架可以扩展为考虑道路边界约束增加状态不等式约束加入障碍物避碰在QP中添加距离约束多车协同控制分布式MPC参数自适应在线调整Q,R权重对于更复杂的场景我建议改用C实现提升速度使用ACADO或HPIPM求解器加入动力学模型魔术公式轮胎模型结合SLAM实现完全自主导航我在实际测试中发现当引入路面坡度估计时需要在状态方程中添加θ俯仰角状态这会增加模型复杂度但能显著提升爬坡性能。另一个实用技巧是在弯道处根据曲率动态调整参考速度这能减少侧向误差。
