第三节 跟踪误差基础

各位同学，今天我们聊聊跟踪误差。说实话，这个概念我刚开始做机器人时也犯过迷糊——总觉得「误差不就是实际位置减去期望位置吗？」后来在项目里摔了几次跟头，才明白事情没那么简单。

一、跟踪误差到底在说什么？

先问大家一个问题：你让机器人沿着一条直线走，结果它走成了S形。这中间的偏差，就是跟踪误差。但具体怎么量化？我习惯把它拆成三块来看：位置、速度、姿态。

二、位置误差

位置误差最好理解。就是机器人实际位置和期望位置之间的空间距离。我一般用欧几里得距离来算：

// 位置误差计算示例
struct PositionError {
    double dx = actual.x - desired.x;
    double dy = actual.y - desired.y;
    double dz = actual.z - desired.z;
    double distance = sqrt(dx*dx + dy*dy + dz*dz);
};

嗯，这里要注意：单纯看距离有时候会骗人。我记得有一次调试AGV小车，距离误差只有2厘米，但小车已经撞到货架了。为什么？因为方向偏了。所以位置误差最好拆成三个轴向来看，别只看模值。

三、速度误差

速度误差很多人会忽略。你想想看，位置对了但速度不对，会怎样？

举个例子：机械臂抓取一个移动的传送带上的零件。位置误差可能很小，但速度误差大——机械臂末端速度跟不上传送带速度，结果就是抓空。我在做产线项目时吃过这个亏，后来学乖了，速度误差必须单独监控。

速度误差的计算很简单：

// 速度误差
struct VelocityError {
    double dvx = actual_vx - desired_vx;
    double dvy = actual_vy - desired_vy;
    double dvz = actual_vz - desired_vz;
};

四、姿态误差

姿态误差是最容易出问题的地方。为什么？因为姿态表示方法太多——欧拉角、四元数、旋转矩阵，每种算误差的方式都不一样。

我个人习惯用四元数来算姿态误差。原因很简单：没有万向锁问题。欧拉角在俯仰角接近90度时会突然跳变，我见过不止一个新手在这里翻车。

// 四元数姿态误差计算
struct AttitudeError {
    // q_actual 和 q_desired 都是单位四元数
    Quaternion q_error = q_actual * q_desired.inverse();
    // 提取旋转角度
    double angle = 2 * acos(q_error.w);
    // 提取旋转轴
    Vector3 axis = q_error.vec().normalized();
};

这里有个坑：四元数误差的物理意义不太直观。你算出来一个四元数，很难直接说「偏了3度」。所以我一般会转成轴角表示，这样调试时一眼就能看出问题。

五、误差来源分析

搞清楚了误差怎么算，接下来得知道误差从哪来。我把它归纳为四大类：

误差来源	典型原因	影响程度
传感器噪声	编码器量化误差、IMU漂移、激光雷达测量噪声	高
执行器延迟	电机响应滞后、液压系统惯性、气动系统压缩性	中高
模型误差	动力学参数不准确、摩擦模型简化、关节柔性忽略	中
外部扰动	地面不平、负载变化、风力干扰	视情况而定

我挑几个重点说说：

1. 传感器噪声

说白了就是测量不准。编码器有量化误差，IMU有零偏和漂移。我记得有一次做移动机器人定位，里程计积分几分钟就偏了十几厘米。后来加了激光雷达修正，才把误差压下来。

2. 执行器延迟

这个最坑。你发出指令，电机要几十毫秒才能响应。这几十毫秒里，机器人还在按旧指令运动。对于高速运动，这个延迟造成的误差非常可观。我建议做轨迹规划时，一定要把执行器延迟建模进去。

3. 模型误差

你设计的控制器是基于一个理想模型，但实际机器人总有偏差。比如你假设关节是刚性的，但实际有柔性；你假设摩擦是线性的，但实际有Stribeck效应。这些模型误差会直接导致跟踪误差。

4. 外部扰动

这个就看运气了。地面有个小坑，负载突然变化，这些都会引入误差。好的控制器应该能抑制这些扰动，但完全消除是不可能的。

六、误差的耦合关系

最后说一个容易被忽略的点：位置、速度、姿态误差不是独立的。它们会互相影响。

举个例子：姿态误差会导致力的方向偏了，进而影响位置误差。速度误差会导致位置积分的偏差越来越大。所以做控制时，不能单独优化某一个误差，要综合考虑。

下面这张图是我自己总结的误差耦合关系：

这张图想表达的核心意思是：误差不是孤立的。你压住了位置误差，速度误差可能变大；你优化了姿态，位置可能又偏了。所以做轨迹跟踪控制，本质上是在多个误差之间找平衡。

好了，这一节就到这里。记住：先搞清楚误差怎么算，再去找误差从哪来，最后理解它们怎么互相影响。这三步走完，你才算真正理解了跟踪误差。