用CasADi和Python搞定差分小车MPC控制：从运动学建模到仿真避坑全流程

用CasADi和Python实现差分小车MPC控制：从零构建到仿真优化的完整指南

引言

在机器人控制领域，模型预测控制（MPC）因其出色的处理多变量约束和优化未来行为的能力而备受青睐。CasADi作为一个强大的符号计算框架，为MPC的实现提供了高效便捷的工具链。本文将带您从零开始，使用Python和CasADi构建一个完整的差分驱动机器人MPC控制器。

无论您是控制工程的学生还是机器人爱好者，本指南都将帮助您：

1. 理解差分驱动机器人的运动学建模原理
2. 掌握CasADi在MPC问题构建中的应用技巧
3. 学会如何将数学模型转化为可执行的Python代码
4. 规避实际实现中的常见陷阱和性能瓶颈

我们将采用循序渐进的方式，从基础理论到代码实现，最后通过仿真验证控制效果。所有代码示例都经过精心设计，可直接运行和修改，为您提供一个真正"开箱即用"的学习体验。

1. 差分驱动机器人运动学建模

1.1 坐标系与状态定义

差分驱动机器人（也称为差速驱动小车）是移动机器人中最常见的构型之一。要描述其在平面中的运动，我们需要定义以下状态变量：

• 全局坐标系状态：
  • x：机器人在全局坐标系下的x坐标
  • y：机器人在全局坐标系下的y坐标
  • θ：机器人前进方向与全局x轴的夹角（航向角）

这三个变量构成了机器人的状态向量：X = [x, y, θ]ᵀ

1.2 控制输入与运动方程

差分驱动机器人通过两个独立驱动的轮子实现运动，其控制输入通常为：

• v：前进速度（线速度）
• ω：转向角速度

基于刚体运动学，我们可以推导出连续时间下的运动学方程：

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

或者用矩阵形式表示为：

[ẋ] [cos(θ) 0][v] [ẏ] = [sin(θ) 0][ω] [θ̇] [0 1]

1.3 离散化模型

为了便于数字控制器实现，我们需要将连续时间模型离散化。采用前向欧拉法，离散时间步长为ΔT时，离散状态更新方程为：

x(k+1) = x(k) + ΔT * v(k) * cos(θ(k)) y(k+1) = y(k) + ΔT * v(k) * sin(θ(k)) θ(k+1) = θ(k) + ΔT * ω(k)

提示：离散化方法的选择会影响控制精度。对于高速运动场景，可考虑更高阶的离散化方法如Runge-Kutta。

2. MPC问题构建与优化目标

2.1 模型预测控制基本原理

MPC的核心思想是在每个控制周期：

1. 基于当前状态和系统模型，预测未来N步的系统行为
2. 求解一个优化问题，得到最优控制序列
3. 只执行第一步控制，然后在下一个周期重复整个过程

这种"滚动时域"策略使MPC能够及时响应系统变化和干扰。

2.2 代价函数设计

一个典型的MPC代价函数包含以下部分：

• 状态偏差惩罚：驱使系统状态趋近参考值
• 控制量惩罚：避免过大的控制输入
• 终端代价：确保预测时域末端接近目标

数学表达式为：

min J = Σ [ (x(k)-x_ref)ᵀQ(x(k)-x_ref) + u(k)ᵀRu(k) ] + (x(N)-x_ref)ᵀP(x(N)-x_ref)

其中Q、R、P为权重矩阵，需要根据控制需求调整。

2.3 约束条件处理

实际系统中，我们需要考虑各种约束：

• 状态约束：如工作空间限制
• 控制约束：如电机速度/扭矩限制
• 动力学约束：由系统物理特性决定

在CasADi中，这些约束可以方便地表示为不等式或等式约束。

3. CasADi实现详解

3.1 CasADi基础与符号系统

CasADi提供了两种主要的符号变量类型：

1. SX：标量符号，适合小型问题
2. MX：矩阵符号，适合大型问题

对于我们的差分小车MPC，使用SX即可满足需求。

import casadi as ca # 定义状态变量 x = ca.SX.sym('x') y = ca.SX.sym('y') theta = ca.SX.sym('theta') states = ca.vertcat(x, y, theta) n_states = states.size()[0] # 定义控制变量 v = ca.SX.sym('v') omega = ca.SX.sym('omega') controls = ca.vertcat(v, omega) n_controls = controls.size()[0]

3.2 运动学模型的符号表达

将离散运动学方程转化为CasADi函数：

# 右端项(RHS)方程 rhs = ca.vertcat(v*ca.cos(theta), v*ca.sin(theta), omega) # 创建运动学函数 f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])

3.3 MPC问题构建

采用Single Shooting方法构建MPC问题：

# 定义优化变量 U = ca.SX.sym('U', n_controls, N) # N步控制序列 X = ca.SX.sym('X', n_states, N+1) # N+1步状态序列 P = ca.SX.sym('P', 2*n_states) # 参数(初始状态和目标状态) # 初始状态约束 X[:, 0] = P[:3] # 构建状态预测 for i in range(N): f_value = f(X[:, i], U[:, i]) X[:, i+1] = X[:, i] + f_value * T # 创建预测函数 ff = ca.Function('ff', [U, P], [X], ['input_U', 'target_state'], ['horizon_states'])

3.4 优化求解器配置

# 权重矩阵 Q = np.diag([1.0, 5.0, 0.1]) # 状态权重 R = np.diag([0.5, 0.05]) # 控制权重 # 构建目标函数 obj = 0 for i in range(N): obj += ca.mtimes([(X[:,i]-P[3:]).T, Q, X[:,i]-P[3:]]) + ca.mtimes([U[:,i].T, R, U[:,i]]) # 定义约束 g = [] for i in range(N+1): g.append(X[0, i]) # x约束 g.append(X[1, i]) # y约束 # NLP问题定义 nlp_prob = { 'f': obj, 'x': ca.reshape(U, -1, 1), 'p': P, 'g': ca.vertcat(*g) } # IPOPT求解器配置 opts = { 'ipopt.max_iter': 100, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.acceptable_tol': 1e-8, 'ipopt.acceptable_obj_change_tol': 1e-6 } # 创建求解器 solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts)

4. 仿真实现与性能优化

4.1 仿真主循环

# 约束条件定义 lbx = [] ubx = [] for _ in range(N): lbx.append(-v_max) ubx.append(v_max) lbx.append(-omega_max) ubx.append(omega_max) # 仿真初始化 x0 = np.array([0, 0, 0]) # 初始状态 xs = np.array([1.5, 1.5, 0]) # 目标状态 u0 = np.zeros((N, 2)) # 初始控制猜测 # 主循环 while np.linalg.norm(x0 - xs) > 1e-2: # 设置参数 c_p = np.concatenate((x0, xs)) # 求解优化问题 res = solver( x0=np.reshape(u0, (-1, 1)), p=c_p, lbg=[-2]*2*(N+1), # x,y约束 ubg=[2]*2*(N+1), lbx=lbx, ubx=ubx ) # 获取最优控制 u_sol = np.reshape(res['x'], (n_controls, N)) # 状态预测 ff_value = ff(u_sol, c_p) # 应用控制并更新状态 x0 = x0 + T * f(x0, u_sol[:, 0]).full().flatten() u0 = np.hstack((u_sol[:, 1:], u_sol[:, -1:]))

4.2 常见问题与调试技巧

在实际实现中，您可能会遇到以下典型问题：

1. 维度不匹配错误：

   • 确保所有向量和矩阵的维度一致
   • 特别注意CasADi中列向量和行向量的区别

2. 求解器收敛困难：

   • 检查约束是否合理
   • 尝试调整IPOPT参数
   • 提供更好的初始猜测

3. 性能瓶颈：

   • 减少预测时域N
   • 尝试MX符号而不是SX
   • 考虑使用Multi-Shooting方法

4.3 仿真结果分析

成功实现后，您应该能看到机器人从起点平滑运动到目标点。典型性能指标包括：

• 计算时间：单次优化求解耗时
• 控制精度：最终位置误差
• 能量消耗：控制输入的积分

通过调整权重矩阵Q、R和预测时域N，可以平衡这些指标。

5. 进阶优化与扩展

5.1 Multi-Shooting方法实现

与Single Shooting相比，Multi-Shooting将状态也作为优化变量，通常能提供更好的数值稳定性：

# Multi-Shooting变量定义 X = ca.SX.sym('X', n_states, N+1) U = ca.SX.sym('U', n_controls, N) # 添加连续性约束 g = [] for i in range(N): x_next = X[:,i] + T*f(X[:,i], U[:,i]) g.append(X[:,i+1] - x_next) # 连续性约束

5.2 实时性能优化技巧

1. 热启动：使用上一周期的解作为当前优化的初始猜测
2. 代码生成：将CasADi问题编译为C代码加速求解
3. 并行计算：利用多核处理器并行计算梯度

5.3 实际系统集成考虑

将MPC控制器部署到真实机器人时，还需考虑：

• 状态估计：融合传感器数据获取准确状态
• 时延补偿：处理计算和通信延迟
• 鲁棒性设计：应对模型不确定性和干扰

6. 完整代码示例与资源

为方便读者实践，我们提供了一个完整的Python实现，包含以下功能：

• 可配置的MPC参数
• 实时可视化
• 性能统计

# 差分小车MPC完整实现 import numpy as np import casadi as ca import matplotlib.pyplot as plt import time class DifferentialDriveMPC: def __init__(self, T=0.2, N=10, Q=None, R=None): # 参数初始化 self.T = T # 采样时间 self.N = N # 预测时域 # 默认权重矩阵 self.Q = np.diag([1.0, 5.0, 0.1]) if Q is None else Q self.R = np.diag([0.5, 0.05]) if R is None else R # 物理约束 self.v_max = 0.6 self.omega_max = np.pi/4.0 self.rob_diam = 0.3 # 构建MPC问题 self.build_mpc() def build_mpc(self): # 状态和控制变量定义 x = ca.SX.sym('x'); y = ca.SX.sym('y'); theta = ca.SX.sym('theta') self.states = ca.vertcat(x, y, theta) self.n_states = self.states.size()[0] v = ca.SX.sym('v'); omega = ca.SX.sym('omega') self.controls = ca.vertcat(v, omega) self.n_controls = self.controls.size()[0] # 运动学模型 rhs = ca.vertcat(v*ca.cos(theta), v*ca.sin(theta), omega) self.f = ca.Function('f', [self.states, self.controls], [rhs]) # 优化变量 U = ca.SX.sym('U', self.n_controls, self.N) X = ca.SX.sym('X', self.n_states, self.N+1) P = ca.SX.sym('P', 2*self.n_states) # 状态预测 X[:,0] = P[:3] for i in range(self.N): f_value = self.f(X[:,i], U[:,i]) X[:,i+1] = X[:,i] + self.T*f_value # 目标函数 obj = 0 for i in range(self.N): obj += ca.mtimes([(X[:,i]-P[3:]).T, self.Q, X[:,i]-P[3:]]) \ + ca.mtimes([U[:,i].T, self.R, U[:,i]]) # 约束 g = [] for i in range(self.N+1): g.append(X[0,i]) # x约束 g.append(X[1,i]) # y约束 # NLP问题 nlp_prob = { 'f': obj, 'x': ca.reshape(U, -1, 1), 'p': P, 'g': ca.vertcat(*g) } # 求解器选项 opts = { 'ipopt.max_iter': 100, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.acceptable_tol': 1e-8, 'ipopt.acceptable_obj_change_tol': 1e-6 } self.solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts) # 约束边界 self.lbg = [-2, -2]*(self.N+1) # 状态约束 self.ubg = [2, 2]*(self.N+1) self.lbx = [] self.ubx = [] for _ in range(self.N): self.lbx += [-self.v_max, -self.omega_max] self.ubx += [self.v_max, self.omega_max] def solve(self, x0, xs): # 初始控制猜测 u0 = np.zeros((self.N, 2)) # 设置参数 c_p = np.concatenate((x0, xs)) # 求解 res = self.solver( x0=np.reshape(u0, (-1, 1)), p=c_p, lbg=self.lbg, ubg=self.ubg, lbx=self.lbx, ubx=self.ubx ) # 获取解 u_sol = np.reshape(res['x'], (self.n_controls, self.N)) return u_sol[:,0] # 返回第一步控制 # 使用示例 mpc = DifferentialDriveMPC(N=10) x_current = np.array([0, 0, 0]) x_target = np.array([1.5, 1.5, 0]) # 仿真循环 trajectory = [x_current.copy()] for _ in range(100): u_opt = mpc.solve(x_current, x_target) x_current += mpc.T * mpc.f(x_current, u_opt).full().flatten() trajectory.append(x_current.copy()) if np.linalg.norm(x_current - x_target) < 0.01: break # 可视化 trajectory = np.array(trajectory) plt.figure() plt.plot(trajectory[:,0], trajectory[:,1], 'b-') plt.plot(x_target[0], x_target[1], 'ro') plt.axis('equal') plt.xlabel('x'); plt.ylabel('y') plt.title('MPC轨迹跟踪') plt.show()

7. 学习资源与后续方向

7.1 推荐学习资料

1. CasADi官方文档：最权威的API参考和教程
2. MPC理论教材：
   • Model Predictive Controlby Eduardo F. Camacho
   • Predictive Control for Linear and Hybrid Systemsby Borrelli et al.
3. 开源项目：
   • ACADO Toolkit
   • do-mpc

7.2 扩展应用方向

掌握了基础MPC实现后，您可以进一步探索：

1. 路径跟踪：跟随预定义的轨迹而非固定点
2. 避障控制：在代价函数中加入障碍物约束
3. 多机器人协同：协调多个机器人的运动
4. 非线性MPC：处理更复杂的动力学模型

在实际项目中，我发现最关键的调参经验是：先确定合理的状态权重Q，然后逐步调整控制权重R，直到获得满意的响应速度和稳定性平衡。预测时域N的选择需要权衡计算负担和控制性能，通常5-20步是一个实用的范围。