3. 人形机器人建模
逆运动学，说白了就是“我知道手要伸到哪儿，你告诉我每个关节该转多少度”。
人形机器人有几十个自由度，腿要站稳，手要抓东西，头要跟着目标转。正向运动学是从关节角度算末端位姿，这个好办，链式乘一遍就行。但反过来——从末端位姿反推关节角度——就没那么直接了。
我刚开始做双足机器人那会儿，觉得逆运动学不就是解方程嘛。后来发现，人形机器人有冗余自由度，解不唯一，有时候甚至无解。嗯，这里面的坑，咱们一个一个填。
3.1 问题定义：从位姿到角度
先明确一下我们要解决什么问题。
给定：机器人末端（比如右手）的目标位置 p 和姿态 R（合起来叫位姿）。
求解：所有关节角度 q = [q₁, q₂, …, qₙ]ᵀ，使得正向运动学 f(q) 等于目标位姿。
数学上写就是：
f(q) = [R(q), p(q)] = [R_d, p_d]人形机器人跟工业机械臂最大的区别在于：人形有浮动基座。脚没固定在地上，躯干本身也在动。所以逆运动学不仅要管手臂，还要管腿、腰、甚至头部。我习惯把整个人形拆成几个链来处理：左腿链、右腿链、躯干+头链、左臂链、右臂链。
3.2 解析法：快，但有条件
解析法就是直接推导出关节角度的闭式表达式。对于某些特定构型的机器人，可以做到一次计算就出结果，速度极快。
3.2.1 什么时候能用解析法？
解析法要求机器人满足 Pieper准则：相邻三个关节轴交于一点，或者相邻三个关节轴平行。工业上很多6轴机械臂满足这个条件，所以能用解析法。
但人形机器人呢？说实话，大部分商用或开源人形机器人的手臂和腿都不满足Pieper准则。为什么呢？因为人形机器人关节布局模仿人体，肩关节是球副（3个旋转轴交于一点），但肘关节和腕关节往往不共点。腿也一样，髋关节三个轴交于一点，但膝关节和踝关节不交。
我遇到过的情况是：只有肩关节和髋关节可以用解析法快速求解，其他部分得另想办法。
3.2.2 解析法示例：人形机器人手臂
假设我们有一个7自由度手臂（肩3+肘1+腕3），肩关节三个轴交于一点。我们可以把逆运动学拆成两步：
- 位置解耦：先根据腕部位置反推肩关节角度（3个自由度，有解析解）。
- 姿态解耦：再根据末端姿态反推肘关节和腕关节角度（剩余4个自由度，其中1个是冗余的，可以选一个最优值）。
// 伪代码：解析法求解肩关节
// 已知：肩关节位置S，腕关节目标位置W，上臂长度L1，前臂长度L2
// 求解：肩关节俯仰角q1、偏航角q2、滚转角q3
d = ||W - S|| // 肩到腕的距离
if (d > L1 + L2 || d < |L1 - L2|) {
// 无解，目标不可达
return error;
}
// 余弦定理求肘关节角（这里肘关节是第4个关节）
q4 = acos((L1^2 + L2^2 - d^2) / (2 * L1 * L2));
// 然后根据几何关系求q1, q2, q3
// ... 具体公式略，不同构型不一样
3.3 数值法：通用，但需要迭代
数值法不依赖机器人构型，只要正向运动学能算，就能用。核心思想是：迭代优化，让当前位姿一步步逼近目标位姿。
3.3.1 雅可比矩阵法
这是最经典的方法。雅可比矩阵 J 把关节速度映射到末端速度：
ẋ = J(q) · q̇反过来，给定末端速度误差 Δx，我们可以求关节角度修正量 Δq：
Δq = J⁺ · Δx其中 J⁺ 是雅可比矩阵的伪逆。
迭代步骤：
- 计算当前位姿 f(q_current) 与目标位姿的误差 Δx
- 如果 ||Δx|| < 阈值，停止
- 计算雅可比矩阵 J(q_current)
- 计算 Δq = J⁺ · Δx
- 更新 q_current = q_current + Δq
- 回到步骤1
3.3.2 阻尼最小二乘法（DLS）
为了解决奇异问题，我们给伪逆加一个阻尼项：
J⁺ = Jᵀ · (J · Jᵀ + λ² · I)⁻¹λ 是阻尼系数。λ 越大，解越平滑，但收敛越慢。我一般这样调：
- 远离奇异时，λ = 0.01 ~ 0.1
- 接近奇异时，λ = 0.5 ~ 1.0
- 可以用奇异值分解（SVD）实时检测最小奇异值，动态调整λ
3.3.3 任务优先级法
人形机器人有多个任务同时要做：
- 首要任务：脚要踩稳（位置和姿态约束）
- 次要任务：手要跟踪目标
- 第三任务：躯干要保持水平，或者重心要落在支撑多边形内
用任务优先级法可以优雅地处理：
Δq = J₁⁺ · Δx₁ + (I - J₁⁺ · J₁) · J₂⁺ · Δx₂ + ...第一个任务占满可用自由度，剩下的自由度给第二个任务，以此类推。这样高优先级任务一定满足，低优先级任务在剩余空间里尽量优化。
3.4 解析法 vs 数值法：怎么选？
| 对比项 | 解析法 | 数值法 |
|---|---|---|
| 计算速度 | 极快（微秒级） | 较慢（毫秒级，需迭代） |
| 通用性 | 仅限特定构型 | 任意构型 |
| 处理约束 | 困难 | 容易（加惩罚项或投影） |
| 多解处理 | 可枚举所有解 | 只给一个解（依赖初值） |
| 奇异处理 | 需单独处理 | 阻尼法可自动处理 |
| 适合场景 | 离线规划、简单构型 | 实时控制、复杂构型 |
我个人习惯是：能解析就解析，解析不了就数值，两者结合最好。比如人形机器人的腿，髋关节用解析法快速算，膝关节和踝关节用数值法微调，这样既保证实时性，又保证精度。
3.5 避坑指南：我踩过的几个坑
- 关节限位没考虑：数值法迭代出来的角度可能超出关节物理限位。一定要在每次迭代后做限位投影。
- 收敛到局部最优：数值法依赖初值。初值不好，可能收敛到奇怪的姿态。我一般用上一帧的解作为初值，或者用解析法先给一个合理的初值。
- 计算雅可比矩阵太慢：如果每个关节都用差分法算雅可比，30个自由度就要算30次正向运动学。我建议用解析雅可比，或者用链式法则推导，能快一个数量级。
- 忽略了浮动基座：人形机器人的基座（躯干）本身也在动。逆运动学求解时，要把基座的位姿也作为变量一起优化，不能假设基座固定。
3.6 本章知识体系
下面这张图总结了逆运动学在人形机器人上的整体思路：
这张图把整个逆运动学求解流程串起来了。从目标位姿出发，先判断能不能用解析法，不能就转数值法。数值法里核心是雅可比伪逆加阻尼，同时要处理好任务优先级、关节限位和奇异规避。最后输出关节角度，交给底层控制器去执行。