Apollo学习笔记（25）旋转矩阵、欧拉角、四元数理论及其转换关系

1.概述

做MPC控制的时候，需要把 map 坐标系下的相关坐标点转换到 car 坐标系中，由于是只考虑 xy 平面，所以计算比较简单。做完之后，考虑到三维空间的坐标系转换还是不太懂，主要是ROS系统中的tf变换，于是摸了两天鱼。今天详细的记录下来，旋转矩阵、四元素、欧拉角以及各个之间的转换关系。
阅读了好几篇大神的文章，具体也找不到连接了，在此顶礼膜拜，如有相似的地方，那就相似吧，毕竟原理都一样。
本文采用的为右手坐标系，以车身为参考的话，就是车头朝向为x轴正向，垂直于x轴且指向车身左侧为y轴正向，z轴垂直于xy平面，且向上为正方向，原点为车辆质心，此时车辆绕z轴旋转角为yaw角 ，y轴旋转为 pitch角 , x轴旋转为 roll角。

2.原理

2.1旋转矩阵

对于两个三维点 $p1(x1,y1,z1)$，$p2(x2,y2,z2)$，由点 $p1$ 经过旋转矩阵 $R$ 旋转到 $p2$，则有：
$$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]$$
$$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=R\ast \left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$
注意：旋转矩阵为正交矩阵$R\ast R^T=E$。
绕x轴旋转角度$\theta$
$$R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\theta )& -sin(\theta )\\ 0& sin(\theta )& cos(\theta )\end{array}\right]$$

绕y轴旋转角度$\theta$
$$R_y(\theta )=\left[\begin{array}{ccc}cos(\theta )& 0& sin(\theta )\\ 0& 1& 0\\ -sin(\theta )& 0& cos(\theta )\end{array}\right]$$

绕z轴旋转角度$\theta$
$$R_z(\theta )=\left[\begin{array}{ccc}cos(\theta )& -sin(\theta )& 0\\ sin(\theta )& cos(\theta )& 0\\ 0& 0& 1\end{array}\right]$$

任意一个坐标系都可以通过依次绕着三个坐标轴旋转到指定的朝向，这三个角度就是欧拉角。

2.2 欧拉角

还是以汽车为例子，车身本身有一个车身坐标系 car ，同时，车辆定位以及路点所采用的是 map 坐标系，车辆的运动相对于这两个坐标系的意义是不同的，这里不考虑车辆的移动变化，仅仅考虑旋转：

1. 不论车辆如何运动，坐标系 car 始终是和车辆的运动保持一致的，所以车辆的运动对于 car 坐标系来说，始终保持静止车辆坐标对于car坐标系不变，car 坐标系会随着车辆的运动而不停的变化，因为坐标系不是静止的，此时的欧拉角称为动态欧拉角
2. 另一种是车辆的运动相对于 map 坐标系，车辆本身的坐标对于坐标系 map 不停的发生变化，但是坐标系 map 却是保持不动的，此时的欧拉角称为静态欧拉角
   使用动态欧拉角的时候会发生万向锁现象，静态欧拉角则不会。万向锁的现象，简单来说，就是坐标系绕某一轴旋转90°后，再换另外一个轴旋转的时候，会失去一个自由度，如下图所示：
   
   
   实际使用中，首先需要选定一个参考系做为参考系（参考系是静止不动的，在做相关的数据处理的时候，其他坐标系的朝向一般都是向参考系转换的），如上图所示，wiki找的图，这里选取XYZ坐标系做为参考系，xyz坐标系为刚体本身的坐标系，会随着刚体的运动而运动。直线N为xy平面与XY平面的交线，$\alpha$为x轴与交线N的夹角，$\beta$为z轴与Z轴的夹角，$\gamma$为交线与X轴的夹角。
   每当使用欧拉角时，必须明确的指出夹角的顺序，并指定其参考轴。即使相同的欧拉角，按照不同的旋转顺序，最终得到的结果是不一样的，这个必须要注意。

2.2.1 由欧拉角求旋转矩阵

设三个轴$x,y,z$的欧拉角分别为${\theta }_x,{\theta }_y,{\theta }_z$，对应的正弦值、余弦值简写为$s_x,c_x,s_y,c_y,s_z,c_z$，且旋转的顺序为$yaw,pitch.roll$,因此旋转矩阵为
$$R({\theta }_z,{\theta }_y,{\theta }_x)=R_z({\theta }_z)\ast R_y({\theta }_y)\ast R_x({\theta }_x)=\left[\begin{array}{ccc}{c}_y{c}_z& c_z{s}_x{s}_y-c_x{s}_z& s_x{s}_z+c_x{c}_z{s}_y\\ c_y{s}_z& c_x{c}_z+s_x{s}_y{s}_z& c_x{s}_y{s}_z-c_z{s}_x\\ -s_y& c_y{s}_x& c_x{c}_y\end{array}\right]$$

2.2.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]=\left[\begin{array}{ccc}{c}_y{c}_z& c_z{s}_x{s}_y-c_x{s}_z& s_x{s}_z+c_x{c}_z{s}_y\\ c_y{s}_z& c_x{c}_z+s_x{s}_y{s}_z& c_x{s}_y{s}_z-c_z{s}_x\\ -s_y& c_y{s}_x& c_x{c}_y\end{array}\right]$$
求解方程，可以有
$${\theta }_x=atan\frac{r_{32}}{r_{33}}$$
$${\theta }_y=atan\frac{-r_{31}}{\sqrt{r_{32}^2+r_{33}^2}}$$
$${\theta }_z=atan\frac{r_{21}}{r_{11}}$$
从上面的公式，也可以看出万向锁的问题，比如$r_{11}，r_{33}$等于0的时候，公式就无法使用了，所以在由旋转矩阵向欧拉角求解的时候，要注意看对应的矩阵元素是不是为0。

2.3 四元素(Quaternion)

学过CAD的都知道，三维控制可以按照一定的方式投影到二维，那么同理，四维空间也可以通过一样的方式投影到三维。然后，四元素就神奇的产生了。
通常情况下，都会采用单位四元素$q=\omega +x\stackrel{\rightharpoonup }{i}+y\stackrel{\rightharpoonup }{j}+z\stackrel{\rightharpoonup }{k}=[s,V]$来表示旋转，其中
$$\Vert q\Vert =x^2+y^2+z^2+{\omega }^2=1$$
具体的四元素相关运算法则，这里就不详细描述了。

2.3.1 由四元素求解旋转矩阵

$$R(q)=\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy-2zw& 2xz+2yw\\ 2xy+2zw& 1-2x^2-2z^2& 2yz-2xw\\ 2xz-2yw& 2yz+2xw& 1-2x^2-2y^2\end{array}\right]$$

2.3.2 由旋转矩阵求解四元素

• 情况1
  $$\omega =\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}$$
  $$x=\frac{r_{32}-r_{23}}{4\omega }$$
  $$y=\frac{r_{13}-r_{31}}{4\omega }$$
  $$z=\frac{r_{21}-r_{12}}{4\omega }$$
  其中，需要满足$\omega \ne 0,1+r_{11}+r_{22}+r_{33}>0$，即$1+tr(R)>0$
• 情况2
  如果$\omega$趋近于0，$tr(R)$趋近于 -1，则求解四元素的过程为
  – 情况 2.1 如果$max\left\{\begin{array}{c}{r}_{11},r_{22},r_{33}\end{array}\right\}=r_{11}$
  $$t=\sqrt{1+r_{11}-r_{22}-r_{33}}$$
  $$\omega =\frac{r_{32}-r_{23}}{t}$$
  $$x=\frac{t}{4}$$
  $$y=\frac{r_{13}+r_{31}}{t}$$
  $$z=\frac{r_{12}+r_{21}}{t}$$
  – 情况 2.2 如果$max\left\{\begin{array}{c}{r}_{11},r_{22},r_{33}\end{array}\right\}=r_{22}$
  $$t=\sqrt{1-r_{11}+r_{22}-r_{33}}$$
  $$\omega =\frac{r_{13}-r_{31}}{t}$$
  $$x=\frac{r_{12}+r_{21}}{t}$$
  $$y=\frac{t}{4}$$
  $$z=\frac{r_{32}+r_{23}}{t}$$
  – 情况 2.3 如果$max\left\{\begin{array}{c}{r}_{11},r_{22},r_{33}\end{array}\right\}=r_{33}$
  $$t=\sqrt{1-r_{11}-r_{22}+r_{33}}$$
  $$\omega =\frac{r_{21}-r_{12}}{t}$$
  $$x=\frac{r_{13}+r_{31}}{t}$$
  $$y=\frac{r_{23}-r_{32}}{t}$$
  $$z=\frac{t}{4}$$

2.4 轴-角（Axis-Angle）

最后在补充一下，轴-角的概念，主要是自己用的不多，然后就不是很熟悉，这里简单记录一下吧！
轴-角（Axis-Angle）顾名思义就可以知道，就是某一坐标系通过围绕某一向量旋转一定角度，从而达到参考系的角度。设定等效旋转的方向向量为$\stackrel{\rightharpoonup }{K}={\left[\begin{array}{c}{k}_x,k_y,k_z\end{array}\right]}^T$,等效旋转角为$\theta$，则可以求出对应的四元素：
$$x=k_x\ast sin\frac{\theta }{2}$$
$$y=k_y\ast sin\frac{\theta }{2}$$
$$z=k_z\ast sin\frac{\theta }{2}$$
$$\omega =cos\frac{\theta }{2}$$
且有$x^2+y^2+z^2+{\omega }^2=1$

3 坐标系的平移

设定坐标点$A(x_1,y_1,z_1)$和坐标点$B(x_2,y_2,z_2)$，原先两个点都是以参考系为坐标系的，现在将坐标系原点移动到$B$点（不考虑坐标系的旋转），则点$A$在以点$B$为坐标系原点的新坐标系中的坐标为$(x_1-x_2,y_1-y_2,z_1-z_2)$。
坐标系的旋转加平移，这样就可以把空间中的任意坐标系重新投影到另一个坐标系中。

这里记录一下踩到的坑，以点$B$为圆心绕着某个轴逆时针旋转相应的角度，角度为正值，顺时针为负值。但是对于点$A$来说，角度的正负取值确实相反的。举个栗子说一下，就是以点$B$为原点的坐标系，绕$z$轴逆时针旋转了角度$\theta$，对于原点$B$来说，旋转的角度是正值，但是对于点$A$来看，坐标系就是按照负角度进行旋转的了，这里尤其需要理解，不然很容易出错，不敢确定的话，就自己找个简单的数据，计算出结果看看，这样最保险！！！

至于不同的表示方式之间的转换方式，自己动手推导一下就行，这里实在是码不动了，公式太难了。。。。。