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维列向量，为齐次变换矩阵。