1. 为什么Apollo要定义这么多套坐标系先交代一个背景。Apollo这套自动驾驶系统从感知到定位再到规划控制涉及传感器种类多、计算模块多、数据流链路长。每个模块看数据的角度都不一样激光雷达拿到的是一个个点在雷达自身坐标系下的三维坐标相机拿到的是像素在图像平面上的二维坐标定位模块输出的是车在世界坐标系下的位姿而规划模块关心的是车在道路上的横向纵向偏差。如果没有一套统一的坐标系体系把这些数据串起来整个系统就会变成一盘散沙。我在最初接触Apollo的时候最直观的感受是代码里边动不动就出现world、localization、lidar、camera这些tf坐标帧名字。当时觉得绕来绕去很麻烦后来真到了拿实车数据调试感知结果叠加显示的时候才意识到坐标系不是麻烦的问题而是整个系统能不能正常工作的地基。地基如果歪了上面盖的楼越高塌得越狠。1.1 每个模块各说各话系统就乱套了想象一个场景毫米波雷达检测到前方20米处有一个障碍物。这个20米是相对雷达本身的如果雷达装在车头那这个点在车辆坐标系下大概是车头前方偏上一点的位置;如果这个点要叠加到相机图像上还必须知道相机相对于车身的安装角度和位置以及相机自身的内参;再如果把车的当前位置放到地图上还需要知道车身相对世界坐标系的位姿。这一条链路里每一步都是一次坐标变换。Apollo之所以要定义那么多坐标系本质上不是因为设计者喜欢复杂而是因为自动驾驶整个过程天然就需要在不同参考基准下描述同一个物理世界。关键不在于坐标系多而在于每个坐标系之间必须能用数学关系精确地互相转换。1.2 一条从地心到像素的完整链路我习惯把Apollo里的坐标系分成四个层次按照空间的尺度从大到小排世界坐标系/全局坐标系描述车在地球上的位置和姿态。Apollo定位模块用的是UTM坐标系配合WGS84经纬度做转换。车身坐标系以车辆本身为参考描述车周围物体相对车的位置。Apollo里常用FLU约定即X轴向前、Y轴向左、Z轴向上。传感器坐标系以每个传感器自身为原点描述传感器看到的原始数据。图像/像素坐标系以相机成像平面为基准描述像素在图像上的位置单位是像素。这四层之间的关系是层层递进的像素坐标通过相机内参变换到相机坐标相机坐标通过外参变换到车身坐标车身坐标通过定位位姿变换到世界坐标。反过来也可以从世界坐标一路变换回像素坐标。感知融合、障碍物投影、地图标注、实车调试所有环节都在这一条链路上工作。1.3 各坐标系在Apollo里的实际角色Apollo代码里比较常见的几个坐标帧我列个表说明它们的角色坐标帧名称角色定位典型用途world或earth全局世界坐标系通常基于UTM投影定位输出、地图匹配、路径规划localization或odom车身在世界中的实时位姿各模块获取当前车身状态vehicle车身坐标系以车体为参考基准的局部坐标系传感器外参标定、障碍物局部表示lidar、camera、radar各传感器坐标系感知原始数据表达image/pixel相机成像平面坐标系目标检测、图像分割输出这里要特别指出一个容易忽略的细节Apollo的localization输出的是UTM坐标加一个车头朝向的yaw角而不是直接输出经纬度。原因很简单UTM是平面直角坐标在局部范围内距离和角度计算非常直观而经纬度是球面坐标算个两点距离还要考虑地球曲率。Apollo在定位内部做的是IMU、GPS、激光点云等多种信号的融合融合结果在UTM平面坐标系里表达规划控制模块直接拿这个结果做计算效率高也不会出大错。2. 从UTM到车身先搞懂全局坐标系2.1 UTM投影坐标系的来历与使用UTMUniversal Transverse Mercator通用横轴墨卡托是一种将地球表面投影到平面上的方法。地球是个椭球体想把它展开成平面一定会有畸变UTM的思路是把地球按经度划分成60个带每个带6度在每一个带内做横轴墨卡托投影。这样在局部区域内距离和角度的畸变很小可以当作平面直角坐标系来用。实际开发中从WGS84经纬度转UTM是非常频繁的操作。Apollo里有一套自己的坐标转换工具但其实本质算法和很多开源库是一致的。转换过程大致是定义椭球模型参数WGS84的长半轴是6378137.0米扁率是1/298.257223563。根据经度计算当前点所在的UTM带号。用墨卡托投影公式计算出东向easting和北向northing坐标。加上500000米的东向偏移中央经线的假东和北半球的假北偏移得到最终的UTM坐标。为什么一定要加这个偏移因为中央经线上的点投影后横坐标应该是0但如果不给一个较大的偏移量西经那半边就会出现负数坐标计算上容易出问题。加上500000米之后整个带内所有点的东向坐标都为正用起来就省心了。2.2 车身坐标系的FLU约定和杆臂补偿车身坐标系是整个自动驾驶系统里最核心的局部基准。Apollo采用FLU约定F代表Front前、L代表Left左、U代表Up上。也就是说X轴指向车头前进方向Y轴指向驾驶员的左侧Z轴垂直向上。这个约定和航空领域常用的NED北东地不一样区别主要在于Z轴的朝向FLU是Z向上NED是Z向下。在写代码之前必须搞清楚当前模块用的是哪套约定否则yaw角的符号就会反。车身坐标系的原点通常定义在后轴中心或者IMU安装位置具体取决于Apollo的配置。这里有个特别重要但容易被忽略的概念——杆臂lever arm。杆臂指的是传感器安装位置相对于车身坐标系原点的平移量。比如GPS天线装在车顶它的坐标在车身坐标系下大概是(1.2, 0.0, 1.5)左右这组数值就是天线相对车身的杆臂。定位融合算法里会用到杆臂参数来补偿GNSS和IMU之间的位置差异如果杆臂配错了融合出来的轨迹就会在转弯时出现奇怪的偏移。2.3 为什么localization的输出用UTM而不是经纬度这里面有一个很实际的原因自动驾驶的很多算法需要对位置做微分、积分、插值等运算。在UTM平面坐标系下速度和位置的换算直接就是线性的加减而在经纬度坐标系下1度经度的实际距离随纬度变化没法直接加减。更关键的是规划控制模块输出的轨迹必须连续平滑UTM坐标下可以直接对位置做多项式拟合或样条插值经纬度坐标下做这些运算就非常别扭。所以你现在打开Apollo的localization输出topic看到的pose.position.x和pose.position.y实际上就是UTM东向和北向坐标pose.position.z是海拔高度。注意z轴的物理意义在UTM坐标系里它指的是相对WGS84椭球面的高度和GPS测出来的椭球高是一回事。后来我在调试中遇到过一个问题定位z值突然跳变了好几米排查到最后发现是GPS的椭球高和海平面高程被混用了这是坐标系和基准面没搞一致造成的典型事故。3. 传感器坐标系标定参数到底在描述什么变换3.1 相机坐标系从三维到二维的投影关系相机是感知模块里最复杂的传感器因为在所有传感器中只有它是把三维世界投影到二维平面上这个投影过程本身就有信息损失。相机相关有两个坐标系相机坐标系和像素坐标系。相机坐标系以相机光心为原点Z轴通常指向相机的正前方X轴向右Y轴向下注意这里的Y轴方向和车身坐标系相反。像素坐标系的原点在图像的左上角u轴向右v轴向下单位是像素。相机坐标系中的一个三维点(X, Y, Z)要变换到像素坐标(u, v)靠的是内参矩阵K[u] [fx 0 cx] [X/Z] [v] [0 fy cy] [Y/Z] [1] [0 0 1] [ 1 ]其中fx和fy是焦距以像素为单位cx和cy是主点坐标。这个公式描述的是针孔成像模型和OpenCV文档里的公式完全一致。实际相机镜头会有畸变所以还需要畸变系数k1、k2、p1、p2、k3在做投影之前先对归一化坐标去畸变。内参解决的是相机坐标系里的点落在图像哪个像素外参解决的是车身坐标系里的点怎么变换到相机坐标系。外参通常表示为一个4x4齐次变换矩阵里面包含一个3x3旋转矩阵和一个3x1平移向量。这个外参矩阵描述了相机相对于车身或者车辆坐标系的安装位置和朝向。标定外参是所有相机相关应用的第一步外参错了后面所有感知结果都是空中楼阁。3.2 激光雷达、IMU和毫米波雷达的坐标系激光雷达的坐标系通常是以雷达自身中心为原点X轴向前、Y轴向左、Z轴向上和车身坐标系的轴向定义基本一致区别只在于原点位置和可能的微小安装偏角。由于激光雷达直接输出三维点云它到车身坐标系的变换主要就是一个外参校准问题。Apollo的点云感知流程里激光雷达点云先变换到车身坐标系然后做地面分割、障碍物聚类、目标跟踪这个过程如果外参有一点偏差障碍物离车头的距离就会产生系统性误差。IMU坐标系以IMU自身的敏感轴为基准Apollo里通常也是X前、Y左、Z上的FLU配置。IMU输出的角速度和加速度是在IMU坐标系下测量的定位融合算法需要将测量值变换到车身坐标系这就要用到IMU的外参。IMU外参里同时包含安装角旋转和杆臂平移角度一般由机械安装保证在几度以内但杆臂必须精确测量。毫米波雷达的坐标系也是X向前、Y向左、Z向上但它输出的是目标级别的数据距离、速度、角度Apollo里一般用radar障碍物的话题直接输出。radar的外参标定相对粗糙因为毫米波雷达的角度分辨率较低但是杆臂误差对目标横向位置的判断影响很大。3.3 外参方向搞反的经典错误我在实际中遇到过一个坑外参矩阵明明是从相机到车身的但在代码里被当成从车身到相机来用结果融合出来的目标在图像上的投影位置整体偏移而且随着车辆转角越大偏移越离谱。排查了很久才发现是变换矩阵方向反了。这里有一个非常实用的经验拿到一个外参标定文件时先确认它的坐标系定义到底是T_cam_vehicle从车身坐标系变换到相机坐标系还是T_vehicle_cam从相机坐标系变换到车身坐标系两者的关系是互为逆矩阵。验证方法也很简单。取车身坐标系里的一个点比如车头正前方10米处(10, 0, 0)用外参变换到相机坐标如果变换正确这一点的Z值应该是正数位于相机前方X、Y值应该比较小接近图像中心。如果算出来Z是负的那肯定是方向搞反了。这种检查不到一分钟但能省下好几个小时的排错时间。4. 旋转数学欧拉角、旋转矩阵、四元数怎么选4.1 三种旋转表达的比较坐标变换里最难的不是平移而是旋转。旋转的表达方式有三种主流方案每种都有自己的适用场景旋转矩阵3x3正交矩阵优点是直观、可以直接叠乘缺点是参数冗余9个数表达3个自由度、必须保证正交性而且插值困难。欧拉角用绕三个轴的旋转角度表示朝向优点是参数少、人类容易理解缺点是存在万向锁问题、且旋转顺序的约定不统一容易出错。四元数用四个参数表达旋转优点是插值平滑、没有万向锁、计算效率高缺点是不直观看到[0.7, 0.1, 0.1]这种数很难想象出朝向。Apollo内部很多地方用四元数做姿态传递比如localization输出的pose.orientation就是四元数格式。但人调试的时候看四元数是没法直觉理解的所以经常要转成欧拉角来看。这里就涉及一个特别容易出错的地方——欧拉角的旋转顺序和内外旋的区别。4.2 绕移动坐标系和绕固定坐标系旋转的区别这是我在学习坐标系时踩过最深的一个坑值得展开仔细讲。假设我们要描述一个物体的朝向有两条路径可以走。第一条路径是绕固定坐标系世界坐标系的三个轴依次旋转先绕X轴转α角再绕Y轴转β角最后绕Z轴转γ角这种旋转方式叫外旋extrinsic rotation。第二条路径是绕物体自身坐标系移动坐标系的三个轴依次旋转先绕自身的X轴转α角然后绕旋转后的Y轴转β角最后绕旋转后的Z轴转γ角这种旋转方式叫内旋intrinsic rotation。关键结论来了绕固定坐标系的XYZ外旋等价于绕移动坐标系的ZYX内旋。这个等价关系很多人理解反了。也就是说同样一组欧拉角(α, β, γ)如果按固定轴XYZ顺序解释和按移动轴ZYX顺序解释得到的最终旋转矩阵是完全一样的。但它们各自对应的先转哪个轴的直觉是不同的。举个具体例子假设现在要把一个物体旋转到朝向(roll30°, pitch45°, yaw60°)按固定轴外旋X-Y-Z理解先绕世界坐标系的X轴转30°再绕世界坐标系的Y轴转45°最后绕世界坐标系的Z轴转60°。按移动轴内旋Z-Y-X理解先绕物体自身的Z轴转60°偏航再绕偏航后的Y轴转45°俯仰最后绕俯仰后的X轴转30°横滚。这两种路径最终得到的是同一个旋转矩阵。Apollo文档和很多机器人教材里欧拉角默认用的是ZYX内旋的顺序也就是yaw-pitch-roll的次序。这个约定和ROS里的RPYroll-pitch-yaw看起来名字一样但实际计算顺序可能完全不同。我自己就在这上面吃过亏把一个按ZYX内旋算出来的四元数用XYZ外旋的公式解码欧拉角结果数值完全对不上一度怀疑是标定数据坏了。实践中最稳妥的做法是尽量用四元数做运算只在人读的时候转成欧拉角并且转的时候明确指定旋转顺序。大部分坐标系工具库都支持参数化选择内外旋顺序不要用默认值。4.3 万向锁问题和实际影响万向锁指的是当物体绕第二个轴的旋转角为90度时第一个轴和第三个轴的功能发生重合导致系统丢失了一个旋转自由度。用欧拉角表达姿态时在万向锁附近会出现奇怪的数值跳变微小角度变化会导致欧拉角分量剧烈跳动而实际物体朝向变化其实很小。在实际工程中因为万向锁导致的最直接问题是姿态插值异常。如果要做相机云台的平滑旋转欧拉角插值在接近万向锁位置时路径会非常诡异甚至出现突然掉头。解决方案就是把姿态全换成四元数做插值四元数的球面线性插值slerp天然没有这个问题。好在自动驾驶车辆本身的运行工况很少到达pitch90°所以万向锁在Apollo的常规运行中不太会真正触发。但如果你的项目里有机械臂、云台、无人机这类大自由度的设备就必须从一开始就用四元数别想着欧拉角更方便了。5. Apollo实战坐标变换链路与验证方法5.1 从传感器原始数据到像素投影的完整链路把前面几节的知识串起来一条完整的坐标变换链路是这样走的激光雷达点云中的一个点在雷达坐标系下的坐标为P_lidar。用激光雷达到车身的外参T_vehicle_lidar把点变换到车身坐标系P_vehicle T_vehicle_lidar * P_lidar。用定位模块输出的车身在世界坐标系的位姿T_world_vehicle把点变换到世界坐标系P_world T_world_vehicle * P_vehicle。如果需要叠加到相机图像上先用车身到相机的外参T_cam_vehicleP_cam T_cam_vehicle * P_vehicle。用相机内参做针孔投影(u, v) K * (P_cam_x / P_cam_z, P_cam_y / P_cam_z)。这条链路在Apollo的很多可视化工具里都在用。平时看数据如果发现激光雷达点云投影到图像上出现偏移基本上就是在第4步或第5步出了问题。5.2 每步变换的矩阵表达和组织方式Apollo代码里的坐标变换一般用4x4齐次矩阵来表达一个完整的变换矩阵长这样R_3x3 t_3x1 0_1x3 1其中R是旋转部分t是平移部分。齐次矩阵的好处是能够把旋转和平移统一成一次矩阵乘法并且可以通过矩阵求逆方便地做反向变换。多个坐标系的级联变换就是矩阵相乘注意乘法的顺序T_a_c T_a_b * T_b_c这表示先做b到c的变换再做a到b的变换也就是右乘。很多人第一次接触这地方会搞混建议自己动手在纸上推一遍把坐标系和矩阵都写清楚。另外还要提一个问题在很多标定结果文件里旋转部分有时给出的是旋转矩阵有时给出的是四元数有时给出的是欧拉角。Apollo通常用四元数存储位姿但标定工具输出的外参可能是欧拉角形式。如果你手动改过标定文件务必保证各种表达方式是一致的否则APollo加载时可能不会报错但坐标全乱了。5.3 时间对齐坐标系变换里的隐藏前提坐标变换还有一个常见隐藏条件就是各传感器的数据必须时间同步。想象一下车身以20m/s的速度行驶传感器数据时间差100ms那么在这段时间里车已经移动了2米。如果拿相机的图像和100ms之前的激光雷达点云做融合即使坐标系变换全部正确融合结果也会有2米的空间误差。Apollo各传感器的话题消息都带有时间戳做坐标变换时最好确认一下数据的采集时间是否接近。在感知融合模块里经常会有一个时间同步的机制把多个传感器的话题按时间戳对齐后再送入融合算法。调试时如果发现投影有拖影或目标错位不要只盯着坐标系看先检查时间戳差了多少。6. 坐标系让我翻车的几个真实教训6.1 毫米还是米单位导致的灾难这个错误看起来低级但实际上非常容易犯。激光雷达点云坐标的单位是米相机标定板物理尺寸的单位通常是毫米或者厘米相机内参里焦距的单位是像素。如果你拿标定板角点的毫米坐标去算外参或者把某个以厘米为单位的坐标直接当成米来用变换出来的结果会偏一两个数量级。我吃过的一次亏是用一个旧的标定文件直接跑新车辆的数据点云和图像的叠加偏离到完全无法对视查了一个下午最后发现是标定文件里的平移单位本来应该是米但生成该文件的工具输出的是毫米。从那以后我就定了个规矩任何外参文件拿到手先看平移量的数值量级。比如车身到相机的平移量如果是[0.5, 0.2, 1.2]这种量级说明单位是米如果是[500, 200, 1200]这种量级说明单位是毫米需要除以1000再使用。6.2 地面真值坐标系基准不一致在评测定位算法精度时经常要拿定位输出和真值做对比。真值可能来自高精地图匹配得到的轨迹也可能来自差分GPS。如果不小心把真值的坐标系基准弄混——比如一个用的是UTM 50N带另一个用的是UTM 51N带——那么两个坐标系的东向值会差几千公里结果误差大得离谱。这个问题在Apollo里也有出现过。因为UTM是按经度分带的车辆跨带行驶时同一个物理位置在50N带的坐标和在51N带的坐标完全不同。跨带的时候不能直接对比坐标值必须先统一坐标系。Apollo的定位模块在跨带时会有特殊的处理逻辑平时我们看单条数据可能留意不到但一旦跨了带所有依赖坐标系一致的算法都要重新检查。6.3 遇到莫名偏移时先查坐标系再查算法这是我想分享的最重要的一条排错经验。很多人在融合结果不对的时候第一反应是调算法参数比如卡尔曼滤波的噪声矩阵、聚类的距离阈值之类。但实际上坐标系变换错误导致的偏差和算法参数导致的偏差看起来非常像都是差一点但对不上。我的排错顺序永远是先查坐标系再查时间戳最后才动算法。具体排查链路是这样的先检查外参是否加载正确打印外参矩阵和标定文件里的数值对比。再检查变换方向用车身坐标系下的已知点投影到传感器看方向对不对。然后检查内部坐标系约定是FLU还是NED欧拉角顺序是什么。接着检查时间戳差两个传感器数据之间的时间差是否可接受。最后才动算法参数。按照这个顺序大部分找不到原因的问题都能在1-2小时内定位。反而是一上来就调算法参数往往调了半天还是不对最后回头发现是坐标系就错了前面的工作全白费。6.4 坐标系不是纯理论而是调试工具回到最开始的问题为什么Apollo要定义这么多套坐标系现在我的理解是坐标系是不同模块之间沟通的共同语言只有共同语言一致整个系统才能协同工作。而对做自动驾驶的人来说坐标系不仅是一个需要用到的知识更是一个强大的调试工具——当你对两路数据的空间关系有疑问时把它们统一变换到同一个坐标系下去对比问题往往立刻水落石出。Apollo学习到这个阶段如果能把每个坐标系的定义、变换关系和常见坑都搞清楚后面看感知融合、定位、规划这些模块的代码都会轻松很多。至少我自己就是在彻底消化了坐标系的这些细节之后才真正敢说我看懂了Apollo的模块间数据流。如果你也正在学Apollo建议不要跳过这一节坐标系的内容虽然不像算法那么炫酷但它是一切上层建筑的地基。