# 附錄1 三維空間剛體運動 ## 旋轉矩陣 $$\quad$$在機器人運動的過程當中，我們通常會設定一個慣性坐標系（或者叫世界坐標系），姑且認為這個坐標系是固定不動的。例如：$$X_W,Y_W,Z_W$$是固定不動的世界坐標系，$$X_C,Y_C,Z_C$$是機器人坐標系。存在一個向量$$P$$，在世界坐標系下的坐標是$$P_W$$，在移動機器人坐標系下的坐標是$$P_C$$，通常情況下，我們通過傳感器已知移動機器人坐標系統下的坐標$$P_C$$，來求$$P$$在世界坐標系下的坐標$$P_W$$  $$\quad$$為了求$$P_W$$，我們必須知道機器人坐標系$$X_C,Y_C,Z_C$$相對與世界坐標$$X_W,Y_W,Z_W$$做了哪些變換。我們定義世界坐標系經過變換矩陣$$T$$之后得到機器人坐標系（這可以通過計算里程和IMU的數據進行測量出來）（這也就說明了為什么在機器人剛剛啟動的時候odom和base\_link坐標系必須是重合的，不然沒有辦法計算旋轉矩陣），另外一般情況下，移動機器人運動是一個剛體運動，也就是說機器人的形狀和大小不會因為坐標系不同而改變，這種變換叫做歐式變換。一個歐式變換可以由旋轉和平移兩個部分組成。首先我們考慮旋轉問題，假設在世界坐標系下的單位正交基$$(e_x,e_y,e_z)$$，在移動機器人坐標系下的單位正交基$$(e^"_x,e^"_y,e^"_z)$$,那么，根據向量$$P$$的模可知： $$ \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad [e_x,e_y,e_z] \cdot P^T_W=[e^"_x,e^"_y,e^"_z] \cdot P^T_C $$ $$\quad$$因此，$$P^T_C=[e_x,e_y,e_z]^T \cdot [e^"_x,e^"_y,e^"_z] \cdot P^T_W$$，我們將$$[e_x,e_y,e_z]^T \cdot [e^"_x,e^"_y,e^"_z]$$記做旋轉矩陣**R**，因此上面的表達式可以簡化為$$P^T_C=R\cdot P^T_W$$。接下來是平移部分，假設平移部分是$$P^T_{C^"}$$經過平移向量t后得到$$P^T_{C}$$，那么可以得到$$P^T_C=P^T_{C^"}+t=R\cdot P^T_w+t$$。所以通過旋轉矩陣**R**和平移 向量**t**，我們可以描述從世界坐標系到移動機器人坐標系的坐標變換。但是這種表達方式存在一個問題，對于連續的位置變換，例如機器人坐標系是隨著時間在不斷變換的，上面這種表達方式并不是一個線性的表達方式，假設我們經歷了兩次變換$$R_1,t_1$$和$$R_2,t_2$$且滿足：從a到b的變換$$b=R_1a+t_1$$，從b到c的變換$$c=R_2b+t_2$$.那么從a到c的變換是$$c=R_2(R_1a+t_1)+t_2$$.并不是我們希望的的形式$$c=Ra+t$$(然后我們采用齊次坐標的方式進行表達，詳細的部分參考李群李代數).