3. 补偿策略设计

标定做完，误差模型建好，接下来就是真刀真枪地干活了——怎么把补偿策略塞进实际系统里？

说实话，这一步才是手眼标定落地最头疼的地方。模型再漂亮，跑不到控制器里就是废纸。我见过太多团队，标定精度做到0.1mm，一上产线就崩，为什么？补偿策略没选对。

这一章，我把补偿策略分成三条路来讲：离线补偿、在线滤波补偿、查表分段补偿。每条路都有自己的脾气，选错了，后面全是坑。

3.1 离线补偿：稳，但不灵活

离线补偿，说白了就是「先算好，再执行」。标定完成后，我们把误差模型算出来的补偿量，提前存成一张表或者一组参数。机器人运行时，直接查表或者代入公式，不做实时估计。

我个人习惯在以下场景用离线补偿：

• 机器人工作环境稳定——温度、负载变化不大，误差基本固定
• 控制器算力紧张——老款PLC或者低端机器人，跑不了复杂滤波
• 节拍要求高——补偿计算不能拖慢主循环

举个例子，我之前做的一个汽车焊装项目，六台机器人围着一台夹具干活。环境恒温，负载固定，标定完一次能跑半年。这种场景，离线补偿就是最优解。

离线补偿的典型实现流程：

1. 标定得到误差参数（比如DH参数偏差、关节间隙、齿轮回差）
2. 在工作空间内采样N个位姿，计算每个位姿的理论值与实际值之差
3. 将补偿量存入查找表（LUT），或者拟合成多项式系数
4. 运行时，根据当前关节角度/末端位姿，查表或代入多项式得到补偿值
5. 将补偿值叠加到目标位姿上，再发给伺服驱动器

// 离线补偿查表示例（伪代码）
// 假设已经预计算了 1000 个位姿的补偿表
struct Pose {
  float x, y, z, rx, ry, rz;
};

Pose compensationTable[1000]; // 离线预计算

Pose getCompensation(Pose target) {
  // 1. 找到最近的三个网格点
  int idx[3];
  findNearestGridPoints(target, idx);
  
  // 2. 加权插值（反距离加权）
  float w[3];
  float totalWeight = 0;
  for (int i = 0; i < 3; i++) {
    w[i] = 1.0 / distance(target, compensationTable[idx[i]]);
    totalWeight += w[i];
  }
  
  // 3. 合成补偿量
  Pose comp = {0};
  for (int i = 0; i < 3; i++) {
    comp.x += compensationTable[idx[i]].x * (w[i] / totalWeight);
    comp.y += compensationTable[idx[i]].y * (w[i] / totalWeight);
    // ... 同理处理 rx, ry, rz
  }
  
  return comp;
}

3.2 在线滤波补偿：实时纠偏，但别小看算力

在线补偿，就是边跑边算。机器人每走一步，都根据当前传感器反馈（比如视觉、激光跟踪仪）来估计误差，然后实时修正。

你想想看，为什么需要在线补偿？因为有些误差是时变的——温度漂移、关节磨损、负载变化。离线表搞不定这些。

我常用的在线补偿工具是卡尔曼滤波。为什么选它？因为卡尔曼滤波能融合多源信息，还能给出误差的置信度。说白了，它知道「现在这个补偿值有多靠谱」。

卡尔曼滤波补偿的核心思路：

• 状态量：机器人末端位姿误差（6维：Δx, Δy, Δz, Δrx, Δry, Δrz）
• 观测量：视觉系统测得的实际位姿与理论位姿之差
• 过程模型：误差的漂移规律（通常用随机游走或一阶马尔可夫）
• 更新频率：每帧视觉数据进来就更新一次

// 卡尔曼滤波在线补偿（简化版）
// 状态向量 x = [Δx, Δy, Δz, Δrx, Δry, Δrz]^T

// 预测步骤（时间更新）
x_pred = A * x_prev;          // A 通常为单位矩阵或漂移模型
P_pred = A * P_prev * A^T + Q; // Q 为过程噪声协方差

// 更新步骤（测量更新）
K = P_pred * H^T * (H * P_pred * H^T + R)^-1; // 卡尔曼增益
x_est = x_pred + K * (z - H * x_pred);         // z 为视觉测量值
P_est = (I - K * H) * P_pred;

// 输出补偿量
Pose compensation = {
  .x = x_est[0], .y = x_est[1], .z = x_est[2],
  .rx = x_est[3], .ry = x_est[4], .rz = x_est[5]
};

在线补偿的优缺点，我用一张表说清楚：

维度	在线滤波补偿	离线补偿
实时性	每帧更新，延迟取决于滤波周期	查表即得，无延迟
适应性	能跟踪温度、磨损等慢变误差	固定补偿，环境变化后精度下降
算力需求	中高（矩阵运算，6维卡尔曼约50μs@1GHz ARM）	极低（查表+插值，<5μs）
标定工作量	需要建误差动态模型，较复杂	一次标定，长期使用
适用场景	高精度、环境变化、视觉引导	稳定环境、低成本控制器

3.3 查表法分段补偿：简单粗暴，但管用

查表法分段补偿，说白了就是把机器人的工作空间切成很多小格子，每个格子单独标定，单独存补偿值。运行时，先看机器人当前在哪个格子里，然后查这个格子的补偿表。

为什么需要分段？因为机器人的误差在空间里不是均匀的。靠近工作空间中心，误差可能只有0.2mm；到了边缘，关节角度大，重力变形明显，误差可能到0.8mm。一个全局模型很难同时拟合好。

我个人的做法是这样的：

1. 网格划分：把工作空间按X/Y/Z或者关节角度分成N×M×L个网格。网格密度取决于精度要求——要求越高，网格越密
2. 每个网格独立标定：在每个网格中心点做一次手眼标定，得到该点的误差补偿值
3. 运行时插值：机器人进入某个网格，用该网格及相邻网格的补偿值做线性或样条插值
4. 边界处理：网格边界处做平滑过渡，防止补偿值跳变导致机器人抖动

查表法分段补偿的代码实现，比卡尔曼滤波简单得多：

// 查表法分段补偿
// 假设工作空间划分为 10×10×5 网格
#define NX 10
#define NY 10
#define NZ 5

// 补偿表：每个网格存一个6维补偿向量
float compTable[NX][NY][NZ][6];

// 查表函数
void lookupCompensation(float x, float y, float z, float* compOut) {
  // 1. 计算网格索引
  int ix = (int)((x - xMin) / gridSizeX);
  int iy = (int)((y - yMin) / gridSizeY);
  int iz = (int)((z - zMin) / gridSizeZ);
  
  // 2. 边界钳位
  ix = clamp(ix, 0, NX-2);
  iy = clamp(iy, 0, NY-2);
  iz = clamp(iz, 0, NZ-2);
  
  // 3. 计算网格内归一化坐标（用于三线性插值）
  float fx = (x - (xMin + ix * gridSizeX)) / gridSizeX;
  float fy = (y - (yMin + iy * gridSizeY)) / gridSizeY;
  float fz = (z - (zMin + iz * gridSizeZ)) / gridSizeZ;
  
  // 4. 三线性插值
  for (int k = 0; k < 6; k++) {
    float c000 = compTable[ix][iy][iz][k];
    float c100 = compTable[ix+1][iy][iz][k];
    float c010 = compTable[ix][iy+1][iz][k];
    float c110 = compTable[ix+1][iy+1][iz][k];
    float c001 = compTable[ix][iy][iz+1][k];
    float c101 = compTable[ix+1][iy][iz+1][k];
    float c011 = compTable[ix][iy+1][iz+1][k];
    float c111 = compTable[ix+1][iy+1][iz+1][k];
    
    // 沿X插值
    float c00 = c000 + fx * (c100 - c000);
    float c10 = c010 + fx * (c110 - c010);
    float c01 = c001 + fx * (c101 - c001);
    float c11 = c011 + fx * (c111 - c011);
    
    // 沿Y插值
    float c0 = c00 + fy * (c10 - c00);
    float c1 = c01 + fy * (c11 - c01);
    
    // 沿Z插值
    compOut[k] = c0 + fz * (c1 - c0);
  }
}

3.4 三种策略怎么选？一张决策表搞定

说了这么多，到底怎么选？我整理了一张决策表，你对着自己的条件打勾就行：

现场条件	推荐策略	理由
环境温度稳定（±2°C以内）	离线补偿	误差固定，离线表足够，省算力
环境温度波动大（±10°C）	在线滤波 + 温度前馈	卡尔曼滤波跟踪热漂移，温度传感器做前馈修正
机器人工作空间大（>2m³）	查表法分段补偿	空间误差非线性强，分段模型更准
控制器算力低（ARM Cortex-M系列）	离线补偿 或 查表法	卡尔曼滤波的矩阵运算可能跑不动
有视觉反馈（相机/激光跟踪仪）	在线滤波补偿	视觉数据作为观测，闭环修正
节拍要求极高（<1s/循环）	离线补偿	在线滤波的延迟可能拖慢节拍
精度要求极高（<0.1mm）	查表法 + 在线滤波混合	查表法处理空间非线性，滤波处理时变漂移