1. 运动控制基础回顾：刚体动力学、运动学正解与逆解、关节空间与任务空间
各位同学，欢迎来到《人形机器人运动控制进阶与提升》的第一章。
说实话，很多做机器人控制的朋友，一上来就盯着MPC、WBC这些高级算法猛啃。结果呢？模型建错了，雅可比矩阵算反了，仿真里跑得飞起，一上真机就抖成筛子。我当年也干过这种蠢事——嗯，不提了。
这一章，咱们把地基夯实。刚体动力学、运动学正逆解、关节空间与任务空间，这几个概念你搞透了，后面学啥都快。
1.1 刚体动力学：机器人运动的“物理法则”
机器人本质上是一串用关节连起来的刚体。刚体动力学，就是描述这些刚体在力作用下怎么动的数学工具。
我个人习惯把动力学方程看作一个“力-加速度”的映射关系。最经典的形式是：
M(q) * q̈ + C(q, q̇) * q̇ + G(q) = τ这里：
- M(q) —— 惯性矩阵。描述每个关节的“重量感”。
- C(q, q̇) —— 科里奥利力和离心力项。关节速度越快，这玩意儿越明显。
- G(q) —— 重力项。机器人站着和躺着，需要的力矩完全不同。
- τ —— 关节驱动力矩。我们要算的，就是给多大劲儿才能让机器人动起来。
核心理解： 动力学方程不是用来“算一遍”的。在控制里，我们经常用它做两件事：
- 前向动力学： 给力矩，算加速度。仿真器里用的就是这个。
- 逆动力学： 给期望加速度，算需要多少力矩。这是实时控制的核心。
我在项目里遇到过一个问题：用逆动力学算出来的力矩，直接发给电机，结果机器人站都站不稳。后来发现，是重力项G(q)没更新——机器人稍微动了一下，重力补偿就不准了。所以，实时更新模型参数，比算法本身更重要。
我的习惯： 调试阶段，先把重力项单独拿出来测试。让机器人悬空，手动推一下各个关节，感受一下重力补偿是否平滑。这一步能筛掉80%的动力学模型错误。
1.2 运动学正解与逆解：关节角度 vs. 末端位置
运动学解决的是“关节怎么动，末端在哪”的问题。说白了，就是几何关系。
1.2.1 正运动学：从关节到末端
给你一组关节角度，算出末端执行器（比如手、脚）在空间里的位置和姿态。这个方向是确定的，没有歧义。
标准做法是用DH参数法建立连杆坐标系，然后连乘变换矩阵。我记得刚入行时，为了算一个六轴机械臂的正解，手算了一整个下午，结果还少乘了一个旋转矩阵……后来全用代码自动生成了。
# 伪代码示例：正运动学计算
def forward_kinematics(joint_angles):
T = eye(4) # 初始化为单位矩阵
for i in range(num_joints):
T_i = dh_transform(alpha[i], a[i], d[i], joint_angles[i])
T = T @ T_i
return T # 末端位姿矩阵1.2.2 逆运动学：从末端到关节
这个就麻烦多了。给你一个末端位姿，反推关节角度。人形机器人全身有30多个自由度，逆解通常没有封闭解，只能靠数值迭代。
我曾经踩过一个坑：用牛顿法做逆解，初始值给得离真实解太远，结果迭代发散，仿真里的机器人直接“反关节”扭断了。后来我学乖了，先用解析法算一个粗略的初始值，再用数值法精调。
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 解析法（封闭解） | 快、精确、无迭代 | 只适用于特定构型（如6轴球形腕） | 手臂、腿部局部逆解 |
| 数值法（牛顿法、LM法） | 通用、可处理冗余自由度 | 慢、依赖初始值、可能陷入局部最优 | 全身运动学、复杂约束 |
| 几何法 | 直观、可调参 | 需要人工推导、不通用 | 特定任务（如四足站立） |
注意： 逆运动学不是“算出来就能用”的。你还要考虑关节限位、自碰撞、奇异性。我建议在逆解之后，一定要加一层关节空间的安全校验，否则真机上一动就撞了。
1.3 关节空间与任务空间：两种控制视角
这是控制里最基础、也最容易混淆的一对概念。你想想看，我们控制机器人，到底控制什么？
- 关节空间： 控制每个电机的角度、速度、力矩。你直接跟电机打交道。
- 任务空间： 控制末端在三维空间里的位置、姿态、力。你关心的是“手往哪伸”，而不是“肩关节转了多少度”。
为什么要有这两个空间？因为人形机器人的任务天然是任务空间描述的——你要它去抓杯子，你不会说“肩关节转30度，肘关节转60度”，你会说“手伸到杯子那里”。但电机只听得懂关节空间的指令。所以，我们需要在这两个空间之间来回切换。
关键工具：雅可比矩阵 J
雅可比矩阵把关节速度映射到末端速度：
ẋ = J(q) * q̇反过来，通过伪逆，我们可以把任务空间的力/速度映射回关节空间：
τ = J(q)^T * F这就是力控的基础。没有雅可比，你没法在任务空间做阻抗控制、导纳控制。
我个人习惯在调试时，先在关节空间做一遍简单的PD控制，确保每个关节都能听话。然后再切换到任务空间，用雅可比把末端的误差映射回关节。这样出了问题，能快速定位是电机没响应，还是运动学算错了。
避坑指南： 我曾经在任务空间做力控时，忘了考虑雅可比矩阵的奇异性。结果机器人走到某个姿态，雅可比矩阵接近奇异，算出来的关节力矩巨大，差点烧了驱动器。所以，实时监测雅可比矩阵的条件数，是任务空间控制的基本素养。
1.4 本章知识体系总览
下面这张图，把本章的核心逻辑串起来了。你可以把它当作一个思维导图，每次做控制前，先过一遍这个流程。
这张图里，三个分支最终都指向了底部的核心工具——雅可比矩阵。你想想看，没有动力学，你算不出力矩；没有运动学，你找不到末端；没有空间概念，你没法把任务翻译成关节指令。这三者缺一不可。