旋转角度和四元数互相转换_角度转四元数

三维坐标系转换过程中，三个坐标轴的旋转角度和对应的四元数相互转换

1.旋转角转为四元数，公式如下：

import math
#输入的Rx, Ry, Rz为角度
def Rxyz_to_Quaternion(Rx, Ry, Rz):
    X, Y, Z = Rx/180 * PI,Ry/180 * PI,Rz/180 * PI
    Qx = math.cos(Y/2)*math.cos(Z/2)*math.sin(X/2)-math.sin(Y/2)*math.sin(Z/2)*math.cos(X/2)                  
    Qy = math.sin(Y/2)*math.cos(Z/2)*math.cos(X/2)+math.cos(Y/2)*math.sin(Z/2)*math.sin(X/2)
    Qz = math.cos(Y/2)*math.sin(Z/2)*math.cos(X/2)-math.sin(Y/2)*math.cos(Z/2)*math.sin(X/2)
    QW = math.cos(Y/2)*math.cos(Z/2)*math.cos(X/2)+math.sin(Y/2)*math.sin(Z/2)*math.sin(X/2)
    print(Qx,Qy,Qz,QW)
    return Qx,Qy,Qz,QW
1
2
3
4
5
6
7
8
9
10

2.四元数转为旋转角度，公式如下：

先绕X轴、再绕Y轴、再绕Z轴：

def Quaternion_to_Rxyz(Qx,Qy,Qz,Qw):
    Rx = math.atan2(2 * (Qw * Qx + Qy * Qz), 1 - 2 * (Qx ** 2 + Qy ** 2))
    Ry = math.asin(2 * (Qw * Qy - Qz * Qx))
    Rz = math.atan2(2 * (Qw * Qz + Qx * Qy), 1 - 2 * (Qy ** 2 + Qz ** 2))
    print(Rx/PI*180,Ry/PI*180,Rz/PI*180)
    return Rx, Ry, Rz
1
2
3
4
5
6