自动驾驶坐标系汇总

系列文章目录

第一章 自动驾驶坐标系汇总

---

---

前言

传统地信领域和自动驾驶领域对坐标系有着各自的理解和定义，本文尝试梳理二者的异同点。

---

一、坐标系定义

1. 地理坐标系

Geographic Coordinate System，一般叫做大地坐标系、LLA坐标系、LLH坐标系，是大地测量以参考椭球面为基准面建立起来的坐标系，地面位置使用经度（longitude）、纬度（latitude）和大地高（altitude/height）表示，包括：
参心坐标系：BLH坐标系，如Beijing 54、Xian 80
地心坐标系：ECEF坐标系，如WGS 84、CGCS 2000

2. 地心地固坐标系 — e系

Earth-Centerd Earth Fixed Coordinate System（ECEF），一种以地心为原点的地固坐标系，如WGS 84。在这个坐标系中，车辆、障碍物和其他静态元素的位置都被定义，不随车辆移动而改变。

3. 投影坐标系

是一种平面坐标系，单位通常为米，投影坐标由地理坐标系及对应的投影方法确定，如WGS 84 + 高斯克吕格投影。

4. 大地站心（地平）坐标系 — L系

Local Cartesian Coordinate System，一般指东北天坐标系（ENU），局部三维直角坐标系，以测站为坐标原点，X轴指向东（East），Y轴指向北（North），Z轴指向天顶（Up）。

5. 导航坐标系 — n系

是惯性导航算法的基本参考坐标系，运动物体在导航坐标系内进行位置、速度、姿态的确定，可选取e系或L系作为导航系。

6. 车身坐标系

以车辆为中心的局部坐标系。原点通常设在车辆的后轴中心，X轴指向车辆的前进方向，Y轴指向车辆的左侧（右手系），Z轴垂直于地面指向天空。车辆坐标系随着车辆的移动和转向而变化。

二、坐标系变换

车身坐标系到地理坐标系的转换包括两步，第一步是平面直角坐标系下的欧式变换，即将感知结果的车身坐标通过先旋转再平移的方式变换为投影坐标，第二步是将投影坐标变换到椭球体下的地理坐标。

1. 平面直角坐标系下的欧式变换

假设时间$\mathbf{t}$下，点$\mathbf{p}$的车身坐标系坐标为：
$$\mathbf{p}=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\in {\mathbb{R}}^3$$
欧式变换后的投影坐标为：
$${\mathbf{p}}_p=\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]\in {\mathbb{R}}^3$$
地理坐标系坐标为：
$${\mathbf{p}}_e=\left[\begin{array}{c}lon\\ lat\\ height\end{array}\right]\in {\mathbb{R}}^3$$
当前时刻车身轨迹点的投影坐标为：
$${\mathbf{v}}_p=\left[\begin{array}{c}{x}_{vp}\\ y_{vp}\\ z_{vp}\end{array}\right]\in {\mathbb{R}}^3$$
在表达三维空间旋转时，为了避免奇异性并减少冗余，通常选择使用四元数（Quaternion），假设时间$\mathbf{t}$下的四元数$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$，$q_0,q_1,q_2,q_3\in \mathbb{R}$，则对应的旋转矩阵$\mathbf{R}$如下：
$$\mathbf{R}=\left[\begin{array}{c}1-2q_2^2-2q_3^{2}2q_1{q}_2-2q_0{q}_{3}2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_{3}1-2q_1^2-2q_3^{2}2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_{2}2q_2{q}_3+2q_0{q}_{1}1-2q_1^2-2q_2^2\end{array}\right]$$
在时间$\mathbf{t}$下，车身坐标系的原点为当前时刻的车身轨迹点，因此此时的平移向量$\mathbf{T}$可以表达为：
$$\mathbf{T}={\mathbf{v}}_p=\left[\begin{array}{c}{x}_{vp}\\ y_{vp}\\ z_{vp}\end{array}\right]$$
对应的车身坐标$\mathbf{p}$到投影坐标${\mathbf{p}}_p$的公式如下：
$${\mathbf{p}}_p=\mathbf{R}\mathbf{p}+\mathbf{T}$$

2. 投影坐标到地理坐标的变换

投影坐标由地理坐标系及对应的投影方式决定，用${\mathbf{P}}_{gk}$表示投影，则地理坐标可以表达为：
$${\mathbf{p}}_e={\mathbf{P}}_{gk}^{-1}{\mathbf{p}}_p$$