欧拉角，四元数和旋转矩阵互转代码【python版】

写在前面

• 欧拉角以Roll、Pitch、Yaw的顺序表示
• 四元数以[$q_w$,$q_x$,$q_y$,$q_z$]的顺序表示
• 代码包括了欧拉角与四元数互转，旋转矩阵与四元数互转，欧拉角与旋转矩阵互转，输入参数均为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/

补充一点关于上面的问题

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