ROS_Noetic使用handeye-calib进行机械臂手眼标定

本文使用Eye-In-Hand方式，对dofbot机械臂进行在线标定

参考：基于ROS的手眼标定程序解决方案|支持眼在手外/上_鱼香ROS的博客-CSDN博客_ros手眼标定

1.概览

本程序包通过输入两组以上的机械臂姿态信息和相机所识别的标志物的姿态信息，经过程序计算可输出，机械臂末端和相机之间（或机械臂基座和相机）的坐标变换矩阵。

2.程序下载与编译

cd ~/dofbot_ws/src
git clone https://gitee.com/ohhuo/handeye-calib.git
cd ..
catkin_make

pip install transforms3d
pip install tabulate

3.在线标定

3.1 配置相机与Aruco

3.1.1 配置相机

（1）安装USB相机驱动usb_cam

sudo apt-get install ros-noetic-usb-cam

（2）查看接入的USB摄像头的设备号

ls /dev/video*

（3）修改launch文件,设置USB摄像头的编号

roscd usb_cam
cd launch
sudo nano usb_cam-test.launch 

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0"/>
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

根据第二步查看的结果，修改<param name="video_device" value="/dev/video0" />的参数。

通过文件内容可知，该launch文件启动了两个节点，分别为 usb_cam 和 image_view；并在参数服务器上设置了一些参数，其中参数camera_frame_id 的值为 usb_cam；重命名的话题名称为 /usb_cam/image_raw

（4）测试摄像头

首先关闭机械臂大程序，解除对摄像头的占用

cd ~/Arm/
./kill_cam.sh

然后运行launch文件启动摄像头

roslaunch usb_cam usb_cam-test.launch

（5）查看相机通过 ROS 发布图像的话题名称

rostopic list

/image_view/output
/image_view/parameter_descriptions
/image_view/parameter_updates
/rosout
/rosout_agg
/usb_cam/camera_info
/usb_cam/image_raw
/usb_cam/image_raw/compressed
/usb_cam/image_raw/compressed/parameter_descriptions
/usb_cam/image_raw/compressed/parameter_updates
/usb_cam/image_raw/compressedDepth
/usb_cam/image_raw/compressedDepth/parameter_descriptions
/usb_cam/image_raw/compressedDepth/parameter_updates
/usb_cam/image_raw/theora
/usb_cam/image_raw/theora/parameter_descriptions
/usb_cam/image_raw/theora/parameter_updates

测试没问题以后 ctrl+c 关闭

3.1.2 配置aruco

（1）安装aruco

sudo apt-get install ros-noetic-aruco*

 失败的话换个镜像源，多试几次，实在不行用下面的方案：

cd ~/dofbot_ws/src
git clone https://github.com/pal-robotics/aruco_ros.git
cd ..
catkin_make

（2）前往 aruco marker 生成网站 打印 marker

注意：一定要选择original，其它版本的检测不出。本文选择的参数为：Original ArUco,50,100

（3）配置handeye-calib

进入 ~/dofbot_ws_raspi/src/handeye-calib

配置src/handeye-calib/launch/aruco/aruco_start_usb_cam.launch文件

<arg name="camera_info_url"  default="file:///home/dofbot/.ros/camera_info/head_camera.yaml"/>
    <arg name="video_device"     default="/dev/video0"/>
    <arg name="image_width"      default="640"/>
    <arg name="image_height"     default="480"/>

    <arg name="markerId"        default="50"/>
    <arg name="markerSize"      default="0.128"/>    
    <arg name="eye"             default="left"/>
    <arg name="marker_frame"    default="aruco_marker_frame"/>
    <arg name="ref_frame"       default=""/>  
    <arg name="corner_refinement" default="LINES" />

主要需修改参数文件如下:

•     相机内参畸变文件，完成相机标定你可以得到一个相机内参配置的yaml文件，请将该文件位置作为参数传入
•     设备video地址，默认/dev/video0可根据你的个人情况修改
•     markerId，你打印的id编号，在线打印地址可关注公众号鱼香ROS，后台回复标定板获取(打印时请选择origin类型)
•     markerSize，实际打印出来标定版的宽度，单位m

3.1.3 启动相机

配置完成后即可运行该launch文件

source devel/setup.bash
roslaunch handeye-calib aruco_start_usb_cam.launch

运行结果：

 程序运行之后如果检测到标定板的markerId为我们设置的id则会输出标定板在相机中的位姿数据，通过/aruco_signal/pose 话题输出，话题类型为PoseStamped。
可以使用下面的命令订阅输出话题数据。

打开终端
rostopic echo /aruco_signal/pose

话题数据打印结果：（移动前）

关于识别误差的校验方法

我们可以利用标记不动，移动机械臂的方法进行检测（假设机械臂是精准的）。由于机械臂与相机末端是固结的，所以当我们让机械臂的末端在空间中移动10mm,那相机在空间中也会移动10mm,同样相机中标记物的位置也应当移动10mm，我们可以通过示教器移动机械臂的方式，观察位姿数据的变化来测量精度。

将link1旋转了一个小角度后：

 

3.2 配置机械臂话题数据

机械臂位置和姿态从 tf树 中获取

眼在手上修改src/handeye-calib/launch/online/online_hand_on_eye_calib.launch文件

    <arg   name="arm_pose_topic"   default="/arm_pose" />
    <arg   name="camera_pose_topic"   default="/aruco_single/pose" />
    
    <arg   name="base_link"   default="/base_link" />
    <arg   name="end_link"   default="/link5" />

 仅需要修改以下参数：

• base_link,机械臂基坐标系名称
• end_link，机械臂末端坐标系名称

3.3 运行在线标定

本文使用ROS多机通讯方式实现机械臂位姿控制，主机为笔记本电脑，从机为树莓派，配置方式参考：（文章还没写，不过网上资料也很多）

（1）主机启动MoveIt

roslaunch dofbot_moveit dofbot_moveit.launch

（2）从机订阅机械臂关节角，并实时驱动机械臂运动

rosrun dofbot_moveit 00_dofbot_move.py

（3）从机运行在线标定程序

source devel/setup.bash
roslaunch handeye-calib online_hand_on_eye_calib.launch

程序运行时会对话题数据进行检测，检测是否收到机械臂数据和标定版位姿数据，如果没有会一直等待。当检测到已经接收到数据之后，就会出现命令提示，命令定义如下：

r  rocord    记录一组手眼数据（当记录的数据大于程序计算所需的数据量之后会进行自动计算）
c  calculate 计算当前数据
s  save      保存数据
p  print     打印当前数据到屏幕上（格式为 type,x,y,z,rx,ry,rz 角度制）
q  quit      退出标定程序

[INFO] [1635233747.563774]: 手眼标定需要两个位置和姿态，一个是机械臂末端的位姿，将从话题/arm_pose中获取 ,另一个相机中标定版的位置姿态将从话题/aruco_single/pose获取，所以请确保两个话题有数据
[INFO] [1635233748.568178]: 等待机械臂位置和姿态话题到位 ...
[INFO] [1635233749.570482]: 等待机械臂位置和姿态话题到位 ...
指令: r 记录,c 计算,s  保存,q  退出:

 （4）查看计算结果

[INFO] [1651042798.015369]: Save result to  /tmp/online_handeye_result_2022_04_27_14_59_58.txt

(5)标定结果正确与否的测试

观察数据计算结果的标准差大小。每次计算之后，程序都会输出不同算法下标定结果点的平均数、方差、标准差三项数值。

眼在手上标定结果验证：由于标定过程中标定板是没有发生移动的，所以我们通过机械臂的末端位置、标定结果（手眼矩阵）、标记物在相机中的位姿即可计算出标定板在机器人基坐标系下的位姿，如果标定结果准确该位姿应该是没有变化的。所以可以比较最终数据的波动情况来判定标定结果的好坏。