📝 机器人学基础：坐标系与姿态变换学习笔记

第一部分：坐标系的构建 (Construction of Frames)

两个基本坐标系

• {W}\{W\}{W} 世界坐标系 (World Frame)：
  • 角色：绝对基准，不动的参考系（如：房间角落、地面）。
  • 视角：“上帝视角” / “定轴”。
• {B}\{B\}{B} 物体坐标系 (Body Frame)：
  • 角色：固连在物体上，随物体运动。
  • 设定规则：
    • 原点 (Origin)：通常设在物体的质心 (CoM)。
    • 轴向 (Basis)：依据物体几何形状或功能设定（如 X轴指机头，Z轴指天）。

向量的两种身份

1. 位置向量 (Position Vector)：描述“在哪儿”。即 {B}\{B\}{B} 的原点在 {W}\{W\}{W} 中的坐标（平移量）。
2. 基向量 (Frame Basis)：描述“朝哪儿”。即 {B}\{B\}{B} 的三个轴在 {W}\{W\}{W} 中的投影（旋转量/姿态）。

1. 世界坐标系 {W}\{W\}{W} 的构建标准

世界坐标系是静止不动的，但在不同行业有不同的“行规”：

• 方案 A：ENU (East-North-Up) —— 机器人/ROS/地面车辆标准
  • 原点：地图的某个固定点（如实验室门口）。
  • $X$ 轴：指向正东。
  • $Y$ 轴：指向正北。
  • $Z$ 轴：指向天空（重力反方向）。
• 方案 B：NED (North-East-Down) —— 航空航天/无人机/潜艇标准
  • 原点：起飞点。
  • $X$ 轴：指向正北。
  • $Y$ 轴：指向正东。
  • $Z$ 轴：指向地心（重力方向）。
  • 注意：因为Z轴向下，所以高度增加时，Z值是变负的。

2. 物体坐标系 {B}\{B\}{B} 的构建步骤

当你拿到一个刚体（比如无人机），你需要按以下步骤“画”出坐标系：

1. 定原点 (Origin)：
   • 找到物体的质心 (Center of Mass)。
   • 数学定义：${\mathbf{r}}_{cm}=\frac{\sum m_i{\mathbf{r}}_i}{\sum m_i}$
2. 定 $X_B$ 轴 (Forward)：
   • 通常定义为物体的运动前方。
3. 定 $Z_B$ 轴 (Vertical)：
   • 通常定义为物体的上方（如果是ENU系）或下方（如果是NED系）。
4. 定 $Y_B$ 轴 (Lateral)：
   • 必须通过右手定则由 $X$ 和 $Z$ 确定：${\hat{Y}}_B={\hat{Z}}_B\times {\hat{X}}_B$ (叉乘)。

---

第二部分：旋转矩阵 (Rotation Matrix) —— 数学核心

旋转矩阵 $R$ 是一个 $3\times 3$ 的矩阵，它是描述姿态的最底层数据。

1. 矩阵的几何定义

假设 {B}\{B\}{B} 系的三个基向量（单位向量）分别是 $\hat{u},\hat{v},\hat{w}$。
旋转矩阵 ${}^A{R}_B$ 的每一列，实际上就是 {B}\{B\}{B} 的基向量在 {A}\{A\}{A} 中的投影。

$${}^A{R}_B=\left[\begin{array}{ccc}|& |& |\\ {\hat{u}}_{\text{in }A}& {\hat{v}}_{\text{in }A}& {\hat{w}}_{\text{in }A}\\ |& |& |\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

• 第一列：{B}\{B\}{B} 的 X轴 在 {A}\{A\}{A} 看来指向哪里。
• 第二列：{B}\{B\}{B} 的 Y轴 在 {A}\{A\}{A} 看来指向哪里。
• 第三列：{B}\{B\}{B} 的 Z轴 在 {A}\{A\}{A} 看来指向哪里。

2. 核心性质

1. 正交性：$R^T=R^{-1}$。
   • 这意味着：矩阵的逆（反向旋转）等于矩阵的转置。这是计算效率极高的原因。
2. 行列式：$\det (R)=+1$。
   • 如果 $\det (R)=-1$，那不是旋转，那是镜像（物体被翻转了里外），物理上不可能发生。

3. 基础旋转公式（右手定则）

• 绕 X 轴旋转 $\varphi$ (Roll):
  $$R_x(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi & \cos \varphi \end{array}\right]$$
• 绕 Y 轴旋转 $\theta$ (Pitch):
  $$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$
• 绕 Z 轴旋转 $\psi$ (Yaw):
  $$R_z(\psi )=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

---

🎓 无人机雷达坐标变换计算实例

这是一个经典的刚体变换 (Rigid Body Transformation) 案例。

---

1. 场景参数定义 (The Setup)

首先，将物理世界的描述转化为数学变量。

A. 坐标系定义

• {W}\{W\}{W} 世界坐标系：房间（不动）。
• {B}\{B\}{B} 机体坐标系：无人机（随动）。

B. 已知条件

1. 平移量 (Translation)：
   无人机（{B}\{B\}{B} 原点）在 {W}\{W\}{W} 中的位置。
   $${}^W{P}_{B_{org}}=\left[\begin{array}{c}10\\ 10\\ 5\end{array}\right]$$

2. 旋转量 (Rotation)：
   无人机绕 Z轴 旋转了 $90^{\circ }$（向左转）。

   • Roll ($\varphi$) = $0^{\circ }$
   • Pitch ($\theta$) = $0^{\circ }$
   • Yaw ($\psi$) = $90^{\circ }$

3. 局部观测点 (Local Point)：
   雷达在 {B}\{B\}{B} 中检测到的障碍物坐标。
   $${}^{B}P=\left[\begin{array}{c}2\\ 3\\ 0\end{array}\right]$$
   (注：这里假设 X=2, Y=3, Z=0)

---

2. 核心公式 (The Formula)

我们的目标是求障碍物在世界坐标系中的位置 ${}^{W}P$：

$${}^{W}P=\underset{\text{步骤1：先旋转}}{\underbrace{{}^W{R}_B\cdot {}^{B}P}}+\underset{\text{步骤2：再平移}}{\underbrace{{}^W{P}_{B_{org}}}}$$

---

3. 详细计算流程 (Step-by-Step)

步骤 1：构建旋转矩阵

描述“绕Z轴旋转90度”的矩阵。公式如下：

$$R_z(\psi )=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

代入 $\psi =90^{\circ }$：

• $\cos (90^{\circ })=0$
• $\sin (90^{\circ })=1$

得到具体的数字矩阵：
$${}^W{R}_B=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$

**💡 **：

• 第1列 $(0,1,0)$：代表机体的旧X轴，现在指向了世界的Y轴方向。
• 第2列 $(-1,0,0)$：代表机体的旧Y轴，现在指向了世界的负X轴方向。

---

步骤 2：执行矩阵向量乘法 (Rotation)

这是最关键的一步。将旋转矩阵与局部点坐标相乘。

$$\text{旋转后向量}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}2\\ 3\\ 0\end{array}\right]$$

运算细节（行 × 列）：

• 计算新的 X 分量 ($x_{new}$)：
  取矩阵第1行，点乘向量。
  $$(0\times 2)+(-1\times 3)+(0\times 0)=0-3+0=-3$$

• 计算新的 Y 分量 ($y_{new}$)：
  取矩阵第2行，点乘向量。
  $$(1\times 2)+(0\times 3)+(0\times 0)=2+0+0=2$$

• 计算新的 Z 分量 ($z_{new}$)：
  取矩阵第3行，点乘向量。
  $$(0\times 2)+(0\times 3)+(1\times 0)=0+0+0=0$$

步骤 2 的中间结果：
相对位移向量现在变成了 $\left[\begin{array}{c}-3\\ 2\\ 0\end{array}\right]$。

---

步骤 3：执行向量加法 (Translation)

最后，将旋转后的相对位移，加上无人机的绝对位置。

$${}^{W}P=\underset{\text{旋转后的相对位移}}{\underbrace{\left[\begin{array}{c}-3\\ 2\\ 0\end{array}\right]}}+\underset{\text{无人机在房间的位置}}{\underbrace{\left[\begin{array}{c}10\\ 10\\ 5\end{array}\right]}}$$

逐分量相加：

• 最终 X：$-3+10=7$
• 最终 Y：$2+10=12$
• 最终 Z：$0+5=5$

---

4. 最终结果与物理验证

$$\text{障碍物绝对坐标}=(7,12,5)$$

🔍 结果是合理的

在脑海中模拟一下：

1. 位置：无人机在 $(10,10)$。
2. 姿态：无人机向左转了90度（机头朝北/Y轴）。
3. 障碍物：在无人机的“机身X轴方向”2米，“机身Y轴方向”3米。
   • 因为转了90度，无人机的“机身X轴”现在指向世界的Y轴。
   • 无人机的“机身Y轴”现在指向世界的负X轴。
4. 推导：
   • 原来的 $x=2$ (前/右) 变成了对世界Y轴的贡献 $\to$ $Y$ 增加了 2 (变为12)。
   • 原来的 $y=3$ (左/前) 变成了对世界X轴的负向贡献 $\to$ $X$ 减少了 3 (变为7)。

---

第三部分：欧拉角 (Euler Angles) —— 详细推导

采用 Z-Y-X (Yaw-Pitch-Roll) 顺序。

1. 旋转合成公式

旋转矩阵 ${}^W{R}_B$ 是通过三次连续旋转相乘得到的。因为是动轴 (Intrinsic) 旋转，乘法顺序是 $R=Z\cdot Y\cdot X$：

$$R_{ZYX}=R_z(\psi )\cdot R_y(\theta )\cdot R_x(\varphi )$$

2. 展开后的完整矩阵

需要写入代码的公式：

$$R=\left[\begin{array}{ccc}c\psi c\theta & c\psi s\theta s\varphi -s\psi c\varphi & c\psi s\theta c\varphi +s\psi s\varphi \\ s\psi c\theta & s\psi s\theta s\varphi +c\psi c\varphi & s\psi s\theta c\varphi -c\psi s\varphi \\ -s\theta & c\theta s\varphi & c\theta c\varphi \end{array}\right]$$
(简写：$c=\cos ,s=\sin$)

3. “万向节死锁”数学解释

请看矩阵的 $R_{31}$ 元素：$-sin(\theta )$。
如果 Pitch $\theta =90^{\circ }$，那么 $cos(\theta )=0$。
此时矩阵会退化，你会发现 Yaw ($\psi$) 和 Roll ($\varphi$) 的项混在了一起，无法区分。数学上称为秩亏 (Rank Deficiency)。

---

第四部分：四元数 (Quaternion) —— 底层原理

四元数是超复数 (Hyper-complex number)。

1. 定义

$$q=w+xi+yj+zk$$
或者写成向量形式：
$$q=[w,\mathbf{v}]=[w,(x,y,z)]$$

其中 $i,j,k$ 遵循哈密顿规则：$i^2=j^2=k^2=ijk=-1$。

2. 物理意义：轴-角 (Axis-Angle)

这是理解四元数的唯一途径。
假设旋转轴是单位向量 $\mathbf{n}=(n_x,n_y,n_z)$，旋转角度是 $\theta$。

$$\{\begin{array}{l}w=\cos (\frac{\theta }{2})\\ x=n_x\cdot \sin (\frac{\theta }{2})\\ y=n_y\cdot \sin (\frac{\theta }{2})\\ z=n_z\cdot \sin (\frac{\theta }{2})\end{array}$$

3. 模长约束

用于表示旋转的四元数必须是单位四元数：
$$||q||=\sqrt{w^2+x^2+y^2+z^2}=1$$

4. 四元数乘法（汉密尔顿积）

如果物体先做姿态 $q_1$，再做姿态 $q_2$，总姿态 $q_{total}$ 为：
$$q_{total}=q_2\otimes q_1$$
(注意：这里通常也是左乘逻辑，取决于库的定义，但标准数学定义是 $q_2{q}_1$)

公式非常复杂（通常由计算机完成）：
$$\left[\begin{array}{c}{w}_2\\ x_2\\ y_2\\ z_2\end{array}\right]\otimes \left[\begin{array}{c}{w}_1\\ x_1\\ y_1\\ z_1\end{array}\right]=\left[\begin{array}{c}{w}_1{w}_2-x_1{x}_2-y_1{y}_2-z_1{z}_2\\ w_1{x}_2+x_1{w}_2+y_1{z}_2-z_1{y}_2\\ w_1{y}_2-x_1{z}_2+y_1{w}_2+z_1{x}_2\\ w_1{z}_2+x_1{y}_2-y_1{x}_2+z_1{w}_2\end{array}\right]$$

3. 姿态表示法“三巨头”

名称	特点	优缺点	适用场景
旋转矩阵 (Rotation Matrix)	$3\times 3$ 矩阵 (9个数)	✅ 显卡计算快 ❌ 数据冗余	底层计算、坐标变换公式
欧拉角 (Euler Angles)	3个角 (Yaw, Pitch, Roll)	✅ 直观，人类易懂 ❌ 万向节死锁 (Gimbal Lock)	人机交互、数据显示
四元数 (Quaternion)	4个数 $(w,x,y,z)$	✅ 无死锁、插值丝滑 (SLERP) ❌ 极其不直观	内部物理引擎、姿态控制

四元数的直觉理解

• 轴-角 (Axis-Angle) 原理：空间中任意旋转，都可以看作是绕着一根特定的“魔法牙签”（轴 $\vec{n}$）转了特定的角度 $\theta$。

---

4. 关键疑问与误区澄清

**Q1: “Inverse is identical to X-Y-Z Fixed angle” **

• 误区：以为旋转顺序不重要。
• 正解：这是指**“动轴”与“定轴”的数学等价性**。
  • 动轴 Z-Y-X（第一人称，绕物体轴转）
  • 等价于 $⟺$
  • 定轴 X-Y-Z（第三人称，绕世界轴转，顺序倒过来）。

Q2: 为什么需要区分动轴和定轴？

• 动轴 (Body)：服务于控制与感知（飞行员视角：“抬头”、“左滚”）。
• 定轴 (World)：服务于导航与规划（GPS视角：“向北”、“向东”）。
• 例子：
  • 开车：你的左手边是动轴（随车动），地图上的东方是定轴（不动）。
  • 看手机：手机屏幕朝向是你定义的动轴，不管你躺着还是坐着看，屏幕相对手机不变。

第五部分：齐次变换矩阵 (Homogeneous Transformation Matrix)

齐次变换矩阵将旋转和平移合并为一个 $4\times 4$ 矩阵，便于计算机处理。
如果把上面的例子-无人机雷达坐标变换计算实例 作为具体案例，齐次变换矩阵表示如下：
$${}^W{T}_B=\left[\begin{array}{cc}{}^W{R}_B& {}^W{P}_{B_{org}}\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 10\\ 0& 0& 1& 5\\ 0& 0& 0& 1\end{array}\right]$$
具体计算步骤为：
$${}^W{P}_{homogeneous}={}^W{T}_B\cdot {}^B{P}_{homogeneous}$$
其中：
$${}^B{P}_{homogeneous}=\left[\begin{array}{c}2\\ 3\\ 0\\ 1\end{array}\right]$$
计算结果与之前的步骤一致，得到障碍物在世界坐标系中的位置：
$${}^{W}P=\left[\begin{array}{c}7\\ 12\\ 5\end{array}\right]$$