别再只用PID了！用Python+OSQP给差速小车做个MPC控制器（附完整代码）

用Python+OSQP实现差速机器人MPC控制：从理论到代码的完整指南

差速轮式机器人作为移动机器人领域的经典平台，其控制算法一直是研究热点。传统PID控制器虽然简单易用，但在复杂路径跟踪场景中往往力不从心。本文将带你用Python和OSQP求解器，从零构建一个完整的模型预测控制(MPC)系统，让你的机器人获得更智能的轨迹跟踪能力。

1. 为什么MPC比PID更适合移动机器人控制

在仓库AGV、服务机器人等实际应用中，差速轮机器人经常需要跟踪复杂路径。传统PID控制器存在三个明显局限：

1. 反应滞后：PID属于"事后纠正"型控制，只有当误差出现后才开始调整
2. 缺乏预见性：无法考虑未来路径变化对当前控制的影响
3. 约束处理困难：难以直接融入速度、加速度等物理限制

相比之下，MPC具有三大优势：

• 预测能力：基于模型预测未来多步状态，提前做出调整
• 约束友好：可自然处理速度、加速度等物理限制
• 优化驱动：通过求解优化问题得到控制量，而非依赖经验调参

# PID与MPC控制效果对比示例 import matplotlib.pyplot as plt # PID控制轨迹 pid_traj = [[0,0], [0.1,0.1], [0.2,0.19], [0.3,0.28], [0.4,0.36]] # MPC控制轨迹 mpc_traj = [[0,0], [0.1,0.1], [0.2,0.2], [0.3,0.3], [0.4,0.4]] plt.plot(*zip(*pid_traj), label='PID') plt.plot(*zip(*mpc_traj), label='MPC') plt.legend() plt.title("轨迹跟踪效果对比")

2. 差速机器人运动学建模与线性化

2.1 建立非线性运动学模型

差速轮机器人的运动学可以用以下方程描述：

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

其中：

• (x,y)为机器人位置
• θ为朝向角
• v为线速度
• ω为角速度

2.2 模型线性化处理

为便于优化求解，需要在参考轨迹点附近对模型进行线性化。使用一阶泰勒展开得到线性化模型：

import numpy as np def linearize_model(x_ref, u_ref): """在参考点线性化模型""" x_r, y_r, θ_r = x_ref v_r, ω_r = u_ref A = np.array([ [0, 0, -v_r*np.sin(θ_r)], [0, 0, v_r*np.cos(θ_r)], [0, 0, 0] ]) B = np.array([ [np.cos(θ_r), 0], [np.sin(θ_r), 0], [0, 1] ]) return A, B

提示：线性化只在参考点附近有效，因此MPC需要频繁重新线性化，这也是其实时计算量大的原因之一。

3. MPC预测方程构建与OSQP求解

3.1 离散化与预测方程

采用前向欧拉法离散化，得到预测方程：

X_{k+1} = (I + T*A)X_k + T*B*u_k + T*O

其中T为控制周期，O为偏移项。将多步预测方程组合得到：

def build_prediction_matrices(A, B, N): """构建预测矩阵""" n_states = A.shape[0] n_controls = B.shape[1] # 初始化预测矩阵 A_bar = np.zeros((N*n_states, n_states)) B_bar = np.zeros((N*n_states, N*n_controls)) for i in range(N): A_bar[i*n_states:(i+1)*n_states] = np.linalg.matrix_power(np.eye(n_states)+A, i+1) for j in range(i+1): B_bar[i*n_states:(i+1)*n_states, j*n_controls:(j+1)*n_controls] = \ np.linalg.matrix_power(np.eye(n_states)+A, i-j) @ B return A_bar, B_bar

3.2 OSQP求解器配置

OSQP是一个高效的二次规划求解器，特别适合MPC这类问题：

import osqp def setup_osqp_solver(Q, R, A_bar, B_bar, x0, u_min, u_max): """配置OSQP求解器""" # 构建二次项矩阵 H = B_bar.T @ Q @ B_bar + R # 构建一次项向量 f = (x0.T @ A_bar.T @ Q @ B_bar).flatten() # 构建约束矩阵 n_controls = R.shape[0] n_steps = n_controls // 2 # 控制量约束 A_con = np.eye(n_controls) l_con = np.tile(u_min, n_steps) u_con = np.tile(u_max, n_steps) # 创建OSQP问题 prob = osqp.OSQP() prob.setup(H, f, A_con, l_con, u_con, verbose=False) return prob

4. 完整MPC控制器实现与仿真

4.1 MPC控制循环框架

class MPController: def __init__(self, N=10, dt=0.1): self.N = N # 预测步长 self.dt = dt # 控制周期 # 权重矩阵 self.Q = np.diag([10, 10, 5]) # 状态误差权重 self.R = np.diag([0.1, 0.1]) # 控制量变化权重 # 控制约束 self.v_min, self.v_max = -0.5, 0.5 self.ω_min, self.ω_max = -1.0, 1.0 def update(self, x, x_ref, u_ref): """MPC控制更新""" # 线性化模型 A, B = linearize_model(x_ref[0], u_ref[0]) # 构建预测矩阵 A_bar, B_bar = build_prediction_matrices(A, B, self.N) # 设置OSQP求解器 prob = setup_osqp_solver( np.kron(np.eye(self.N), self.Q), np.kron(np.eye(self.N), self.R), A_bar, B_bar, x, [self.v_min, self.ω_min], [self.v_max, self.ω_max] ) # 求解 res = prob.solve() return res.x[:2] # 返回第一个控制量

4.2 仿真测试与结果分析

def simulate_mpc(): # 初始化 x = np.array([0, 0, 0]) # 初始状态 controller = MPController() # 参考轨迹 (圆形) theta = np.linspace(0, 2*np.pi, 100) ref_path = np.column_stack([np.cos(theta), np.sin(theta), theta]) # 仿真循环 actual_traj = [] for i in range(len(ref_path)-10): x_ref = ref_path[i:i+10] u_ref = np.ones((10,2)) * [0.5, 0.5] # 参考控制量 u = controller.update(x, x_ref, u_ref) x = simulate_robot(x, u, controller.dt) actual_traj.append(x.copy()) return np.array(actual_traj)

注意：实际应用中需要考虑计算延迟问题。当单次MPC计算时间超过控制周期时，需要采用异步计算或降低预测步长。

5. 进阶优化与工程实践技巧

5.1 参考轨迹生成策略

• 速度自适应采样：在曲率大的区域增加参考点密度
• 前视距离调节：根据速度动态调整预测时域
• 速度规划：结合路径曲率预先规划速度曲线

5.2 实时性能优化

5.3 常见问题排查

1. 控制器震荡

   • 检查权重矩阵Q和R的平衡性
   • 确认约束条件是否合理
   • 尝试减小控制周期

2. 跟踪滞后

   • 增加预测步长
   • 调整前视距离
   • 检查模型准确性

3. 求解失败

   • 确认问题是否可行(约束是否冲突)
   • 检查矩阵条件数
   • 尝试不同的初始猜测

# 热启动示例 def warm_start_solver(prob, prev_solution): """使用上次解作为初始猜测""" prob.warm_start(x=prev_solution) return prob.solve()

在实际项目中，MPC参数调优往往需要结合具体机器人动力学特性。一个实用的方法是先在仿真环境中验证基本性能，再逐步移植到真实机器人上测试。