Ubuntu18.04下UR5机械臂与kinect2（kinect for xbox ONE）相机的标定

一、本文运行环境

Ubuntu18.04

ROS melodic（安装Universal Robot，并且使用ur_robot_driver）

机械臂驱动包括：

melodic-devel的Universal Robot链接https://github.com/ros-industrial/universal_robot/tree/melodic-devel

ur_robot_driver的链接https://github.com/UniversalRobots/Universal_Robots_ROS_Driver

相机驱动包括：

libfreenect2, which is ubuntu下的kinect2相机驱动

iai_kinect2, which is a bridge between libfreenect2 and ROS

安装教程Ubuntu 18.04 安装iai_kinect2遇到的问题_wyf187的博客-优快云博客

标定程序包括：

aruco_ros

easy_handeye

以及一些相关依赖

二、标定程序安装教程

1. 安装aruco_ros，下载地址https://github.com/pal-robotics/aruco_ros/tree/2.1.4

2. ****重要****

melodic下aruco_ros的版本选择tag里2.1.4的版本，最新的版本有bug会报错

如果你最后运行时显示错误为!isValid(): invalid marker. It is not possible to calculate extrinsics in function calculateExtrinsics，然后显示的是~/ur_ws/src/aruco_ros-2.1.4/aruco/src/aruco/marker.cpp报错，那么请把aruco的包删除，melodic用aruco 2.1.4的版本重新安装，noetic可能是3.03的版本.

3. 解压到~/工作空间/src/

4.catkin_make

5. 安装easy_handeye下载链接https://github.com/IFL_CAMP/easy_handeye

6. 解压到~/工作空间/src/

7. catkin_make

8.  安装vision_visp 下载链接https://github.com/lagadic/vision_visp/tree/melodic-devel

9. 解压到~/工作空间/src/

10. catkin_make

三、系统软件的安装

标定需要用到一些python的库和rqt，所以安装一下必备组件

1. sudo apt-get install python-pip

2. pip install pip确保自己pip是最新版本不然可能pip不了

3. pip install transforms3d

4. sudo apt-get install ros-melodic-moveit-commander有时候会报错没有moveit-commander，所以安装一下

5. pip install opencv-contrib-python 这个是python的opencv库，也是必要的，ros里可能自带了opencv但是python里不一定有

6. sudo apt-get install ros-melodic-rqt

sudo apt-get install ros-melodic-rqt-common-plugins

这两个都是用来更新rqt版本的，确保自己rqt能打开image view界面就行

运行roscore

新终端运行rosrun rqt_image_view rqt_image_view

显示

就是没问题

四、标定步骤

1. 打印aruco标签，网址https://chev.me/arucogen/，Dictionary选择Original ArUco，本文marker ID选取100，Markersize选取0.1m

2. 把打印好的标签贴在末端，这个位置是没有要求的，因为计算的时候选取的都是相对值。保证标签表面平整，连接牢固即可

3. 编写.launch文件

进入easy_handeye安装的根目录，找到/easy_handeye/launch，空白处右击终端打开

输入touch ur_calibrate.launch新建空白.launch文件

双击进入ur_calibrate.launch

我编写的.launch文件代码如下，供参考

<launch>
    <arg name="namespace_prefix" default="ur5_kinect_handeyecalibration" />
 
    <arg name="robot_ip" doc="The IP address of the UR5 robot" />
		<!--<arg name="marker_frame" default="aruco_marker_frame"/>-->
    <arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.1" />
    <arg name="marker_id" doc="The ID of the ArUco marker used" default="100" />
    <!-- start the Kinect -->
    <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch" />
    <!--    <arg name="depth_registration" value="true" />
    </include>-->
 
    <!-- start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <remap from="/camera_info" to="/kinect2/hd/camera_info" />
        <remap from="/image" to="/kinect2/hd/image_color_rect" />
        <param name="image_is_rectified" value="true"/>
        <param name="marker_size"        value="$(arg marker_size)"/>
        <param name="marker_id"          value="$(arg marker_id)"/>
        <param name="reference_frame"    value="kinect2_rgb_optical_frame"/>
        <param name="camera_frame"       value="kinect2_rgb_optical_frame"/>
        <param name="marker_frame"       value="camera_marker" />
    </node>
 
    <!-- start the robot -->
    <include file="$(find ur_robot_driver)/launch/ur5_bringup.launch">
        <arg name="robot_ip" value="192.168.56.2" />
    </include>
    <include file="$(find ur5_moveit_config)/launch/moveit_planning_execution.launch">
    </include>
 
    <!-- start easy_handeye -->
    <include file="$(find easy_handeye)/launch/calibrate.launch" >
        <arg name="namespace_prefix" value="$(arg namespace_prefix)" />
        <arg name="eye_on_hand" value="false" />
 
        <arg name="tracking_base_frame" value="kinect2_rgb_optical_frame" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <arg name="robot_base_frame" value="base_link" />
        <arg name="robot_effector_frame" value="wrist_3_link" />
 
        <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>

 把robot_ip改成你自己的，marker_size和marker_id改成你选用的，注意size的单位是米。我用的是ur_robot_driver

完了以后保存，cd ~/工作空间

catkin_make

source ~/工作空间/devel/setup.bash

为了方便起见，在主目录下ctrl+H显示隐藏文件，进入.bashrc，在最后一行添加

source /home/wyf/ur_ws/devel/setup.bash

省得每次都需要source

4. 打开ur5，连接至网络

运行roscore

运行roslaunch easy_handeye ur_calibrate.launch

完了以后等1秒钟左右，点击示教板上运行外部控制的程序ur5_externalcontrol

成功后应该弹出三个界面，缺一不可

打开新终端运行rqt

选择plugins->visualization->image view

底下选择/aruco_tracker/result

看到标定板的坐标出现表示没有问题

在rviz中把fixed frame改成base_link，应该可以在3d界面中看到关节和相机的位姿

5. 依次点击nextpose->plan->excute,观察到机械臂运行到为后，点击take sample

6.循环采集17个点后，点击compute

7. 如图，即可得到标定后的矩阵，可以用来计算相机坐标与机械臂坐标之间的转换啦！！！

其中translation代表平移，rotation代表姿态，用四元数表示，终端里也同时给出了其相对应的齐次矩阵。其中四元数旋转的物理意义可以看下面这个博文。用四元数表示旋转优点在于参数较少

 四元数旋转的物理意义以及代码实现-偏应用向_hehedadaq的博客-优快云博客_四元数物理意义