鑽石舞台

在機器人視覺系統，利用AprilTag得到的標籤信息，進一步得到物體識別區域的位置，發送給底盤即可驅動底盤移動到識別區域。

也需要用AprilTag進行手眼標定，把子坐標系camera_rgb_optical_frame到基坐標系base_link的相對位置關係標定，完成機器人的手眼標定過程。

這幾章內容主要集中在機器人的視覺控制、三維目標抓取這一部分，也算順着之前的CT-LineMod算法繼續談一下計算機視覺這一部分的內容。第一節這個總體框架設計在接下來的視覺、抓取、導航部分都會放在前面，以幫助讀者理解一下各個部分的功能以及把握整體性。

由於機器人主機NUC並不知道自身機械臂、深度相機的相對位置關係，因此需要藉助手眼標定系統完成機器人手（機械臂和機械手）眼（深度相機RealsenseZR300）坐標系的標定。

其中，基坐標系base_tree即主機IntelNUC所在位置為中心的三維坐標系，在基坐標系下則是主機以及機械手臂控制系統所在的坐標系。而相機坐標系camera_tree即以深度相機ZR300所在位置為中心的三維坐標系。

在ROS操作環境中兩個坐標系隸屬於不同的坐標樹。

因此，可以通過標籤定位的方法，在機械手上貼有獨特信息標籤，利用深度相機識別出標籤所在的位置（position）和方向（orientation）信息。

通過ROS環境自帶的hand_eye標定包得到機械手marker_frame坐標系相對於base_tree的位置關係，進而得到子坐標系camera_rgb_optical_frame到基坐標系base_link的相對位置關係，完成機器人的手眼標定過程。

之後，在真實場景下識別到的三維物體通過主機控制深度相機運行目標識別算法得到在camera_tree坐標系下的物體位置方向信息，通過TF坐標轉換模塊將識別結果轉換到base_tree坐標系，進而用於機械臂的規劃及抓取。

手眼標定模塊分為深度相機在機械手上以及深度相機在底座上兩種情況，本文屬於後者。

在標定過程中共採集17組標籤在不同位置和方向時的深度相機信息，通過比較base_tree坐標系下機械手移動到對應位置時所處的位置信息，得到base_tree和camera_tree的17組相對位置關係。

通過計算平均值，得到兩坐標系的平移和旋轉關係。如下圖所示為一組坐標位置關係及最終的base_tree和camera_tree的坐標系轉換關係

在得到base_tree與camera_tree坐標系的位置轉換關係後，需要利用兩坐標系平移旋轉關係，創建tf_broadcaster來在ROS環境廣播其位置關係，並在tf::transform函數中存儲兩者坐標關係。

之後利用TF坐標轉換把深度相機識別到的物體位置和方向信息轉換到機械手坐標系下進行規劃抓取。

首先，在主機IntelNUC中需要將深度相機識別到的物體位置和方向信息存儲所在位置RecognizedObjectArray數組中提取出來，利用rospy.Subscriber函數判斷是否有該類信息即可進一步判斷是否識別到物體。

接着，定義一用於存放識別到的物體位置信息的臨時數組data.object[]，定義一節點tf_listener用於訂閱深度相機data.object[]的物體位置消息real_pose，並利用tf::transform中的坐標轉換關係將物體位置信息從camera_tree坐標系下轉換為base_tree坐標系下。

如下圖所示為實時坐標轉換關係圖。

之後，在tf_listener節點設置發布程序，將轉換後的基坐標系下的物體坐標transed_pose發布出去。

最後，機械臂UR10中的節點將訂閱tf_listener節點發布的消息，利用Moveit!完成機械手臂的運動規劃與抓取。

整個過程中物體位置的消息均為geometry_msgs::PoseStamped消息類型的。如圖所示為控制系統話題信息圖（不包含機器人底盤）及機器人控制系統全部坐標系連接關係圖。

其中，控制系統在執行抓取時的程序主要包含移動底盤驅動到識別區域、物體位置判斷、位姿獲取、位置偏移計算、機械臂運動規劃到達偏移位置處、從偏移位置處移動到真實位置處、機械手手爪閉合到罐體直徑寬度、機械手抓取、物體移動放置與桌子平面上、機械手臂收放以及移動底盤返回到停放區域。

Transform話題所發布的物體位置轉換消息如圖所示。其中包含物體位置信息，空間位置信息x、y、z以及方向信息x、y、z、w七個位置信息以及坐標對應所在的坐標系。

另外通過增加軟件限定的方式，限制機械手臂的移動範圍，使機械臂只可以針對深度相機前的範圍內進行安全的抓取操作，如超過限制範圍系統會重新選取最佳抓取路線，並且在仿真中機械手臂模型會變成紅色以警示仿真操作合適，這樣也方便了之後增加移動機器人的其他任務的可能（如按電梯等）。

機器人控制系統深度相機與機械手臂部分運行程序後的所有ROS話題如下圖所示。其中，camera/driver話題用於深度相機的初始化驅動話題，ur_driver為機械手臂的驅動話題。

將Camera_info消息輸入object_recognition_server話題後，識別到的物體6D位姿估計通過recognized_object_array消息傳遞到tf_listener訂閱話題接收位置消息，並通過tf_broadcaster廣播到ROS的各個部分模塊，實現整個機器人的通信。