手眼标定

本文参考了OPENCV

问题描述

在机器人系统中，经常遇到需要确定相机（眼睛）与机器人末端（TCP）之间的安装关系。如果相机不在机械臂末端，往往末端会安装一个相机能够识别的标记物，相机系统给出该标记物在相机空间的三维坐标和姿态。无论上述哪种安装类型，都需要确定一个方程的解：，是已知的齐次矩阵，是未知的齐次矩阵。

利用李代数和最小二乘解决的问题。

平移旋转分开求解

从齐次等式提取旋转部分

李代数和李群之间的相互映射

在SO(3)上，旋转矩阵的李代数 是一个三维向量(是一个单位向量，)。是一个反对称矩阵。

so(3)李代数到SO(3)的指数映射--罗德里格斯公式：

SO(3)到so(3)的对数映射：

其中，

李代数对应的反对称矩阵的定义和性质：

并且，该反对称矩阵有性质：(体现了是旋转对时间导数的性质，越大，意味着绕着转轴转动越快速)。

李群相等转换到李代数相等

问题转化：找到一个，使得李群中的旋转和相等（这里是理想情况下，测量不带噪声）。 如果测量值，不带噪声，给定任意一组测量值和。看看李代数so(3)中的两个向量怎么和上述的旋转相等建立关系。

因为绕着一个轴旋转和角度效果是一样的，上面的推导有一个条件，就是约束,。

求解过程

测量带噪声

在给定数据带有噪声的情况下，我们把每一组转化之间的误差进行平方和最小化：

根据Nadas：

当且仅当使得最小，其中，

数据预处理

采集了和数据。先根据上面的方法，求出和。 将和组成矩阵:

求解旋转矩阵

先求旋转参数

求解平移向量

还有平移的参数需要估计。

上面的最小化，是典型的最小二乘问题:投影矩阵和最小二乘

到此，旋转和平移全部求出。

代码

void park_martin(std::vector<Eigen::Matrix4d> A, std::vector<Eigen::Matrix4d> B, Eigen::Matrix4d& result)
{
    int dataLength = A.size();
    if(dataLength != B.size())
    {
        return;
    }

    Eigen::Matrix3d M = Eigen::Matrix3d::Zero();

    for (int i = 0; i < dataLength; i++)
    {
        Eigen::Matrix3d Ra = A[i].toMatrix().block<3, 3>(0, 0);
        Eigen::Matrix3d Rb = B[i].toMatrix().block<3, 3>(0, 0);
        M += log(Rb) * log(Ra).transpose();
    }

    //best rotation
    Eigen::Matrix3d theta_X = invsqrt(M.transpose() * M) * M.transpose();

    // best translation
    Eigen::MatrixXd C(3 * dataLength, 3);
    Eigen::MatrixXd y(3 * dataLength, 1);

    for (int i = 0; i < dataLength; i++)
    {
        Eigen::Matrix3d Ra = A[i].toMatrix().block<3, 3>(0, 0);
        Eigen::Vector3d b_a = A[i].toMatrix().block<3, 1>(0, 3);

        Eigen::Matrix3d Rb = B[i].toMatrix().block<3, 3>(0, 0);
        Eigen::Vector3d b_b = B[i].toMatrix().block<3, 1>(0, 3);
        C.block<3, 3>(i * 3, 0) = Ra - Eigen::Matrix3d::Identity();
        y.block<3, 1>(i * 3, 0) = theta_X * b_b - b_a;
    }
    Eigen::Vector3d b_x = (C.transpose() * C).inverse() * C.transpose() * y;

    Eigen::Matrix4d result_M = Eigen::Matrix4d::Identity();
    result_M.block<3, 3>(0, 0) = theta_X;
    result_M.block<3, 1>(0, 3) = b_x;
    result = result_M;
}

Eigen::Vector3d DrsToolCalibration::log(Eigen::Matrix3d rotM)
{
    double theta = std::acos((rotM(0, 0) + rotM(1, 1) + rotM(2, 2) - 1.0) / 2.0);
    return Eigen::Vector3d(rotM(2, 1) - rotM(1, 2), rotM(0, 2) - rotM(2, 0), rotM(1, 0) - rotM(0, 1))* theta / (2 * std::sin(theta));
}

Eigen::Matrix3d DrsToolCalibration::invsqrt(Eigen::Matrix3d rotM)
{
    if (rotM.rows() != rotM.cols()) {
        return Eigen::MatrixXd();
    }
   
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(rotM, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::VectorXd s = svd.singularValues();
    for (int i = 0; i < s.size(); ++i) {
        if (s(i) <= 0) {
            break;
        }
    }
    Eigen::VectorXd inv_sqrt_s = s.array().inverse().sqrt();
    return svd.matrixU() * inv_sqrt_s.asDiagonal() * svd.matrixV().transpose();
}

如何使用：

Eigen::Matrix4d park_martin_result;
std::vector<Eigen::Matrix4d> tcp_list;
std::vector<Eigen::Matrix4d> rh_list;

// here fill tcp_list and rh_list with data

park_martin(rh_list, tcp_list, park_martin_result);

// park_martin_result is the hand-eye calibration result