2. 轨迹基础
轨迹质量评估的第一步，不是急着算误差，而是先弄清楚“轨迹从哪里来、长什么样、在时间和空间上有什么脾气”。同一段轨迹，来自RTK、激光SLAM、视觉里程计或手机GNSS，其误差结构、采样节奏和失效模式完全不同。如果忽略数据来源与表示形式，后续的平滑、对齐和评分都会建立在错误假设上。
一、轨迹数据来源：不同传感器决定误差底色
工程中常见的轨迹来源可分为卫星定位、惯性/轮速递推、激光/视觉SLAM、组合导航以及人工标注或仿真真值。它们输出的轨迹虽然都能写成“时间戳+位姿”，但内部误差机制差异很大。
- GNSS/RTK：绝对位置稳定，但受遮挡、多路径影响会出现跳点；RTK固定解与浮点解精度差异显著。
- IMU+轮速：短时平滑，长期存在漂移；航向误差会随距离累积成横向偏移。
- 激光SLAM：在结构化环境中局部精度高，但长廊、隧道、空旷场景可能退化，导致尺度漂移或Z轴漂移。
- 视觉里程计：纹理丰富时精度好，弱纹理、快速运动、光照突变时容易丢失或产生尺度跳变。
- 组合导航：通过滤波融合多源，输出连续，但融合权重和延迟会引入新的动态误差。
二、表示形式：从连续位姿到离散序列
轨迹在数学上可视为时间参数化的位姿函数 T(t) = [x(t), y(t), z(t), q(t)]，工程中通常离散为带时间戳的位姿序列。常见表示包括：
- 位姿序列：每帧包含 timestamp、position(x,y,z)、orientation(qx,qy,qz,qw)，适合SLAM和组合导航。
- 经纬高序列：GNSS常用，需投影到局部ENU或UTM坐标系后才能与地图对齐。
- 里程计增量：以Δs、Δθ表示，适合轮式机器人，但需积分才能得到全局轨迹。
- 控制点/样条：用于路径规划或平滑表达，评估时需重采样到统一时间轴。
不同表示形式之间的转换会引入误差。例如经纬高转ENU时，投影原点选择不同会导致整体平移；四元数与欧拉角互转时，万向锁附近会出现数值不稳定。因此，评估管线中应固定坐标系和插值方式。
# 典型轨迹记录格式（TUM风格）
timestamp tx ty tz qx qy qz qw
1700000000.123 12.345 6.789 0.123 0.0 0.0 0.707 0.707
1700000000.223 12.456 6.801 0.124 0.0 0.0 0.708 0.706
三、时空特性：采样、同步与运动约束
轨迹不是普通点云，它同时具有时间维和空间维。评估时必须关注以下特性：
- 采样频率：GNSS常为1~10Hz，IMU可达100~1000Hz，SLAM输出10~30Hz。频率不一致时需重采样，但插值会平滑掉高频抖动。
- 时间同步：多传感器轨迹若存在毫秒级时间偏移，在高速运动下会转化为米级空间误差。
- 运动连续性：真实轨迹的速度和加速度有物理上限，突变往往意味着噪声或异常。
- 空间自相关：相邻轨迹点误差高度相关，不能简单当作独立样本做统计检验。
四、常见噪声与异常模式
轨迹噪声可分为随机噪声和系统性异常。随机噪声表现为高频小幅抖动，系统性异常则表现为跳点、漂移、尺度错误、时间戳错乱或轨迹断裂。
| 异常模式 | 典型来源 | 表现特征 | 检测线索 |
|---|---|---|---|
| 跳点/离群点 | GNSS多路径、SLAM误匹配 | 单帧位置突变数十米 | 相邻点速度超物理上限 |
| 累积漂移 | IMU积分、里程计 | 回环处无法闭合 | 起点终点偏差随距离增大 |
| 尺度错误 | 视觉单目、轮速标定 | 轨迹整体放大或缩小 | 与真值长度比偏离1 |
| 时间戳错乱 | 多传感器触发不同步 | 时间倒序或重复 | dt为负或为0 |
| 轨迹断裂 | 跟踪丢失、信号遮挡 | 序列中间出现大间隔 | 相邻时间差远大于采样周期 |
下面这张图把轨迹从数据源到异常模式的整体关系梳理出来，便于在评估前快速定位问题来源。
五、案例：同一段轨迹的三种异常叠加
某园区配送机器人同时使用RTK和激光SLAM。回放数据发现：RTK在楼宇间出现三次跳点，每次约15米；SLAM在长廊中航向缓慢偏转，200米后横向漂移约3米；融合模块因时间戳未对齐，在转弯处产生约0.5秒延迟。若直接计算ATE，RMSE被跳点主导，掩盖了漂移问题。正确做法是先按异常模式分段：跳点段标记为无效，漂移段单独评估RPE，时间延迟段检查对齐参数。
六、复盘：建立轨迹质量档案
每段轨迹进入评估管线前，建议生成一份质量档案，至少包含：来源与传感器配置、坐标系与时间基准、采样频率与时间跨度、异常模式清单、有效段与无效段划分、重采样与对齐策略。这样后续的ATE、RPE、闭环误差才有可比性，也才能在排障时快速回溯到数据源头。
- 来源不明或元数据缺失的轨迹，不应直接参与横向对比。
- 异常段应先标记再评估，避免污染整体统计量。
- 时间同步检查应优先于空间精度计算。
- 所有插值和滤波操作必须记录参数，保证可复现。
轨迹基础工作的目标，是让后续的误差指标建立在干净、一致、可解释的数据之上。只有先看清轨迹的来路和脾气，质量评估才不会变成数字游戏。