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

BZ_PP

已于 2023-05-10 11:48:01 修改

阅读量1.8k

点赞数 2

CC 4.0 BY-SA版权

文章标签：几何学

于 2023-03-08 15:03:51 首次发布

本文链接：https://blog.youkuaiyun.com/BZ_PP/article/details/129403308

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

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

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