1. Johngo学长首页
  2. 技术点详细复盘专题
  3. 人工智能

自动驾驶中的坐标系

• 人工智能 • 阅读 119

自动驾驶中的坐标系

*
– 0.引言
– 1.相机传感器坐标系
– 2.激光雷达坐标系
– 3.车体坐标系
– 4.世界坐标系
–
+ 4.1.地理坐标系
+ 4.2.投影坐标系
+
* 4.2.1.投影方式
* 4.2.2.墨卡托(Mercator)投影
* 4.2.3.高斯-克吕格(Gauss-Kruger)投影
* 4.2.4.通用横轴墨卡托UTM（UniversalTransverseMercator）投影
+ 4.3.WGS84与UTM相互转换
– 5.自动驾驶中的坐标定位

0.引言

总结于网络。重点在世界坐标系理解。

自动驾驶中的坐标系

自动驾驶中的坐标系

; 1.相机传感器坐标系

针孔模型：

自动驾驶中的坐标系

自动驾驶中的坐标系

2.激光雷达坐标系

Velodyne生产的64线激光雷达HDL64：

  • 64线激光雷达进行环境扫描过程：单帧扫描数据
    自动驾驶中的坐标系
  • 64线激光雷达进行环境扫描过程：全周扫描数据
    自动驾驶中的坐标系
  • 激光雷达坐标系统

自动驾驶中的坐标系

旋转式激光雷达一般选择激光发射中心作为坐标系原点，向上为 Z Z Z 轴正方向，X X X 轴 Y Y Y 轴构成水平平面。图中红色线条为激光雷达发出的激光束，在任意静止时刻形成平行于 Z Z Z 轴，垂直于 X Y XY X Y 平面的扇形扫描区。每束出射激光在竖直方向上的俯仰角 θ i θ_i θi ​ 为固定值，在设计制造时确定，属于激光雷达的内部参数。扇形扫描平面绕Z轴旋转的角度 φ ( t ) φ(t)φ(t ) 随时间变化，并会在原始测量数据中给出。

第 i i i 束激光在某 t 0 t_0 t 0 ​ 时刻照射到某物体表面的 P P P 点，测距结果显示 P P P 点与激光雷达间的距离为 L L L，则该测量点 P P P 的原始测量数据可以极坐标形式 ( φ ( t 0 ) , θ i , L ) (φ(t_0), θ_i, L )(φ(t 0 ​),θi ​,L ) 来表示。同时，P P P 点在激光雷达的正交坐标系 ( X L , Y L , Z L ) (X_L,Y_L,Z_L)(X L ​,Y L ​,Z L ​) 中表示为 P ( x L , y L , z L ) P(x_L,y_L,z_L)P (x L ​,y L ​,z L ​)，存在如下转换关系。用正交坐标系表示的点云数据，在实际中更为常用。

X L = L cos ⁡ ( θ i ) cos ⁡ ( φ ( t 0 ) ) Y L = L cos ⁡ ( θ i ) sin ⁡ ( φ ( t 0 ) ) Z L = L sin ⁡ ( θ i ) \begin{aligned} &X_{L}=L \cos \left(\theta_{i}\right) \cos \left(\varphi\left(t_{0}\right)\right) \ &Y_{L}=L \cos \left(\theta_{i}\right) \sin \left(\varphi\left(t_{0}\right)\right) \ &Z_{L}=L \sin \left(\theta_{i}\right) \end{aligned}​X L ​=L cos (θi ​)cos (φ(t 0 ​))Y L ​=L cos (θi ​)sin (φ(t 0 ​))Z L ​=L sin (θi ​)​

; 3.车体坐标系

车体坐标系用来描述车辆周围的物体和本车之间的相对位置关系。目前学术界和工业界有几种比较常用的车体坐标系定义方式。分别是ISO国际标准定义，SAE（Society of Automotive Engineers）汽车工程师协会定义，和基于惯性测量单元IMU的坐标定义。

自动驾驶中的坐标系

在车辆动力学分析中，ISO定义的车体坐标系较为常见。SAE定义的车体坐标系与航空航天领域常用的机体坐标系相一致。基于IMU定义的车体坐标系，则在IMU的相关应用中较为常见。

自动驾驶中的坐标系

4.世界坐标系

自动驾驶中的坐标系
  • 地理坐标系统（英文简写GCS，Geographical Coordinate System）地理坐标系单位 度°分’秒”–> 擅长定位
  • 投影坐标系统（英文简写PCS，Projection Coordinate System）投影坐标系单位 米m–> *擅长计算

; 4.1.地理坐标系

自动驾驶中的坐标系

大地坐标系与地理坐标系有何不同？

大地坐标系和地理坐标系都是经纬度表示的坐标系，本身并不包含投影信息，很多时候，这两种说法都是相同的。

自动驾驶中的坐标系
  • 蓝色：实际地球
  • 红色：地心坐标系–>全球拟合表达
  • 绿色：参心坐标系–>局部拟合表达（对于局部精度更高！）

自动驾驶中的坐标系
大地坐标系(经纬度)
  • L：longitude –>经度
  • B：latitude –>纬度

  • H: altitude –>海拔height above sea level

  • 0度经线：国际上将通过英国伦敦格林尼治天文台原址的那条经线称为0°经线，也叫本初子午线：分别向东向西从 0°到 180°

  • 0度纬线：赤道，越往两极纬度越高。纬线在南北极缩成一个点，叫极点，也分别是90°纬线。
  • 表述：东经、西经、南纬、北纬–>”E”东经、”W”西经、”N”北纬、”S”南纬–>平时一般使用东经和北纬(也即是以东经北纬方向为正)

自动驾驶中的坐标系

自动驾驶中的坐标系

WGS84坐标系，全称为世界大地测量系统（英语：World Geodetic System）。WGS包含一套地球的标准经纬坐标系、一个用于计算原始海拔数据的参考椭球体，和一套用以定义海平面高度的引力等势面数据。 WGS-84坐标为GPS(经过差分后)测量出来的，属于大地坐标系。

; 4.2.投影坐标系

4.2.1.投影方式

自动驾驶中的坐标系

自动驾驶中的坐标系

自动驾驶中的坐标系

; 4.2.2.墨卡托(Mercator)投影

自动驾驶中的坐标系

墨卡托(Mercator)投影，是一种”等角正切圆柱投影”.墨卡托投影没有角度变形，由每一点向各方向的长度比相等，它的经纬线都是平行直线，且相交成直角，经线间隔相等，纬线间隔从标准纬线向两极逐渐增大。墨卡托投影的地图上长度和面积变形明显，但标准纬线无变形，从标准纬线向两极变形逐渐增大，但因为它具有各个方向均等扩大的特性，保持了方向和相互位置关系的正确。

4.2.3.高斯-克吕格(Gauss-Kruger)投影

  • *1.高斯-克吕格投影

自动驾驶中的坐标系
高斯-克吕格(Gauss-Kruger)投影简称”高斯投影”，又名”等角横切椭圆柱投影”，地球椭球面和平面间正形投影的一种。投影后， 除中央子午线和赤道为直线外， 其他子午线均为对称于中央子午线的曲线。投影在长度和面积上变形很小，中央经线无变形，自中央经线向投影带边缘，变形逐渐增加（为什么会有变形，想象橘子皮剥开后压平），变形最大之处在投影带内赤道的两端。
  • *2.高斯-克吕格投影分带

按照经线每隔多少度(三分度，六分度)进行划分：三分度：每隔三度一个中央经线（360/3 =120条中央子午线）

自动驾驶中的坐标系
  • 无带号坐标：

自动驾驶中的坐标系
  • 有带号坐标：

自动驾驶中的坐标系

自动驾驶中的坐标系

三度带变形小，覆盖区域小

六度带变形大，覆盖区域大

; 4.2.4.通用横轴墨卡托UTM（UniversalTransverseMercator）投影

和高斯克吕格投影非常相似，只不过它并不是切于经线圈，而是穿地球而过。我们从侧面来看，高斯投影面和经线圈是完全重合的，但是UTM则不是：

自动驾驶中的坐标系
UTM投影现在采用WGS84地理坐标系统进行投影。

UTM投影全称为”通用横轴墨卡托投影”，是等角横轴割圆柱投影（高斯-克吕格为等角横轴切圆柱投影），圆柱割地球于南纬80度、北纬84度两条等高圈， 该投影将地球划分为60个投影带，每带经差为6度，已被许多国家作为地形图的数学基础。UTM投影与高斯投影的主要区别在南北格网线的比例系数上，高斯-克吕格投影的中央经线投影后保持长度不变，即比例系数为1，而UTM投影的比例系数为0.9996。UTM投影沿每一条南北格网线比例系数为常数，在东西方向则为变数，中心格网线的比例系数为0.9996，在南北纵行最宽部分的边缘上距离中心点大约 363公里，比例系数为 1.00158。

高斯-克吕格投影与UTM投影可近似采用 Xutm=0.9996 * X高斯，Yutm=0.9996 * Y高斯进行坐标转换。

顺带知识点：

全球分为二十四个时区，以能够被15整除的经度作为该区域的中央子午线，每一时区占经度15度。
在该时区中央子午线以东的地区，时间要加，以西的地区，时间要减，一度4分钟。
中国共分五个时区：
(1)中原时区：以东经120度为中央子午线。
(2)陇蜀时区：以东经105度为中央子午线。
(3)新藏时区：以东经90度为中央子午线。
(4)昆仑时区：以东经75(82.5)度为中央子午线。
(5)长白时区：以东经135(127.5)度为中央子午线。

一个时区的”标准时”，只是一个大地区的统一时间，大家共同遵守的”人工”时间而已，并不是该时区内每个地点的”本地时间(LMT)”——真正的经度时。要用出生地的经度与出生大地区的标准时来加减，全球任何地点都用这个原则。

例如：中原时区包括内蒙古、辽宁、河北、山西、山东、河南、安徽、江苏、湖北、湖南、江西、浙江、福建、广东、海南、香港、澳门、台湾。这个大地区当时钟敲定正午12点时，只有位于东经120度线上的地点才是12点，其它的地方是少于或多于12点。如香港位于东经114度10分，比东经120度偏西5度50分，其真正经度时是11时36分40秒。

自动驾驶中的坐标系

; 4.3.WGS84与UTM相互转换

在每个纵向区域内，横向墨卡托投影用于给出以米为单位的坐标（东距和北距）。

  • 对于东距，原点定义为每个纵向带中央子午线以西 500,000 米的点，给出中央子午线的东距 500,000 米。
  • 对于北半球的北向，原点被定义为赤道。
  • 对于南半球的北向，原点定义为赤道以南 10,000,000 米的点。

自动驾驶中的坐标系
  • X轴：指向东边
  • Y轴：指向北边
  • Z轴：指向天顶

将中央子午线指定为每个区域内坐标系的参考 y 轴（即东距 = 0）将导致中央子午线以西的点的东距值为负。为此，中央子午线被指定为 500000 米的任意值，从而避免任何负东坐标；位于其东面的点的东距值将大于 500000m，位于西面的点的值将小于 500000m。此分配会将原点放置在中央子午线以西 500000m 的区域外，因此原点称为假原点，东坐标称为假东。

赤道被指定为 UTM 北向坐标的水平参考轴，并为北半球的区域分配了 0 米北的值。为了避免负数，赤道被分配了一个10,000,000米南的假北距，用于参考南半球的北距坐标。（但是好像图中没有给出假北距）

UTM的东北坐标只有两个数字，还需要zone的区间号才能转换为经纬度坐标因此准确的UTM坐标包括三个参数，区间+东北坐标。

void LonLat2UTM(double longitude, double latitude, double& UTME, double& UTMN)
{
    double lat = latitude;
    double lon = longitude;

    double kD2R = PI / 180.0;
    double ZoneNumber = floor((lon - 1.5) / 3.0) + 1;
    double L0 = ZoneNumber * 3.0;

    double a = 6378137.0;
    double F = 298.257223563;
    double f = 1 / F;
    double b = a * (1 - f);
    double ee = (a * a - b * b) / (a * a);
    double e2 = (a * a - b * b) / (b * b);
    double n = (a - b) / (a + b);
    double n2 = (n * n);
    double n3 = (n2 * n);
    double n4 = (n2 * n2);
    double n5 = (n4 * n);
    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2.0;
    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32.0;
    double gm = 15 * n2 / 16 - 15 * n4 / 32;
    double dt = -35 * n3 / 48 + 105 * n5 / 256;
    double ep = 315 * n4 / 512;
    double B = lat * kD2R;
    double L = lon * kD2R;
    L0 = L0 * kD2R;
    double l = L - L0;
    double cl = (cos(B) * l);
    double cl2 = (cl * cl);
    double cl3 = (cl2 * cl);
    double cl4 = (cl2 * cl2);
    double cl5 = (cl4 * cl);
    double cl6 = (cl5 * cl);
    double cl7 = (cl6 * cl);
    double cl8 = (cl4 * cl4);
    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
    double t = tan(B);
    double t2 = (t * t);
    double t4 = (t2 * t2);
    double t6 = (t4 * t2);
    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
    double yt = e2 * cos(B) * cos(B);
    double N = lB;
    N = N + t * Nn * cl2 / 2;
    N = N + t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
    N = N + t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
    N = N + t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
    double E = Nn * cl;
    E = E + Nn * cl3 * (1 - t2 + yt) / 6;
    E = E + Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
    E = E + Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
    E = E + 500000;
    N = 0.9996 * N;
    E = 0.9996 * (E - 500000.0) + 500000.0;

    UTME = E;
    UTMN = N;
}

感悟：没有系统的学习，去网上查资料都是零碎的业余选手讲解，很难形成系统，所以有时间还是需要多读书，但是也不知道这个知识点在那本书中有详细的讲解，就像这篇博客下面的评论一样，想问问是那本书上的公式都没人理。

  • *根据经纬度算UTM带号

UTM是由美国制定，因此起始分带并不在本初子午线，而是在180度，因而所有美国本土都处于0－30带内。UTM投影采用6度分带，从东经180度（或西经180度）开始，自西向东算起，因此1带的中央经线为-177（-180 -(-6)），而0度经线为30带和31带的分界，这两带的分界分别是-3和3度。纬度采用8度分带，从80S到84N共20个纬度带（X带多4度），分别用C到X的字母来表示。为了避免和数字混淆，I和O没有采用。UTM的”false easting”值为500km，而南半球UTM带的”false northing”为10000km

“WGS 1984″坐标系的墨卡托投影分度带（UTM ZONE）选择方法如下：
（1）北半球地区，选择最后字母为”N”的带；
（2）可根据公式计算，带数=（经度整数位/6）的整数部分+31

如：北京经度约116.4，
带数=116/6+31=50，选50N，即WGS 1984 UTM ZONE 50N

- 根据经度求带号
namespace {
int GetLongZone(double longitude) {
  double longZone = 0.0;
  if (longitude < 0.0) {
    longZone = ((180.0 + longitude) / 6.0) + 1;
  } else {
    longZone = (longitude / 6.0) + 31;
  }
  return static_cast<int>(longZone);
}
}

5.自动驾驶中的坐标定位

GPS/RTK获得经度lon、纬度lat、高程height、偏航角yaw、俯仰角pitch、横滚角roll 。

将lon,lat转化为utm坐标，（utm_x, utm_y, height）就可以作为GPS此时在世界坐标系下位置，再把yaw,pitch,roll转化为旋转矩阵，就可以得到GPS在世界坐标系下的姿态q，合起来就得到了GPS在世界坐标系下的位姿

通过标定可得到GPS到车身base_link坐标系的变换，这样就能得到当前车身base_link在世界坐标系下的位姿，再由标定可得到lidar到车身base_link坐标系的变换，这样就能把lidar坐标系下的点云变换到世界坐标系下，也就是高精地图中点云的坐标。

由于地图太大，需要对其进行方形网格分块进行存储和查询，每一块的id可通过resolution, col(utm_x/resolution), row(utm_y/resolution), utm_zone, utm_band进行编码。

同时，将GPS得到的经纬度转换为火星坐标系 (在中国大陆使用地图服务的话，我们可能会看到GCJ-02这个坐标系统，这又是什么东西呢？GCJ-02是国家测绘局(国测局，GCJ，这样的缩写比较罕见)02号标准的意思。它是一种对经纬度数据的加密算法，即加入随机的偏差。国内出版的各种地图系统(包括电子形式)，必须至少采用GCJ-02对地理位置进行首次加密。由于最开始的时候一些国外地图服务商没搞明白这点，直接使用了GCJ-02加密的坐标来显示POI，从而把这些POI移到太平洋上去了，所以网友也戏称火星坐标。) ，就通过一些地理信息可视化库在地图上可视化轨迹，如通过轨迹坐标生成kml文件，就可以在谷歌地球上显示轨迹。

Original: https://blog.csdn.net/fb_941219/article/details/124444541
Author: 古路
Title: 自动驾驶中的坐标系

原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/558886/

转载文章受原作者版权保护。转载请注明原作者出处！

赞 (0)
0

【自取】最近整理的，有需要可以领取学习：



Linux核心资料大放送~

全栈面试题汇总（持续更新&可下载）

一个提高学习100%效率的工具！

【超详细】深度学习面试题目！

LeetCode Python刷题答案下载！

LeetCode Java版刷题答案下载！

LeetCode C++ 版本，抓紧保存！

LeetCode GO语言 刷题答案下载！

大家都在看

亲爱的 Coder【最近整理，可免费获取】👉 最新必读书单  | 👏 面试题下载  | 🌎 免费的AI知识星球