旋转矩阵乘法，自动驾驶中的点及坐标系变换推导

目录

1. 矩阵乘法的内项相消

2. 左右乘，内外旋与动静坐标系

3. 点变换

3.1 点旋转后的点坐标表示

3.2 坐标系旋转后的点坐标表示

4. 坐标变换的实质

1. 矩阵乘法的内项相消

关于旋转变换，离不开矩阵的乘法，而矩阵乘法的物理意义和本身数学上矩阵乘法的内向相消决定了乘法的顺序，确定内项相消的规则，是对后面矩阵乘法的顺序理解的重要基础。

首先确定旋转矩阵的写法, 如果理解成旋转，则表示A坐标系到B坐标系的旋转变换；如果理解成姿态，则表示B坐标系在A坐标系中的姿态（如果有位置坐标，可以理解成位姿）。注意A，B的顺序非常的重要，在自动驾驶中，可以分别表示世界坐标系，IMU坐标系等。

按照如上旋转矩阵的定义，那么内项相消的公式就可以写成

2. 左右乘，内外旋与动静坐标系

关于左右乘，内外旋有如下结论(记住即可)：

相对固定（静）坐标系的旋转 = 外旋 = 左乘， 如绕Z-Y-X固定轴旋转，旋转矩阵的乘法应写为：

相对自身（动）坐标系的旋转 = 内旋 = 右乘，如绕Z-Y-X自身轴旋转，旋转矩阵的乘法应写为：

3. 点变换

3.1 点旋转后的点坐标表示

旋转矩阵的乘法往往会和某个点进行，这是最直观的一种矩阵乘法。通常我们会理解成这个点代表的向量，经过了旋转变换的，得到一个新的向量（点）。如下图所示：

CA（1，1）向量经过逆时针旋转90°，得到CB向量（-1，1），向量的旋转矩阵不存在坐标系的变换，不用在意上面的定义，直接用点左乘具体的旋转矩阵即可。逆时针旋转90°的旋转矩阵为：

CB = R * CA

3.2 坐标系旋转后的点坐标表示

这个比较重要，也是理解本篇的重点。坐标系发生变化后，点即使没有发生位置的改变，其坐标也会发生变化。这种在自动驾驶中很常见，一个点在传感器中有一个坐标，想求它在世界坐标系中的坐标。就是此类应用。

如下图所示：

在CBD坐标系中（以坐标系G代表），其中BC为x轴，BD为y轴，A点的坐标为（2，1）

我们将原坐标系逆时针旋转90°，得到DBE坐标，其中BD为x轴，BE为y轴，在DBE坐标系中（以坐标系G'），A点在G’坐标系中的坐标表示为A'（1，-2）。

这时候逆时针旋转90°表示从坐标系G到坐标系G’的变换关系，用1中的定义表示为。通过计算发现，即如果是坐标发生了变化，不能直接左乘坐标系之间的旋转关系，要左乘旋转关系的逆。

4. 坐标变换的实质

坐标变换的实质就是投影。

单位向量在坐标系中的投影就是该向量与坐标系各轴夹角的余弦值，这个结论我们并不陌生。坐标系之间的旋转矩阵，其实就是将G'的三个轴单位化之后投影到G的三个轴上，即

每一行表示目标坐标系在三个轴在原坐标系某个轴的投影。