3、运动学基础：正运动学与逆运动学求解方法（DH参数、数值迭代）、雅可比矩阵及奇异性分析
双足机器人的运动学建模是行走控制的理论基石。无论是规划髋关节的摆动轨迹，还是计算踝关节的落地姿态，本质上都是在解决“关节空间”与“笛卡尔空间”之间的映射问题。本章围绕三个核心任务展开：第一，利用DH参数建立正运动学模型，解决“给定关节角求足端位姿”的问题；第二，针对逆运动学中难以解析求解的耦合关节，引入数值迭代方法；第三，通过雅可比矩阵分析速度映射关系，并重点讨论奇异位形对行走稳定性的影响。理解这三者之间的逻辑递进关系，是后续进行ZMP（零力矩点）控制、步态规划与全身动力学控制的前提。
3.1 DH参数建模与正运动学求解
3.1.1 核心原理：从关节角到位姿的齐次变换
Denavit-Hartenberg（DH）参数法通过四个几何参数（连杆长度 \(a_{i-1}\)、连杆扭转角 \(\alpha_{i-1}\)、连杆偏距 \(d_i\)、关节角 \(\theta_i\)）唯一确定相邻坐标系间的变换关系。对于双足机器人，通常将基坐标系固定在髋关节中心，依次经过髋关节的横滚、俯仰、偏航轴，再到膝关节俯仰轴，最后到踝关节的俯仰与横滚轴，最终到达足底参考点。每个关节的齐次变换矩阵为：
T_i = Rot(x, α_{i-1}) · Trans(x, a_{i-1}) · Rot(z, θ_i) · Trans(z, d_i)正运动学即按顺序连乘所有关节的变换矩阵：\(T_{foot}^{hip} = T_1 \cdot T_2 \cdot T_3 \cdots T_n\)。最终得到的4×4矩阵中，左上3×3子矩阵为足端姿态（旋转矩阵R），右上3×1向量为足端位置（平移向量p）。
3.1.2 双足机器人DH参数表构建方法
以6自由度单腿为例（髋横滚、髋俯仰、髋偏航、膝俯仰、踝俯仰、踝横滚），建立DH参数表时需要遵循以下步骤：
- 确定关节轴线：沿每个关节的旋转轴方向定义z轴，x轴沿相邻z轴的公垂线方向。
- 测量几何尺寸：使用CAD模型或实际测量获得连杆长度（如大腿长度L1=0.4m，小腿长度L2=0.4m）和偏距。
- 填写参数表：按标准DH或改进DH约定填写四个参数，注意\(\theta_i\)为变量，其余为常量。
- 验证模型：将机器人置于零位姿态，检查正运动学输出是否与机械设计零位一致。
| 关节i | a_{i-1} (m) | α_{i-1} (rad) | d_i (m) | θ_i (变量) |
|---|---|---|---|---|
| 1 髋横滚 | 0 | π/2 | 0.085 | θ₁ |
| 2 髋俯仰 | 0 | -π/2 | 0 | θ₂ |
| 3 髋偏航 | 0.4 | π/2 | 0 | θ₃ |
| 4 膝俯仰 | 0 | -π/2 | 0.4 | θ₄ |
| 5 踝俯仰 | 0 | π/2 | 0 | θ₅ |
| 6 踝横滚 | 0 | 0 | 0.03 | θ₆ |
3.1.3 案例拆解：单腿摆动相正运动学计算
假设机器人处于单腿支撑相，摆动腿从初始位置抬起。给定关节角向量 \(q = [0.05, 0.3, -0.1, -0.6, 0.2, 0.03]\)（单位：rad），通过正运动学计算得到足端相对于髋关节的位置为 \(p = [0.02, -0.15, 0.72]^T\) 米，姿态矩阵R反映足底大致水平。该结果用于摆动腿轨迹跟踪控制中的前馈项。实际调试中发现，若髋偏航角θ₃存在0.01rad的零偏，足端横向位置误差会放大至约4mm，因此需要定期进行关节角零位标定。
3.2 逆运动学求解：解析法与数值迭代法
3.2.1 核心原理：从期望位姿反解关节角
逆运动学问题可表述为：已知足端期望位姿 \(T_{des}\)，求解关节角向量 \(q\) 使得 \(T(q) = T_{des}\)。对于双足机器人，由于髋关节存在三个旋转轴，且踝关节与膝关节之间存在耦合，解析解往往难以直接获得。数值迭代法通过线性化误差逐步逼近真实解，其核心思想是：
Δx = x_des - x_cur
Δq = J⁺ · Δx
q_new = q_cur + Δq其中 \(J⁺\) 为雅可比矩阵的伪逆（\(J⁺ = J^T(JJ^T)^{-1}\)）。为提高数值稳定性，常采用阻尼最小二乘法（DLS）：\(J^* = J^T(JJ^T + λ²I)^{-1}\)，其中λ为阻尼因子。
3.2.2 数值迭代求解流程
- 初始化：以当前关节角作为迭代初值 \(q_0\)。
- 正运动学计算：求当前位姿 \(T(q_k)\)，计算位姿误差 \(\Delta x\)（包含位置误差和姿态误差，姿态误差用旋转向量表示）。
- 雅可比计算：数值微分或解析法求取当前关节角下的雅可比矩阵 \(J(q_k)\)。
- 求解增量：\(\Delta q = J^T(JJ^T + λ²I)^{-1} \Delta x\)。
- 更新关节角：\(q_{k+1} = q_k + \Delta q\)。
- 收敛判断：若 \(\|\Delta x\| < \epsilon\)（如1e-6）或达到最大迭代次数（如50次），则停止。
3.2.3 案例拆解：落脚点规划中的逆解实时性优化
在动态步行中，落脚点需要以500Hz-1kHz的频率实时更新。某双足平台在采用纯数值迭代时，单次逆解耗时约2.3ms，无法满足1kHz控制周期。优化方案如下：
- 热启动：以上一控制周期的解作为初值，迭代次数从平均15次降至4次，耗时降至0.6ms。
- 解析-数值混合：利用踝关节两个轴的几何关系先解析求解踝关节角，再对髋关节三个轴进行数值迭代，将问题降维至3自由度。
- 结果对比：优化后逆解成功率从92%提升至99.7%，最大位置误差从3.2mm降至0.4mm。
3.3 雅可比矩阵与奇异性分析
3.3.1 核心原理：速度映射与力映射
雅可比矩阵 \(J(q)\) 建立了关节速度 \(\dot{q}\) 与足端笛卡尔速度 \(\dot{x}\) 之间的线性映射：\(\dot{x} = J(q)\dot{q}\)。同时，根据虚功原理，足端力 \(F\) 与关节力矩 \(\tau\) 满足 \(\tau = J^T F\)。雅可比矩阵的列数等于关节数（6），行数等于笛卡尔空间维数（通常为6，包含3个线速度和3个角速度）。
3.3.2 奇异性判断与分类
当雅可比矩阵的秩小于其最大可能秩时，机器人处于奇异位形。此时机器人丧失某个方向上的运动能力，且关节速度趋向无穷大。双足机器人常见的奇异位形包括：
| 奇异类型 | 发生条件 | 对行走的影响 | 规避策略 |
|---|---|---|---|
| 膝关节完全伸直 | θ₄ = 0 | 无法产生踝关节垂直方向速度 | 步态规划中限制膝关节最小弯曲角（如5°） |
| 髋关节与踝关节轴线重合 | θ₂ + θ₄ + θ₅ = 0 | 丧失绕某一水平轴的转动能力 | 增加髋关节偏航角补偿 |
| 足端位于髋关节正上方 | 横向位置为零 | 侧向运动不可控 | 避免规划此类落足点 |
3.3.3 案例拆解：上楼梯过程中的奇异规避
某双足机器人在攀爬15cm高台阶时，规划器给出的踝关节轨迹导致膝关节在某一时刻接近完全伸直（θ₄=2.3°），可操作度降至0.02。实测关节速度指令出现高频振荡，电流峰值超过额定值30%。
分析过程：通过计算雅可比矩阵的条件数（最大奇异值与最小奇异值之比），发现条件数高达850，表明矩阵严重病态。进一步检查发现，规划器未考虑膝关节角度下限约束。
解决方案：
- 在逆解目标函数中加入关节限位惩罚项：\(L = \|\Delta x\|^2 + w_q \cdot \max(0, \theta_{min} - \theta)^2\)。
- 将膝关节最小角度约束从0°调整为8°，牺牲5mm的足端高度余量。
- 在步态规划阶段增加奇异规避预检：若可操作度低于0.05，则自动调整髋关节高度。
结果：修改后最大条件数降至120，关节速度振荡消失，电流峰值回落至额定值以内。该案例表明，奇异性分析不能仅作为事后诊断，必须前馈到步态规划阶段。
3.4 常见误区与工程复盘
3.4.1 误区一：忽略DH参数约定差异
标准DH与改进DH在坐标系建立上存在差异，若混用会导致正运动学结果完全错误。建议团队统一采用一种约定，并在代码注释中明确标注。
3.4.2 误区二：数值迭代初值选取随意
初值距离真实解过远时，迭代可能发散或收敛到错误的解。对于双足机器人，应利用上一时刻的关节角作为初值，并设置合理的迭代上限和误差阈值。
3.4.3 误区三：忽视雅可比矩阵的维度匹配
当足端位置和姿态同时约束时，雅可比矩阵为6×6。但若只关心位置控制（如ZMP控制中的质心位置），可只取前3行，此时矩阵变为3×6，需使用伪逆求解。错误使用完整雅可比会导致姿态误差被过度放大。
运动学基础是连接机械本体与控制算法的桥梁。正运动学提供了从关节空间到位姿空间的确定性映射，逆运动学解决了轨迹跟踪中的关节角求解问题，而雅可比矩阵与奇异性分析则为速度规划和稳定性评估提供了理论工具。在实际双足机器人开发中，这三者往往需要联合使用：步态规划器输出足端轨迹，逆运动学模块实时求解关节角，雅可比矩阵则用于评估当前位形下的运动学性能并触发奇异规避。掌握这些方法，是后续学习ZMP控制、步态优化与全身动力学控制的前提。