2. 姿态表示基础

在视觉与IMU融合姿态估计中，姿态表示是所有算法的基础。无论是视觉里程计输出的相机位姿，还是IMU预积分得到的旋转增量，最终都需要统一到同一种姿态描述形式中才能进行融合。本节将系统梳理四种主流姿态表示方法：欧拉角、旋转矩阵、四元数和旋转向量，并讨论它们之间的转换关系以及万向节锁问题。

一、四种姿态表示的核心原理

欧拉角用三个角度描述刚体绕三个坐标轴的依次旋转，常见形式有ZYX（偏航-俯仰-横滚）和XYZ等。它的最大优点是直观：偏航角（yaw）对应绕Z轴旋转，俯仰角（pitch）对应绕Y轴旋转，横滚角（roll）对应绕X轴旋转。在无人机、机器人领域，工程师常用欧拉角来快速判断姿态是否异常。

旋转矩阵是一个3×3的正交矩阵，满足R^TR=I且det(R)=1。它可以直接作用于三维向量完成坐标变换，即p_B=R_AB·p_A。旋转矩阵的优点是复合旋转只需矩阵相乘，没有奇异点；缺点是9个参数中只有3个自由度，存在冗余，且数值积分后需要重新正交化。

四元数用一个实部和三个虚部组成的四维向量q=[w,x,y,z]表示旋转，满足单位模长约束。四元数没有奇异点，插值平滑（SLERP），计算效率高，是视觉惯性SLAM系统中最常用的姿态表示。VINS-Mono、ORB-SLAM3等系统内部均采用四元数进行旋转传播和优化。

旋转向量（轴角表示）用一个单位轴n和旋转角θ组合成三维向量r=θ·n，其方向表示旋转轴，模长表示旋转角度。旋转向量与李代数so(3)直接对应，在优化问题中用于表示旋转增量，通过指数映射可转换为旋转矩阵。

二、相互转换方法与代码实现

四种表示之间的转换是姿态估计中的高频操作。以下给出关键转换的Python代码示例（基于NumPy）：

import numpy as np

# 欧拉角(ZYX) -> 旋转矩阵
def euler_to_R(roll, pitch, yaw):
    Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]])
    Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
    return Rz @ Ry @ Rx

# 旋转向量 -> 旋转矩阵 (Rodrigues)
def rvec_to_R(r):
    theta = np.linalg.norm(r)
    if theta < 1e-10: return np.eye(3)
    n = r / theta
    K = np.array([[0,-n[2],n[1]],[n[2],0,-n[0]],[-n[1],n[0],0]])
    return np.eye(3) + np.sin(theta)*K + (1-np.cos(theta))*(K@K)

# 四元数 -> 旋转矩阵
def quat_to_R(q):
    w,x,y,z = q / np.linalg.norm(q)
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y-w*z), 2*(x*z+w*y)],
        [2*(x*y+w*z), 1-2*(x*x+z*z), 2*(y*z-w*x)],
        [2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x*x+y*y)]])

三、万向节锁问题深入剖析

万向节锁（Gimbal Lock）是欧拉角表示固有的奇异问题。当俯仰角pitch=±90°时，绕X轴的旋转和绕Z轴的旋转效果完全重合，系统丢失一个自由度。此时无法唯一确定roll和yaw的值，姿态解算会出现跳变。

解决万向节锁的常用方案包括：

• 使用四元数或旋转矩阵替代欧拉角进行内部运算，仅在显示时转换为欧拉角；
• 采用旋转向量（李代数）表示旋转增量，在流形上进行优化；
• 若必须使用欧拉角，需在接近奇异点时切换旋转顺序或引入阻尼。

四、四种表示方法对比与选型建议

五、案例：VIO系统中的姿态表示选择

以VINS-Mono为例，系统内部状态量使用四元数表示旋转，但在优化过程中，旋转增量用旋转向量（李代数）参数化。具体流程为：IMU预积分得到相对旋转四元数→构建残差时转换为旋转向量→求解增量后通过指数映射更新四元数。这种“四元数存储+旋转向量优化”的组合既避免了万向节锁，又保证了流形上的正确更新。

理解四种姿态表示及其转换关系，是后续学习IMU预积分、视觉重投影误差构建和滑窗优化的前提。尤其要注意四元数的符号二义性（q和-q表示同一旋转）以及旋转向量在零附近的处理，这些细节直接影响融合系统的精度和鲁棒性。