前段时间我在整理一批路测数据文件名全是带ego前缀的序列文件GNSS、IMU、轮速计、局部路径点混在一起时间戳有 12 位的也有 10 位的坐标系有车体系也有地图系。组里新来的同学拿到这批数据问我ego 数据到底怎么用我当时没有直接回答而是把这套处理过程沉淀成了一个叫 EgoWAM 的小工具链。这篇文章就是把 EgoWAM 的核心用法拆开讲一遍它不解决感知算法也不替你做决策它就是回答一个问题——当手里只有一堆自我车辆数据时如何高效、不犯错地把数据处理成能直接用于分析、回放和规划的东西。如果你在做自动驾驶、移动机器人、智能驾驶仿真或者只是最近在折腾 ROS2 bag 里的自车数据这篇文章会给你一个可以直接抄的落地流程。1. Ego数据究竟指什么先搞清处理对象很多人在第一步就栽了拿到一个名为ego_vehicle_data.csv的文件打开一看几十个字段不知道该信谁、该用谁。其实 ego 数据的边界非常明确它描述的是以自车为中心的所有状态量而不是环境里的障碍物或车道线。1.1 实际项目中 ego 数据都包含哪些内容我遇到过的最典型 ego 数据至少包括这几类位姿和时间时间戳、经度、纬度、海拔、航向角、横滚、俯仰、四元数底盘信号轮速脉冲、转向角、挡位、油门开度、制动压力惯性测量IMU 的三轴加速度、三轴角速度、温度定位融合结果卡尔曼滤波后的 x/y/z、速度、协方差矩阵规划结果全局路径、局部轨迹、期望速度曲线、期望角速度这些数据往往来自不同的传感器和不同的处理器发布频率完全不一样。GNSS 通常是 10HzIMU 是 100Hz 到 200Hz轮速计可能只有 50Hz而规划模块可能只有 20Hz。1.2 为什么 raw bag 里的 ego 数据一团乱麻直接打开原始 bag 文件看你看到的不是一个整齐的时间表而是一条条互相穿插的 topic 消息。最让人头疼的问题有三个。第一个是时间戳不一致。有的传感器时间戳是 UTC 秒有的是 Unix 毫秒IMU 则经常自带一个传感器固有延迟。第二个是坐标系混乱。GNSS 给出的是经纬高但规划模块要的是以地图原点为基准的局部坐标IMU 又是在车体坐标系下读数的三个坐标系之间的外参如果不清楚数据就是废的。第三个是频率不同步。直接按时间先后把数据拼成一张大表后面的分析一定会出现时间上错位的问题。1.3 EgoWAM 为什么不直接打开 bag 给你看EgoWAM 的处理思路是先把原始 bag 或原始文件里的 ego 相关消息全部抽出来统一清洗成结构化的时间序列表再做时间对齐和坐标变换最后输出成标准格式。它关心的不是消息流长什么样而是自车状态随时间变化成什么样。这也是我把它定位成工具链而不是播放器的原因。2. 上手EgoWAM需要什么配置环境和数据准备EgoWAM 不是一个重型平台不需要 GPU也不需要搭集群。它就是一个偏底层的 Python 工具集需要的东西很朴素。2.1 环境依赖和安装我的建议是用 Python 3.10 或更高的版本配合 ROS2 环境如果你是要直接读 bag 的话。核心依赖是这些numpy矩阵运算和四元数处理pandas时间序列表操作matplotlib轨迹和曲线可视化pyarrow列式存储导出ParquetrosbagsROS1/ROS2 bag 的读取比如 Humble 版本安装 EgoWAM 本体我用的是pip install egowam不过它本质上是一组命令行的集合。装完之后你会在环境里获得三个子命令egowam info查看数据概况、egowam extract抽取并清洗、egowam viz可视化。2.2 准备一份测试 ego 数据如果你手头暂时没有现成的路测数据我建议先构造一份简单的测试数据把流程跑通。这里我给一个生成样例数据的脚本字段参考真实路测环境import csv import math import random with open(ego_demo.csv, w, newline) as f: writer csv.writer(f) writer.writerow([timestamp_ms, lat, lon, imu_ax, imu_az, yaw, vx]) t0 1700000000000 # 毫秒 lat0, lon0 31.2304, 121.4737 yaw 0.0 for i in range(1000): t t0 i * 100 # 100Hz v 5.0 0.5 * math.sin(i / 100.0) yaw 0.001 * math.cos(i / 200.0) # 用航向和速度计算一个简单轨迹 lat0 (v * 0.1 * math.cos(yaw)) / 111320.0 lon0 (v * 0.1 * math.sin(yaw)) / (111320.0 * math.cos(math.radians(31.23))) writer.writerow([t, lat0, lon0, 0.1, 9.8, yaw, v])这份数据模拟了一辆直线带轻微转向的车辆GNSS 位置、IMU 加速度、航向和速度都在一个表里。真实路测数据一定比这个乱但处理流程是相通的。2.3 先用 info 命令看清数据全貌拿到数据不要急着画图。先用egowam info看一遍字段的时间范围、频率和缺失率egowam info --input ego_demo.csv --time-col timestamp_ms它会把时间戳格式自动识别出来告诉你是毫秒还是微秒每个字段的缺失百分比、数值范围都会列出来。这一步能帮你提前发现单位不一致的问题。我见过太多人直接拿经纬度和规划 x/y 混着算最后画出全球轨迹的情况。3. EgoWAM读取ego数据的核心流程从原始帧到结构化表这个阶段是 EgoWAM 的重头戏。所谓读取数据不是把文件读进内存而是完成时间对齐、坐标变换、轨迹构建这三步清洗动作。3.1 时间戳对齐不同频率怎么合并成一张表假设你的 GNSS 是 10HzIMU 是 100Hz轮速是 50Hz直接拼接成一条按时间排序的长表是可以的但如果你想做每个 GNSS 时刻对应的 IMU 值是多少这种操作就需要按主时间戳做最近邻对齐或插值对齐。更实际的做法是滑窗平均或者线性插值。EgoWAM 默认采用最近邻 前向填充策略以频率最低的传感器时间戳为准其他高频变量取最近的那一帧值。这个策略在大多数场景下误差足够小而且能避免插值带来的伪数据。示例代码如下import pandas as pd df pd.read_csv(ego_demo.csv) df[timestamp_ms] pd.to_datetime(df[timestamp_ms], unitms) df.set_index(timestamp_ms, inplaceTrue) # 重采样到 20Hz df_resampled df.resample(50L).asfreq().ffill().dropna() print(df_resampled.head())这里50L表示 50 毫秒一个点。要注意的是如果原始数据是 100Hz你降到 20Hz 不会丢失太多信息但如果原始数据只有 5Hz就不要硬凑 20Hz。3.2 坐标系换算经纬度、局部坐标、车体系别搞混处理 ego 数据一定要在脑子里有一张坐标系清单。最常见的三套坐标是 WGS84 经纬高、以地图原点为原点的局部笛卡尔坐标系、以车辆后轴中心为原点的车体坐标系。局部坐标系的转换一个常用做法是以第一帧经纬度为原点用等距投影近似import math import numpy as np def gps_to_local(lat, lon, lat0, lon0): dx (lon - lon0) * 111320.0 * math.cos(math.radians(lat0)) dy (lat - lat0) * 111320.0 return np.array([dx, dy, 0.0]) lat0, lon0 df[lat].iloc[0], df[lon].iloc[0] local_xy np.array([gps_to_local(r.lat, r.lon, lat0, lon0) for r in df.itertuples()])如果你的定位模块已经输出了 x/y那就不用再做这个转换但要确认 x 轴朝东还是朝北、y 轴朝北还是朝东。不同设备厂家的默认定义不一样。EgoWAM 里通过--xy-order xy参数来纠正这种顺序问题。车体坐标系下的 IMU 数据要转成全局坐标系需要用到航向角。比如汽车在车体系下的纵向加速度 ax转到全局坐标系就是global_ax imu_ax * math.cos(yaw) - imu_ay * math.sin(yaw) global_ay imu_ax * math.sin(yaw) imu_ay * math.cos(yaw)四元数版本就是构造旋转矩阵原理一样只是不要手动去解欧拉角直接用scipy.spatial.transform.Rotation最稳。3.3 轨迹构建位置、速度、行驶距离一次性算齐ego 数据最终要能回答三个问题车在哪里、车速多少、这一段开了多远。EgoWAM 在清洗之后会自动累加航向、速度、位移和累计里程df[dt] df.index.to_series().diff().dt.total_seconds().fillna(0) df[dx] df[vx] * df[dt] * np.cos(df[yaw]) df[dy] df[vx] * df[dt] * np.sin(df[yaw]) df[cum_dist] np.sqrt(df[dx]**2 df[dy]**2).cumsum()这里有一个隐含假设速度方向等于航向方向。这种情况适合阿克曼转向的车如果是四足机器人、双轮差速机器人就得单独处理线速度和角速度的关系。千万不要盲目复用同一个公式。3.4 把清洗后的数据导出成通用格式处理完的数据建议导出两种格式。一种是Parquet供后续批量分析和机器学习使用另一种是GeoJSON供 GIS 或地图工具直接叠加显示。egowam extract --input ego_demo.csv --output ego_clean.parquet egowam export --input ego_clean.parquet --format geojson导出的 GeoJSON 里会包含一条LineString轨迹和若干Point信息点每个点带时间、速度、航向。这样做的好处是你可以把处理后的 ego 数据直接丢进 QGIS 或者 Web 地图里做可视化不必依赖专用平台。4. 把ego数据变成判断依据可视化与一致性检查清洗完之后最直接的价值就是一眼看出数据有没有问题。EgoWAM 的可视化模块其实非常简单但非常实用包含三个维度。4.1 规划轨迹和实际轨迹的对比在路径规划系统的调试中我常做的一件事是画双轨迹规划器输出的参考轨迹是虚线车辆实际行驶轨迹是实线。两者偏差超过一个阈值就说明控制或者定位有问题。import matplotlib.pyplot as plt plt.plot(plan_x, plan_y, --, labelplan) plt.plot(real_x, real_y, labelego) plt.legend() plt.axis(equal) plt.show()这里有个细节axis(equal)必须加否则 x 轴和 y 轴比例不一致圆形轨迹看起来像椭圆容易误判横向误差。4.2 IMU 和轮速的一致性校验把 IMU 解算出的速度曲线和轮速计换算出的速度曲线放在同一张图里观察它们是否一致。正常情况下两者趋势相同但会有小的纹波。如果出现持续背离基本可以断定 IMU 安装角度标定错误或者轮速计的有效滚动半径错了。更麻烦的是 IMU 加速度里的零偏。静止时 IMU 加速度应该只有重力分量如果水平静止时 z 轴不是 9.8 附近读取到的 ax、ay 又不接近 0那说明传感器没有水平安装或者存在零偏。EgoWAM 会在可视化时把静止段的加速度均值自动算出来作为零偏估计提醒。4.3 逐帧回放EgoWAM 自带的轻量级 ego browser很多人以为数据可视化就是画曲线但调试定位问题的时候曲线看不出先向左偏再向右偏这种空间问题需要逐帧看车辆位姿。EgoWAM 配套了一个极轻量级的逐帧查看器名字叫ego browser它不是浏览器而是一个单窗口、逐帧绘制自车位姿和局部障碍物的小工具。启动命令很简单egowam viz --input ego_clean.parquet回车之后你可以用空格键播放/暂停左右方向键逐帧移动。窗口里会画三样东西自车矩形轮廓、前向路径、速度指示条。这个工具在跑仿真数据对比的时候特别有用比把视频录下来再一帧帧看效率高很多。5. 和ego planner联动清洗后的ego数据到底给谁用数据处理本身不是目的ego 数据最终要反馈给规划、仿真或者测评模块。EgoWAM 的输出格式之所以选 Parquet 和 GeoJSON就是为了方便下游直接消费。5.1 作为规划模型输入的真值轨迹在基于学习的方法里经常需要把人类驾驶的 ego 轨迹作为模仿学习的真值。EgoWAM 清洗后的轨迹可以直接转成 planner 需要的轨迹格式关键字段包括时间戳、x、y、yaw、vx、vy、加速度。如果你的 ego planner 是在仿真环境里做的还可以把路测采集的真实 ego 数据做成一个数据回灌器把车辆真实位姿按时间戳注入到规划器的状态栏里用来验证规划器在给定真实状态下的响应。这种方法跑回归测试比纯随机仿真靠谱得多。5.2 用 ego 数据做参数标定和传感器校验很多标定工作的一半时间浪费在数据清洗上。比如标定 IMU 到车体的安装角你需要一段静止数据求重力方向、一段直线加速数据求前向方向、一段转弯数据求横摆方向。EgoWAM 里有现成的静态区段提取指令egowam segment --input ego_clean.parquet --static-threshold 0.1它会自动把速度低于阈值的连续区段切出来标注为静止段方便你直接取均值做零偏估计。里程计标定也一样。利用清洗后的 GNSS 位置和轮速累计里程做线性回归就可以估算轮速脉冲对应的实际滚动半径。这个操作如果直接用原始数据做常会被异常点干扰但清洗之后拟合出来就非常稳定。5.3 在线部署时 EgoWAM 的轻量化思路有人会问EgoWAM 是不是只能在离线阶段用其实它的算法核心只是时间对齐、坐标变换和状态估计计算量很小。我在实际工程里会把 EgoWAM 的算法包拆出来做成 ROS2 节点在每个回调里维护一个滑窗窗口长度取 50 个时间对齐后的状态点每个新帧只更新窗口末尾并作滑动窗口去抖坐标系变换用预变换矩阵避免每帧重新构造这样单帧处理时延能控制在 1 毫秒以内。当然在线版本一般不输出可视化只输出清洗后的状态量给下游规划和控制。6. 踩坑记用EgoWAM处理ego数据时遇到的实际问题最后这部分是我认为真正有价值的部分。EgoWAM 开发初期我在处理各种路测数据时踩了一堆坑每一个都花了不少时间排查。挑几个有代表性的写在这里。6.1 时间戳是毫秒还是微秒差点毁掉整条轨迹有一次我用一批数据画轨迹发现车辆位置每隔几十秒就跳变一次。排查了很久最后发现文件里的时间戳其实是微秒但被当成毫秒处理了。相差 1000 倍导致部分数据在时间上被错误地折叠到一起。现在 EgoWAM 的info命令会通过时间戳位数和相邻差值的分布自动判断单位。但你自己写脚本的时候还是要在开头就固定好单位并在整个管线里统一使用int64纳秒或int64毫秒中途不要混用。6.2 经纬度转局部坐标时的投影基准如果数据范围横跨几十公里用等距投影做的局部坐标会有明显变形。我在一条长直线测试道上就吃过亏起点和终点相距 20 公里直接用第一节的近似公式轨迹末尾会偏离真实值几米到几十米。解决办法是改用 UTM 坐标投影或者先挑一个中间点作为投影原点而不是用第一个点。EgoWAM 提供--proj-center参数可以指定一个先验参考点。如果你的数据只在园区几百米范围内跑那等距投影完全够用。6.3 IMU 加速度积分累计漂移很多人拿到 IMU 加速度就想着积分成速度再积分成位置。理论上可行实际上一分钟内就会漂到离谱。EgoWAM 默认不直接使用 IMU 积分结果作为位置输出而是用 GNSS 位置计算轨迹用航向角做速度分解。IMU 只用来做短时间的高频插值。如果你确实要做惯性导航实验至少要做两步先用静止段数据估计加速度零偏再用轮速计/GNSS 做零速修正。否则积分出来的轨迹只能当定性参考不能用于定量评估。6.4 可视化时镜像翻转的错觉画车辆轨迹时如果 x 轴朝东、y 轴朝北但屏幕坐标是 x 轴朝右、y 轴朝上曲线方向是一致的。但你从某些数据源拿到的可能是像素坐标或者前视相机归一化坐标画出来轨迹就是镜像的。判断方法很简单在轨迹起点画一个朝北的箭头如果箭头往上坐标系就是正常的如果往下或朝左就把某个轴取反。这种问题通常在单独看数据时发现不了一旦和地图叠加就非常明显。所以我的习惯是任何 ego 数据可视化第一件事先在地图上叠加一个起点和终点的标记确认车辆是从左下向右上走的再做分析。最后再分享一个小技巧处理 ego 数据之前先把时间基准、坐标系约定、单位约定三样东西写在一个注释文件里和原始数据放在一起。工具能帮你清洗数据但填不了缺席的先验信息。把这套流程跑熟练之后你会发现自己拿到任何 ego 数据都能在十几分钟内回答出这条数据能不能用、哪里有问题、后面的规划器能不能吃进去这三个核心问题。
到std::chrono，避开性能分析陷阱)
