3、核心概念解析：轨迹、位姿、时间戳、坐标系

好，咱们直接进入正题。做轨迹评估，说白了就是跟四个概念打交道：轨迹、位姿、时间戳、坐标系。这四个东西搞不清楚，后面所有分析都是空中楼阁。我见过太多人拿着工具跑出一堆漂亮曲线，结果一问坐标系都没对齐——那数据就是废的。

轨迹是什么？简单说，就是机器人从起点到终点，一路走过来的完整路径。但它不只是路径，还包含了时间信息。

我个人习惯把轨迹想象成「带时间戳的路径」。路径只告诉你「去了哪」，轨迹还告诉你「什么时候到的」。你想想看，两个机器人走同样的路线，一个匀速，一个急停急走，路径一模一样，但轨迹完全不同。

核心公式（非数学专业可跳过）：

Trajectory = { (Pose₀, Timestamp₀), (Pose₁, Timestamp₁), ..., (Poseₙ, Timestampₙ) }

每个时刻，机器人都有一个位姿。把这些位姿按时间串起来，就是轨迹。

我在项目中遇到过一件事：有个同事拿了两条轨迹做对比，一条是激光雷达生成的，一条是视觉里程计生成的。两条轨迹形状看着差不多，但一算误差特别大。后来发现，激光雷达那条轨迹采样频率是100Hz，视觉那条只有20Hz，时间戳根本没对齐。嗯，这就是典型的「轨迹」概念没吃透——你连采样率都不匹配，怎么比？

位姿包含两个部分：位置和姿态。位置就是三维坐标 (x, y, z)，姿态就是朝向 (roll, pitch, yaw) 或者用四元数表示。

说白了，位姿回答了机器人两个问题：

为什么姿态这么重要？我举个例子。你让机器人去抓一个杯子，位置对了但姿态偏了5度，可能就抓空了。做轨迹评估时，很多人只盯着位置误差，忽略了姿态误差。我曾经吃过这个亏——评估AGV的轨迹，位置误差只有2cm，但姿态误差到了8度，结果车子在窄通道里直接卡住。

我的习惯：评估轨迹时，永远把位置误差和姿态误差分开看。位置用RMSE（均方根误差），姿态用角度误差。混在一起算一个分数，反而掩盖了问题。

时间戳是轨迹里最容易忽略、但最要命的东西。没有时间戳，轨迹就是一张静态地图，你根本不知道机器人什么时候加速、什么时候转弯、什么时候停下来。

时间戳有两种常见格式：

格式	说明	典型场景
Unix时间戳（秒）	从1970年1月1日开始的秒数	ROS bag文件、日志文件
相对时间（秒）	从轨迹起点开始的偏移量	离线仿真、后处理分析

这里有个坑：时间戳的精度。我曾经处理过一组数据，时间戳只精确到毫秒，但机器人运动速度很快，两个相邻位姿之间只差了0.3毫秒。结果呢？时间戳重复了，导致轨迹评估时出现「时间倒流」的荒谬情况。嗯，后来我养成了一个习惯——拿到数据先检查时间戳是否严格递增。

避坑指南：我曾经因为时间戳单位搞混，把秒当毫秒用了，算出来的速度大了1000倍。所以，拿到数据第一件事：确认时间戳的单位！

坐标系是轨迹评估里最绕、也最容易出错的概念。同一个位姿，在不同坐标系下看，数值完全不同。

常见的坐标系有：

你想想看，如果激光雷达的数据是在传感器坐标系下，而你要跟全局轨迹对比，不转换坐标系，那误差能大到离谱。我记得有一次做多传感器融合评估，三个传感器各自输出轨迹，结果坐标系没统一，三条轨迹画出来方向都是反的——折腾了两天才发现是坐标系定义不一致。

下面这张图，是我自己总结的四个概念的关系。你看一眼就明白了。

这张图想表达的核心就一句话：轨迹 = 带时间戳的位姿序列，且所有位姿必须统一到同一坐标系下。缺一个，评估就没法做。

最后，我把自己踩过的坑总结一下，你遇到类似情况可以少走弯路：

坐标系没对齐：两个轨迹一个用ENU坐标系，一个用NED坐标系，直接对比。结果误差大得离谱。解决办法：先做坐标系转换。
时间戳不同步：一个轨迹是100Hz，另一个是20Hz，直接插值对比。插值方法不对，误差全被插值算法吃了。解决办法：用线性插值或样条插值，但一定要检查插值后的时间戳是否合理。
位姿表示不一致：一个用四元数，一个用欧拉角。四元数转欧拉角时万向锁问题没处理好。解决办法：统一用四元数做内部计算，最后展示时再转成欧拉角。
忽略姿态误差：只看位置RMSE，不看角度误差。结果轨迹评估分数很好看，实际机器人跑起来歪歪扭扭。解决办法：位置和姿态分开评估，各自给一个指标。

一个小技巧：拿到轨迹数据后，我习惯先画一个「时间戳间隔分布图」。如果时间戳间隔不均匀，说明数据采集有问题，后面评估再漂亮也没用。这个习惯帮我省了至少三次返工。

好了，这四个核心概念就讲到这里。记住：轨迹是骨架，位姿是血肉，时间戳是脉搏，坐标系是灵魂。四者缺一不可，搞清楚了，后面的评估工作才能站得住脚。