跳转至

第 7 章 姿态与坐标表达

反馈量进回路之前，先得"说得出方向"。姿态（attitude）是估计链（第 8 章「滤波与融合」）与机器人学几何（robotics-note 第 2 章「旋转的表示与互转」）的接缝：本章取状态反馈镜头——SO(3) 上的误差怎么定义、角速度怎么映射到姿态微分、IMU 读数怎么进状态方程——把旋转的四表示、互转、以及万向锁/乘法序/Hamilton-vs-JPL 三类约定坑一次钉清。

几何侧（为 FK/雅可比服务的 SE(3) 复合互转）在 robotics-note，两章互相引用不重写。运行环境 python 3.11 + scipy 1.17.1（scipy.spatial.transform.Rotation 为主线）。

姿态进状态方程的两条输入，与三类约定坑的位置：

flowchart LR
    rep["四表示与互转"] --> err["SO(3) 误差<br/>R_err = R_d Rᵀ"]
    imu["IMU：角速度 ω、比力 a"] --> diff["姿态微分<br/>Ṙ = R[ω]×"]
    err --> fb["状态反馈用的姿态误差"]
    diff --> ekf["进第 8 章状态方程"]
    pit["约定坑：乘法序 / 万向锁 / Hamilton vs JPL"] -.-> rep

1. 四种表示与互转

同一旋转有四种常用写法，各有脾气：

  • 旋转矩阵 \(R\in SO(3)\)：9 个数、2 个约束（正交 + \(\det=1\)），复合是矩阵乘、作用于向量最省事，做优化的自由度过剩。
  • 轴角 / 旋转向量 \(\omega\theta\in\mathbb{R}^3\)：3 个数无冗余，李代数 \(\mathfrak{so}(3)\) 指数映射的落点，第 8 章「滤波与融合」做增量更新与误差就靠它。
  • 四元数 \(q=(x,y,z,w)\)，单位模：4 个数 1 个约束，无奇异、插值平滑（slerp），IMU/姿态库与 ROS 都用它。
  • 欧拉角（ZYX：偏航/俯仰/横滚）：3 个数最贴人体直觉，但有万向锁（见「万向锁」一节）、不可连续（同一姿态多组解）。

scipy 三件套一行互转，闭环自洽：

from scipy.spatial.transform import Rotation as R
r = R.from_euler('ZYX', np.radians([30, 45, 60]))
r.as_matrix(); r.as_quat(); r.as_rotvec()          # 矩阵 / 四元数(xyzw) / 轴角
## 四元数往返最大矩阵差 = 0.0；欧拉往返 = [30., 45., 60.]

约定要点两处：Rotation 的 as_quat() 返回 (x,y,z,w) 顺序（标量在末），与 ROS 的 geometry_msgs/Quaternion（也是 xyzw）一致，但与不少教材的 \((w,x,y,z)\) 相反——取数先确认标量位置。from_euler 的轴串大写=静态轴（内旋）、小写=动态轴（外旋），'ZYX' 与 'zyx' 给出不同结果，务必与来源文档对齐。

2. 乘法序：旋转不可交换

\(SO(3)\) 非交换群，"先 A 后 B"与"先 B 后 A"结果不同。绕 x 与绕 z 各 90°，两种顺序作用 \(e_1\)：

先 x 后 z (rz*rx) 作用 e1 -> [0. 1. 0.]
先 z 后 x (rx*rz) 作用 e1 -> [0. 0. 1.]     两者相等? False

scipy 里 rA*rB 读作"先施加 rB、再施加 rA"（右乘先作用）。这决定了链式变换的书写序：世界到机体 \(R_{wb}=R_{wi}R_{ib}\)，与 robotics-note 的齐次变换复合序必须同向。多坐标系串联时，乘法序与"active/passive（主动旋转向量 / 被动换基底）"两条约定交叉，是最常见的静默 bug 来源——同一份四元数在错误的乘序下会把姿态越转越偏。

3. 万向锁：欧拉角的两自由度塌缩

ZYX 欧拉里俯仰 \(\beta\) 进中间，旋转矩阵元 \(R_{02}=\sin\beta\)。当 \(\beta\to90^\circ\)，\(\cos\beta\to0\)，偏航与横滚只以差值 \(\alpha-\gamma\) 出现、无法各自定值——两自由度塌成一维：

pitch=90°, 输入 (yaw=40, pitch=90, roll=15) -> scipy 反解 = [25. 90. 0.]
（只有 yaw−roll=25 可定，各自不可定；伴随 "Gimbal lock detected" 警告）

这不是数值 bug，是欧拉参数化的拓扑缺陷（\(SO(3)\) 无法用 \(\mathbb{R}^3\) 全局无奇异覆盖）。后果：垂直俯仰的无人机/云台，欧拉反馈会出现 yaw、roll 同时乱跳。工程对策统一——内部状态用四元数或轴角，欧拉只作人机界面；四元数无奇异（代价是双覆盖 \(q\) 与 \(-q\) 表同一旋转，做差分时须先对齐符号）。

4. Hamilton vs JPL：同姿态、符号相反

四元数有两条活约定：Hamilton（ROS、scipy、绝大多数机器人库）与 JPL（部分航天/MATLAB 工具）。同一物理旋转，JPL 的向量部与 Hamilton 反号（相差一次共轭）。把 \(\pm\) 两种读法喂进同一个 scipy：

Hamilton 90°绕z 四元数 (0,0,0.707,0.707) 作用 e1 -> [0. 1. 0.]   （逆时针 +90°）
JPL 形式(虚部取反) 喂入同一 scipy          -> [ 0. -1.  0.]   （翻转成 −90°）

拿 JPL 存的四元数当 Hamilton 读，旋转方向直接反掉、姿态取逆——症状是姿态"镜像翻转"或与 IMU 差一个共轭。跨库/跨设备搬四元数前先锁定约定，并在转换处留一行注释。scipy 的 Rotation 是 Hamilton 且 (x,y,z,w)，与 REP 105 的 ROS 侧一致，本章全程沿此。

5. 角速度与姿态微分：IMU 进状态的桥

IMU 陀螺给的是角速度 \(\omega\)（机体坐标下），姿态是它的积分。四元数运动学：

\[ \dot q = \tfrac12\, q \otimes \begin{bmatrix}0\\ \omega\end{bmatrix}, \]

常 \(\omega\) 下解析解是指数映射 \(q(t)=q_0\otimes\exp(\tfrac12\omega t)\)。绕 z 以 10°/s 积分 3.6 s，scipy 用 from_rotvec(ω dt) 逐步右乘（体速率更新）：

积分到 t=3.6s -> 偏航 = 36.0°（应=36°，无漂移）
qdot 一阶数值差分 vs 公式 0.5 q⊗(0,ω): 差 = 0.0

两条用法在此交汇：离散积分用指数映射（\(q\leftarrow q\otimes\exp(\frac12\hat\omega\,dt)\)）而非把 \(\omega\) 当欧拉角速度加，后者在大步长会引入误差。

误差定义上，姿态误差取 \(R_{err}=R_{des}^\top R\) 再映到轴角（3 维），是第 6 章「状态反馈与现代控制」与第 8 章「滤波与融合」的 EKF 在 \(SO(3)\) 上的正确增量式——直接在四元数四个分量上做加减会破坏单位模。

这条微分关系到第 8 章「滤波与融合」的 IMU 航位推算作状态传播方程；纯几何的指数/对数映射细节归 robotics-note 第 2 章「旋转的表示与互转」。

6. REP 105 与坐标系约定

跨设备姿态要先统一"谁相对谁、哪个轴朝哪"。ROS 的 REP 105（约定坐标系方向）规定：机体 \(x\) 前、\(y\) 左、\(z\) 上（右手）；全球/固定系 \(x\) 东、\(y\) 北、\(z\) 上；map→odom→base_link 的父子链。姿态在这些系间用 TF2 树传播——TF2 的实现、TransformStamped、四元数在 ROS 消息里的具体形态归 ROS2-note 第 5 章「TF2 与机器人建模」。

约定层只有两条：任何两帧变换须成对存逆（\(T_{ab}=T_{ba}^{-1}\)）、四元数按 Hamilton xyzw 传。

Rotation 矩阵的 \(\det=+1\)（本例单位姿态 det=1.0）可作加载外部姿态数据的健全性检查——det=-1 说明混进了反射（镜像），不是合法旋转。视觉侧相机到机体的外参姿态与 camera-note 的射影约定同源但镜头不同：camera-note 第 1 章「相机模型与标定（算法侧）」处理像素—世界投影，本章处理机体姿态的时间演化，两者在"旋转如何进观测模型"处相接。

7. 要点

  • 四表示各有分工：矩阵好复合、轴角无冗余适增量、四元数无奇异适 IMU/ROS、欧拉贴直觉但有奇异——内部状态用前三种，欧拉仅人机界面。
  • scipy Rotation 是 Hamilton、四元数 (x,y,z,w)；from_euler 大写=内旋小写=外旋，取数与传参都要锁定约定。
  • 旋转不可交换：rA*rB 是先 B 后 A，链式变换书写序必须与几何侧一致，否则静默转偏。
  • 万向锁是欧拉参数化的拓扑缺陷（pitch→90° 时 yaw/roll 塌成差值），四元数避开它（代价是 q/−q 双覆盖需对齐符号）。
  • Hamilton vs JPL 差一个虚部符号，跨库搬四元数不锁约定会让姿态翻转取逆。
  • 姿态运动学 \(\dot q=\tfrac12 q\otimes(0,\omega)\)：离散更新走指数映射、误差在 \(SO(3)\) 上用轴角增量——第 8 章 IMU 航位推算与状态反馈误差定义的共同底座。

参考文献与延伸阅读

  • Solà, J. Quaternion Kinematics for the Error-State Kalman Filter. arXiv:1711.02508, 2017. arXiv
  • Markley, F. L., & Crassidis, J. L. Fundamentals of Spacecraft Attitude Determination and Control. Springer, 2014. DOI
  • Shuster, M. D. A Survey of Attitude Representations. Journal of the Astronautical Sciences, 1993.