通过ROS控制真实机械臂(15) --- 视觉抓取之手眼标定

通过视觉传感器赋予机械臂“眼睛”的功能，配合ATI力和力矩传感器，就可以完成机械臂“手眼”结合的能力，完成视觉抓取过程。目前测试的视觉传感器为 ZED mini双目相机，配置安装过程 https://blog.csdn.net/qq_34935373/article/details/103481401 。

1.手眼标定（内参）

对于单目usb相机，可以通过ROS包usb_cam来启动相机，通过camera_calibration功能包http://wiki.ros.org/camera_calibration/完成相机内参的标定过程。本次使用的ZED双目相机，出厂的时候已经完成了内参的标定。如果需要手动重新标定，可以通过ZED自带软件完成标定，双目不仅需要单独标定每个摄像头得到两个摄像头各自的内参矩阵和畸变参数向量，还需要要计算目标点在左右两个视图上形成的视差，首先要把该点在左右视图上两个对应的像点匹配起来。然而，在二维空间上匹配对应点是非常耗时的，为了减少匹配搜索范围，我们可以利用极线约束使得对应点的匹配由二维搜索降为一维搜索。而双目校正的作用：把消除畸变后的两幅图像严格地行对应，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。

2.手眼标定（外参）

内参的标定是确定摄像机从三维空间到二位图像的投影关系，而外参则是确定摄像机坐标与世界坐标系之间的相对位置关系。他们之间关系为 (详情请看视觉SLAM十四讲第五讲相机与图像)：

$Pc = RPw + T$

其中Pw为世界坐标，Pc是摄像机坐标，式中，T= (Tx,Ty,Tz)，是平移向量，R = R(α,β,γ)是旋转矩阵，分别是绕摄像机坐标系z轴旋转角度为γ，绕y轴旋转角度为β，绕x轴旋转角度为α。6个参数组成（α,β,γ,Tx,Ty,Tz)为摄像机外参。

3.手眼标定（调包）

对于手眼外参标定，场景主要有以下两种，

eye in base，眼在手外，这种场景下我们已知机械臂终端end_link与base_link、相机camera_link与识别物体object_link之间的关系，需要求解camera_link与base_link之间的变换；
eye on hand，眼在手上，这种场景base_link和机械臂各关节joint_link、end_link已经通过URDF发布了，只需要求解camera_link与end_link之间的变换。

1. ROS也为我们提供了功能包，visp_hand2eye_calibration和ros_easy_handeye。首先测试visp_hand2eye_calibration，这个包是用来标定eye in base 眼在手外的情形。

# 安装功能包
$ sudo apt-get install ros-indigo-visp-hand2eye-calibration# 启动测试样例
$ rosrun visp_hand2eye_calibration visp_hand2eye_calibration_client# 启动 calibrator 节点
$ rosrun visp_hand2eye_calibration visp_hand2eye_calibration_calibrator

将会显示一些输出如下,表明安装成功，接下来就可以集成到自己的标定环境中了：

[ INFO] [1329226083.184531090]: Waiting for topics...
[ INFO] [1329226084.186233704]: 1) GROUND TRUTH:
[ INFO] [1329226084.186327570]: hand to eye transformation:
translation: x: 0.1y: 0.2z: 0.3
rotation: x: 0.96875y: 0.0863555z: -0.0863555w: 0.215889

修改到自己的标定环境中：

# 启动主节点
$ roscore
# 启动 calibrator 节点
$ rosrun visp_hand2eye_calibration visp_hand2eye_calibration_calibrator# 终端输出提示如下：[ WARN] [1577429182.106793405]: The input topic '/camera_object' is not yet advertised[ WARN] [1577429182.106871954]: The input topic '/world_effector' is not yet advertised

calibrator需要我们发布如下两个话题：

world_effector (visp_hand2eye_calibration/TransformArray)

transformation between the world and the hand frame. The node expects to receive several of those transformations.

camera_object (visp_hand2eye_calibration/TransformArray)

transformation between the camera and the object frame. The node expects to receive several of those transformations.

calibrator发布如下服务：

compute_effector_camera (visp_hand2eye_calibration/compute_effector_camera)

Returns the hand to camera transformation result after calibration based on the data published on the subscribed topics.

compute_effector_camera_quick (visp_hand2eye_calibration/compute_effector_camera_quick)

Returns the hand to camera transformation result after calibration. This service is not based on the data published on the subscribed topics. Instead, the transformation are passed as service parameters.

reset (visp_hand2eye_calibration/reset)

Reset all the data published on the subscribed topics. Only new publications (transformations) will be taken into account.

总结来说就是移动机械臂采集多个点发送到：world_effector 和 camera_object 两个topic下，通过 compute_effector_camera 这个service得到变换矩阵。这位老哥写的挺好，https://blog.csdn.net/MzXuan/article/details/79177747

2. 第一种方法用到人貌似比较少，使用场景也比较局限，第二种方法基于第一种方法，这个包可以标定eye-in-hand和eye-on-base两种场景，还提供了GUI界面预期的输入为TF中发布的坐标变换。此外，还提供了一种保存和发布标定结果的方法。如下图所示，该种方法可以完成两种类型的标定。值得一提的是作者还提供了一个基于iiwa机械臂的使用例程，该例程可以在没有任何硬件的情况下使用，它使用RViz仿真的机械臂和虚拟发布的相机坐标系。

# 进入工作目录
$ cd catkin_ws/src
# 安装例程依赖包
$ git clone https://github.com/IFL-CAMP/iiwa_stack
# 手眼标定功能包
$ git clone https://github.com/IFL-CAMP/easy_handeye
# 例程功能包
$ git clone https://github.com/marcoesposito1988/easy_handeye_demo

下载完成之后，编译iiwa包，提示CMake版本过低，但由于我们只是简单的使用一下iiwa作为仿真对象，只需要修改一下CMakeLists.txt，将cmake_minimum_required(VERSION 3.5)修改成cmake_minimum_required(VERSION 2.8.3)即可。有些包编译缺少依赖或者不通过也不太影响。启动launch文件查看是否编译成功，打开TF观察发布的关节坐标系。

如上图所示，iiwa机械臂的机器人坐标系为iiwa_link_0和世界坐标系world重合，而末端执行器为iiwa_link_ee，iiwa_link_7坐标系就是安装标定板的位置，所以我们后面只需要修改launch文件中的坐标系名称为自己的机器人坐标系即可将手眼标定包集成到自己的项目当中。

easy_handeye_demo包中还有一个check_calibration.launch,其功能是检测标定板相对与末端执行器(或者机器人基坐标系)是否处于固定位置，以便于后面的手眼标定过程。

# 编译例程包
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="easy_handeye_demo"# 启动eye_in_base标定
$ roslaunch easy_handeye_demo calibrate.launch
# 启动eye_on_hand标定
$ roslaunch easy_handeye_demo calibrate.launch eye_on_hand:=true

上图分别对应eye_on_hand和eye_in_base两种情况，其中tracking_origin表示的是相机坐标系，而tracking_marker则表示标定板的位置。因此通过矩阵计算，就可以获得相机坐标系到机器人坐标系的变换矩阵。

由于没有硬件，所以tracking_marker和tracking_origin的位置是通过手动发布的静态坐标变化得到的。可以在launch文件中看到：

<param name="eye_on_hand" value="$(arg eye_on_hand)" />
<!-- transformations for the eye-on-base case -->
<param unless="$(arg eye_on_hand)" name="ground_truth_calibration_transformation" value="1 0 0.5 0 0 0 1" />
<param unless="$(arg eye_on_hand)" name="arbitrary_marker_placement_transformation" value="0.12 0.21 0.137 0 0 0 1" />
<!-- transformations for the eye-on-hand case -->
<param if="$(arg eye_on_hand)" name="ground_truth_calibration_transformation" value="0.12 0.21 0.137 0 0 0 1" />
<param if="$(arg eye_on_hand)" name="arbitrary_marker_placement_transformation" value="1 0 0.5 0 0 0 1" />

当想要修改该demo包用于自己的仿真机器人的时候，修改launch的如下位置中的value即可：

<arg name="robot_base_frame" value="iiwa_link_0" />
<arg name="robot_effector_frame" value="iiwa_link_ee" />
<arg name="tracking_base_frame" value="tracking_origin" />
<arg name="tracking_marker_frame" value="tracking_marker" />

当有硬件的时候，上面的demo中发布静态坐标的方式显然就不合理了，为此我们需要使用真正的easy_handeye完成标定过程了。为此，作者也为我们提供了一个基于真实机械臂UR5和Kinect2相机的例程。

如下所示，首先需要启动kinect和ArUco，aruco是用来识别特定图案的。用aruco_ros识别末端上的标志物, 得到camera_link到marker_link的转换. 之后按照GUI控制机械臂在相机视野内产生17个姿态, 每次都把样本记录下来. 最后点击求解, easy_handeye会调用visp的代码得到标定结果. 然后点save, 标定结果会被保存在~/.ros/easy_handeye/---.yaml. 之后就可以关掉标定程序,再运行publish.launch即可发布base_link到camera_link的转换.参考链接：https://zhuanlan.zhihu.com/p/33441113

1. aruco_ros需要配置两个参数. 首先是相机和机器人的参考坐标系:

tracking_base_frame就是相机的参考坐标系
tracking_marker_frame是标志物的参考坐标系
robot_base_frame就是机械臂参考坐标系
robot_effector_frame是末端执行器参考坐标系

2. 第二个参数是需要知道aruco的ID跟边长. 通过网站http://chev.me/arucogen/可以生成这种图案.（选择参数为：original 50 100）

# 安装aruco_ros
$ cd ~/catkin_ws/src
$ git clone https://github.com/pal-robotics/aruco_ros.git# 如果编译aruco不通过，提示缺少OpenCV3依赖的话，如下操作
# ros indigo版本需要手动安装opencv3
$ sudo apt-get install ros-indigo-opencv3# 如果报错CMake Error in whycon/CMakeLists.txt: Imported target "opencv_xphoto" includes non-existent path  "/usr/include/opencv-3.1.0-dev/opencv"的话
$ sudo vim /opt/ros/indigo/share/OpenCV-3.1.0-dev/OpenCVConfig.cmake +113
修改如下：
113:get_filename_component(OpenCV_CONFIG_PATH "${CMAKE_CURRENT_LIST_FILE}" PATH CACHE)
改成
113:get_filename_component(OpenCV_CONFIG_PATH "${CMAKE_CURRENT_LIST_FILE}" PATH )# 启动标定
$ roslaunch easy_handeye ur5_kinect_calibration.launch# 点GUI里的check, 初始化17个姿态# 然后next pose一下就记录一下sample# 计算, 保存, 关掉标定程序# 通过launch发布标定结果
$ roslaunch easy_handeye publish.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_size" doc="Size of the ArUco marker used, in meters" /><arg name="marker_id" doc="The ID of the ArUco marker used" /><!-- start the Kinect --><include file="$(find freenect_launch)/launch/freenect.launch" ><arg name="depth_registration" value="true" /></include><!-- start ArUco --><node name="aruco_tracker" pkg="aruco_ros" type="single"><remap from="/camera_info" to="/camera/rgb" /><remap from="/image" to="/camera/rgb/image_rect_color" /><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_rgb_optical_frame"/><param name="marker_frame"       value="camera_marker" /></node><!-- start the robot --><include file="$(find ur_bringup)/launch/ur5_bringup.launch"><arg name="limited" value="true" /><arg name="robot_ip" value="192.168.0.21" /></include><include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch"><arg name="limited" value="true" /></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="camera_link" /><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>

接下来是利用Realsense相机和UR5机器人完成标定的过程，原本包里面提供的easy_handeye_demo中的ur5标定使用的是ur_driver这个驱动，在一波更新之后，实验室使用的是Universal_Robots_ROS_Driver这个包，所以需要修改一些东西。

如下所示，标记物的尺寸和ID以及机器人的ip地址需要修改，因为实验室使用的是realsense了，所以将freenect这部分修改成realsense的启动launch，同样的，对于相机和图像的坐标系，也要相应的改变，可以通过topic查看realsense的话题，修改成对应的(刚开始坐标系没有修改，启动之后一直有一些错误)。

对于UR5的驱动部分，将原本的ur_driver变成Universal_Robots_ROS_Driver的启动，这里需要注意的是，要先启动launch文件，这样在示教器中才能运行URCap的external_control，具体参考Universal_Robots_ROS_Drive的使用方法：https://blog.csdn.net/qq_34935373/article/details/109238762

<launch><arg name="namespace_prefix" default="ur5_kinect_handeyecalibration" /><arg name="robot_ip" doc="The IP address of the UR5 robot" default="192.168.1.101"/><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="50"/><!-- start the Kinect --><!-- <include file="$(find freenect_launch)/launch/freenect.launch" ><arg name="depth_registration" value="true" /></include> --><include file="$(find realsense2_camera)/launch/rs_rgbd.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 the robot --><!-- <include file="$(find ur_bringup)/launch/ur5_bringup.launch"> --><include file="$(find ur_robot_driver)/launch/ur5_bringup.launch"><!-- <arg name="limited" value="true" /> --><arg name="robot_ip" value="192.168.1.101" /></include><include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch"><arg name="limited" value="true" /></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="camera_link" /><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>

启动之后，会出现如下三个界面：

$ roslaunch easy_handeye_demo ur5_kinect_calibration.launch marker_size:=0.1 marker_id:=50

第一个图中显示的是水平躺着的机械臂，而且TF坐标映射不完整，这个说明和真实的UR链接出现问题了，要不就是URCap程序没运行，要不就是通信没连接之类的，正确没问题的情况是TF完整，而且RVIZ中的机械臂和真实世界机械臂的状态是一致的，处在相同的关节位置。

第二个界面初始化状态如下，需要全屏这个界面然后在左上角找出image view打开才能完成接下来的采样：

打开界面之后，如图所示，选择/aruco_ros/result话题，有可能会出现灰白界面没有图像，多重试几次，切换切换话题：

第三个界面可以完成采样，选取位姿：

最后按照界面的提示,next pose -> plan -> execute -> take sample -> ... -> compute:

可以在RViz中看到camera_link和camera_marker的坐标变换还是有错误的,我第一次完成的时候,还以为标定错了,但实际的坐标变换已经保存在~/.ros/easy_handeye下的yaml文件了,接下来通过在launch文件启动该坐标变换即可.

  <node pkg="tf" type="static_transform_publisher" name="ur5_realsense_calibration" args=" 0.854278519500625 1.209861820016321 0.19546056257158806 -0.11061812678943928 -0.040388645131856346 -0.8716081983804171 0.4758482277849228 base_link camera_link 100" />

结合https://blog.csdn.net/qq_34935373/article/details/104794634 就可以在RViz中启动Octomap: