第2章：空间几何基础——坐标系与刚体姿态描述

各位同学，欢迎来到第二章。

说实话，Stewart平台的核心就两件事：正解和逆解。但无论解什么，你都得先搞清楚——我们在哪个坐标系里说话？

这一章，我们就来把空间几何的基础打牢。别小看这些概念，我在项目里见过太多人因为坐标系搞反了，调试了三天才发现问题。嗯，咱们一步到位。

2.1 世界坐标系与动坐标系

先问个问题：你站在地面上，看到桌上一杯水。你说“杯子在桌子左边”。但如果我站在桌子对面，我说的“左边”可能就是你眼中的“右边”。

你看，同一个物体，参考系不同，描述就不同。

在Stewart平台里，我们通常定义两个坐标系：

• 世界坐标系（固定坐标系）：一般固定在地面或基座上。我习惯叫它 {B}（Base）。
• 动坐标系（运动坐标系）：固定在动平台上。我习惯叫它 {P}（Platform）。

动平台怎么动？说白了，就是 {P} 相对于 {B} 在平移和旋转。

2.2 刚体姿态描述

描述一个刚体的“姿态”，就是描述它转了多少。常用的方法有三种：旋转矩阵、欧拉角、四元数。

2.2.1 旋转矩阵

旋转矩阵是一个 3×3 的正交矩阵，行列式为 +1。它把动坐标系中的向量，映射到世界坐标系中。

举个例子：

# 绕Z轴旋转θ角的旋转矩阵
R_z(θ) = [[cosθ, -sinθ, 0],
          [sinθ,  cosθ, 0],
          [0,      0,    1]]

我在项目中遇到过一个问题：旋转矩阵虽然数学上很完美，但9个参数有6个约束条件，用起来不够直观。你想想看，让你直接写一个绕X轴转30度再绕Y轴转45度的矩阵，是不是有点头疼？

2.2.2 欧拉角

欧拉角就直观多了。它用三个角度来描述旋转：比如绕Z轴转γ（偏航）、绕Y轴转β（俯仰）、绕X轴转α（滚转）。

但注意——顺序很重要！Z-Y-X顺序和X-Y-Z顺序，结果完全不同。

所以，欧拉角适合人机交互（比如遥控器输入），但不适合内部连续运算。

2.2.3 四元数

四元数，说白了就是“没有奇点的旋转描述方式”。它用四个数表示：

q = [w, x, y, z]  其中 w² + x² + y² + z² = 1

四元数的好处：

• 无万向锁
• 插值平滑（球面线性插值，SLERP）
• 计算效率高

我个人建议：在控制算法内部，一律用四元数。只在最后输出给用户时，才转成欧拉角。

2.3 齐次变换矩阵

好了，现在我们把旋转和平移合在一起。

齐次变换矩阵是一个 4×4 的矩阵：

T = [[R, t],
     [0, 1]]

其中：
R：3×3 旋转矩阵
t：3×1 平移向量

它能把动坐标系中的点 p_P 变换到世界坐标系中：

p_B = T * p_P

举个例子：

# 动坐标系原点在世界坐标系中的位置为 (1, 2, 3)
# 动坐标系相对于世界坐标系绕Z轴转了30°
import numpy as np

theta = np.deg2rad(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], [2], [3]])

T = np.vstack([np.hstack([R, t]), [0, 0, 0, 1]])
print(T)

2.4 知识体系总览

下面这张图，是我自己梳理的本章知识结构。你看一眼，心里就有谱了。

2.5 避坑指南

最后，分享几个我自己的血泪教训：

• 坐标系方向要统一：我习惯用右手系，Z轴向上。如果你用左手系，所有旋转方向都会反。
• 欧拉角顺序要明确：代码里一定要注释清楚是 ZYX 还是 XYZ，不然别人看不懂。
• 四元数要归一化：计算误差会导致四元数模长偏离1，记得每步都归一化一次。
• 齐次变换矩阵的逆：不要直接求逆矩阵，用公式 T_inv = [R^T, -R^T*t; 0, 1]，效率高且数值稳定。

好了，这一章就到这里。坐标系和姿态描述是Stewart平台的“地基”，地基不牢，后面算法再漂亮也白搭。下一章，我们正式进入运动学分析。

---