Python - 欧拉角 - 四元素 - 旋转矩阵

Python 计算欧拉角，四元素，旋转矩阵之间的转换

欧拉角(roll, pitch, yaw)

滚转角Φ（roll）：围绕X轴旋转的角度
俯仰角θ（pitch）：围绕Y轴旋转的角度
偏航角ψ（yaw）：围绕Z轴旋转的角度
描述物体在参考坐标系下的姿态，物体绕参考坐标系三个坐标轴(x, y, z轴）的旋转角度。

四元数(q0, q1, q2, q3)

q=q0 + q1 * i + q2 * j + q3 * k
描述物体在参考坐标系下的姿态，等效为物体绕某一转轴旋转一定的角度。
其中 q0, q1, q2, q3均为实数, q0² + q1² + q2² + q3² = 1
对于 i, j, k 本身的几何意义可以理解为一种旋转，
其中 i 代表 x 轴与 y 轴相交平面中 x 轴正向向 y 轴正向的旋转，
j 旋转代表 z 轴与 x 轴相交平面中 z 轴正向向 x 轴正向的旋转，
k 旋转代表 y 轴与 z 轴相交平面中 y 轴正向向 z 轴正向的旋转，
−i,−j,−k 分别代表 i, j, k 的反向旋转。

旋转矩阵

空间坐标系A(矩阵VA={XA，YA，ZA}T)，绕某一固定坐标系旋转后得到空间坐标系B(矩阵VB={XB，YB，ZB}T), 有VB=R*VA， R就是我们所说的旋转矩阵。（也可以是绕自身坐标系旋转，只是公式不同，但R还是我们所说的旋转矩阵）
可确定出坐标系B至坐标系A的坐标变换矩阵，矩阵R中A上标B下标

一般空间坐标系其矩阵为单位矩阵(|XA|=1; |YA|=1;|ZA|=1)

-	-	-	-
-	r11	r12	r13
R中A上标B下标	r21	r22	r23
-	r31	r32	r33

r11为cos(向量XB与向量XA夹角)，即XB(空间B中x轴的方向)在空间A中x轴的分量。
r12为cos(向量YB与向量XA夹角)，即YB(空间B中y轴的方向)在空间A中x轴的分量。
r13为cos(向量ZB与向量XA夹角)，即ZB(空间B中z轴的方向)在空间A中x轴的分量。
r21为cos(向量XB与向量YA夹角)，即XB(空间B中x轴的方向)在空间A中y轴的分量。
r22为cos(向量YB与向量YA夹角)，即YB(空间B中y轴的方向)在空间A中y轴的分量。
r23为cos(向量ZB与向量YA夹角)，即ZB(空间B中z轴的方向)在空间A中y轴的分量。
r31为cos(向量XB与向量ZA夹角)，即XB(空间B中x轴的方向)在空间A中z轴的分量。
r32为cos(向量YB与向量ZA夹角)，即YB(空间B中y轴的方向)在空间A中z轴的分量。
r33为cos(向量ZB与向量ZA夹角)，即ZB(空间B中z轴的方向)在空间A中z轴的分量。

from math import cos, sin, pi, atan2 ,asin
#欧拉角(x, y, z)转换为四元数(q0, q1, q2, q3)
#x , y , z 单位为角度
x, y, z  = 0 ,0 ,90
x, y, z  = x*pi/180 ,y*pi/180 ,z*pi/180
q0 ,q1 ,q2 ,q3 = 0 ,0 ,0 ,0
q0 = cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2)
q1 = sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2)
q2 = cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2)
q3 = cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)
print('欧拉角({0:f}, {1:f}, {2:f})转换为四元数(q0, q1, q2, q3)'.format(x*180/pi, y*180/pi, z*180/pi))    
print("q0 = {0:f}".format(q0))
print("q1 = {0:f}".format(q1))
print("q2 = {0:f}".format(q2))
print("q3 = {0:f}".format(q3))    
print() 

#四元数q=(q0,q1,q2,q3)到欧拉角(x, y, z)
q0 ,q1 ,q2 ,q3 = 0.707 ,0 ,0 ,0.707
x, y, z  = 0 ,0 ,0
x = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2))*180/pi
y = asin(2*(q0*q2 - q1*q3))*180/pi #asin = arcsin
z = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3))*180/pi
print('四元数q=({0:f},{1:f},{2:f},{3:f})到欧拉角(x, y, z))'.format(q0, q1, q2, q3))    
print("x = {0:f}".format(x))
print("y = {0:f}".format(y))
print("z = {0:f}".format(z))   
print() 

#欧拉角(x, y, z)转换为旋转矩阵
x, y, z  = 0 ,0 ,90
x, y, z  = x*pi/180 ,y*pi/180 ,z*pi/180
r11 ,r12 ,r13 = cos(z)*cos(y), cos(z)*sin(y)*sin(x) - sin(z)*cos(x), cos(z)*sin(y)*cos(x) + sin(z)*sin(x)
r21 ,r22 ,r23 = sin(z)*cos(y), sin(z)*sin(y)*sin(x) + cos(z)*cos(x), sin(z)*sin(y)*cos(x) - cos(z)*sin(x)
r31 ,r32 ,r33 = -sin(y), cos(y)*sin(x), cos(y)*cos(x)
print('欧拉角({0:f}, {1:f}, {2:f})转换为旋转矩阵'.format(x*180/pi, y*180/pi, z*180/pi))      
print('{0:f} , {1:f} , {2:f}'.format(r11 ,r12 ,r13))
print('{0:f} , {1:f} , {2:f}'.format(r21 ,r22 ,r23))
print('{0:f} , {1:f} , {2:f}'.format(r31 ,r32 ,r33))    
print() 

#四元数q=(q0,q1,q2,q3)到旋转矩阵
q0 ,q1 ,q2 ,q3 = 0.707 ,0 ,0 ,0.707
r11 ,r12 ,r13 = 1-2*(q2*q2 + q3*q3), 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)
r21 ,r22 ,r23 = 2*(q1*q2 + q0*q3), 1-2*(q1*q1 + q3*q3), 2*(q2*q3 - q0*q1)
r31 ,r32 ,r33 = 2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), 1-2*(q1*q1 + q2*q2)
print('四元数q=({0:f},{1:f},{2:f},{3:f})转换为旋转矩阵'.format(q0, q1, q2, q3))    
print('{0:f} , {1:f} , {2:f}'.format(r11 ,r12 ,r13))
print('{0:f} , {1:f} , {2:f}'.format(r21 ,r22 ,r23))
print('{0:f} , {1:f} , {2:f}'.format(r31 ,r32 ,r33))      
print()