开发者说丨无人驾驶中的坐标转换

坐标转换是空间实体的位置描述，是从一种坐标系统变换到另一种坐标系统的过程。通过建立两个坐标系统之间一一对应关系来实现，两个及以上的坐标转换时由极坐标相对参照确定维数空间。

 

其中，我们常见的坐标转换有以下几种：大地坐标（BLH）对平面直角坐标（XYZ）、北京54全国80及WGS84坐标系的相互转换、任意两空间坐标系的转换、在十进制角度和度/分/秒格式之间进行转换、在经纬度和 UTM 坐标之间进行转换。

在当今的驾驶领域中，无论是高级负载驾驶ADAS系统，还是自动驾驶系统，一个必不可少的重要工作就是计算自己的位置，以及自己与道路、车辆、行人等交通元素之间的相对位置关系、速度关系，为了描述这些复杂的空间、时间关系，建立一套时空坐标体系，是实现各种环境感知、决策规划算法的前提条件。

 

自动驾驶中的坐标系统包括有：摄像机坐标系统、激光雷达坐标系系统、车载坐标系系统、世界坐标系系统，在实际应用中，我们需要通过不同传感器确定障碍物与自动驾驶汽车间的相对位置，这就需要将不同的坐标系关联起来，并建立他们之间的转换关系。

 

本文由百度Apollo智能汽车事业部 自动驾驶工程师——陈光撰写，对无人驾驶中的坐标转换进行了详细讲解，希望这篇文给感兴趣的同学带来更多帮助。

 

  以下，ENJOY  

 

前言

在第十一期的分享中，以CAN总线的解析为例，介绍了如何通过解析CAN总线的消息，获取传感器的数据。这一期的分享将会集中在如何将传感器坐标系下的数据转换到自车坐标系下。

 

无人车上拥有各种各样的传感器，每个传感器的安装位置和角度又不尽相同。对于传感器的提供商，开始并不知道传感器会以什么角度，安装在什么位置，因此只能根据传感器自身建立坐标系。无人驾驶系统是一个多传感器整合的系统，需要将不同位置的传感器数据统一到一个固定的坐标系——自车坐标系下，才能分析当前无人车所在的道路场景。

 

 

正文

无人车的自车坐标系

不同的无人驾驶团队对于坐标系的定义可能不同，但这并不影响开发，只要团队内部达成一致即可。

 

以百度Apollo提供的自车坐标系为例，自车坐标系的定义为：

 

• z轴 – 通过车顶垂直于地面指向上方；

   

• y轴 – 在行驶的方向上指向车辆前方；

   

• x轴 – 自车面向前方时，指向车辆右侧；

 

车辆坐标系的原点在车辆后轮轴的中心，如下图所示。

 

车辆坐标系的原点位置

图片出处：https://github.com/ApolloAuto/apollo/blob/master/docs/specs/coordination_cn.md

 

传感器坐标系与自车坐标系的转换

 

对于无人车的传感器安装位置，业内大同小异，比如奥迪A8的传感器配置如下图所示。

 

奥迪A8的传感器配置

 

以安装在无人车左前方的角雷达（Corner Radar）为例，进行后面的介绍，角雷达的安装位置和坐标系如下图绿线所示。

 

雷达的安装位置和坐标系

 

角雷达检测到的障碍物如图中的绿点所示，绿点在雷达坐标系下的坐标为(x1,y1），为了便于理解暂不加入z方向的坐标。

 

绿点转换到自车坐标系下需要经过一定的数学运算。基本思路是这样的：

 

平移

 

先将角雷达坐标系的O点平移到与自车坐标系的O点重合，此时(x1,y1)需要减去两个坐标系在x和y方向的距离，如下图所示。

 

平移

 

旋转

 

在两个坐标系的O点重合后，将角雷达坐标系沿着z轴进行一定角度的旋转，这样(x1,y1)就转到了自车坐标系上。这个过程在数学上称为欧拉旋转。

 

坐标系的平移和旋转是两件相互独立的事情，先平移再旋转和先旋转再平移并不会影响最终的结果。

 

以上是感性的分析过程，下面我们将这整个过程在数学上实现。

 

平移

 

平移步骤根据传感器安装位置和自车后轴的距离进行计算，仅仅是xyz三个方向加减运算。

 

旋转

 

绕轴旋转需要引入角度，不是简单的加减运算，所以我们通过图示来推导一下。

先将两个坐标系变换到正常的视角，如下图所示：

 

旋转

 

障碍物在角雷达坐标系下的坐标为(x1,y1)，假设障碍物在自车坐标系下的坐标为(x0,y0)，需要根据安装角度α（可测量），用x1，y1，α这三个已知量表示x0，y0，求得他们的数学关系。

 

通过做辅助线进行计算，如下图蓝线所示：

 

 

几何关系可用以下两个等式表示：

 

 

使用矩阵表示，可以简化表达，用一个等式代替两个等式，是这样的

 

 

由于这次旋转是绕z轴旋转，因此旋转前和旋转后的z值是保持不变的

 

将z方向的值也放到上面的等式中，即可得到

 

 

那就意味着，只要把角雷达采集到的障碍物坐标值与上面这个矩阵进行矩阵乘法运算，即可完成沿Z轴的旋转。在这里我们把这个矩阵叫做Z轴旋转矩阵RZ，那必然还有沿着X轴和Y轴的旋转矩阵RX和RY。

 

角雷达目标的坐标依次右乘这三个矩阵，就完成了沿着Z轴，Y轴，X轴的旋转，得到的结果就是自车坐标系下的坐标值了。即

 

 

再加上一个平移的矩阵，就能够完整描述整个坐标转换的关系了

 

不同的坐标系定义，会有不同的RX，RY和RZ，因此需要根据实际情况计算旋转矩阵和平移矩阵。

 

欧拉旋转所存在的问题

 

以上过程也称为欧拉旋转，但是欧拉旋转会有一个不可避免的问题——万向锁。欧拉旋转的过程中在某些特殊情况时，会导致少一个维度。万向锁通过文字解释起来会有点困难。

 

为了解决欧拉旋转所带来的万向锁问题，业界引入了四元数。四元数除了解决万向锁的问题外，还能在一定程度上简化计算。因而百度Apollo也选择了四元数作为各个传感器安装位置和角度的存储介质。有关四元数的定义和使用方法这里不做详细讨论，可参看Apollo的《Calibration》模块。

 

结语

 

实际参与到开发中你才会发现，自己在大学学习的高数和线性代数等都是很重要的数学工具，而不是仅仅用来考试的。

 

以上就是有关无人驾驶技术中传感器到车体坐标系的坐标转换原理及数学推导，该原理不仅用在无人车领域，同时在机器人、无人机、三维建模等领域也得到了广泛使用。

 

文章中使用的软件CANad++Editor下载+安装方法及Mobileye车道的原始.dbc文件，可以关注陈光的公众号：自动驾驶干货铺，在后台回复关键字"CAN"获取。

 

本文部分内容参考链接：

*《欧拉旋转》

【https://link.zhihu.com/target=https%3A//v.youku.com/v_show/id_XNzkyOTIyMTI

%3D.html%3Fspm%3Da2h0k.11417342.soresults.dtitle】

*《Calibration》

【https://link.zhihu.com/target=https%3A//github.com/ApolloAuto/apollo/tree/master/modules/calibration】