【自动驾驶】19.自动驾驶各个坐标系

WGS84 : 世界坐标系，即经纬高坐标系【经度(longitude)，纬度(latitude)和高度(altitude)即LLA坐标系】

UTM: 世界坐标系
ENU: 世界坐标系，东北天
ENU 和 UTM 的区别：

• ENU局部坐标系采用三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。
• UTM: 在众多二维投影坐标系中，统一横轴墨卡托（The Universal Transverse Mercator，UTM）坐标系是一种应用较为广泛的一种。 UTM坐标系使用基于网格的方法表示坐标，它将地球分为60个经度区，每个区包含6度的经度范围，每个区内的坐标均基于横轴墨卡托投影。

FLU: 车身坐标系，前左上
SL: SL坐标系：某点距离车子的横向和纵向距离，纵向距离为s,横向距离为l

以下是坐标系俯视图：

车辆坐标系的原点根据不同车可能会有所不一样。一般情况下，车辆坐标系原点的高度和IMU的高度保持一致，xy相交于两个前轮中心。
现在少部分车就把IMU设为车辆坐标系原点，只是imu坐标系和车辆坐标系的xy方向不一样(见上图)，所以需要绕Z轴旋转90度，转化为车身坐标系。
imu2car文件的参数：

header:
  seq: 0
  frame_id: car
  stamp:
    secs: 0
    nsecs: 0
child_frame_id: imu
transform:
  rotation:
    x: 0
    y: 0
    z: -0.707
    w: 0.707
  translation:
    x: 0
    y: 0
    z: 0

上面的参数translation是平移向量：

• $x、y、z$都为0，就是IMU坐标系和车辆坐标系原点原点重合。
• 如果$x、y、z$不为0，就说明中心不重合；
• 如果$z$为0，就说明两个中心 的高度一致。

上面的参数rotation是旋转四元数：
$x=0、y=0、z=-0.707$、$w=0.707$，

注：$sin(\frac{\pi }{4})=0.707107$

即
$x=x^{\prime }\ast sin\frac{\theta }{2}=0$、

$y=y^{\prime }\ast sin\frac{\theta }{2}=0$、

$z=z^{\prime }\ast sin\frac{\theta }{2}=-0.707$、

$w=cos\frac{\theta }{2}=0.707$，

其中$x^{\prime },y^{\prime },z^{\prime }$是旋转轴的三个方向的大小。由w可以求解出$\theta =\frac{\pi }{2}$或者$\theta =-\frac{\pi }{2}$。

再根据前面的联立方程组，可以得出旋转向量(0,0,1)对应的$\theta =-\frac{\pi }{2}$，或者旋转向量(0,0,-1)对应的$\theta =\frac{\pi }{2}$。
但是因为Z轴正方向是(0,0,1),所以最终求得的结果是第一个结果，即旋转向量(0,0,1)对应的$\theta =-\frac{\pi }{2}$，也就是四元数表示的是绕Z轴旋转$\theta =-\frac{\pi }{2}$。

由于四元数表示的是坐标点的变换矩阵，不是坐标轴的变换矩阵，所以得到的角度也是坐标点的变换。我们知道，坐标轴旋转的角度是坐标点变换的角度的相反数，请参考【自动驾驶】29.坐标变换与坐标轴旋转，所以imu坐标系的点经过旋转角度$-90°$的四元数表示的变换之后，得到车辆坐标系的点。

但是imu坐标系绕$Z$轴旋转 $+90°$，得到车辆坐标系。

想想上面为什么是$\frac{\theta }{2}$呢?
想找到答案，可以参考这篇博客：【Unity技巧】四元数（Quaternion）和旋转。

---

x方向被放在前面表述，例如：

• 东北天坐标系，东就是表示x方向；
• 北西天坐标系，北就是表示x方向；
• 右前天坐标系，右就是表示x方向；
• 前左上坐标系，前就是表示x方向；

东北天坐标系也叫右前天坐标系，前左上坐标系也叫北西天坐标系，只是叫法不同，两种叫法根据上北、下南、左西、右东进行对应。

并且，同一种坐标系叫法，可以用在不同地方，他们的坐标系原点也可能不一样，因为不同传感器或车身的坐标系原点不同。
如：车身坐标系和激光雷达坐标系都是前左天坐标系，但是他们的坐标系原点却不一样。