2.1.1　旋轉和平移變換

1．旋轉矩陣

我們首先考慮兩坐標系間具有共同原點且僅有相對旋轉的情況。如圖2-2所示，假設空間中的任意激光點相對坐標系的位置可以用三維向量表示，則點P在坐標系1和坐標系2下的三維向量可分別表示為

（2-1）

（2-2）

圖2-2　兩坐標系間的旋轉變換示意圖

若進一步有兩坐標系的單位正交基分別為、、和、、，則式（2-1）和式（2-2）可進一步改寫為

（2-3）

（2-4）

假設坐標系2的單位正交基在坐標系1下的投影關系如下：

（2-5）

（2-6）

（2-7）

將式（2-5）～式（2-7）代入式（2-4）并整理，便可得到點基于坐標系1中三個單位正交基的另一種表示：

（2-8）

結合式（2-3）和式（2-8），可得點在兩坐標系間的表示具有下述關系：

（2-9）

（2-10）

（2-11）

進一步將式（2-9）～式（2-11）改寫為矩陣形式，則有

（2-12）

其中

考慮到所作用的坐標在矩陣的右側，所以這里矩陣的標號應該由下（2）向上（1）讀。可以看出，矩陣可以將點位于坐標系2中的坐標或投影轉換成坐標系1中的坐標或投影，該矩陣描述了兩個坐標系之間的旋轉關系（具體地說，就是坐標系2到坐標系1的旋轉），因此我們稱其為旋轉矩陣。因為矩陣的各分量為兩坐標系基矢量間的投影關系，基矢量的模長為1，其分量實際上為各基矢量夾角的余弦值，所以該矩陣又被稱作方向余弦矩陣。此外，我們可以進一步分析得到旋轉矩陣是行列式為1的正交陣，因此旋轉矩陣與其轉置矩陣的相乘結果為單位陣，即有

（2-13）

我們通常可以通過旋轉矩陣的轉置矩陣得到其逆矩陣，即

（2-14）

進一步地，以圖2-3為例，可以看出，若坐標系2相對坐標系1逆時針方向旋轉角度，則固定點P在坐標系2下的表示相當于P點在坐標系1中反方向旋轉角度后P?點的表示。這表明物體相對于坐標軸的旋轉和坐標軸相對于物體的等角度反向旋轉在描述上是等效的。

2．齊次變換矩陣

接下來，我們進一步考慮兩坐標系間同時具有旋轉和平移的情況。如圖2-4所示，坐標系3為激光點對應的激光雷達參考坐標系，坐標系1為車體的參考坐標系，二者之間具有三維空間中的相對旋轉和平移。假設激光點在激光雷達坐標系下的坐標已知，并且可用向量表示，則我們可以通過下述步驟求得激光點P在車體坐標系下的表示。

圖2-3　OP線段在兩坐標系下的方位表示

圖2-4　激光點P在兩坐標系間的歐氏變換示意圖

如果將坐標系3在其原點處旋轉至和坐標系1軸向一致的姿態，并構建中間坐標系2，則激光點在坐標系2中可表示為

（2-15）

由于坐標系2和坐標系1的姿態一致，因此進一步有

在得到激光點P在坐標系2中的表示后，進一步通過兩坐標系原點之間的位置矢量，可得到激光點P在坐標系1中的表示為

（2-16）

結合式（2-15）和式（2-16），可得到激光點P在車體坐標系中的表示為

（2-17）

我們可以將式（2-17）改寫成等價的齊次變換形式：

（2-18）

其中，,和為4維列向量，為齊次變換矩陣。