别再死记硬背了！用Python+NumPy手把手带你玩转捷联惯导中的方向余弦矩阵与四元数

用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 ])

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