Apollo学习笔记（4）：坐标系与欧拉角四元数转换详解

做无人车的人一定听过一句话坐标不对代码白费。我啃Apollo源码的头两周有一半时间耗在坐标系上。这篇笔记既写给正在看Apollo定位、感知、规划模块的同学也写给那些被欧拉角、旋转矩阵、外参标定折磨到想扔键盘的兄弟。项目名字很明确——Apollo学习笔记4坐标系关键词也直白坐标系。但坐标系恰恰是整套自动驾驶系统里最绕、最容易出错又最不能跳过去的地基。定位模块把GPS/IMU转成车身位姿感知模块把障碍物从雷达或摄像头坐标系搬到世界坐标系规划模块再把轨迹从车身坐标系下发到控制模块任何一个环节坐标没对齐车就会像一个方向感失灵的人明明地图是对的却总往沟里跑。这篇笔记适合三类人第一类刚入手Apollo、准备跑Demo但看不懂代码里Quaternion和TransformWrapper的新手第二类正在做多传感器标定、被外参矩阵搞到头大的工程师第三类只是被导师或领导丢了一句“你去把坐标系理清楚”的苦命人。我会先讲清楚Apollo到底有哪几套坐标系再从数学原理拆解欧拉角、旋转矩阵和四元数然后落到代码层面告诉你转换函数在哪里最后用我在实际调试里踩过的坑帮你躲开那些教科书不会写的雷区。1. 先别急着背公式理解Apollo坐标系的设计逻辑1.1 为什么说坐标系是无人车的地基自动驾驶本质上是一个“换了坐标系工作”的过程。车上的传感器各测各的激光雷达告诉你“障碍物在我前方5米偏左0.3米”摄像头告诉你“目标在图像第480行第620列”GPS告诉你“我所在的经纬度是xxx”惯导告诉你“我现在的朝向是北偏东30度”。如果各说各话系统是没法做决策的。所以必须有一套统一的坐标框架把这些来自不同传感器、不同参考点的信息先变换到同一个坐标系里才能做融合、预测、规划和控制。我在带新人时经常打一个比方坐标系就像团队里的沟通语言。中国同事说“往前挪半米”德国同事说“nach vorne einen halben Meter”法国同事说“avancez d’un demi-mètre”意思一样但没法直接配合。先把大家统一到普通话事情才谈得下去。Apollo的统一“普通话”就是车身坐标系、世界坐标系、传感器坐标系之间的转换关系。理解了这一点你再看Apollo的代码就不会觉得TransformWrapper那类东西是莫名其妙冒出来的。1.2 Apollo里到底有哪几套坐标系Apollo里常碰到的坐标系掰着指头数其实不少但核心就四类世界坐标系、车身坐标系、传感器坐标系、图像坐标系。我整理了一张速查表方便你对照坐标系参考点轴方向约定典型用途WGS84经纬度地球质心经度、纬度、高度GPS原始输出、高精地图采集UTM投影坐标各分带中央经线东向E、北向N、高程Z区域级地图、局部路径规划ENU局部坐标系自定义原点X东、Y北、Z天融合定位、运动规划常用车身坐标系FLU后轴中心或车辆质心X前、Y左、Z上底盘信号、控制指令、车身位姿激光雷达坐标系雷达几何中心按安装位置定义感知障碍物点云输出摄像头坐标系光学中心Z前、X右、Y下视觉感知、3D检测图像像素坐标系图像左上角u右、v下2D检测框、目标跟踪这里要特别强调车身坐标系FLUApollo的车辆坐标系就是FLU很多自研系统的习惯是FRDX前、Y右、Z下。FLU与FRD的Z轴相反、Y轴相反如果不小心看错会导致角度计算全部反号。Apollo文档里写的vehicle coordinate system一般默认FLU。FLU的好处是Z朝上和ENU世界坐标系习惯一致转换时少一步翻转坏处是和车辆动力学里常用SAE标准X前、Y右、Z下不完全一致读底盘信号时容易搞混。1.3 数据流向里的坐标系接力我建议你用一条完整的数据流把坐标系串起来看。拿激光雷达感知举个例子真实跑通这条链路是这样的激光雷达输出的是雷达坐标系下的点云坐标我们用外参矩阵把它变换到车身坐标系再用定位模块给出的车辆位姿把车身坐标变换到世界坐标ENU或UTM之后感知模块在世界坐标系下做障碍物聚类和跟踪规划模块在世界坐标系下生成轨迹最后需要下发控制指令时再把目标点从世界坐标系变回车身坐标系。我写代码时习惯把这条链画成一张数据流图雷达原始点云 (雷达系) - 外参T_radar_body (雷达系转车身系) - 车身系点云 - 定位位姿T_body_world (车身系转世界系) - 世界系点云 - 感知障碍物 (世界系) - 规划轨迹点 (世界系) - 逆变换 T_world_body - 车身系控制指令这条链里每一步都是一个坐标系变换漏掉任何一步结果都会“鬼畜”。我在实际调试时见过最典型的问题就是有人把雷达外参标定的结果直接用来转换定位输出的位姿看起来都是矩阵却张冠李戴最后障碍物位置整体偏移几十米。所以学Apollo坐标系脑子里一定要时刻装着这张接力图。2. 坐标系转换的数学原理从欧拉角到四元数2.1 欧拉角与绕固定轴、绕移动轴旋转的区别坐标系转换的本质就是平移加旋转。平移很简单就是两个原点之间的偏移量旋转则复杂得多因为三维空间中绕不同轴的旋转会互相影响。我们常用的欧拉角就是把一个任意姿态拆成依次绕三个轴的转动偏心于Z轴的Yaw、绕Y轴的Pitch、绕X轴的Roll。Apollo里位姿输出经常就是这组数Roll/Pitch/Yaw单位是弧度。欧拉角一个很坑的地方在于绕固定坐标系旋转和绕自身移动坐标系旋转结果完全不同。打个比方你站在操场上面向北先向左转90度再蹲下低头和你先蹲下低头、再向左转90度最后身体朝向不一样。前者是绕固定世界系的轴转后者是绕自己当前朝向的轴转。数学上绕固定轴旋转相当于左乘旋转矩阵绕自身轴旋转相当于右乘旋转矩阵顺序一换结果就变了。很多人在标定时容易在这上面栽跟头因为IMU或者车辆动力学给的角度定义和外参标定工具里的定义未必一致。举个我遇到的真实例子某次标定摄像头外参按工具文档要求依次绕固定轴X、Y、Z旋转但标定板角点怎么都对不齐后来发现是工具内部按Z、Y、X顺序绕自身轴旋转计算两个顺序差了十万八千里。查了半天最后发现欧拉角顺序本身就是外参里最容易埋雷的地方。Apollo官方文档中习惯按Z-Y-X的顺序来描述车身姿态也就是先Yaw再Pitch再Roll这一点在写代码时务必先确认清楚。2.2 旋转矩阵、方向余弦矩阵与坐标变换用欧拉角做计算不直观工程上更常用旋转矩阵也叫方向余弦矩阵。一个三维旋转矩阵R可以把一个向量从一个坐标系变到另一个坐标系p_new R * p_old。旋转矩阵是正交矩阵行列式等于1逆矩阵就是转置矩阵这个性质让它在坐标反变换时特别方便。绕X、Y、Z轴的三个基本旋转矩阵分别是Rx(θ) [1, 0, 0] [0, cosθ, -sinθ] [0, sinθ, cosθ] Ry(θ) [ cosθ, 0, sinθ] [ 0, 1, 0] [-sinθ, 0, cosθ] Rz(θ) [cosθ, -sinθ, 0] [sinθ, cosθ, 0] [ 0, 0, 1]结合前面说的绕固定轴和绕自身轴的区别Apollo里由Yaw、Pitch、Roll合成姿态旋转矩阵一般写作R Rz(yaw) * Ry(pitch) * Rx(roll)。这个顺序相当于先绕自身X轴转Roll再绕当前Y轴转Pitch最后绕当前Z轴转Yaw是标准的“内旋ZYX”。理解了这个顺序再看Apollo代码里Quaternion转欧拉角的公式你就知道为什么有那些加减号了。我建议你手推一遍这个矩阵别只在代码里看结论推过之后你对坐标转换的理解会上一个台阶。2.3 四元数Apollo代码里最常见的姿态表达Apollo定位模块输出的位姿里角度部分用的不是欧拉角而是四元数q [w, x, y, z]。为什么不用欧拉角因为欧拉角有两个硬伤一是有万向锁问题当Pitch接近±90度时Roll和Yaw变得不可分辨求解会退化二是插值困难两个欧拉角之间线性插值中间姿态会乱飘。四元数没有这些问题它相当于用四维单位球上的一个点来表达三维旋转紧凑且无奇异性。四元数转旋转矩阵有标准公式Apollo在modules/common/math/quaternion.h里直接提供了。反过来旋转矩阵转四元数也可以用经典算法关键看对角线元素的大小来选稳定性最好的分支避免接近零时开根号产生数值问题。这些都是造好的轮子你直接调用即可但我强烈建议你把公式在纸上推一遍。我的习惯是开一个Python终端用numpy手写一个Euler转Quaternion的函数然后和Apollo的结果对照import numpy as np def euler_to_quaternion(roll, pitch, yaw): cr, sr np.cos(roll / 2), np.sin(roll / 2) cp, sp np.cos(pitch / 2), np.sin(pitch / 2) cy, sy np.cos(yaw / 2), np.sin(yaw / 2) w cr * cp * cy sr * sp * sy x sr * cp * cy - cr * sp * sy y cr * sp * cy sr * cp * sy z cr * cp * sy - sr * sp * cy return w, x, y, z print(euler_to_quaternion(0.1, 0.2, 0.3)) # 输出可以对照Apollo的EulerAnglesToQuaternion结果跑一遍你就会发现如果不注意欧拉角顺序和旋转方向右手系、正方向逆时针结果可能差一个符号。四元数共轭表示反向旋转很多人漏了这一步导致坐标转换结果震荡。3. 坐标系转换在Apollo代码里怎么落地3.1 关键模块与核心函数位置Apollo的坐标转换代码散落在几个模块中我列一下常用的位置照着找省时间modules/common/math/quaternion.h欧拉角和四元数互转的核心函数比如EulerAnglesToQuaternion、QuaternionToRotationMatrix。modules/transform/cyber里的TF树实现类似ROS的TF负责管理各个坐标系之间的动态变换关系可以随时查询任意两个坐标系之间的转换。modules/localization/proto/localization.proto定位结果的消息定义里面的pose包含position和headingheading本质是Yaw角。modules/perception/各传感器外参的解析和坐标转换比如lidar的hdf5、pcd数据转换时会通过外参把点云转到世界系。modules/transform/transform_broadcaster.cc把定位模块计算出的位姿发布成TF变换供感知、规划模块使用。我刚上手时总在各个模块里翻箱倒柜找“坐标转换”关键字后来发现一条捷径全局搜TransformWrapper或者搜“Eigen::Affine3d”。Apollo内部大量用Eigen的Affine3d表达变换矩阵这比单独旋转矩阵加平移向量更直观。Affine3d本质是一个4x4矩阵左上3x3是旋转右上3x1是平移最后一行是0001齐次坐标表示。用齐次坐标的好处是旋转和平移可以统一成一次矩阵乘法连续变换可以直接乘起来。3.2 TransformWrapper与Pose的用法Apollo里很多坐标转换调用都封装在一个叫TransformWrapper的类里内部维护了源坐标系和目标坐标系的变换关系。实际使用姿势大概是// 伪代码示意 TransformWrapper wrapper; wrapper.Init(lidar, body); // 从lidar坐标系变到body坐标系 Eigen::Affine3d trans wrapper.Transform(); // 对点云里的每个点做变换 auto pt_in_body trans * pt_in_lidar;这里的核心是TransformWrapper返回的Transformation是一个齐次变换矩阵可以直接用Eigen的运算符。如果你拿到的是某个位姿里面有位置和四元数就可以这样构造Eigen::Affine3d pose Eigen::Affine3d::Identity(); pose.translation() Eigen::Vector3d(x, y, z); pose.linear() quaternion.toRotationMatrix();构造完之后pose就是“从自身坐标系到目标坐标系的变换”。用这个方式把定位结果往世界系一推然后把感知的障碍物都用同一个变换转过去整个场景就对齐了。我在写融合模块时经常就是这一套理解之后代码量很少。3.3 自己写一个小工具验证坐标转换光看代码不够我建议你趁热打铁做一个坐标系转换验证工具。思路特别简单在地图上取一个已知点比如某个路口的中心点记录它的UTM坐标和经纬度然后通过定位模块输出的位姿反推这个点在车身坐标系下的坐标再从车身坐标系转回世界坐标系看看能不能回到原地。能对得上说明你调用的转换链路没问题。我做过一个更粗暴但有效的实验把车停在原地手动绕车走一圈在车上不同的传感器位置标记几个点然后从Apollo的感知可视化里看这些点是不是落在实际位置。第一次跑就发现激光雷达点云整体旋转了45度排查了一下原来外参里多了个绕Z轴的旋转量没清零。这种验证方式不用跑复杂仿真只需要一个空旷场地和一只笔但非常能检验你对坐标系的掌握程度。4. 从传感器到地图不同坐标系的打通方法4.1 摄像头图像坐标系与OpenCV的约定摄像头坐标系的坑主要在图像坐标和相机坐标混为一谈。相机坐标系的原点在光学中心Z轴朝前X轴朝右Y轴朝下走的是OpenCV的右手系约定而图像像素坐标系的原点在图像左上角u轴朝右v轴朝下单位是像素。从相机坐标系到像素坐标系需要通过内参矩阵做透视投影[u, v, 1]^T (1/Z) * K * [X, Y, Z]^T其中的K就是内参矩阵包含焦距fx、fy和主点cx、cy。Apollo的摄像头标定延续了这个约定所以你在处理YOLO检测结果或者摄像头3D检测时一定要搞清楚网络输出是像素框还是3D框在相机坐标系下的坐标。直接把2D像素坐标当成3D坐标去和激光雷达点云对齐是我见过最多的错误没有之一。我处理视觉和雷达融合时会写一个转换流程先把3D雷达点在激光雷达系下通过外参变到车身系再变到摄像头坐标系最后投影到像素坐标系判断投影点是否落在检测框里。整个流程里最容易被忽略的是摄像头外参的旋转部分因为摄像头Y轴朝下和车身FLUY轴朝左差一个轴的翻转。你要是直接把FLU下的平移向量用作相机外参平移视觉投影一定会偏。4.2 激光雷达外参标定从雷达系到车身系激光雷达的外参标定本质就是求一个从雷达坐标系到车身坐标系的齐次变换矩阵T_radar_body。这个矩阵包括三维平移和三维旋转也就是6个自由度。最传统的方式是拿标定板或者场地里的参照物分别量出它们在雷达系和车身系下的坐标然后求解刚体变换。Apollo提供了一套标定工具但原理还是最小二乘求解匹配点对。我自己在项目里发现一个更简单的土办法把车停在一个规则直角墙边雷达扫出来的墙面点云如果外参正确点云应该和墙面完全重合如果不正确墙面就会偏转或错位。通过调整外参里的Roll、Pitch、Yaw让墙面点云与RTK打出来的墙线对齐反复迭代几次效果并不差。这个方法虽然没有专业标定那么精确但作为快速排查工具五分钟就能发现外参是差在平移还是差在旋转。标定还有一个特别容易翻车的地方激光雷达坐标系原点和车身后轴中心之间的平移量。很多人拿了个外参能对得上局部点云但在车辆转弯时就露馅因为平移误差会随着旋转被放大。所以标定之后一定要做动态验证直行、绕圈、走S形确认远处点云不飘。转弯时刻才是外参平移和旋转精度的试金石。4.3 GPS和UTM从WGS84经纬度到局部平面坐标Apollo的高精地图和定位模块都有经纬度但路径规划不能直接在经纬度上算直线距离因为纬度线在不同经度的长度不同直接用经纬度做欧氏距离计算会出笑话。普遍做法是把WGS84经纬度投影到UTM坐标系或者一个自定义的本地平面坐标系。UTM把地球分成60个投影带每带6度经宽分带内坐标是米误差能控制在厘米到分米级别能满足自动驾驶区域路径规划的需求。如果你处理的不是标准WGS84而是国内测绘成果比如栅格影像需要从WGS84转到CGCS2000注意这两个椭球体之间有几十厘米级别的框架差异简单加平移参数是不够的通常需要七参数布尔莎模型。这一点在做地图采集和国内项目对接时容易遇见。Apollo本身默认按WGS84/UTM体系来但你要是接国内测绘地图就要在数据预处理阶段把坐标系统一好否则地图和定位会有“系统性的对不齐”。我写过一个Python脚本用pyproj库批量把CSV里的经纬度点转成UTM或CGCS2000投影坐标再输出成KML和Shapefile检查。类似的需求网上常搜“coord怎么导入csv或txt做坐标转换”本质就是用现成的GIS库把批量点位文件读进来定义好源坐标系和目标坐标系一次性转完再导出。不要一个个点手工转浪费时间还容易出错。# 批量坐标转换示意WGS84 - UTM Zone 50N from pyproj import Transformer transformer Transformer.from_crs(EPSG:4326, EPSG:32650, always_xyTrue) # 假设读取CSV中的经纬度列表 lon_lat_pairs for lon, lat in lon_lat_pairs: easting, northing transformer.transform(lon, lat) print(easting, northing)这种工具脚本在数据预处理中很实用。Apollo真车上跑的时候定位模块内部已经打理好了WGS84和UTM之间的投影你不用每帧都手动转但理解和验证流程能在出问题时快速定位是投影参数设错了还是数据本身不对。4.4 坐标系绕任意轴旋转与罗德里格公式除了绕X、Y、Z三个基本轴旋转工程上有时需要把点云绕空间中任意一个轴旋转。比如雷达安装面本身不是水平面点云整体倾斜你既可以用三个欧拉角分步纠正也可以用罗德里格旋转公式一步到位。罗德里格公式给定单位旋转轴n [nx, ny, nz]和旋转角θ旋转矩阵为R cosθ * I (1 - cosθ) * n * n^T sinθ * [n]_x其中的[n]_x是轴向量构成的反对称矩阵。这个公式看着抽象但代码实现很简单numpy里几行就能写完。实际调激光雷达时如果发现点云整体绕某一个方向轴旋转用这个公式把修正矩阵算出来乘到外参上比盲调三个欧拉角快得多。需要提醒的是坐标轴方向的约定会影响“从哪儿转到哪儿”的符号。Apollo用的是右手坐标系绕轴旋转的正方向遵循右手定则拇指指向轴正方向四指弯曲方向为正。同一套数据在左手系和右手系里转同一个角度结果可能方向相反。遇到方向不对先检查是不是坐标系左右手搞错了别急着加负号。5. 调试中常见的坑坐标跳变、万向锁与分带误差5.1 欧拉角跳变与万向锁问题我在跑定位的时候遇到过一个问题车辆静止不动但输出的Yaw角在某个瞬间突然从179度跳到-179度或者从90度跳到270度。这不是定位坏了而是角度表示方式在边界处的正常现象。欧拉角是周期性的超过±π就会回绕。回绕在数学上正确但对下游模块不友好比如规划模块做轨迹平滑时目标点角度从179度变到-179度插值会走过358度产生一个大回旋。处理办法有两种一是尽可能在传递链路里用四元数而不是欧拉角Apollo内部也是这样做的二是在使用欧拉角的边界位置做角度展开比如当相邻两帧角度差超过180度时认为发生了回绕把新的角度加减360度保证连续性。这个逻辑写起来不难但容易忘忘一次就会被一个莫名其妙的掉头逻辑折磨一整天。万向锁问题的根源是欧拉角退化当Pitch等于±90度时Roll和Yaw的旋转轴重合自由度丢失矩阵分解得到的欧拉角会剧烈抖动。这种情况在无人车正常行驶时很少出现但如果车辆停在坡上或者遇到特殊姿态IMU解算出来就可能触发。定位模块一般用四元数做状态量就是为了避开这个问题。5.2 外参互转方向搞错导致坐标飘我反复强调绕固定轴和绕移动轴的区别是因为这是外参标定里最高频的坑。很多标定工具输出的欧拉角顺序各不相同有的按ZYX有的按XYZ有的按YPR有的先绕固定轴有的先绕自身轴。你用Apollo的EulerAnglesToQuaternion之前一定要先确认你手上的欧拉角是按什么顺序、什么旋转方向算出来的。这个确认工作花五分钟能省后面五小时的查错时间。一个非常实用的排查技巧是把已知点从源坐标系转到目标坐标系再逆变换回来看能不能回到原始坐标。如果两次变换不能还原说明你的变换矩阵用反了。齐次变换矩阵的逆不是随便转置一下就行旋转部分可以转置但平移部分要带上旋转一起计算即T_inv的平移部分是 -R^T * t。直接把4x4矩阵取逆是最简单稳妥的办法Eigen里一行trans.inverse()搞定但很多人图省事只对3x3旋转转置忘了平移项结果坐标偏移越来越大。5.3 UTM分带边界和CGCS2000的锅UTM分带在跨带区域会让坐标出现比较明显的跳变。比如车从第50带开到第51带如果地图和定位用的带号不一致坐标会瞬间跳到一千公里开外。Apollo的定位模块一般会固定在一个投影带内运行但你要是在跨省测试或者接的是全国范围的地图就不得不处理分带切换问题。我的建议是在区域边缘预留重叠缓冲区或者在全局框架里直接选一个覆盖全项目的自定义中央经线做投影而不是死守标准UTM。另外WGS84和CGCS2000之间虽然差异小但在厘米级定位需求下不能忽略。网上常有人问“栅格影像WGS84转CGCS2000怎么转”本质上就是两个椭球基准之间做七参数转换。如果只是出图肉眼看不出来如果要做高精地图和RTK定位对齐那就是“差之毫厘谬以千里”直接导致车道级定位偏移半个车身。项目开始前先确定统一的坐标系基准比事后折腾转换要省心得多。5.4 外参标定错误导致的感知“鬼影”现象最后分享一个让我印象深刻的调试案例。一次路测激光雷达识别前方有车但视觉模块在同一位置没有检测到目标打开可视化工具一看雷达点云和摄像头画面里车道线位置相差半个车道点云里的“车”像鬼影一样飘在路边。后来检查发现雷达外参里的Z轴平移填反了符号导致点云整体下移了十几厘米再叠加一点Pitch误差远处就偏了大半米。这类问题最迷惑的地方在于近距离目标看起来都正常远距离才暴露。原因是旋转误差会随着距离放大线性误差叠加在远距离上的偏移更明显。所以标定完外参一定要跑一段包含远距离目标的路段验证而不是只对着停车场的墙看近距离重合。外参验证的标准我建议至少做到100米距离上点云和实际目标偏差不超过0.3米这个量级才能满足车道级规划的需求。排查坐标问题时我的顺序永远是先确认源坐标系定义再确认目标坐标系定义然后检查变换顺序最后验证往返还原。这个顺序看着老土但每次都能让我快速定位到问题是在数学上还是在代码上。我不止一次看到有人熬到半夜调坐标最后发现只是某一行旋转矩阵转置写错了。学习Apollo坐标系最忌讳的就是把别人的坐标转换代码抄来直接用而不去理解背后的坐标定义和旋转顺序。我自己的体会是花一个下午把欧拉角、旋转矩阵、四元数互推一遍比刷十篇博客都有用。等你想清楚了“从哪个系到哪个系”“先转哪个轴再转哪个轴”后面看感知融合、规划控制的代码都会顺畅不少。