CasADi学习系列写到第三篇很多人会卡在一个微妙的阶段前面的符号变量、简单NLP都能跑但回到自己的项目里突然不知道该怎么把动力学方程、目标函数和约束完整地交给它。我一开始也是这样看文档里的例子总觉得每个都懂组合起来就发懵。后来反复试了几周真正想明白的其实不是某个求解器参数而是建模的思维顺序——先选对数据类型再把表达式装进Function最后才谈求解。这篇就顺着这个思路从SX/MX/DM的选择开始到一个完整的非线性模型预测控制问题收尾把过程里的坑一并说清楚。这篇的主角是CasADi一个用于非线性优化和算法微分的建模框架。适合已经写完入门例子、准备用CasADi解决真实最优控制和运动规划问题的读者。如果你现在卡在“能跑通Demo但不会写自己问题”的状态这篇应该能帮你打通那道墙。1. 从符号变量到真正求解第三篇要解决的核心断层1.1 前两篇通常停在哪儿大多数CasADi学习路线的开头都很一致装好库用SX.sym造几个符号写一个标量函数算算导数或者丢一个带不等式约束的小NLP到Opti里跑通。学完这些你会觉得工具本身并不难难的是把自己实际问题里的矩阵、向量、隐式约束全部翻译成CasADi能理解的表达式图。这个坎比想象中普遍。我观察到的典型现象是看Opti栈的Variable、Constraint、solver这些API都很眼熟但真到自己建模时经常会纠结变量到底是横着排还是竖着排状态转移该写欧拉还是梯形约束应该写成等号还是不等号目标函数里的加权系数该给多少。这些细节在上手阶段不会被提及因为官方文档默认你已经具备优化建模的基础知识。实际项目中另一个常见误区是把CasADi当成求解器本身。CasADi的核心定位是“问题描述自动微分”框架它帮你把优化问题的目标、约束、导数关系构建出来最终数值求解交给IPOPT、SQP这类优化器。理解这一点很关键否则你会期望它自己搞定所有数值细节一旦发散就会误以为是工具不行。1.2 第三篇怎么升级我给第三篇定的主线是从“会写表达式”升级到“能描述并求解一个完整的工程问题”。具体拆成三个能力块。第一能根据问题规模正确选择SX、MX、DM三种数据类型因为选错直接决定内存占用和编译速度。第二能把自己的动力系统封装成Function对象并利用它做自动微分这是后续所有优化的基础。第三能把连续时间的模型离散化后交给优化求解器并顺利调到一个收敛的解。这一篇会用一个小型倒立摆NMPC非线性模型预测控制来串起全部内容。倒立摆规模小、非线性强、不稳定是练习最优控制建模最合适的试验台之一。把它跑通再去处理机械臂、移动机器人、车辆横摆问题思路完全一样。2. SX、MX、DM三种数据类型选错类型等于白做模型2.1 DM到底做什么用很多初学者很容易把DM和SX搞混因为构造DM的代码看着和numpy太像。DM是纯数值矩阵容器里面只能存具体的数不能存符号变量。它的作用主要有两个一是给优化问题提供初值、参数二是从求解结果中取出数值。比如我们用DM([[0.2, 0.0]]) 定义初始状态用DM去设置某个约束的右端项这些操作都不会参与符号求导。注意不要想着用DM来参与符号运算。一次真实项目里我见过有人试图在DM矩阵中加入SX表达式结果代码直接报类型错误。DM、SX、MX在CasADi内部是三条不同的数据结构互相之间能转换但混用会打断表达式图的自动微分链。2.2 SX小规模问题的最优选择SX是CasADi最基础的符号类型。它的特点是展开到每一个标量元素整个表达式图是“扁平化”的。SX.sym(x, 5) 生成长度为5的符号向量每个分量都对应一个符号标量节点。这种设计的优点是小规模表达式求导很快因为每个标量之间的依赖关系非常直接编译器能优化得很死。对于变量数量在几十个以下、函数表达式不算太复杂的问题我建议直接用SX。比如单个机械臂的逆运动学、小型参数辨识、或者像本文这个倒立摆NMPCSX通常就够用而且速度比MX快。很多人一上来就喜欢用MX觉得它是“高级版”其实在小问题上反而多了一层间接性。2.3 MX大规模结构化问题的骨架MX是矩阵级别的符号节点。和SX最大的区别在于MX把整个矩阵运算当成一个节点来保存而不是把内部每个标量都展开。这意味着什么以最优控制为例你在N个时间步里反复复用同一段动力学表达式如果用SX每次复用都会把表达式复制一份展开表达式图会迅速膨胀如果用MX同一个动力学矩阵运算节点可以被多个时间步共享引用内存占用和化简压力差别非常大。MX的优势在模型预测控制里尤其明显。状态维度几十维、控制步长上百步时SX的展开图会非常臃肿求解器在求导和稀疏模式提取阶段就会被拖慢。但MX也不是万能的有些元素级操作在MX上的支持不如SX灵活个别运算符还会出现性能回退。2.4 我的选型经验我自己的经验可以总结成一句话两个变量以内或者几十维小问题优先SX高维、长时域、重复结构多的大规模问题优先MX。如果你不确定可以先按小规模逻辑用SX建模跑顺之后再换MX对比一下内存和耗时。直接在项目里无脑用MX有时候反而会让你中间的代码更绕。这里再补充一个对比表格方便新手快速定位类型本质适用场景性能特点DM纯数值容器初值、参数、结果提取无法参与符号求导SX标量符号展开图小型代数表达式、低维NLP小问题下求导快内存占用低MX矩阵符号节点大型NLP、MPC、复杂动力学节点复用省内存高维问题优势明显初学者最容易犯的错误是试图在DM上做符号优化或者在SX里表达一个上千维的稀疏矩阵。这两个方向都会在后续调试里浪费大量时间。3. Function对象把表达式打包成可部署的模块3.1 基本创建方式CasADi里Function是一个非常重要的抽象。它本质上是在说给这些输入经过一段符号表达式计算给出这些输出。你可以把Function理解成一个可以被反复调用、也可以被反复求导的“函数模块”。定义好一次后面无论做仿真、求雅可比还是生成代码都用这个统一入口。举个例子倒立摆的动力学方程可以这样封装import casadi as ca g 9.81 L 1.0 m 1.0 x ca.SX.sym(x, 2) # [theta, omega] u ca.SX.sym(u) # 控制力矩 xdot ca.vertcat( x[1], g / L * ca.sin(x[0]) u / (m * L * L) ) f ca.Function(f, [x, u], [xdot], [x, u], [xdot])这段代码里x是二维向量u是标量。动力学方程用vertcat拼成一个两行的向量表达式然后通过Function定义输入输出名。以后我们想算某个状态下对应的导数直接调用f(x_val, u_val)就能拿到数值。3.2 利用Function做自动微分在日常工程任务中光有数值求值是不够的优化器需要目标函数和约束对决策变量的导数。CasADi的杀手级功能就是自动微分你不需要手推任何导数公式它能根据符号表达式直接生成雅可比和黑森矩阵。使用方式也很直接J ca.jacobian(xdot, x) dfdx ca.Function(dfdx, [x, u], [J])这样dfdx就是一个输入(x,u)、输出雅可比矩阵的Function。对小规模问题直接这样写就够了。如果你在用IPOPT它需要的雅可比稀疏结构也会由CasADi自动生成不需要像手写求解器接口那样一个个标注非零元位置。这个特性在模型复杂之后尤其珍贵我自己早期手写过雅可比那个痛苦至今记得。对于二阶信息比如处理非线性规划时需要用拉格朗日函数的黑森矩阵可以用ca.hessian(expr, x)或者配合ca.derivative来实现。初学阶段不用把每个函数都背下来只要理解“表达式一旦建立导数就免费”的原则就够了。3.3 稀疏性对优化性能的影响提到雅可比就绕不开稀疏性。真实工程模型里任何一个变量通常只影响局部几个方程所以雅可比矩阵中绝大多数元素是0。如果优化器把整个稠密矩阵都计算一遍计算量会随着状态维度的增加呈平方甚至立方级增长很多NMPC根本等不及。CasADi在构建表达式图时就会记录非零元的传播路径自动推导出稀疏模式。我们在写Problem时不需要手动标记稀疏位置但心里要清楚你的表达式写得越规整稀疏模式越好IPOPT求导和线性代数就越快。比如在循环里逐时间步写约束比用一个巨大矩阵硬算要清晰得多这也让最终生成的雅可比稀疏结构一眼就能看出规律。很多人忽略的一点是Function不只是为了好看它还能复用。你定义好一次f所有后续需要在多个时间步使用同一段动力学的场合都可以直接引用这个函数而不必每次复制表达式。这就是从玩具代码走向工程代码的起点。4. 实战用梯形法离散化一个倒立摆NMPC问题4.1 为什么选倒立摆作为测试床选择倒立摆有三个原因。第一它非线性足够强sin(theta)项是NMPC区别于线性MPC的天然素材能逼你认真处理离散化方式。第二它不稳定但可控能让预测控制的价值体现得非常直观。第三这个模型很小即使用笔记本也能在几秒内完成求解适合学习阶段快速迭代。这个问题的描述是状态为角度theta和角速度omega控制量为作用于转轴处的力矩u采样周期0.05秒预测步数30步。目标是让系统从初始角度0.2rad回到0同时角速度归零控制量有界。4.2 用Opti栈组织优化变量CasADi提供两套求解问题的方式一种是手动把决策变量拼成一个大符号向量再通过索引拆开另一种是用Opti栈它更接近“数学语言”变量定义更清晰。我强烈建议初学者直接用Opti栈因为调试时不用操心索引映射。Opti栈的核心思路是先创建优化对象然后声明变量再逐条添加约束和目标opti ca.Opti() N 30 dt 0.05 nx 2 nu 1 x opti.variable(nx, N 1) # 每一列是一个时间步的状态 u opti.variable(nu, N) # 每一列是一个时间步的控制这里x是一个2行31列的矩阵第0列是初始状态第30列是终端状态。u是1行30列。矩阵布局一旦确定后面写约束时思路就非常清楚。4.3 梯形法隐式离散连续动力学方程离散化有很多方式最简单的是欧拉法。但欧拉法对采样时间和系统刚性的容忍度一般在倒立摆这类不稳定的模型上采样时间稍大一点就会出现明显的数值偏差。这次我用梯形法格式是x_{k1} x_k (dt / 2) * (f(x_k, u_k) f(x_{k1}, u_k))注意右边出现了x_{k1}这是一个隐式格式。隐式的好处是数值稳定性更好代价是多了一层耦合我们在写约束时要把等式关系表达完整而不是简单的前向代入。在Opti里写梯形法很直观omega x[1, :] theta x[0, :] torque u[0, :] # 初始状态约束 opti.subject_to(x[:, 0] ca.DM([0.2, 0.0])) # 控制约束 opti.subject_to(opti.bounded(-2.0, torque, 2.0)) # 动力学约束梯形法 for k in range(N): f_k ca.vertcat( omega[k], g / L * ca.sin(theta[k]) torque[k] / (m * L * L) ) f_kp1 ca.vertcat( omega[k 1], g / L * ca.sin(theta[k 1]) torque[k] / (m * L * L) ) opti.subject_to( x[:, k 1] - x[:, k] - (dt / 2) * (f_k f_kp1) 0 )这里第k步约束里同时包含theta[k]、theta[k1]、omega[k]、omega[k1]形成了相邻两步之间的隐式关系。对方程的雅可比矩阵来说这会引入额外的非零模式但IPOPT处理这种结构毫无压力反而因为隐式格式带来的稳定性让求解更稳健。4.4 目标函数设计与权重依据目标函数我设计成终端状态惩罚加控制能量惩罚# 终端角度和角速度尽可能靠近零 terminal_cost 50 * ca.sumsqr(theta[:, N]) 10 * ca.sumsqr(omega[:, N]) # 控制能量尽量小 control_cost 0.1 * ca.sumsqr(torque) opti.minimize(terminal_cost control_cost)权重为什么是50、10、0.1而不是全部取1因为目标函数中的量纲不同。角度单位是rad数值一般在零点几附近角速度单位是rad/s动态过程中会出现一个短暂波动控制力矩可以冲到2左右。如果不区分权重优化器会优先压那个数值最大的项导致别的项“失宠”。关键是终端角度项倒立摆回到0时theta必须精确归零这直接影响任务成败所以给了50的大权重角速度不需要像角度那样苛刻给10控制能量是次要项给0.1起一个平滑作用即可。实际工程中权重就是这样一个反复试的过程不存在万能配置。4.5 初值猜测与求解求解前需要设置初值。很多人直接设成全0这是最省事但未必最好的做法。我这里先给状态一个简单的线性递减猜测让theta从0.2均匀过渡到0theta_guess np.linspace(0.2, 0.0, N 1) omega_guess np.zeros(N 1) opti.set_initial(x, np.vstack([theta_guess, omega_guess])) opti.set_initial(u, np.zeros((1, N))) opti.solver(ipopt) sol opti.solve() x_sol sol.value(x) u_sol sol.value(u)求解完成后用sol.value取出数值结果画出来就能看到角度从0.2rad平滑回到0角速度先朝负方向变化再回到0控制量在约束边界附近短暂停留后回到平衡。这是一个典型的预测控制特征控制器会“提前”规划好减速路径而不是等误差发生了才反应。一个小提醒如果在你的电脑上IPOPT报出“不可行问题”或者迭代卡在某个可行域边界大多数情况下不是模型写错而是初值给得太随意或者控制边界太紧。遇到这种情况先放宽控制约束跑一次拿到一个可行轨迹再把这个轨迹当作初值回到原问题求解。这个方法我后面还会提到。5. 从发散到收敛NLP求解的数值细节5.1 量纲不一致是隐形的坑我在实际调试中遇到的第一类问题就是量纲。有时候状态变量包含角度、角速度、位移控制变量用的是牛·米或牛顿它们的数值量级可能差出好几个数量级。IPOPT内部虽然会做尺度化处理但尺度选择并不总是理想模型的天然尺度太差时求解器很容易进入数值困难。解决思路有两种。一种是在目标函数里调整权重让各项模型量级匹配这是入门阶段最常见的做法。另一种更彻底的做法是对决策变量做归一化比如定义新的变量theta_scaled theta / 0.5让系统在优化时里面每个变量都在相近范围。归一化在大型项目中几乎是必须的但作为初学者先在目标函数里做权重配平就够了。5.2 初值猜测不是小事IPOPT属于内点法它天然需要一个可行的起始点。如果从一个严重违反动力学约束的初值出发求解器会先花大量迭代在恢复可行性上甚至在某些情况下直接宣告失败。我看到太多人因为初值设成全零结果IPOPT退出说“problem is infeasible”然后开始怀疑模型写错了其实模型没问题只是起跑点太差。给初值的好办法是先用简单的欧拉或梯形法做一次开环仿真。比如保持控制为0从初始状态前向积分N步得到一条开环轨迹直接作为优化初猜。这条轨迹可能不满足最终任务目标但它至少满足动力学关系IPOPT起点在可行域附近收敛速度快很多。对于正在做实时MPC的人有一个实用技巧把上一采样周期的解平移一下作为当前采样周期的初值。上一周期的最优轨迹去掉开头一小段再延伸一点本质上是一个很好的热启动。加上这个技巧PID之外的预测控制在很多嵌入式场景里才真正跑得动。5.3 调参与逻辑判断如果求解器从发散到收敛依然不顺利我会按这个顺序排查。第一步放宽所有控制边界和状态约束只保留动力学与终端一个极弱目标看能不能求到一个动力学可行轨迹。第二步在满足动力学的基础上去掉终端约束延长预测时域看能不能找到一个把状态拉回来的解。第三步逐步增加约束、调高权重每次只改一个参数保持其余配置不变。这里有一个很容易忽略的实践原则一次只改变一个变量。很多人调参时同时改权重、改时域、改边界一旦结果变好根本不知道是哪一个改动起了作用一旦变差也不知道该怎么回滚。学会用控制变量的方式调参比所谓的调参经验重要得多。6. 从仿真到实时代码生成与性能优化6.1 为什么需要代码生成如果你只是做一个离线轨迹规划Python环境里的CasADi已经足够。但在真实机器人系统里控制周期往往是10毫秒甚至1毫秒你不可能在每一次控制周期都启动一个Python解释器去解析表达式图并求解优化问题。这时候就要用到CasADi的代码生成能力。代码生成的做法是把你已经定义好的Function转换成一段C代码。这段C代码不再需要CasADi符号引擎的运行时来求值它直接按照计算图展开为具体的数值计算路径。也就是说模型每毫秒只执行一段硬编码的数值计算速度可以比单纯Python调用快一两个数量级。6.2 生成代码的思路对一个普通的动力学函数f生成C代码的入口很简单f.generate(f.c)它会输出一个C文件函数名就是你在Function构造时给的名字。这个文件包含了从输入到输出的全部计算过程。如果你的Function内部只包含纯数学运算那么生成出来的代码是稳定可靠的。如果你的目标是整个NMPC的代码生成思路也是一样的把“给定当前状态和参考轨迹求最优控制序列”这个过程封成一个Function然后对外只暴露输入输出接口。生成的C文件中会嵌入雅可比计算和求解器相关逻辑集成到上游控制栈时只需要包含头文件并调用接口函数即可。6.3 我踩过的代码生成坑我之前在一个项目里试着把一个MPC模型封装成Function然后代码生成结果生成的C代码里出现了一大堆乱七八糟的数字和逻辑跳转难以阅读调试。后来发现问题出在我使用了带逻辑分支的表达式比如条件判断、取最大最小这类不光滑操作。这类操作在符号求值时没问题但代码生成阶段会把分支结构展开得极其丑陋性能反而不如写死的平台代码。所以我现在有一个原则凡是准备做代码生成的模型从一开始就尽量只使用光滑的数学运算。如果要实现限制幅值用饱和函数的平滑近似如果要实现状态依赖的切换逻辑重新建模思路而不是在表达式里硬编码if。细节上会麻烦一点但换来的稳定性和生成效率完全值得。另外代码生成只是把单次求值速度提上去它没有改变问题本身的复杂度。如果你的模型本来就是一个很难收敛的NLP生成C代码后该发散还是会发散。代码生成解决的是“计算速度”不是“优化收敛性”这两个问题一定要分开对待。个人走过这一段之后最大的体会是CasADi真正的价值不在于它是一个什么神秘的框架而在于它把优化问题建模和自动微分这两件事做得特别统一。你只要把自己的模型按照规范的思路组织好——选对数据类型、封装成Function、用清晰的离散格式表达动力学剩下的求导、稀疏结构、求解器对接都可以交给工具。而在实际项目里真正决定成败的往往是那些看起来不起眼的选择初值给得够不够好权重配得是否合理离散化够不够稳。希望这篇能帮准备上手和正在卡壳的人减少一些弯路。