第24章 视觉引导插补：视觉定位数据融合、实时路径修正、手眼标定

视觉引导插补，说白了就是给运动控制系统装上「眼睛」。我做了这么多年运动控制，发现很多工程师把视觉和运动割裂开来搞——视觉只管拍照定位，运动只管走轨迹。结果呢？精度上不去，效率也提不起来。

这一章，咱们就聊聊怎么把视觉数据和插补算法真正融合在一起。我会结合我实际项目中的踩坑经验，把视觉定位数据融合、实时路径修正、手眼标定这几个核心问题讲透。

24.1 视觉定位数据融合——别让视觉数据「裸奔」

视觉传感器采集到的数据，其实挺「脏」的。我遇到过最夸张的一次，相机拍同一个点，十次结果能差0.5mm。你想想看，要是直接把这种数据丢给插补器，那轨迹肯定抖成筛子。

所以，数据融合是第一步。我个人习惯用卡尔曼滤波来做视觉定位数据的平滑处理。为什么选它？因为卡尔曼滤波能根据历史状态预测当前值，再结合观测值做最优估计。说白了，就是给视觉数据加个「阻尼」，不让它乱跳。

这里给出一段我常用的简化版卡尔曼滤波代码，用于视觉位置数据的融合：

// 视觉定位数据卡尔曼滤波
typedef struct {
    float x;        // 状态：位置
    float v;        // 状态：速度
    float P[2][2];  // 协方差矩阵
    float Q;        // 过程噪声
    float R;        // 观测噪声
} KalmanFilter2D;

void Kalman_Init(KalmanFilter2D *kf, float init_x, float init_v) {
    kf->x = init_x;
    kf->v = init_v;
    kf->P[0][0] = 1.0f; kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f; kf->P[1][1] = 1.0f;
    kf->Q = 0.01f;      // 过程噪声，根据运动平稳性调整
    kf->R = 0.1f;       // 观测噪声，根据视觉精度调整
}

float Kalman_Update(KalmanFilter2D *kf, float measure, float dt) {
    // 预测
    float x_pred = kf->x + kf->v * dt;
    float v_pred = kf->v;
    
    float P00_pred = kf->P[0][0] + kf->P[1][0]*dt + kf->P[0][1]*dt + kf->P[1][1]*dt*dt + kf->Q;
    float P01_pred = kf->P[0][1] + kf->P[1][1]*dt;
    float P10_pred = kf->P[1][0] + kf->P[1][1]*dt;
    float P11_pred = kf->P[1][1] + kf->Q;
    
    // 更新
    float K0 = P00_pred / (P00_pred + kf->R);
    float K1 = P10_pred / (P00_pred + kf->R);
    
    kf->x = x_pred + K0 * (measure - x_pred);
    kf->v = v_pred + K1 * (measure - x_pred);
    
    kf->P[0][0] = (1 - K0) * P00_pred;
    kf->P[0][1] = (1 - K0) * P01_pred;
    kf->P[1][0] = -K1 * P00_pred + P10_pred;
    kf->P[1][1] = -K1 * P01_pred + P11_pred;
    
    return kf->x;
}

24.2 实时路径修正——插补器不能「死脑筋」

视觉引导的核心，就是让插补器能根据视觉反馈实时调整路径。我见过不少方案，视觉拍完照，算完偏移，然后重新规划一条轨迹再执行。这太慢了！

真正的实时路径修正，应该是在插补过程中动态调整。举个例子，你正在走一条直线插补，视觉突然发现目标偏移了2mm。这时候，你不能停下来重新规划，而应该在当前插补点的基础上，叠加一个修正量。

我个人习惯用「增量式修正」策略。具体做法是：

1. 视觉采样周期：一般设为10-30ms，与插补周期（通常1ms）解耦
2. 偏差计算：视觉反馈位置 vs 理论插补位置
3. 修正量生成：通过PID或前馈控制器，将偏差转化为速度/位置修正量
4. 叠加到插补输出：在插补器的最终输出端，叠加修正量

这里有个关键点——修正量必须平滑。我曾经在项目里直接用偏差值做修正，结果轨迹出现了明显的「锯齿」。后来加了个二阶低通滤波器，问题就解决了。

24.3 手眼标定——视觉和运动的「坐标系对齐」

手眼标定，说白了就是让相机知道「我看到的点，在机器人坐标系里到底在哪」。这个问题看似简单，但坑特别多。

我遇到过最典型的问题：标定完精度很好，换了个工件就不行了。后来发现是标定板平面和工件平面不平行，导致标定参数只在特定高度有效。

手眼标定主要分两种场景：

对于Eye-to-Hand场景，我常用的是「三点标定法」。原理很简单：让机器人末端分别走到三个已知视觉位置的点，记录下机器人坐标和视觉坐标，然后解算变换矩阵。

这里给出一个简化的标定计算流程：

// 三点标定法：计算视觉到机器人的变换矩阵
// 输入：三个点在视觉坐标系下的坐标 (uv1, uv2, uv3)
//       三个点在机器人坐标系下的坐标 (xy1, xy2, xy3)
// 输出：2x3 仿射变换矩阵 [a b c; d e f]

void Calibrate_3Points(
    float uv[3][2], float xy[3][2],
    float *a, float *b, float *c,
    float *d, float *e, float *f) 
{
    // 构建线性方程组
    // [u v 1 0 0 0] * [a b c d e f]^T = x
    // [0 0 0 u v 1] * [a b c d e f]^T = y
    
    float A[6][6] = {0};
    float B[6] = {0};
    
    for(int i = 0; i < 3; i++) {
        A[i*2][0] = uv[i][0]; A[i*2][1] = uv[i][1]; A[i*2][2] = 1.0f;
        A[i*2+1][3] = uv[i][0]; A[i*2+1][4] = uv[i][1]; A[i*2+1][5] = 1.0f;
        B[i*2] = xy[i][0];
        B[i*2+1] = xy[i][1];
    }
    
    // 使用高斯消元法求解 A * X = B
    // 实际项目中建议用SVD分解，更稳定
    // 这里省略具体求解过程...
}

24.4 视觉引导插补的RTOS实现架构

在RTOS中实现视觉引导插补，任务划分是关键。我一般这样设计：

// 任务划分示例（基于FreeRTOS）
TaskHandle_t hTaskVision;   // 视觉处理任务，优先级中等
TaskHandle_t hTaskInterp;   // 插补计算任务，优先级最高
TaskHandle_t hTaskFusion;   // 数据融合任务，优先级较高

void TaskVision(void *param) {
    while(1) {
        // 触发相机拍照
        // 图像处理，提取特征点
        // 计算视觉位置
        xQueueSend(qVisionResult, &vision_data, 0);
        vTaskDelay(pdMS_TO_TICKS(20)); // 50Hz视觉采样
    }
}

void TaskFusion(void *param) {
    while(1) {
        // 从队列接收视觉数据
        xQueueReceive(qVisionResult, &vision_data, portMAX_DELAY);
        
        // 卡尔曼滤波融合
        float fused_pos = Kalman_Update(&kf, vision_data.pos, dt);
        
        // 计算修正量
        float correction = PID_Calc(&pid, fused_pos, interp_pos);
        
        // 发送修正量到插补任务
        xQueueOverwrite(qCorrection, &correction);
    }
}

void TaskInterp(void *param) {
    while(1) {
        // 1ms插补周期
        // 生成理论插补点
        float target = Interp_GenerateNextPoint();
        
        // 叠加视觉修正量
        float correction;
        if(xQueuePeek(qCorrection, &correction, 0) == pdTRUE) {
            target += correction;
        }
        
        // 输出到驱动器
        Motor_SetPosition(target);
        
        vTaskDelay(pdMS_TO_TICKS(1));
    }
}

这个架构的好处是：视觉处理和插补计算完全解耦。视觉任务可以跑得慢一点（50Hz），插补任务保持高速（1kHz）。数据融合任务作为中间层，负责把视觉数据「翻译」成插补能用的修正量。

24.5 视觉引导插补的知识体系

为了让你更直观地理解整个知识体系，我画了一张流程图：

这张图把整个视觉引导插补的流程串起来了。从视觉感知开始，经过数据融合、路径修正，最终到插补执行。手眼标定是贯穿始终的基础，而RTOS任务架构则是整个系统的「骨架」。

嗯，这一章的内容就到这里。视觉引导插补是个系统工程，每个环节都值得深入钻研。如果你在实际项目中遇到问题，不妨从数据融合的延迟补偿和手眼标定的精度验证这两个方向入手——我敢说，80%的问题都出在这两个地方。