附录3 三维空间刚体运动

四元数

$\quad$旋转矩阵用9个量来描述3自由度的旋转，具有冗余性；欧拉角虽然用3个量来描述3自由度的旋转，但是具有万向锁的问题，因此我们选择用四元数，（ROS当中描述转向的都是采用的四元数）。一个四元数拥有一个实部和三个虚部组成。

$\qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad q=w+xi=yj+zk$

$\quad$三个虚部满足以下关系

$\qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad \begin{cases} i^2=j^2=k^2=-1 \\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,jk=-j\\ \end{cases}$

$\quad$写成矩阵的样子就是$q=\begin{bmatrix} w,x,y,z\end{bmatrix}^T$,其中$\begin{vmatrix} q\end{vmatrix}^2=w^2=x^2+y^2+z^2=1$，从欧拉角到四元数的公式：

$\qquad \qquad q=\begin{bmatrix} w\\x\\y\\z \end{bmatrix}=\begin{bmatrix} cos(roll/2)cos(pitch/2)cos(yaw/2)+sin(roll/2)sin(pitch/2)sin(yaw/2)\\sin(roll/2)cos(pitch/2)cos(yaw/2)-cos(roll/2)sin(pitch/2)sin(yaw/2)\\cos(roll/2)sin(pitch/2)cos(yaw/2)+sin(roll/2)cos(pitch/2)sin(yaw/2)\\cos(roll/2)cos(pitch/2)sin(yaw/2)-sin(roll/2)sin(pitch/2)cos(yaw/2) \end{bmatrix}$

$\quad$从四元数转化到欧拉角公式

$\qquad \qquad \qquad \qquad \qquad \qquad \begin{bmatrix} roll\\pitch\\yaw \end{bmatrix}=\begin{bmatrix} atan2(2(wx+yz),1-2(x^2+y^2))\\ arcsin(2(wy-zx))\\atan2(2(wz+xy),1-2(y^2+z^2)) \end{bmatrix}$