（三）手眼标定结果的应用
記錄自己手眼標(biāo)定（眼在手上）結(jié)果的應(yīng)用過程，本人使用的是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.場景需求
- 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)前場景是不斷更換目標(biāo)位姿，但仍要求機(jī)械臂移動(dòng)到指定目標(biāo)上方的固定距離位置，且位置和姿態(tài)都要和目標(biāo)同步
- 3.2.1 公式
- 3.2.2 計(jì)算過程
- 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)，有的資料中寫的和這個(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)為歐拉角，再通過運(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é)角度值，雖然它們的位置和歐拉角都相同，萬一解出一個(gè)值機(jī)器人會(huì)撞到工作平面，所以需要我們通過運(yùn)動(dòng)學(xué)逆解直接輸入關(guān)節(jié)角值，來避免這種情況。）
上式中各個(gè)參數(shù)的含義和計(jì)算方法：
- 1.T：變換矩陣，由旋轉(zhuǎn)矩陣（3×3）和平移向量（3×1）組成，在旋轉(zhuǎn)矩陣下面加一行[0, 0, 0, 1]構(gòu)成齊次矩陣；格式如下：
- 2.
??這個(gè)值由示教器面板直接讀取或通過示教器的SDK獲取，但獲得的位姿形式是（X、Y、Z、RX、RY、RZ），需要將其轉(zhuǎn)換為旋轉(zhuǎn)矩陣R和平移向量t，然后再拼接為齊次矩陣（4×4），將歐拉角（RX、RY、RZ）轉(zhuǎn)為旋轉(zhuǎn)矩陣，這里要注意歐拉角的排列順序（機(jī)械臂的位姿類型有zyx,yzx,zxy,yxz,xyz,xzy幾種，需要區(qū)分），之后再和平移向量（XYZ）進(jìn)行拼接。
- 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
2.手眼標(biāo)定的運(yùn)算法則和對(duì)應(yīng)的物理意義
參考：https://zhuanlan.zhihu.com/p/388952714
3.場景需求
??前提都是水平面上的目標(biāo)移動(dòng)，非水平面的還沒做。。。若有大佬做過的迫切希望得到指導(dǎo)。
3.1 目標(biāo)(aruco標(biāo)記圖)在水平面上不動(dòng)，指定機(jī)械臂末端運(yùn)動(dòng)到目標(biāo)上方的固定距離
??PS：這里我沒有直接將機(jī)械臂末端直接移動(dòng)到目標(biāo)點(diǎn)這個(gè)位置，因?yàn)槲覜]有在機(jī)械臂末端加裝機(jī)械手或者相關(guān)示教工具，所以想讓機(jī)械臂末端運(yùn)動(dòng)到一個(gè)相對(duì)的位置。
??思路：通過上面的手眼標(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’），姿勢（歐拉角RX、RY、RZ）還使用原來機(jī)械臂末端到機(jī)械臂基底的姿勢就行。
??
??輸入（X、Y、Z’、RX、RY、RZ），獲取當(dāng)前機(jī)械臂相對(duì)于基底的6個(gè)關(guān)節(jié)角值（J1、J2、J3、J4、J5、J6，通過SDK中的 impl->getRobotState()->getJointPositions()接口獲得），再經(jīng)過運(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)前場景是不斷更換目標(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ì)算過程
3.2.3 執(zhí)行步驟
- ①相機(jī)固定在機(jī)械臂末端，將目標(biāo)擺正置于相機(jī)的視野內(nèi)，設(shè)置好固定高度值H。
- ②按照3.1的方法，先將機(jī)械臂末端移動(dòng)到目標(biāo)上方H高度的位置。
- ③再通過3.2中的方法，算出T_wp2target，該值是不變的，表示無論目標(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的位置，且姿勢也和新擺放的目標(biāo)一致。
總結(jié)
以上是生活随笔為你收集整理的（三）手眼标定结果的应用的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
