最近在做机械臂末端轨迹生成的时候一直在折腾一套组合工具链用 Pinocchio 做运动学解算、用 CasADi 把 IK 逆运动学变成优化问题、最后用 MuJoCo 做物理环境验证。折腾完以后回头想这个组合基本上覆盖了从几何求解到动力学验证的全流程而且每一步都能对上业界在足式机器人和机械臂项目里常用的路线。如果你也正准备搭一套 IK 求解 仿真验证的 pipeline这篇文章能给你省掉不少弯路。这套流程适合谁凡是需要做逆运动学、又不想被单一库绑死的人都可以参考包括刚入门机器人学的同学、做机械臂路径规划的工程师、做足式机器人全身 IK 的人。只要手里有 URDF 描述文件基本都能往下复现。我会把环境安装、模型统一、CasADi 求解、MuJoCo 仿真这四步拆开讲再贴一批可直接改的代码。1. 组合拳出发点为什么 IK 要专门拉出三个库来干1.1 单工具绕不过去的坎单独写 IK 其实不难难的是“够用”的 IK。纯解析 IK 只适合非冗余机构。像 6 自由度机械臂这种几何构型固定以后能推出闭式解但代码写死之后一旦换模型就得重推一遍。更麻烦的是奇异位形附近数值会变得很丑关节轨迹容易跳变。数值 IK 比如用雅可比法虽然通用但要自己处理关节极限、阻尼最小二乘、多解选择这些细节一不留神就发散。Pinocchio 自带了一个pin.ik接口也能解逆运动学但它本质上做的是单目标跟踪想加一些自定义约束比如“末端同时避碰”“关节尽量接近某个舒适位形”“速度平滑”就很吃力。而实际工程里这些需求几乎总是存在的于是我就想换成一个优化框架让 IK 变成非线性规划问题随便加约束加目标解出来还带自动微分梯度。1.2 三件套的真正分工在这个流程里三者的分工很清楚Pinocchio 负责机器人模型的运动学和动力学算法包括正运动学、雅可比、质量矩阵、重力项。它支持 URDF接口干净计算速度也快。CasADi 负责“把 IK 变成优化问题”。CasADi 是符号计算和最优控制框架顺手提供了Opti栈几乎就是个专用于运动规划的优化建模工具。关键在于 Pinocchio 还提供了一个pinocchio.casadi子模块可以直接把 Pinocchio 的运动学算法映射到 CasADi 的符号变量上这样自动微分就是白送的不用自己手推雅可比。MuJoCo 负责物理验证。IK 解出来的只是一串几何上合理的关节角放到真实环境里能不能跟踪、会不会抖、会不会撞到障碍物还得靠动力学仿真摸底。MuJoCo 的接触模型和渲染能力都很成熟机器人训练场景基本绕不开它。这个组合还有一个额外好处每个环节都能替换。今天想换成别的求解器只动 CasADi 层明天想换仿真引擎只要模型文件再造一份。对一个人维护的机器人项目来说这比硬编码一个巨型函数要灵活得多。1.3 整体信息流整套流程可以归纳成一条直线从 URDF 文件构建 Pinocchio 模型用pinocchio.casadi构架符号化的正运动学函数以“末端位姿和参考位姿的误差最小化”为目标关节限位为约束在 CasADi 里构建 NLP 并求解对整条末端轨迹逐点求解得到关节角时间序列把关节角输入 MuJoCo 模型用 PD 控制器跟踪对比期望末端轨迹和实际末端轨迹评估 IK 和解算质量。下面开始从环境搭起一步一步讲。2. 环境准备和模型统一2.1 安装 Pinocchio、CasADi 和 MuJoCo依赖安装本身不复杂。三个库都有 Python 接口直接走 pip 就可以pip install pin casadi mujoco如果怕依赖冲突也可以用 condaconda install -c conda-forge pinocchio casadi pip install mujoco我自己是在 Ubuntu 22.04 Python 3.10 下跑的Windows 11 下面也可以装但 Pinocchio 在 Windows 上偶尔会遇到编译二进制不匹配的问题真的不行可以考虑 WSL。安装完以后快速做个版本检查import pinocchio as pin import casadi as cs import mujoco as mj print(pin.__version__) print(cs.__version__) print(mj.__version__)Pinocchio 建议至少 2.7 以上因为pinocchio.casadi这个模块在早期的 Python 绑定里不够稳。2.2 准备机械臂模型我选的是 Panda为了演示我选的是 Franka Emika Panda 机械臂。原因很简单它是 7 自由度冗余臂正好能展示优化 IK 的价值同时 Pinocchio 自带 Panda URDF 示例MuJoCo Menagerie 仓库里也有现成的 Panda MJCF 模型省去格式转换的麻烦。如果你的模型不是现成的可以走两条路找一份 URDF交给 Pinocchio 直接加载如果想在 MuJoCo 里用建议直接用 Menagerie 的 MJCF 版本或者用mujoco_urdf之类的工具把 URDF 转成 MJCF。直接给 MuJoCo 喂 URDF 是不行的MuJoCo 的主格式是 MJCF硬加载会报算子错误。我自己踩过一次坑URDF 里引用的 mesh 都是相对路径转到 MuJoCo 后如果 mesh 目录没放对会直接File not found。这类格式转换问题我放在了第 6 章统一列出来。2.3 三库模型一致性检查这是最容易被忽略的一步。Pinocchio、CasADi 优化模型、MuJoCo 模型三个地方各有一份模型如果关节顺序不一致后面解出来全是错的。我通常会在开始前做三件事比较关节数Pinocchio 里是model.nqMuJoCo 里是m.nq7 自由度固定底座机器人两边都应该是 7比较零位时的末端位置让所有关节都在零位分别用 Pinocchio 和 MuJoCo 算一次末端位置误差应该是毫米级以下比较关节质量或者连杆质量总和如果差太多说明模型数据有问题。这一步花不了几分钟但能省下后面查错的一个通宵。3. 用 Pinocchio 和 CasADi 构建 IK 优化问题3.1 为什么选择用pinocchio.casadi桥接最直接的做法是用 Pinocchio 的正运动学结果自己手写一个目标函数然后传给 CasADi 的求解器但这样存在一个问题Pinocchio 的 Python 接口返回的是numpy数组不是符号变量CasADi 对它无法自动微分。以前的常见做法是自己再用 DH 参数在 CasADi 里手撸一遍运动学等于维护了两套运动学实现模型一旦更新非常痛苦。pinocchio.casadi解决的就是这个问题。它是 Pinocchio 的 CasADi 兼容版本接收的是 CasADi 符号变量返回的也是符号表达式。用它能直接复用同一份 URDF 描述把正运动学封装成一个支持自动微分的 CasADi 函数。3.2 符号化正运动学封装核心代码很短不要被pinocchio.casadi这名字吓到import pinocchio as pin import pinocchio.casadi as cpin import casadi as cs import numpy as np # 普通 Pinocchio 模型用于加载 URDF model pin.buildModelFromUrdf(./models/panda/panda.urdf) frame_id model.getFrameId(panda_hand) # 转换成 CasADi 兼容模型 cmodel cpin.Model(model) cdata cmodel.createData() # 符号化关节变量 q_sym cs.SX.sym(q, cmodel.nq) # 用符号变量跑一次正运动学 cpin.forwardKinematics(cmodel, cdata, q_sym) cpin.updateFramePlacements(cmodel, cdata) # 取出末端位置结果是 CasADi 符号表达式 p_sym cdata.oMf[frame_id].translation # 封装成 CasADi 函数输入 q - 输出末端位置 fk_pos cs.Function(fk_pos, [q_sym], [p_sym])这段代码做完以后fk_pos(q)就能当普通数学函数用但内部带自动梯度你不需要自己算雅可比也不需要担心数值求导的精度问题。如果你还需要姿态信息比如末端旋转矩阵可以同样把R cdata.oMf[frame_id].rotation取出来封装成函数。3.3 单点 IK 的优化建模有了fk_pos单点 IK 就变成了一个标准的最小二乘问题。目标函数是“末端位置和期望位置的距离平方”再加一个小正则项让解出来的关节角尽量接近初始值避免冗余臂在无意识情况下乱动。约束则是关节上下限。Panda 每个关节都有范围这些参数通常写在 URDF 里Pinocchio 加载完可以直接读model.lowerPositionLimit和model.upperPositionLimit。opti cs.Opti() q opti.variable(cmodel.nq) p_target opti.parameter(3) # 末端跟踪误差 p fk_pos(q) error cs.sumsqr(p - p_target) # 再加一个小正则项 q_init opti.parameter(cmodel.nq) reg 1e-3 * cs.sumsqr(q - q_init) opti.minimize(error reg) # 关节限位 opti.subject_to(opti.boundary(model.lowerPositionLimit, q, model.upperPositionLimit)) # 用 IPOPT 求解 opti.solver(ipopt, { print_level: 0, sb: yes, tol: 1e-6, }) opti.set_value(q_init, np.zeros(cmodel.nq)) opti.set_value(p_target, np.array([0.4, 0.2, 0.6])) opti.set_initial(q, np.zeros(cmodel.nq)) sol opti.solve() q_sol opti.value(q) print(IK solution:, q_sol)运行以后你会看到opti.solve()返回一个解这就是当前末端位置对应的一个关节构型。这里有几个细节值得说一下opti.parameter是 CasADi 里做“参数”的标准方式求解前可以反复改参数值而不改变符号图结构适合后面跑整条轨迹正则项系数1e-3不是随便拍的。太大会限制末端精度太小又起不到稳定多解的作用。我的经验是先从1e-3试误差太大再降IPOPT 的收敛容差我习惯设1e-6。对一般机械臂 IK 来说位置误差已经能到亚毫米级继续缩小只会增加求解时间。3.4 姿态误差的补充方案如果只控制末端位置很多场景其实不够比如你希望机械臂末端一直“水平”朝下。姿态误差就不能用欧拉角直接做差了万向锁会烦死你。一个稳定的做法是直接用旋转矩阵的误差范数或者四元数差。用旋转矩阵写误差可以定义成R_ref opti.parameter(3, 3) R fk_rot(q) error_rot cs.sumsqr(R - R_ref)目标函数里把位置误差和姿态误差加权合并。权重的比例没有绝对标准需要结合任务调节。我是这样想的如果末端允许小范围角度偏差就把位置权重设大一些如果姿态要求很严就把旋转矩阵部分权重上调。或者可以给两个误差各自乘个特征尺度比如位置误差除以0.1 m姿态误差除以0.1 rad之类的让量纲统一。4. 整条轨迹的解算代码实战4.1 从单点 IK 到轨迹 IK单点解完以后真正干活还是得解一条轨迹。我的做法是把一条空间曲线离散成几百个目标点对每个点解一次 IK并把上一帧的解当作下一帧的初值。这样有一个天然好处轨迹平滑不会无缘无故跳到一个远端的同解。假设目标末端轨迹是一个绕 Z 轴的圆形半径 0.2 m高度 0.6 m代码如下traj_target [] for i in range(200): theta 2 * np.pi * i / 200 pos np.array([ 0.4 0.2 * np.cos(theta), 0.2 0.2 * np.sin(theta), 0.6 ]) traj_target.append(pos) traj_q [] last_q np.zeros(cmodel.nq) for pos in traj_target: opti.set_value(p_target, pos) opti.set_initial(q, last_q) try: sol opti.solve() last_q opti.value(q) except RuntimeError: # 如果当前点求解失败回退到上一帧初值再试一次 opti.set_initial(q, last_q) opti.set_value(p_target, pos 1e-6) sol opti.solve() last_q opti.value(q) traj_q.append(last_q.copy()) traj_q np.array(traj_q)这一段就是整套 IK 的核心。每帧计算量其实很小7 自由度的 IPOPT 求解通常几十毫秒内能搞定200 个点也就几秒钟。如果非要实时还能用cs.generator之类的方式提前编译或者换用sqpmethod这种更轻的求解器。4.2 轨迹平滑和频率适配IK 直接算出来的关节角度序列虽然相对稳定但相邻帧之间还是可能有高频抖动尤其是奇异点附近。我把关节轨迹存下来以后习惯再做一次 Savitzky-Golay 平滑或者用 CasADi 里面做一个最小加速度的三次样条优化。简单一点用 scipy 的savgol_filter就够了from scipy.signal import savgol_filter traj_q_smooth np.zeros_like(traj_q) for j in range(cmodel.nq): traj_q_smooth[:, j] savgol_filter(traj_q[:, j], window_length15, polyorder3)平滑以后关节速度曲线会干净很多MuJoCo 里做 PD 跟踪时也不会因为目标角度跳变产生很大的脉冲力矩。4.3 验证 IK 结果的正确性这里有个我很推荐的验证法直接用 Pinocchio 做一遍正运动学把求出的关节角度映射回末端位置再和目标轨迹对比。fk_pos_numeric pin.computeFramePlacement(model, data, traj_q_smooth[-1], frame_id) end_eff_pos fk_pos_numeric.translation.ravel() print(Expected:, traj_target[-1]) print(Actual :, end_eff_pos)如果位置误差超过毫米级先检查是不是正则项权重太大或者 IPOPT 容差太松。这一步是检验 IK 求解质量的底线能跑通之后再去 MuJoCo 仿真才不心虚。5. 在 MuJoCo 里跑物理仿真5.1 加载模型和控制方式MuJoCo 侧的模型我用 Menagerie 里的 Panda MJCF 文件路径大概长这样panda/mjcf/panda.xml。加载方式很直接import mujoco m mujoco.MjModel.from_xml_path(./models/panda/mjcf/panda.xml) d mujoco.MjData(m) print(nu:, m.nu) print(nq:, m.nq)固定底座机械臂的nq和nu通常都是 7。Panda 在 MJCF 里的默认驱动器是 motor位置控制需要自己写 PID 逻辑。MuJoCo 默认的驱动器系数为零不设kp的话机械臂会直接软掉这事我头一次跑的时候完全懵了。5.2 PD 控制器跟踪关节轨迹机械臂关节空间跟踪最稳妥的方案就是 PD 控制器。角度误差和速度误差的线性组合kp np.array([200.0] * m.nu) kv np.array([20.0] * m.nu) for t in range(int(5.0 / m.opt.timestep)): idx min(int(t / (5.0 / m.opt.timestep) * len(traj_q_smooth)), len(traj_q_smooth) - 1) q_des traj_q_smooth[idx] dq_des np.zeros(m.nu) q_cur d.qpos[:m.nu] dq_cur d.qvel[:m.nu] d.ctrl[:] kp * (q_des - q_cur) kv * (dq_des - dq_cur) mujoco.mj_step(m, d)关于 PD 增益的选择kp200在位置控制里算比较激进实际会引起电机力矩饱和所以我会配合kv调节一般取kv 2 * sqrt(kp)左右对应阻尼比接近 1 的经验公式。如果机械臂跟踪时出现明显抖动优先降kp如果跟踪滞后明显则适当升kp。5.3 末端实际轨迹和期望轨迹对比仿真跑完以后还要看一下末端实际走的轨迹。在 MuJoCo 里可以直接用 body 的xpos当末端位置hand_body m.body(panda_hand).id actual_pos [] for t in range(0, int(5.0 / m.opt.timestep), 10): # 这里应该在仿真循环里记录后续再画图 actual_pos.append(d.body(hand_body).xpos.copy())把actual_pos和traj_target画在一张图里你会发现期望轨迹是一个圆实际轨迹初始阶段会有一定跟踪误差尤其加速度最大的地方。这是正常的PD 控制下的机械臂不可能完美复现目标轨迹。如果误差还在可接受范围内流程就算闭环了。5.4 记录和重放仿真动画MuJoCo 自带的 viewer 可以直接看实时仿真但它有一个更好用的模式仿完以后保存数据用 view 重新播放。具体说就是仿真循环里持续记录d.qpos仿真结束后用一个临时的mujoco.MjData重新写入这些数据再配合mujoco.viewer逐帧播放——类似现在很多开源项目里那个 “重新播放” 按钮做的事情。我一般会顺带渲染成 mp4renderer mujoco.Renderer(m, height480, width640) frames [] # 循环内每若干步保存一帧 renderer.update_scene(d) frames.append(renderer.render().copy())然后传给imageio.mimsave或者cv2.VideoWriter导出视频。这个能力在做方案汇报或者调试 bug 时很实用。6. 常见问题与踩坑实录6.1 模型格式转换和 mesh 路径问题这是比例最大的一类问题。URDF 转 MJCF 时mesh 路径通常还是相对 URDF 的位置。转完以后 MuJoCo 报Could not find model file: .../stl/link1.stl解决方案很简单转换时显式指定file_root参数或者把 mesh 目录复制到 MJCF 同级目录下。Menagerie 的现成模型已经处理好了所以再次建议直接下载现成 MJCF而不是自己转。如果你是 Windows 11 用户装上 MuJoCo 以后遇到.so或者.dll缺失的问题多半是 VC 运行库没装去微软官网装一下最新版的 Visual C Redistributable 基本能解决。经常有人以为是 mujoco 装错了其实不是。6.2 IPOPT 不收敛或者跳出奇解IK 求解失败是最容易恶心的事。常见原因有三个目标点在工作空间之外。本来就不存在解IPOPT 怎么试都不收敛。初始值太离谱比如零位距离目标非常远导致优化掉进奇解。关节限位设置错误可能下限比上限大Pinocchio 加载模型时通常会警告但不阻止运行。工作空间外的情况程序里一定要先做可到达性判断最简单的方法是计算目标点到基座的距离超出可达半径直接跳过。初始值问题可以用上一帧解作为初值我前面的轨迹代码里就是这么做的。奇解问题则要靠正则项和锁关节策略必要时约束某一个关节的区间人为打破对称性。CasADi 的 IPOPT 求解失败后opti.solve()会抛RuntimeError。不要慌先opti.debug()看一下哪个约束和代价的量级异常这是 CasADi 最实用的调试功能。6.3 MuJoCo 仿真不稳定或机械臂抖动仿真里机械臂抖一般不是模型问题而是控制器增益太高或太低。我在第 5 章说过kv对应阻尼比的经验公式。另外要注意 MuJoCo 的m.opt.timestep默认很小如果机械臂做高速运动每个仿真步进里 PD 控制器输出变化很大我建议把控制输出做一阶低通滤波ctrl_smooth 0.8 * ctrl_smooth 0.2 * ctrl_raw看起来简单但实测对稳定性帮助极大。6.4 Pinocchio 和 MuJoCo 的单位和坐标系不一致还有些问题不是代码逻辑而是单位。URDF 里默认单位是米和千克MJCF 也遵循 SI 单位但有些网上下载的模型会混入厘米或者克导致力的大小完全不对。碰到机械臂飘在半空不落下来这类奇怪现象先查质量和长度单位。6.5 常见问题速查表现象常见原因解决思路IPOPT 报Infeasible Problem目标点超出工作空间检查可达性缩小目标范围解出来的关节角不连续初始值差异太大用上一帧解做初值加正则项MuJoCo 模型加载报错mesh 路径相对路径错误指定file_root用 Menagerie 模型仿真中机械臂剧烈抖动PD 增益过高或kv太小调低kp按经验公式配kv和气动/扭矩控制不匹配驱动器类型未配置检查 MJCF 中的motor配置Pinocchio 和 MuJoCo 位置对不上模型文件版本不同统一 URDF/MJCF做零位检查一点实操体会和后续可扩展的方向这套流程我现在已经用成了固定套路。最满意的地方还是pinocchio.casadi那一步以前我做 IK 要么手推雅可比要么用数值差分调试起来特别费劲现在直接在符号层面解算和纯手写数值优化完全不是一个体验。你如果后面想加接触力、加整身动力学约束这个框架还能继续往轨迹优化和 MPC 的方向扩展甚至不局限于机械臂足式机器人、轮足机器人、扫地机器人这类项目也常见到用 MuJoCo 做强化学习环境而环境里的目标生成恰恰就需要这样一套稳定高效的 IK 链路。