ur3_kinect v2手眼标定

ur3_kinect v2利用ros手眼标定

笔者软硬件环境：
Ubuntu18.04
ros_melodic
ur3机械臂，polyscope 3.12
Kinect v2传感器

本文参考链接：
Kinect v2 在ros上利用easy_handeye进行手眼标定

硬件准备：

首先讲标定板贴到机械臂的末端，如果没有标定版的话可以在
aruco_ros/aruco_ros/etc文件夹下找到，并且打印，记得一定按照1：1的比例打印。
这里附一个标定板自动生成的网址分享给大家，挺好用的
https://chev.me/arucogen/
相机内参标定板网址：
https://calib.io/pages/camera-calibration-pattern-generator

一、首先环境配置

1,安装universal_robot（并不局限于这个操作，此处使用ur_modern_drive,可以参考我另一篇博客：

mkdir -p ~/universal_robot/src
cd ~/universal_robot/src
git clone https://github.com/ros-industrial/universal_robot.git
git clone https://github.com/ros-industrial/industrial_core.git
cd universal_robot
git clone https://github.com/beta-robots/ur_modern_driver.git
cd ~/universal_robot
catkin_make
source  ~/devel/setup.bash（不要忘记source将包添加到当前工作目录）
我是直接在ros下的bashrc文件下添加了这句话，避免每次source
gedit .bashrc
source /home/harry/catkin_ws/devel/setup.bash（前面两级目录需要根据自己的名字改变）

2,安装iai_kinect2 （kinect v2的启动节点

参考网址：https://blog.csdn.net/u012424737/article/details/80609451
首先对libfreenect2 驱动配置，我是将libfreenect2 安装在home目录下，按照文章操作即可，知道在build文件夹下运行./bin/Protonect显示以下图像，便安装完驱动，下一步便是安装kinect v2的节点包，步骤如下：
2-1在ROS中配置Kinect的是通过 IAI Kinect2 包的安装实现的

cd ~/catkin_ws/src/  #进行ROS的工作目录下的源文件目录
git clone https://github.com/code-iai/iai_kinect2.git  
cd iai_kinect2 
rosdep install -r --from-paths . 
cd ~/catkin_ws 
catkin_make
由于这里我是直接修改了bashrc，所以不用每次都source

通过运行验证是否成功，成功后显示如下图像：
roslaunch kinect2_bridge kinect2_bridge.launch
rosrun kinect2_viewer kinect2_viewer

3,安装ros-kinetic-visp

直接安装到计算机目录下 ：
sudo apt-get install ros-melodic-visp

4,安装vision_visp

cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/lagadic/vision_visp.git
catkin_make --pkg visp_hand2eye_calibration

针对编译会卡住的情况，把下面两个包删掉，因为这两个包编译时需要下载文件，但是文件现在不存在了，而且这两个是用于跟踪，不影响手眼标定。

5,aruco_ros

cd ~/catkin_ws/src
 git clone -b kinetic-devel https://github.com/pal-robotics/aruco_ros
 catkin_make

6,easy_handeye ，这一步编译时间会慢一些

cd ~/catkin_ws/src
 git clone https://github.com/IFL-CAMP/easy_handeye
catkin_ws$ catkin_make

二、验证

对aruco_ros/aruco_ros/launch/single.launch文件如下，这里我修改了几个参数，582和0.067分别对应的ID号和标签尺寸，这是根据我选择并打印出来的标签实际决定的：

<?xml version="1.0"?>
<launch>

    <arg name="markerId"        default="582"/>
    <arg name="markerSize"      default="0.067"/>    <!-- in m -->
    <arg name="eye"             default="left"/>
    <arg name="marker_frame"    default="aruco_marker_frame"/>
    <arg name="ref_frame"       default=""/>  <!-- 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="/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="0.067"/>
        <param name="marker_id"          value="582"/>
        <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>

    <!--node pkg="aruco_ros" type="single" name="aruco_single">
        <remap from="/camera_info" to="/stereo/$(arg eye)/camera_info" />
        <remap from="/image" to="/stereo/$(arg eye)/image_rect_color" />
        <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)"/> 
        <param name="camera_frame"       value="stereo_gazebo_$(arg eye)_camera_optical_frame"/>
        <param name="marker_frame"       value="$(arg marker_frame)" />
        <param name="corner_refinement"  value="$(arg corner_refinement)" />
    </node-->

</launch>

注意：aruco标签在~/catkin_ws/src/aruco_ros/aruco_ros/etc有打印出的标签才可以被检测到，这里我直接打印的里面存在的标签号582
然后我先判断用kinect v2能否检测到aruco标签，如果显示坐标，便可以检测，注意修改image_View左边订阅的节点为 /aruco_single/result
下方为/aruco_single/result_mouse_left

运行程序：

roslaunch kinect2_bridge kinect2_bridge.launch
roslaunch aruco_ros single.launch
rqt_image_view

现在已经可以检测到aruco标签，下面就正式开始进行手眼标定操作：
首先在easy_handeye下新建了ur3_kinectv2_calibration.launch，

注意
name=“robot_ip” value=“192.168.56.1” />与实际IP相一致，代码如下：

<?xml version="1.0"?>
<launch>
    <arg name="namespace_prefix" default="ur3_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.150" />
 <arg name="marker_id" doc="The ID of the ArUco marker used" default="582" />
 
    <!-- 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_modern_driver)/launch/ur3_bringup.launch">
        <arg name="limited" value="true" />
        <arg name="robot_ip" value="192.168.56.2" />
    </include>
 
    <include file="$(find ur3_moveit_config)/launch/ur3_moveit_planning_execution.launch">
        <arg name="limited" value="true" />
    </include> 
    
</launch>

然后新建了easy.launch文件代码如下：

<?xml version="1.0"?>
<launch>
<!-- start easy_handeye -->
 
<arg name="namespace_prefix" default="ur3_kinect_handeyecalibration" />
    <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.1" />
        <arg name="robot_acceleration_scaling" value="0.1" />
    </include>
 
</launch>

注意上面： tracking_base_frame就是相机的frame, tracking_marker_frame是标志物的frame, 这两者之间的关系由aruco_ros发布, 所以要对上; robot_base_frame就是机械臂底座, robot_effector_frame是末端, 这两个跟UR3发布的要对上.
在工作空间的src文件目录下打开终端，运行下面程序进行手眼标定：

roslaunch kinect2_bridge kinect2_bridge.launch 
roslaunch easy_handeye ur3_kinectv2_calibration.launch
roslaunch easy_handeye easy.launch 

然后弹出三个窗口：
注意一定要在第一个图中打开image_view，显示采集到的标签图像
直到窗口顶部会出现选项目录plugins->Visualization->image_View;
图3中点击check_starting pose应该为0/17,连接成功后。

image_View下左边订阅的节点为 /aruco_single/result
下方为/aruco_single/result_mouse_left，显示如下：
如果没有此选项，就在rviz添加image，然后订阅 /aruco_single/result，效果是一样的，目的是确认采集的标签不会超出相机的视野边缘之外。

三，进行手眼标定

具体操作步骤，
1,为了安全流畅地操作，我们先在rviz的Motion Planning界面中将机械臂的速度和加速度都设为0.1。
2,图3中是否显示0/17，如果不是就按check starting pose
3,在图1中点击take sample，采样完后点击在图3界面点击next pose，再点击plan，最后点击excute后机械臂会运动到新的位置，再点击take sample，这样反复进行17次，便可以进行采集17点后，点击compute后，可以得到相机与机械臂的位姿变换矩阵。

再点击save后，便可以在.ros隐藏文件下（在home目录下按ctrl+h便可以显示隐藏文件）看到easy_handeye的文件下的ur3_kinect_handeyecalibration_eye_on_base.yaml文件，文件内容如下：

eye_on_hand: false
robot_base_frame: base_link
tracking_base_frame: kinect2_rgb_optical_frame
transformation:
  qw: 0.1434016682482555
  qx: -0.16165056534351638
  qy: 0.7128775670495471
  qz: -0.6671661192426197
  x: -0.4258619055680306
  y: -0.03867852023257894
  z: 0.68036831927051

标定从开始到结束过程问题及解决办法：

1.问题一：

标定板的安装位置，这个应该是可以随意安装的，可以用夹爪夹着，也可以贴了机械臂的末端

2.问题二：

在运行时候可能会显示cv2没有cv2.CALIB_HAND_EYE_TSAI，并且只弹出了rviz一个窗口，没有弹出另外俩个

'Tsai-Lenz': cv2.CALIB_HAND_EYE_TSAI, AttributeError: 'module' object has no attribute 'CALIB_HAND_EYE_TSAI' ，说是cv没有CALIB_HAND_EYE_TSAI

此处要感谢我的师弟，之前他遇到过这个问题，并得到了解决。
重装一遍OpenCV，运行命令：

python -m pip install opencv-contrib-python

问题三：

弹出三个窗口，当点击check staring pose时，一直报错，并不能显示0/17，而是一直显示0/1。

如果出现错误提示：Can't calibrate from this position!

可以修改目录easy_handeye/easy_handeye/src/easy_handeye中的python脚本handeye_robot.py

将函数_check_target_poses(self, joint_limits)定义中的下面两行注释掉即可

            # if len(plan.joint_trajectory.points) == 0 or CalibrationMovements._is_crazy_plan(plan, joint_limits):

            #     return False

参考链接：
优遨机器人UR5与RealSense深度摄像头D435的手眼标定
解决办法：
找到handeye_robot.py

注释掉

if len(plan.joint_trajectory.points) == 0 or CalibrationMovements._is_crazy_plan(plan, joint_limits):
                #return False

问题四：

很多时候进行点击check starting pose或者take sample会出现闪退现象，此时查看错误信息，可能机械臂的姿态不对，不能进行初始化
解决办法：
将机械臂回零点以后，再运行标定程序，打开三个窗口，然后再将机械臂拖动到想要标定的Kinect的视野中去。