从理论到实践：基于双轮差速模型的MPC轨迹跟踪全解析

1. 双轮差速机器人轨迹跟踪的挑战与MPC优势

让机器人沿着预定轨迹移动听起来简单，但实际实现时会遇到各种棘手问题。我曾在实验室调试双轮差速机器人时，亲眼目睹它像醉汉一样走出"S形"路线，甚至直接偏离目标轨迹。传统PID控制在这种场景下往往力不从心，而模型预测控制（MPC）却能优雅地解决这些问题。

双轮差速机器人是最常见的移动机器人平台之一，它的运动控制有两个关键特点：一是通过左右轮速差实现转向，二是低速运动时动力学影响较小。这两个特点使得我们可以基于运动学模型进行控制，大大简化了问题复杂度。但在实际应用中，机器人仍面临三大挑战：

1. 非完整约束：机器人不能像汽车那样横向移动，运动方向受当前朝向限制
2. 状态耦合：位置(x,y)和航向角θ之间存在非线性耦合关系
3. 实时性要求：需要在毫秒级完成计算并输出控制指令

MPC之所以适合这个场景，是因为它能将这些问题统一转化为优化问题求解。与PID的"事后纠正"不同，MPC是"事前预测"——它会模拟未来多个时间步的运动状态，提前计算出最优控制序列。这种"走一步看三步"的方式，特别适合处理带有约束条件的轨迹跟踪问题。

我在实际项目中对比过几种控制方法的效果：

• PID控制：跟踪曲线轨迹时超调明显
• 纯追踪算法：转弯处会出现截断现象
• MPC控制：能平滑贴合参考轨迹，且速度变化更自然

2. MPC核心原理与双轮模型适配

2.1 MPC的三步走策略

MPC的核心可以用"预测-优化-校正"三个词概括。具体到双轮机器人控制，每个步骤都有其特殊处理：

模型预测阶段：我们基于双轮差速运动学模型，推导出状态空间方程。这个非线性方程需要经过线性化和离散化处理才能用于MPC。线性化时我常用一阶泰勒展开，虽然会损失一些精度，但计算量大幅降低。离散化则推荐使用后向差分法，它在保持稳定性的同时实现简单。

滚动优化阶段：这里需要构建两个关键矩阵——预测矩阵Θ和控制矩阵Φ。它们将未来Np个时刻的状态预测表示为当前状态和控制输入的线性组合。在我的实现中，Np一般取20，Nc取5，这样能在预测深度和计算效率间取得平衡。

反馈校正阶段：每次只执行第一个控制指令，然后重新采样状态进行下一轮计算。这种闭环策略能有效克服模型误差和外部干扰。实际测试表明，即使模型精度只有80%，跟踪误差也能控制在5cm以内。

2.2 运动学建模细节

双轮机器人的状态方程看似简单，但藏着不少玄机。状态量选择[x,y,θ]ᵀ，控制量[v,ω]ᵀ，这个组合能完整描述机器人的运动状态。推导时要注意：

1. 非完整约束表现为ẋ·sinθ - ẏ·cosθ = 0
2. 线性化时需要在参考点附近展开，我通常取期望轨迹上的最近点
3. 离散化时采样周期T的选择很关键，太大会导致不稳定，太小会增加计算负担

经过这些处理，我们得到离散状态方程： ξ(k+1) = A_kξ(k) + B_ku(k) 其中A_k和B_k是时变矩阵，但为了简化计算，可以假设它们在预测时域内不变。

3. 预测模型构建与优化问题转化

3.1 预测方程推导

预测模型是MPC的大脑，它要能预见未来。构建过程就像搭积木：

1. 将当前状态和过去控制量组合成增广状态ξ = [x, y, θ, v, ω]ᵀ
2. 递归应用离散状态方程，推导出预测时域内的状态序列
3. 用矩阵形式表示这些关系，得到紧凑的预测方程

这个过程中最易出错的是矩阵维度匹配。我建议先用小规模时域（如Np=3）手工推导，确认无误后再推广。预测方程的最终形式为： Y = Θξ(k) + ΦΔU 其中Y是预测输出序列，ΔU是待求的控制增量序列。

3.2 二次规划问题构建

将控制问题转化为二次规划(QP)是MPC的精华所在。我们需要设计合理的目标函数：

J = (Y-Y_ref)ᵀQ(Y-Y_ref) + ΔUᵀRΔU + ρε²

三项分别对应：

1. 轨迹跟踪误差惩罚
2. 控制量变化率惩罚
3. 松弛因子保证可行性

权重矩阵Q和R的选取直接影响控制效果。我的经验是：

• Q对角元素取[10,10,1]，强调位置精度胜过角度
• R取较小值如0.1I，允许控制量适度变化
• ρ取1e4，平衡约束违反代价

转化为标准QP形式后，问题变为： min ½ΔUᵀHΔU + fᵀΔU s.t. AΔU ≤ b

4. MATLAB实现与调参技巧

4.1 仿真框架搭建

完整的MATLAB实现需要以下几个模块：

%% 轨迹生成 function ref_traj = generate_trajectory(type) % 支持直线、圆形、八字形等轨迹 switch type case 'line' % 直线轨迹代码 case 'circle' % 圆形轨迹代码 end end %% MPC控制器 function [v, w] = mpc_controller(x, ref) % 构建预测矩阵 THETA = compute_theta(A, B, Np); % 构造QP问题 H = THETA'*Q*THETA + R; f = 2*(PHI*kesi)'*Q*THETA; % 求解 options = optimoptions('quadprog','Algorithm','interior-point-convex'); delta_u = quadprog(H,f',A_cons,b_cons,[],[],lb,ub,[],options); % 输出控制量 v = last_v + delta_u(1); w = last_w + delta_u(2); end

4.2 参数调试经验

调参是MPC实现中最耗时的环节。根据我的项目经验，这些参数需要特别注意：

1. 预测时域(Np)：通常取10-20，对应1-2秒的预测范围
2. 控制时域(Nc)：取Np的1/4到1/2，太大反而会降低性能
3. 采样周期(T)：建议50-100ms，要匹配实际控制器的执行频率
4. 权重矩阵：先调Q确保跟踪精度，再调R平滑控制量

调试时可以先用正弦轨迹测试，它能同时检验直线和转弯性能。一个实用的技巧是记录优化问题的exitflag，如果经常返回非正值，说明问题设置可能不合理。

5. 实际工程中的问题与解决方案

5.1 实时性保障

MPC最大的挑战是计算耗时。在我的嵌入式实现中，采用了以下优化措施：

1. 热启动：用上一周期的解作为初始猜测
2. 代码生成：将MATLAB代码转为C代码
3. 固定点计算：对QP求解器进行定点化处理

经过这些优化，在STM32F4系列MCU上能将计算时间控制在20ms以内。

5.2 模型失配处理

当机器人负载变化或地面摩擦系数改变时，运动学模型会出现误差。我常用的补偿方法有：

1. 增加扰动观测器
2. 在线更新B矩阵参数
3. 适当增大松弛因子权重

实验表明，即使模型参数有20%误差，跟踪性能仍能保持在可接受范围内。