當(dāng)前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

机械臂手眼标定原理及代码

發(fā)布時間：2023/12/31 编程问答 26 豆豆

生活随笔收集整理的這篇文章主要介紹了机械臂手眼标定原理及代码小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

描述

本文將簡要介紹機(jī)械臂手眼標(biāo)定原理及相關(guān)知識，包會

基礎(chǔ)知識

了解手眼標(biāo)定原理，就必須先了解一句話，叫做“右乘連體左乘基”

這句話實(shí)際上是讀碩士期間，機(jī)器人學(xué)課程上老師講的，我會專門寫一篇文章來介紹這個定則，這篇我們先不介紹，只用最后的結(jié)論。

公式
$Tobject?in?base=Thand?in?baase?Tcamera?in?hand?Tobject?in?cameraT_{object-in-base\; }=T_{hand-in-baase}\cdot T_{camera-in-hand}\cdot T_{object-in-camera}$

手眼標(biāo)定原理

明確坐標(biāo)系

首先明確坐標(biāo)系：機(jī)械臂項(xiàng)目一般存在四個坐標(biāo)系

基坐標(biāo)系：機(jī)械臂的基座為原點(diǎn)的坐標(biāo)系

末端關(guān)節(jié)坐標(biāo)系：一般機(jī)械臂末端關(guān)節(jié)會覆蓋一層法蘭，因此機(jī)械臂末端關(guān)節(jié)坐標(biāo)系又為法蘭末端坐標(biāo)系，實(shí)際上，末端關(guān)節(jié)坐標(biāo)系在基坐標(biāo)系下的位姿，就是我們常說的機(jī)械臂位姿

工具末端坐標(biāo)系：顯然機(jī)械臂在工作過程中，機(jī)械臂末端會搭載工具，而我們往往希望工具移動到某個位置，而不是機(jī)械臂末端運(yùn)動到某個位置，這時我們就需要為工具建立一個它自己的坐標(biāo)系，也就是工具末端坐標(biāo)系

相機(jī)坐標(biāo)系：一般機(jī)械臂AI項(xiàng)目，會搭載相機(jī)作為視覺傳感器，搭載位置可以放在機(jī)械臂末端上，稱為“眼在手上（on hand）”；搭載在機(jī)械臂之外的固定位置，稱為“眼在手下（off hand）”

坐標(biāo)系應(yīng)用

上面提到的四個坐標(biāo)系，在機(jī)械臂項(xiàng)目中會被稱為位姿，如機(jī)械臂位姿，工具位姿，相機(jī)位姿。但實(shí)際上你要知道，這些所說的位姿，實(shí)際上的含義是各個坐標(biāo)系在基坐標(biāo)系下的表示

重要的公式：
$Tobject?in?base=Thand?in?base?Tcamera?in?hand?Tobject?in?cameraT_{object-in-base\; }=T_{hand-in-base}\cdot T_{camera-in-hand}\cdot T_{object-in-camera}$
公式的含義很簡單：
假設(shè)機(jī)械臂搭載了相機(jī)，在關(guān)節(jié)末端也搭載了某個工件（或者沒有搭載，這不是關(guān)鍵）。這時相機(jī)觀察到某個物體，那么物體究竟在機(jī)械臂的什么位置呢？
結(jié)論是：
物體在基坐標(biāo)系下的位姿 = 手在基坐標(biāo)系下的位姿 * 相機(jī)在末端坐標(biāo)系下的位姿 * 物體在相機(jī)中的位姿
這也就是 “左乘基” 的含義，每個位姿左乘它的基坐標(biāo)系
，不在這里詳細(xì)解釋

手眼標(biāo)定原理

在上個公式中， $T_{camera-in-hand}$ 這個矩陣就是手眼矩陣，手眼標(biāo)定求的就是相機(jī)在手的位姿（求出來手在相機(jī)的位姿其實(shí)也一樣，自行變換一下就是了）

顯然，機(jī)械臂項(xiàng)目中，大家大多數(shù)都想知道待檢測物體究竟在機(jī)械臂的什么位置，這是核心。這樣請你回頭看核心公式，我在這兒再次貼出

$Tobject?in?base=Thand?in?base?Tcamera?in?hand?Tobject?in?cameraT_{object-in-base\; }=T_{hand-in-base}\cdot T_{camera-in-hand}\cdot T_{object-in-camera}$

上述公式，我們已知的只有 $T_{hand-in-base}$ 和 $T_{object-in-camera}$ ，我們要求的是手眼矩陣 $T_{camera-in-hand}$ 。我們要利用的就是，在這一過程一直未改變的矩陣 $T_{object-in-base}$

$Tobject=T1pose?Thand?T1det?ect=T2pose?Thand?T2det?ect......=Tnpose?Thand?Tndet?ectT_{object}=T_{1}^{pose}\cdot T^{hand}\cdot T_{1}^{\det ect}=T_{2}^{pose}\cdot T^{hand}\cdot T_{2}^{\det ect}......=T_{n}^{pose}\cdot T^{hand}\cdot T_{n}^{\det ect}$

你可以拿到n個等式，通過解矩陣的方式就能解出手眼矩陣了。

2D相機(jī)手眼標(biāo)定

事實(shí)上，很多人都通過棋盤格的方式（張正友標(biāo)定法）完成手眼標(biāo)定，就是通過相機(jī)檢測棋盤格的角點(diǎn)，來得到棋盤格在相機(jī)中的位姿。張正友標(biāo)定法也是我常用的標(biāo)定法，十分適合2D相機(jī)的標(biāo)定。

3D點(diǎn)云相機(jī)手眼標(biāo)定

有的時候我們會使用到點(diǎn)云相機(jī)，假設(shè)我們?nèi)〔坏?D圖像時，我們該如何完成手眼標(biāo)定呢？有關(guān)這一問題，我另外寫了一篇文章

手眼標(biāo)定步驟

前提：

機(jī)械臂已經(jīng)能夠按照位姿正常移動，機(jī)械臂能夠正確讀取末端（或者工具）末端的位姿，機(jī)械臂搭載相機(jī)成功，相機(jī)能夠檢測到物體，得到物體在相機(jī)中的位姿

將機(jī)械臂移動到某個位姿，記錄機(jī)械臂在當(dāng)前位姿時末端的姿態(tài)，一般的機(jī)械臂會在示教器上存在顯示，否則的話你可以使用代碼進(jìn)行讀取

使用機(jī)械臂上的相機(jī)，采集到待檢測物體在相機(jī)中的位姿，并記錄

這時你會有一對位姿了，一對位姿包括：機(jī)械臂的位姿X + 機(jī)械臂在位姿X時相機(jī)采集到物體在相機(jī)中的位姿

重復(fù)1、2步驟，直到你采了10組以上（不同算法有不同的要求，但有10組數(shù)據(jù)是最基本的）

傳入代碼中進(jìn)行計算，就能標(biāo)定出手眼矩陣了

代碼

這篇代碼是我摘抄的某段網(wǎng)上的代碼，原文鏈接https://blog.csdn.net/hellohake/article/details/104808149。
我驗(yàn)證過，這段可以直接正常運(yùn)行，按照代碼中的位姿可以正確計算出手眼矩陣。使用時，將代碼中的13對數(shù)字，替換成你自己的數(shù)字就好

但是不知道這段代碼標(biāo)定精度是怎么樣，因?yàn)槲颐鎸Φ那闆r比較特殊，屬于特殊的手眼標(biāo)定，不能使用這段代碼。

這段代碼希望自己有機(jī)會完成驗(yàn)證。

#include <opencv2/opencv.hpp> #include <iostream> #include <math.h>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); //數(shù)據(jù)使用的已有的值 // 相機(jī)中13組標(biāo)定板的位姿，x,y,z，rx,ry,rz, Mat_<double> CalPose = (cv::Mat_<double>(13, 6) <<-0.072944147641399, -0.06687830562048944, 0.4340418493881254, -0.2207496117519063, 0.0256862005614321, 0.1926014162476009,-0.01969337858360518, -0.05095294728651902, 0.3671266719105768, 0.1552099329677287, -0.5763323472739464, 0.09956130526058841,0.1358164536530692, -0.1110802522656379, 0.4001396735998251, -0.04486168331242635, -0.004268942058870162, 0.05290073845562016,0.1360676260120161, -0.002373036366121294, 0.3951670952829301, -0.4359637938379769, 0.00807193982932386, 0.06162504121755787,-0.1047666520352697, -0.01377729010376614, 0.4570029374109721, -0.612072103513551, -0.04939465180949879, -0.1075464055169537,0.02866460103085085, -0.1043911269729344, 0.3879127305077527, 0.3137563103168434, -0.02113958397023016, 0.1311397970432597,0.1122741829392126, 0.001044006395747612, 0.3686697279333643, 0.1607160803445018, 0.2468677059920437, 0.1035103912091547,-0.06079521129779342, -0.02815190820828123, 0.4451740202390909, 0.1280935541917056, -0.2674407142401368, 0.1633865613363686,-0.02475533256363622, -0.06950841248698086, 0.2939836207787282, 0.1260629671933584, -0.2637748974005461, 0.1634102148863728,0.1128618887222624, 0.00117877722121125, 0.3362496409334229, 0.1049541359309871, -0.2754352318773509, 0.4251492928748009,0.1510545750008333, -0.0725019944548204, 0.3369908269102371, 0.2615745097093249, -0.1295598776133405, 0.6974394284203849,0.04885313290076512, -0.06488755216394324, 0.2441532410787161, 0.1998243391807502, -0.04919417529483511, -0.05133193756053007,0.08816140480523708, -0.05549965109057759, 0.3164905645998022, 0.164693654482863, 0.1153894876338608, 0.01455551646362294);//機(jī)械臂末端13組位姿,x,y,z,rx,ry,rz Mat_<double> ToolPose = (cv::Mat_<double>(13, 6) <<-0.3969707, -0.460018, 0.3899877, 90.2261, -168.2015, 89.7748,-0.1870185, -0.6207147, 0.2851157, 57.2636, -190.2034, 80.7958,-0.1569776, -0.510021, 0.3899923, 90.225, -178.2038, 81.7772,-0.1569787, -0.5100215, 0.3299975, 90.2252, -156.205, 81.7762,-0.3369613, -0.4100348, 0.3299969, 90.2264, -146.2071, 71.778,-0.2869552, -0.6100449, 0.4299998, 90.2271, -199.2048, 86.7806,-0.2869478, -0.6600489, 0.4299948, 105.2274, -189.2053, 86.7814,-0.286938, -0.6300559, 0.4299997, 75.2279, -189.2056, 86.783,-0.2869343, -0.5700635, 0.2800084, 75.2291, -189.2055, 86.7835,-0.1669241, -0.5700796, 0.280015, 75.2292, -189.205, 101.7845,-0.236909, -0.4700997, 0.3600046, 87.2295, -196.2063, 118.7868,-0.2369118, -0.6201035, 0.2600001, 87.2297, -192.2087, 75.7896,-0.2468983, -0.620112, 0.359992, 97.2299, -190.2082, 80.7908);int main(int argc, char** argv) {//數(shù)據(jù)聲明vector<Mat> R_gripper2base;vector<Mat> T_gripper2base;vector<Mat> R_target2cam;vector<Mat> T_target2cam;Mat R_cam2gripper = Mat(3,3,CV_64FC1); //相機(jī)與機(jī)械臂末端坐標(biāo)系的旋轉(zhuǎn)矩陣與平移矩陣Mat T_cam2gripper = Mat(3,1,CV_64FC1);Mat Homo_cam2gripper = Mat(4,4,CV_64FC1);vector<Mat> Homo_target2cam;vector<Mat> Homo_gripper2base;Mat tempR, tempT, temp;for (int i = 0; i < CalPose.rows; i++) //計算標(biāo)定板與相機(jī)間的齊次矩陣（旋轉(zhuǎn)矩陣與平移向量）{ temp = attitudeVectorToMatrix(CalPose.row(i), false, ""); //注意seq為空，相機(jī)與標(biāo)定板間的為旋轉(zhuǎn)向量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++) //計算機(jī)械臂末端坐標(biāo)系與機(jī)器人基坐標(biāo)系之間的齊次矩陣{temp = attitudeVectorToMatrix(ToolPose.row(j), false, "xyz"); //注意seq不是空，機(jī)械臂末端坐標(biāo)系與機(jī)器人基坐標(biāo)系之間的為歐拉角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 << "手眼標(biāo)定結(jié)果為:" << endl;cout << Homo_cam2gripper << endl;cout << "Homo_cam2gripper 是否包含旋轉(zhuǎn)矩陣：" << isRotatedMatrix(Homo_cam2gripper) << endl;////*************************************************** @note 手眼系統(tǒng)精度測試，原理是標(biāo)定板在機(jī)器人基坐標(biāo)系中位姿固定不變，* 可以根據(jù)這一點(diǎn)進(jìn)行演算**************************************************///使用1,2組數(shù)據(jù)驗(yàn)證標(biāo)定板在機(jī)器人基坐標(biāo)系中位姿固定不變cout << "1 : " << Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;cout << "2 : " << Homo_gripper2base[1] * Homo_cam2gripper * Homo_target2cam[1] << endl;//標(biāo)定板在相機(jī)中的位姿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 << "----手眼系統(tǒng)測試-----" << endl;cout << "機(jī)械臂下標(biāo)定板XYZ為：" << endl;for (int i = 0; i < Homo_target2cam.size(); i++){Mat chessPos{ 0.0,0.0,0.0,1.0 }; //4*1矩陣，單獨(dú)求機(jī)械臂坐標(biāo)系下，標(biāo)定板XYZMat worldPos = Homo_gripper2base[i] * Homo_cam2gripper * Homo_target2cam[i] * chessPos;cout << i << ": " << worldPos.t() << endl;}waitKey(0);return 0; }/************************************************** * @brief 將旋轉(zhuǎn)矩陣與平移向量合成為齊次矩陣 * @note * @param Mat& R 3*3旋轉(zhuǎn)矩陣 * @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 齊次矩陣分解為旋轉(zhuǎn)矩陣與平移矩陣 * @note * @param const Mat& HomoMtr 4*4齊次矩陣 * @param Mat& R 輸出旋轉(zhuǎn)矩陣 * @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 檢查是否是旋轉(zhuǎn)矩陣 * @note * @param * @param * @param * @return true : 是旋轉(zhuǎn)矩陣， false : 不是旋轉(zhuǎn)矩陣 **************************************************/ bool isRotatedMatrix(Mat& R) //旋轉(zhuǎn)矩陣的轉(zhuǎn)置矩陣是它的逆矩陣，逆矩陣 * 矩陣 = 單位矩陣 {Mat temp33 = R({ 0,0,3,3 }); //無論輸入是幾階矩陣，均提取它的三階矩陣Mat Rt;transpose(temp33, Rt); //轉(zhuǎn)置矩陣Mat shouldBeIdentity = Rt * temp33;//是旋轉(zhuǎn)矩陣則乘積為單位矩陣Mat I = Mat::eye(3, 3, shouldBeIdentity.type());return cv::norm(I, shouldBeIdentity) < 1e-6; }/************************************************** * @brief 歐拉角轉(zhuǎn)換為旋轉(zhuǎn)矩陣 * @note * @param const std::string& seq 指定歐拉角的排列順序；（機(jī)械臂的位姿類型有xyz,zyx,zyz幾種，需要區(qū)分） * @param const Mat& eulerAngle 歐拉角（1*3矩陣）, 角度值 * @param * @return 返回3*3旋轉(zhuǎn)矩陣 **************************************************/ Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq) {CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);//檢查參數(shù)是否正確eulerAngle /= (180 / CV_PI); //度轉(zhuǎn)弧度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方向的旋轉(zhuǎn)矩陣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);//按順序合成后的旋轉(zhuǎn)矩陣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)) //歐拉角特殊情況下會出現(xiàn)死鎖{cout << "Euler Angle convert to RotatedMatrix failed..." << endl;exit(-1);}return rotMat; }/************************************************** * @brief 將四元數(shù)轉(zhuǎn)換為旋轉(zhuǎn)矩陣 * @note * @param const Vec4d& q 歸一化的四元數(shù): q = q0 + q1 * i + q2 * j + q3 * k; * @return 返回3*3旋轉(zhuǎn)矩陣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;//根據(jù)公式得來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 將采集的原始數(shù)據(jù)轉(zhuǎn)換為齊次矩陣（從機(jī)器人控制器中獲得的） * @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 原始數(shù)據(jù)是否使用四元數(shù)表示 * @param string& seq 原始數(shù)據(jù)使用歐拉角表示時，坐標(biāo)系的旋轉(zhuǎn)順序 * @return 返回轉(zhuǎn)換完的齊次矩陣 **************************************************/ Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq) {CV_Assert(m.total() == 6 || m.total() == 10);//if (m.cols == 1) //轉(zhuǎn)置矩陣為行矩陣// m = m.t(); Mat temp = Mat::eye(4, 4, CV_64FC1);if (useQuaternion){Vec4d quaternionVec = m({ 3,0,4,1 }); //讀取存儲的四元數(shù)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旋轉(zhuǎn)向量，否則，傳入的是歐拉角if (0 == seq.compare("")){Rodrigues(rotVec, temp({ 0,0,3,3 })); //羅德利斯轉(zhuǎn)換}else{eulerAngleToRotateMatrix(rotVec, seq).copyTo(temp({ 0,0,3,3 }));}}//存入平移矩陣temp({ 3,0,1,3 }) = m({ 0,0,3,1 }).t();return temp; //返回轉(zhuǎn)換結(jié)束的齊次矩陣 }

總結(jié)

以上是生活随笔為你收集整理的机械臂手眼标定原理及代码的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

上一篇：恩智浦智能汽车竞赛电磁组总结
下一篇：华中师范大学计算机考研论坛,2020年华