3、姿态解算基础：陀螺仪原理、角速度积分、姿态更新算法、四元数更新方程
3.1 陀螺仪原理与角速度测量
陀螺仪是捷联惯导系统中测量载体角运动的核心传感器。其基本原理基于哥里奥利效应：当一个质量块在旋转参考系中做径向运动时，会受到一个与旋转角速度成正比的横向力。
在MEMS陀螺仪中，通常采用振动式结构：
- 驱动模态：使质量块沿某一轴（如X轴）做高频简谐振动。
- 敏感模态：当载体绕垂直于驱动方向的轴（如Z轴）旋转时，哥里奥利力使质量块在Y轴方向产生位移。
- 电容检测：通过检测敏感模态的位移量，即可解算出角速度 \(\omega\)。
陀螺仪的输出通常为三个轴向的角速度：
\[ \boldsymbol{\omega}_{ib}^b = \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \]其中，上标 \(b\) 表示载体坐标系，下标 \(ib\) 表示相对于惯性系 \(i\) 的角速度在 \(b\) 系下的投影。
陀螺仪误差模型（简化）：
\[ \tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{s}_g \cdot \boldsymbol{\omega} + \mathbf{n}_g \]- \(\tilde{\boldsymbol{\omega}}\)：测量值
- \(\mathbf{b}_g\)：零偏（bias）
- \(\mathbf{s}_g\)：比例因子误差
- \(\mathbf{n}_g\)：高斯白噪声
3.2 角速度积分与姿态更新
姿态更新的本质是求解微分方程。设载体坐标系相对于导航坐标系（如东北天坐标系 \(n\)）的姿态用方向余弦矩阵 \(C_b^n\) 表示，其微分方程为：
\[ \dot{C}_b^n = C_b^n \cdot [\boldsymbol{\omega}_{nb}^b \times] \]其中 \([\boldsymbol{\omega}_{nb}^b \times]\) 是角速度的反对称矩阵：
\[ [\boldsymbol{\omega} \times] = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \]角速度积分方法：
在离散时间系统中，假设在采样间隔 \(\Delta t\) 内角速度恒定，则姿态更新可近似为：
\[ C_b^n(t + \Delta t) = C_b^n(t) \cdot \exp\left( [\boldsymbol{\omega}_{nb}^b \cdot \Delta t \times] \right) \]对于小角度旋转，可使用一阶近似：
\[ C_b^n(t + \Delta t) \approx C_b^n(t) \cdot \left( I + [\boldsymbol{\omega}_{nb}^b \cdot \Delta t \times] \right) \]注意：实际应用中，由于陀螺仪输出的是相对于惯性系的角速度 \(\boldsymbol{\omega}_{ib}^b\)，需要扣除地球自转和导航系旋转的影响才能得到 \(\boldsymbol{\omega}_{nb}^b\)：
\[ \boldsymbol{\omega}_{nb}^b = \boldsymbol{\omega}_{ib}^b - C_n^b \cdot (\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \]3.3 四元数更新方程
四元数因其无奇异性、计算效率高，成为捷联惯导姿态解算的首选工具。姿态四元数定义为：
\[ \mathbf{q} = \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} = \begin{bmatrix} \cos(\theta/2) \\ \mathbf{e} \sin(\theta/2) \end{bmatrix} \]其中 \(\mathbf{e}\) 为旋转轴单位向量，\(\theta\) 为旋转角。
四元数微分方程：
\[ \dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega}_{nb}^b \]其中 \(\otimes\) 表示四元数乘法，\(\boldsymbol{\omega}_{nb}^b\) 以纯四元数形式表示：\(\begin{bmatrix} 0 & \omega_x & \omega_y & \omega_z \end{bmatrix}^T\)。
写成矩阵形式：
\[ \dot{\mathbf{q}} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \mathbf{q} \]3.4 四元数离散更新算法
方法一：一阶龙格-库塔法（欧拉法）
\[ \mathbf{q}(t + \Delta t) = \mathbf{q}(t) + \dot{\mathbf{q}}(t) \cdot \Delta t \]优点：简单；缺点：精度低，需对四元数进行归一化。
方法二：精确解析解（角增量法）
定义角增量向量：
\[ \boldsymbol{\Delta \theta} = \int_t^{t+\Delta t} \boldsymbol{\omega}_{nb}^b dt \approx \boldsymbol{\omega}_{nb}^b \cdot \Delta t \]令 \(\Delta \theta = \|\boldsymbol{\Delta \theta}\|\)，则精确更新公式为：
\[ \mathbf{q}(t + \Delta t) = \left( \cos\frac{\Delta \theta}{2} \cdot I + \frac{1}{\Delta \theta} \sin\frac{\Delta \theta}{2} \cdot \boldsymbol{\Omega} \right) \mathbf{q}(t) \]其中 \(\boldsymbol{\Omega}\) 为角增量反对称矩阵：
\[ \boldsymbol{\Omega} = \begin{bmatrix} 0 & -\Delta\theta_x & -\Delta\theta_y & -\Delta\theta_z \\ \Delta\theta_x & 0 & \Delta\theta_z & -\Delta\theta_y \\ \Delta\theta_y & -\Delta\theta_z & 0 & \Delta\theta_x \\ \Delta\theta_z & \Delta\theta_y & -\Delta\theta_x & 0 \end{bmatrix} \]方法三：二阶毕卡逼近（工程常用）
\[ \mathbf{q}(t + \Delta t) = \left[ I + \frac{1}{2}\boldsymbol{\Omega} + \frac{1}{2}\left(\frac{1}{2}\boldsymbol{\Omega}\right)^2 \right] \mathbf{q}(t) \]展开后：
\[ \mathbf{q}(t + \Delta t) = \left( 1 - \frac{\Delta\theta^2}{8} \right) \mathbf{q}(t) + \frac{1}{2} \boldsymbol{\Omega} \mathbf{q}(t) \]3.5 Python实现示例：四元数更新
import numpy as np
def quaternion_update(q, omega, dt, method='exact'):
"""
四元数姿态更新函数
参数:
q: 当前姿态四元数 [q0, q1, q2, q3]
omega: 角速度向量 [wx, wy, wz] (rad/s)
dt: 采样时间间隔 (s)
method: 更新方法 ('euler', 'exact', 'picard')
返回:
q_new: 更新后的四元数
"""
wx, wy, wz = omega
dtheta = np.linalg.norm(omega) * dt
if method == 'euler':
# 一阶欧拉法
q_dot = 0.5 * np.array([
[-q[1]*wx - q[2]*wy - q[3]*wz],
[ q[0]*wx + q[2]*wz - q[3]*wy],
[ q[0]*wy - q[1]*wz + q[3]*wx],
[ q[0]*wz + q[1]*wy - q[2]*wx]
]).flatten()
q_new = q + q_dot * dt
elif method == 'exact':
# 精确解析解
if dtheta < 1e-12:
return q
sin_half = np.sin(dtheta / 2) / dtheta
cos_half = np.cos(dtheta / 2)
Omega = np.array([
[0, -wx*dt, -wy*dt, -wz*dt],
[wx*dt, 0, wz*dt, -wy*dt],
[wy*dt, -wz*dt, 0, wx*dt],
[wz*dt, wy*dt, -wx*dt, 0 ]
])
q_new = (cos_half * np.eye(4) + sin_half * Omega) @ q
elif method == 'picard':
# 二阶毕卡逼近
dtheta2 = dtheta**2
Omega = np.array([
[0, -wx*dt, -wy*dt, -wz*dt],
[wx*dt, 0, wz*dt, -wy*dt],
[wy*dt, -wz*dt, 0, wx*dt],
[wz*dt, wy*dt, -wx*dt, 0 ]
])
q_new = ((1 - dtheta2/8) * np.eye(4) + 0.5 * Omega) @ q
# 归一化
q_new = q_new / np.linalg.norm(q_new)
return q_new
# 示例使用
if __name__ == "__main__":
# 初始四元数（无旋转）
q_init = np.array([1.0, 0.0, 0.0, 0.0])
# 绕Z轴旋转 0.1 rad/s
omega = np.array([0.0, 0.0, 0.1])
dt = 0.01
q_new = quaternion_update(q_init, omega, dt, method='exact')
print(f"更新后四元数: {q_new}")
print(f"模长: {np.linalg.norm(q_new):.6f}")
3.6 姿态解算中的关键注意事项
| 问题 | 说明 | 解决方案 |
|---|---|---|
| 四元数归一化 | 数值积分误差导致四元数模长偏离1 | 每次更新后强制归一化 |
| 圆锥误差 | 角速度方向变化时，简单积分产生漂移 | 使用多子样算法（如二子样、三子样） |
| 不可交换性误差 | 有限旋转的次序不可交换 | 采用精确角增量法或高阶算法 |
| 地球自转补偿 | 陀螺仪测量包含地球自转分量 | 扣除 \(\boldsymbol{\omega}_{ie}^n\) 和 \(\boldsymbol{\omega}_{en}^n\) |
总结：姿态解算的核心在于准确积分角速度并更新四元数。实际工程中，推荐使用精确解析解或多子样圆锥补偿算法，并配合高精度归一化处理，以保证长时间导航的稳定性。