2. 运动学基础（一）
各位，欢迎来到运动学基础的第一讲。
做四足机器人，你最先要搞定的，就是怎么描述它的身体和腿在空间里的位置和姿态。说白了，就是「它在哪」和「它朝哪看」。这两个问题搞不清楚，后面什么步态规划、地形适应，全是空中楼阁。
我个人习惯把这一章叫做「数学工具箱」。里面的工具不多，但每一个你都会反复用到。咱们一个一个来拆。
1. 刚体变换：从「点」到「身体」
先想一个问题：机器人的大腿，相对于它的身体，到底在什么位置？
我们通常把机器人每个部件都看作一个「刚体」——就是不会变形的物体。刚体在空间里有6个自由度：3个平移（前后、左右、上下），3个旋转（俯仰、偏航、滚转）。
刚体变换，就是描述从一个坐标系到另一个坐标系的运动。它包含两部分：
- 旋转：改变朝向
- 平移：改变位置
我在做第一个四足机器人仿真时，犯过一个低级错误：把旋转和平移的顺序搞反了。结果机器人的腿直接穿模到了地里。嗯，顺序很重要，后面会细说。
核心概念：刚体变换 = 旋转 + 平移。两者顺序不可交换。
2. 旋转矩阵：最直观的旋转表达
旋转矩阵，说白了就是一个3x3的方阵。它能把一个向量从一个坐标系，转到另一个坐标系。
它的数学形式长这样：
R = [[r11, r12, r13],
[r21, r22, r23],
[r31, r32, r33]]这个矩阵有几个硬规矩：
- 它是正交矩阵：R的转置等于R的逆（R^T = R^{-1}）
- 它的行列式为+1（保证是右手系，不会镜像翻转）
- 每一列都是单位向量，且两两正交
你想想看，这三个条件一卡，9个数里其实只有3个是自由的。这就是为什么后面会有欧拉角——用3个参数就能描述旋转。
我的经验：在实际编程中，我很少手动去算旋转矩阵的9个元素。都是用现成的库（比如NumPy的旋转模块，或者PyBullet的getMatrix）直接拿。但理解它的几何意义，能帮你debug。
绕三个坐标轴的基本旋转矩阵，你得记住：
| 旋转轴 | 旋转矩阵（绕θ角） |
|---|---|
| X轴 | [[1, 0, 0], [0, cosθ, -sinθ], [0, sinθ, cosθ]] |
| Y轴 | [[cosθ, 0, sinθ], [0, 1, 0], [-sinθ, 0, cosθ]] |
| Z轴 | [[cosθ, -sinθ, 0], [sinθ, cosθ, 0], [0, 0, 1]] |
注意符号。不同教材可能定义不一样（左手系 vs 右手系）。我建议你固定用右手系，跟ROS和大多数物理引擎保持一致。
3. 欧拉角：直观但暗藏陷阱
欧拉角，就是用三个角度来描述旋转。最常见的组合是：
- 滚转（Roll）：绕X轴
- 俯仰（Pitch）：绕Y轴
- 偏航（Yaw）：绕Z轴
听起来很直观对吧？但这里有个大坑——万向锁。
我曾经踩过的坑：有一次做四足机器人姿态控制，用欧拉角做插值。当俯仰角接近90度时，滚转和偏航突然变得无法区分，机器人直接原地打转。这就是万向锁——丢失了一个自由度。
为什么会这样？因为欧拉角的旋转顺序是固定的。比如ZYX顺序：先绕Z转，再绕Y转，最后绕X转。当第二次旋转（绕Y）达到±90°时，第一次和第三次的旋转轴会重合，自由度就从3降到了2。
所以我的建议是：
- 人机交互时用欧拉角（直观，好理解）
- 内部计算时用旋转矩阵或四元数（避免万向锁）
4. 齐次坐标：把旋转和平移统一起来
前面说了，刚体变换 = 旋转 + 平移。但如果我们只用3x3的旋转矩阵，平移就得单独加一个向量。这样写起来很啰嗦，而且没法把多次变换串起来。
齐次坐标就是来解决这个问题的。它把一个3D点 (x, y, z) 扩展成4D向量 (x, y, z, 1)。然后用一个4x4的齐次变换矩阵，把旋转和平移塞到一起：
T = [[R(3x3), t(3x1)],
[0 0 0, 1 ]]其中R是旋转矩阵，t是平移向量。用这个矩阵左乘一个点的齐次坐标，就能同时完成旋转和平移。
举个例子：
import numpy as np
# 定义旋转（绕Z轴转30度）和平移
theta = np.radians(30)
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]])
t = np.array([1.0, 2.0, 0.0])
# 构建齐次变换矩阵
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
# 定义一个点（在原始坐标系中）
p = np.array([0.5, 0.0, 0.0, 1.0]) # 齐次坐标
# 变换
p_new = T @ p
print(p_new[:3]) # 输出变换后的3D坐标你看，一行矩阵乘法，旋转和平移全搞定。这就是齐次坐标的魅力。
实用技巧：在仿真中，我习惯用齐次变换矩阵来维护机器人的「运动树」。每个关节、每条腿，都用一个4x4矩阵表示它相对于父节点的位姿。这样整个机器人的运动学链，就是一连串矩阵的乘积。
5. 知识结构总览
下面这张图，把本章的核心逻辑串起来了：
6. 本章小结
这一章的内容，是后面所有运动学计算的基石。你不需要死记硬背公式，但一定要理解：
- 刚体变换 = 旋转 + 平移
- 旋转矩阵是基础，但9个参数有冗余
- 欧拉角直观，但小心万向锁
- 齐次坐标把旋转和平移统一成4x4矩阵，是工程实现的首选
下一章我们会用这些工具，来推导四足机器人单腿的正运动学。到时候你会发现，今天学的这些矩阵乘法，会变成一条条腿的「关节角度 → 足端位置」的映射关系。
嗯，先把基础打牢。后面的事，后面再说。