物體位姿估計精度驗證實驗（涉及位姿估計，相手眼標定，機械臂運動）
物體位姿估計精度驗證實驗（涉及位姿估計，相手眼標定，機械臂運動）
1.位姿估計
簡單介紹，採用雙目結構光相機，利用拍攝的點雲資料和CAD模型點雲進行ICP配準，獲取物體在相機座標系下的位姿（R，t）
2.手眼標定
本文采用的是Eye to Hand 方式，與opencv中講解的Eye in Hand方式不同；因此花了點時間，首先還是先來看Opencv官方的手眼標定函式
Opencv 手眼標定函式calibrateHandEye()
（1）Eye in Hand
在opencv 3.4版本以上，提供了手眼標定函式，本文采用的是4.4版本，貼一個官方文件
基本輸入輸出描述：
看著這個旋轉矩陣，有點迷，來看一下官方的圖
可以看出，官方求解的是相機到夾爪的變換矩陣：gTc；
其兩個輸入：
- 夾爪到機器人基座變換矩陣bTg
- 目標物體（一般是棋盤格）到相機的變換矩陣cTt
這就感覺很反人類，機器人每次運動讀出來的坑定是基座到機械臂末端的變換矩陣，按照opencv的表示方法應該是gTb；然後pnp演算法可以獲取得到相機每次拍照相對於棋盤格的外參cTt（這個引數和opencv的是相符的）；
- 所以Eye in Hand相機標定使用opencv的標定函式，應該是要把機器人的位姿估計去逆？？？？
- 那為啥廣大csdn朋友都是直接用的？？？納悶 （看到最後）
（1）Eye to Hand
這裡我們還是按照opencv的定義吧；
根據target到base從紅色路徑走和藍色路徑走，一次拍攝可以獲得一個等式
bTg · gTt = bTc · cTt
兩次拍攝，有
bTg1 · gTt = bTc · cTt1 （1）
bTg2 · gTt = bTc · cTt2 （2）
用（1）消去（2）中的 gTt，有
bTg2 · inv(bTg1 ) · bTc · cTt1 = bTc · cTt2
挪一挪，成AX=XB的形式：
bTg2 · inv(bTg1 ) · bTc = bTc · cTt2 · inv(cTt1)
對比一下eye in hand：好像bTg逆了一下？？？
那根據opencv變換矩陣的定義，是不是對於eye to hand直接可以用機器人讀出的基座到末端的矩陣？？？
我懷著好奇試了一下，發現。。。。嚶嚶嚶，錯了；
幾種情況都試了幾下，發現需要求逆的是eye to hand中的bTg，也就是機器人末端位姿取個逆,或許這裡能解釋廣大的csdn友eye in hand中機器人末端矩陣不取逆的原因？？？
所以我感覺opencv的矩陣定義很迷。。。。。。。。不過反正是可以用了
其他標定函式
我自己用的是網上網友把opencv中的程式碼自己實現了一下
另外還有一個github的matlab版本，這個沒試過
總之原理就這樣
3.機械臂運動
我們設想的是去驗證整個位姿估計框架的精度：
包含了物體到相機，相機到機器人基座兩部分變換關係
實驗方案：
大概就這樣，實際中B是一個框，A是中間那部分，初始狀態就是A放在B中
機器人位姿校正推導：
這裡的座標系我喜歡用自己的定義，不用opencv那裡 的反人類定義
那麼就有：
然後實驗就是完事了