轉自：http://www.cnblogs.com/gaoxiang12/p/5113334.html
剛體運動
本篇討論一個很基礎的問題：如何描述機器人的位姿。這也是SLAM研究的一個很基本的問題。這里的位姿表示了位置和姿態。描述位置很簡單，如果機器人在平面內運動，那么用兩個坐標來描述它的位置：
如果在三維空間中運動，則它的位置就用三個空間坐標來表示：
對於姿態來說，在2D情況下還需要增加一個旋轉角θ；在3D情況下表達的方式就有多種，常見的如歐拉角、四元素、旋轉矩陣等。那么有了位置和姿態，就可以描述一個坐標系；進一步，還能描述坐標系之間的轉換關系。常見的問題如：機器人視野中某個點，對世界坐標系的（或地圖的）哪個點？這時，就需要先得到該點針對機器人坐標系坐標值，再根據機器人位姿轉換到世界坐標系中。
齊次坐標系
在位姿轉換中，通常采用射影空間的齊次坐標表示。齊次坐標是什么呢？記n維射影空間為
其中一個空間點的坐標為普通的3D坐標加一個齊次分量：
例如，在2維和3維射影空間中的點，分別表示為：
用四個數來表示點，說明點和坐標肯定不是一一對應的。沒錯，在齊次坐標中，某個點x的分量同乘一個非零常數k后，仍然表示的是同一個點。因此，一個點的具體坐標值不是唯一的。如
但是在w不等於0，可以對每一個坐標除以最后一項w，強制最后一項為1，從而得到一個點唯一的坐標表示：
那么為什么要使用齊次坐標來表示呢？原因如下：
1）齊次坐標下點和直線（高維空間里為超平面）能夠使用同樣的表達。
把點和超平面采用同樣的表示，這種做法一個非常直接的好處，是射影幾何里的“對偶原理”。該原理是說，任何有關“點”與“平面”的命題，都可以交換“點”與“平面”的概念，得到一個對偶的命題。對偶命題和原命題是一樣的。通過“對偶原理”，射影幾何的數學家就可以偷懶，只需要證一半定理，因為對偶命題和原命題有同樣的涵義。例如，我們證明了
中某條件下三點共線，那么替換概念后的三線共點則自然成立。
2）齊次坐標能囊括無窮遠點與無窮遠超平面
θθ3）齊次坐標可以方便地將平移與旋轉放在一個矩陣中
有關坐標系怎么用齊次坐標進行變換，后文會詳細解釋。現在我們能表達點了，還剩下一個姿態。由於2D與3D差別較大，我們分而述之。
2D姿態的描述
3D變換
3D的旋轉可以由旋轉矩陣、歐拉角、四元素等若干種方式描述，它 們也統稱為三維旋轉群SO(3);而3D的變換即旋轉加上位移，是SE(3)。為了和2D變換統一起見，我們首先介紹旋轉矩陣表示法。
旋轉矩陣描述
