# 附录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}$$