（三）手眼标定结果的应用
記錄自己手眼標定（眼在手上）結果的應用過程，本人使用的是2D相機。
內容
- 1.核心公式
- 1.1 命名規定
- 1.2 歐拉角、四元數、旋轉矩陣的關系
- 1.3 公式
- 2.手眼標定的運算法則和對應的物理意義
- 3.場景需求
- 3.1 目標(aruco標記圖)在水平面上不動，指定機械臂末端運動到目標上方的固定距離
- 3.2 上小節中是相機識別到了目標，將機械臂移動到指定目標上方的固定高度位置。當前場景是不斷更換目標位姿，但仍要求機械臂移動到指定目標上方的固定距離位置，且位置和姿態都要和目標同步
- 3.2.1 公式
- 3.2.2 計算過程
- 3.2.3 執行步驟
1.核心公式
1.1 命名規定
??表示以base(機器臂基底)為基準，end(機械臂末端)在其下的位姿描述，即機械臂末端坐標系到機械臂基底坐標系的變換矩陣。
（本文所有公式中的變量都以這個為標準，有的資料中寫的和這個相反，容易記混了，注意哪個是基準就行。）
1.2 歐拉角、四元數、旋轉矩陣的關系
??關于這幾個的關系轉換如下圖所示，在之后的計算中，需要頻繁進行轉換。
1.3 公式
??上面是三個4×4的矩陣相乘，得到的結果就是在機械臂基底坐標系base下，識別到的目標target的位姿。計算結果仍然是個4×4矩陣，包含一個旋轉矩陣+一個平移向量，將旋轉矩陣轉為歐拉角，再通過運動學逆解計算將歐拉角轉為6個關節角度值，將這6個值輸入到示教器中，即可將機械臂末端運動到當前目標在基底下的位姿狀態。
??（PS：如果直接輸入X、Y、Z、RX、RY、RZ可能會得到好幾種不同的關節角度值，雖然它們的位置和歐拉角都相同，萬一解出一個值機器人會撞到工作平面，所以需要我們通過運動學逆解直接輸入關節角值，來避免這種情況。）
上式中各個參數的含義和計算方法：
- 1.T：變換矩陣，由旋轉矩陣（3×3）和平移向量（3×1）組成，在旋轉矩陣下面加一行[0, 0, 0, 1]構成齊次矩陣；格式如下：
- 2.
??這個值由示教器面板直接讀取或通過示教器的SDK獲取，但獲得的位姿形式是（X、Y、Z、RX、RY、RZ），需要將其轉換為旋轉矩陣R和平移向量t，然后再拼接為齊次矩陣（4×4），將歐拉角（RX、RY、RZ）轉為旋轉矩陣，這里要注意歐拉角的排列順序（機械臂的位姿類型有zyx,yzx,zxy,yxz,xyz,xzy幾種，需要區分），之后再和平移向量（XYZ）進行拼接。
- 3.
??這是手眼標定的結果。
關于手眼標定，參考：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.
??根據自己選擇的目標放在相機視野內進行拍照識別，我選擇的是對aruco標記圖進行識別，會得到目標（標記圖）在相機坐標系下的位姿（X、Y、Z、w、x、y、z），需要將后面的四元數轉為旋轉矩陣，再和前面的（X、Y、Z）組成齊次矩陣。
四元數->旋轉矩陣，參考：https://zhuanlan.zhihu.com/p/45404840
2.手眼標定的運算法則和對應的物理意義
參考：https://zhuanlan.zhihu.com/p/388952714
3.場景需求
??前提都是水平面上的目標移動，非水平面的還沒做。。。若有大佬做過的迫切希望得到指導。
3.1 目標(aruco標記圖)在水平面上不動，指定機械臂末端運動到目標上方的固定距離
??PS：這里我沒有直接將機械臂末端直接移動到目標點這個位置，因為我沒有在機械臂末端加裝機械手或者相關示教工具，所以想讓機械臂末端運動到一個相對的位置。
??思路：通過上面的手眼標定結果應用的公式，計算得出aruco標記圖在機械臂基底坐標系下的位姿T_target2base，假設我現在想讓機械臂末端運動到aruco標記圖的正上方500mm處，只需要將T_target2base中的平移向量（X、Y、Z）中的Z軸方向加上500mm得到新的（X、Y、Z’）。
??
??由于識別到的結果是aruco標志圖相對于機械臂基底的位置和姿態，而我現在輸入到機械臂中的是將機械臂末端移動到aruco標志圖上500mm的指定位置，所以需要的是機械臂末端相對于基底的坐標，于是直接在示教器輸入上面中的指定點相對于基底的位置（X、Y、Z’），姿勢（歐拉角RX、RY、RZ）還使用原來機械臂末端到機械臂基底的姿勢就行。
??
??輸入（X、Y、Z’、RX、RY、RZ），獲取當前機械臂相對于基底的6個關節角值（J1、J2、J3、J4、J5、J6，通過SDK中的 impl->getRobotState()->getJointPositions()接口獲得），再經過運動學逆解，得出移動到指定點所需要的6個關節角值，最后輸入到機械臂中，即可將機械臂末端運動到目標上方的固定距離500mm處。（此時只是將機械臂末端移動到了目標上方的指定高度位置，位姿和目標卻不同步。）
3.2 上小節中是相機識別到了目標，將機械臂移動到指定目標上方的固定高度位置。當前場景是不斷更換目標位姿，但仍要求機械臂移動到指定目標上方的固定距離位置，且位置和姿態都要和目標同步
3.2.1 公式
??參數含義：
- target：表示相機識別到的目標，這里我用的是aruco標記圖。
- wp：工作點(waypoint)，可以是指定目標上方固定距離的一個點，也可以是機械臂末端加裝夾爪或者工具的位置點（上小節中我這里的工作點用的是法蘭中心即機械臂末端），這個點根據實際需要自行定義。
- T_target2base：目標坐標系到機械臂基底坐標系的變換矩陣，這是手眼標定的應用結果，即三個4×4矩陣相乘的結果。
- T_wp2target：工作點坐標系到目標坐標系的變換矩陣，這是你指定要運動到以目標為基準的固定點。
- T_wp2base：工作點坐標系到機械臂基底坐標系的變換矩陣，這是輸入到機械臂中的數值。
3.2.2 計算過程
3.2.3 執行步驟
- ①相機固定在機械臂末端，將目標擺正置于相機的視野內，設置好固定高度值H。
- ②按照3.1的方法，先將機械臂末端移動到目標上方H高度的位置。
- ③再通過3.2中的方法，算出T_wp2target，該值是不變的，表示無論目標的位置和姿態如何改變，始終是移動到目標上方高度H的位置，這個點是跟隨目標變化而變化的。
- ④將機械臂恢復到初始位姿，再次改變目標的位姿，然后對目標進行識別，得到T_target2cam_new，根據1.3中的公式重新計算得到T_target2base_new = T_end2base * T_cam2end * T_target2cam_new，再根據3.2.1中的公式得出T_wp2base_new = T_target2base_new * T_wp*target，將這結果轉化為六個關節角值輸入到示教器中，此時機械臂末端會再次移動到新擺放的目標上方高度H的位置，且姿勢也和新擺放的目標一致。
總結
以上是生活随笔為你收集整理的（三）手眼标定结果的应用的全部內容，希望文章能夠幫你解決所遇到的問題。
