什么是旋轉矩陣
旋轉矩陣描述了兩個坐標系(SLAM中兩個時刻機器人的姿態)之間的相互關系，是SLAM中描述姿態變換的數學工具。為了更深入地理解旋轉矩陣及坐標變化，我們簡單地以兩個原點重合的坐標系來闡釋旋轉矩陣的含義及其性質。
有兩個坐標系:$B$,$A$,其中$B$為剛體坐標系;兩坐標系原點重合,點$p$是剛體上的一點 坐標系B的坐標軸單位向量在坐標系A中的描述為: $$ \vec X_B=\begin{bmatrix}\alpha_x \\\beta_x \\\gamma_x \\\end{bmatrix},\vec Y_B=\begin{bmatrix}\alpha_y \\\beta_y \\\gamma_y \\\end{bmatrix},\vec Z_B=\begin{bmatrix}\alpha_z \\\beta_z \\\gamma_z \\\end{bmatrix} $$ 點$p$在坐標系$B$和坐標系$A$中的坐標分別為 $p_ B$ ,$p_A$,如下 $$ p_B=\begin{bmatrix}x_B \\y_B \\z_x \\\end{bmatrix},p_A=\begin{bmatrix}x_A \\y_A \\z_A \\\end{bmatrix} $$ 這里有一個重要的理解：**坐標是基於某一個坐標系下滿足一定條件的若干參考向量的度量**。點$p_B$在坐標系$B$中的坐標，其實是基於坐標系$B$中三個單位正交基$e_x=\begin{bmatrix}1&0&0\end{bmatrix}^T$,$e_y=\begin{bmatrix}0&1&0\end{bmatrix}^T$,$e_z=\begin{bmatrix}0&0&1\end{bmatrix}^T$,進行的度量，即: $$ p_B=x_B\cdot\begin{bmatrix}1 \\0 \\0 \\\end{bmatrix}+y_B\cdot\begin{bmatrix}0 \\1 \\0 \\\end{bmatrix}+z_B\cdot\begin{bmatrix}0 \\0 \\1 \\\end{bmatrix} $$ 點$q$在坐標系$A$中的坐標$p_A$可以以坐標系$A$中三個單位正交向量$\vec X_B=\begin{bmatrix}\alpha_x&\alpha_y&\alpha_y\end{bmatrix}^T$，$\vec Y_B=\begin{bmatrix}\beta_y&\beta_y&\beta_y\end{bmatrix}^T$，$\vec Z_B=\begin{bmatrix}\gamma_z&\gamma_z&\gamma_z\end{bmatrix}^T$來進行度量，坐標分別為$x_B$，$y_B$，$z_B$，則點$p$在坐標系$A$中表達如下： $$ \begin{align} p_A&=x_B\cdot \vec X_B+y_B\cdot \vec Y_B+z_B\cdot \vec Z_B \\ &=x_B\cdot \begin{bmatrix}\alpha_x \\\beta_x \\\gamma_x \\\end{bmatrix}+ y_B\cdot \begin{bmatrix}\alpha_y \\\beta_y \\\gamma_y \\\end{bmatrix}+z_B\cdot \begin{bmatrix}\alpha_z \\\beta_z \\\gamma_z \\\end{bmatrix} \\ &=\begin{bmatrix}\alpha_x & \alpha_y &\alpha_z \\\beta_x & \beta_y & \beta_z \\\gamma_x & \gamma_y & \gamma_z \\\end{bmatrix}\cdot \begin{bmatrix}x_B \\y_B \\z_x \\\end{bmatrix} \\ &=R_B^A \cdot p_B \end{align} $$ 綜上：點$p$在坐標系$A$中坐標和在坐標系$B$中坐標有如下關系： $$ p_A=R_B^A\cdot p_B $$ $R_B^A$為旋轉矩陣：是坐標系$A$到坐標系$B$的旋轉矩陣，也是坐標系$B$在坐標系$A$中的描述（即剛體在坐標系$A$中的姿態） 上述結論成立的條件：坐標系$A$與坐標系$B$的原點相同 關於旋轉矩陣作用的直觀理解： $$ p_A（在A中坐標）=R_B^A（B在A中的描述）\cdot p_B(在B中坐標) $$ 這是對旋轉矩陣作用比較本質的理解，在后續**IMU標定**相關的博客中會用到這一理解 ### 旋轉矩陣的性質 - **旋轉矩陣的合成法則**：兩個坐標系之間的旋轉矩陣可以利用它們與中間坐標系間的旋轉矩陣相乘得到，該結論可以拓展到多個矩陣 $$ R_C^A=R_B^A\cdot R_C^B $$ - **性質１** 設$R$是旋轉矩陣,$r_1$ ,$r_2$ ,$r_3$ 是其列向量，則它們是相互正交的，即: $$ r_i^Tr_j=\begin{cases}0 &i\neq j \\ 1 &i=j \end{cases} $$ - **性質２** 設 R是旋轉矩陣,則有: $$ RR^T=R^TR=I $$ - **性質３** $$ detR=1 $$ 滿足上述性質的3×3矩陣的集合定義為**特殊正交群$SO(3)$:**（**群** 的概念后面會介紹到） $$ SO(3)=\{R\in \mathcal R^{3×3}|RR^T=I，det R = 1\} $$
變換矩陣與齊次坐標
如上圖，有兩個坐標系:$B$，$A$，其中$B$為剛體坐標系；$B$坐標系原點在$A$坐標系中的位置是 $t_ B^A$ ,點$p$是剛體上的一點。 **一般運動剛體上點的位置:**當坐標系$B$與坐標系$A$不重合,剛體上點p在坐標系A中的坐標該如何描述? $$ p_A=R_B^A\cdot p_B + t_B^A $$ $R_B^A$是坐標系$B$相對於坐標系$A$的旋轉矩陣，$t_B^A$是坐標系$B$相對於坐標系$A$的平移量，也即坐標系$B$原點在坐標系$A$中的坐標。 引入齊次坐標,將上式變為純乘法操作: $$ \widetilde p_A =\begin{bmatrix}p_A \\ 1\\\end{bmatrix},\widetilde p_B =\begin{bmatrix}p_B \\ 1\\\end{bmatrix},T_B^A =\begin{bmatrix}R_B^A& t_B^A \\0^T & 1\\\end{bmatrix} $$ 得到： $$ \widetilde p_A =T_B^A\cdot \widetilde p_B $$ 其中，$\widetilde p_A$和$\widetilde p_B$為**齊次坐標**，$T_B^A$為齊次坐標$\widetilde p_B$到$\widetilde p_A$的**變換矩陣** 變換矩陣構成**特殊歐氏群**$SE(3)$ $$ SE(3)=\begin{Bmatrix}T=\begin{bmatrix}R & t \\0^T & 1\\\end{bmatrix}\in \mathcal R^{4×4}|R \in SO(3)，t \in \mathcal R^{3×１}\\\end{Bmatrix} $$
變換矩陣的合成法則：兩個坐標系之間的變換矩陣可以利用它們與中間坐標系間的變換矩陣相乘得到，該結論可以拓展到多個矩陣
\[T_A^C =T_B^C \cdot T_A^B \]
變換矩陣的逆
\[T^{-1} =\begin{bmatrix}R^T& -R^Tt \\0^T & 1\\\end{bmatrix} \]
注意:坐標系\(A\)原點在坐標系\(B\)中的位置
\[t_A^B=-R_A^B\cdot t_B^A=-\begin{pmatrix}R_B^A\end{pmatrix}^T\cdot t_B^A \]
上式在工程當中有如下常見應用及易錯點：
\[t_B^A + t_C^B = t_C^A \\ t_B^A-R_A^C \cdot t_C^A = t_B^A + t_A^C=t_B^C \]
尤其注意：
\[t_B^A-t_C^A \neq t_B^C \]
