第5章：坐标系与姿态表示

机器人系统的精确控制依赖于对空间位置和姿态的准确描述。本章深入探讨机器人学中的坐标系建立、姿态表示方法及其工程权衡。我们将对比传统的DH参数法与现代的指数积方法，分析四元数、旋转矩阵、欧拉角在不同场景下的优劣，并介绍李群/李代数这一强大的数学工具。通过NASA瓦尔基里机器人的实例，我们将看到这些理论如何应用于复杂系统的设计中。

5.1 坐标系建立方法

5.1.1 Denavit-Hartenberg (DH) 参数法

DH参数法是1955年提出的经典方法，通过四个参数完整描述相邻关节间的坐标变换。其核心思想是通过系统化的坐标系分配，将任意复杂的关节用最少的参数表示。

标准DH参数定义：

• $a_i$：连杆长度（沿$x_i$轴从$z_{i-1}$到$z_i$的距离）
• $\alpha_i$：连杆扭转（绕$x_i$轴从$z_{i-1}$到$z_i$的角度）
• $d_i$：连杆偏移（沿$z_{i-1}$轴从$x_{i-1}$到$x_i$的距离）
• $\theta_i$：关节角（绕$z_{i-1}$轴从$x_{i-1}$到$x_i$的角度）

变换矩阵： $$T_i^{i-1} = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ DH参数的工程优势：

1. 参数最少（每个关节仅4个参数）
2. 系统化流程，易于教学和实施
3. 大量机器人制造商采用，工业标准成熟

DH参数的局限性：

1. 坐标系分配规则复杂，初学者易出错
2. 存在奇异性配置（如连续平行轴）
3. 不同DH约定（标准vs修改）导致混淆
4. 数值稳定性问题：小角度近似时精度损失

标准DH坐标系建立流程：
      z_{i-1}
        |
        | d_i
        |
  x_{i-1}----> x_i (沿公垂线)
       /|
      / |a_i
     /  |
    z_i |

5.1.2 Product of Exponentials (PoE) 方法

PoE方法基于螺旋理论，将刚体运动表示为绕螺旋轴的指数映射。这种方法在现代机器人学中越来越受欢迎，特别是在需要优雅处理奇异性和进行几何直觉分析时。

螺旋轴表示： 对于旋转关节，螺旋轴$\xi_i$定义为： $$\xi_i = \begin{bmatrix} \omega_i \\ v_i \end{bmatrix} = \begin{bmatrix} \omega_i \\ -\omega_i \times q_i \end{bmatrix}$$ 其中$\omega_i$是旋转轴方向，$q_i$是轴上任意一点。

指数映射： $$T(\theta) = e^{\hat{\xi}\theta} = I + \hat{\xi}\theta + \frac{(\hat{\xi}\theta)^2}{2!} + \frac{(\hat{\xi}\theta)^3}{3!} + ...$$ 对于纯旋转，Rodrigues公式给出闭式解： $$e^{[\omega]\theta} = I + [\omega]\sin\theta + [\omega]^2(1-\cos\theta)$$ PoE的工程优势：

1. 全局坐标系，避免累积误差
2. 奇异性处理优雅，无需特殊案例
3. 与现代优化算法（如梯度下降）兼容性好
4. 几何直觉清晰，便于可视化

PoE的实施考虑：

1. 需要更强的数学背景（李代数）
2. 工业支持相对较少
3. 参数识别需要专门工具

5.1.3 DH vs PoE 工程选择指南

| 场景 | 推荐方法 | 原因 |

5.2 姿态表示方法对比

5.2.1 旋转矩阵

旋转矩阵$R \in SO(3)$是最直接的姿态表示，保持向量长度和夹角不变。

性质：

• 正交性：$R^TR = RR^T = I$
• 行列式：$\det(R) = 1$
• 群结构：旋转复合通过矩阵乘法

工程优势：

1. 无奇异性，全局唯一
2. 直接用于坐标变换
3. 数值稳定性好（配合正交化）

工程劣势：

1. 9个参数表示3个自由度，冗余度高
2. 存储开销大
3. 插值困难（不在线性空间）
4. 需要定期正交化以维持约束

正交化算法（Gram-Schmidt）：

# 伪代码
def orthogonalize(R):
    c1 = R[:, 0]
    c1 = c1 / ||c1||
    c2 = R[:, 1] - (c1 · R[:, 1]) * c1
    c2 = c2 / ||c2||
    c3 = c1 × c2
    return [c1, c2, c3]

5.2.2 欧拉角

欧拉角用三个连续旋转表示任意姿态，常见约定包括ZYX（航空）、ZYZ（机械臂）等。

ZYX欧拉角（Roll-Pitch-Yaw）： $$R_{ZYX}(\phi, \theta, \psi) = R_z(\psi)R_y(\theta)R_x(\phi)$$ 工程优势：

1. 最小参数表示（3个角度）
2. 直观易懂，便于人机交互
3. 存储效率高

工程劣势：

1. 万向节锁（Gimbal Lock）：当pitch接近±90°时，roll和yaw退化
2. 不连续性：同一姿态多种表示
3. 插值路径不直观
4. 数值微分在奇异点附近不稳定

万向节锁检测与处理：

# 检测万向节锁
if abs(pitch) > 85 * pi/180:  # 接近±90度
    # 切换到四元数表示
    use_quaternion_control()
else:
    # 安全使用欧拉角
    use_euler_control()

5.2.3 四元数

四元数$q = [q_w, q_x, q_y, q_z]$提供了紧凑且无奇异性的姿态表示。

基本运算：

• 单位约束：$||q|| = 1$
• 旋转复合：$q_3 = q_1 \otimes q_2$（Hamilton乘积）
• 逆旋转：$q^{-1} = q^* = [q_w, -q_x, -q_y, -q_z]$
• 向量旋转：$v' = q \otimes v \otimes q^*$

工程优势：

1. 无奇异性，全局表示
2. 紧凑（4参数，1约束）
3. 插值简单（SLERP算法）
4. 计算效率高于矩阵

工程劣势：

1. 不直观，调试困难
2. 双覆盖问题：$q$和$-q$表示同一旋转
3. 需要归一化维护
4. 与经典控制理论接口复杂

SLERP插值实现： $$q_t = \frac{\sin((1-t)\theta)}{\sin\theta}q_0 + \frac{\sin(t\theta)}{\sin\theta}q_1$$ 其中$\cos\theta = q_0 \cdot q_1$

5.2.4 工程选择决策树

开始
  |
  v
需要人机交互？ --是--> 欧拉角（配合范围限制）
  |
  否
  v
需要优化/插值？ --是--> 四元数（SLERP）
  |
  否
  v
需要直接变换？ --是--> 旋转矩阵
  |
  否
  v
存储受限？ --是--> 轴角表示
  |
  否
  v
默认：四元数（通用性最好）

5.3 李群与李代数在机器人学中的应用

5.3.1 李群基础

李群是既具有群结构又具有流形结构的数学对象，在机器人学中主要涉及：

• $SO(3)$：特殊正交群，表示3D旋转
• $SE(3)$：特殊欧氏群，表示3D刚体运动
• $SO(2)$、$SE(2)$：平面运动的对应群

$SE(3)$群元素： $$T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \in SE(3)$$ 其中$R \in SO(3)$，$p \in \mathbb{R}^3$

群运算性质：

1. 封闭性：$T_1 \cdot T_2 \in SE(3)$
2. 结合律：$(T_1 \cdot T_2) \cdot T_3 = T_1 \cdot (T_2 \cdot T_3)$
3. 单位元：$I = \begin{bmatrix} I_3 & 0 \\ 0 & 1 \end{bmatrix}$
4. 逆元素：$T^{-1} = \begin{bmatrix} R^T & -R^Tp \\ 0 & 1 \end{bmatrix}$

5.3.2 李代数表示

李代数是李群在单位元处的切空间，提供了李群的线性化表示：

• $\mathfrak{so}(3)$：$SO(3)$的李代数，3维反对称矩阵空间
• $\mathfrak{se}(3)$：$SE(3)$的李代数，螺旋运动空间

$\mathfrak{se}(3)$元素（螺旋）： $$\xi^{\wedge} = \begin{bmatrix} [\omega]_{\times} & v \\ 0 & 0 \end{bmatrix} \in \mathfrak{se}(3)$$ 其中$[\omega]_{\times}$是反对称矩阵： $$[\omega]_{\times} = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}$$

5.3.3 指数映射与对数映射

指数映射（李代数→李群）： $$\exp: \mathfrak{se}(3) \rightarrow SE(3)$$ 对于纯旋转（$v=0$）： $$e^{[\omega]_{\times}\theta} = I + \frac{\sin\theta}{\theta}[\omega]_{\times} + \frac{1-\cos\theta}{\theta^2}[\omega]_{\times}^2$$ 对于一般螺旋运动： $$e^{\xi^{\wedge}\theta} = \begin{bmatrix} e^{[\omega]_{\times}\theta} & J(\theta)v \\ 0 & 1 \end{bmatrix}$$ 其中$J(\theta)$是左雅可比： $$J(\theta) = I\theta + \frac{1-\cos\theta}{\theta}[\omega]_{\times} + \frac{\theta-\sin\theta}{\theta}[\omega]_{\times}^2$$ 对数映射（李群→李代数）： $$\log: SE(3) \rightarrow \mathfrak{se}(3)$$ 给定$T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}$：

1. 计算旋转角：$\theta = \arccos\left(\frac{\text{tr}(R)-1}{2}\right)$
2. 提取旋转轴：$\omega = \frac{1}{2\sin\theta}\begin{bmatrix} R_{32}-R_{23} \\ R_{13}-R_{31} \\ R_{21}-R_{12} \end{bmatrix}$
3. 计算线速度：$v = J^{-1}(\theta)p$

5.3.4 工程应用

1. 轨迹优化中的应用： 李代数提供了无约束优化空间，避免了旋转矩阵的正交约束：

优化问题：
min_{ξ} ||f(exp(ξ^∧)T_0) - T_target||²

梯度计算：
∂f/∂ξ = ∂f/∂T · ∂exp(ξ^∧)/∂ξ

2. 滤波器设计： 扩展卡尔曼滤波在$SE(3)$上的实现：

• 状态预测：$T_{k+1|k} = T_{k|k} \cdot \exp(\xi^{\wedge}_k \Delta t)$
• 误差状态：$\delta\xi \in \mathfrak{se}(3)$（6维向量）
• 协方差更新：在李代数空间进行

3. 插值与平滑：

# SE(3)测地线插值
def geodesic_interp(T1, T2, t):
    T_rel = inv(T1) @ T2
    xi = log(T_rel)
    return T1 @ exp(t * xi)

4. 速度与加速度计算： 空间速度（Body Velocity）： $$V^b = T^{-1}\dot{T} = \begin{bmatrix} [\omega^b]_{\times} & v^b \\ 0 & 0 \end{bmatrix}$$ 空间加速度需要考虑科里奥利项： $$\dot{V}^b = \text{ad}_{V^b}V^b + T^{-1}\ddot{T}$$

5.3.5 数值实现注意事项

1. 小角度处理： 当$\theta < \epsilon$时，使用泰勒展开避免数值不稳定： $$\frac{\sin\theta}{\theta} \approx 1 - \frac{\theta^2}{6}$$ $$\frac{1-\cos\theta}{\theta^2} \approx \frac{1}{2} - \frac{\theta^2}{24}$$

2. 归一化策略： 定期将$SO(3)$元素正交化，$SE(3)$保持最后一行为$[0,0,0,1]$

3. 计算效率优化： - 预计算常用的指数映射 - 使用闭式解而非级数展开 - 利用对称性减少计算量

5.4 奇异性分析与规避策略

5.4.1 奇异性类型

机器人系统中的奇异性主要分为三类：

1. 运动学奇异性： - 边界奇异性：机械臂完全伸展或收缩 - 内部奇异性：多个关节轴共线 - 腕部奇异性：腕部关节轴相交于一点

2. 表示奇异性： - 欧拉角万向节锁 - DH参数的平行轴问题 - 四元数的双覆盖

3. 算法奇异性： - 雅可比矩阵秩降 - 优化问题的鞍点 - 数值积分的刚性问题

5.4.2 奇异性检测

雅可比奇异性检测：

def detect_singularity(J, threshold=1e-3):
    # 方法1：条件数
    cond_num = np.linalg.cond(J)
    if cond_num > 1/threshold:
        return True, "High condition number"

    # 方法2：最小奇异值
    _, s, _ = np.linalg.svd(J)
    if s[-1] < threshold:
        return True, "Small singular value"

    # 方法3：可操作度
    manipulability = np.sqrt(np.linalg.det(J @ J.T))
    if manipulability < threshold:
        return True, "Low manipulability"

    return False, "Non-singular"

几何奇异性检测：

• 监测关节角接近极限
• 检查连杆是否共线
• 分析工作空间边界

5.4.3 奇异性规避策略

1. 阻尼最小二乘（DLS）： $$\dot{q} = J^T(JJ^T + \lambda^2I)^{-1}\dot{x}$$ 其中$\lambda$是阻尼因子，可自适应调整： $$\lambda = \begin{cases} 0 & \text{if } \sigma_{min} \geq \epsilon \\ \lambda_{max}\left(1 - \left(\frac{\sigma_{min}}{\epsilon}\right)^2\right) & \text{if } \sigma_{min} < \epsilon \end{cases}$$

2. 梯度投影法： 利用零空间进行二次任务优化： $$\dot{q} = J^+\dot{x} + (I - J^+J)\nabla H$$ 其中$H$是避奇异性的势函数： $$H = -\frac{1}{2}\log(\det(JJ^T))$$

3. 路径规划层面： - 添加奇异性约束到轨迹优化 - 使用RRT*等采样方法绕过奇异区域 - 预计算奇异性地图

4. 冗余度利用：

# 7自由度机械臂的肘部角优化
def optimize_elbow_angle(q, J, task_vel):
    # 主任务
    q_dot_task = pinv(J) @ task_vel

    # 零空间基
    N = eye(7) - pinv(J) @ J

    # 肘部角梯度
    elbow_gradient = compute_elbow_gradient(q)

    # 综合速度
    q_dot = q_dot_task + N @ elbow_gradient
    return q_dot

5.4.4 实时奇异性处理

分层控制架构：

层级1：任务空间控制器
  ↓ (检测奇异性)
层级2：奇异性处理器

  - 切换到关节空间控制
  - 激活阻尼项
  - 调整任务优先级
  ↓
层级3：关节控制器

状态机设计：

正常模式 ←→ 近奇异模式 ←→ 奇异模式
   |            |              |
   ↓            ↓              ↓
笛卡尔控制   混合控制      关节控制

5.5 案例研究：NASA瓦尔基里机器人的坐标系设计

5.5.1 背景介绍

NASA瓦尔基里（Valkyrie/R5）是为火星探索任务设计的类人机器人，高1.8米，重125公斤，具有44个自由度。其坐标系设计需要满足：

• 太空环境的极端可靠性要求
• 与NASA任务控制系统的兼容性
• 遥操作时的直观性
• 多传感器融合的精确性

5.5.2 坐标系架构

基础坐标系定义：

1. 世界坐标系：北东地（NED）航空标准
2. 基座坐标系：位于骨盆中心，Z轴向上
3. 关节坐标系：修改DH参数，优化数值稳定性
4. 传感器坐标系： - IMU：与基座对齐，考虑振动隔离 - 相机：前视坐标系，Z轴向前 - 力传感器：腕部和踝部，局部坐标系

坐标系层级：

世界坐标系
    ↓
基座坐标系（骨盆）
    ├── 躯干链
    │   ├── 腰部（3 DOF）
    │   └── 头部（2 DOF + 相机）
    ├── 左臂链（7 DOF + 手）
    ├── 右臂链（7 DOF + 手）
    ├── 左腿链（6 DOF）
    └── 右腿链（6 DOF）

5.5.3 姿态表示选择

四元数为主，欧拉角为辅：

• 内部计算：全部使用四元数，避免奇异性
• 人机接口：显示RPY欧拉角，限制pitch在±85°
• 通信协议：四元数传输，减少带宽需求
• 日志记录：同时保存四元数和欧拉角

实现细节：

class ValkyriePose:
    def __init__(self):
        self.quat = [1, 0, 0, 0]  # w, x, y, z
        self.euler_display = None  # 仅用于显示

    def update_orientation(self, quat_new):
        # 主更新路径
        self.quat = normalize(quat_new)

        # 辅助显示更新
        if needs_display():
            self.euler_display = quat_to_euler_safe(self.quat)

    def quat_to_euler_safe(self, q):
        # 检测万向节锁
        test = q[0]*q[2] - q[1]*q[3]
        if abs(test) > 0.4999:  # 接近±90度
            return None  # 返回警告
        return quat_to_euler(q)

5.5.4 多传感器融合策略

扩展卡尔曼滤波（EKF）设计：

• 状态向量：$[p, v, q, b_g, b_a]$（位置、速度、四元数、陀螺仪偏差、加速度计偏差）
• 误差状态：$[\delta p, \delta v, \delta \theta, \delta b_g, \delta b_a]$（使用李代数）

传感器同步：

IMU (1000Hz) ──┐
                ├─→ 时间同步器 ──→ EKF (200Hz)
相机 (30Hz) ────┤      │
                │      ↓
力传感器 (500Hz)┘   状态估计

5.5.5 遥操作坐标系映射

操作员视角到机器人视角：

1. 操作员使用VR头显，自然坐标系
2. 映射到机器人基座坐标系
3. 考虑通信延迟（地火通信14-24分钟）
4. 预测显示补偿延迟

坐标变换链： $$T_{robot}^{world} = T_{base}^{world} \cdot T_{link}^{base} \cdot T_{tool}^{link}$$

5.5.6 经验教训

1. 坐标系文档的重要性：NASA要求每个坐标系都有详细的定义文档和可视化
2. 冗余表示的价值：关键姿态同时用四元数和旋转矩阵存储，用于交叉验证
3. 坐标系可视化工具：开发专门的RViz插件显示所有坐标系
4. 单元测试覆盖：所有坐标变换函数100%测试覆盖
5. 故障恢复机制：当检测到数值错误时，自动切换到备份表示法

5.6 高级话题：双四元数与螺旋理论

5.6.1 双四元数基础

双四元数提供了统一表示旋转和平移的优雅框架： $$\hat{q} = q_r + \epsilon q_d$$ 其中$q_r$是旋转四元数，$q_d$是对偶部分，$\epsilon^2 = 0$。

优势：

• 8参数表示6自由度刚体运动
• 插值路径是螺旋运动
• 避免了$SE(3)$矩阵的16个元素

5.6.2 螺旋理论应用

瞬时螺旋轴： 任何刚体运动都可分解为绕某轴的旋转加沿该轴的平移： $$\mathcal{S} = \{\mathbf{s}; \mathbf{s}_0\}$$ 其中$\mathbf{s}$是轴方向，$\mathbf{s}_0$是轴上一点。

Plücker坐标： 用6维向量表示空间直线： $$\mathcal{L} = (\mathbf{l}; \mathbf{m})$$ 其中$\mathbf{l}$是方向向量，$\mathbf{m} = \mathbf{p} \times \mathbf{l}$是力矩。

5.6.3 工程应用实例

并联机构分析： Stewart平台的运动学用螺旋理论更简洁：

• 6个支链提供6个约束螺旋
• 动平台的可行运动是约束螺旋的零空间
• 奇异性对应约束螺旋线性相关

抓取分析： 接触点的约束用旋量表示：

• 点接触：3个约束（限制平移）
• 线接触：4个约束
• 面接触：5个约束

本章小结

本章系统介绍了机器人学中的坐标系建立和姿态表示方法：

关键概念：

1. DH参数提供系统化但可能遇到奇异性，PoE基于螺旋理论更现代
2. 旋转矩阵无奇异但冗余，欧拉角直观但有万向节锁，四元数紧凑无奇异
3. 李群/李代数为优化和滤波提供优雅框架
4. 奇异性需要多层次检测和规避策略
5. 实际系统常需多种表示法协同工作

关键公式：

• DH变换：$T = \text{Trans}_z(d) \cdot \text{Rot}_z(\theta) \cdot \text{Trans}_x(a) \cdot \text{Rot}_x(\alpha)$
• Rodrigues公式：$e^{[\omega]\theta} = I + [\omega]\sin\theta + [\omega]^2(1-\cos\theta)$
• 四元数SLERP：$q_t = \frac{\sin((1-t)\theta)}{\sin\theta}q_0 + \frac{\sin(t\theta)}{\sin\theta}q_1$
• 阻尼伪逆：$J^{\dagger} = J^T(JJ^T + \lambda^2I)^{-1}$

练习题

基础题

习题5.1 给定DH参数表： | i | $a_i$ | $\alpha_i$ | $d_i$ | $\theta_i$ |

计算当$\theta_1 = 30°$，$\theta_2 = 45°$，$\theta_3 = -60°$时末端执行器的位姿。

Hint: 逐个计算变换矩阵，然后连乘。

习题5.2 证明旋转矩阵的行列式必须为1，并解释为什么-1的情况对应反射而非旋转。

Hint: 考虑旋转保持手性（chirality）。

习题5.3 实现四元数到旋转矩阵的转换函数，并验证单位四元数$q = [0.5, 0.5, 0.5, 0.5]$对应的旋转矩阵。

Hint: 使用公式$R = I + 2q_w[\mathbf{q}_v]_{\times} + 2[\mathbf{q}_v]_{\times}^2$

习题5.4 某机械臂在工作时检测到雅可比矩阵的条件数为1000，这意味着什么？应该采取什么措施？

Hint: 条件数反映矩阵的"病态"程度。

挑战题

习题5.5 设计一个算法，检测6自由度机械臂的所有奇异配置，并生成奇异性地图。考虑如何在实时控制中使用这个地图。

Hint: 考虑采样关节空间，计算雅可比行列式。

习题5.6 推导并实现SE(3)上的B样条插值算法，使其能生成$C^2$连续的轨迹。与线性插值和SLERP对比。

Hint: 在李代数空间进行B样条，然后指数映射回李群。

习题5.7 某火星探测机器人使用IMU和视觉进行状态估计。由于火星重力（3.71 m/s²）与地球不同，如何修改EKF？考虑IMU标定误差的影响。

Hint: 重力向量影响加速度计测量模型。

习题5.8 分析并联机器人Stewart平台在接近奇异配置时的坐标系选择策略。如何设计坐标系使奇异性分析更简单？

Hint: 考虑动平台中心vs关节空间表示。

常见陷阱与错误 (Gotchas)

1. DH参数符号混淆 - 错误：混用标准DH和修改DH参数 - 正确：明确声明使用的约定，保持一致性

2. 四元数归一化遗忘 - 错误：累积运算后不归一化 - 正确：每次运算后检查模长，必要时归一化

3. 欧拉角顺序错误 - 错误：假设所有系统都用ZYX顺序 - 正确：查阅文档确认旋转顺序，编写转换函数时明确标注

4. 奇异性处理不当 - 错误：使用固定阈值检测所有奇异性 - 正确：根据机构特点设计自适应阈值

5. 坐标系方向假设 - 错误：假设Z轴总是向上 - 正确：明确定义每个坐标系的方向约定

6. 数值精度问题 - 错误：直接比较浮点数是否相等 - 正确：使用容差范围，如abs(a - b) < epsilon

7. 插值路径选择 - 错误：对旋转矩阵元素直接线性插值 - 正确：转换到四元数或李代数进行插值

8. 时间同步忽视 - 错误：假设所有传感器数据时间戳对齐 - 正确：实现时间同步机制，考虑传感器延迟

最佳实践检查清单

设计阶段

• [ ] 明确定义所有坐标系的原点和轴方向
• [ ] 选择合适的姿态表示方法（考虑应用需求）
• [ ] 设计坐标系时考虑制造和装配公差
• [ ] 为每个坐标系创建可视化工具
• [ ] 编写坐标系定义文档

实现阶段

• [ ] 实现多种姿态表示的相互转换函数
• [ ] 添加数值稳定性检查（正交性、归一化）
• [ ] 实现奇异性检测和处理机制
• [ ] 设计单元测试覆盖所有变换函数
• [ ] 实现坐标系标定程序

调试阶段

• [ ] 使用可视化工具验证坐标变换
• [ ] 记录关键位姿的多种表示用于交叉验证
• [ ] 监控数值健康指标（条件数、正交性误差）
• [ ] 实现坐标系一致性检查
• [ ] 准备奇异配置的测试案例

运行阶段

• [ ] 定期检查和修正数值漂移
• [ ] 监控接近奇异性的预警
• [ ] 记录异常姿态用于事后分析
• [ ] 实现紧急情况下的坐标系重置
• [ ] 保持坐标系文档与代码同步更新