描述

本文將簡要介紹機械臂手眼標定原理及相關知識，包會

基礎知識

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

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

公式
$T_{object?in?base}=T_{hand?in?baase}?T_{camera?in?hand}?T_{object?in?camera}$

手眼標定原理

明確坐標系

首先明確坐標系：機械臂項目一般存在四個坐標系

基坐標系：機械臂的基座為原點的坐標系

末端關節坐標系：一般機械臂末端關節會覆蓋一層法蘭，因此機械臂末端關節坐標系又為法蘭末端坐標系，實際上，末端關節坐標系在基坐標系下的位姿，就是我們常說的機械臂位姿

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

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

坐標系應用

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

• 重要的公式：
  $T_{object?in?base}=T_{hand?in?base}?T_{camera?in?hand}?T_{object?in?camera}$

• 公式的含義很簡單：
  假設機械臂搭載了相機，在關節末端也搭載了某個工件（或者沒有搭載，這不是關鍵）。這時相機觀察到某個物體，那么物體究竟在機械臂的什么位置呢？

• 結論是：
  物體在基坐標系下的位姿 = 手在基坐標系下的位姿 * 相機在末端坐標系下的位姿 * 物體在相機中的位姿
  這也就是 “左乘基” 的含義，每個位姿左乘它的基坐標系
  ，不在這里詳細解釋

手眼標定原理

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

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

$T_{object?in?base}=T_{hand?in?base}?T_{camera?in?hand}?T_{object?in?camera}$

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

$T_{object}=T_1^{pose}?T^{hand}?T_1^{\det ect}=T_2^{pose}?T^{hand}?T_2^{\det ect}......=T_n^{pose}?T^{hand}?T_n^{\det ect}$

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

2D相機手眼標定

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

3D點云相機手眼標定

有的時候我們會使用到點云相機，假設我們取不到2D圖像時，我們該如何完成手眼標定呢？有關這一問題，我另外寫了一篇文章

手眼標定步驟

前提：

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

將機械臂移動到某個位姿，記錄機械臂在當前位姿時末端的姿態，一般的機械臂會在示教器上存在顯示，否則的話你可以使用代碼進行讀取

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

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

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

傳入代碼中進行計算，就能標定出手眼矩陣了

代碼

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

但是不知道這段代碼標定精度是怎么樣，因為我面對的情況比較特殊，屬于特殊的手眼標定，不能使用這段代碼。

這段代碼希望自己有機會完成驗證。

#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); //數據使用的已有的值 // 相機中13組標定板的位姿，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);//機械臂末端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) {//數據聲明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);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), false, ""); //注意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), false, "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 << "手眼標定結果為:" << endl;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() == 6 || 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; //返回轉換結束的齊次矩陣 }

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。