机器人手眼标定实战：从原理到Matlab代码全解析

刚接触机器人视觉的开发者，往往会在手眼标定这个环节卡壳。看着论文里复杂的数学公式和术语，实际操作时却不知从何下手。本文将用最直白的语言和可运行的代码，带你彻底掌握手眼标定的核心要点。

1. 手眼标定基础概念

手眼标定要解决的根本问题是确定相机坐标系与机器人坐标系之间的转换关系。想象一下，当机器人"看到"一个物体时，它需要知道这个物体在自己坐标系中的具体位置，才能准确执行抓取或操作。

两种主要配置模式：

• Eye-in-Hand（眼在手）：相机安装在机器人末端执行器上，随机械臂一起移动。这种配置下，相机与机器人末端之间的相对位置固定不变。

  典型应用场景：

  • 精密装配作业
  • 需要实时跟踪末端工具的场景
  • 动态目标抓取

• Eye-to-Hand（眼在外）：相机固定在机器人工作空间外的某个位置，独立于机械臂运动。这种配置下，相机与机器人基座之间的相对位置固定。

  典型优势：

  • 视野范围不受机械臂运动影响
  • 适合大范围监控
  • 标定后稳定性更高

数学本质：无论是哪种配置，最终都需要求解一个齐次变换矩阵X，满足AX=XB的基本方程。其中：

• A表示机器人末端（或基座）的位姿变化
• B表示相机观察到的标定板位姿变化

2. 准备工作与环境搭建

在开始编码前，我们需要准备好以下工具和数据：

必备工具：

• MATLAB R2016a或更新版本
• Peter Corke的机器人工具箱（安装命令：run('rvctools/startup_rvc.m')）
• 标定数据文件（机器人位姿和标定板位姿）

数据文件格式示例：

# 机器人位姿文件 Kinova_pose.txt
-0.0860801 -0.641813 -0.0987199 3.13316 0.000389122 -0.297456
...

# 标定板位姿文件 Pattern_pose.txt 
0.011651 -0.069043 0.857845 -3.12825 3.137609 3.048224
...

每行数据包含6个值：x,y,z（位置，单位：米），rx,ry,rz（欧拉角，单位：弧度）

环境检查代码：

% 检查机器人工具箱是否安装
if ~exist('rotx', 'file')
    error('请先安装机器人工具箱');
end

% 验证MATLAB版本
if verLessThan('matlab', '9.0')
    warning('建议使用R2016a或更高版本以获得最佳性能');
end

3. 核心算法实现

手眼标定主要有两种经典算法：Shiu方法和Tsai方法。下面我们分别实现这两种算法，并比较它们的差异。

3.1 数据加载与预处理

首先需要将原始数据转换为齐次变换矩阵：

function [A_cell, B_cell] = load_and_convert(robot_file, pattern_file)
    % 加载机器人位姿数据
    robot_data = importdata(robot_file);
    [m, ~] = size(robot_data);
    A_cell = cell(1,m);
    
    for i = 1:m
        A_cell{i} = transl(robot_data(i,1:3)) * ...
                   trotx(robot_data(i,4)) * ...
                   troty(robot_data(i,5)) * ...
                   trotz(robot_data(i,6));
    end
    
    % 加载标定板位姿数据
    pattern_data = importdata(pattern_file);
    B_cell = cell(1,m);
    
    for i = 1:m
        B_cell{i} = transl(pattern_data(i,1:3)) * ...
                   trotx(pattern_data(i,4)) * ...
                   troty(pattern_data(i,5)) * ...
                   trotz(pattern_data(i,6));
    end
end

3.2 Tsai算法实现

Tsai算法是手眼标定中最常用的方法之一，其核心是通过SVD分解求解旋转矩阵：

function X = tsai(A, B)
    % 构建运动对
    n = length(A);
    M = zeros(3,3);
    
    for i = 1:n
        [theta_a, axis_a] = tr2angvec(A{i});
        [theta_b, axis_b] = tr2angvec(B{i});
        
        if theta_a > 1e-6 && theta_b > 1e-6
            ka = axis_a';
            kb = axis_b';
            M = M + kb * ka;
        end
    end
    
    % SVD分解求解旋转
    [U, ~, V] = svd(M);
    R = V * U';
    
    % 求解平移
    C = zeros(3*n, 3);
    d = zeros(3*n, 1);
    
    for i = 1:n
        C(3*i-2:3*i, :) = eye(3) - A{i}(1:3,1:3);
        d(3*i-2:3*i, 1) = A{i}(1:3,4) - R * B{i}(1:3,4);
    end
    
    t = pinv(C) * d;
    
    % 组合为齐次变换矩阵
    X = [R t; 0 0 0 1];
end

3.3 Shiu算法实现

Shiu算法采用不同的旋转求解策略，在某些情况下可能更稳定：

function X = shiu(A, B)
    n = length(A);
    C = zeros(4*n, 4);
    
    for i = 1:n
        % 四元数表示旋转
        qa = rotm2quat(A{i}(1:3,1:3));
        qb = rotm2quat(B{i}(1:3,1:3));
        
        % 构建系数矩阵
        C(4*i-3:4*i, :) = [qa(1) -qb(1) -qa(2:4) qb(2:4);
                           qa(2:4)' qb(2:4)' (qa(1)-qb(1))*eye(3)+skew(qa(2:4)+qb(2:4))];
    end
    
    % SVD求解
    [~, ~, V] = svd(C);
    q = V(:,end);
    qx = q(1:4)/norm(q(1:4));
    qy = q(5:8)/norm(q(5:8));
    
    R = quat2rotm(qx)' * quat2rotm(qy);
    
    % 求解平移
    C_t = zeros(3*n, 3);
    d = zeros(3*n, 1);
    
    for i = 1:n
        C_t(3*i-2:3*i, :) = eye(3) - A{i}(1:3,1:3);
        d(3*i-2:3*i, 1) = A{i}(1:3,4) - R * B{i}(1:3,4);
    end
    
    t = pinv(C_t) * d;
    X = [R t; 0 0 0 1];
end

function S = skew(v)
    S = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];
end

4. 完整流程与结果可视化

现在我们将上述模块组合起来，完成整个标定流程：

% 主程序
clear; close all; clc;

% 1. 加载数据
robot_file = 'Kinova_pose.txt';
pattern_file = 'Pattern_pose.txt';
[A, B] = load_and_convert(robot_file, pattern_file);

% 2. 构建运动对
n = length(A);
TA = cell(1,n-1);
TB = cell(1,n-1);

for i = 1:n-1
    TA{i} = A{i+1} / A{i};  % 注意：eye-in-hand和eye-to-hand这里不同
    TB{i} = B{i+1} / B{i};
end

% 3. 调用标定算法
X_tsai = tsai(TA, TB);
X_shiu = shiu(TA, TB);

% 4. 结果输出与可视化
fprintf('Tsai方法结果：\n');
disp(X_tsai);

fprintf('Shiu方法结果：\n');
disp(X_shiu);

% 可视化
figure;
trplot(eye(4), 'frame', 'R', 'color', 'r', 'length', 0.5);
hold on;
trplot(X_tsai, 'frame', 'Tsai', 'color', 'b');
trplot(X_shiu, 'frame', 'Shiu', 'color', 'g');
title('手眼标定结果对比');
xlabel('X'); ylabel('Y'); zlabel('Z');
grid on; axis equal;

关键点说明：

1. 运动对构建时，eye-in-hand和eye-to-hand模式的区别体现在TA/TB的计算方式上：

   • Eye-in-hand: TA = A{i} \ A{i+1}; TB = B{i} \ B{i+1};
   • Eye-to-hand: TA = A{i+1} / A{i}; TB = B{i+1} / B{i};

2. 数据采集建议：

   • 至少需要3组不同位姿（推荐5-10组）
   • 位姿间应有明显的旋转和平移变化
   • 避免所有运动都在同一平面内

3. 精度验证方法：

% 精度验证
errors = zeros(n,1);
for i = 1:n
    errors(i) = norm(A{i}*X_tsai - X_tsai*B{i}, 'fro');
end
fprintf('平均误差：%.4f\n', mean(errors));

5. 平面九点标定法

对于只需要平面定位的应用（如传送带抓取），可以采用简化的九点标定法。这种方法只需要求解一个3×3的投影矩阵。

实现步骤：

1. 采集至少4组对应点（推荐9组）
2. 构建方程组求解投影矩阵
3. 验证标定精度

Matlab实现：

function H = nine_point_calibration(pixel_pts, robot_pts)
    % pixel_pts: N×2 图像像素坐标
    % robot_pts: N×2 机器人坐标系坐标
    
    N = size(pixel_pts,1);
    A = zeros(2*N, 6);
    b = zeros(2*N, 1);
    
    for i = 1:N
        A(2*i-1, :) = [pixel_pts(i,1) pixel_pts(i,2) 1 0 0 0];
        A(2*i, :) = [0 0 0 pixel_pts(i,1) pixel_pts(i,2) 1];
        b(2*i-1:2*i) = robot_pts(i,:)';
    end
    
    x = A \ b;
    H = [x(1) x(2) x(3); x(4) x(5) x(6); 0 0 1];
end

使用示例：

% 像素坐标（从图像中获取）
pixel_points = [480.8, 639.4; 
               227.1, 317.5; 
               292.4, 781.6;
               597.4, 1044.1;
               557.7, 491.6;
               717.8, 263.7];

% 对应的机器人坐标（通过示教获取）
robot_points = [197170, 185349;
               195830, 186789;
               196174, 184591;
               197787, 183176;
               197575, 186133;
               198466, 187335];

H = nine_point_calibration(pixel_points, robot_points);

% 测试转换
test_pixel = [500, 600];
test_robot = H * [test_pixel, 1]';
fprintf('像素坐标(%.1f,%.1f) → 机器人坐标(%.1f,%.1f)\n',...
        test_pixel(1), test_pixel(2), test_robot(1), test_robot(2));

6. 常见问题排查

在实际应用中，可能会遇到各种问题。以下是几个典型问题及解决方案：

问题1：标定结果不稳定，每次运行结果差异大

可能原因：

• 数据采集时位姿变化不足
• 标定板检测精度不够
• 机器人定位精度问题

解决方案：

• 确保采集足够多（>10组）且差异明显的位姿
• 检查标定板检测算法，确保角点检测亚像素精度
• 验证机器人重复定位精度

问题2：旋转矩阵不是正交矩阵

处理方法：

% 正交化旋转矩阵
[U, ~, V] = svd(R);
R_corrected = U * V';

问题3：平移量明显偏大

检查点：

• 确认单位一致（米/毫米）
• 检查机器人位姿数据是否正确
• 验证标定板尺寸参数

问题4：Eye-in-hand和Eye-to-hand模式混淆

判断方法：

• 观察相机安装位置
• 检查运动对构建公式
• 通过实验验证：移动末端时，如果标定板在图像中位置变化小，可能是eye-to-hand配置

7. 高级技巧与优化

对于追求更高精度的应用，可以考虑以下优化措施：

数据采集策略优化：

• 采用五角星轨迹采集数据，确保覆盖工作空间
• 加入不同高度的平面
• 包含绕X/Y/Z轴的单轴旋转

算法增强：

• 加入RANSAC剔除异常点
• 采用非线性优化进一步 refine 结果
• 结合多种算法结果进行融合

标定验证方法：

% 重投影误差计算
function error = compute_reprojection_error(A, B, X)
    n = length(A);
    errors = zeros(n,1);
    for i = 1:n
        errors(i) = norm(A{i}*X - X*B{i}, 'fro');
    end
    error = mean(errors);
end

自动标定脚本示例：

function [X, error] = auto_calibration(robot_file, pattern_file, mode)
    % mode: 'eye-in-hand' 或 'eye-to-hand'
    
    % 加载数据
    [A, B] = load_and_convert(robot_file, pattern_file);
    
    % 构建运动对
    n = length(A);
    TA = cell(1,n-1);
    TB = cell(1,n-1);
    
    for i = 1:n-1
        if strcmp(mode, 'eye-in-hand')
            TA{i} = A{i} \ A{i+1};
            TB{i} = B{i} \ B{i+1};
        else
            TA{i} = A{i+1} / A{i};
            TB{i} = B{i+1} / B{i};
        end
    end
    
    % 多种算法求解
    X_tsai = tsai(TA, TB);
    X_shiu = shiu(TA, TB);
    
    % 选择最优解
    error_tsai = compute_reprojection_error(TA, TB, X_tsai);
    error_shiu = compute_reprojection_error(TA, TB, X_shiu);
    
    if error_tsai < error_shiu
        X = X_tsai;
        error = error_tsai;
    else
        X = X_shiu;
        error = error_shiu;
    end
    
    % 非线性优化
    options = optimoptions('fminunc', 'Display', 'off');
    X_opt = fminunc(@(x)calibration_cost(x, TA, TB), X(:), options);
    X_opt = reshape(X_opt, 4, 4);
    
    error_opt = compute_reprojection_error(TA, TB, X_opt);
    if error_opt < error
        X = X_opt;
        error = error_opt;
    end
end

function cost = calibration_cost(x, TA, TB)
    X = reshape(x, 4, 4);
    n = length(TA);
    cost = 0;
    for i = 1:n
        cost = cost + norm(TA{i}*X - X*TB{i}, 'fro')^2;
    end
end

在实际项目中，我发现数据质量对标定结果的影响远大于算法选择。一组好的数据应该覆盖工作空间的各个区域，包含不同方向的旋转和平移。曾经在一个装配项目中，我们最初只采集了XY平面内的数据，结果在Z方向的操作精度很差。后来改为采集三维空间分布的数据后，整体精度提高了近5倍。