四足开发实战（七）：建模篇——运动学与 URDF 从零写起
给定一条腿的三个关节角，求解足端在世界系中的位置，是运动学建模的第一步。该问题一旦出现符号错误，后续的仿真、步态与强化学习都将随之出错。实际开发中常见的情形是：模型在 MuJoCo 中导入后仿真表现正常，固件运行后四条腿却朝反方向摆动，排查后确认问题来自 URDF 中一个 axis 的负号。本文先将运动学推导到无歧义的程度，再说明如何将正确的模型写入文件并验证到可以信赖。
在开始之前，先梳理整条运动链。运动学中一共涉及六个坐标系，它们之间的父子关系即后续 URDF 中的 link 树；每个坐标系的位姿由关节角逐级叠加得到，任何一层出错都会波及全部结果：
flowchart LR |
一条腿的几何：余弦定理即可求解
首先需要明确坐标系定义，后续公式均以此为前提。世界系与机身系均为 x 轴朝前、y 轴朝左、z 轴朝上的右手系，髋、大腿、小腿、足端依次串联为一条链，共六个坐标系，与 URDF 中的 link 树一一对应。参考设计中大腿与小腿各长 0.22 m，髋相对机身中线的侧向偏置为 0.08 m，前后髋距为 0.20 m，足端为半径 0.02 m 的球体，站立高度为 0.40 m。此处有一个容易混淆的细节：L1 + L2 度量到足端球心，而站立高度度量到接地点，二者恰好相差一个球半径。若少加或多加这个 0.02 m，站立高度将偏移 2 cm，IK 解出的膝角随之出错，落足规划也会整体受到影响。
正运动学采用几何法推导，较记忆旋转矩阵更为直观。髋 pitch 角 q2 使大腿在矢状面内向前抬起，膝关节 q3 使小腿相对大腿向后折叠，因此小腿相对竖直方向的绝对角度为 q2 − q3，两个角在同一平面内直接相减。两段连杆在矢状面内的投影可写为：足端相对髋的前后偏移为 L1·sin q2 + L2·sin(q2 − q3)，竖直长度为 h = L1·cos q2 + L2·cos(q2 − q3)。髋 roll 角 q1 的作用较为简单：它不改变这条腿的长度，只将这段长度为 h 的矢量由竖直方向转向侧面。综上，足端相对髋点的位置为
$$
p = \begin{pmatrix} L_1\sin q_2 + L_2\sin(q_2-q_3) \ \text{side}\cdot h\sin q_1 \ -h\cos q_1 \end{pmatrix}, \qquad h = L_1\cos q_2 + L_2\cos(q_2-q_3)
$$
side 在左腿取 +1、右腿取 −1；若右腿符号出错，两条腿将朝同一侧摆动，机身被推着横向移动。该公式不含矩阵乘法，这是四足腿 3 自由度的简化之处；机械臂的 6/7 自由度才需要 DH 参数表与数值迭代。
得到位置后，对其求一次偏导即可得到雅可比矩阵。雅可比的第 i 列表示“关节 i 以 1 rad/s 旋转时足端的速度”，因此 v_foot = J·q̇；反之它也是静力映射，足端需要输出力 F 时，关节力矩为 τ = Jᵀ·F——同一矩阵同时服务于速度与力。对上述 FK 逐项求偏导，注意 h 与 q2、q3 都相关，整理为 3×3 矩阵：
$$
J = \begin{bmatrix} 0 & h & -L_2\cos(q_2-q_3) \ \text{side}\cdot c_1 h & -\text{side}\cdot s_1 p_x & \text{side}\cdot s_1 L_2\sin(q_2-q_3) \ s_1 h & c_1 p_x & -c_1 L_2\sin(q_2-q_3) \end{bmatrix},\qquad c_1=\cos q_1,\ s_1=\sin q_1,\ p_x=L_1\sin q_2+L_2\sin(q_2-q_3)
$$
理解该矩阵的物理结构比机械记忆更有价值：第一列只有 y、z 分量，因为 q1 不改变腿长，只在侧向与竖直方向移动足端；后两列对应矢状面内两段连杆的摆动速度。二者共同决定奇异位形——行列式 |det J| = L1·L2·h·sin q3，当腿完全伸直（q3→0）或足端与髋同高（h→0）时退化为零。落实到实机可概括为：站立时膝角应保留 0.05 rad 余量，不应处于完全伸直位。靠近奇异区时 IK 对噪声极为敏感，1 mm 的传感器误差可放大为数度的关节角跳变。
逆运动学按三步求解，结构清晰。已知足端相对髋点的坐标 (x, y, z)，第一步求解髋 roll：q1 = atan2(side·y, −z)，同时 h = √(y²+z²) 即腿在矢状面内的长度。第二步将问题简化为平面三角形：髋、膝、足三点，两边为 L1、L2，第三边为髋到足端的直线距离 ρ = √(x²+h²)。使用余弦定理时有一个容易出错的细节——膝的内角是 π − q3 而非 q3，因此 cos(π − q3) = −cos q3；等式中的正负号一旦反向，IK 将解出朝反方向弯曲的膝盖：
$$
q_3 = \arccos\frac{\rho^2 - L_1^2 - L_2^2}{2L_1L_2}, \qquad q_2 = \operatorname{atan2}(x, h) + \operatorname{atan2}(L_2\sin q_3,\ L_1 + L_2\cos q_3)
$$
第二项 β 的物理含义较为直接：膝盖弯曲多少，大腿就需向前补偿多少；q3 = 0 时该项归零，q2 即髋到足端连线本身的仰角。整段推导仅用到两次 atan2 与一次余弦定理，在主控中一个 acosf 加限幅即可实现为十几行 C 代码。下面这份骨架可直接使用，需要注意 side 参数贯穿始终，且 acosf 之前务必进行 clamp，越界目标应返回 false，而非输出 NaN 使后续控制环出错：
typedef struct { float x, y, z; } vec3; |
调试时应遵循一条顺序：先验证 FK，再验证 IK。FK 具有唯一解，符号错误会立即从足端坐标的数值中暴露；IK 的错误则更隐蔽——它可能解出镜像的膝，也可能在奇异区附近将 1 mm 的误差放大为数度。因此联调时应始终先用 FK 对比 CAD 量出的三个姿态，在 FK 验证通过之前，不应处理 IK 的 bug。
arccos 仅返回 [0, π]，但方程对 ±q3 均成立，对应小腿向后折与向前折两组解。参考设计的机械限位只允许向后折，因此可直接排除向前折；若结构允许双向，则按连续性选择——取与上一拍更接近的那一支，否则摆动过程中膝盖会瞬间翻转，机器人当场失稳跌倒，这一冲击足以使减速器产生背隙。工作空间的边界同样由几何决定：腿完全伸直时 ρ = L1+L2 = 0.44 m，受膝限位约束的实际最远为 0.440 m、最近为 0.139 m；对越界目标，IK 必须返回错误码，而不是由 acos 输出 NaN。
站立角同样可由余弦定理求出。站高 0.40 m、球半径 0.02 m，球心高度为 0.38 m，代入 cos q3 = (0.38² − 2×0.22²)/(2×0.22²) = 0.4917，解得 q3 = 1.0567 rad、q2 = 0.5284 rad。这组数值在后续反复使用。站立过直意味着 q3 逼近 0、进入奇异区，落地冲击会直接冲击关节限位；保留约 13% 的屈曲量，膝关节才有力矩余量用于缓冲。此处也可用雅可比验证一次选型——站立位单腿承重 29.43 N（12 kg 均分四腿），由 τ = JᵀF 计算仅膝盖输出 3.26 N·m，因为竖直力的作用线恰好穿过髋轴，只有膝到足端那 0.111 m 的水平力臂受力。该值乘以安全系数，即为减速器额定扭矩的选取依据。雅可比本身已用数值求导验证：(fk(q+ε) − fk(q−ε))/2ε 与解析式逐元素比较，最大偏差为 7.5e-10，符号与系数均正确。10 万组随机姿态经过 q → fk → ik → q' 往返测试，最大关节误差为 6.6e-15 rad，属纯浮点噪声，说明该组公式自洽。若真机髋部存在横向偏置、连杆不共面，解析解将产生数毫米系统误差，此时可用牛顿迭代 q ← q + J⁻¹(p_target − fk(q)) 兜底，初值取上一拍的解，数次即可收敛，仅在标定与诊断时使用。更一般化的 DH 法与全身运动学推导见理论篇 四足机器人控制理论（公式推导汇总），本文仅保留工程上日常使用的内容。
写 URDF：一半时间用于处理默认值
运动学公式确定后，将其翻译为 URDF 又会遇到一系列问题。URDF 的结构本身并不复杂：一个 <robot> 内直接列出 link 与 joint，四足 12 个关节即 17 个机体 link 加 13 个 joint（12 转动 + 1 浮动），再加一个 world 参考。浮动关节写作 type="floating"，MuJoCo 会编译为 6 自由度的 free joint。以下先看一条腿的最小骨架，每个字段在后续逐一说明：
<robot name="quadruped"> |
joint 的 origin/xyz 是子 link 原点在父 link 系中的位置（关节零位时），axis 在关节坐标系中会被归一化，写作 0 -1 0 与 0 1 0 即表示相反方向。参考设计的髋 pitch 特意使用 -y 轴，因为 +y 旋转会使足端向后（右手定则），而步态中“正角度 = 向前迈”更符合直觉，代价是 axis 中的负号不可遗漏。limit 中的 effort/velocity 取自第 3 篇定稿的峰值力矩与空载转速（23.7 N·m、30 rad/s），它们是仿真中执行器钳位的来源，并非装饰。
origin 中的 rpy 采用固定轴 X-Y-Z 顺序，等价于 R = Rz(yaw)·Ry(pitch)·Rx(roll)，矩阵从右往左乘；大部分 rpy 相关问题都源于将其理解为“绕自身轴依次旋转”。参考设计将四条腿的 origin rpy 全部写为 0 0 0，左右镜像仅通过 axis 的正负号实现，使 FK 公式对四条腿保持一致。也可将右腿整体旋转 180° 再修改 axis，两种镜像方式均可使用，但只能选择其一；混用会导致腿部朝异常方向摆动。另有一个反直觉的易错点：box 的 size 是半边长，0.42 × 0.20 × 0.12 m 的机身应写为 0.21 0.10 0.06；若写成全尺寸，模型会扩大一倍，质量与站立高度都不再匹配。mesh 的常见问题同样突出——CAD 导出单位为毫米，而 URDF 按米读取，若未写 scale="0.001 0.001 0.001"，模型将放大一千倍；且 visual 与 collision 必须同步修改，否则会出现“视觉上为一米、碰撞上为一毫米”的不一致。碰撞体建议直接使用胶囊、球、盒等原语而非高模网格，因为网格被 MuJoCo 转换为凸包后接触法向会跳变，机器人落地时即会抖动。
MuJoCo 直接加载 URDF 时，若干编译默认值与手写 MJCF 相反，这是“模型一导入即散架”问题的最主要来源：
| 编译选项 | MJCF 默认 | URDF 输入时默认 | 建议 |
|---|---|---|---|
fusestatic |
false | true | 设 false，否则足端的 fixed link 被融进小腿，FL_foot_link 直接消失 |
strippath |
false | true | 设 false，否则两个子目录里的同名 thigh.stl 会相互混淆 |
discardvisual |
true | true | 设 false，否则只剩碰撞体，看不见外观 |
惯量是另一处常见问题所在。URDF 的 inertial/origin 是质心在 link 系中的位置，inertia 的六个分量是绕质心的惯量，而非绕关节。从 CAD 导出时常见三类错误：单位使用 kg·mm²（惯量比真值大 10⁶ 倍，机器人表现得异常沉重，仿真中轻微受力即倾倒）；惯量绕关节而非质心（关节响应异常、静止时也抖动）；镜像件的非对角项符号未同步变化（左右大腿外观一致，但 ixy 一正一负，直接复制即出错）。SolidWorks 中导出前应先将单位改为 kg/m、将惯性矩报告改为相对质心；FreeCAD 使用 FCInfo 宏或直接读取 MatrixOfInertia，注意其结果可能相对原点，写入 URDF 前需用平行轴定理转换到质心。完成后无需肉眼检查 XML，直接在 MuJoCo 中打印 body_mass 与 body_inertia，与 CAD 逐件核对：相差 10⁶ 是单位错误，相差数倍是取错零件，相差 1~2% 是圆角与螺纹被简化，可接受。该方法还可发现“某个 link 未填写质量”——质量填 0 在 MuJoCo 中会直接拒绝编译，但 0.05 kg 的小件也应给出示例值。
MJCF：补上 URDF 缺少的内容
URDF 描述的是“机构”，执行器与传感器属于仿真侧，因此正确的转换方式为：MuJoCo 读入 URDF 之后，用 mj_saveLastXML 另存为 MJCF，后续修改均在 MJCF 上进行，不再回头修改 URDF。实测已确认：将 <actuator> 写入 URDF 的 <mujoco> 块不会生效（编译结果 nu 仍为 0），必须在另存的 MJCF 中补充。
最小可用的 MJCF 如下——包含浮动基、原语碰撞、12 个执行器与 IMU 传感器：
<mujoco model="quadruped"> |
该段中有两个细节值得单独说明。第一，capsule 的 fromto 表示轴线两端，实际总长 = 轴线长 + 2×半径：大腿 0.16 + 2×0.03 = 0.22，与 L1 一致，小腿同理——使用原语做碰撞，接触法向光滑，比网格更稳定。第二，执行器有两条路线：教学与慢速步态使用 <position kp="200" dampratio="1"/> 的位置伺服；运行 RL、对齐实机时使用 <motor ctrlrange="-23.7 23.7"/> 的力矩执行器，并配合代码中的外部 PD——ctrlrange 必须采用实机力矩限，否则仿真中能输出、实机无法输出，策略一上机即失效。传感器至少应加入 gyro、accelerometer 与 12 个 jointpos，注意 gyro/accelerometer 输出在 site 自身坐标系中，不应与世界系的量混淆。
加载后五行输出值得熟记：nq=19（有浮动基）、nu=12（执行器已补充）、nbody=18（没有 link 被合并）、mass=12.0、extent 处于 0.5~1.5 m 量级（并非毫米或百米），十秒的检查即可避免后续数小时的“无法稳定站立”排查。mesh 路径也应统一规范：CAD 的 STL 统一放置于 meshes/，URDF 中写相对路径，MJCF 中配置 meshdir="meshes"；路径不应包含空格与中文，scale 只在 URDF 写一次，转为 MJCF 后需检查 asset/mesh 是否仍带有 scale，两处都写会导致重复缩放。
三张编号表必须一一对应，否则“仿真正确、实机反向”会消耗大量排查时间：URDF 的 joint 名、固件中 q[12] 的数组下标、RS485 帧中的电机 ID。建议在固件中固定写入一张名字表与 _Static_assert(J_COUNT == 12)，更换腿部、电机或调整装配顺序时，先改表、再改 URDF、最后运行验证。该表需精确到轴符号——左右腿同名关节的 axis 正好相反，相差一个负号即会导致两条腿朝同侧摆动：
| 关节 | 固件下标 | 电机 ID【示例】 | 轴符号 |
|---|---|---|---|
| FL 髋 roll / pitch / 膝 | q[0…2] | 1 / 2 / 3 | +x / −y / +y |
| FR 髋 roll / pitch / 膝 | q[3…5] | 4 / 5 / 6 | +x / +y / −y |
| RL 髋 roll / pitch / 膝 | q[6…8] | 7 / 8 / 9 | +x / −y / +y |
| RR 髋 roll / pitch / 膝 | q[9…11] | 10 / 11 / 12 | +x / +y / −y |
电机 ID 的分配以实际接线为准（两条 RS485 总线各接 6 个），但“先定该表、再写固件、再写 URDF”的顺序不可改变。该表可打印后贴于工作台旁，作为后续步态篇、仿真篇中调试的共同语言——“左前膝”的表述不明确，使用 q[2] 或 ID 3 则含义清晰。
XML 本身的低级错误也值得单独说明，其报错往往不含定位信息。少写一个 </link> 是解析错误，在编辑器中折叠 link 即可发现；revolute 关节漏写 effort/velocity，部分工具会静默按 0 处理；两个 link 重名会使转换后的关节引用错乱，可全局搜索去重；手填的惯量不满足三角不等式时会报 inertia must satisfy A+B>=C，此时应回 CAD 重新计算，不应使用 balanceinertia 掩盖错误——它会代为修改物理量，并非无代价。
模型验证通过，才能继续推进
模型编写完成并不等于正确，本文用四项测试收束验证，而非仅凭肉眼判断。第一，FK 与 CAD 对比：将 CAD 中一条腿摆到零位、站立、前伸三个姿态，量取足端球心坐标并与代码输出比较，偏差应 ≤1 mm；若超标，先确认 L1/L2 度量的是轴心距而非零件长度。第二，IK 十万组往返，关节误差应 ≤1e-6 rad，越界目标返回错误码而不产生 NaN；边界测试专门选择 ρ 略超 0.44 与接近 0.139 的目标。第三，MuJoCo 站立测试：初始角设为站立位运行 10 秒，机身高度从 0.400 降至 0.39596 后不再变化（这 4 mm 是接触模型“软”特性的正常下陷，并非模型错误）、接触数恰为 4、四条腿法向力之和 117.72 N 等于 mg。第四，总质量 12.0 kg、质心偏离应 ≤30 mm——质心 x 向偏离 21 mm 是因为四条腿同向弯曲整体前移，属设计问题而非书写错误，可接受或用配重修正。
模型并非编写完成后即固定不变，修改一个参数需要连带更新一系列参数：腿长 L1/L2 变化后，URDF 关节 origin、IK 限幅、站立角、步长上限均需重算；足端半径变化后，站立高度与抬腿高度随之变化；质量惯量变化后，站立 kp、力矩限与后续 MPC 的单刚体参数均需更新。最常见的错误是将 0.5284 / 1.0567 这两个站立角直接复制到已修改腿长的模型中——看似能够站立，实际上腿部处于弯曲状态，此后在步态篇中将在一个错误的模型上反复调试。反之，这套验证测试应在参数一变时即重跑一遍，其成本低到没有省略的理由。
下一篇将讨论如何让可稳定站立的模型迈出第一步：状态估计与 trot 行走，从 Mahony 滤波到落脚点，说明机器人如何确定自身位置并规划下一步落点。
