1. 运动学基础：刚体变换与齐次坐标

各位同学，欢迎来到《人形机器人动作规划与轨迹跟踪》的第一章。

做机器人运动学，说白了就是回答三个问题：它在哪？它朝哪？它怎么动？ 人形机器人有几十个关节，每个关节都在动，你得有个统一的数学语言来描述这一切。我个人习惯，一上来先把刚体变换和齐次坐标讲透——这是后面所有运动学、动力学、轨迹规划的基石。

1.1 位置与姿态：描述一个刚体的“状态”

想象你手里拿着一个手机。它在空间中的状态，由两部分组成：

位置：手机中心在哪儿（x, y, z）
姿态：手机屏幕朝哪、顶部指向哪（旋转）

这两者合起来，叫一个刚体的位姿。人形机器人的躯干、头部、手掌、脚掌，每个都是刚体，都需要用位姿来描述。

核心概念：位姿 = 位置 + 姿态。位置用三维向量，姿态用旋转矩阵。合在一起，就是4×4的齐次变换矩阵。

1.2 旋转矩阵：描述“朝哪”的数学工具

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^TR = I，det(R) = 1。它把一个向量从一个坐标系旋转到另一个坐标系。

举个例子，绕 Z 轴旋转 θ 角：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

绕 X 轴、Y 轴类似。三个轴的旋转组合，可以表达任意姿态。

我的经验：我在做双足平衡控制时，经常需要把躯干的姿态从IMU坐标系转换到世界坐标系。旋转矩阵虽然直观，但9个参数有6个约束，实际计算时容易引入数值误差。所以我后来更常用四元数——不过那是后话了，先把矩阵搞明白。

1.3 齐次坐标：把旋转和平移统一起来

你想想看，如果只用旋转矩阵 R 和平移向量 t，一个变换写成：

p' = R·p + t

这没问题。但如果你连续做多个变换，比如从世界→躯干→左臂→左手，公式会越来越长，越来越乱。

齐次坐标就是来解决这个问题的。它把一个三维点 (x, y, z) 扩展成四维 (x, y, z, 1)，把旋转和平移合并成一个 4×4 矩阵：

T = [R   t]
    [0   1]

这样，一次变换就变成了矩阵乘法：

p' = T · p

连续变换就是矩阵连乘：

p_hand = T_{world→torso} · T_torso→arm · T_arm→hand · p_{hand_local}

避坑指南：我曾经在写逆运动学求解器时，把齐次矩阵的乘法顺序搞反了。记住：左乘表示从当前坐标系变换到新坐标系，右乘表示在当前坐标系下做变换。人形机器人运动学里，绝大多数情况是左乘——从父坐标系到子坐标系。

1.4 刚体变换的性质

刚体变换有几个重要性质，做运动学时必须牢记：

性质	数学表达	物理含义
保距性	‖p' - q'‖ = ‖p - q‖	刚体上两点距离不变
保角性	夹角不变	刚体上两条线的夹角不变
可逆性	T^-1 = [R^T -R^Tt; 0 1]	变换可逆，逆变换就是反向
封闭性	T₁·T₂ 仍是刚体变换	连续变换结果仍是刚体变换

嗯，这里要注意：逆变换的公式很多人记错。我建议你手推一遍，比死记硬背强。

1.5 知识体系总览

下面这张图，把本章的核心逻辑串起来了。你可以把它当作整个运动学部分的“地图”。

1.6 实战：用Python实现刚体变换

光说不练假把式。下面是我写的一个简单实现，你可以在自己的项目里直接用：

import numpy as np

def rotation_x(theta):
    """绕X轴旋转矩阵"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1, 0, 0],
                     [0, c, -s],
                     [0, s, c]])

def rotation_y(theta):
    """绕Y轴旋转矩阵"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, 0, s],
                     [0, 1, 0],
                     [-s, 0, c]])

def rotation_z(theta):
    """绕Z轴旋转矩阵"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0],
                     [s, c, 0],
                     [0, 0, 1]])

def homogeneous_transform(R, t):
    """由旋转矩阵R和平移向量t构造齐次变换矩阵"""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def transform_point(T, p):
    """用齐次变换矩阵T变换点p（p是3维向量）"""
    p_h = np.append(p, 1)  # 转为齐次坐标
    p_h_transformed = T @ p_h
    return p_h_transformed[:3]  # 转回三维坐标

# 示例：先绕Z轴转30度，再沿X轴平移1米
theta = np.deg2rad(30)
R = rotation_z(theta)
t = np.array([1.0, 0.0, 0.0])
T = homogeneous_transform(R, t)

p = np.array([0.5, 0.0, 0.0])
p_new = transform_point(T, p)
print(f"变换后位置: {p_new}")

我的建议：刚开始学的时候，别急着用现成的库。自己手写一遍旋转矩阵和齐次变换，哪怕只是二维的，也能帮你建立直觉。我在带团队时，新来的实习生我都会让他们先手写这个——写一遍，比看十遍都管用。

1.7 本章小结

这一章我们讲了：

位姿 = 位置 + 姿态，是描述刚体的基本方式
旋转矩阵描述姿态，有3个自由度、9个参数
齐次坐标把旋转和平移统一成4×4矩阵，方便连续变换
刚体变换有保距、保角、可逆、封闭四个性质

这些东西看着简单，但后面每一章都会用到。尤其是齐次变换矩阵，正运动学、逆运动学、雅可比矩阵、轨迹插值……全都离不开它。你把它吃透了，后面的路就好走了。

📚 课程目录

1. 运动学基础：刚体变换与齐次坐标

1.1 位置与姿态：描述一个刚体的“状态”

1.2 旋转矩阵：描述“朝哪”的数学工具

1.3 齐次坐标：把旋转和平移统一起来

1.4 刚体变换的性质

1.5 知识体系总览

1.6 实战：用Python实现刚体变换

1.7 本章小结