旋转矩阵、欧拉角、四元数、轴/角之间的转换

在机器人学中，表示旋转的有四种方式。不同的人可能习惯于用不同的方法，现将四种方式之间的转换整理出来如下。

• 旋转矩阵

旋转矩阵R表示坐标系`O-x'y'z'`中的向量坐标变换为同一向量在坐标系`O-xyz`中的坐标的变换矩阵（transformation matrix）。
p=Rp' 
p'=R'p

旋转矩阵属于特殊正交群（special orthonormal group）；正交矩阵，每一列为单位矩阵，行列式为1。

- 描述了两个坐标系之间的相对指向。

- 表示了同一点在不同坐标系下（原点相同，即只有转动，没用平动）的坐标之间的坐标变换。

- 是将向量在同一坐标系下进行旋转的算子。	

• 欧拉角（RPY）

  绕Z轴旋转称为回转（Roll），绕Y轴旋转称为俯仰（Pitch），绕X轴旋转称为偏转（Yaw）。

  {A}为参考坐标系，将{A}分别按顺序沿$x_A,y_A,z_A$旋转$\gamma ,\beta ,\alpha$后，和{B}重合，{A}和{B}之间的旋转方程：
  $${}_B^A{R}_{xyz}=(\gamma ,\beta ,\alpha )=R(z_A,\alpha )R(y_A,\beta )R(x_A,\alpha )$$

• 四元数

    是角/轴的扩展。
  

• 轴/角

    描述一个坐标系沿某一条直线旋转一定的角度，即与另一个坐标系重合。
  

经常要用到他们之间的相互转换。

##一、旋转矩阵

###1、旋转矩阵转换为欧拉角

$${}_B^A{R}_{xyz}(\gamma ,\beta ,\alpha )=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

$$\{\begin{array}{l}\beta =atan2(-r_{31},\sqrt{r_{11}^2+r_{21}^2})\\ \alpha =atan2(r_{21},r_{11}),if\beta \in [-\pi /2,\pi /2]\\ \gamma =atan2(r_{32},r_{33}),if\beta \in [-\pi /2,\pi /2]\end{array}$$
或者：
$$\{\begin{array}{l}\beta =atan2(-r_{31},\sqrt{r_{11}^2+r_{21}^2})\\ \alpha =atan2(-r_{21},-r_{11}),if\beta \in [\pi /2,3\pi /2]\\ \gamma =atan2(-r_{32},-r_{33}),if\beta \in [\pi /2,3\pi /2]\end{array}$$

###2、旋转矩阵转化为 角/轴

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

$$\theta =acos(\frac{r_{11}+r_{22}+r_{33}-1}{2})$$
$$\overrightarrow{r}=\frac{1}{2sin\theta }\left[\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right]$$

###3、旋转矩阵转化为四元数

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

$$w=\frac{\sqrt{r_{11}+r_{22}+r_{33}+1}}{2}$$

$$\overrightarrow{v}=\frac{1}{2}\left[\begin{array}{c}sgn(r_{32}-r_{23})\sqrt{r_{11}-r_{22}-r_{33}+1}\\ sgn(r_{13}-r_{31})\sqrt{r_{22}-r_{11}-r_{33}+1}\\ sgn(r_{21}-r_{12})\sqrt{r_{33}-r_{22}-r_{11}+1}\end{array}\right]$$

##二、欧拉角（RPY）

1、欧拉角转换为旋转矩阵

$${}_B^A{R}_{xyz}(\gamma ,\beta ,\alpha )=\left[\begin{array}{ccc}{c}_{\alpha }{c}_{\beta }& c_{\alpha }{s}_{\beta }{s}_{\gamma }-s_{\alpha }{c}_{\gamma }& c_{\alpha }{s}_{\beta }{c}_{\gamma }-s_{\alpha }{s}_{\gamma }\\ s_{\alpha }{c}_{\beta }& s_{\alpha }{s}_{\beta }{s}_{\gamma }-c_{\alpha }{c}_{\gamma }& s_{\alpha }{s}_{\beta }{c}_{\gamma }-c_{\alpha }{s}_{\gamma }\\ -s_{\beta }& c_{\beta }{s}_{\gamma }& c_{\beta }{c}_{\gamma }\end{array}\right]$$

##三、四元数

1、四元数转化为旋转矩阵

$$R=\left[\begin{array}{ccc}2(w^2+v_x^2)-1& 2(v_x{v}_y-w{v}_z)& 2(v_x{v}_z+w{v}_x)\\ 2(v_x{v}_y+w{v}_z)& 2(w^2+v_y^2)-1& 2(v_y{v}_z-w{v}_x)\\ 2(v_x{v}_z-w{v}_x)& 2(v_y{v}_z+w{v}_x)& 2(w^2+v_z^2)-1\end{array}\right]$$

##四、轴/角

1、轴/角 转化为旋转矩阵

$$R=\left[\begin{array}{ccc}{r}_x^2(1-c\theta )+c\theta & r_x{r}_y(1-c\theta )-r_{z}s\theta & r_x{r}_z(1-c\theta )+r_{y}s\theta \\ r_x{r}_y(1-c\theta )+r_{z}s\theta & r_y^2(1-c\theta )+c\theta & r_y{r}_z(1-c\theta )-r_{x}s\theta \\ r_x{r}_z(1-c\theta )-r_{y}s\theta & r_y{r}_z(1-c\theta )+r_{x}s\theta & r_z^2(1-c\theta )+c\theta \end{array}\right]$$