VR-四元素、欧拉角转换条件

Posted 2023-04-25

参考技术A

转自 欧拉角细节/旋转顺序/内旋外旋

欧拉角是由 Leonhard Euler 提出的概念,用来描述刚体/移动坐标系在一个固定坐标系中的姿态.简单的说是使用XYZ三个轴的旋转分量,来描述一个6自由度的旋转.

欧拉角一般具有两大类表示方式,每类按照旋转次序的不同分为6小类:

Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)

Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z).

每个大类都使用了3个变量描述三次旋转过程中的旋转角度, 差别在于Proper Euler angles只涉及两个转轴.而Tait–Bryan angles涉及三个转轴.一般在SLAM中我们使用的是Tait–Bryan angles.

Tait–Bryan angles 也被称为Cardan angles, nautical angles, (heading, elevation, and bank),(yaw, pitch, and roll). 我们接触的比较多的是yaw(偏航), pitch(俯仰), roll(横滚).三个变量一般对应(车体,飞行器的)z,y,x三个坐标轴.

一般对于旋转矩阵(3 3),旋转向量/角轴(3 1),四元数(4*1),我们给定一串数字,就能表示清楚一个姿态/旋转.比如这里给出一个旋转矩阵R:

表示刚体在A某坐标系下的姿态, 我们就可以确切的画出刚体A的姿态.

但如果我给出一组欧拉角(后面都是指Tait–Bryan angles),绕x,y,z三个轴的转角分别为(α,β,γ),我们不能能确定一个明确的姿态.需要再追加两个属性:(1)旋转顺序(2)内旋/外旋.才能确定的给出这组欧拉角对应的姿态.

旋转顺序就是我们上文提到的 Tait–Bryan angles ( x - y - z , y - z - x , z - x - y , x - z - y , z - y - x , y - x - z ).

我们指定绕x轴旋转α,绕y轴旋转β.但是可以有多个旋转顺序, 比如:

情况1:先绕x轴旋转α,再绕y轴旋转β.得到姿态O1
情况2:先绕y轴旋转β,再绕x轴旋转α,得到姿态O2

得到的O1一般是不等于O2的.

对于x,y,z三个轴的不同旋转顺序一共有( x - y - z , y - z - x , z - x - y , x - z - y , z - y - x , y - x - z )六种组合.我们需要明确旋转顺序,才能确定欧拉角所指的姿态.

首先列出几种等价的概念:
内旋(intrinsic rotations) = 旋转轴(rotated axis)
外旋(extrinsic rotations) = 固定轴(static/fixed axis)

我们后面直接用内旋/外旋来描述.

内旋/外旋的定义:

假设在世界坐标系中XYZ中存在物体,物体自身坐标系为xyz,假设初始状态物体相对XYZ的旋转为(0,0,0),即xyz与XYZ重合.我们定义旋转顺序为z->y->x,转角分别为γ,β,α.
先绕z轴旋转γ,旋转过后,物体的x,y轴的坐标系发生了改变,z轴不变,得到新的物体自身坐标系,此时的坐标轴
不再与世界坐标系的坐标轴XY重合.

内旋,外旋的区别在于:
在转β(第二个转角)时:
内旋按照旋转后物体的坐标y轴.
外旋按照世界坐标系中的Y轴旋转.

旋转最后一个角度时亦然.

因此, 增加了这两个概念(旋转顺序, 内外旋)后,我们描述一个能表示确定姿态/旋转的欧拉角,应该这样:

旋转角度(α,β,γ),旋转顺序(z->y->x),外旋.
旋转角度(α,β,γ),旋转顺序(x->y->z),内旋.
等等, 三个元素缺一不可.

如何将欧拉角转换为四元数并从四元数中得到相同的欧拉角？

我使用Euler角度以XYZ的顺序旋转n形状，这意味着对象首先沿着X轴旋转，然后是Y然后Z。我想将欧拉角转换为四元数，然后使用一些[最好] Python代码或只是一些伪代码或算法从Quaternion中获得相同的欧拉角。下面，我有一些代码将欧拉角转换为四元数，然后转换四元数以获得欧拉角。但是，这并没有给我相同的欧拉角。

我认为问题是我不知道如何将偏航，俯仰和滚动与X，Y和Z轴联系起来。此外，我不知道如何改变代码中的转换顺序，以正确地将欧拉角转换为四元数，然后将四元数转换为欧拉角，以便我能够获得相同的欧拉角。有人可以帮我弄这个吗？

这是我使用的代码：

此函数将Euler角度转换为Quaternions：

def euler_to_quaternion(yaw, pitch, roll):

        qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
        qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
        qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
        qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)

        return [qx, qy, qz, qw]

这会将四元数转换为欧拉角：

def quaternion_to_euler(x, y, z, w):

        import math
        t0 = +2.0 * (w * x + y * z)
        t1 = +1.0 - 2.0 * (x * x + y * y)
        X = math.degrees(math.atan2(t0, t1))

        t2 = +2.0 * (w * y - z * x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        Y = math.degrees(math.asin(t2))

        t3 = +2.0 * (w * z + x * y)
        t4 = +1.0 - 2.0 * (y * y + z * z)
        Z = math.degrees(math.atan2(t3, t4))

        return X, Y, Z

我用它们如下：

import numpy as np
euler_Original = np.random.random(3) * 360).tolist() # Generate random rotation angles for XYZ within the range [0, 360)
quat = euler_to_quaternion(euler_Original[0], euler_Original[1], euler_Original[2]) # Convert to Quaternion
newEulerRot = quaternion_to_euler(quat[0], quat[1], quat[2], quat[3]) #Convert the Quaternion to Euler angles

print (euler_Original)
print (newEulerRot)

打印语句为euler_Original和newEulerRot打印不同的数字，我不希望如此。例如，如果euler_original包含像弧度的(0.2, 1.12, 2.31)这样的数字我得到这个四元数 - > [0.749, 0.290, -0.449, 0.389]并将四元数转换为欧拉角给我这个 - > (132.35, 64.17, 11.45)这是非常错误的。我想知道如何解决这个问题？

虽然我有兴趣通过对其进行更改来使上述代码工作，但我宁愿学习如何正确设置方程式。通过这种方式，即使改变应用欧拉角的旋转顺序（XYZ - > YZX等），我也知道如何得到正确的四元数。

答案

主要问题：

euler_to_quaternion的输入顺序与quaternion_to_euler的输出顺序不同

前者采用Z, Y, X（偏航，俯仰，滚动）的顺序，而后者返回X, Y, Z。固定：

def euler_to_quaternion(roll, pitch, yaw):
# or
euler_to_quaternion(euler_Original[2], euler_Original[1], euler_Original[0])

---

小问题

euler_to_quaternion获得弧度，而quaternion_to_euler获得度数。

本身并不是一个问题，但由于大多数库函数都使用它们，所以最好将角度保持为弧度。

X = math.atan2(t0, t1)
Y = math.asin(t2)
Z = math.atan2(t3, t4)