机械臂eye-in-hand手眼标定方法-GrantLi

一、RealSense相机驱动安装

根据选择的相机安装相应的驱动，这里选择realsense相机作为例程。

（一）RealSense SDK deb安装及测试

注册服务器的公钥

sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

添加下载源

sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

安装依赖库

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils

sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg

测试

realsense-viewer

如果没有显示图像，进行下面操作更新相机硬件：

#查看usb接口
lsusb

#升级固件
intel-realsense-dfu -b 002 -d 002 -f -i /home/intel/Downloads/Signed_Image_UVC_5_10_6_0.bin

参考教程硬件下载网站

（二）RealSense SDK源码安装及测试(可选)

安装相应的依赖

sudo apt-get install -y libudev-dev \
        pkg-config \
        libgtk-3-dev \
        libusb-1.0-0-dev \
        pkg-config  \
        libglfw3-dev  \
        libssl-dev

RealSense SDK源码

mkdir -p ~/realsense_ws/src
cd  ~/realsense_ws

git clone https://gitee.com/gjxs1980/librealsense.git

cd ~/realsense_ws/librealsense
sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && udevadm trigger

RealSense SDK源码编译

cd ~/realsense_ws/librealsense

mkdir build
cd build
cmake ../ -DBUILD_EXAMPLES=true
make
sudo make install

测试

cd ~/realsense_ws/librealsense/build/examples/capture
./rs-capture

（三）RealSense ROS 安装及测试

RealSense-ROS安装

sudo apt-get install ros-${ROS_DISTRO}-realsense2-*

测试

#	启动点云
roslaunch realsense2_camera demo_pointcloud.launch

#	启动rgb相机
roslaunch realsense2_camera rs_camera.launch

（四）RealSense 相机内参标定

官方标定参考教程
 kinect标定参考教程

camera_calibration安装

sudo apt-get install ros-${ROS_DISTRO}-camera-calibration

标定棋盘格下载
标定板下载
相机标定

#  终端1
roslaunch realsense2_camera rs_camera.launch

#  终端2
rosrun camera_calibration cameracalibrator.py --size 5x7 --square 0.03 image:=/camera/color/image_raw camera:=/camera_color_optical_frame --no-service-check

其中，/camera_color_optical_frame是通过查看相机的内参文件话题rostopic echo /camera/color/camera_info中的frame_id.
4. 标定完成之后，将标定结果的yaml拷贝到file://$(find realsense2_camera)/calibration/目录下的realsense_rgb.yaml文件里面。

二、标定功能包安装及使用

本次教程适用与eye-in-hand模式的手眼标定，eye-to-hand需要修改相应的link和模式选择。

安装依赖

sudo apt-get install -y ros-${ROS_DISTRO}-aruco-ros \
	ros-${ROS_DISTRO}-moveit*  \
    ros-${ROS_DISTRO}-gazebo-ros*  \
    ros-${ROS_DISTRO}-rviz-visual-tools \
    ros-${ROS_DISTRO}-gazebo-plugins \
    ros-${ROS_DISTRO}-joint-state-controller \
    ros-${ROS_DISTRO}-position-controllers \
    ros-${ROS_DISTRO}-joint-trajectory-controller \
    ros-${ROS_DISTRO}-industrial-* 

# libprotobuf9
wget http://archive.ubuntu.com/ubuntu/pool/main/p/protobuf/libprotobuf9v5_2.6.1-1.3_amd64.deb 
sudo dpkg -i libprotobuf9v5_2.6.1-1.3_amd64.deb

# eigen3软链接
sudo apt-get install libeigen3-dev
sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen
sudo ln -sf /usr/include/eigen3/unsupported//usr/include/unsupported

lib库环境变量配置
(1)进入aubo sdk库的路径

cd ~/aubo_ws/
source devel/setup.bash
roscd aubo_driver/lib/lib64/

# 打印当前文件夹绝对路径
pwd

记录当前文件夹绝对路径
(2)ldconfig配置文件
新增并编辑aubo_driver.conf文件

sudo gedit /etc/ld.so.conf.d/aubo_driver.conf

添加下面内容到aubo_driver.conf里面：

~/aubo_ws/src/aubo_robot/aubo_driver/lib/lib64
~/aubo_ws/src/aubo_robot/aubo_driver/lib/lib64/aubocontroller
~/aubo_ws/src/aubo_robot/aubo_driver/lib/lib64/config
~aubo_ws/src/aubo_robot/aubo_driver/lib/lib64/log4cplus

其中~代表/home/your_name。
(3)使配置生效

sudo ldconfig

aruco码生成及配置
在线生成aruco码
其中arkerSize为aruco的尺寸大小，markerId为aruco的id。
配置aubo和easy_handeye功能包

mkdir -p ~/aubo_ws/src
cd ~/aubo_ws/src
git clone http://119.29.41.151:1080/GrantLi/aubo_robot.git
git clone http://119.29.41.151:1080/GrantLi/easy_handeye.git
cd .. && catkin_make

开始标定
终端1启动auboe5机械臂：

cd ~/aubo_ws && source devel/setup.bash
roslaunch easy_handeye auboe5.launch

终端2启动easy_handeye和realsense：

cd ~/aubo_ws && source devel/setup.bash
roslaunch easy_handeye auboe5_realsense_calibration.launch

终端3启动rqt_image_view订阅AR码识别话题：

rqt_image_view

三、标定结果验证

数据处理
标定拿到的是相机（camera_link）相对于末端关节（wrist3_Link）位置xyz加上四元数wxyz，其中四元数需要转换成欧拉角rpy，具体转换可以通过python三行代码完成：

import tf
(r, p, y) = tf.transformations.euler_from_quaternion([x, y, z, w])
print((r, p, y))

根据处理的数据创建TF并发布处理

<?xml version='1.0' encoding='utf-8'?>
<launch>
    <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster0" args="x y z r p y wrist3_Link camera_link  40" />  
</launch>

打开rviz查看TF对应关系，验证与实际安装位置是否重合。

四、附录

(1)rs_camera_qr.launch

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>

  <arg name="fisheye_width"       default="640"/>
  <arg name="fisheye_height"      default="480"/>
  <arg name="enable_fisheye"      default="true"/>

  <arg name="depth_width"         default="640"/>
  <arg name="depth_height"        default="480"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="infra_width"        default="640"/>
  <arg name="infra_height"       default="480"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>

  <arg name="color_width"         default="640"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="30"/>
  <arg name="depth_fps"           default="30"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="30"/>
  <arg name="gyro_fps"            default="400"/>
  <arg name="accel_fps"           default="250"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>

  <arg name="enable_sync"           default="false"/>
  <arg name="align_depth"           default="false"/>

  <arg name="filters"               default=""/>
  <arg name="clip_distance"         default="-2"/>
  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="initial_reset"         default="false"/>
  <arg name="unite_imu_method"      default=""/>

  <arg name="camera/color/camera_info" value="file://$(find realsense2_camera)/calibration/realsense_rgb.yaml" />
  
  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
    </include>
  </group>
</launch>

(2)auboe5_realsense_calibration.launch

<?xml version='1.0' encoding='utf-8'?>
<launch>

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

    <arg name="robot_ip" doc="The IP address of the auboe5 robot" />

    <arg name="marker_size" value="0.1" doc="Size of the ArUco marker used, in meters" />
    <arg name="marker_id" value="1" doc="camera_marker" />

    <!-- start the realsense -->
    <include file="$(find easy_handeye)/launch/rs_camera_qr.launch" />

    <!-- start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="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 marker_size)"/>
        <param name="marker_id"          value="$(arg marker_id)"/>
        <param name="reference_frame"    value="camera_link"/>
        <param name="camera_frame"       value="camera_color_optical_frame"/>
        <param name="marker_frame"       value="camera_marker" />
    </node>

    <!-- 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="true" />

        <arg name="tracking_base_frame" value="camera_link" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <arg name="robot_base_frame" value="base_link" />
        <arg name="robot_effector_frame" value="wrist3_Link" />
    </include>

</launch>

(3)auboe5.launch

<?xml version='1.0' encoding='utf-8'?>
<launch>
    <!-- start the robot -->
    <include file="$(find aubo_e5_moveit_config)/launch/moveit_planning_execution.launch">
        <arg name="sim" value="true" />
        <arg name="robot_ip" value="192.168.3.20" />
    </include>
</launch>

(4)tf_auboe5.launch

<?xml version='1.0' encoding='utf-8'?>
<launch>
    <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster0" args="-0.08190519647302377 0.06475754411136914 -0.08640952142638286 -0.0286 -0.057 -1.76 wrist3_Link camera_link  40" />  
</launch>

1. 棋盘格在线生成网站

目录CONTENT

机械臂eye-in-hand手眼标定方法