在Python中可视化四元数

Question

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

所以我想创建某种盒子对象来显示基于四元数数据的方向。我实现了以下教程，将四元数转换为欧拉以进行可视化：https://www.youtube.com/watch?v=S77r-P6YxAU&list=PLGs0VKk2DiYwEo-k0mjIkWXlkrJWAU4L9&index=21

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

Answer 1

您可以将四元数可视化，而无需将其转换为欧拉角。在下面的代码中，我创建了一个 IMU 四元数示例，对应横滚 = 5°、俯仰 = 45° 和横滚 = 30°。我使用这个四元数来旋转代表与地球相关的笛卡尔坐标系的单位向量的点。因此，变换后的点表示与无人机相关的机身框架的单位向量。这些矢量显示了当前无人机的姿态。

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

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

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

输出：

您可以通过更直观的飞机轮廓来替换这些点，选择要旋转的适当点。

---

首先我创建 IMU 示例，您不必执行此操作，而是使用 IMU 输出：

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

# Attitude example
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

的倒数，在本例中是共轭。调整代码以循环处理四元数。

# 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')