MPC在两轮差速机器人轨迹跟踪中的实践与优化

1. 项目背景与核心价值

两轮差速移动机器人作为服务机器人、仓储AGV等领域的常见载体，其轨迹跟踪精度直接影响作业效果。传统PID控制面对复杂轨迹时存在响应滞后、超调明显等问题。我在某仓储机器人项目中实测发现：当以1.5m/s速度跟踪S形轨迹时，PID的横向误差可达0.3m以上。而采用MPC（Model Predictive Control）后，相同工况下误差可控制在0.05m内。

MPC的核心优势在于：

• 预测时域内多步优化，提前补偿系统惯性
• 显式处理控制量约束（如电机转速限制）
• 天然适应非线性系统特性

2. 系统建模与控制器设计

2.1 运动学模型建立

采用经典的两轮差速模型，定义机器人状态量为：

code复制x = [x_pos, y_pos, theta]^T
u = [v_left, v_right]^T

其中(x_pos,y_pos)为全局坐标，theta为航向角，v_left/v_right为左右轮线速度。

离散化后的状态方程：

code复制x_{k+1} = x_k + T * [ (v_left + v_right)/2 * cos(theta_k),
                      (v_left + v_right)/2 * sin(theta_k),
                      (v_right - v_left) / L ]^T

L为轮距，T为控制周期（推荐10-50ms）

注意：实际项目中需考虑轮径校准误差，建议通过实验数据拟合修正模型参数

2.2 MPC问题构建

定义优化目标函数：

code复制min J = Σ( (x_k - x_ref)^T Q (x_k - x_ref) ) 
       + Σ( u_k^T R u_k )
       + Σ( Δu_k^T S Δu_k )

其中：

• Q：状态误差权重矩阵（需调参）
• R：控制量权重矩阵（抑制剧烈变化）
• S：控制增量权重（平滑性保障）

约束条件示例：

code复制|v_left| ≤ 1.2 m/s  
|Δv_right| ≤ 0.3 m/s^2

2.3 实时求解优化

推荐采用以下工具链：

1. ACADO Toolkit：自动生成高效C++求解代码
2. OSQP：轻量级二次规划求解器
3. CasADi：Python/Matlab符号计算框架

实测对比（树莓派4B平台）：

3. 轨迹跟踪实现细节

3.1 参考轨迹处理

对于离散轨迹点序列，建议采用：

1. 三次样条插值保证C2连续性
2. 前向差分计算参考速度/加速度
3. 动态调整预测时域（曲率大时缩短）

Python示例：

python复制from scipy.interpolate import CubicSpline
cs = CubicSpline(t_ref, x_ref, bc_type='clamped')
x_ref = cs(t_now)  # 位置
dx_ref = cs(t_now,1) # 一阶导（速度）

3.2 误差补偿策略

1. 横向误差计算：

code复制e_y = -(x - x_ref)*sin(theta_ref) + (y - y_ref)*cos(theta_ref)

2. 前馈补偿项：

code复制u_ff = [v_ref, w_ref]^T + K * [e_y, e_theta]^T

K为前馈增益矩阵（需实验标定）

3.3 参数整定经验

1. 权重矩阵初始值建议：

code复制Q = diag([1.0, 1.0, 0.5])  
R = diag([0.1, 0.1])

2. 调整顺序：

• 先调Q使跟踪误差收敛
• 再调R避免控制量饱和
• 最后加S抑制抖动

4. 典型问题与解决方案

4.1 计算延迟应对

现象：实际控制量输出滞后导致振荡
解决方案：

1. 在状态预测中加入延迟补偿：

code复制x_k = x_measured + v_measured * delay_time

2. 减小求解器迭代次数（牺牲少量最优性）

4.2 电机饱和处理

当优化解超出物理限制时：

1. 采用约束优先级策略：
   • 先保证速度绝对值限制
   • 再尽量满足加速度约束
2. 触发安全策略：

c复制if (v_cmd > v_max) {
    v_cmd = v_max;
    recompute_MPC(); // 用新约束重新求解
}

4.3 定位噪声影响

实测数据表明，当定位误差σ>0.03m时：

1. 增加状态观测器（如EKF）
2. 在MPC代价函数中加入平滑项：

code复制J += ρ * ||x_k - x_{k-1}||^2

ρ建议取值0.01-0.1

5. 进阶优化方向

1. 自适应预测时域：

python复制horizon = max(5, int(1.5 * curvature))

2. 多速率控制架构：

• 100Hz执行MPC计算
• 500Hz底层电机控制

3. 机器学习辅助调参：
   用强化学习自动优化Q,R,S矩阵

实验数据对比（圆形轨迹跟踪）：

这个项目让我深刻体会到：MPC的调试是个系统工程，需要同时关注模型精度、求解效率、实时补偿等多个维度。建议新手先从直线轨迹调试开始，逐步增加路径复杂度。在实际部署时，一定要预留足够的计算余量——我曾遇到因后台日志写入导致控制周期波动，最终引发系统失稳的案例。