4、抖动量化指标：用数字说话

做轨迹优化，最怕什么？

怕「我觉得不抖了」，但客户说「还是晕」。

所以我们必须把抖动变成数字。没有量化，就没有优化。这一章，我就带你看看我平时最常用的四个指标：ATE、RPE、平滑度指标和频谱分析。每个指标都有它的脾气，用对了地方，问题一眼就能看穿。

4.1 绝对轨迹误差（ATE）

ATE，全称 Absolute Trajectory Error。说白了，就是算你估计的轨迹和真实轨迹之间，每个点差了多少。

我习惯先做一步：把两条轨迹对齐。因为ATE对坐标系原点敏感，不先对齐的话，误差里会混入平移和旋转的偏差，那不是我们想看的抖动。

计算步骤

1. 时间戳对齐：把估计轨迹和真值轨迹按时间配对。
2. 位姿对齐：用Umeyama算法或ICP，把估计轨迹对齐到真值坐标系。
3. 逐点计算误差：每个配对点，算平移向量的欧氏距离。
4. 统计：RMSE、均值、中位数、标准差。

import numpy as np

def compute_ate(est_poses, gt_poses):
    """
    est_poses, gt_poses: Nx4x4 齐次变换矩阵
    返回 RMSE, mean, median, std
    """
    errors = []
    for est, gt in zip(est_poses, gt_poses):
        t_est = est[:3, 3]
        t_gt  = gt[:3, 3]
        error = np.linalg.norm(t_est - t_gt)
        errors.append(error)
    errors = np.array(errors)
    rmse = np.sqrt(np.mean(errors ** 2))
    return rmse, np.mean(errors), np.median(errors), np.std(errors)

4.2 相对位姿误差（RPE）

ATE看的是全局，RPE看的是局部。它衡量的是「每隔一段距离/时间，位姿变化量的误差」。说白了，就是看你的轨迹在短时间内的相对运动准不准。

RPE对抖动特别敏感。因为抖动往往表现为高频的小幅度位姿变化，ATE可能被平均掉，但RPE能抓住。

两种常见形式

类型	含义	适用场景
平移RPE	相对平移误差（m）	评估局部位置抖动
旋转RPE	相对旋转误差（deg）	评估局部姿态抖动

def compute_rpe(est_poses, gt_poses, delta=1):
    """
    delta: 间隔帧数
    返回平移RPE和旋转RPE的列表
    """
    trans_errors = []
    rot_errors = []
    for i in range(len(est_poses) - delta):
        # 计算相对变换
        est_rel = np.linalg.inv(est_poses[i]) @ est_poses[i + delta]
        gt_rel  = np.linalg.inv(gt_poses[i])  @ gt_poses[i + delta]
        # 误差矩阵
        err_mat = np.linalg.inv(gt_rel) @ est_rel
        # 平移误差
        trans_err = np.linalg.norm(err_mat[:3, 3])
        # 旋转误差（角度）
        rot_err = np.arccos(np.clip((np.trace(err_mat[:3, :3]) - 1) / 2, -1, 1))
        trans_errors.append(trans_err)
        rot_errors.append(rot_err)
    return trans_errors, rot_errors

4.3 平滑度指标：Jerk和加速度方差

ATE和RPE都需要真值轨迹。但很多时候，我们没有真值。比如在未知环境里跑机器人，你拿什么当ground truth？

这时候，平滑度指标就派上用场了。它只依赖轨迹本身，不需要真值。

Jerk（加加速度）

Jerk是加速度的变化率。物理意义很直观：加速度变化越剧烈，运动越「顿挫」。人坐在车上，感觉到的就是Jerk。

def compute_jerk(positions, dt):
    """
    positions: Nx3 位置序列
    dt: 时间间隔
    返回每个点的Jerk大小
    """
    # 速度
    vel = np.diff(positions, axis=0) / dt
    # 加速度
    acc = np.diff(vel, axis=0) / dt
    # 加加速度
    jerk = np.diff(acc, axis=0) / dt
    # 每个点的Jerk大小（L2范数）
    jerk_mag = np.linalg.norm(jerk, axis=1)
    return jerk_mag

加速度方差

加速度方差衡量的是加速度的离散程度。方差大，说明加速度忽大忽小，运动不平滑。

def compute_acc_variance(positions, dt):
    vel = np.diff(positions, axis=0) / dt
    acc = np.diff(vel, axis=0) / dt
    # 每个轴的方差
    var_x = np.var(acc[:, 0])
    var_y = np.var(acc[:, 1])
    var_z = np.var(acc[:, 2])
    # 总方差（迹）
    total_var = var_x + var_y + var_z
    return total_var

4.4 频谱分析（FFT）

前面几个指标告诉你「抖不抖」，但没告诉你「为什么抖」。频谱分析就是干这个的。

把轨迹的位置或姿态信号做FFT，转到频域，你就能看到抖动能量集中在哪些频率上。比如：

• 低频（<1 Hz）：可能是路径规划的问题，或者传感器漂移。
• 中频（1-10 Hz）：可能是控制器的震荡，或者IMU的噪声。
• 高频（>10 Hz）：通常是传感器噪声，或者机械振动。

import numpy as np
from scipy.fft import fft, fftfreq

def analyze_spectrum(signal, dt):
    """
    signal: 一维信号（比如x轴位置）
    dt: 采样间隔
    返回频率和幅度
    """
    n = len(signal)
    yf = fft(signal)
    xf = fftfreq(n, dt)[:n//2]
    magnitude = 2.0/n * np.abs(yf[:n//2])
    return xf, magnitude

4.5 知识体系总览

这四个指标不是孤立的。它们从不同角度描述抖动，组合起来才能全面诊断。下面这张图是我自己总结的框架：

4.6 指标选择指南

这么多指标，到底用哪个？我一般按这个逻辑来：

场景	推荐指标	原因
有真值轨迹	ATE + RPE	全局+局部，全面评估
无真值轨迹	Jerk + 加速度方差	不需要真值，自评估平滑度
需要诊断抖动来源	频谱分析	定位频段，对症下药
实时监控	加速度方差	计算量小，适合嵌入式
最终验收	ATE + Jerk	一个看精度，一个看舒适度