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