写在前面

  • 欧拉角以Roll、Pitch、Yaw的顺序表示
  • 四元数以[ q w q_w qw, q x q_x qx, q y q_y qy, q z q_z qz]的顺序表示
  • 代码包括了欧拉角与四元数互转旋转矩阵与四元数互转欧拉角与旋转矩阵互转,输入参数均为np.array形式
  • 代码内置了角度制和弧度制😃😃当时因为这块吃了好多亏
  • 顺便测试了一下pydrake库,发现:
    1. pydrake库中是弧度制
    2. 输出结果与代码输出结果几乎一致(但pydrake精度更高)
  • 由于原理这块肯定已经有很多很完善的资料了所以不做过多赘述

思路

东拼西凑找出的这些公式,为了验证他们的正确性,采用以下方法验证:
随机生成一个欧拉角e,将其与
a. 欧拉角e -> 旋转矩阵r -> 欧拉角e
b. 欧拉角e -> 四元数q -> 欧拉角e
c. 欧拉角e -> 四元数q -> 旋转矩阵r -> 欧拉角e
d. 欧拉角e -> 旋转矩阵r -> 四元数q -> 欧拉角e
比较,看误差有多大(如图所示),并重复n轮
在这里插入图片描述
直接上代码!

注意⚠️

涉及到旋转矩阵的四个函数似乎有点问题【虽然自洽但是结果不对!】
如果最后导出的是旋转矩阵,请暂时不要使用本代码!!!
红豆泥私密马赛!


代码部分

utils.py

import math
import numpy as np 

# from pydrake.all import RotationMatrix, RollPitchYaw
 
# 四元数转欧拉角
# ================OKOK
def quaternion_to_euler(q, degree_mode=1):
    qw, qx, qy, qz = q

    roll = math.atan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx ** 2 + qy ** 2))
    pitch = math.asin(2 * (qw * qy - qz * qx))
    yaw = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy ** 2 + qz ** 2))
    # degree_mode=1:【输出】是角度制,否则弧度制
    if degree_mode == 1:
        roll = np.rad2deg(roll)
        pitch = np.rad2deg(pitch)
        yaw = np.rad2deg(yaw)
    euler = np.array([roll, pitch, yaw])
    return euler
 
# 欧拉角转四元数
# ================OKOK
def euler_to_quaternion(euler, degree_mode=1):
    roll, pitch, yaw = euler
    # degree_mode=1:【输入】是角度制,否则弧度制
    if degree_mode == 1:
        roll = np.deg2rad(roll)
        pitch = np.deg2rad(pitch)
        yaw = np.deg2rad(yaw)

    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)
    q = np.array([qw, qx, qy, qz])
    return q

# 四元数转旋转矩阵:
def quaternion_to_rot(q): 
    q0,q1,q2,q3=q 

    R=np.array([[1-2*(q2**2+q3**2),2*(q1*q2-q3*q0),2*(q1*q3+q2*q0)], 
                     [2*(q1*q2+q3*q0),1-2*(q1**2+q3**2),2*(q2*q3-q1*q0)], 
                     [2*(q1*q3-q2*q0),2*(q2*q3+q1*q0),1-2*(q1**2+q2**2)]]) 
    return R 
 
# 旋转矩阵转四元数:
# ================OKOK
def rot_to_quaternion(R): 
    R=R 
    
    qw=np.sqrt(1+R[0,0]+R[1,1]+R[2,2])/2 
    qx=(R[2,1]-R[1,2])/(4*qw) 
    qy=(R[0,2]-R[2,0])/(4*qw) 
    qz=(R[1,0]-R[0,1])/(4*qw) 
    q = np.array([qw, qx, qy, qz])
    return q

# 旋转矩阵转欧拉角
# ================OKOK
def rot_to_euler(R, degree_mode=1):
    sy = np.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
 
    singular = sy < 1e-6
 
    if not singular :
        roll = np.arctan2(R[2,1] , R[2,2])
        pitch = np.arctan2(-R[2,0], sy)
        yaw = np.arctan2(R[1,0], R[0,0])
    else :
        roll = np.arctan2(-R[1,2], R[1,1])
        pitch = np.arctan2(-R[2,0], sy)
        yaw = 0
    
    # degree_mode=1:【输出】是角度制,否则弧度制
    if degree_mode == 1:
        roll = np.rad2deg(roll)
        pitch = np.rad2deg(pitch)
        yaw = np.rad2deg(yaw)

    euler = np.array([roll, pitch, yaw])
    return euler

# 欧拉角转旋转矩阵
# # ================OKOK
def euler_to_rot(euler, degree_mode=1):
    roll, pitch, yaw = euler
    # degree_mode=1:【输入】是角度制,否则弧度制
    if degree_mode == 1:
        roll = np.deg2rad(roll)
        pitch = np.deg2rad(pitch)
        yaw = np.deg2rad(yaw)

    R_x = np.array([
    [1,     0,              0              ],
    [0,     math.cos(roll), -math.sin(roll)],
    [0,     math.sin(roll), math.cos(roll) ]
    ])
 
    R_y = np.array([
    [math.cos(pitch),  0,   math.sin(pitch) ],
    [0,                1,   0               ],
    [-math.sin(pitch), 0,   math.cos(pitch) ]
    ])
 
    R_z = np.array([
    [math.cos(yaw), -math.sin(yaw),  0],
    [math.sin(yaw), math.cos(yaw),   0],
    [0,             0,               1]
    ])
    
    R = np.dot(R_z, np.dot( R_y, R_x ))
    return R


if __name__ == "__main__":
    # 测试思路:随机生成几组欧拉角,用各种转换后看是否与原数据相同
    # 备注:np.sin(),math.sin()的返回值都是弧度制
    # np.atan2(), math.asin()这些也是弧度制
    print(np.sin(90), np.sin(45))
    print(np.sin(3.14), np.sin(1.57))
    print(math.sin(90), math.sin(45))
    print(math.sin(3.14), math.sin(1.57))
    mode = 0  # 当这个值等于1时, 输入和输出的欧拉角均为 <角度制>
    for i in range(10):
        print('=' * 32, "index:", i, '=' * 35)
        euler = np.random.randint(0, 360, 3) - 180
        # euler = np.array([60, 30, 45])

        if mode != 1:
            # 限制最多小数点后三位
            euler = np.array([round(t * math.pi / 180, 3) for t in euler])

        print("euler = ", euler)
        e2q2e = quaternion_to_euler(euler_to_quaternion(euler, mode), mode)
        e2r2e = rot_to_euler(euler_to_rot(euler, mode), mode)
        e2r2q2e = quaternion_to_euler(rot_to_quaternion(euler_to_rot(euler, mode)), mode)
        e2q2r2e = rot_to_euler(quaternion_to_rot(euler_to_quaternion(euler, mode)), mode)
        
        # 限制这些值为[-180,179)
        if mode == 1:
            e2q2e = [(int((180 * 3 + t)) % 360) - 180 for t in e2q2e]
            e2r2e = [(int((180 * 3 + t)) % 360) - 180 for t in e2r2e]
            e2r2q2e = [(int((180 * 3 + t)) % 360) - 180 for t in e2r2q2e]
            e2q2r2e = [(int((180 * 3 + t)) % 360) - 180 for t in e2q2r2e]
        else:  # 限制最多小数点后三位
            e2q2e = [round((int((math.pi * 3 * 1000 + t * 1000)) % 6283 - 3142) / 1000, 3)\
                 for t in e2q2e]    
            e2r2e = [round((int((math.pi * 3 * 1000 + t * 1000)) % 6283 - 3142) / 1000, 3)\
                 for t in e2r2e]   
            e2r2q2e = [round((int((math.pi * 3 * 1000 + t * 1000)) % 6283 - 3142) / 1000, 3)\
                 for t in e2r2q2e]   
            e2q2r2e = [round((int((math.pi * 3 * 1000 + t * 1000)) % 6283 - 3142) / 1000, 3)\
                 for t in e2q2r2e]     

        print("e -> q -> e = ", e2q2e, "norm of delta = ", np.linalg.norm(e2q2e - euler))
        print("e -> r -> e = ", e2r2e, "norm of delta = ", np.linalg.norm(e2r2e - euler))
        print("e -> r -> q -> e = ", e2r2q2e, "norm of delta = ", np.linalg.norm(e2r2q2e - euler))
        print("e -> q -> r -> e = ", e2q2r2e, "norm of delta = ", np.linalg.norm(e2q2r2e - euler))

        # if mode == 0:   
        #     # pydrake中的旋转角是弧度制
        #     print("drake actual rot = ", RotationMatrix(RollPitchYaw(euler[0],euler[1],euler[2])))
        #     print("rot =", euler_to_rot(euler, mode))
        #     print("e -> q -> rot =", quaternion_to_rot(euler_to_quaternion(euler, mode)))

那串看起来非常笨重的限制我感觉我玷污了这个语言的美、、

一个问题

  • 使用随机数,会发现部分随机的欧拉角会变成另一个角,这两个角的效果应该是等效的
    在这里插入图片描述
    (使用弧度制也是一样)

  • 不过个人感觉在实际应用中应该也不会出现这种"错误的角度",因为角度都是通过各种正确值之间的转换得来的

补充

这里感谢下这个博主——在这里插入图片描述
下面这几个网站都是从他的这篇文章转过来的👇

工具网站推荐 - 欧拉角四元数在线可视化转化网站/三维在线旋转变换网站

1 欧拉角四元数在线可视化转换网站

内容:四元数与欧拉角的可视化
特点:直观,明显
缺点:没有旋转矩阵
网站地址:https://quaternions.online/
在这里插入图片描述

2 三维在线旋转变换网站

内容:四元数与欧拉角与旋转矩阵的转换
特点:全面,丰富,可以调欧拉角形式
缺点:有一些并不常用的参数,容易搞混懵圈
网站地址:https://www.andre-gaschler.com/rotationconverter/
在这里插入图片描述

补充一点关于上面的问题

  • 输入这个"错误欧拉角"在两个网站里也会得到一个不同的欧拉角,见上图和下图,所以应该不影响,也就是说正常使用时不会出现这种"错误欧拉角"
    在这里插入图片描述
Logo

开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!

更多推荐