記錄自己手眼標(biāo)定（眼在手上）結(jié)果的應(yīng)用過(guò)程，本人使用的是2D相機(jī)。

內(nèi)容

• 1.核心公式
• • 1.1 命名規(guī)定
  • 1.2 歐拉角、四元數(shù)、旋轉(zhuǎn)矩陣的關(guān)系
  • 1.3 公式
• 2.手眼標(biāo)定的運(yùn)算法則和對(duì)應(yīng)的物理意義
• 3.場(chǎng)景需求
• • 3.1 目標(biāo)(aruco標(biāo)記圖)在水平面上不動(dòng)，指定機(jī)械臂末端運(yùn)動(dòng)到目標(biāo)上方的固定距離
  • 3.2 上小節(jié)中是相機(jī)識(shí)別到了目標(biāo)，將機(jī)械臂移動(dòng)到指定目標(biāo)上方的固定高度位置。當(dāng)前場(chǎng)景是不斷更換目標(biāo)位姿，但仍要求機(jī)械臂移動(dòng)到指定目標(biāo)上方的固定距離位置，且位置和姿態(tài)都要和目標(biāo)同步
  • • 3.2.1 公式
    • 3.2.2 計(jì)算過(guò)程
    • 3.2.3 執(zhí)行步驟

1.核心公式

1.1 命名規(guī)定

??表示以base(機(jī)器臂基底)為基準(zhǔn)，end(機(jī)械臂末端)在其下的位姿描述，即機(jī)械臂末端坐標(biāo)系到機(jī)械臂基底坐標(biāo)系的變換矩陣。
（本文所有公式中的變量都以這個(gè)為標(biāo)準(zhǔn)，有的資料中寫(xiě)的和這個(gè)相反，容易記混了，注意哪個(gè)是基準(zhǔn)就行。）

1.2 歐拉角、四元數(shù)、旋轉(zhuǎn)矩陣的關(guān)系

??關(guān)于這幾個(gè)的關(guān)系轉(zhuǎn)換如下圖所示，在之后的計(jì)算中，需要頻繁進(jìn)行轉(zhuǎn)換。

1.3 公式

??上面是三個(gè)4×4的矩陣相乘，得到的結(jié)果就是在機(jī)械臂基底坐標(biāo)系base下，識(shí)別到的目標(biāo)target的位姿。計(jì)算結(jié)果仍然是個(gè)4×4矩陣，包含一個(gè)旋轉(zhuǎn)矩陣+一個(gè)平移向量，將旋轉(zhuǎn)矩陣轉(zhuǎn)為歐拉角，再通過(guò)運(yùn)動(dòng)學(xué)逆解計(jì)算將歐拉角轉(zhuǎn)為6個(gè)關(guān)節(jié)角度值，將這6個(gè)值輸入到示教器中，即可將機(jī)械臂末端運(yùn)動(dòng)到當(dāng)前目標(biāo)在基底下的位姿狀態(tài)。
??（PS：如果直接輸入X、Y、Z、RX、RY、RZ可能會(huì)得到好幾種不同的關(guān)節(jié)角度值，雖然它們的位置和歐拉角都相同，萬(wàn)一解出一個(gè)值機(jī)器人會(huì)撞到工作平面，所以需要我們通過(guò)運(yùn)動(dòng)學(xué)逆解直接輸入關(guān)節(jié)角值，來(lái)避免這種情況。）

上式中各個(gè)參數(shù)的含義和計(jì)算方法：

• 1.T：變換矩陣，由旋轉(zhuǎn)矩陣（3×3）和平移向量（3×1）組成，在旋轉(zhuǎn)矩陣下面加一行[0, 0, 0, 1]構(gòu)成齊次矩陣；格式如下：
  
• 2.
  ??這個(gè)值由示教器面板直接讀取或通過(guò)示教器的SDK獲取，但獲得的位姿形式是（X、Y、Z、RX、RY、RZ），需要將其轉(zhuǎn)換為旋轉(zhuǎn)矩陣R和平移向量t，然后再拼接為齊次矩陣（4×4），將歐拉角（RX、RY、RZ）轉(zhuǎn)為旋轉(zhuǎn)矩陣，這里要注意歐拉角的排列順序（機(jī)械臂的位姿類(lèi)型有zyx,yzx,zxy,yxz,xyz,xzy幾種，需要區(qū)分），之后再和平移向量（XYZ）進(jìn)行拼接。

舉例：獲取到當(dāng)前機(jī)器人的位姿（m，弧度）和6個(gè)關(guān)節(jié)角（弧度） （X、Y、Z、RX、RY、RZ）-> （0.243749, 0.508627, 0.369233, -3.136813, 0.000506, 2.746928） 關(guān)節(jié)角：（J1、J2、J3、J4、J5、J6）->（1.341016、-0.193995、1.538816、0.152183、1.571031 、0.166330）// 歐拉角轉(zhuǎn)換為旋轉(zhuǎn)矩陣 cv::Mat eulerAngleToRotateMatrix(const cv::Mat& eulerAngle, const std::string& seq) {cv::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)矩陣cv::Mat RotX = (cv::Mat_<double>(3, 3) << 1, 0, 0,0, rxc, -rxs,0, rxs, rxc);cv::Mat RotY = (cv::Mat_<double>(3, 3) << ryc, 0, rys,0, 1, 0,-rys, 0, ryc);cv::Mat RotZ = (cv::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..." << endl;}return rotMat; }cv::Mat_<double> CalPose = (cv::Mat_<double>(1, 6) << 0.243749, 0.508627, 0.369233, -3.136813, 0.000506, 2.746928); cv::Mat T_end2base; T_end2base = attitudeVectorToMatrix(CalPose.row(0), false, "xyz"); 上面的位姿轉(zhuǎn)換為： T_end2base： [-0.9231254383299377, 0.3844964842228446, -0.001370667148604696, 0.243749;0.3844985943695793, 0.923114082176005, -0.004606757663443459, 0.5086270000000001;-0.000505999978407631, -0.004779634779333107, -0.9999884494609915, 0.369233;0, 0, 0, 1]

• 3.
  ??這是手眼標(biāo)定的結(jié)果。

關(guān)于手眼標(biāo)定，參考：https://blog.csdn.net/qq_45445740/article/details/122170029

舉例：T_cam2end： [-0.99798, 0.0607526, -0.0185534, -0.017052;-0.0603958, -0.997989, -0.0192232, 0.132618;-0.019684, -0.0180638, 0.9996429, -0.026936;0, 0, 0, 1]

• 4.
  ??根據(jù)自己選擇的目標(biāo)放在相機(jī)視野內(nèi)進(jìn)行拍照識(shí)別，我選擇的是對(duì)aruco標(biāo)記圖進(jìn)行識(shí)別，會(huì)得到目標(biāo)（標(biāo)記圖）在相機(jī)坐標(biāo)系下的位姿（X、Y、Z、w、x、y、z），需要將后面的四元數(shù)轉(zhuǎn)為旋轉(zhuǎn)矩陣，再和前面的（X、Y、Z）組成齊次矩陣。

四元數(shù)->旋轉(zhuǎn)矩陣，參考：https://zhuanlan.zhihu.com/p/45404840

舉例： aruco的識(shí)別結(jié)果： Find Board！ （X、Y、Z、w、x、y、z）->（-0.00257137, -0.0155088, 0.779451, 0.0245001, -0.702148, 0.711493, -0.0128749）// 將四元數(shù)轉(zhuǎn)換為旋轉(zhuǎn)矩陣 // 歸一化的四元數(shù): q = q0 + q1 * i + q2 * j + q3 * k; // q0=w, q1=x, q2=y, q3=z cv::Mat quaternionToRotatedMatrix(const cv::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ù)公式得來(lái)cv::Mat RotMtr = (cv::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; }四元數(shù)->旋轉(zhuǎn)矩陣結(jié)果： [-0.01277610419802011, -0.99851590125302, 0.052943469869;-0.99977764660298, 0.01364484609198002, 0.0160845899782;-0.0167831287282, -0.052726194881, -0.9984682059060002]T_target2cam： [-0.01277610419802011, -0.99851590125302, 0.052943469869, -0.002571;-0.99977764660298, 0.01364484609198002, 0.0160845899782, -0.015509;-0.0167831287282, -0.052726194881, -0.9984682059060002, 0.779451;0, 0, 0, 1]

2.手眼標(biāo)定的運(yùn)算法則和對(duì)應(yīng)的物理意義

參考：https://zhuanlan.zhihu.com/p/388952714

3.場(chǎng)景需求

??前提都是水平面上的目標(biāo)移動(dòng)，非水平面的還沒(méi)做。。。若有大佬做過(guò)的迫切希望得到指導(dǎo)。

3.1 目標(biāo)(aruco標(biāo)記圖)在水平面上不動(dòng)，指定機(jī)械臂末端運(yùn)動(dòng)到目標(biāo)上方的固定距離

??PS：這里我沒(méi)有直接將機(jī)械臂末端直接移動(dòng)到目標(biāo)點(diǎn)這個(gè)位置，因?yàn)槲覜](méi)有在機(jī)械臂末端加裝機(jī)械手或者相關(guān)示教工具，所以想讓機(jī)械臂末端運(yùn)動(dòng)到一個(gè)相對(duì)的位置。

??思路：通過(guò)上面的手眼標(biāo)定結(jié)果應(yīng)用的公式，計(jì)算得出aruco標(biāo)記圖在機(jī)械臂基底坐標(biāo)系下的位姿T_target2base，假設(shè)我現(xiàn)在想讓機(jī)械臂末端運(yùn)動(dòng)到aruco標(biāo)記圖的正上方500mm處，只需要將T_target2base中的平移向量（X、Y、Z）中的Z軸方向加上500mm得到新的（X、Y、Z’）。
??
??由于識(shí)別到的結(jié)果是aruco標(biāo)志圖相對(duì)于機(jī)械臂基底的位置和姿態(tài)，而我現(xiàn)在輸入到機(jī)械臂中的是將機(jī)械臂末端移動(dòng)到aruco標(biāo)志圖上500mm的指定位置，所以需要的是機(jī)械臂末端相對(duì)于基底的坐標(biāo)，于是直接在示教器輸入上面中的指定點(diǎn)相對(duì)于基底的位置（X、Y、Z’），姿勢(shì)（歐拉角RX、RY、RZ）還使用原來(lái)機(jī)械臂末端到機(jī)械臂基底的姿勢(shì)就行。
??
??輸入（X、Y、Z’、RX、RY、RZ），獲取當(dāng)前機(jī)械臂相對(duì)于基底的6個(gè)關(guān)節(jié)角值（J1、J2、J3、J4、J5、J6，通過(guò)SDK中的 impl->getRobotState()->getJointPositions()接口獲得），再經(jīng)過(guò)運(yùn)動(dòng)學(xué)逆解，得出移動(dòng)到指定點(diǎn)所需要的6個(gè)關(guān)節(jié)角值，最后輸入到機(jī)械臂中，即可將機(jī)械臂末端運(yùn)動(dòng)到目標(biāo)上方的固定距離500mm處。（此時(shí)只是將機(jī)械臂末端移動(dòng)到了目標(biāo)上方的指定高度位置，位姿和目標(biāo)卻不同步。）

3.2 上小節(jié)中是相機(jī)識(shí)別到了目標(biāo)，將機(jī)械臂移動(dòng)到指定目標(biāo)上方的固定高度位置。當(dāng)前場(chǎng)景是不斷更換目標(biāo)位姿，但仍要求機(jī)械臂移動(dòng)到指定目標(biāo)上方的固定距離位置，且位置和姿態(tài)都要和目標(biāo)同步

3.2.1 公式

??參數(shù)含義：

• target：表示相機(jī)識(shí)別到的目標(biāo)，這里我用的是aruco標(biāo)記圖。
• wp：工作點(diǎn)(waypoint)，可以是指定目標(biāo)上方固定距離的一個(gè)點(diǎn)，也可以是機(jī)械臂末端加裝夾爪或者工具的位置點(diǎn)（上小節(jié)中我這里的工作點(diǎn)用的是法蘭中心即機(jī)械臂末端），這個(gè)點(diǎn)根據(jù)實(shí)際需要自行定義。
• T_target2base：目標(biāo)坐標(biāo)系到機(jī)械臂基底坐標(biāo)系的變換矩陣，這是手眼標(biāo)定的應(yīng)用結(jié)果，即三個(gè)4×4矩陣相乘的結(jié)果。
• T_wp2target：工作點(diǎn)坐標(biāo)系到目標(biāo)坐標(biāo)系的變換矩陣，這是你指定要運(yùn)動(dòng)到以目標(biāo)為基準(zhǔn)的固定點(diǎn)。
• T_wp2base：工作點(diǎn)坐標(biāo)系到機(jī)械臂基底坐標(biāo)系的變換矩陣，這是輸入到機(jī)械臂中的數(shù)值。

3.2.2 計(jì)算過(guò)程

3.2.3 執(zhí)行步驟

• ①相機(jī)固定在機(jī)械臂末端，將目標(biāo)擺正置于相機(jī)的視野內(nèi)，設(shè)置好固定高度值H。
• ②按照3.1的方法，先將機(jī)械臂末端移動(dòng)到目標(biāo)上方H高度的位置。
• ③再通過(guò)3.2中的方法，算出T_wp2target，該值是不變的，表示無(wú)論目標(biāo)的位置和姿態(tài)如何改變，始終是移動(dòng)到目標(biāo)上方高度H的位置，這個(gè)點(diǎn)是跟隨目標(biāo)變化而變化的。
• ④將機(jī)械臂恢復(fù)到初始位姿，再次改變目標(biāo)的位姿，然后對(duì)目標(biāo)進(jìn)行識(shí)別，得到T_target2cam_new，根據(jù)1.3中的公式重新計(jì)算得到T_target2base_new = T_end2base * T_cam2end * T_target2cam_new，再根據(jù)3.2.1中的公式得出T_wp2base_new = T_target2base_new * T_wp*target，將這結(jié)果轉(zhuǎn)化為六個(gè)關(guān)節(jié)角值輸入到示教器中，此時(shí)機(jī)械臂末端會(huì)再次移動(dòng)到新擺放的目標(biāo)上方高度H的位置，且姿勢(shì)也和新擺放的目標(biāo)一致。

總結(jié)

以上是生活随笔為你收集整理的（三）手眼标定结果的应用的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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