【自动驾驶】29.坐标变换与坐标轴旋转

从一个坐标系的点变换到另一个坐标系的点，旋转矩阵的角度我们不能直接知道，但是可以通过两个坐标系之间的旋转来间接得到。
如：
世界坐标系有一个点P，我们要描述它，就得给他一个坐标系原点，如果他放在车身坐标系，坐标值为P1(2,-1,z)，怎么知道该点P1在另一个imu坐标系中的坐标值呢？

这里说明一下，imu坐标系和车身坐标系原点重合，x轴和y轴方向不一样，z轴重合(方向都向上)。

`先说结论：`

由于`车身坐标系`和`imu坐标系`只是旋转90°的关系，所以可以很容易得到`车身坐标系`的点坐标在`imu坐标系`的点坐标为：

Ximu= -1 * Ycar
Yimu=  Xcar

以下是俯视图，z轴向上，在图中没画出来，用圆圈内一个实心点表示向上。

从图中可以看出，这个点P1在对应的imu坐标系中的坐标为P2(1,2,z)。

当然，这是人眼看出来的，怎么用算法求解出来呢？

坐标变换需要用到变换矩阵，这个变换矩阵在这里就是旋转矩阵，需要知道旋转矩阵的角度，就得借助坐标系的旋转了。

记住一句话：`坐标系旋转是坐标变换的逆过程。`

先解释这句话：坐标系旋转是坐标变换的逆过程。 即，如果从A坐标系绕Z轴逆时针旋转α°\alpha°α°，可以得到B坐标系；那么在A坐标系的某点PAP_APA，该点在B坐标系的点坐标为：在A坐标系下将PAP_APA点通过旋转矩阵(角度是绕Z轴逆时针旋转−α°-\alpha°−α°)计算得到，即 PB=T(−α)∗PA\color{red}P_B=T(-\alpha)*P_APB=T(−α)∗PA ，其中T(−α)T(-\alpha)T(−α)表示旋转矩阵逆时针旋转−α°-\alpha°−α°。

`用实例来证明：`

将车身坐标系绕Z轴逆时针旋转-90°(即顺时针旋转90°，会描述成逆时针旋转-90°。)，就得到了imu坐标系。那么，车身坐标系的点P1(2,-1,z)，在imu坐标系中坐标值是多少呢？

将车身坐标系绕Z轴右手法则正方向旋转-90°，就得到了imu坐标系。那么，车身坐标系的点P1(2,-1,z)，在imu坐标系中坐标值是多少呢？

车身坐标系的某点P1(2,-1,z)，在对应的imu坐标系中的坐标为P2(1,2,z)，就有T*P1=P2，可以解得旋转矩阵T为(1)式：

我们知道，绕Z轴旋转的旋转矩阵是:

由于(1) = (2)，解出旋转角度 α=90°\color{red}\alpha=90°α=90°。坐标变换就是将该点P1在自身车身坐标系中右手定则正方向旋转90°，得到了在imu坐标系中的坐标值。

上面没考虑z轴方向的值，因为z轴重合，z值一样。下面是考虑z轴方向的推导过程：

注意，这里由车身坐标系的P1到imu坐标系中P2点旋转矩阵的旋转角度是右手法则绕Z轴正方向旋转90°，但是从车身坐标系旋转到imu坐标系是绕Z轴右手法则正方向旋转-90°。

已知车身坐标系的点P1，我们的目的是求P1在IMU坐标系的坐标P2。
我们只能通过旋转矩阵来计算出来P2，并不是描述坐标轴旋转 α°\color{red}\alpha°α°，因为你旋转 α°\color{red}\alpha°α°还是得通过旋转矩阵才能得到具体结果坐标。那么，前面计算得到的车辆坐标系下的旋转矩阵是右手法则绕Z轴正方向旋转90°，而从车辆坐标系到imu坐标系需要绕车辆坐标系的Z轴右手法则正方向旋转-90°，正好一正一负，即坐标系旋转是坐标变换的逆过程。

对于坐标轴旋转需要旋转两次的情况，可以自己做一下测试，应该还是一样顺序，例如坐标轴先绕X轴旋转a°，再绕Z轴旋转b°，那么坐标变换就是旋转矩阵先绕X轴旋转-a°，再绕Z轴旋转-b°，而不是旋转矩阵先绕Z轴旋转-b°，再绕X轴旋转-a°。我这里没验证，按照我代码里面的应该是这样。

`关于右手坐标系与右手法则：`

如果不明白这两个概念，可以参考右手坐标系与右手定则。

右手坐标系是用来指定X、Y、Z轴的正方向，并不能随意指定X、Y、Z轴的正方向；
右手定则是用来规定绕某轴旋转的正方向。

`右手法则的四指旋转方向为正`。

三维空间下，逆时针旋转不一定是正方向，所以只能描述为右手定则的绕某轴正方向旋转：

比如你右手握成拳头，大拇指指向下，那么你四个手指的指向就是顺时针，即正方向，那么逆时针就不是正方向了，你就不能套用逆时针是正方向的理论。
所谓的逆时针方向为正，是由于人们平时的习惯，约定俗成的。如，打牌时是逆时针出牌、坐标系象限是逆时针命名一二三四象限，角度也是。
我们平时在三维空间的旋转，也习惯称呼顺时针旋转与逆时针旋转，是因为，右手定则的绕某轴旋转的旋转方向不好描述(例如，你说，“右手握住Z轴，大拇指指向Z轴正方向，绕着四指指向的方向旋转30度”，这样描述太麻烦，不如一句，“绕Z轴逆(或顺)时针旋转30度”，描述的简洁清晰)，所以使用顺时针逆时针旋转来描述更容易让人明白是怎样旋转的，和顺时针逆时针旋转的正反无关。

将两条结论列出来：

将车身坐标系绕Z轴右手法则正方向旋转-90°，就得到了imu坐标系；
车身坐标系点P1经过旋转矩阵T得到imu坐标系点P2,这个变换矩阵T是将车身坐标系点P1绕车身坐标系Z轴右手法则正方向旋转90°。

看明白没，一个是坐标系的坐标轴绕Z轴右手法则正方向旋转-90°得到另一个坐标系，一个是坐标点绕Z轴右手法则正方向旋转90°得到在另一个坐标系下对应的坐标值。

变换矩阵是将某点在自己的坐标系内通过变换矩阵进行变换，但是得到的旋转后的坐标值是其他坐标系的坐标值。

需要注意，不要混淆的是，这里的坐标系原点和该坐标系某点P是相对不动的，如果是一个相对坐标系运动的物体，那就是自己在自己坐标系的坐标已经变了,此时如果需要预测运动后的坐标值，需要用到车的速度、角度等odometry的信息进行同步。和本文讨论的坐标变换不一样。

平移向量

对于坐标系平移，也是逆过程。
例如，两个坐标系A和坐标系B的各个轴方向一样，只是原点不在一起。
坐标系B的原点(0,0,0)在坐标系A中的坐标为(-4,-2,0),那么，坐标系A中的某点P(Px,Py,Pz)P(P_x,P_y,P_z)P(Px,Py,Pz)在坐标系B中的坐标为多少？
一画图就知道结果了，A坐标系沿X正向平移了-4，沿Y正向平移了-2，那么A中的某点P(Px,Py,Pz)P(P_x,P_y,P_z)P(Px,Py,Pz)在坐标系B中的坐标为(Px+4,Py+2,Pz)(P_x+4,P_y+2,P_z)(Px+4,Py+2,Pz) ，即相当于A坐标系移动到B坐标系的逆过程。