Как преобразовать углы Эйлера в кватернионы и получить те же углы Эйлера от кватернионов? - PullRequest
Новая
       23

Как преобразовать углы Эйлера в кватернионы и получить те же углы Эйлера от кватернионов?

0 голосов
/ 28 октября 2018

Я вращаю n трехмерную фигуру, используя углы Эйлера в порядке XYZ, что означает, что объект сначала вращается вдоль оси X, затем Y, а затем Z.Я хочу преобразовать угол Эйлера в кватернион, а затем получить те же углы Эйлера от кватерниона, используя некоторый [предпочтительно] код Python или просто какой-то псевдокод или алгоритм.Ниже у меня есть некоторый код, который преобразует угол Эйлера в кватернион, а затем преобразует кватернион, чтобы получить углы Эйлера.Однако это не дает мне одинаковые углы Эйлера.

Я думаю, проблема в том, что я не знаю, как связать рыскание, тангаж и крен с осями X, Y и Z.Кроме того, я не знаю, как изменить порядок преобразований в коде, чтобы правильно преобразовать углы Эйлера в кватернион, а затем преобразовать кватернион в угол Эйлера, чтобы я мог вернуть тот же угол Эйлера.Может ли кто-нибудь помочь мне с этим?

А вот код, который я использовал:

Эта функция преобразует углы Эйлера в кватернионы: 

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 и т. Д.) Для применения углов Эйлера.

1 Ответ

0 голосов
/ 29 октября 2018

Основная проблема :

Порядок ввода 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)
...