用Python+NumPy实战捷联惯导：方向余弦矩阵与四元数的可视化编程指南

捷联惯导系统的核心在于姿态解算，而方向余弦矩阵（DCM）和四元数是两种最常用的姿态表示方法。对于刚接触这一领域的工程师或学生来说，数学公式往往显得抽象难懂。本文将带你用Python和NumPy库，从零实现DCM的构建、四元数运算以及它们之间的转换，通过代码和可视化让这些概念变得直观易懂。

1. 环境准备与基础概念

在开始之前，确保你的Python环境已安装以下库：

pip install numpy matplotlib scipy

方向余弦矩阵是一个3×3的矩阵，用于描述两个坐标系之间的旋转关系。矩阵的每个元素表示两个坐标系对应轴之间夹角的余弦值。例如，从导航系（n系）到载体系（b系）的DCM可以表示为：

$$ C_n^b = \begin{bmatrix} c_{11} & c_{12} & c_{13} \ c_{21} & c_{22} & c_{23} \ c_{31} & c_{32} & c_{33} \end{bmatrix} $$

其中$c_{ij}$表示n系的第i轴与b系的第j轴之间夹角的余弦。

四元数则是用四个参数表示旋转的数学工具，比DCM更紧凑且计算效率更高。一个单位四元数可以表示为：

$$ q = q_0 + q_1i + q_2j + q_3k = [q_0, q_1, q_2, q_3] $$

2. 方向余弦矩阵的实现与应用

2.1 构建基本旋转矩阵

在三维空间中，任何旋转都可以分解为绕X、Y、Z三个轴的连续旋转。我们先实现这三个基本旋转矩阵：

import numpy as np

def rotation_x(angle):
    """绕X轴旋转矩阵"""
    c, s = np.cos(angle), np.sin(angle)
    return np.array([
        [1, 0, 0],
        [0, c, -s],
        [0, s, c]
    ])

def rotation_y(angle):
    """绕Y轴旋转矩阵"""
    c, s = np.cos(angle), np.sin(angle)
    return np.array([
        [c, 0, s],
        [0, 1, 0],
        [-s, 0, c]
    ])

def rotation_z(angle):
    """绕Z轴旋转矩阵"""
    c, s = np.cos(angle), np.sin(angle)
    return np.array([
        [c, -s, 0],
        [s, c, 0],
        [0, 0, 1]
    ])

2.2 组合旋转与可视化

实际应用中，我们通常需要组合多个基本旋转。例如，按照3-1-2顺序（Z-X-Y）的旋转可以表示为：

def dcm_312(yaw, pitch, roll):
    """按照3-1-2顺序构建方向余弦矩阵"""
    Rz = rotation_z(yaw)    # 航向角
    Rx = rotation_x(pitch)  # 俯仰角
    Ry = rotation_y(roll)   # 横滚角
    return Ry @ Rx @ Rz     # 注意矩阵乘法的顺序

为了直观理解这些旋转，我们可以使用Matplotlib进行可视化：

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def plot_rotation(angles):
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')
    
    # 初始坐标系
    origin = np.array([0, 0, 0])
    x_axis = np.array([1, 0, 0])
    y_axis = np.array([0, 1, 0])
    z_axis = np.array([0, 0, 1])
    
    # 应用旋转
    C = dcm_312(*angles)
    x_rotated = C @ x_axis
    y_rotated = C @ y_axis
    z_rotated = C @ z_axis
    
    # 绘制
    ax.quiver(*origin, *x_axis, color='r', label='X轴')
    ax.quiver(*origin, *y_axis, color='g', label='Y轴')
    ax.quiver(*origin, *z_axis, color='b', label='Z轴')
    ax.quiver(*origin, *x_rotated, color='r', linestyle='--')
    ax.quiver(*origin, *y_rotated, color='g', linestyle='--')
    ax.quiver(*origin, *z_rotated, color='b', linestyle='--')
    
    ax.set_xlim([-1, 1])
    ax.set_ylim([-1, 1])
    ax.set_zlim([-1, 1])
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.legend()
    plt.title('坐标系旋转可视化')
    plt.show()

# 示例：航向30度，俯仰15度，横滚10度
plot_rotation([np.radians(30), np.radians(15), np.radians(10)])

3. 四元数运算与转换

3.1 四元数基本运算

四元数的运算规则比矩阵更复杂，但计算效率更高。我们先实现四元数的基本运算：

def quat_mult(q1, q2):
    """四元数乘法"""
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2
    return np.array([w, x, y, z])

def quat_conj(q):
    """四元数共轭"""
    return np.array([q[0], -q[1], -q[2], -q[3]])

def quat_norm(q):
    """四元数范数"""
    return np.sqrt(np.sum(q**2))

def quat_normalize(q):
    """四元数归一化"""
    return q / quat_norm(q)

3.2 四元数与DCM的相互转换

四元数和DCM可以相互转换，这在捷联惯导系统中非常有用：

def quat_to_dcm(q):
    """四元数转方向余弦矩阵"""
    q = quat_normalize(q)
    w, x, y, z = q
    return np.array([
        [1-2*(y**2+z**2), 2*(x*y-w*z), 2*(x*z+w*y)],
        [2*(x*y+w*z), 1-2*(x**2+z**2), 2*(y*z-w*x)],
        [2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x**2+y**2)]
    ])

def dcm_to_quat(C):
    """方向余弦矩阵转四元数"""
    q0 = 0.5 * np.sqrt(1 + C[0,0] + C[1,1] + C[2,2])
    q1 = (C[2,1] - C[1,2]) / (4*q0)
    q2 = (C[0,2] - C[2,0]) / (4*q0)
    q3 = (C[1,0] - C[0,1]) / (4*q0)
    return quat_normalize(np.array([q0, q1, q2, q3]))

3.3 四元数微分方程与姿态更新

在捷联惯导中，我们需要通过陀螺仪测量的角速度来更新姿态。四元数微分方程为：

$$ \dot{q} = \frac{1}{2}q \otimes \omega $$

其中$\omega$是角速度四元数（实部为0）。Python实现如下：

def quat_derivative(q, omega):
    """四元数微分方程"""
    omega_quat = np.array([0, *omega])  # 实部为0
    return 0.5 * quat_mult(q, omega_quat)

def quat_update(q, omega, dt):
    """四元数姿态更新"""
    dq = quat_derivative(q, omega)
    q_new = q + dq * dt
    return quat_normalize(q_new)

4. 等效旋转矢量与不可交换性误差

4.1 等效旋转矢量概念

等效旋转矢量$\phi$用一个三维向量表示旋转，其方向表示转轴，长度表示转角大小。它与四元数的关系为：

$$ q = [\cos(\frac{\phi}{2}), \frac{\phi_x}{\phi}\sin(\frac{\phi}{2}), \frac{\phi_y}{\phi}\sin(\frac{\phi}{2}), \frac{\phi_z}{\phi}\sin(\frac{\phi}{2})] $$

其中$\phi = \sqrt{\phi_x^2 + \phi_y^2 + \phi_z^2}$。

4.2 不可交换性误差的验证

在非定轴转动情况下，旋转的顺序会影响最终结果，这就是不可交换性误差。我们可以用Python验证这一点：

# 定义两个小角度旋转
angle1 = np.radians(5)  # 5度
angle2 = np.radians(10) # 10度

# 先绕X转angle1，再绕Y转angle2
C1 = rotation_y(angle2) @ rotation_x(angle1)

# 先绕Y转angle2，再绕X转angle1
C2 = rotation_x(angle1) @ rotation_y(angle2)

# 计算差异
diff = np.linalg.norm(C1 - C2)
print(f"两种旋转顺序的差异: {diff:.6f}")

运行结果会显示两种旋转顺序确实会导致不同的结果，验证了不可交换性。

4.3 等效旋转矢量补偿

为了补偿不可交换性误差，我们可以使用等效旋转矢量方法。双子样算法的Python实现如下：

def rotvec_dual_sample(theta1, theta2):
    """等效旋转矢量双子样算法"""
    return theta1 + theta2 + (2/3) * np.cross(theta1, theta2)

# 示例：两个连续角增量
theta1 = np.array([0.1, 0.05, 0.02])  # 弧度
theta2 = np.array([0.08, 0.03, 0.01]) # 弧度

phi = rotvec_dual_sample(theta1, theta2)
print(f"等效旋转矢量: {phi}")

5. 实际应用与性能优化

5.1 姿态解算流程

完整的捷联惯导姿态解算流程通常包括以下步骤：

1. 从陀螺仪读取角增量
2. 使用等效旋转矢量算法计算旋转矢量
3. 将旋转矢量转换为四元数增量
4. 更新当前姿态四元数
5. 将四元数转换为DCM或其他所需形式

def attitude_update(q_prev, gyro_data, dt):
    """完整的姿态更新流程"""
    # 1. 获取角增量
    theta = gyro_data * dt
    
    # 2. 等效旋转矢量补偿
    phi = theta  # 简化为单子样，实际应用应使用双子样或三子样
    
    # 3. 转换为四元数增量
    phi_norm = np.linalg.norm(phi)
    if phi_norm > 1e-6:
        delta_q = np.concatenate((
            [np.cos(phi_norm/2)],
            np.sin(phi_norm/2) * phi / phi_norm
        ))
    else:
        delta_q = np.array([1, 0, 0, 0])  # 无旋转
    
    # 4. 更新四元数
    q_new = quat_mult(q_prev, delta_q)
    
    return quat_normalize(q_new)

5.2 性能优化技巧

在实际应用中，姿态解算需要高效运行。以下是一些优化建议：

• 避免重复计算：预先计算并重用三角函数等耗时操作
• 使用SIMD指令：NumPy已经优化，但可以进一步利用特定硬件指令
• 定点数运算：在资源受限的嵌入式系统中，定点数比浮点数更高效
• 查表法：对于频繁使用的三角函数，可以预先计算并存储

# 示例：优化的四元数乘法
def fast_quat_mult(q1, q2):
    """优化的四元数乘法"""
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

在开发捷联惯导算法时，我经常遇到的一个问题是数值误差累积。特别是在长时间运行时，四元数可能会逐渐失去归一化特性。解决这个问题的一个实用技巧是定期重新归一化四元数，或者在每次更新后都进行归一化。