欧拉角，四元数与旋转矩阵

---

一、欧拉角

对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角(x,y,z)来表现。对于夹角的顺序和标记，夹角的两个轴的指定，并没有任何常规规定。所以每当用到欧拉角时，我们必须明确表示出夹角的顺序，指定其参考轴。
首先绕z轴转动α角，然后是绕X’轴转动β角，最后是绕Z’轴转动γ角，这是zxz顺规（先绕z轴，再绕x轴再绕z‘轴）的欧拉角表示方法。（除了zxz顺规外还有其他的规定方法，如xyx，zyz。）
欧拉角包括3个旋转，根据这3个旋转来指定一个刚体的朝向。这3个旋转分别绕x轴，y轴和z轴，分别称为 Pitch，Yaw 和 Roll。

优点：

• 由三个角度组成，直观易懂
• 可以进行大于180度的旋转

缺点：

• 欧拉角是不可传递的，旋转的顺序影响旋转的结果，不同的应用又可能使用不同的旋转顺序，旋转顺序无法统一
• 3个旋转的角度可以不受限制，即取值范围是（-inf，inf）
• 存在万向节死锁问题

二、四元数

既然欧拉角是多次旋转后才能得到，那么能否一步到位，只旋转一次呢？--------> 四元数
对于一个物体的旋转，我们只需要知道四个值：一个旋转的向量 + 一个旋转的角度。而四元数也正是这样的设计：
q=(x,y,z,w)。其中x,y,z 代表的是向量的三维坐标，w代表的是角度。四元数本质上是一个超复数:q=xi+yj+zk+w。
如：
一个向量：v1，要让它绕 v2 旋转θ度（顺时针转动），那么有p = (v1, 0); q = ( v2 * sin(θ/2) , cos(θ/2) )，旋转后的四元数为（得到的四元数实部为0，虚部为新的坐标）：优点：

• 存储空间小，计算效率高
• 不存在万向节锁问题

缺点：

• 数字表示不直观
• 单个四元数不能表示在任何方向上超过180度的旋转

三、旋转矩阵

假设绕XYZ三个轴旋转的角度分别为 α ，β ，γ ，则三次旋转的旋转矩阵计算方法如下：

若按Z-Y-X旋转顺序（指先绕自身轴Z，再绕自身轴Y，最后绕自身轴X），则旋转矩阵为：

四、Python下欧拉角、四元数和旋转矩阵的相互转换

主要依赖于scipy库下的Rotation包

from scipy.spatial.transform import Rotation as R

具体代码如下：

from scipy.spatial.transform import Rotation as R

def quaternion2euler(quaternion): #四元数转欧拉角
        r = R.from_quat(quaternion)
        euler = r.as_euler('xyz', degrees=True)
        return euler

def euler2quaternion(euler):   #欧拉角转四元数
        r = R.from_euler('xyz', euler, degrees=True)
        quaternion = r.as_quat()
        return quaternion

def euler2rot(euler):   #欧拉角转旋矩阵
        r = R.from_euler('xyz', euler, degrees=True)
        rotation_matrix = r.as_matrix()
       return rotation_matrix

def isRotationMatrix(R):  #判断是否为旋转矩阵
        Rt = np.transpose(R)
        shouldBeIdentity = np.dot(Rt, R)
        I = np.identity(3, dtype=R.dtype)
        n = np.linalg.norm(I - shouldBeIdentity)
        return n < 1e-6

def rot2quaternion(rot):   #旋转矩阵转四元数
        rot = rot[:3,:3]
        quat = R.from_matrix(rot).as_quat()
        return quat

def quaternion2rot(quat):    #四元数转旋转矩阵
        rot  = R.from_quat(quat).as_matrix()
        return rot

---

总结

概念介绍主要内容摘自欧拉角，轴角，四元数与旋转矩阵详解