做MPC控制的时候，需要把 map 坐标系下的相关坐标点转换到 car 坐标系中，由于是只考虑 xy 平面，所以计算比较简单。做完之后，考虑到三维空间的坐标系转换还是不太懂，主要是ROS系统中的tf变换，于是摸了两天鱼。今天详细的记录下来，旋转矩阵、四元素、欧拉角以及各个之间的转换关系。 阅读了好几篇大神的文章，具体也找不到连接了，在此顶礼膜拜，如有相似的地方，那就相似吧，毕竟原理都一样。

本文采用的为右手坐标系，以车身为参考的话，就是车头朝向为x轴正向，垂直于x轴且指向车身左侧为y轴正向，z轴垂直于xy平面，且向上为正方向，原点为车辆质心，此时车辆绕z轴旋转角为yaw角 ，y轴旋转为 pitch角 , x轴旋转为 roll角。

这里需要注意的点，如果是右手坐标系的话，逆时针旋转了角度 θ \theta θ的话，对应的旋转角为 − θ -\theta −θ，顺时针的话旋转角为 θ \theta θ；左手坐标系与之相反。

对于两个三维点 p 1 ( x 1 , y 1 , z 1 ) p1(x1,y1,z1) p1(x1,y1,z1)， p 2 ( x 2 , y 2 , z 2 ) p2(x2,y2,z2) p2(x2,y2,z2)，由点 p 1 p1 p1 经过旋转矩阵 R R R 旋转到 p 2 p2 p2，则有： R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} R= ​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​ ​ [ x 2 y 2 z 2 ] = R ∗ [ x 1 y 1 z 1 ] \begin{bmatrix} x_2 \\ y_2 \\ z_2 \end{bmatrix} = R*\begin{bmatrix} x_1 \\ y_1 \\ z_1 \end{bmatrix} ​x2​y2​z2​​ ​=R∗ ​x1​y1​z1​​ ​ 注意：旋转矩阵为正交矩阵 R ∗ R T = E R*R^T=E R∗RT=E。 绕x轴旋转角度 θ \theta θ R x ( θ ) = [ 1 0 0 0 c o s ( θ ) − s i n ( θ ) 0 s i n ( θ ) c o s ( θ ) ] R_x(\theta)=\begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{bmatrix} Rx​(θ)= ​100​0cos(θ)sin(θ)​0−sin(θ)cos(θ)​ ​

绕y轴旋转角度 θ \theta θ R y ( θ ) = [ c o s ( θ ) 0 s i n ( θ ) 0 1 0 − s i n ( θ ) 0 c o s ( θ ) ] R_y(\theta)=\begin{bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \end{bmatrix} Ry​(θ)= ​cos(θ)0−sin(θ)​010​sin(θ)0cos(θ)​ ​

绕z轴旋转角度 θ \theta θ R z ( θ ) = [ c o s ( θ ) − s i n ( θ ) 0 s i n ( θ ) c o s ( θ ) 0 0 0 1 ] R_z(\theta)=\begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz​(θ)= ​cos(θ)sin(θ)0​−sin(θ)cos(θ)0​001​ ​

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

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

设三个轴 x , y , z x,y,z x,y,z的欧拉角分别为 θ x , θ y , θ z \theta_x,\theta_y,\theta_z θx​,θy​,θz​，对应的正弦值、余弦值简写为 s x , c x , s y , c y , s z , c z s_x,c_x,s_y,c_y,s_z,c_z sx​,cx​,sy​,cy​,sz​,cz​，且旋转的顺序为 y a w , p i t c h . r o l l yaw,pitch.roll yaw,pitch.roll,因此旋转矩阵为 R ( θ z , θ y , θ x ) = R z ( θ z ) ∗ R y ( θ y ) ∗ R x ( θ x ) = [ 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 ] R(\theta_z,\theta_y,\theta_x)=R_z(\theta_z)*R_y(\theta_y)*R_x(\theta_x)=\begin{bmatrix} c_yc_z & c_zs_xs_y-c_xs_z & s_xs_z+c_xc_zs_y \\ c_ys_z & c_xc_z+s_xs_ys_z & c_xs_ys_z-c_zs_x \\ -s_y & c_ys_x & c_xc_y \end{bmatrix} R(θz​,θy​,θx​)=Rz​(θz​)∗Ry​(θy​)∗Rx​(θx​)= ​cy​cz​cy​sz​−sy​​cz​sx​sy​−cx​sz​cx​cz​+sx​sy​sz​cy​sx​​sx​sz​+cx​cz​sy​cx​sy​sz​−cz​sx​cx​cy​​ ​

R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] = [ 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 ] R=\begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}=\begin{bmatrix} c_yc_z & c_zs_xs_y-c_xs_z & s_xs_z+c_xc_zs_y \\ c_ys_z & c_xc_z+s_xs_ys_z & c_xs_ys_z-c_zs_x \\ -s_y & c_ys_x & c_xc_y \end{bmatrix} R= ​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​ ​= ​cy​cz​cy​sz​−sy​​cz​sx​sy​−cx​sz​cx​cz​+sx​sy​sz​cy​sx​​sx​sz​+cx​cz​sy​cx​sy​sz​−cz​sx​cx​cy​​ ​ 求解方程，可以有 θ x = a t a n r 32 r 33 \theta_x=atan \frac{r_{32}}{r_{33}} θx​=atanr33​r32​​ θ y = a t a n − r 31 r 32 2 + r 33 2 \theta_y=atan \frac{-r_{31}}{\sqrt{r_{32}^2+r_{33}^2}} θy​=atanr322​+r332​ ​−r31​​ θ z = a t a n r 21 r 11 \theta_z=atan \frac{r_{21}}{r_{11}} θz​=atanr11​r21​​ 从上面的公式，也可以看出万向锁的问题，比如 r 11 ， r 33 r_{11}，r_{33} r11​，r33​等于0的时候，公式就无法使用了，所以在由旋转矩阵向欧拉角求解的时候，要注意看对应的矩阵元素是不是为0。

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

R ( q ) = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 z w 2 x z + 2 y w 2 x y + 2 z w 1 − 2 x 2 − 2 z 2 2 y z − 2 x w 2 x z − 2 y w 2 y z + 2 x w 1 − 2 x 2 − 2 y 2 ] R(q)=\begin{bmatrix} 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{bmatrix} R(q)= ​1−2y2−2z22xy+2zw2xz−2yw​2xy−2zw1−2x2−2z22yz+2xw​2xz+2yw2yz−2xw1−2x2−2y2​ ​

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

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

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

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