1.環(huán)境要求

本教程主要介紹RM機械臂與Realsense D435相機手眼標定的配置及方法墨闲，由于不同處理器架構(gòu)在系統(tǒng)環(huán)境及相關(guān)功能包的安裝配置上存在差異均驶，所以在此不做相關(guān)介紹，如果使用我司提供的設(shè)備竿屹，出廠默認會將系統(tǒng)環(huán)境及相關(guān)功能包安裝配置好或者提供配套的相關(guān)系統(tǒng)環(huán)境搭建的教程音五，在此默認系統(tǒng)環(huán)境滿足以下要求：

系統(tǒng)：Ubuntu 18.04.6

ROS：melodic

OpenCV庫：OpenCV 3.2.0

Realsense D435：librealsense sdk（2.50.0）、realsense-ros功能包（2.3.2）

Marker標記識別：Aruco功能包

手眼標定：easy_handeye功能包

Moveit!

RM機械臂ROS功能包（根據(jù)不同型號機械臂提供配套的ROS包羔沙，使用方法基本相同，提供有配套的使用說明文檔厨钻，這里以RM75-B機械臂配套的ROS包即rm_75_robot為例）

Catkin-tools工具包

2.概述

手眼標定原理參考文章：3D 視覺之手眼標定 (qq.com)

手眼標定分兩種情況：

????Eye-to-hand 眼在手外：標定的是相機坐標系相對于機器人基座坐標系的位姿

????Eye-in-hand 眼在手上：標定的是相機坐標系相對于機器人工具坐標系的位姿

本教程主要介紹Realse D435相機安裝在RM機械臂上即眼在手上的標定方法扼雏。

3.開始前準備

3.1 將相機安裝固定到機械臂末端

將相機通過轉(zhuǎn)接件固定到機械臂末端坚嗜，如圖所示（這里未安裝末端工具，用戶如需要可以一并裝上）：

3.2 接線

????1.機械臂接上電源

????2.機械臂外側(cè)網(wǎng)口通過網(wǎng)線連接到主機或交換機上（注意：機械臂默認固定IP為192.168.1.18诗充，所以要確保主機與機械臂在同一局域網(wǎng)內(nèi)能夠連通）

????3.相機通過Type-C數(shù)據(jù)線將相機與主機USB3.0接口相連

3.3 測試

主機與機械臂都上電啟動后苍蔬，在主機打開終端，執(zhí)行以下命令測試主機與機械臂是否連通：

ping 192.168.1.18

?

在終端中執(zhí)行如下命令打開可視化測試界面(若未檢測到相機嘗試重新連接USB)：

realsense-viewer

相機連接成功后蝴蜓，將左側(cè)【Stereo Module】和【RGB Camera】選擇“on”狀態(tài)碟绑，右側(cè)會 顯示深度圖像和RGB圖像畫面。鼠標移動到深度圖像位置左下方畫面會顯示實時測量的深度 距離茎匠，如圖說明相機連接正常：?

3.4 打印Marker標簽

打印標定需要使用到的Marker標簽格仲，可以在aruco_ros包中找到提供好的marker標簽進行打印，如圖：

也可以從下面的網(wǎng)站下載marker標簽并打印出來：

Online ArUco markers generator (chev.me)

注意：

? ? 1.Dictionary 一定要選 Original ArUco

? ? 2.Marker ID和Marker size自選诵冒，在launch文件中做相應(yīng)的修改（本教程演示使用Marker ID：582凯肋，Marker size：50mm）

? ? 3.打印時，要選擇原始大小汽馋，否則要測量一下打印出來的真實大小

4.aruco_ros配置

4.1 簡介

aruco是一種類似二維碼的定位標記輔助工具侮东，通過在環(huán)境中部署Markers，可以輔助機器人進行定位豹芯，彌補單一傳感器的缺陷悄雅，糾正誤差，本教程使用的手眼標定easy_handeye功能包需要借助這個工具進行手眼標定铁蹈。

4.2 配置aruco_ros的launch文件

在aruco_ros功能包的launch目錄下拷貝一份原有的single.launch文件或新建一個launch文件宽闲，命名為single_realsense.launch，執(zhí)行以下命令（本教程默認用戶ROS的工作空間為cakin_ws木缝，用戶根據(jù)實際的環(huán)境進入相應(yīng)路徑進行操作）：

cd ~/catkin_ws/src/aruco_ros/aruco_ros/launch

touch single_realsense.launch

gedit single_realsense.launch

根據(jù)以下內(nèi)容進行修改或者覆蓋為以下內(nèi)容后保存：

<launch>

<arg name="markerId" ???????default="582"/>

<arg name="markerSize" ?????default="0.05"/> ???<!-- in m -->

<arg name="eye" ????????????default="left"/>

<arg name="marker_frame" ???default="aruco_marker_frame"/>

<arg name="ref_frame" ??????default="camera_color_frame"/> ?<!-- leave empty and the pose will be published wrt param parent_name -->

<arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->

<node pkg="aruco_ros" type="single" name="aruco_single">

<remap from="/camera_info" to="/camera/color/camera_info" />

<remap from="/image" to="/camera/color/image_raw" />

<param name="image_is_rectified" value="True"/>

<param name="marker_size" ???????value="$(arg markerSize)"/>

<param name="marker_id" ?????????value="$(arg markerId)"/>

<param name="reference_frame" ???value="$(arg ref_frame)"/> ??<!-- frame in which the marker pose will be refered -->

<param name="camera_frame" ??????value="camera_color_frame"/>

<param name="marker_frame" ??????value="$(arg marker_frame)" />

<param name="corner_refinement" ?value="$(arg corner_refinement)" />

</node>

</launch>

markerId：使用的Marker標簽的ID便锨，這里在準備時打印的Marker標簽ID為582

markerSize：Marker標簽的實際大小，單位為m我碟，這里在準備時打印的Marker標簽實際大小為5厘米即0.05m

ref_frame：參考坐標系名稱放案，這里選擇Realsense的camera_color_frame作為參考坐標系

<remap from="/camera_info" to="/camera/color/camera_info" />：將/camera_info重映射為對應(yīng)Realsense實際發(fā)布的相應(yīng)的Topic即/camera/color/camera_info

<remap from="/image" to="/camera/color/image_raw" />：將/image重映射為對應(yīng)Realsense實際發(fā)布的相應(yīng)的Topic即/camera/color/image_raw

camera_frame：相機坐標系，修改為實際的相機坐標系camera_color_frame

5.easy_handeye配置

5.1 簡介

easy_handeye是用于手眼標定的功能包之一矫俺，借助它可以實現(xiàn)眼在手上和眼在手外的手眼標定吱殉，在這里我們介紹使用easy_handeye完成眼在手上的手眼標定。

5.2 配置easy_handeye標定的launch文件

在easy_handeye功能包的launch目錄下新建一個launch文件厘托，命名為eye_in_hand_calibrate.launch友雳，執(zhí)行以下命令（本教程默認用戶ROS的工作空間為cakin_ws，用戶根據(jù)實際的環(huán)境進入相應(yīng)路徑進行操作）：

cd ~/catkin_ws/src/easy_handeye/easy_handeye/launch

touch eye_in_hand_calibrate.launch

gedit eye_in_hand_calibrate.launch

編輯輸入以下內(nèi)容并保存：

<?xml version="1.0" ?>

<launch>

<!-- 生成標定文件的名稱 -->

<arg name="namespace_prefix" default="rm_rs_d435" />

<!-- RM機械臂MoveIt!配置的move_group為arm铅匹，所以修改為arm -->

<arg name="move_group" default="arm" />

<!-- start easy_handeye -->

<include file="$(find easy_handeye)/launch/calibrate.launch" >

<arg name="namespace_prefix" value="$(arg namespace_prefix)" />

<arg name="move_group" value="$(arg move_group)" />

<!-- 這里使用眼在手上的方式進行標定押赊，所以此處改成true -->

<arg name="eye_on_hand" value="true" />

<!--tracking_base_frame為realsense的相機坐標系-->

<arg name="tracking_base_frame" value="camera_color_frame" />

<!--tracking_marker_frame對應(yīng)aruco_ros包中single_realsense.launch中的marker_frame的值-->

<arg name="tracking_marker_frame" value="aruco_marker_frame" />

<!--robot_base_frame為機器人基座坐標系-->

<arg name="robot_base_frame" value="base_link" />

<!--robot_effector_frame為工具坐標系，如夾爪包斑，吸盤等流礁，但實際的rm機器人模型未添加夾爪涕俗，所以這里設(shè)置為末端關(guān)節(jié)Link7-->

<arg name="robot_effector_frame" value="Link7" />

<arg name="freehand_robot_movement" value="false" />

<arg name="robot_velocity_scaling" value="0.5" />

<arg name="robot_acceleration_scaling" value="0.2" />

</include>

</launch>

注意：<arg name="robot_effector_frame" value="Link7" />這里是以RM75即7軸機械臂為例所以為Link7，如果使用的是RM65即6軸機械臂神帅，則應(yīng)該修改為Link6

5.3 配置發(fā)布TF的launch文件

手眼標定完成后需要根據(jù)標定的文件發(fā)布TF坐標轉(zhuǎn)換才能夠?qū)⑾鄼C識別到物體的坐標轉(zhuǎn)換到相應(yīng)機械臂的坐標系上再姑，修改easy_handeye功能包的launch目錄下的publish.launch文件，執(zhí)行以下命令（本教程默認用戶ROS的工作空間為cakin_ws找御，用戶根據(jù)實際的環(huán)境進入相應(yīng)路徑進行操作）：

cd ~/catkin_ws/src/easy_handeye/easy_handeye/launch

gedit publish.launch

編輯修改對應(yīng)的內(nèi)容并保存：

<?xml version="1.0"?>

<launch>

<!--修改eye_on_hand參數(shù)默認為true -->

<arg name="eye_on_hand" doc="eye-on-hand instead of eye-on-base" default="true" />

<!--修改namespace_prefix參數(shù)元镀，與眼在手上標定launch文件[eye_in_hand_calibrate.launch]中的“namespace_prefix”一致，這樣才能找到標定好的YAML文件-->

<arg name="namespace_prefix" default="rm_rs_d435" />

<arg if="$(arg eye_on_hand)" name="namespace" value="$(arg namespace_prefix)_eye_on_hand" />

<arg unless="$(arg eye_on_hand)" name="namespace" value="$(arg namespace_prefix)_eye_on_base" />

<!--it is possible to override the link names saved in the yaml file in case of name clashes, for example-->

<arg if="$(arg eye_on_hand)" name="robot_effector_frame" default="" />

<arg unless="$(arg eye_on_hand)" name="robot_base_frame" default="" />

<arg name="tracking_base_frame" default="" />

<arg name="inverse" default="false" />

<arg name="calibration_file" default="" />

<!--publish hand-eye calibration-->

<group ns="$(arg namespace)">

<param name="eye_on_hand" value="$(arg eye_on_hand)" />

<param unless="$(arg eye_on_hand)" name="robot_base_frame" value="$(arg robot_base_frame)" />

<param if="$(arg eye_on_hand)" name="robot_effector_frame" value="$(arg robot_effector_frame)" />

<param name="tracking_base_frame" value="$(arg tracking_base_frame)" />

<param name="inverse" value="$(arg inverse)" />

<param name="calibration_file" value="$(arg calibration_file)" />

<node name="$(anon handeye_publisher)" pkg="easy_handeye" type="publish.py" output="screen"/>

</group>

</launch>

6.啟動相關(guān)launch文件開始標定

6.1 啟動Realsense節(jié)點

首先確認相關(guān)ROS包都已編譯霎桅，執(zhí)行如下命令確保能夠編譯成功：

cd ~/catkin_ws

catkin build

確認相機通過Type-C數(shù)據(jù)線將相機與主機USB3.0接口相連栖疑，執(zhí)行如下命令啟動Realsense節(jié)點：

cd ~/catkin_ws

source devel/setup.bash

roslaunch realsense2_camera rs_camera.launch

6.2 啟動aruco節(jié)點識別Marker標簽

打開一個新的終端，執(zhí)行以下命令啟動single_realsense.launch：

cd ~/catkin_ws

source devel/setup.bash

roslaunch aruco_ros single_realsense.launch

6.3 啟動image_view節(jié)點顯示圖像

打開一個新的終端哆档，執(zhí)行以下命令啟動image_view訂閱/aruco_single/result顯示圖像：

cd ~/catkin_ws

source devel/setup.bash

rosrun image_view image_view image:=/aruco_single/result

6.4 啟動RM機械臂的control和driver節(jié)點

打開一個新的終端蔽挠，執(zhí)行以下命令啟動control節(jié)點：

cd ~/catkin_ws

source devel/setup.bash

roslaunch rm_75_control rm_75_control.launch

注意：本教程以RM75機械臂為例，如果使用的是6軸即RM65機械臂瓜浸，則啟動control的命令為：roslaunch rm_control rm_control.launch

再打開一個新的終端澳淑，執(zhí)行以下命令啟動driver節(jié)點：

cd ~/catkin_ws

source devel/setup.bash

roslaunch rm_75_bringup rm_robot.launch

注意：本教程以RM75機械臂為例，如果使用的是6軸即RM65機械臂插佛，則啟動driver的命令為：roslaunch rm_bringup rm_robot.launch

運行成功后在rviz中可以看到機器人模型與真實機械臂的狀態(tài)保持一致杠巡，如圖所示：

在rviz的MotionPlanning下的Planning，Goal State選擇forward然后點擊Plan & Execute雇寇，可以看到真實機械臂會按照rviz中MoveIt!規(guī)劃的路徑運動氢拥，如圖所示：

6.5 啟動easy_handeye手眼標定節(jié)點

在啟動標定程序前，建議先在rviz中通過Moveit或手動調(diào)節(jié)機械臂到一個合適的姿態(tài)锨侯，然后將aruco二維碼移動至相機視野中心處附近嫩海，如圖所示（建議參考圖中的姿態(tài)，避免在標定時有不可達的規(guī)劃）：

打開一個新的終端囚痴，執(zhí)行以下命令啟動手眼標定節(jié)點：

cd ~/catkin_ws

source devel/setup.bash

roslaunch easy_handeye eye_in_hand_calibrate.launch

成功運行后叁怪，會同時打開三個界面：

界面1

界面2

界面3

標定步驟：

? ? （1）在界面3中，點擊Check starting pose深滚，稍微等待奕谭，若檢查成功，界面會出現(xiàn)：0/17痴荐，Ready to start血柳，如圖所示（如果檢測失敗或者沒有出現(xiàn)0/17，則說明當前機械臂的姿態(tài)不合適生兆，可能有規(guī)劃不可達难捌，需要關(guān)閉標定節(jié)點然后調(diào)節(jié)機械臂到一個合適姿態(tài)并將aruco二維碼移動至相機視野中心處附近再重啟啟動標定節(jié)點繼續(xù)標定）：

（2）界面3中依次點擊 Next Pose，Plan，Execute根吁，機械臂會移動至新的位置昆汹，若二維碼完全的在相機視野范圍內(nèi)，且能檢測成功婴栽，則進行下一步，若二維碼不在相機視野范圍內(nèi)或部分在視野外辈末，檢測不到愚争，則再次依次點擊Next Pose，Plan挤聘，Execute轰枝，直到二維碼完全在相機視野范圍內(nèi)，且能檢測成功组去，再進行下一步（一般前4次會依據(jù)aruco二維碼位置機械臂移動到上下左右位置鞍陨，二維碼都不完全在相機視野范圍內(nèi)，從第五次開始檢測成功）从隆，如圖所示：

（3）界面2中點擊Take Sample谤绳，若Samples對話框中出現(xiàn)有效信息檐嚣，說明第一個點標定成功

（4）重復(fù)執(zhí)行步驟2和步驟 3，直至 17 個點全部標定完畢

（5）界面2中點擊Compute，則Result對話框中會出現(xiàn)結(jié)果

（6）界面2中Save胎撤，會將結(jié)果保存為一個YAML文件，路徑為?~/.ros/easy_handeye

????標定完成后在每個終端按Ctrl+C即可結(jié)束運行節(jié)點扇住。

6.6 簡單測試

按照6.1至6.4啟動Realsense迈窟、aruco、image_view挎塌、rm_control徘六、rm_driver相關(guān)節(jié)點，不需要再啟動easy_handeye手眼標定節(jié)點榴都，然后再打開一個新的終端待锈，執(zhí)行以下命令發(fā)布手眼標定后的TF：

roslaunch easy_handeye publish.launch

執(zhí)行以下命令可以查看相機與機械臂基座標系的TF轉(zhuǎn)換：

rosrun tf tf_echo /base_link /camera_color_frame

執(zhí)行以下命令如果檢測到marker標簽，會顯示marker在相機坐標系下的位姿：

rostopic echo /aruco_single/pose

標定過程可能會受相關(guān)硬件及環(huán)境等條件因素的影響缭贡，所以標定結(jié)果可能存在一定的誤差炉擅，用戶標定完成后可以通過獲取物體在相機坐標系下的位姿，然后通過TF轉(zhuǎn)換到機械臂下的位姿進行測試阳惹，存在誤差可以后續(xù)在程序進行補償計算谍失。