节卡MinoCobo机器人单目视觉抓取的ROS实现

最近做了一个机器人视觉抓取的项目,目标是让机器人准确地拿到桌子上摆放的咖啡,递给旁边的人。目前的实现结果是能够抓取到一定区域内任意摆放的纸杯,水平方向定位精度在20mm以内,竖直方向定位精度较差,会有20~35mm的误差,原因是竖直方向对应着相机的深度方向。

  • 硬件:MiniCobo协作机器人、单目相机、大寰夹爪PGC-50、电脑
  • 技术方案:ROS下的AprilTag视觉定位、机器人API接口使用、手眼标定

下面总结一下项目中一些代码使用和bug解决,大部分代码都是已经开源的方案,不过用的时候总会出现各式各样的问题,都是靠着其他人的博客和代码库的issue才解决的。

1. ApirlTag定位

1.1 参数设置

AprilTag有专门写了一个ROS版本的代码(https://github.com/AprilRobotics/apriltag_ros),跑AprilTag代码之前得先装好相机驱动,并且做好相机标定。
启动节点前要先设置一下apirltag_ros/apriltag_ros/config目录下的tags.yaml和settings.yaml。
apirltag_ros/apriltag_ros/config/settings.yaml

tag_family:        'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
tag_threads:       2          # default: 2
tag_decimate:      1.0        # default: 1.0

settings.yaml得注意tag_family和你打印的tag一致,一般好像都用36h11。
apirltag_ros/apriltag_ros/config/tags.yaml

standalone_tags:
  [
    # {id: 1, size: 0.058},   #size对应标签的大小
    # {id: 0, size: 0.048},
    # {id: 0, size: 0.04},
    # {id: 1, size: 0.04}, 
    # {id: 2, size: 0.04},
    # {id: 3, size: 0.04}, 
    # {id: 4, size: 0.04}
  ]
  tag_bundles:
  [
    {
      name: 'my_bundle',
      layout:
        [
          {id: 0, size: 0.044, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 4, size: 0.044, x: 0.0300, y: -0.0830, z:  0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 3, size: 0.044, x: -0.0300, y: -0.0830, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 2, size: 0.044, x: 0.0300, y: 0.0830, z:  0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 1, size: 0.044, x: -0.0300, y: 0.0830, z:  0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}
        ]
    }
  ]

tags.yaml的话就得注意standalone_tags和tag_bundles的区别。

  • 如果你只用一个tag来定位,就设置前面的standalone_tags,只需要将id改为你打印的tag的id,size和实际大小一致,单位m;
  • 如果你用多个tag同时定位,目的应该是以为可以提高精度(不知道能不能,我没做实验),就设置后面的tag_bundles,我这里用了5个tag同时定位。第一个参数是主定位的二维码,最后识别输出的位置也就是第一个id的位置。它的xyz、四元数都是没有偏差(注意qw=1)。其他tag是用来辅助定位的,需要知道它们与主定位的id0之间的偏差,所以xyz和四元数都得是实际的偏差值。这里我是用直尺测量的tag之间的距离,因为都在一个平面上,所以四元数就是无旋转对应的数值。

1.2 usb相机启动

我源码安装了usb_cam驱动包,不过源码也没什么用,没看也没改,整个项目就只用到了usb_cam-test.launch一个文件。
usb_cam/launch/usb_cam-test.launch

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="camera_info_url" type="string" value="file:///home/jeremy/.ros/camera_info/head_camera.yaml"/>
    <param name="video_device" value="/dev/video2" />
    <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>

第3行代码camera_info_url是加上了标定后相机参数的位置,源码应该没有这一句,我是看到手眼标定里面有个启动文件这样写了,就加上去了。(其实单目相机标定的作用我也没看出来,删掉这句我观察输出图像也没看到啥变化。但为了心理安慰,还是别删,要不然我精度又没了… …)
还有第4行的video_device,如果启动launch文件后发现打开的是笔记本自带相机(突然出现自己的脸,吓一跳),就把video2改成video0,或者video0改成video2,反正就是换一下。不过别改成video1,会说找不到的。

1.3 报错

  1. [ WARN] [1639733001.211902775]: Requested description of standalone tag ID [0], but no description was found...
    这个问题是因为tags.yaml中没有正确设置二维码id,要注意id必须和打印出来二维码一致,还有就是单个tag设置standalone_tags,多个就设置后面的tag_bundles。

  2. 启动相机后发现图像黑屏
    遇到这个问题我本来上网找半天,按照其他博主的方法改各种.launch文件中的参数,但是都没用,最后哭笑不得地发现是单目相机的光圈调到最小了,调大光圈就出现图像了,亏得我还linux和window下反复测试相机能不能用。

  3. 输出位置信息都是0
    未进行相机标定,相机参数没有保存至.ros/camera_info目录下。

2. JAKA机器人API使用

JAKA有官方的API二次开发文档,python的文档写得详细一点,不过我的机器人控制代码用的C++写的,所以开发的时候我python和C++的API都用了一遍。一般用python先试一下,成功了再换成C++。

2.1 python二次开发

Linux平台下使用机器人API的话,需要将JAKA给的libjakaAPI.so, jkrc.so两个编译好的库放到和python代码相同的文件夹,运行代码前还需要代码文件夹路径添加到环境变量中,就是在终端中运行下面这行代码。我的代码就放在grab/script路径下。

export LD_LIBRARY_PATH=~/project/jaka/SendCoffee/src/grab/script

还有一点需要注意的是,节卡机器人的API使用环境是python3.5,如果你的linux默认python环境是2.7的话,可以用python3 ***.py的命令来运行代码。我python3的环境是3.6.9,与官方要求的3.5不一致,但是目前运行也没遇到问题。

2.2 C++二次开发

C++要编译,所以设置会比python麻烦,我是通过参考JAKA给的文件中的linux示例代码才编译成功的。具体步骤是先在grab功能包建立grab/include文件夹,放入JAKAZuRobot.h, jkerr.h, jktypes.h3个头文件。接着建立grab/lib文件夹,放入编译好的C++静态库libjakaAPI.a。最后只需要正确编写Cmake编译文件,就可以用catkin_make命令编译。哦对了,还得将你要运行的C++代码放到grab/src文件夹中。
grab/CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(grab)
find_package(catkin REQUIRED COMPONENTS 
   roscpp  
   rospy  
   std_msgs  
   apriltag_ros 
)
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES test
#  CATKIN_DEPENDS roscpp
#  DEPENDS system_lib
)
include_directories(
  ${catkin_INCLUDE_DIRS}
)
include_directories( "/usr/include/eigen3" )

# include 头文件目录
include_directories(include)
LINK_DIRECTORIES($(SRC_ROOT_PATH))
LINK_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/lib)
LINK_LIBRARIES(pthread)
 
# 编译成可执行文件,如果编译动态库可以用:target_link_libraries
add_executable(control_robot src/control_robot.cpp)
target_link_libraries(control_robot jakaAPI)
target_link_libraries(control_robot  ${catkin_LIBRARIES})

find_package(Threads)
target_link_libraries (control_robot ${CMAKE_THREAD_LIBS_INIT})

3. 机器人手眼标定(眼在手外)

眼在手外的手眼标定是为了求解相机相对于机器人底部的位姿,在本项目中,AprilTag定位能够得到tag在相机坐标系下的位姿,而抓取目标与tag的相对位置是固定的。因此,完成手眼标定后,即可实现抓取目标在tag坐标系–>相机坐标系–>机器人坐标系的位姿转换。
求解得到抓取目标在机器人坐标系下的位置后,即可控制机器人运动至目标点,进行抓取。

3.1 标定方法

我用的是鱼香ROS写的handeye-calib包(https://gitee.com/ohhuo/handeye-calib),代码给出了操作步骤的详细介绍,我是跟着在线标定的流程实现的。handeye-calib其实与easy_handeye功能包的原理一样,都是调用OpenCV的cv2.calibrateHandEye()函数完成手眼标定。easy_handeye有一个更好的可视化界面,但是相应的代码也更难读懂。
很多手眼标定博客都是用moveit驱动机器人,通过tf树来获取机器人位姿的,但是我不知道怎样用moveit,所以我决定直接调用JAKA SDK来获取机器人位姿,并且通过ros的geometry_msgs::PoseStamped消息格式发布话题/robot_pos。这样标定程序就能直接订阅机器人位姿话题,下面是修改后的手眼标定launch文件。
handeye-calib/launch/online/online_hand_to_eye_calib.launch

<launch>
    
    <arg   name="camera_pose_topic"   default="/aruco_single/pose" />
    
    <arg   name="arm_pose_topic"   default="/robot_pos" />

    <node pkg="handeye-calib" type="online_hand_to_eye_calib.py" name="online_hand_to_eye_calib" output="screen" >
         <param name="arm_pose_topic" value="$(arg arm_pose_topic)" />
         <param name="camera_pose_topic" value="$(arg camera_pose_topic)" />
    node>
launch>

3.2 问题

  1. no moudle named 'rospkg'
    将handeye-calib/src/handeye/online_hand_ro_eye_calib.py的第一句#!/usr/bin/env python3中的python3改为python,原因是ros melodic版本只支持python2,所以python3的代码无法使用ros包。
  2. no moudle named 'transforms3d'
    通过pip install transforms3d安装一下该库,如果没有pip,请先安装pip,sudo apt-get install python-pip。如果手眼标定代码出现其他库找不到的错误,也可以先尝试用pip安装,另外网上搜索一下报错信息会找到很多安装库的教程。不过需要注意库需要安装到python2还是python3,python2就用pip,python3用pip3。
  3. 'module' object has no attribute 'CALIB_HAND_EYE_TSAI'
    参考博主miss_future的kinova与intel realsense D435手眼标定中的错误2。
  4. super().__init() Error: super() takes at least 1 argument(0 given)
    删除super().__init(),这一句代码是用来继承父类的初始化函数的,作者这里可能写错了。
  5. 标定结果精度很差
    标定过程不应该大范围移动机器人,不同组记录之间的末端旋转角度要求比较大。提高标定精度的方法可参考博主Yoyo_wym的手眼标定(二): Tsai求解方法,我采用博主提到的措施后,标定结果的精度在10mm范围内。

收藏网址

  • 相机标定棋盘格在线生成:https://calib.io/pages/camera-calibration-pattern-generator
  • OpenMV生成ApirlTag.png图片 https://blog.csdn.net/u013952812/article/details/106766132
  • ArUco markers在线生成:https://chev.me/arucogen/
  • 旋转矩阵 欧拉角 四元数的在线转换 https://www.andre-gaschler.com/rotationconverter/

你可能感兴趣的:(笔记,计算机视觉,人工智能)