1. 为什么选择CasADi做运动学建模与MPC控制第一次接触CasADi是在研究生课题里当时需要给移动机器人开发轨迹跟踪控制器。试过用MATLAB的Symbolic Math Toolbox做符号推导也折腾过Python的SymPy但遇到复杂约束优化问题时总感觉力不从心。直到导师推荐了CasADi这个神器才发现原来运动学建模和MPC实现可以这么优雅。CasADi最大的优势在于它把符号计算和数值优化无缝衔接。举个具体例子当我们需要给差分驱动机器人建模时传统方法要手动推导雅可比矩阵而CasADi能自动完成符号微分。更妙的是它内置了IPOPT、SQPMethod等求解器接口建模完成后直接就能求解非线性优化问题。实测下来用CasADi实现的MPC控制器比手写C版本开发效率提升至少3倍。这里分享一个实际对比数据在双轮差速小车轨迹跟踪场景中我用CasADi从零搭建完整MPC控制器只用了200行Python代码包含可视化而同样功能的C实现需要800行。更关键的是当需要调整模型参数时CasADi版本只需修改几行符号定义而传统方法要重新推导整个运动学方程。2. 差分小车运动学建模实战2.1 建立符号变量与状态方程先来看差分驱动机器人的经典运动学模型。假设小车宽度为2b左右轮速度分别为v_l和v_r那么系统状态可以表示为import casadi as ca # 定义符号变量 x ca.MX.sym(x) # X坐标 y ca.MX.sym(y) # Y坐标 theta ca.MX.sym(theta) # 航向角 v ca.MX.sym(v) # 线速度 omega ca.MX.sym(omega) # 角速度 # 输入变量 v_l ca.MX.sym(v_l) # 左轮速度 v_r ca.MX.sym(v_r) # 右轮速度 # 运动学方程 b 0.2 # 半轮距 dx v * ca.cos(theta) dy v * ca.sin(theta) dtheta omega v (v_r v_l) / 2 omega (v_r - v_l) / (2 * b)这个模型有几个关键点需要注意使用MX.sym创建的是符号变量不同于具体数值运动学方程保持连续时间形式后续离散化时会用到轮速到线速度/角速度的转换体现了差分驱动特性2.2 模型离散化处理实际控制器需要在离散时间步长下运行我们需要对连续模型做离散化。CasADi提供了多种数值积分方法这里采用最常用的欧拉法dt 0.1 # 时间步长100ms # 离散状态更新 x_next x dt * dx y_next y dt * dy theta_next theta dt * dtheta # 打包为状态更新函数 state ca.vertcat(x, y, theta) state_next ca.vertcat(x_next, y_next, theta_next) f_disc ca.Function(f_disc, [state, v_l, v_r], [state_next])测试这个离散模型时有个实用技巧可以用f_disc.mapaccum(N)方法快速模拟N步运动轨迹。比如想看看小车在恒定输入下的运动路径import numpy as np N 50 # 50步模拟 inputs np.tile([1.0, 0.8], (N,1)) # 恒定左右轮速 initial_state [0, 0, 0] trajectory f_disc.mapaccum(N)(initial_state, inputs.T)3. MPC控制器设计与实现3.1 构建优化问题框架MPC的核心是在每个控制周期求解一个有限时域的优化问题。我们需要定义预测时域N10即预测未来1秒状态代价跟踪参考轨迹的偏差输入代价控制量的变化率惩罚opti ca.Opti() # 创建优化问题 # 决策变量预测时域内的状态和输入 X opti.variable(3, N1) # 状态序列 U opti.variable(2, N) # 输入序列 # 初始状态约束 opti.subject_to(X[:,0] x_current) # 动态约束 for k in range(N): opti.subject_to(X[:,k1] f_disc(X[:,k], U[0,k], U[1,k])) # 输入约束 opti.subject_to(opti.bounded(-2, U[0,:], 2)) # 左轮速限制 opti.subject_to(opti.bounded(-2, U[1,:], 2)) # 右轮速限制3.2 设计目标函数目标函数要平衡轨迹跟踪精度和控制平滑性。这里采用二次型代价# 参考轨迹 X_ref np.array(...) # 形状(3,N1) # 状态代价权重矩阵 Q np.diag([10, 10, 5]) # 输入变化率代价 R np.diag([0.1, 0.1]) # 构建目标函数 cost 0 for k in range(N1): err X[:,k] - X_ref[:,k] cost err.T Q err for k in range(N-1): du U[:,k1] - U[:,k] cost du.T R du opti.minimize(cost)实际调试中发现角度的误差权重需要特别处理。因为航向角theta是周期性的直接做差可能得到不合理的大误差如π和-π实际上是相同角度。解决方案是使用角度差归一化def angle_diff(a, b): return ca.atan2(ca.sin(a-b), ca.cos(a-b)) err_theta angle_diff(X[2,k], X_ref[2,k])4. 仿真分析与性能调优4.1 基础性能测试搭建完控制器后我设计了三种测试场景直线跟踪参考轨迹为x轴方向匀速运动圆形轨迹半径2m的圆周运动八字形轨迹更复杂的路径变化# 仿真循环 for _ in range(100): sol opti.solve() u_opt sol.value(U[:,0]) x_current f_disc(x_current, u_opt[0], u_opt[1]) # 记录状态并更新参考轨迹实测数据显示在直线和圆形轨迹下位置跟踪误差能控制在5cm以内。但八字形轨迹转弯处会出现最大约12cm的跟踪误差这引出了下一个优化点。4.2 提升转弯性能的技巧分析发现转弯性能下降的主要原因是预测时域不够长无法预见急转弯角速度约束太保守改进措施包括动态调整预测时域转弯时增加N引入前馈控制项补偿模型误差松弛角速度约束修改后的控制器在转弯处误差降低到8cm以内同时保持直线段的稳定性。这里有个重要经验MPC的预测时域不是越长越好过长的时域会导致计算延迟需要根据运动特性动态调整。5. 工程实践中的注意事项在实际部署时有几个容易踩坑的地方值得分享求解器选择IPOPT适合精度要求高的场景但计算较慢SQPMethod速度更快但可能陷入局部最优。移动机器人上我最终选择了SQPMethod配合warm start技巧。实时性保障在树莓派上运行时发现当预测时域N15时单次求解可能超过100ms。解决方案是使用opti.solver(ipopt, {print_time:0})关闭求解器输出采用热启动复用上一帧的解作为初始猜测数值稳定性遇到过因为角度未归一化导致雅可比矩阵计算失败的情况。现在会在每次状态更新后执行theta np.arctan2(np.sin(theta), np.cos(theta))可视化调试用Matplotlib实时绘制预测轨迹和实际轨迹能快速定位问题。推荐保存每次求解的预测序列这是调试MPC的黄金资料。