睿尔曼超轻量仿人机械臂--Realsense D435手眼标定

1.环境要求

2.概述

3.开始前准备

4.aruco_ros配置

5.easy_handeye配置

6.启动相关launch文件开始标定

1.环境要求

本教程主要介绍RM机械臂与Realsense D435相机手眼标定的配置及方法，由于不同处理器架构在系统环境及相关功能包的安装配置上存在差异，所以在此不做相关介绍，如果使用我司提供的设备，出厂默认会将系统环境及相关功能包安装配置好或者提供配套的相关系统环境搭建的教程，在此默认系统环境满足以下要求：

系统：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功能包（根据不同型号机械臂提供配套的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 将相机安装固定到机械臂末端

将相机通过转接件固定到机械臂末端，如图所示（这里未安装末端工具，用户如需要可以一并装上）：

3.2 接线

机械臂接上电源
机械臂外侧网口通过网线连接到主机或交换机上（注意：机械臂默认固定IP为192.168.1.18，所以要确保主机与机械臂在同一局域网内能够连通）
相机通过Type-C数据线将相机与主机USB3.0接口相连

3.3 测试

主机与机械臂都上电启动后，在主机打开终端，执行以下命令测试主机与机械臂是否连通：

ping 192.168.1.18

在终端中执行如下命令打开可视化测试界面(若未检测到相机尝试重新连接USB)：

realsense-viewer

相机连接成功后，将左侧【Stereo Module】和【RGB Camera】选择“on”状态，右侧会显示深度图像和RGB图像画面。鼠标移动到深度图像位置左下方画面会显示实时测量的深度距离，如图说明相机连接正常：

3.4 打印Marker标签

打印标定需要使用到的Marker标签，可以在aruco_ros包中找到提供好的marker标签进行打印，如图：

也可以从下面的网站下载marker标签并打印出来：

Online ArUco markers generator (chev.me)

注意：

Dictionary 一定要选 Original ArUco
Marker ID和Marker size自选，在launch文件中做相应的修改（本教程演示使用Marker ID：582，Marker size：50mm）
打印时，要选择原始大小，否则要测量一下打印出来的真实大小

4.aruco_ros配置

4.1 简介

aruco是一种类似二维码的定位标记辅助工具，通过在环境中部署Markers，可以辅助机器人进行定位，弥补单一传感器的缺陷，纠正误差，本教程使用的手眼标定easy_handeye功能包需要借助这个工具进行手眼标定。

4.2 配置aruco_ros的launch文件

在aruco_ros功能包的launch目录下拷贝一份原有的single.launch文件或新建一个launch文件，命名为single_realsense.launch，执行以下命令（本教程默认用户ROS的工作空间为cakin_ws，用户根据实际的环境进入相应路径进行操作）：

cd ~/catkin_ws/src/aruco_ros/aruco_ros/launchtouch single_realsense.launchgedit single_realsense.launch

根据以下内容进行修改或者覆盖为以下内容后保存：

<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重映射为对应Realsense实际发布的相应的Topic即/camera/color/camera_info
<remap from="/image" to="/camera/color/image_raw" />：将/image重映射为对应Realsense实际发布的相应的Topic即/camera/color/image_raw
camera_frame：相机坐标系，修改为实际的相机坐标系camera_color_frame

5.easy_handeye配置

5.1 简介

easy_handeye是用于手眼标定的功能包之一，借助它可以实现眼在手上和眼在手外的手眼标定，在这里我们介绍使用easy_handeye完成眼在手上的手眼标定。

5.2 配置easy_handeye标定的launch文件

在easy_handeye功能包的launch目录下新建一个launch文件，命名为eye_in_hand_calibrate.launch，执行以下命令（本教程默认用户ROS的工作空间为cakin_ws，用户根据实际的环境进入相应路径进行操作）：

cd ~/catkin_ws/src/easy_handeye/easy_handeye/launchtouch eye_in_hand_calibrate.launchgedit eye_in_hand_calibrate.launch

编辑输入以下内容并保存：

<?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对应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机器人模型未添加夹爪，所以这里设置为末端关节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轴机械臂，则应该修改为Link6

5.3 配置发布TF的launch文件

手眼标定完成后需要根据标定的文件发布TF坐标转换才能够将相机识别到物体的坐标转换到相应机械臂的坐标系上，修改easy_handeye功能包的launch目录下的publish.launch文件，执行以下命令（本教程默认用户ROS的工作空间为cakin_ws，用户根据实际的环境进入相应路径进行操作）：

cd ~/catkin_ws/src/easy_handeye/easy_handeye/launchgedit publish.launch

编辑修改对应的内容并保存：

<?xml version="1.0"?><launch><!--修改eye_on_hand参数默认为true --><arg name="eye_on_hand" doc="eye-on-hand instead of eye-on-base" default="true" /><!--修改namespace_prefix参数，与眼在手上标定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.启动相关launch文件开始标定

6.1 启动Realsense节点

首先确认相关ROS包都已编译，执行如下命令确保能够编译成功：

cd ~/catkin_wscatkin build

确认相机通过Type-C数据线将相机与主机USB3.0接口相连，执行如下命令启动Realsense节点：

cd ~/catkin_wssource devel/setup.bashroslaunch realsense2_camera rs_camera.launch

6.2 启动aruco节点识别Marker标签

打开一个新的终端，执行以下命令启动single_realsense.launch：

cd ~/catkin_wssource devel/setup.bashroslaunch aruco_ros single_realsense.launch

6.3 启动image_view节点显示图像

打开一个新的终端，执行以下命令启动image_view订阅/aruco_single/result显示图像：

cd ~/catkin_wssource devel/setup.bashrosrun image_view image_view image:=/aruco_single/result

6.4 启动RM机械臂的control和driver节点

打开一个新的终端，执行以下命令启动control节点：

cd ~/catkin_wssource devel/setup.bashroslaunch rm_75_control rm_75_control.launch

注意：本教程以RM75机械臂为例，如果使用的是6轴即RM65机械臂，则启动control的命令为：roslaunch rm_control rm_control.launch

再打开一个新的终端，执行以下命令启动driver节点：

cd ~/catkin_wssource devel/setup.bashroslaunch rm_75_bringup rm_robot.launch

注意：本教程以RM75机械臂为例，如果使用的是6轴即RM65机械臂，则启动driver的命令为：roslaunch rm_bringup rm_robot.launch

运行成功后在rviz中可以看到机器人模型与真实机械臂的状态保持一致，如图所示：

在rviz的MotionPlanning下的Planning，Goal State选择forward然后点击Plan & Execute，可以看到真实机械臂会按照rviz中MoveIt!规划的路径运动，如图所示：

6.5 启动easy_handeye手眼标定节点

在启动标定程序前，建议先在rviz中通过Moveit或手动调节机械臂到一个合适的姿态，然后将aruco二维码移动至相机视野中心处附近，如图所示（建议参考图中的姿态，避免在标定时有不可达的规划）：

打开一个新的终端，执行以下命令启动手眼标定节点：

cd ~/catkin_wssource devel/setup.bashroslaunch easy_handeye eye_in_hand_calibrate.launch

成功运行后，会同时打开三个界面：

界面1

界面2

界面3

标定步骤：

在界面3中，点击Check starting pose，稍微等待，若检查成功，界面会出现：0/17，Ready to start，如图所示（如果检测失败或者没有出现0/17，则说明当前机械臂的姿态不合适，可能有规划不可达，需要关闭标定节点然后调节机械臂到一个合适姿态并将aruco二维码移动至相机视野中心处附近再重启启动标定节点继续标定）：
界面3中依次点击 Next Pose，Plan，Execute，机械臂会移动至新的位置，若二维码完全的在相机视野范围内，且能检测成功，则进行下一步，若二维码不在相机视野范围内或部分在视野外，检测不到，则再次依次点击 Next Pose，Plan，Execute，直到二维码完全在相机视野范围内，且能检测成功，再进行下一步（一般前4次会依据aruco二维码位置机械臂移动到上下左右位置，二维码都不完全在相机视野范围内，从第五次开始检测成功），如图所示：
界面2中点击Take Sample，若Samples对话框中出现有效信息，说明第一个点标定成功
重复执行步骤 2 和步骤 3，直至 17 个点全部标定完毕
界面2中点击Compute，则Result对话框中会出现结果
界面2中Save，会将结果保存为一个YAML文件，路径为 ~/.ros/easy_handeye

标定完成后在每个终端按Ctrl+C即可结束运行节点。

6.6 简单测试

按照6.1至6.4启动Realsense、aruco、image_view、rm_control、rm_driver相关节点，不需要再启动easy_handeye手眼标定节点，然后再打开一个新的终端，执行以下命令发布手眼标定后的TF：

roslaunch easy_handeye publish.launch

执行以下命令可以查看相机与机械臂基座标系的TF转换：

rosrun tf tf_echo /base_link /camera_color_frame

执行以下命令如果检测到marker标签，会显示marker在相机坐标系下的位姿：

rostopic echo /aruco_single/pose

标定过程可能会受相关硬件及环境等条件因素的影响，所以标定结果可能存在一定的误差，用户标定完成后可以通过获取物体在相机坐标系下的位姿，然后通过TF转换到机械臂下的位姿进行测试，存在误差可以后续在程序进行补偿计算。