(三)手眼标定结果的应用
記錄自己手眼標(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)容,希望文章能夠幫你解決所遇到的問題。