利用Tsai-lenz算法实现手眼标定

自己的理解

手眼标定我主要参考了 link.通过该作者的连续几篇内容，我基本搞清楚了利用Tsai-lenz算法实现手眼标定的基本原理和算法所做的工作。以防后期忘了，因此写出来以便自己查阅。
按照我的理解，手眼标定说白了就是各种坐标系之间的转换。通过坐标系的转换从而使得相机和机械臂的末端可以看作是同一个点。
而要进行手眼标定我们需要进行标定数据的获取。标定数据获取的精确度在上文链接的相关文章中也有提到。按照我自己实践来看，标定数据的精确度与标定的结果有着非常大的联系，影响着标定结果的准确度。

算法的推导

如何计算相机和机械臂末端的变换矩阵（包括旋转矩阵和偏移矩阵）？这方面的原理和推导。上面链接中也已经说明的非常详细了。（哈哈哈，怎么都是看别人的）
在理解之后我们就需要获取标定所需要的数据了。

标定数据的获取和使用

标定的数据需要通过相机和机械臂末端来获取，有的工业机械臂会有示教器？（或许是示教器，也可能记错了。）获取的数据是机械臂相对于世界坐标系的坐标和四元数，具体四元数请参考百度。同样，相机获取的数据也是标定板相对于相机坐标系的坐标和四元数。其实如果获取的是欧拉角也可以，因为四元数和欧拉角是可以相互转化的。但要注意的是，欧拉角可能有多种，机械臂有的是xyz的有的是yzx。（也许是这样，没有考证，凭记忆写的，需要用时还要查证）
然后在以下的程序中输入多组标定数据，一组数据分别对应机械臂和相机的数据。推荐输入的标定数据组数在15组左右。以下程序参考了我师兄的程序link
师兄使用的是欧拉角，而我使用的四元数，所以可以对照两份代码的不同进行修改。当然代码的主体是一样的。同时，代码也不是单纯参考师兄的，还参考了另外一位博主的，但是找不到了，惭愧。
归根结底，代码是想把位姿转换称旋转矩阵然后送入opencv内置的一个函数calibrateHandEye()进行求解，用的算法就是上文所说的Tsai-lenz算法。仔细阅读代码可对上述所说有所明白。

//#include <opencv2/opencv.hpp>
#include <iostream>
#include <math.h>
#include<eigen3\Eigen\Dense>using namespace std;
using namespace cv;Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T);
void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T);
bool isRotatedMatrix(Mat& R);
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq);
Mat quaternionToRotatedMatrix(const Vec4d& q);
Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq);
Mat_<double> q = (cv::Mat_<double>(1, 4) << -0.774647132989, -0.138703323173, 0.59915243478, 0.147307730246);
//数据使用的已有的值
//相机中13组标定板的位姿，x,y,z,w,q1,q2,q3
Mat_<double> CalPose = (cv::Mat_<double>(13, 7) <<0.0452462647436,    0.0327054799247,    0.172991838627, 0.0632869471649,0.532853235423, 0.837595946624, 0.102446190836,0.0388497651175,    0.0479508427745,    0.171064977207, 0.0828774100789,0.539034747402, 0.833179689483, 0.0915668118087,0.028328088994,     0.0252225698526,    0.164136104336, 0.0766029914989,0.53219226938,  0.833107370839, 0.129751604074,0.00447711600106,   0.0147803504366,    0.149693748863, 0.0996515424584,0.527920850231, 0.826882414545, 0.16623663403,-0.000871782710522,    0.0280600137579,    0.148260774669, 0.119569784017, 0.533038536357, 0.822904197206, 0.1562103317,-0.00454990691132, 0.0375296320808,    0.146823354797, 0.133753766388, 0.536198873306, 0.820126393385, 0.148301709751,0.0159389062622,    - 0.0109475052566,   0.149740140779, 0.0630777786609,0.516394185309, 0.832275091103, 0.191510866397,0.0219877442865,    - 0.023570535672,   0.148370142112, 0.0459609200971,0.510359257262, 0.833721322589, 0.205742019556,-0.00985416634311,    0.00845427020451,   0.13914292002,  0.111806619583, 0.525036766535, 0.822392299937, 0.188431893816,-0.0235599361194,    0.00237936186087,   0.127320172953, 0.123256282431, 0.521372909897, 0.817810911646, 0.210151113391,0.00252505272966,    0.0141049454382,    0.139188107322, 0.0984359661148,0.527910010541, 0.826799153439, 0.167405320161,-0.0227503625961,    0.00290110800445,   0.11921334782,  0.120014391923, 0.521919452416, 0.819086060912, 0.205656645262,-0.0158975128932,    - 0.0131103269009,  0.119844419659, 0.101578321977, 0.514675553878, 0.82376078019,  0.214963010226);//机械臂末端13组位姿,x,y,z,rx,ry,rz
Mat_<double> ToolPose = (cv::Mat_<double>(13, 7) <<-0.119068317273,  0.0183054173689,    0.326418520745, 0.147307730246, - 0.774647132989,   - 0.138703323173,   0.59915243478,-0.117923938994,  0.0314365363501,    0.326422378073, 0.161032347485, - 0.777629186448,   - 0.120838792607,   0.595616837949,-0.127588063019, 0.0188505930921,    0.308755034618, 0.142106184689, - 0.790919630198,   - 0.141580853503,   0.578106246949,-0.138323018987, 0.0195375193875,    0.282058685087, 0.134329642947, - 0.813716329775,   - 0.145670932314,   0.546444197666,-0.137298104833, 0.0317269498021,    0.282066453389, 0.145254214409, - 0.816451379011,   - 0.129409496569,   0.54365571928,-0.136209260479,  0.0408421388867,    0.282066453389, 0.153394092149, - 0.818300964964,   - 0.117146074751,   0.541415349249,-0.13890306169,  - 0.00522042120178, 0.282066453389, 0.112124675281, - 0.807139447744,   - 0.178509338653,   0.551442095874,-0.13844987551,  - 0.0174444690322,  0.282066453389, 0.101078345521, - 0.803409553394,   - 0.194609253649,   0.553573388067,-0.143939777501, 0.0198969339143,    0.264962341081, 0.129353382177, -0.827311957668,    - 0.148104812179,   0.526201094477,-0.148694820656, 0.0202012075591,    0.247613037613, 0.124309474069, - 0.840381768577,   - 0.150463749095,   0.505634549757,-0.150318771474, 0.0203051236368,    0.272278728281, 0.134340699038, - 0.813716006995,   - 0.145689454114,   0.546437022448,-0.158882254384, 0.0208530980868,    0.240284392514, 0.125332497512, - 0.837807345188,   - 0.150012383713,   0.509771421472,-0.159518090201, 0.00453523383602,   0.240292118299, 0.112544043611, - 0.833788262373,   - 0.1709033633,     0.512760189748);int main(int argc, char** argv)
{//数据声明vector<Mat> R_gripper2base;vector<Mat> T_gripper2base;vector<Mat> R_target2cam;vector<Mat> T_target2cam;Mat R_cam2gripper = Mat(3, 3, CV_64FC1);                //相机与机械臂末端坐标系的旋转矩阵与平移矩阵Mat T_cam2gripper = Mat(3, 1, CV_64FC1);Mat Homo_cam2gripper = Mat(4, 4, CV_64FC1);//Eigen::Quaterniond quaternion(//  0.147307730246, -0.774647132989, -0.138703323173, 0.59915243478,//  0.161032347485, - 0.777629186448, -0.120838792607, 0.595616837949);//Eigen::Vector3d eulerAngle = quaternion.matrix().eulerAngles(0, 1, 2);//cout << eulerAngle;vector<Mat> Homo_target2cam;vector<Mat> Homo_gripper2base;Mat tempR, tempT, temp;for (int i = 0; i < CalPose.rows; i++)                //计算标定板与相机间的齐次矩阵（旋转矩阵与平移向量）{temp = attitudeVectorToMatrix(CalPose.row(i), true, "");   //注意seq为空，相机与标定板间的为旋转向量Homo_target2cam.push_back(temp);HomogeneousMtr2RT(temp, tempR, tempT);/*cout << i << "::" << temp << endl;cout << i << "::" << tempR << endl;cout << i << "::" << tempT << endl;*/R_target2cam.push_back(tempR);T_target2cam.push_back(tempT);}for (int j = 0; j < ToolPose.rows; j++)                //计算机械臂末端坐标系与机器人基坐标系之间的齐次矩阵{temp = attitudeVectorToMatrix(ToolPose.row(j), true, "xyz");  //注意seq不是空，机械臂末端坐标系与机器人基坐标系之间的为欧拉角Homo_gripper2base.push_back(temp);HomogeneousMtr2RT(temp, tempR, tempT);/*cout << j << "::" << temp << endl;cout << j << "::" << tempR << endl;cout << j << "::" << tempT << endl;*/R_gripper2base.push_back(tempR);T_gripper2base.push_back(tempT);}//TSAI计算速度最快calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI);Homo_cam2gripper = R_T2HomogeneousMatrix(R_cam2gripper, T_cam2gripper);cout << Homo_cam2gripper << endl;cout << "Homo_cam2gripper 是否包含旋转矩阵：" << isRotatedMatrix(Homo_cam2gripper) << endl;////*************************************************** @note   手眼系统精度测试，原理是标定板在机器人基坐标系中位姿固定不变，*        可以根据这一点进行演算**************************************************///使用1,2组数据验证  标定板在机器人基坐标系中位姿固定不变cout << "1 : " << Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;cout << "2 : " << Homo_gripper2base[1] * Homo_cam2gripper * Homo_target2cam[1] << endl;//标定板在相机中的位姿cout << "3 : " << Homo_target2cam[1] << endl;cout << "4 : " << Homo_cam2gripper.inv() * Homo_gripper2base[1].inv() * Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;cout << "----手眼系统测试-----" << endl;cout << "机械臂下标定板XYZ为：" << endl;for (int i = 0; i < Homo_target2cam.size(); i++){Mat chessPos{ 0.0,0.0,0.0,1.0 };  //4*1矩阵，单独求机械臂坐标系下，标定板XYZMat worldPos = Homo_gripper2base[i] * Homo_cam2gripper * Homo_target2cam[i] * chessPos;cout << i << ": " << worldPos.t() << endl;}waitKey(0);return 0;
}/**************************************************
* @brief   将旋转矩阵与平移向量合成为齐次矩阵
* @note
* @param   Mat& R   3*3旋转矩阵
* @param   Mat& T   3*1平移矩阵
* @return  Mat      4*4齐次矩阵
**************************************************/
Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T)
{Mat HomoMtr;Mat_<double> R1 = (Mat_<double>(4, 3) <<R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),0, 0, 0);Mat_<double> T1 = (Mat_<double>(4, 1) <<T.at<double>(0, 0),T.at<double>(1, 0),T.at<double>(2, 0),1);cv::hconcat(R1, T1, HomoMtr);      //矩阵拼接return HomoMtr;
}/**************************************************
* @brief    齐次矩阵分解为旋转矩阵与平移矩阵
* @note
* @param   const Mat& HomoMtr  4*4齐次矩阵
* @param   Mat& R              输出旋转矩阵
* @param   Mat& T              输出平移矩阵
* @return
**************************************************/
void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T)
{//Mat R_HomoMtr = HomoMtr(Rect(0, 0, 3, 3)); //注意Rect取值//Mat T_HomoMtr = HomoMtr(Rect(3, 0, 1, 3));//R_HomoMtr.copyTo(R);//T_HomoMtr.copyTo(T);/*HomoMtr(Rect(0, 0, 3, 3)).copyTo(R);HomoMtr(Rect(3, 0, 1, 3)).copyTo(T);*/Rect R_rect(0, 0, 3, 3);Rect T_rect(3, 0, 1, 3);R = HomoMtr(R_rect);T = HomoMtr(T_rect);}/**************************************************
* @brief   检查是否是旋转矩阵
* @note
* @param
* @param
* @param
* @return  true : 是旋转矩阵， false : 不是旋转矩阵
**************************************************/
bool isRotatedMatrix(Mat& R)        //旋转矩阵的转置矩阵是它的逆矩阵，逆矩阵 * 矩阵 = 单位矩阵
{Mat temp33 = R({ 0,0,3,3 });  //无论输入是几阶矩阵，均提取它的三阶矩阵Mat Rt;transpose(temp33, Rt);  //转置矩阵Mat shouldBeIdentity = Rt * temp33;//是旋转矩阵则乘积为单位矩阵Mat I = Mat::eye(3, 3, shouldBeIdentity.type());return cv::norm(I, shouldBeIdentity) < 1e-6;
}/**************************************************
* @brief   欧拉角转换为旋转矩阵
* @note
* @param    const std::string& seq  指定欧拉角的排列顺序；（机械臂的位姿类型有xyz,zyx,zyz几种，需要区分）
* @param    const Mat& eulerAngle   欧拉角（1*3矩阵）, 角度值
* @param
* @return   返回3*3旋转矩阵
**************************************************/
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq)
{CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);//检查参数是否正确eulerAngle /= (180 / CV_PI);        //度转弧度Matx13d m(eulerAngle);                //<double, 1, 3>auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);auto rxs = sin(rx), rxc = cos(rx);auto rys = sin(ry), ryc = cos(ry);auto rzs = sin(rz), rzc = cos(rz);//XYZ方向的旋转矩阵Mat RotX = (Mat_<double>(3, 3) << 1, 0, 0,0, rxc, -rxs,0, rxs, rxc);Mat RotY = (Mat_<double>(3, 3) << ryc, 0, rys,0, 1, 0,-rys, 0, ryc);Mat RotZ = (Mat_<double>(3, 3) << rzc, -rzs, 0,rzs, rzc, 0,0, 0, 1);//按顺序合成后的旋转矩阵cv::Mat rotMat;if (seq == "zyx") rotMat = RotX * RotY * RotZ;else if (seq == "yzx") rotMat = RotX * RotZ * RotY;else if (seq == "zxy") rotMat = RotY * RotX * RotZ;else if (seq == "yxz") rotMat = RotZ * RotX * RotY;else if (seq == "xyz") rotMat = RotZ * RotY * RotX;else if (seq == "xzy") rotMat = RotY * RotZ * RotX;else{cout << "Euler Angle Sequence string is wrong...";}if (!isRotatedMatrix(rotMat))      //欧拉角特殊情况下会出现死锁{cout << "Euler Angle convert to RotatedMatrix failed..." << endl;exit(-1);}return rotMat;
}/**************************************************
* @brief   将四元数转换为旋转矩阵
* @note
* @param   const Vec4d& q   归一化的四元数: q = q0 + q1 * i + q2 * j + q3 * k;
* @return  返回3*3旋转矩阵R
**************************************************/
Mat quaternionToRotatedMatrix(const Vec4d& q)
{double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];double q0q0 = q0 * q0, q1q1 = q1 * q1, q2q2 = q2 * q2, q3q3 = q3 * q3;double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;double q1q2 = q1 * q2, q1q3 = q1 * q3;double q2q3 = q2 * q3;//根据公式得来Mat RotMtr = (Mat_<double>(3, 3) << (q0q0 + q1q1 - q2q2 - q3q3), 2 * (q1q2 + q0q3), 2 * (q1q3 - q0q2),2 * (q1q2 - q0q3), (q0q0 - q1q1 + q2q2 - q3q3), 2 * (q2q3 + q0q1),2 * (q1q3 + q0q2), 2 * (q2q3 - q0q1), (q0q0 - q1q1 - q2q2 + q3q3));//这种形式等价/*Mat RotMtr = (Mat_<double>(3, 3) << (1 - 2 * (q2q2 + q3q3)), 2 * (q1q2 - q0q3), 2 * (q1q3 + q0q2),2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3), 2 * (q2q3 - q0q1),2 * (q1q3 - q0q2), 2 * (q2q3 + q0q1), (1 - 2 * (q1q1 + q2q2)));*/return RotMtr;
}/**************************************************
* @brief      将采集的原始数据转换为齐次矩阵（从机器人控制器中获得的）
* @note
* @param     Mat& m    1*6//1*10矩阵 ， 元素为： x,y,z,rx,ry,rz  or x,y,z, q0,q1,q2,q3,rx,ry,rz
* @param     bool useQuaternion      原始数据是否使用四元数表示
* @param     string& seq         原始数据使用欧拉角表示时，坐标系的旋转顺序
* @return    返回转换完的齐次矩阵
**************************************************/
Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq)
{CV_Assert(m.total() == 7 || m.total() == 10);//if (m.cols == 1)  //转置矩阵为行矩阵//    m = m.t(); Mat temp = Mat::eye(4, 4, CV_64FC1);if (useQuaternion){Vec4d quaternionVec = m({ 3,0,4,1 });   //读取存储的四元数quaternionToRotatedMatrix(quaternionVec).copyTo(temp({ 0,0,3,3 }));}else{Mat rotVec;if (m.total() == 6){rotVec = m({ 3,0,3,1 });   //读取存储的欧拉角}if (m.total() == 10){rotVec = m({ 7,0,3,1 });}//如果seq为空，表示传入的是3*1旋转向量，否则，传入的是欧拉角if (0 == seq.compare("")){Rodrigues(rotVec, temp({ 0,0,3,3 }));   //罗德利斯转换}else{eulerAngleToRotateMatrix(rotVec, seq).copyTo(temp({ 0,0,3,3 }));}}//存入平移矩阵temp({ 3,0,1,3 }) = m({ 0,0,3,1 }).t();return temp;   //返回转换结束的齐次矩阵
}//#define _USE_MATH_DEFINES
//#include <cmath>struct Quaternion {double w, x, y, z;
};struct EulerAngles {double roll, pitch, yaw;
};EulerAngles ToEulerAngles(Quaternion q) {EulerAngles angles;// roll (x-axis rotation)double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);angles.roll = std::atan2(sinr_cosp, cosr_cosp);// pitch (y-axis rotation)double sinp = 2 * (q.w * q.y - q.z * q.x);if (std::abs(sinp) >= 1)angles.pitch = std::copysign(CV_PI / 2, sinp); // use 90 degrees if out of rangeelseangles.pitch = std::asin(sinp);// yaw (z-axis rotation)double siny_cosp = 2 * (q.w * q.z + q.x * q.y);double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);angles.yaw = std::atan2(siny_cosp, cosy_cosp);return angles;
}

利用Tsai-lenz算法实现手眼标定相关推荐

Tsai两步法求手眼标定矩阵
Tsai两步法求手眼标定矩阵 Tsai方法介绍术语概念齐次变换矩阵和坐标系的定义旋转轴和旋转角度引理的证明和解释 AX=XB构造引理1 引理2 引理3 引理4 引理5 引理6 --得到公式( ...
Tsai手眼标定算法
A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration(Tsai手眼标定算法) 符号定义 ...
matlab tsai手眼标定程序代码_标定系列一 | 机器人手眼标定的基础理论分析
旷视MegMaster机器人系列是旷视自主研发的一系列AI智能机器人硬件设备,基于旷视全球领先的人工智能算法及机器人技术,可实现搬运.分拣.托举.存储等功能,被广泛应用于物流仓储.工厂制造等场景.旷视 ...
经典手眼标定算法之Navy的OpenCV实现
经典手眼标定算法之Navy的OpenCV实现在我的上一篇博客中已经介绍了Tsai的手眼标定算法,下面主要介绍Frank C. Park and Bryan J. Martin在文献Robot sen ...
经典手眼标定算法之Tsai-Lenz的OpenCV实现
本文主要是讲解经典手眼标定问题中的TSAI-LENZ 文献方法,参考文献为"A New Technique for Fully Autonomous and Efficient 3D Rob ...
经典手眼标定算法之Navy
文章转载自:经典手眼标定算法之Navy的OpenCV实现 :http://blog.csdn.net/yunlinwang/article/details/51871520 在我的上一篇博客中已经介绍 ...
手眼标定算法---Navy算法（Robot sensor calibration: solving AX=XB on the Euclidean group）
本文主要介绍Frank C. Park and Bryan J. Martin在文献Robot sensor calibration: solving AX=XB on the Euclidean g ...
手眼标定算法Tsai-Lenz代码实现（Python、C++、Matlab）
你好,我是小智. 上一节介绍了手眼标定算法Tsai的原理,这一节介绍算法的代码实现,分别有Python.C++.Matlab版本的算法实现方式. 该算法适用于将相机装在手抓上和将相机装在外部两种情况 ...
halocn标定找旋转中心_一种利用旋转中心进行手眼标定的原理性介绍
首先,我们要了解一下常规的手眼标定流程是怎么样的. (一)如果吸嘴中心就是法兰盘的中心则是下面这样的:按九宫格走九个点,取得九组吸嘴的像素坐标与法兰盘的机械坐标 (图1) 进行标定 (二)如果吸嘴位 ...

利用Tsai-lenz算法实现手眼标定

利用Tsai-lenz算法实现手眼标定

目录

自己的理解

算法的推导

标定数据的获取和使用

利用Tsai-lenz算法实现手眼标定相关推荐

最新文章

热门文章