一、轨迹评估基础：为什么需要评估轨迹？

做SLAM这么多年，我经常被问到这样一个问题：「我的算法跑起来看着挺稳的，到底好不好？」

嗯，这个问题其实没那么简单。你想想看，机器人明明在走廊里走直线，但轨迹画出来却像喝醉了酒——这算好还是不好？

说白了，轨迹评估就是给SLAM系统「打分」的。没有评估，你根本不知道自己的算法到底行不行。

1.1 为什么非评估不可？

我在项目里遇到过好几次这样的情况：算法在仿真环境里跑得飞起，一上真机就翻车。为什么会这样？因为仿真环境没有真实传感器噪声，没有光照变化，没有地面打滑。

评估轨迹有三大核心目的：

• 量化精度：你的定位误差是5厘米还是50厘米？没有数字，全是感觉
• 发现漂移：长时间运行后，轨迹是不是慢慢偏了？偏了多少？
• 对比算法：ORB-SLAM3和VINS-Mono，到底谁更适合你的场景？

1.2 绝对轨迹误差（ATE）是什么？

ATE，全称是Absolute Trajectory Error。我习惯叫它「绝对偏差」。

它的思路很简单：把估计轨迹和真实轨迹对齐，然后逐帧算位置差。

打个比方：你让机器人从A点走到B点。真实轨迹是一条直线，但算法估计出来的轨迹可能弯弯曲曲。ATE就是算每个时刻，估计位置和真实位置之间的距离。

数学上长这样：

ATE = sqrt( (x_est - x_gt)² + (y_est - y_gt)² + (z_est - z_gt)² )

其中(x_est, y_est, z_est)是估计位置，(x_gt, y_gt, z_gt)是真实位置。

我记得有一次做AGV导航评估，ATE算出来是0.12米。客户问：「这算好还是差？」我说：「看场景。仓库通道宽2米，0.12米完全够用。但如果是手术机器人，0.01米都嫌大。」

1.3 相对轨迹误差（RPE）是什么？

RPE，Relative Pose Error。我管它叫「局部抖动」。

ATE看的是全局偏差，RPE看的是局部一致性。你想想看，如果机器人每走1米就偏2厘米，走100米就偏了2米——ATE会很大，但RPE能告诉你这个「每米偏2厘米」的规律。

RPE的计算方式：

RPE = 相邻两帧之间的位姿变化量 - 真实位姿变化量

说白了，就是看你的算法在短时间内的相对运动准不准。

我曾经调试一个视觉SLAM系统，ATE看起来还不错，但机器人走起来总是一顿一顿的。一查RPE，好家伙，每帧之间的旋转误差高达0.5度。这就是典型的「全局还行，局部抖动」。

1.4 ATE vs RPE：什么时候用哪个？

指标	关注点	适用场景	典型值（好）
ATE	全局位置精度	导航、建图、回环检测	< 0.1m（室内）
RPE	局部运动一致性	VIO、里程计、平滑性评估	< 0.02m/m

我个人的经验是：

• 做导航规划，优先看ATE——你关心的是终点准不准
• 做建图或AR，优先看RPE——你关心的是局部一致性
• 做完整SLAM系统，两个都得看——缺一不可

1.5 知识体系总览

下面这张图是我自己总结的轨迹评估知识框架，你可以对照着看：

1.6 实际项目中的经验

最后分享一个真实案例。去年我做了一个仓储机器人项目，用的是轮式里程计+IMU融合。一开始ATE是0.3米，客户说不行。我查了RPE，发现每米误差0.05米——这意味着走6米就偏了0.3米。

问题出在哪里？轮子打滑。我加了一个打滑检测模块，RPE降到了0.02米/米，ATE也降到了0.08米。你看，RPE帮我们定位了问题根源。