自动驾驶中的三大坐标系

自动驾驶中涉及3大坐标系：世界坐标系、车体坐标系、各个传感器坐标系

世界坐标系

世界坐标系是一个描述地球上位置关系的系统。地球是个不规则的椭球，描述地球表面上的相位关系就没有那么直接。人们早在15世纪就开始了对地球上几何位置关系的研究，并开始了世界地图的绘制。到了现代，人们已经制定了多种世界坐标系统作为国际标准，用来描述地球上的绝对位置和相对位置关系。

wgs-84坐标系

wgs-84坐标系是一种国际上采用的地心坐标系。坐标原点为地球质心，其地心空间直角坐标系的Z轴指向BIH （国际时间服务机构）1984.0定义的协议地球极（CTP)方向，X轴指向BIH 1984.0的零子午面和CTP赤道的交点，Y轴与Z轴、X轴垂直构成右手坐标系，称为1984年世界大地坐标系统。GPS广播星历是以WGS-84坐标系为根据的。

WGS-84 坐标系定义

UTM坐标系

在测绘（例如高精地图的绘制）和导航（例如无人车的导航）中，常常需要用米为单位表示距离和大小。然而GPS的定位结果所使用的wgs84坐标却是用经纬度表示位置。于是需要一种坐标转换或者映射关系将经纬度坐标转换为以米为单位的平面直角坐标。

目前，这种坐标映射关系有多种标准，比如国际上通用的UTM坐标系，我国的北京54坐标系和西安80坐标系。其基本思想都是把椭球形的地球表面按照小的区块展开，投影到一个曲面（圆柱面或椭圆柱面）上，曲面再次展开铺平成平面，进而构成平面直角坐标系。这里对国际上较为常用的UTM坐标系做一个简单介绍。

UTM(Universal Transverse Mercartor, 通用横轴横墨卡托)坐标系统使用UTM投影将椭球面分区块映射到平面直角坐标系中。这种坐标系统及其所依据的投影已经广泛用于地形图，作为卫星影像和自然资源数据库的参考格网以及要求精确定位的其他应用。

UTM投影是等角横轴割圆柱投影，圆柱割地球于南纬80度、北纬84度两条等高圈，之间的地球表面积按经度6度划分为南北纵带（投影带）。从180度经线开始向东将这些投影带编号，从1编至60(北京处于第50带）。UTM投影沿每一条南北格网线比例系数为常数，在东西方向则为变数，中心格网线的比例系数为0.9996，在南北纵行最宽部分的边缘上距离中心点大约 363公里，比例系数为 1.00158。

横轴墨卡托投影示意图

如果把投影中的割圆柱平行于地轴放置，则为正轴墨卡托投影。以正轴墨卡托投影得到的平面直角坐标系下世界地图，其比例尺随纬度变化而变化。赤道的比例尺最小，比例尺随纬度升高急剧变大。比如在正轴摩卡多投影地图上，位于高纬地区的格林兰岛（面积216万平方公里）看上去面积要比澳大利亚（面积769万平方公里）还大得多。

正轴墨卡托投影示意图

车体坐标系

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

多种车体坐标系定义举例

在车辆动力学分析中，ISO定义的车体坐标系较为常见。SAE定义的车体坐标系与航空航天领域常用的机体坐标系相一致。基于IMU定义的车体坐标系，则在IMU的相关应用中较为常见。无论使用哪一种坐标系定义，只要使用正确，都可以完成对车身位姿的描述，以及确定周围物体和本车间的相对位置关系。研发人员可以根据应用需求和使用习惯来选择车体坐标系。

各个传感器坐标系

相机坐标系

摄像机/摄像头以其低廉的价格、丰富的图像信息，成为自动驾驶中最受人们关注的传感器之一。摄像头的作用是把三维世界中的形状、颜色信息，压缩到一张二维图像上。基于摄像头的感知算法则是从二维图像中提取并还原三维世界中的元素和信息，如车道线，车辆、行人等，并计算他们与自己的相对位置。

1，图像坐标系

电脑上存储的照片或图像，一般以左上角为原点，向右为x正方向，向下为y正方向，单位以“像素”最为常用。图像坐标系为二维坐标系，标记为(Xi, Yi)。

2，摄像机坐标系

由于图像坐标系向右为x，向下为y，所以摄像机坐标系以镜头主光轴中心为原点，一般向右为x正方向，向下为y正方向，向前为z正方向。这样，x，y方向与图像坐标系的方向吻合，z方向即为景深，同时符合右手坐标系的定义，便于算法中的向量计算。摄像机坐标系记为(Xc, Yc)。

3，像平面坐标系

为了能够定量描述三维空间到二维图像的映射关系，图形学里引入了像平面坐标系。它是摄像机坐标系的一个平移，中心仍在摄像机主光轴上，距离光轴中心的距离等于摄像机的焦距。

我们知道摄像机会在光轴中心后方的底片上成一个缩小的倒像，是真正的像平面(X’f, Y’f)。但是为了分析和计算方便，我们会在光轴中心前方设立一个虚拟像平面(Xf, Yf)。虚拟像平面上的成像为正像，大小与真实倒像相同。

摄像机坐标系统

4，坐标间的转换关系

简单来讲，从摄像机坐标系到像平面坐标系存在以下简单映射关系。

 

其中f为摄像机焦距。

从以毫米为单位的像平面坐标系到以像素为单位的图像坐标系，存在线性转换关系：

其中sx，sy是图像上每个像素在像平面上所对应的物理尺寸，单位是像素/毫米。(Cx, Cy)是像平面中心在图像中的位置，单位是像素。

5，摄像机的内参和外参

摄像机的焦距f, 像素尺寸sx，sy，和图像中成像中心的位置(Cx, Cy)在计算机图形学中被称为摄像机的内部参数，简称内参，用来确定摄像机从三维空间到二维图像的投影关系。实际应用中摄像机的内参会更为复杂，还包括图像的畸变率等参数。在自动驾驶应用中，摄像机的内参为常数，使用中不会发生变化，但需要在使用前做好标定工作。

摄像机的拍摄过程，可以抽象成是从三维摄像机坐标系映射到二维像平面坐标系，再映射到图像坐标系的过程。图像感知算法则是这一过程的逆过程，通过二维图像推断物体在三维摄像机坐标系中的位置，例如获得距离（深度）信息。

从二维图像中获取三维距离信息

如果需要获得物体在世界坐标系中的位置，则还需要知道摄像机在世界坐标系中的位姿。这一位姿表示被称为摄像机的外部参数，简称外参，用来决定摄像机坐标与世界坐标系之间相对位置关系。自动驾驶应用中，得到这一位置关系还需要一系列的标定和定位工作。在后边的坐标系关联中进行介绍。

激光雷达坐标系

激光雷达是自动驾驶，特别是无人驾驶中最重要的传感器之一。目前世界上几乎所有L4级别以上的自动驾驶试验测试车都配备了不同型号的激光雷达。

激光雷达的基本原理非常简单。多线激光雷达基本上可以看做是按一定角度绑在一起，并且不停旋转的高速激光测距仪。从激光雷达的英文名（Lidar, Light Detection and Ranging, 光学检测与测距）中也可以看出，它的最基本功能就是测距，只不过它测距的速度非常快。

以Velodyne生产的64线激光雷达HDL64为例，其在垂直方向上可以几乎同时在64个方向上发射激光。根据反射回的激光在空中的飞行时间（TOF, time of fly）就可以计算出激光雷达距离物体表面的距离。这64束垂直分布的激光，随上部机体一起旋转，从而完成对环境360度的扫描。大量的数据点绘制在3维空间中，形成了云状分布，被称为激光点云（Point Cloud）。

64线激光雷达进行环境扫描过程
（上：单帧扫描数据；下：全周扫描数据）

单束激光的测距结果是一个1维数据，我们需要建立三维坐标系，并对原始测距数据进行转换，才能得到空间中的三维点云数据。

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

激光雷达坐标系统

例如，第i束激光在某t0时刻照射到某物体表面的P点，测距结果显示P点与激光雷达间的距离为L，则该测量点P的原始测量数据可以极坐标形式(φ(t0), θi, L )来表示。同时，P点在激光雷达的正交坐标系(XL,YL,ZL)中表示为P(xL,yL,zL)，存在如下转换关系。用正交坐标系表示的点云数据，在实际中更为常用。

如果需要得到P点在世界坐标系中的位置，则还需要一系列的标定和定位工作。