在 Python 中可视化四元数

Question

我在无人机上安装了一个 IMU，每 0.1 秒收集一次四元数数据 (w,x,y,z)。现在我想将四元数数据与实际的无人机方向（视频数据）进行比较。

所以我想创建某种盒子对象来显示基于四元数数据的方向。

我实现了本教程，该教程将转换将四元数转换为欧拉以进行可视化。

有没有一种方法可以直接可视化四元数数据，而不需要先将其转换为欧拉？我觉得这应该是可能的，但我不确定如何实现。

Answer 1

您可以将四元数可视化，而无需将其转换为欧拉角。四元数只不过是 3D 空间中的一个复数，但它不是沿着 i 具有一个虚部，而是沿着 i、j 和 k 具有三个虚部。因此，您可以使用它来旋转 3D 参考位置向量，就像您在 2D 中使用复数进行旋转一样，而不依赖于其角度。

---

在下面的代码中，我创建了一个 IMU 四元数示例，对应横滚 = 5°、俯仰 = 45° 和横滚 = 30°。我使用这个四元数来旋转 3 个点（位置向量），这些点代表与地球相关的笛卡尔坐标系的单位向量。因此，3 个变换点代表与无人机相关的机身框架的单位向量。这些矢量显示了当前无人机的姿态。您需要对 att 四元数做的就是：

# Quaternion inverse (equal conjugate in this case)
c_att = att.conjugate()

# Rotate the points
px_rot = att * px * c_att
py_rot = att * py * c_att
pz_rot = att * pz * c_att

使用一些约定（您可能需要根据您的特定需求更改它们）：

• 地球是一个 NED（北、东、下）坐标系。
• 旋转前，机身坐标系与地球坐标系对齐，即横滚轴与 N 重合，俯仰轴与 E 重合，偏航轴与 D 重合
• 。两个坐标系均为右旋，旋转也为右旋。

为了减轻四元数相乘的负担，我使用了一个模块将四元数添加到 Numpy（您可以在 此处）。

输出：

蓝色为地球框架 (NED)，红色为无人机框架（机身框架） 您可以用更直观的飞机轮廓替换这 3 个点。

---

首先，我需要创建 IMU 样本，您不必这样做，而是使用 IMU 输出：

from numpy import cos, sin, radians, quaternion
import matplotlib.pyplot as plt

# Attitude example, according to Tait-Bryan 321 convention
u, v, w = radians(5), radians(45), radians(30) # roll, pitch, yaw

# IMU quaternion corresponding to attitude example
cu, cv, cw = cos(u/2), cos(v/2), cos(w/2)
su, sv, sw = sin(u/2), sin(v/2), sin(w/2)
q0 = cu*cv*cw + su*sv*sw
q1 = su*cv*cw - cu*sv*sw
q2 = cu*sv*cw + su*cv*sw
q3 = cu*cv*sw - su*sv*cw
att = quaternion(q0, q1, q2, q3)

地球框架单位向量的头：

# Quaternions representing heads of a Cartesian frame unit vectors
px = quaternion(0, 1, 0, 0)
py = quaternion(0, 0, 1, 0)
pz = quaternion(0, 0, 0, 1)

这些点被旋转。旋转涉及四元数积，格式为 rotated = q * point * q'，其中 q' 是 q 的倒数，在本例中共轭（调整代码以在循环中处理 IMU 四元数）。

# Quaternion inverse (equal conjugate in this case)
c_att = att.conjugate()

# Rotate the points
px_rot = att * px * c_att
py_rot = att * py * c_att
pz_rot = att * pz * c_att

绘制两个框架，地球为蓝色，旋转（身体）为红色。

 # Plot points before/after
 # Convention: x is the roll axis, -y is the pitch axis, -z is the yaw axis
fig, ax = plt.subplots(figsize=(5, 6), subplot_kw={'projection': '3d'})
ax.set_xlabel('Roll axis')
ax.set_ylabel('Pitch axis')
ax.set_zlabel('Yaw axis')
for qx, qy, qz, c in [[px, py, pz, 'blue'],
                      [px_rot, py_rot, pz_rot, 'red']]:
    for q in qx, qy, qz:
        ax.plot(xs=(0, q.x), ys=(0, -q.y), zs=(0, -q.z), c=c)

# Adjust plots
ax.set_aspect('equal')