一文详解自动驾驶中的时空坐标系

开垦者

本文介绍自动驾驶技术中几种常用的坐标系统，以及他们之间如何完成关联和转换，最终构建出统一的环境模型。

所谓时空坐标系，包括三维空间坐标系和一维时间坐标系。在此基础上，用解析的形式（坐标）把物体在空间和时间的位置、姿态表示出来。一般三维空间坐标系用三个正交轴表示物体的位置，用绕这三个正交轴的旋转角度（roll 横滚角, pitch 俯仰角, yaw 偏航角）表示物体的姿态。时间坐标系只有一个维度。为了表述方便，我们一般将空间坐标和时间坐标分开讨论。

PART 01
线性代数位姿

1.1
矩阵伴随


1.2
四元数实部和虚部一个标量和一个矢量来表示四元数就会如下图所示，这样会便于我们计算四元数之间的累乘变换



1.3
四元数与时间我们说四元数是除了旋转矩阵以外的另一种对旋转表达方式，并且它不具备奇异性，可以表达任意三维旋转，因此有必要学习一下它对时间的求导方式。



下面对时间求导：


1.4
旋转矩阵和旋转向量从旋转向量到旋转矩阵基本就是通过罗德里格斯公式表明：



1.5
李群李代数扰动


1.6
李群累加


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



2.1
图像坐标系（or 像素坐标系）电脑上存储的照片或图像，一般以左上角为原点，向右为正方向，向下为正方向，单位以“像素”最为常用。图像坐标系为二维坐标系，标记为。

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

2.3
像平面坐标系（or 成像坐标系）为了能够定量描述三维空间到二维图像的映射关系，图形学里引入了像平面坐标系。它是摄像机坐标系的一个平移，中心仍在摄像机主光轴上，距离光轴中心的距离等于摄像机的焦距。
我们知道摄像机会在光轴中心后方的底片上成一个缩小的倒像，是真正的像平面。但是为了分析和计算方便，我们会在光轴中心前方设立一个虚拟像平面。虚拟像平面上的成像为正像，大小与真实倒像相同。









2.4
相机坐标系系统各坐标间的转换关系简单来讲，从摄像机坐标系到像平面坐标系存在以下简单映射关系。

其中f为摄像机焦距。
从以毫米为单位的像平面坐标系到以像素为单位的图像坐标系，存在线性转换关系：


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

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

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


从二维图像中获取三维距离信息
如果需要获得物体在世界坐标系中的位置，则还需要知道摄像机在世界坐标系中的位姿。这一位姿表示被称为摄像机的外部参数，简称外参，用来决定摄像机坐标与世界坐标系之间相对位置关系。自动驾驶应用中，得到这一位置关系还需要一系列的标定和定位工作。在后边的坐标系关联中进行介绍。                                 PART 03
激光雷达坐标系系统
激光雷达是自动驾驶，特别是无人驾驶中最重要的传感器之一。目前世界上几乎所有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点在世界坐标系中的位置，则还需要一系列的标定和定位工作。在后边的坐标系对齐中进行介绍。
PART 04
车体坐标系系统
车体坐标系用来描述车辆周围的物体和本车之间的相对位置关系。目前学术界和工业界有几种比较常用的车体坐标系定义方式。分别是ISO国际标准定义，SAE（Society of Automotive Engineers）汽车工程师协会定义，和基于惯性测量单元IMU的坐标定义。



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


多种车体坐标系定义举例
PART 05
世界坐标系系统
前面介绍的几种坐标系都是基于设备的局部坐标系，世界坐标系则是一个描述地球上位置关系的系统。地球是一个不规则的椭球，描述地球表面上的相位关系就没有那么直接。人们早在15世纪就开始了对地球上几何位置关系的研究，并且开始了世界地图的绘制。到了现代，人们已经制定了多种世界坐标系统作为国际标准，用来描述地球上的绝对位置和相对位置关系。


古老的世界地图

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

WGS-84坐标系采用大地经度、纬度、和大地高程来描述地球上任意一点的位置。经纬线相互交织构成经纬网，用经度、纬度表示地面上点的位置就是地理坐标。用经纬度表示的大地坐标是一种椭球面上的坐标，不能直接应用于测绘。因此需要将他们按一定的数学规律转换为平面直角坐标。在平面直角坐标系中我们能较方便地计算地表（公路上）两个物体的相对距离和位置关系。

WGS-84 坐标系定义
PART 06
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万平方公里）还大得多。


正轴墨卡托投影示意图
PART 07
坐标系间的关联
前边介绍了用于自动驾驶的传感器局部坐标系，车体坐标系，以及基于大地坐标的世界坐标系。在实际应用中，我们需要通过不同传感器确定障碍物与自动驾驶汽车间的相对位置，同时需要知道本车在世界坐标和地图中的位置与姿态。这就需要将不同的坐标系关联起来，并建立他们之间的转换关系。


自动驾驶系统中的多个坐标系

7.1
从传感器坐标系到车体坐标系自动驾驶汽车一般都装有多个传感器，每个传感器安装的位置、方向都不一样。同一个目标（如车辆、行人）在各个传感器视野中出现的位置也都不同。为了将不同传感器间彼此独立的结果关联起来，建立统一的环境模型，我们需要找到各个传感器与车体间的位置关系，这也是自动驾驶中感知融合算法的最基本步骤。传感器在车体上的安装位置一旦确定，在运行中就会保持固定，所以可以采用离线标定的方法确定各传感器相对车体的精确位置。
传感器离线标定的方法有很多，这里不一一介绍。算法的总体思想是通过调整各坐标系之间的转换关系，使同一个物体（如下图中的棋盘格标定板）通过多个传感器感知得到的独立结果，经过坐标系转换后，可以在车体坐标系下精确吻合，形成统一结果。


传感器标定及标定板示例

7.2
从车体坐标系到世界坐标系车体坐标系和世界坐标系之间的关系是由车辆本身的位置和姿态决定的，这一转换关系可以从车辆的定位结果中直接得到。通过车体和世界坐标系的转换关系，可以确定车体在高精地图中的位置和方向，进而可以计算出车体和其他道路元素，例如车道线、红绿灯、停止线，之间的相对关系。
PART 08
时间坐标系统
自动驾驶应用所应对的是一个随时间变化的环境，所以时间坐标系统的设立于统一也是至关重要的一环。自动驾驶中一般使用多种不同类型的传感器，彼此独立地对环境进行感知。这样会造成各传感器收集的环境数据并不在同一个时间点。即便空间坐标系已经建立了完美的转换关系，在时间上也无法将环境数据进行统一。所以除了空间坐标系需要进行精确标定外，各个设备之间的时间坐标系也需要进行同步。

8.1
统一的时间系统自动驾驶系统中含有多个主机、传感器、和控制器，一般都具有自己独立的时钟。为了建立统一的时间坐标系统，让各个设备使用相同的时间基准，一个高精度授时系统是必不可少的。

自动驾驶中一般采用GPS的时钟系统作为各个系统的时间基准。GPS时间系统规定1980年1月6日零时为时间坐标系的原点，时间向上累加，系统授时精度可以达到纳秒量级。同时自动驾驶中所使用的大部分设备都具备接受外部授时的功能。以Velodyne激光雷达为例，设备可以接收标准的PPS（Pulse per Second，秒脉冲）和NMEA报文（一种串口通讯格式）的授时方法。

8.2
硬件同步触发一些设备的数据采集可以通过外部触发的方式进行激活，于是可以使用同一个外部信号，同时激活多个传感器，从而得到同一个时间点上的环境信息。

例如摄像机的曝光可以通过外部开关信号进行触发，于是自动驾驶汽车上的多个摄像机可以使用同一个开关信号进行曝光和采样的硬同步。进而，这一开关信号还可以与激光雷达等其他传感器进行协同，完成不同种类传感器间的同步触发操作。

8.3
软件时间对齐另一些传感器的采样不支持外部触发，同时有些设备的工作频率也不一致，无法做到严格的硬时间同步，这就需要在软件中进行处理。
有了前面提到的统一的时间系统，通过不同传感器获得的环境信息即便不在同一个时间点上，也有着统一的时间标记。这样通过软件计算，对非同步采样结果进行差值或外推，就可以近似得到同一个时间点上的环境信息，成为决策控制系统进行判断的依据。
PART 09
雷达坐标系
雷达坐标系的方向一般为：Z->上，X->右，Y->前



PART 10
IMU 坐标系
IMU坐标系的方向一般为：Z->下，X->右，Y->后



PART 11
GPS 坐标系
GPS 自身没有坐标系，一般是由经度和纬度组成。在世界坐标系（WGS84）下的坐标值我们可以通过变换转到本地坐标系，一般会和里程计的坐标系绑定在一起。

PART 12
轮速坐标系/车体坐标系
车体坐标系原点在载体质量中心与载体固链，x轴沿载体轴指向右，y轴指向前，z轴和xy满足右手坐标法则指天，又称为右-前-上（r-f-u）坐标系