自动驾驶中使用到的坐标转换

猫猫猫猫猫大人

已于 2022-02-16 14:30:38 修改

阅读量9.5k

点赞数 4

文章标签： gnss 自动驾驶

于 2022-02-07 15:39:21 首次发布

本文链接：https://blog.csdn.net/mao834099514/article/details/122809704

版权

一、简介

1.1 地心地固直角坐标系（ECEF）

也叫地心地固直角坐标系。其原点为地球的质心，x轴延伸通过本初子午线（0度经度）和赤道（0deglatitude）的交点。 z轴延伸通过的北极（即，与地球旋转轴重合）。 y轴完成右手坐标系，穿过赤道和90度经度。

1.2 WGS-84坐标（LLA）

也就是也叫经纬高坐标系(经度(longitude)，纬度(latitude)和高度(altitude)LLA坐标系)。，全球地理坐标系、大地坐标系。可以说是最为广泛应用的一个地球坐标系，它给出一点的大地纬度、大地经度和大地高程而更加直观地告诉我们该点在地球中的位置，故又被称作纬经高坐标系。WGS-84坐标系的X轴指向BIH(国际时间服务机构)1984.0定义的零子午面(Greenwich)和协议地球极(CTP)赤道的交点。Z轴指向CTP方向。Y轴与X、Z轴构成右手坐标系。

一句话解释就是：把前面提到的ECEF坐标系用在GPS中，就是WGS-84坐标系。
其中：
（1）：大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。

（2）：大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。

（3）：大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

在这里插入图片描述

1.3 东北天坐标系（ENU）

也叫站心坐标系以用户所在位置P为坐标原点。

坐标系定义为： X轴：指向东边 Y轴：指向北边 Z轴：指向天顶

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

1.4 车身坐标系—右-前-天坐标（RFU）

车身坐标系—右-前-天坐标（RFU）的定义如下：

X轴：面向车辆前方，右手所指方向

Y轴：车辆前进方向

Z轴：与地面垂直，指向车顶方向
注意：车辆参考点为后轴中心。该坐标系一般用于感知模块。

如下图所示：

1.5 车身坐标系—前-左-天坐标（FLU）

车身坐标系—前-左-天坐标（FLU）的定义如下：

        X轴：车辆前进方向
        Y轴：面向车辆前方，左手所指方向
        Z轴：与地面垂直，指向车顶方向
        注意：车辆参考点为后轴中心。该坐标系常用于实时相对地图模块。将RFU坐标系向左旋转90度即可得到FLU坐标系。

车身坐标系

1.6 Frenet坐标系

Frenet坐标系又称Frenet–Serret公式，Apollo项目文档未提及，但在规划模块中广泛使用。Frenet–Serret公式用于描述粒子在三维欧氏空间 R 3 ℝ^3 R3内沿一条连续可微曲线的运动学特征，如下图所示：

二、坐标系间的转换

2.1 LLA坐标系转ECEF坐标系

（1）LLA坐标系下的(lon,lat,alt)转换为ECEF坐标系下点(X,Y,Z)

在这里插入图片描述

（2）其中e为椭球偏心率，N为基准椭球体的曲率半径

在这里插入图片描述

（3）由于WGS-84下极扁率，偏心率e和极扁率f之间的关系：

在这里插入图片描述

(4) 坐标转换公式也可以为

2.2 ECEF坐标系转LLA坐标系

ECEF坐标系下点(X,Y,Z)转换为LLA坐标系下的(lon,lat,alt)
一开始lon是未知的，可以假设为0，经过几次迭代之后就能收敛

在这里插入图片描述

2.3 ECEF坐标系转ENU坐标系

2.4 ENU坐标系转ECEF坐标系

在这里插入图片描述

2.5 LLA坐标系直接转ENU坐标系

上述可以看到，从LLA坐标系转换到enu坐标系有较多计算量，在考虑地球偏心率e ee很小的前提下，可以做一定的近似公式计算

在这里插入图片描述

2.6 Frenet坐标系与笛卡尔坐标系的转换公式

为什么要将笛卡尔坐标系转换为Frenet坐标系？因为可以这样可以将车辆的二维运动问题解耦合为两个一维运动问题。显然，一维问题比二维问题容易求解，这就是笛卡尔坐标系转换为Frenet坐标系的必要性。

Apollo项目中Frenet坐标系与笛卡尔坐标系转换代码：函数声明文件planning/math/frame_conversion/cartesian_frenet_conversion.h

原文链接：https://blog.csdn.net/davidhopper/article/details/79162385

2.7 LLA坐标系转局部笛卡尔积坐标系

可使用C++库: GeographicLib download | SourceForge.net 中的GeographicLib::LocalCartesion::Forward(double lat, double lon, double h, double &x, double &y, double &z);