别再硬啃公式了！用MATLAB从零复现双轮差速机器人的MPC轨迹跟踪（附完整代码）-平芜编程栈

双轮差速机器人MPC轨迹跟踪实战：从理论到MATLAB代码的完整实现

在机器人运动控制领域，轨迹跟踪是一个经典而富有挑战性的问题。当我们面对双轮差速驱动机器人时，如何让它精确地跟踪预定轨迹？传统PID控制虽然简单，但在复杂轨迹和非线性系统中往往表现不佳。这时，模型预测控制(MPC)以其优秀的处理多变量、约束条件和未来预测的能力脱颖而出。

1. MPC基础与双轮差速模型搭建

MPC的核心思想可以用"预测-优化-执行"三个词概括。与传统的控制方法不同，MPC不是在当前时刻做出单一决策，而是考虑未来一段时间内的系统行为，通过优化得到一系列控制输入，但只执行第一个控制量，然后在下一个采样周期重复这个过程。

对于双轮差速机器人，我们首先需要建立其运动学模型。假设机器人的状态量为[x, y, θ]ᵀ，分别代表机器人在全局坐标系下的x坐标、y坐标和航向角；控制量为[v, ω]ᵀ，即线速度和角速度。其连续时间运动学模型可以表示为：

ẋ = v * cosθ ẏ = v * sinθ θ̇ = ω

这个非线性模型需要进行线性化和离散化处理才能用于MPC。线性化通常在期望轨迹点附近进行一阶泰勒展开，而离散化则可以采用欧拉方法。最终得到的离散状态空间方程为：

x(k+1) = A_k x(k) + B_k u(k)

其中A_k和B_k是时变矩阵，但在实际实现中，我们常常假设它们在预测时域内保持不变以简化计算。

2. MPC预测方程与优化问题构建

MPC的关键在于构建预测方程，将未来状态表示为当前状态和控制输入的函数。假设预测时域为Np，控制时域为Nc（通常Nc ≤ Np），我们可以将预测方程表示为：

X = Ψ x(k) + Θ U

其中：

X是预测时域内的状态序列
U是控制时域内的控制输入序列
Ψ和Θ是由系统矩阵A和B构成的系数矩阵

目标函数的构建需要考虑两个主要方面：

跟踪误差最小化（使实际轨迹尽可能接近参考轨迹）
控制输入变化平滑（避免剧烈变化的控制量）

这可以表示为标准的二次型优化问题：

min 1/2 (X_ref - X)' Q (X_ref - X) + 1/2 U' R U

其中Q和R是权重矩阵，分别表示对跟踪误差和控制量变化的重视程度。在实际应用中，我们还需要考虑各种约束条件，如控制量的上下限、变化率限制等。

3. MATLAB实现关键步骤详解

3.1 参考轨迹生成

在MATLAB中，我们可以先定义参考轨迹。以下代码生成一个正弦波轨迹：

% 轨迹参数设置 pos_num = 500; % 轨迹点数量 sampling_period = 0.05; % 采样周期50ms reference_pos = zeros(pos_num,3); % 存储[x,y,θ] % 生成正弦轨迹 for i = 1:pos_num reference_pos(i,1) = (i-1)*sampling_period; reference_pos(i,2) = sin(reference_pos(i,1)/2); if i>1 reference_pos(i-1,3) = atan2(reference_pos(i,2)-reference_pos(i-1,2),... reference_pos(i,1)-reference_pos(i-1,1)); end end reference_pos(end,3) = reference_pos(end-1,3);

3.2 MPC控制器构建

MPC控制器的核心是二次规划问题的构建和求解。MATLAB提供了quadprog函数来求解这类问题。以下是关键步骤：

% 构建预测矩阵PHI和THETA PHI = zeros(state_dim*Np, state_dim); THETA = zeros(state_dim*Np, control_dim*Nc); for i=1:Np PHI((i-1)*state_dim+1:i*state_dim, :) = A^i; for j=1:min(i,Nc) THETA((i-1)*state_dim+1:i*state_dim, (j-1)*control_dim+1:j*control_dim) = A^(i-j)*B; end end % 构建二次规划问题的H和f矩阵 H = THETA'*Q*THETA + R; H = (H+H')/2; % 确保对称性 f = -2*(X_ref-PHI*x0)'*Q*THETA; % 添加控制量变化约束 A_cons = [eye(control_dim*Nc); -eye(control_dim*Nc)]; b_cons = [delta_u_max*ones(control_dim*Nc,1); delta_u_max*ones(control_dim*Nc,1)]; % 求解二次规划问题 options = optimset('Algorithm','interior-point-convex','Display','off'); [u_opt, ~, exitflag] = quadprog(H,f',A_cons,b_cons,[],[],[],[],[],options);

3.3 权重矩阵调参技巧

Q和R矩阵的选择对MPC性能至关重要。以下是一些实用建议：

参数	影响	调整建议
Q(1,1)	x位置跟踪权重	增大可提高x方向跟踪精度
Q(2,2)	y位置跟踪权重	增大可提高y方向跟踪精度
Q(3,3)	航向角权重	过大可能导致振荡
R(1,1)	线速度变化权重	增大可使速度变化更平滑
R(2,2)	角速度变化权重	增大可使转向更平缓

提示：初始调参可以从对角线元素Q=diag([10,10,1])和R=diag([0.1,0.1])开始，然后根据实际效果微调。

4. 实际应用中的问题与解决方案

4.1 计算效率优化

MPC的在线优化计算量较大，特别是当预测时域较长时。以下方法可以提高计算效率：

减少预测时域Np和控制时域Nc
使用热启动（将上一周期的解作为初始猜测）
考虑显式MPC或近似方法
采用更高效的QP求解器

4.2 常见问题排查

当MPC控制器表现不佳时，可以检查以下方面：

模型准确性：
- 确认运动学模型是否正确
- 检查线性化和离散化过程
- 验证系统矩阵A和B的计算
约束设置：
- 确认约束是否合理（特别是控制量上下限）
- 检查约束是否过于严格导致无解
数值稳定性：
- 检查H矩阵是否正定
- 考虑添加松弛变量处理硬约束

4.3 实时实现考虑

在实际机器人上实现MPC时，还需要考虑：

传感器数据的噪声处理
状态估计的准确性
计算延迟补偿
采样时间的一致性

% 状态估计示例（扩展卡尔曼滤波） function x_est = ekf_update(x_pred, z, P_pred, Q, R) % 预测步骤 H = jacobian_h(x_pred); % 测量雅可比矩阵 K = P_pred * H' / (H * P_pred * H' + R); % 卡尔曼增益 % 更新步骤 x_est = x_pred + K * (z - h(x_pred)); P_est = (eye(size(P_pred)) - K*H) * P_pred; end