经典手眼标定算法之Tsai-Lenz的OpenCV实现

 

       本文主要是讲解经典手眼标定问题中的TSAI-LENZ 文献方法,参考文献为“A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration”,并且实现了基于OpenCV的C++代码程序,code可去CSDN资源下载,MATLAB版本作者为苏黎世理工的Christian Wengert,也可在此处下载。


手眼标定问题描述

       在机器人校准测量、机器人手眼协调以及机器人辅助测量等领域,都要求知道机器人执行器末端(抓取臂)坐标系和传感器(比如用来测量三维空间中目标位置和方向并固定在机器人手上的摄像机)坐标系之间的相互关系,确定这种转换关系在机器人领域就是通常所说的手眼标定

       将手眼标定系统如下图所示,其中HgijHgij为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵;HcijHcij为摄像机坐标系之间相对位置姿态的齐次变换矩阵;HcgHcg为像机与机器人执行器末端之间的相对位置姿态齐次矩阵。

手眼标定系统

       经过坐标系变换,HgijHgij、HcijHcij和HcgHcg满足如下关系: 

HgijHcg=HcgHcij⇔(Rgij0Tgij1)(Rcg0Tcg1)=(Rcg0Tcg1)(Rcij0Tcij1)HgijHcg=HcgHcij⇔(RgijTgij01)(RcgTcg01)=(RcgTcg01)(RcijTcij01)


       将上式展开,可以得到手眼标定的基本方程: 

{RgijRcg=RcgRcij(Rgij−I)Tcg=RcgTcij−Tgij{RgijRcg=RcgRcij(Rgij−I)Tcg=RcgTcij−Tgij


       因此,手眼标定问题也就转化为从上述方程组中求解出RcgRcg和TcgTcg,下面就按照TSAI文献所述求解该方程组。

 


“两步法”手眼标定

       一般用“两步法”求解基本方程,即先从基本方程上式求解出RcgRcg,再代入下式求解出TcgTcg。在TSAI文献中引入旋转轴-旋转角系统来描述旋转运动来进行求解该方程组,具体的公式推导可以查看原始文献,这里只归纳计算步骤,不明白的地方可阅读文献,计算步骤如下:

Step1:利用罗德里格斯变换将旋转矩阵转换为旋转向量

 

{rgij=rodrigues(Rgij)rcij=rodrigues(Rcij){rgij=rodrigues(Rgij)rcij=rodrigues(Rcij)

 

Step2:向量归一化

 

⎧⎩⎨θgij=∥rgij∥2Nrgij=rgijθgijθcij=∥rcij∥2Nrcij=rcijθcij{θgij=‖rgij‖2Nrgij=rgijθgijθcij=‖rcij‖2Nrcij=rcijθcij

 

Step3:修正的罗德里格斯参数表示姿态变化

 

⎧⎩⎨Pgij=2sinθgij2NrgijPcij=2sinθcij2Nrcij{Pgij=2sin⁡θgij2NrgijPcij=2sin⁡θcij2Nrcij

 

Step4:计算初始旋转向量P′cgPcg′

 

skew(Pgij+Pcij)Pcg′=Pcij−Pgijskew(Pgij+Pcij)Pcg′=Pcij−Pgij

 

       其中,skew为反对称运算,假设一个三维向量V=[vx;vy;vz]V=[vx;vy;vz],其反对称矩阵为: 

skew(V)=⎡⎣⎢0vz−vy−vz0vxvy−vx0⎤⎦⎥skew(V)=[0vz−vy−vz0vxvy−vx0]

 

Step5:计算旋转向量PcgPcg

 

Pcg=2Pcg′1+|Pcg′|2−−−−−−−−√Pcg=2Pcg′1+|Pcg′|2

 

Step6:计算旋转矩阵RcgRcg

 

Rcg=(1−|Pcg|22)I+12(PcgPcgT+4−|Pcg|2−−−−−−−−√skew(Pcg))Rcg=(1−|Pcg|22)I+12(PcgPcgT+4−|Pcg|2skew(Pcg))

 

Step7:计算平移向量TcgTcg

 

(Rgij−I)Tcg=RcgTcij−Tgij(Rgij−I)Tcg=RcgTcij−Tgij

 


算法源代码

       根据上述基本计算步骤,在利用OpenCV 2.0开源库的基础上,编写Tsai手眼标定方法的c++程序,其实现函数代码如下:

void Tsai_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij)
{
    CV_Assert(Hgij.size() == Hcij.size());
    int nStatus = Hgij.size();

    Mat Rgij(3, 3, CV_64FC1);
    Mat Rcij(3, 3, CV_64FC1);

    Mat rgij(3, 1, CV_64FC1);
    Mat rcij(3, 1, CV_64FC1);

    double theta_gij;
    double theta_cij;

    Mat rngij(3, 1, CV_64FC1);
    Mat rncij(3, 1, CV_64FC1);

    Mat Pgij(3, 1, CV_64FC1);
    Mat Pcij(3, 1, CV_64FC1);

    Mat tempA(3, 3, CV_64FC1);
    Mat tempb(3, 1, CV_64FC1);

    Mat A;
    Mat b;
    Mat pinA;

    Mat Pcg_prime(3, 1, CV_64FC1);
    Mat Pcg(3, 1, CV_64FC1);
    Mat PcgTrs(1, 3, CV_64FC1);

    Mat Rcg(3, 3, CV_64FC1);
    Mat eyeM = Mat::eye(3, 3, CV_64FC1);

    Mat Tgij(3, 1, CV_64FC1);
    Mat Tcij(3, 1, CV_64FC1);

    Mat tempAA(3, 3, CV_64FC1);
    Mat tempbb(3, 1, CV_64FC1);

    Mat AA;
    Mat bb;
    Mat pinAA;

    Mat Tcg(3, 1, CV_64FC1);

    for (int i = 0; i < nStatus; i++)
    {
        Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
        Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);

        Rodrigues(Rgij, rgij);
        Rodrigues(Rcij, rcij);

        theta_gij = norm(rgij);
        theta_cij = norm(rcij);

        rngij = rgij / theta_gij;
        rncij = rcij / theta_cij;

        Pgij = 2 * sin(theta_gij / 2)*rngij;
        Pcij = 2 * sin(theta_cij / 2)*rncij;

        tempA = skew(Pgij + Pcij);
        tempb = Pcij - Pgij;

        A.push_back(tempA);
        b.push_back(tempb);
    }

    //Compute rotation
    invert(A, pinA, DECOMP_SVD);

    Pcg_prime = pinA * b;
    Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
    PcgTrs = Pcg.t();   
    Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));

    //Computer Translation 
    for (int i = 0; i < nStatus; i++)
    {
        Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
        Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
        Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);
        Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);


        tempAA = Rgij - eyeM;
        tempbb = Rcg * Tcij - Tgij;

        AA.push_back(tempAA);
        bb.push_back(tempbb);
    }

    invert(AA, pinAA, DECOMP_SVD);
    Tcg = pinAA * bb;

    Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));
    Tcg.copyTo(Hcg(Rect(3, 0, 1, 3)));
    Hcg.at<double>(3, 0) = 0.0;
    Hcg.at<double>(3, 1) = 0.0;
    Hcg.at<double>(3, 2) = 0.0;
    Hcg.at<double>(3, 3) = 1.0;

}
  • 2
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
Tsai-Lenz算法是一种常用的手眼标定算法,用于确定机器人手和相机之间的相对位置和姿态关系。这个算法的原理和实现方式在引用和引用中有详细介绍。你可以在这些文章中找到对应的Python、C和Matlab版本的代码实现方式。这个算法实现可以通过传入相机和机器人手的关键点坐标,然后利用几何计算和优化算法来计算出相机和机器人手之间的转换矩阵。这个转换矩阵可以用于校准相机和机器人手的位置和姿态。在机器人应用中,手眼标定算法能够提供准确的相对位置和姿态信息,使机器人能够更精确地执行任务。<span class="em">1</span><span class="em">2</span><span class="em">3</span> #### 引用[.reference_title] - *1* *2* [手眼标定算法Tsai-Lenz代码实现(Python、C++、Matlab)](https://blog.csdn.net/qq_27865227/article/details/114266140)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v92^chatsearchT3_1"}}] [.reference_item style="max-width: 50%"] - *3* [机器人手眼标定原理介绍(含详细推导过程)使用Tsai-Lenz算法](https://blog.csdn.net/qq_27865227/article/details/114011388)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v92^chatsearchT3_1"}}] [.reference_item style="max-width: 50%"] [ .reference_list ]
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值