刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数

1.转化关系图

坐标变换是实际导航与定位中必不可少的一环，而其中旋转变换至关重要，本文对学习过程中关于坐标变换内容做一点总结，以下是本文提供的转换关系。

说明：旋转矩阵和四元数提供与其他三种旋转表示的转换关系，欧拉角与旋转向量只提供与旋转矩阵和四元数的换算关系。

2 换算关系

3.1 旋转矩阵换算至其他

• 旋转矩阵→旋转向量
  

• 旋转矩阵→欧拉角
  
  

• 旋转矩阵→四元数

3.2 四元数换算至其他

• 四元数->旋转矩阵
  

• 四元数->欧拉角
  

• 四元数->旋转向量

3.3 旋转向量转换至旋转矩阵与四元数

假设这里有一个旋转向量：q=nθ

• 旋转向量->旋转矩阵
  使用罗德里格式公式：
  
  n^表示向量n的取反对称矩阵操作，展开公式如下：
  
• 旋转矩阵->四元数
  

3.3 欧拉角转换到旋转矩阵和四元数

• 欧拉角->旋转矩阵
  

• 欧拉角->四元数
  
  欧拉角到四元数的计算为：
  

3 坐标变换

三维空间中，同一空间点在不同坐标系中的坐标换算。在计算机三维视觉的定位与位姿估算时，由于观测的位置在变动，不同时刻的观测位置就是一个个局部坐标系，而当我们需要把上一帧图像点换算到当前帧时，使用的就是不同坐标系中同一点的坐标换算。

4 坐标变换方法概述

4.1 换算公式

同一三维欧氏空间，一个坐标系相对于另一坐标系的关系可以用一个旋转加平移来描述，为便于理解，现做如下关于两个坐标系及相它规定：

• 世界坐标系（World）：固定不动的全局坐标系，这里记为坐标系C1。（在实际使用时也可以是上一帧坐标系）

• 局部坐标系（Local）:动坐标系，或者当前坐标系，这里记为坐标系C2。

• P-c2、P-c1为空间点P在C2、C1中的坐标。

• R12为在C1中C2的姿态矩阵（C2相对于C1的旋转矩阵），t12为C1中C2坐标原点的坐标（C2相对于C1的平移向量）。

则有：P-c1=R12*P-c2+t12
也可以反向的，及从世界坐标系换算到局部坐标系，此时公式为:P-c2=R21*P-c1+t21

注意：坐标变换与向量运动的关系

• 坐标变换时，向量（表示三位点）不动，坐标系运动，是同一向量的不同坐标系表示，此时向量是不变的（注意区分向量及向量的坐标是不同概念）。
• 向量运动时，向量绕某轴旋转（也可以加上平移），此时向量改变。

二者关系：

反向等效性：

• 旋转：向量主动绕某轴旋转θ°，其旋转后的坐标等于原向量保持不动，原坐标系绕该轴旋转 -θ°（即反向旋转θ°）后的该向量在新坐标系下的坐标。
• 平移：与旋转类似，坐标系反向运动后的向量坐标等效于向量正向运动后的坐标。

利于这一等效性，可以推导出如何利用旋转矩阵来计算坐标换算。

4.2 各旋转表示特点

由于两个坐标系间的平移很好表示，使用三维向量即可，但旋转表示比较复杂，主要有以下四种形式：

• 旋转矩阵：使用SO3(特殊正交群，即三阶正交矩阵构成的群)表示旋转。该形式可以这么理解，当点使用坐标基与点坐标相乘这种形式时（基向量线性组合），旋转矩阵左乘或者右乘该组合，相当于对原基向量进行线性变换，形成了新的正交基，其坐标也会进行相应的线性变换。

特点：换算直观方便，用坐标直接左乘或者右乘旋转矩阵就可以得到不同旋转情况下的换算结果。但由于该表示是非紧凑表示，旋转是三个自由度的量，旋转矩阵含有九个量。另外SO3带有正交约束，在进行非线性优化时不方便。

• 旋转向量：使用一个单位向量n表示旋转方向，角度为θ，则θn可以表示该旋转。即一个向量的方向表示旋转轴，模长表示旋转角度，这个和物理中的角速度定义很类似。

特点：紧凑表示，但具有奇异性（旋转超过2Π）。换算到旋转矩阵和四元数也很方便，在opencv中常用旋转向量（轴角）作为求得的旋转结果，你可以方便的转化为其他旋转表示形式。（有点像中间桥梁）

• 欧拉角：将任意旋转分解为绕X、Y、Z轴的旋转，然后依次将绕这三个轴的单次旋转矩阵相乘，得出最后的旋转矩阵。

特点：紧凑表示，对人类友好，在实际飞行控制或者姿态显示时，常常将欧拉角rpy作为旋转的表示形式，即roll：翻滚角，pitch:俯仰角，yaw:偏航角。但依次绕三个轴旋转时，在特定旋转角度（比如90°）,会形成万向锁问题（丢失一个自由度），即也具有奇异性。

• 四元数：一个实部加三个虚部形成的代数结构。

特点：它既是紧凑的，也不带奇异性，常用于以位姿为优化变量的优化问题中，是优化中的旋转表示形式，但它不直观，计算也相对复杂一点。

注：复数表示旋转具有天然优势，基于这一点，很多人对复数进行研究，想找出一种表示三维旋转的复数形式，最后有一个人成功了，发明了四元数，这人十分高兴，将他发明的四元数公式刻在了桥边的一块石碑上，供来往人瞻仰。

参考

1.旋转矩阵、欧拉角、四元数、轴/角之间的转换
2.欧拉角、四元数、旋转矩阵推导及相互关系
3.三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换
4.欧拉角wiki