3. 运动学基础：正运动学与逆运动学求解方法，D-H参数法，雅可比矩阵，以及双足机器人运动学约束
双足机器人的运动学是连接关节空间与操作空间（或工作空间）的数学桥梁。无论是规划质心轨迹、计算踝关节落点，还是进行柔顺控制与力反馈，都离不开对运动学模型的精确描述。本章将系统讲解正运动学与逆运动学的求解逻辑、D-H参数法的建模规范、雅可比矩阵的速度映射与奇异性分析，并重点剖析双足机器人特有的闭链约束与欠驱动特性。这些内容不仅是后续步态规划的基础，也是理解ZMP稳定性判据、摆动腿轨迹跟踪和全身协调控制的前提。
上图展示了双足机器人运动学四大模块之间的逻辑关系：正运动学与逆运动学互为逆映射，雅可比矩阵则连接两者的微分关系，而运动学约束（尤其是闭链与欠驱动特性）贯穿于所有计算之中。下面我们将逐一展开。
3.1 正运动学：D-H参数法与齐次变换
3.1.1 核心原理
正运动学解决的是“给定关节角度，求末端执行器（如踝关节或髋关节）在笛卡尔空间中的位置与姿态”的问题。对于双足机器人，通常将每条腿视为一个6自由度（或5自由度）的串联机械臂，从骨盆基准坐标系出发，依次经过髋关节（3自由度）、膝关节（1自由度）、踝关节（2自由度），最终到达脚掌参考点。D-H参数法通过四个参数（θ、d、a、α）描述相邻连杆间的相对位姿关系，从而建立统一的齐次变换矩阵。
标准D-H参数定义如下：
- θ（关节角）：绕Z轴旋转的角度，对于旋转关节为变量。
- d（连杆偏距）：沿Z轴平移的距离。
- a（连杆长度）：沿X轴平移的距离，即相邻两关节轴线的公垂线长度。
- α（连杆扭转角）：绕X轴旋转的角度，即相邻两Z轴之间的夹角。
相邻连杆间的齐次变换矩阵为：
T_i = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)对于一条完整的腿部链，末端位姿为：
T_0n = T_1 * T_2 * ... * T_n3.1.2 操作流程
- 建立各关节坐标系，遵循D-H约定（Z轴沿关节轴线，X轴沿公垂线）。
- 填写D-H参数表，注意双足机器人左右腿的对称性。
- 逐次计算相邻变换矩阵并连乘。
- 提取末端位置（P_x, P_y, P_z）与姿态（欧拉角或旋转矩阵）。
3.1.3 案例拆解：单腿正运动学计算
以一款典型的6自由度双足机器人右腿为例，其D-H参数表如下（单位：mm，角度：°）：
| 关节 i | θ_i | d_i | a_i | α_i | 关节范围 |
|---|---|---|---|---|---|
| 1（髋偏航） | θ1 | 0 | 0 | -90 | -30 ~ +30 |
| 2（髋滚转） | θ2 | 0 | 0 | +90 | -45 ~ +45 |
| 3（髋俯仰） | θ3 | L1=85 | 0 | -90 | -90 ~ +30 |
| 4（膝俯仰） | θ4 | 0 | L2=110 | 0 | 0 ~ +150 |
| 5（踝俯仰） | θ5 | 0 | L3=100 | 0 | -60 ~ +60 |
| 6（踝滚转） | θ6 | 0 | 0 | 0 | -30 ~ +30 |
当给定一组关节角（θ1=0°, θ2=0°, θ3=10°, θ4=20°, θ5=-10°, θ6=0°）时，通过连乘变换矩阵，可求得脚掌中心相对于骨盆坐标系的位置约为（0, 0, -295）mm，姿态为俯仰角约20°。该结果可直接用于步态仿真中的可视化验证。
3.2 逆运动学：解析法与数值法
3.2.1 核心原理
逆运动学是正运动学的逆过程：已知期望的脚掌位姿（位置+姿态），求解各关节角度。双足机器人中，逆运动学主要用于摆动腿的轨迹规划——在单脚支撑期，摆动腿需要从当前位置移动到预定的落点，且脚掌姿态需保持水平。由于腿部结构通常满足Pieper准则（相邻三轴交于一点或平行），可以采用解析法求解，计算速度快且精度高。
3.2.2 判断方法与操作流程
解析法求解步骤：
- 将目标位姿分解为位置向量和旋转矩阵。
- 利用几何关系（余弦定理）求解膝关节角度。
- 根据髋关节位置反推髋关节三个角度。
- 由踝关节姿态残差求解踝关节两个角度。
当解析法不可行（如存在关节限位或结构偏移）时，可采用数值法（如雅可比迭代法、阻尼最小二乘法）。数值法通用性强，但计算量大，且可能陷入局部极小值。
3.2.3 案例拆解：摆动腿落点规划
假设机器人单脚支撑，期望摆动腿脚掌从当前位置（0, 0, -295）mm移动到目标位置（120, 0, -295）mm，且脚掌保持水平（姿态角为0°）。采用解析法：
- 首先计算髋关节到目标点的距离，结合大腿长度L2和小腿长度L3，用余弦定理求出膝关节角度θ4。
- 然后根据目标点相对髋关节的方向角，求解髋关节俯仰角θ3。
- 最后调整踝关节角度θ5、θ6，使脚掌姿态满足水平要求。
计算结果为：θ3≈15.2°，θ4≈28.5°，θ5≈-13.7°，θ6=0°。该组解满足所有关节限位，可直接作为轨迹规划的中间点。
3.3 雅可比矩阵：速度映射与奇异性
3.3.1 核心原理
雅可比矩阵J建立了关节速度与末端笛卡尔速度之间的线性映射关系：
v_end = J(q) * q_dot其中v_end为末端线速度和角速度组成的6维向量，q_dot为关节角速度向量。雅可比矩阵同时可用于力的映射：
τ = Jᵀ * F即关节力矩τ与末端力F之间的关系。在双足机器人中，雅可比矩阵常用于：
- 计算摆动腿末端速度，用于轨迹跟踪控制。
- 分析奇异位形，避免关节速度突变。
- 实现柔顺控制（如导纳控制），通过力反馈调整关节位置。
3.3.2 判断方法与操作流程
雅可比矩阵的构建方法：
- 对正运动学方程求偏导，得到位置雅可比。
- 对姿态部分求偏导，得到姿态雅可比。
- 合并为6×n矩阵（n为关节数）。
奇异位形判断：计算雅可比矩阵的行列式（方阵时）或条件数（非方阵时）。当行列式接近零时，机器人处于奇异位形，此时某些方向的运动无法实现。
3.3.3 案例拆解：奇异位形分析
以单腿完全伸直（θ4=0°）为例，此时膝关节锁死，雅可比矩阵的秩下降，末端在竖直方向上的速度无法通过膝关节产生。实际表现为：机器人腿部完全伸直时，无法通过膝关节调节脚掌高度，只能依赖髋关节和踝关节，导致运动灵活性下降。在步态规划中，应避免摆动腿完全伸直，通常保持膝关节微屈（约5°~10°），以确保运动学可控性。
3.4 双足机器人运动学约束
3.4.1 核心原理
双足机器人区别于固定基座机械臂的核心在于其运动学约束：
- 闭链约束：双脚支撑期，两条腿与地面构成闭链，导致关节角度之间存在耦合关系，不能独立规划。
- 欠驱动特性：脚掌与地面之间无固定连接，机器人依靠摩擦力与重力维持平衡，存在被动自由度（如脚掌绕支撑边线的转动）。
- 支撑域约束：质心投影必须落在支撑多边形内（ZMP准则），这间接约束了髋关节和踝关节的运动范围。
3.4.2 判断方法与操作流程
处理闭链约束的常用方法：
- 将闭链拆分为两个开链（左腿和右腿），分别进行运动学计算。
- 引入闭环约束方程（如两脚掌相对位姿固定），通过拉格朗日乘子或约束雅可比求解。
- 在步态规划中，优先规划支撑腿的运动，摆动腿轨迹需满足与支撑腿的相容性条件。
3.4.3 案例拆解：双脚支撑期的运动学协调
在双脚支撑期，假设两脚掌均平放于地面，间距为W=150mm。此时，左腿和右腿的踝关节位置相对固定，但髋关节位置由两条腿共同决定。若左腿各关节角度已知，则右腿的关节角度必须满足髋关节位置一致的条件。实际工程中，通常采用“虚拟腿”概念：将双腿等效为一条虚拟腿，从质心到ZMP点，从而简化运动学计算。
| 约束类型 | 数学表达 | 工程影响 | 应对策略 |
|---|---|---|---|
| 闭链约束 | f(q_L, q_R) = 0 | 关节角度耦合，不能独立规划 | 虚拟腿模型、约束雅可比 |
| 欠驱动 | 脚掌无固定约束 | 存在被动自由度，控制难度大 | ZMP控制、踝关节柔顺 |
| 支撑域 | ZMP ∈ 支撑多边形 | 限制质心运动范围 | 步态参数调整、上身配重 |
3.4.4 常见误区与总结复盘
总结复盘：本章的核心在于建立完整的运动学模型，并理解双足特有的约束。正运动学是基础，逆运动学是工具，雅可比矩阵是桥梁，而约束条件则是双足机器人区别于其他机器人的关键。在实际工程中，建议采用模块化设计：将运动学求解封装为独立函数，输入关节角或末端位姿，输出对应的位姿或关节角，并集成奇异检测与限位检查功能。