02 · 三维空间数学
02 · 三维空间数学
这一章要解决什么问题：机械臂的"手"在空中的哪个位置、朝哪个方向？如何用数字精确描述它？
这是全书最重要的数学基础。不懂这一章，后面所有代码都只能照抄而不能理解。
配套代码：code/ch02_math_3d.py
本章学习目标
读完本章，你将能够：
- 在 Z-up 坐标系中描述位置和朝向：说出本项目的重力方向、up 轴，以及
PICK_POS = [-0.18, -0.30, 0.02]中每个数字的含义。 - 区分点和向量：理解"两个点相减得到向量"、"点加向量得到另一个点"的几何意义。
- 用三种方式表示朝向：旋转矩阵、欧拉角、四元数，并能在它们之间转换；知道 MuJoCo 用
(w,x,y,z)而 scipy 默认(x,y,z,w)。 - 构造和使用齐次变换矩阵：把旋转和平移统一成 4×4 矩阵，理解"写法 A vs 写法 B"的关键区别，能正确串联多个连杆。
- 求变换的逆：把世界坐标转换到局部坐标系，理解为什么旋转部分转置就是逆。
- 用 scipy.spatial.transform.Rotation 做实际计算：从欧拉角/四元数/旋转向量构造，输出各种格式，组合旋转。
📌 前置知识：本章需要第 01 章的 Python 基础（列表、函数、类）。如果你对矩阵完全陌生，建议先看任意线性代数入门教程的"矩阵乘法"一节。
2.1 从一个问题开始
假设机械臂的大臂长 0.3 米，根部在原点，绕 Y 轴（俯仰轴）转了 30°。大臂末端（手肘）在哪里？
import math
L = 0.3
theta = math.radians(30) # 30° → 弧度
x = L * math.sin(theta)
z = L * math.cos(theta)
print(f"手肘位置: ({x:.4f}, 0, {z:.4f})")
# 手肘位置: (0.1500, 0, 0.2598)
逐步推导（每一步在做什么）：
第1步：确定坐标系
本项目是 Z-up：+Z 向上，+X 向右，机械臂在 XZ（竖直）平面内运动，
俯仰关节绕 +Y 转 —— 所以旋转过程中 Y 坐标恒定不变。
这里取一个教学简化：初始时（θ=0）杆沿 +Z 竖直向上，末端在 (0, 0, 0.3)。
（实际机械臂零位是沿 −X 平伸，详见第 18 章模型分析。算法完全一样，
区别只在"0 度时杆指向哪个轴"。）
第2步：理解旋转的几何效果
绕 +Y 轴转 θ 角后，原来沿 +Z 的杆会朝 +X 方向"倒"过去。
这就像一根竖直插在地上的杆子被你推了一把，顶端画了一个圆弧。
（判断转轴的小技巧：**旋转时哪个坐标不变，转轴就是哪个轴**。
这里 y 始终为 0，所以转轴是 Y 轴。）
第3步：用三角函数分解
杆长 L = 0.3，角度 θ = 30°
（Z-up，XZ 平面视图：X 水平向右，Z 竖直向上 —— 图上画的就是真实方向，
不需要再做任何"心智旋转"）
──────────────────────────────────┐
│ Z轴（上） │
│ ↑ │
│ │ 末端 (x=0.15, z=0.2598) │
│ │ ╱ │
│ │ ╱ L=0.3 │
│ │ ╱ │
│ │╱ θ=30° │
│ └─────────────→ X轴（右） │
│ 原点 │
└──────────────────────────────────┘
x = L · sin(θ) = 0.3 × sin(30°) = 0.3 × 0.5 = 0.15
z = L · cos(θ) = 0.3 × cos(30°) = 0.3 × 0.8660 = 0.2598
y = 0（在 XZ 平面内旋转，Y 坐标不变）
第4步：得到结果
手肘位置 = (0.1500, 0, 0.2598)
💡 为什么 x 用 sin 而 z 用 cos？ 因为初始方向是 +Z（不是 +X）。如果初始方向是 −X（真实机械臂零位），那就是
x = -L·cos(θ), z = L·sin(θ)。关键是搞清楚"0 度时杆指向哪个轴"。
这就是正运动学的最简形态。但真实机械臂有 6 个关节，每个关节的旋转都会改变后面所有连杆的位置和朝向。手动算会疯掉。
解决方案是矩阵。本章的目标就是让你掌握这套"用矩阵描述空间变换"的语言。
2.2 坐标系、点与向量
2.2.1 坐标系：先确定"向上"是哪边
在三维空间里描述位置，第一步是约定坐标系。常见的有两种：
| 约定 | 含义 | 谁在用 |
|---|---|---|
| Y-up | Y 轴向上，重力 (0, -9.81, 0) | Unity、Unreal |
| Z-up | Z 轴向上，重力 (0, 0, -9.81) | MuJoCo 默认、ROS、Blender、本项目 |
本项目的坐标系（Z-up 右手系）示意图（参考 MuJoCo / ROS 标准）：
Z（上）
↑
│
│
│
−Y（前） │ ↘ +X（右）
└────── 原点
(基座中心)
机械臂零位沿 −X 方向平伸（臂平面 = XZ，所有连杆偏移 y = 0）
工作区在 −Y 侧（前方）：PICK / PLACE 的 y 分量都是负的
重力方向：(0, 0, -9.81) ← 沿 Z 轴负方向
在这个坐标系下（标准 Z-up 右手系）：
- X 轴 = 左右方向（+X 向右，−X 向左）← 机械臂零位平伸方向是 −X
- Y 轴 = 前后方向（−Y 朝前/工作区，+Y 朝后/朝观察者）
- Z 轴 = 上下方向（+Z 向上，−Z 向下，重力反方向）← 高度
⚠️ 本项目用的是 Z-up！模型文件里写着：
<option timestep="0.002" gravity="0 0 -9.81" .../>重力是
-9.81作用在 Z 轴上，所以 Z 是"上"。这意味着：机械臂的"高度"是 Z 坐标，
PICK_POS = [-0.18, -0.30, 0.02]里的0.02是物块中心的高度（物块是边长 0.04 的立方体，底面贴地 → 中心 z = 0.02，顶面 z = 0.04）。
PICK_POS 逐分量解读：
PICK_POS = [-0.18, -0.30, 0.02]
│ │ │
X Y Z
│ │ │
左侧-0.18m 前方-0.30m 高度+0.02m
（基座左侧 （基座前方 （物块中心，
18厘米） 30厘米） 底面贴地）
搞错 up 轴是新手最常见的崩溃来源——你会看到机器人躺在地上、或者物体往上飞。遇到这种情况，先检查重力方向。
2.2.2 点 vs 向量
- 点（position）：空间中的一个位置，
(0.3, 0.1, 0.5)。 - 向量（vector）：一个方向和长度，比如"向 X 正方向 0.5 米"。
两者都用 3 个数字表示，但含义不同。区分方法：
# 点：两个点相减 = 向量
p1 = np.array([0.3, 0.1, 0.5])
p2 = np.array([0.1, 0.1, 0.2])
v = p2 - p1 # 从 p1 指向 p2 的向量
# 点 + 向量 = 另一个点
p3 = p1 + v # = p2
2.3 向量运算（三个必须掌握的）
📌 打印 NumPy 数组前先设置格式。本书所有配套代码开头都有这一行：
import numpy as np np.set_printoptions(precision=4, suppress=True)
precision=4：浮点数只显示 4 位小数（不会打出一长串0.30000000000000004）。suppress=True：很小的数（如1e-15）直接显示为0.，而不是科学计数法。这两个参数让调试输出干净可读，机器人代码里几乎必加。
2.3.1 模长（长度）
length = np.linalg.norm(v) # sqrt(v·v)
几何意义：向量的"长度"或"大小"。
机器人里的用途：
- 计算末端到目标的距离：
distance = np.linalg.norm(target - ee_pos) - 判断是否到达目标：
if distance < 0.005: # 5mm 以内算到达 - 计算关节速度的大小：
speed = np.linalg.norm(qvel)
2.3.2 点积（dot product）
dot = np.dot(a, b) # 或 a @ b
几何意义：a · b = |a||b|cos θ，其中 θ 是两向量夹角。
b
╱
╱ θ
───────── a
a·b = |a| × |b| × cos(θ)
当 θ=90°（垂直）：cos(90°)=0 → a·b=0
当 θ=0°（同向）：cos(0°)=1 → a·b=|a||b|（最大）
当 θ=180°（反向）：cos(180°)=-1 → a·b=-|a||b|（最小）
最重要的用途：
- 判断垂直：
a · b == 0则垂直。 - 求投影：a 在 b 方向上的投影长度 =
a · b / |b|。 - 求夹角：
θ = arccos(a·b / (|a||b|))。
机器人里的用途：
- 判断两个方向是否对齐：比如夹爪朝向是否和物体表面垂直。
- 计算力在某个方向上的分量：比如重力在臂方向上的投影（用于力矩计算）。
- 判断末端移动方向是否正确：
move_dir · target_dir > 0表示在朝目标靠近。
2.3.3 叉积（cross product）
cross = np.cross(a, b) # 只在 3 维有定义
几何意义：结果是一个同时垂直于 a 和 b 的新向量，长度 = |a||b|sin θ（即两向量张成的平行四边形面积）。
a × b（结果向量，同时垂直于 a 和 b）
↑
│
│ b
│ ╱
│ ╱ θ
│ ╱
└──────── a
|a × b| = |a| × |b| × sin(θ) = 平行四边形面积
方向：右手定则——四指从 a 弯向 b，大拇指指向 a×b 的方向
在机器人里，叉积最重要的用途是求旋转轴——给定两个方向，叉积给出"从 a 转到 b 的旋转轴"。
z_axis = np.cross(x_axis, y_axis) # X 叉 Y = Z（右手定则）
💡 右手定则：右手四指从 a 弯向 b，大拇指指向的就是
a × b的方向。
机器人里的用途：
- 从两个方向向量构造坐标系：已知 X 轴和 Y 轴，
Z = X × Y。 - 求旋转轴：当前朝向和目标朝向的叉积就是"从当前转到目标"的旋转轴。
- 计算力矩：
τ = r × F（力臂叉乘力 = 力矩），这是动力学的基础。
2.4 描述"朝向"：三种方式
位置用 3 个数字就够了，但朝向有几种不同的表示法，这是新手最容易混乱的地方。
2.4.1 旋转矩阵（Rotation Matrix）
一个 3×3 矩阵。每一列是旋转后新坐标系的某个轴在原坐标系下的方向。
旋转矩阵的几何意义：
原坐标系（世界系） 旋转后的坐标系（局部系）
Y ↑ Y' ↑
│ │ ╱ X'
│ │ ╱
│ │ ╱ θ
└────────→ X └────────→ X
旋转矩阵 R 的三列 = 新坐标系的三个轴在原坐标系下的方向：
R = [ x'_x y'_x z'_x ] ← 第1列 = 新X轴在原系下的(x,y,z)
[ x'_y y'_y z'_y ] ← 第2列 = 新Y轴在原系下的(x,y,z)
[ x'_z y'_z z'_z ] ← 第3列 = 新Z轴在原系下的(x,y,z)
比如绕 Y 轴转 θ 后：
新 X 轴 = (cosθ, 0, -sinθ) ← 原来的 X 轴向 -Z 方向倒了 θ
新 Y 轴 = (0, 1, 0) ← Y 轴不变（绕 Y 转嘛）
新 Z 轴 = (sinθ, 0, cosθ) ← 原来的 Z 轴向 +X 方向倒了 θ
绕 Y 轴旋转 θ 的矩阵（本项目 Z-up，俯仰关节最常用）：
Ry(θ) = [ cos θ 0 sin θ ]
[ 0 1 0 ]
[-sin θ 0 cos θ ]
逐元素解释：
| 元素 | 值 | 含义 |
|---|---|---|
R[0,0] | cos θ | 新 X 轴在原 X 方向的分量 |
R[0,2] | sin θ | 新 Z 轴在原 X 方向的分量 |
R[1,1] | 1 | Y 轴不变（绕 Y 旋转） |
R[2,0] | -sin θ | 新 X 轴在原 Z 方向的分量（负号！） |
R[2,2] | cos θ | 新 Z 轴在原 Z 方向的分量 |
⚠️ 注意负号：
R[2,0] = -sin θ。绕 Y 轴旋转时，X 轴的正方向会向 -Z 方向偏转（右手定则：右手大拇指指向 +Y，四指弯曲方向就是正旋转方向，从 +X 转向 -Z）。这个负号是最容易写错的地方。
用矩阵旋转一个点：
import numpy as np
def rot_y(theta):
"""绕 Y 轴旋转 θ（弧度）的 3x3 旋转矩阵。"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[ c, 0.0, s],
[0.0, 1.0, 0.0],
[-s, 0.0, c],
])
p = np.array([0.3, 0.0, 0.0]) # X 轴上的点
p_rot = rot_y(np.pi / 2) @ p # 绕 Y 转 90°
print(p_rot) # [0, 0, -0.3]
逐步验证：p = (0.3, 0, 0)，绕 Y 转 90°：
x' = cos90° × 0.3 + sin90° × 0 = 0y' = 0（Y 不变）z' = -sin90° × 0.3 + cos90° × 0 = -0.3- 结果
(0, 0, -0.3)✓ —— X 轴上的点转到了 -Z 方向，和右手定则一致。
旋转矩阵的三个性质（很重要）：
- 正交：
R^T R = I（转置 = 逆）。 det(R) = 1。- 求逆很简单：
R⁻¹ = R^T（转置即可）。
性质 3 极其有用："撤销一个旋转"只需要转置，不用求逆矩阵。
绕 X 轴和 Z 轴的旋转矩阵
除了绕 Y 轴（本项目最常用），还有绕 X 轴和 Z 轴的旋转：
绕 X 轴旋转 θ：
Rx(θ) = [ 1 0 0 ]
[ 0 cos θ -sin θ ]
[ 0 sin θ cos θ ]
绕 Z 轴旋转 θ：
Rz(θ) = [ cos θ -sin θ 0 ]
[ sin θ cos θ 0 ]
[ 0 0 1 ]
记忆方法：绕哪个轴转，哪个轴的行列就是单位向量（不变），另外两个轴组成 2×2 旋转块。注意负号位置：
- 绕 X：负号在
[1,2]（第2行第3列） - 绕 Y：负号在
[2,0]（第3行第1列）——和 X/Z 不同！ - 绕 Z：负号在
[0,1]（第1行第2列）
⚠️ 绕 Y 轴的负号位置最容易记错。因为右手坐标系下，绕 Y 轴的正旋转方向是"从 Z 转向 X"（而不是"从 X 转向 Z"），所以负号在
R[2,0]而不是R[0,2]。
def rot_x(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
在本项目中的应用：
- J1 绕 Z 轴转（基座腰转，绕竖直轴）→ 用
rot_z - J2/J3/J5 绕 Y 轴转（俯仰）→ 用
rot_y - J4/J6 绕 X 轴转（腕部翻转/法兰自转，沿小臂方向）→ 用
rot_x
具体每个关节的旋转轴见第 00 章的关节规格表和第 05 章的 DH 参数。
2.4.2 欧拉角（Euler Angles）
用三个角度描述朝向，最直观：绕 X 转 α、绕 Y 转 β、绕 Z 转 γ。
# Roll-Pitch-Yaw（航空常用：滚转-俯仰-偏航）
roll, pitch, yaw = 0.1, 0.2, 0.3
优点：人类可读，示教器上显示的就是它。
缺点：
- 顺序依赖：先绕 X 还是先绕 Y，结果完全不同（必须约定顺序，如 “XYZ” 或 “ZYX”）。
- 万向锁（Gimbal Lock）：某些角度下会丢失一个自由度。
万向锁的直观理解：想象一个陀螺仪（三个环嵌套）。当中间那个环转到 90° 时，最内环和最外环的旋转轴变成了同一条线——此时无论转哪个，效果都一样，等于丢了一个自由度。这就是"万向锁"名字的由来。
在欧拉角中，当第二个旋转角（pitch）为 ±90° 时，第一个和第三个旋转轴重合，导致无法区分 roll 和 yaw。此时从旋转矩阵转回欧拉角会有无数种等价表示。
scipy 的 GimbalLock 警告：当你调用 r.as_euler("XYZ") 且旋转恰好处于（或接近）万向锁奇异点时，scipy 会发出 GimbalLockWarning。这不是错误，而是提醒你：“当前姿态下欧拉角表示不稳定，换一组欧拉角顺序或用四元数更可靠。”
from scipy.spatial.transform import Rotation as R
import numpy as np
# pitch=90° 正好是 XYZ 欧拉角的万向锁奇异点
r = R.from_euler("XYZ", [0, np.pi/2, 0])
print(r.as_euler("XYZ", degrees=True))
# 可能输出: [0. 90. 0.] 并伴随 GimbalLockWarning
# 也可能输出: [30. 90. -30.] 等——都是同一个旋转的等价表示！
⚠️ 这就是为什么内部计算不要用欧拉角。万向锁下欧拉角不唯一、不连续，做插值或微分都会出问题。内部用四元数或旋转矩阵，只在最终显示给人看时才转欧拉角。
⚠️ 永远要问"欧拉角的顺序是什么"。MuJoCo 的
euler属性顺序可在<compiler eulerseq="XYZ">配置，默认是XYZ。
2.4.3 四元数（Quaternion）
用 4 个数字 (w, x, y, z) 描述朝向。数学上优雅，没有万向锁，插值平滑。
四元数是什么？大白话解释：
想象你要描述"一个物体怎么转"，最直观的方式是说：
- 绕哪个轴转（一个 3D 方向向量，比如"绕 Y 轴"）
- 转多少度（一个角度，比如"转 90°"）
四元数就是把这两样东西打包成 4 个数字：
w= 跟"转多少度"有关（w = cos(θ/2)）(x, y, z)= 跟"绕哪个轴转"有关（轴 × sin(θ/2)）
四元数 = (w, x, y, z)
│ └──┬──┘
│ └── 旋转轴方向（乘以 sin(θ/2)）
└───────── 旋转角度（cos(θ/2)）
比如：绕 Y 轴转 90°
θ = 90°, 轴 = (0, 1, 0)
w = cos(45°) = 0.7071
x = 0 × sin(45°) = 0
y = 1 × sin(45°) = 0.7071
z = 0 × sin(45°) = 0
四元数 = (0.7071, 0, 0.7071, 0)
💡 为什么用 θ/2 而不是 θ？ 这是四元数的数学性质决定的——旋转 360° 时四元数会变成
(-w, -x, -y, -z)（取负），旋转 720° 才回到原值。这就是所谓的"四元数双覆盖"。但对我们来说，只需要知道 q 和 -q 表示同一个旋转就行了。
几何含义：绕单位轴 (x, y, z) 旋转角度 θ，则：
w = cos(θ/2)
x = axis_x · sin(θ/2)
y = axis_y · sin(θ/2)
z = axis_z · sin(θ/2)
⚠️ 本书实测踩坑：四元数的顺序！
系统 顺序 说明 MuJoCo (w, x, y, z) w 在前！ scipy Rotation(x, y, z, w) 默认 w 在后 ROS / Eigen (x, y, z, w) 同 scipy 两者互转时必须显式指定顺序，否则旋转结果会莫名其妙地错。
用 scipy 做四元数转换（本书推荐方式）：
from scipy.spatial.transform import Rotation as R
# 从 MuJoCo 的四元数 (w,x,y,z) 构造，注意 scalar_first=True
quat_wxyz = np.array([0.7071, 0.7071, 0.0, 0.0]) # MuJoCo 格式
rot = R.from_quat(quat_wxyz, scalar_first=True)
# 转成旋转矩阵
print(rot.as_matrix())
# 转成欧拉角（默认内旋 XYZ）
print(rot.as_euler("XYZ", degrees=True))
scalar_first=True是 scipy 1.4+ 的参数，专门用于对接 MuJoCo 的 (w,x,y,z) 格式。这是本书最重要的一个 API 细节。
四元数的基本运算
虽然本书推荐用 scipy 做四元数运算，但了解基本原理有助于调试：
1. 单位四元数（无旋转）：(1, 0, 0, 0) —— w=1, x=y=z=0，表示"不转"。
2. 四元数的逆：对于单位四元数，逆 = 共轭（把 x,y,z 取负）：
q⁻¹ = (w, -x, -y, -z)
这和旋转矩阵的"转置=逆"是对应的。
3. 四元数乘法（组合旋转）：
q1 ⊗ q2 = 先做 q2 的旋转，再做 q1 的旋转
注意顺序和矩阵乘法一样，从右往左应用。
4. 用四元数旋转一个向量：
v' = q ⊗ (0, vx, vy, vz) ⊗ q⁻¹
把向量转成"纯四元数"（w=0），左乘 q 右乘 q⁻¹。
💡 实际项目中不用手写这些运算，全部用
scipy.spatial.transform.Rotation：r = R.from_quat(q_wxyz, scalar_first=True) r_inv = r.inv() # 逆旋转 r_combined = r1 * r2 # 组合旋转（先 r2 后 r1） v_rotated = r.apply(v) # 旋转向量
2.4.4 三种表示法怎么选
| 场景 | 用什么 | 理由 |
|---|---|---|
| 给人看 | 欧拉角 | 直观 |
存进 MuJoCo data.qpos | 四元数 (w,x,y,z) | MuJoCo 内部格式 |
| 做数学运算/插值 | 旋转矩阵 / 四元数 | 无万向锁 |
| 组合多个旋转 | 旋转矩阵 | 矩阵乘法最直接 |
本书的约定：内部计算用旋转矩阵和四元数，只在打印时转欧拉角。
旋转的组合与顺序
多个旋转组合时，顺序极其重要——矩阵乘法不满足交换律，R1 @ R2 ≠ R2 @ R1。
先绕 X 转 90°，再绕 Y 转 90°：
R_total = Ry(90°) @ Rx(90°) （从右往左：先 Rx 后 Ry）
先绕 Y 转 90°，再绕 X 转 90°：
R_total = Rx(90°) @ Ry(90°) （从右往左：先 Ry 后 Rx）
这两个结果完全不同！
直观理解：想象你手里拿着一本书。
- 先把书绕 X 轴（左右方向）翻 90°（书立起来），再绕 Y 轴（前后方向）转 90°——书的朝向是 A。
- 先绕 Z 轴转 90°，再绕 X 轴翻 90°——书的朝向是 B，和 A 完全不同。
这就是为什么欧拉角必须约定顺序（如 “XYZ” 或 “ZYX”）——不同顺序结果完全不同。
在机器人中的应用：正运动学就是把 6 个关节的旋转按顺序乘起来：
T_total = T1 @ T2 @ T3 @ T4 @ T5 @ T6
↑ ↑
最先应用（基座） 最后应用（末端）
顺序错了，末端位置就完全错了。
2.5 齐次变换矩阵（本章核心）
2.5.1 为什么要"齐次"
旋转和平移是两种运算：
- 旋转：
p' = R · p（矩阵乘） - 平移：
p' = p + t（向量加）
把它们塞进一个 4×4 矩阵，就能用统一的矩阵乘法表达：
T = [ R t ] (R 是 3x3 旋转，t 是 3x1 平移)
[ 0 1 ] (最后一行固定是 0 0 0 1)
逐步推导：为什么补一个 1？
我们想要：p' = R·p + t
如果把 p 补成 [p, 1]^T（4维），把 R 和 t 塞进 4×4 矩阵：
[ R t ] [p] = [R·p + t·1] = [R·p + t] = [p']
[ 0 1 ] [1] [0·p + 1·1] [ 1 ] [1 ]
完美！一次矩阵乘法同时完成了旋转和平移。
最后那个 1 要在结果里保留，这样才能继续和下一个变换矩阵相乘。
使用时要给点补一个 1：p_homogeneous = [x, y, z, 1]
def make_transform(R, t):
"""把 3x3 旋转矩阵和 3 维平移向量组装成 4x4 齐次变换矩阵。"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
return T
def transform_point(T, p):
"""用 4x4 变换矩阵变换一个 3 维点。"""
p_h = np.append(p, 1.0) # [x, y, z] → [x, y, z, 1]
p_new = T @ p_h
return p_new[:3] # 去掉最后的 1
逐行解读 transform_point：
np.append(p, 1.0)：把 3 维点补成 4 维齐次坐标，最后一位是 1。T @ p_h：4×4 矩阵乘 4 维向量，得到变换后的 4 维齐次坐标。p_new[:3]：取前 3 个元素，就是变换后的 3 维点（丢掉最后的 1）。
一个矩阵同时表达了"在哪里"和"朝哪个方向" —— 这就是"位姿（pose）"。
2.5.2 从矩阵里读信息
T = np.eye(4)
position = T[:3, 3] # 最后一列的前三个 = 位置
rotation = T[:3, :3] # 左上 3x3 = 旋转
MuJoCo 和 ikpy 里到处都是这种 4×4 矩阵：
data.site_xpos→ 位置（3 维）data.site_xmat→ 旋转矩阵（9 维，需 reshape 成 3×3）chain.forward_kinematics(q)→ 4×4 齐次变换矩阵
2.5.3 ⚠️ 关键陷阱：两种"旋转+平移"的语义
这是初学者最常踩的坑，也是正运动学的核心语义，务必分清：
可视化对比：
写法 A：make_transform(R, t) → p' = R·p + t
─────────────────────────────────────────────
平移量 t 是【世界坐标系下】的固定偏移，不受旋转 R 影响。
世界系 Z ↑
│
│ t=(0,0,0.3) ← 平移在世界系的 Z 方向
│ ●
│ ╱ 杆（旋转后）
│ ╱
│ ╱ R=绕Y转45°
│ ╱
└────────→ 世界系 X
原点
作用于原点 → 得到 t 本身 = (0, 0, 0.3)
（平移"没跟着转"，杆的根部在原点，末端在(0,0,0.3)，杆本身转了45°）
写法 B：make_transform(R, 0) @ make_transform(I, t) → p' = R·(p + t)
─────────────────────────────────────────────────────────────────────
先沿【本地方向】平移 t，再整体旋转 R。
世界系 Z ↑
│
│ ● 末端 = R·t = (0.212, 0, 0.212)
│ ╱
│ ╱ 杆（先沿本地Z伸0.3，再整体转45°）
│ ╱
│ ╱
│ ╱
└────────→ 世界系 X
原点
作用于原点 → 得到 R·t = (0.212, 0, 0.212)
（平移"跟着转了"——杆先沿自己的Z方向伸出去，再整体转45°）
# 写法 A：make_transform(R, t)
# 数学含义 p' = R·p + t
# 平移量 t 是【世界坐标系下】的固定偏移，不受旋转 R 影响。
# 作用于原点 → 得到 t 本身
T_A = make_transform(rot_y(45°), [0, 0, 0.3])
T_A @ [0,0,0] → [0, 0, 0.3] # 平移"没跟着转"
# 写法 B：make_transform(R, 0) @ make_transform(I, t)
# 数学含义 p' = R·(p + t)
# 先沿【本地方向】平移 t，再整体旋转 R。
# 作用于原点 → 得到 R·t
T_B = make_transform(rot_y(45°), [0,0,0]) @ make_transform(I, [0, 0, 0.3])
T_B @ [0,0,0] → [0.2121, 0, 0.2121] # 平移"跟着转了"
机器人串联连杆必须用写法 B。因为现实中，大臂转 45° 时，挂在它末端的小臂是跟着一起转的——小臂末端的位移会被大臂的旋转带着走。
生活类比：
- 写法 A = 你站在原地转了个身（旋转），然后往前走了 3 步（平移在世界系的前方）。你的位置 = (往前走3步)，转身不影响你走的方向。
- 写法 B = 你先往前走了 3 步（沿你自己的前方），然后你和你走过的路径一起转了 45°。你的最终位置 = 旋转后的(3步前方)。
机械臂的关节就是写法 B：大臂先"伸出去"（沿自己的连杆方向），然后大臂的旋转会把整个小臂和末端一起带着转。
把它封装成一个函数，后面第 5 章会反复用到：
def link_transform(rot, local_offset):
"""一个连杆的变换：先沿本地方向平移 local_offset，再应用旋转 rot。"""
return make_transform(rot, np.zeros(3)) @ make_transform(np.eye(3), local_offset)
💡 一句话记忆：写法 A 是"在世界系里挪一下再转"，写法 B 是"沿自己的方向伸出去再整体转"。机械臂关节属于后者。
2.6 变换的复合（串联多个关节）
假设：大臂把末端送到 A 位置，小臂在此基础上再送一段。总的变换是矩阵相乘：
T_total = T_base @ T_upper @ T_lower
顺序极其重要：矩阵乘法不满足交换律。A @ B ≠ B @ A。
从右往左读：先应用 T_lower，再 T_upper，再 T_base。
这正是正运动学的核心——把每个关节的变换矩阵依次乘起来，就得到末端位姿（详见第 05 章）。
# 大臂：沿本地 Z 伸出 0.3 米，再整体绕 Y 转 30°（注意是写法 B）
T1 = make_transform(rot_y(np.pi/6), np.zeros(3)) # 旋转
T2 = make_transform(np.eye(3), np.array([0, 0, 0.3])) # 本地平移
T_total = T1 @ T2 # 从右往左：先平移，再旋转
end = transform_point(T_total, np.array([0, 0, 0]))
print(f"末端位置: {end}") # [0.15 0. 0.2598]
结果和 2.1 节手算的完全一致！这就是矩阵方法的威力——无论多少关节，都是一路乘下去。
两个连杆串联：
T_a = link_transform(rot_y(np.radians(45)), [0, 0, 0.3]) # 大臂
T_b = link_transform(rot_y(np.radians(-30)), [0, 0, 0.2]) # 小臂
end = transform_point(T_a @ T_b, [0, 0, 0])
print(end) # [0.2639 0. 0.4053]
手算交叉验证：大臂末端在 [0.2121, 0, 0.2121]；小臂相对大臂的朝向是 45° + (-30°) = 15°，贡献 [0.2·sin15°, 0, 0.2·cos15°] = [0.0518, 0, 0.1932]。相加得 [0.2639, 0, 0.4053] ✓
2.7 变换的逆（“反过来”）
已知末端在世界坐标的位置，想知道它在某个连杆的局部坐标系下的位置，用逆变换：
T⁻¹ = [ R^T -R^T·t ]
[ 0 1 ]
逐步推导：
已知 T = [R t]，求 T⁻¹ 使得 T · T⁻¹ = I（单位矩阵）
[0 1]
设 T⁻¹ = [A b]，则：
[0 1]
T · T⁻¹ = [R t] [A b] = [R·A + t·0 R·b + t·1] = [R·A R·b + t]
[0 1] [0 1] [0·A + 1·0 0·b + 1·1] [ 0 1 ]
要等于单位矩阵 I = [I 0]：
[0 1]
第1块：R·A = I → A = R⁻¹ = R^T（旋转矩阵的逆=转置）
第2块：R·b + t = 0 → R·b = -t → b = -R⁻¹·t = -R^T·t
所以 T⁻¹ = [R^T -R^T·t]
[ 0 1 ]
def invert_transform(T):
"""求 4x4 齐次变换矩阵的逆。"""
T_inv = np.eye(4)
R = T[:3, :3]
t = T[:3, 3]
T_inv[:3, :3] = R.T # 旋转部分：转置即是逆
T_inv[:3, 3] = -R.T @ t # 平移部分
return T_inv
逐行解读：
np.eye(4)：创建 4×4 单位矩阵作为基础。R = T[:3, :3]：取出左上 3×3 旋转部分。t = T[:3, 3]：取出最后一列的前 3 个元素 = 平移向量。T_inv[:3, :3] = R.T：旋转部分的逆 = 转置（利用旋转矩阵的正交性）。T_inv[:3, 3] = -R.T @ t：平移部分按公式-R^T·t计算。
注意：这里用了旋转矩阵的性质
R⁻¹ = R^T。如果直接调用np.linalg.inv(T)也可以，但理解原理更重要。为什么不直接用
np.linalg.inv(T)？ 因为：
- 手动公式更快（不需要做高斯消元）。
- 手动公式数值更稳定（利用了旋转矩阵的正交性，不会累积浮点误差）。
- 更重要的是——理解公式能让你知道逆变换在几何上做了什么：先反向旋转（RT），再反向平移（-RT·t）。
2.8 用 scipy 处理旋转（推荐做法）
手写旋转矩阵容易出错。实际项目请用 scipy.spatial.transform.Rotation：
from scipy.spatial.transform import Rotation as R
# 各种构造方式
r1 = R.from_euler("XYZ", [0, np.pi/2, 0]) # 从欧拉角（指定顺序！）
r2 = R.from_quat([0, 0, 0.7071, 0.7071]) # 从四元数 (x,y,z,w)
r3 = R.from_rotvec([0, np.pi/2, 0]) # 从旋转向量（轴*角度）
r4 = R.from_matrix([[0,0,1],[0,1,0],[-1,0,0]]) # 从 3x3 旋转矩阵
# 各种输出
r1.as_matrix() # → 3x3 旋转矩阵
r1.as_quat() # → (x,y,z,w) 四元数
r1.as_euler("XYZ", degrees=True) # → 欧拉角（度）
r1.as_rotvec() # → 旋转向量（轴*角度）
# 组合旋转
r_total = r1 * r2 # 注意用 * 不是 @
# 旋转一个向量
v_rotated = r1.apply([1, 0, 0])
四种构造方式详解
| 方法 | 输入格式 | 适用场景 |
|---|---|---|
from_euler(seq, angles) | 顺序字符串 + 3 个角度 | 从示教器/配置文件读取姿态 |
from_quat(quat, scalar_first=False) | 4 个数字，默认 (x,y,z,w) | 从 MuJoCo qpos 读取（需 scalar_first=True） |
from_rotvec(rotvec) | 3 个数字 = 旋转轴 × 角度 | 从轴角表示构造，数值最稳定 |
from_matrix(matrix) | 3×3 矩阵 | 从 MuJoCo xmat 读取（需先 reshape） |
旋转向量（rotvec）详解：这是最紧凑的旋转表示，用一个 3 维向量同时表达"绕哪个轴"和"转多少度"：
旋转向量 v = 轴 × 角度
例如：[0, 0, π/2] 表示：
轴方向 = (0, 0, 1) = Z 轴正方向
角度 = π/2 = 90°
含义 = 绕 Z 轴正方向转 90°
向量的长度 = 旋转角度（弧度）
向量的方向 = 旋转轴（右手定则：大拇指指向轴正方向，四指弯曲方向为正旋转）
# 绕 Z 轴转 90°
r = R.from_rotvec([0, 0, np.pi/2])
print(r.as_euler("XYZ", degrees=True)) # [0, 0, 90]
# 绕任意轴转：轴 (1,1,0) 归一化后转 45°
axis = np.array([1, 1, 0]) / np.sqrt(2)
angle = np.radians(45)
r = R.from_rotvec(axis * angle)
旋转向量的优点是没有万向锁、没有奇点（除了角度为 0 时轴方向无意义，但这是平凡情况），在数值优化和插值中非常稳定。第 06 章的 IK 迭代中，姿态误差就常用旋转向量表示。
用 apply() 旋转向量
有了 Rotation 对象后，用 .apply(v) 把它作用到一个向量上：
r = R.from_euler("XYZ", [0, np.pi/2, 0]) # 绕 Y 转 90°
v = np.array([1.0, 0.0, 0.0]) # X 轴上的点
v_rot = r.apply(v)
print(v_rot) # [0, 0, -1] —— X 轴上的点绕 Y 转 90° 后到了 -Z 方向
apply() 等价于 r.as_matrix() @ v，但更简洁、数值上也可能更高效（内部不一定真的构造矩阵）。
批量旋转多个向量：apply() 支持 (N, 3) 的数组输入，一次性旋转 N 个向量（向量化，无循环）：
points = np.array([[1,0,0], [0,1,0], [0,0,1]]) # 3 个点
rotated = r.apply(points) # 一次性全部旋转
print(rotated.shape) # (3, 3)
组合旋转的顺序
用 * 组合两个 Rotation 对象，从右往左应用（和矩阵乘法一致）：
r1 = R.from_euler("XYZ", [0, np.pi/2, 0]) # 先做：绕 Y 转 90°
r2 = R.from_rotvec([0, 0, np.pi/2]) # 后做：绕 Z 转 90°
r_total = r1 * r2
# 等价于：先应用 r2（绕 Z 转 90°），再应用 r1（绕 Y 转 90°）
# 数学上：R_total = R1 · R2，向量 v' = R1 · (R2 · v)
⚠️ 组合旋转后转回欧拉角可能触发 GimbalLockWarning。如果
r1含 90° pitch，r1 * r2的结果可能正好落在万向锁奇异点上，此时as_euler("XYZ")会发出警告。这是预期行为，不是 bug——换一种欧拉角顺序（如 “ZYX”）或直接用四元数/矩阵即可。
与 MuJoCo 对接的四个转换（背下来）
# ① MuJoCo 四元数 (w,x,y,z) → scipy Rotation
rot = R.from_quat(data.xquat[body_id], scalar_first=True)
# ② scipy Rotation → MuJoCo 四元数 (w,x,y,z)
q = rot.as_quat() # (x,y,z,w)
quat_wxyz = np.array([q[3], q[0], q[1], q[2]]) # 把 w 挪到最前面
# 或者用 np.roll 一行搞定：
quat_wxyz = np.roll(rot.as_quat(), 1) # (x,y,z,w) → (w,x,y,z)
# ③ MuJoCo 的 site_xmat（9 维扁平数组）→ 3x3 矩阵
rot_mat = data.site_xmat[site_id].reshape(3, 3)
# ④ 旋转矩阵 → scipy Rotation
rot = R.from_matrix(rot_mat)
📌 记忆口诀：MuJoCo 的 w 在前（wxyz），scipy 的 w 在后（xyzw）。
2.9 本项目的坐标约定总结
读本书实战部分时，请记住这张表：
| 项目 | 值 | 出处 |
|---|---|---|
| Up 轴 | Z | <option gravity="0 0 -9.81"/> |
| 角度单位 | 弧度 | <compiler angle="radian"/> |
| 四元数顺序 | (w, x, y, z) | MuJoCo 标准 |
| 取料点 | PICK_POS = [-0.18, -0.30, 0.02] | pick_and_place.py |
| 放料点 | PLACE_POS = [0.30, -0.12, 0.02] | pick_and_place.py |
| 接近高度 | APPROACH_HEIGHT = 0.16（加在 Z 上） | pick_and_place.py |
注意 APPROACH_HEIGHT 是加到 Z 分量上的（Z-up，Z 就是高度）：
above_pick = PICK_POS + np.array([0.0, 0.0, APPROACH_HEIGHT])
# ^^^^^^^^^^^^^^ Z 方向 = 上方
2.10 动手练
-
手算验证：大臂长 0.3 m，绕 Y 轴（俯仰）转 45°，求末端位置。先用三角函数手算，再用矩阵验证。
-
两级串联：在练习 1 的基础上，末端再接一根 0.2 m 的小臂，绕 Y 轴再转 -30°。求最终位置。（提示：两个 4×4 矩阵相乘）
-
四元数互通：把绕 Y 轴 90° 的旋转分别表示为旋转矩阵、欧拉角、四元数（MuJoCo 格式和 scipy 格式各写一遍），并验证三者等价。
-
逆变换：已知末端在世界坐标
[0.3, 0.2, 0.5]，基座变换矩阵为T（绕 Y 转 30° + 平移[0.1, 0, 0]），求末端在基座局部坐标系下的位置。
参考答案与解析
练习 1：手算 + 矩阵验证
手算（三角函数）：
θ = 45°, L = 0.3m，初始沿 Z 轴
x = L·sin(45°) = 0.3 × 0.7071 = 0.2121
z = L·cos(45°) = 0.3 × 0.7071 = 0.2121
y = 0
末端 = (0.2121, 0, 0.2121)
矩阵验证：
T = link_transform(rot_y(np.radians(45)), [0, 0, 0.3]) # 写法 B
end = transform_point(T, [0, 0, 0])
# end = [0.2121, 0, 0.2121] ✓ 与手算一致
常见错误：误用写法 A（make_transform(rot_y(45°), [0,0,0.3])）会得到 (0, 0, 0.3)——平移没被旋转，结果错误。
练习 2：两级串联
T_a = link_transform(rot_y(np.radians(45)), [0, 0, 0.3]) # 大臂
T_b = link_transform(rot_y(np.radians(-30)), [0, 0, 0.2]) # 小臂
end = transform_point(T_a @ T_b, [0, 0, 0])
# end = [0.2639, 0, 0.4053]
手算交叉验证：
- 大臂末端：
(0.2121, 0, 0.2121) - 小臂相对大臂的朝向：
45° + (-30°) = 15° - 小臂贡献：
x = 0.2·sin(15°) = 0.0518,z = 0.2·cos(15°) = 0.1932 - 相加：
(0.2121+0.0518, 0, 0.2121+0.1932) = (0.2639, 0, 0.4053)✓
练习 3：三种表示法互通
from scipy.spatial.transform import Rotation as R
r = R.from_euler("XYZ", [0, np.pi/2, 0]) # 绕 Y 转 90°
# 旋转矩阵
print(r.as_matrix())
# [[ 0. 0. 1.]
# [ 0. 1. 0.]
# [-1. 0. 0.]]
# 欧拉角（度）
print(r.as_euler("XYZ", degrees=True)) # [0, 90, 0]
# 四元数 scipy 格式 (x,y,z,w)
q_xyzw = r.as_quat() # [0, 0.7071, 0, 0.7071]
# 四元数 MuJoCo 格式 (w,x,y,z)
q_wxyz = np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]])
# [0.7071, 0, 0.7071, 0]
# 验证等价：从 MuJoCo 格式读回
r_back = R.from_quat(q_wxyz, scalar_first=True)
print(np.allclose(r_back.as_matrix(), r.as_matrix())) # True
练习 4：逆变换
T_base = make_transform(rot_y(np.radians(30)), [0.1, 0, 0])
p_world = [0.3, 0.2, 0.5]
p_local = transform_point(invert_transform(T_base), p_world)
# p_local ≈ [-0.0768, 0.2, 0.5330]
验证：transform_point(T_base, p_local) 应还原为 [0.3, 0.2, 0.5] ✓
完整可运行代码见
code/ch02_math_3d.py末尾。
2.11 常见错误与排查
| 症状 | 原因 | 解决方法 |
|---|---|---|
| 末端位置算出来和预期差很远 | 用了写法 A（make_transform(R,t)）而不是写法 B | 串联连杆必须用 link_transform()（写法 B） |
| 旋转方向反了（该左转的右转了） | 旋转矩阵的负号写错了 | 绕 Y 轴时 R[2,0] = -sinθ，绕 X/Z 轴也有对应负号 |
| 四元数转换后旋转完全不对 | 忘了 scalar_first=True | MuJoCo 是 (w,x,y,z)，scipy 默认 (x,y,z,w)，必须显式指定 |
data.site_xmat reshape 后形状不对 | 忘了 .reshape(3,3) | MuJoCo 的 xmat 是 9 维扁平数组，用前必须 reshape |
| 物体往上飞/机器人躺在地上 | up 轴搞错了（把 Y-up 当成 Z-up） | 检查模型文件的 gravity 属性，本项目是 0 0 -9.81（Z-up） |
| 角度算出来差了 57 倍 | 角度/弧度混用了 | 代码内部一律用弧度，math.sin() 接受弧度；显示时才转角度 |
| 矩阵乘法结果不对 | 用了 * 而不是 @ | * 是逐元素乘，@ 才是矩阵乘法 |
💡 排查流程：遇到位置/朝向算错时，按这个顺序检查：
- 坐标系对不对？（Y-up 还是 Z-up？本项目是 Z-up：高度 = Z 坐标）
- 单位对不对？（弧度还是角度？）
- 变换顺序对不对？（写法 A 还是写法 B？矩阵乘法从右往左读）
- 四元数顺序对不对？（w 在前还是在后？）
2.12 扩展阅读方向
- 《3D Math Primer for Graphics and Game Development》：最通俗易懂的 3D 数学入门书，覆盖了本章全部概念。
- Visualizing quaternions (3Blue1Brown)：https://www.youtube.com/watch?v=zjMuIxRvygQ （四元数的可视化解释，强烈推荐）
- Robotics: Modelling, Planning and Control (Siciliano)：机器人学经典教材，第 2 章讲空间描述和变换。
- scipy.spatial.transform 文档：https://docs.scipy.org/doc/scipy/reference/spatial.transform.html （官方 API 文档）
2.13 小结
- 描述一个刚体需要 6 个数：位置 3 个 + 朝向 3 个。
- 本项目是 Z-up 坐标系，重力
0 0 -9.81；高度是 Z 坐标，PICK_POS = [-0.18, -0.30, 0.02]中0.02是物块中心高度（物块边长 0.04，底面贴地）。零位时机械臂沿 −X 平伸。 - 点 vs 向量：点减点 = 向量，点加向量 = 另一个点；两者都用 3 个数字但含义不同。
- 向量三运算：模长（
np.linalg.norm）、点积（np.dot，求夹角/投影/判断垂直）、叉积（np.cross，求旋转轴/力矩）。 - 朝向有三种表示：
- 旋转矩阵（3×3，运算友好，每一列是新坐标系的轴方向）
- 欧拉角（3 个角度，人类友好，有万向锁，必须约定顺序）
- 四元数（4 个数字
(w,x,y,z)，无万向锁，MuJoCo 内部格式）
- **齐次变换矩阵（4×4）**把旋转和平移统一成一次矩阵乘法——这是正运动学的核心工具。
- 写法 A vs 写法 B：串联连杆必须用写法 B（先沿本地方向平移，再整体旋转），这是最常见的错误来源。
- 串联多个关节 = 矩阵依次相乘，顺序从右往左。
- 变换的逆：
T⁻¹ = [R^T, -R^T·t; 0, 1]，旋转部分转置即是逆。 - MuJoCo 四元数是 (w,x,y,z)，scipy 默认 (x,y,z,w)，转换时用
scalar_first=True。 - MuJoCo 的 xmat 是 9 维扁平数组，用前必须
.reshape(3,3)。
下一章用 NumPy 把这些数学真正跑起来。
上一章：01 · Python 基础速成 ｜ 下一章：03 · NumPy 实战
openEuler 是由开放原子开源基金会孵化的全场景开源操作系统项目，面向数字基础设施四大核心场景（服务器、云计算、边缘计算、嵌入式），全面支持 ARM、x86、RISC-V、loongArch、PowerPC、SW-64 等多样性计算架构
更多推荐
0
0- 0
已为社区贡献1条内容
所有评论(0)