实际的机械臂控制（7）USB相机与小强机械臂Lobot机械臂的标定方法

引言

本文是针对一个实际的机械臂来控制的，而且这个机械臂很便宜，即使是平民玩家也玩得起。
在前面两章，我们介绍了对机械手臂的开环控制，主要是让初学者明白一些简单的东西。
第一章，根据小强给的少的可怜的教程简单的驱动机械手臂。
第二章，自己写一个publisher和一个launch文件，实现对动作的发布和执行，让机械手臂不停地动起来。

下面，我们将进入实际的人工智能模式，利用opencv PCL等工具实现对机械手臂的控制。

一、相关工作

1. 这个手眼标定是相机在机械手臂上的，但是他介绍了很多理论知识，我们在这部分的理论知识也是照搬他们了，。
2. 这里有更多的参考，反正在准备之前需要大量阅读资料，想吃现成的基本不可能。
3. 这个是关于Halcon标定的教学，就4分钟，英文的，但是说的很慢，可以听得懂，主要是人家有字母。

二、数学原理

手眼标定实际上是求解矩阵方程：AX = XB ;
A是摄像机（单目或双目）前后两次空间变换的齐次矩阵 ； B是机械臂末端坐标系前后两次变换的齐次矩阵 ； X为待求解的手眼矩阵；通过多次求解该方程，即可解出X（A 、B矩阵求法如下）

三、基于easy_handeye的相机标定的准备工作

aruco_ros安装

安装这个是为了识别ArUco marker

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

vision_visp / visp_hand2eye_calibration安装

cd ~/catkin_ws
sudo apt-get install ros-melodic-visp
cd src
git clone -b melodic-devel https://github.com/lagadic/vision_visp.git
cd ..
catkin_make --pkg visp_hand2eye_calibration

easy_handeye安装

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

打印ArUco marker

在线生成地址：https://chev.me/arucogen/
注意，一定要在Dictionary中选择而Original Arcuo

然后找个打印机按照原尺寸打印下来，注意，打印下来要自己用尺寸量一下尺寸，后面有用。
同样，把这个码贴在一个硬纸板上。

四、基于easy_handeye的相机标定的准备工作

首先看看进行 手眼标定的机械臂的启动文件

<?xml version="1.0"?>
<launch>
<!-- start action server node  -->
  <node pkg="probot_grasp" name="qidong_robot" type="myrobot_driver" />
<!-- start robot model and  rviz -->
  <include file="$(find lobot_arm_moveit_config)/launch/demo.launch"/>
<!-- srart the USB Serial port -->
  <node pkg="rosserial_python" name="qidong_chuankou" type="serial_node.py" args="/dev/ttyUSB0"/>

<!-- start USB camera -->
   
   <!-- start the calibration publish-->

</launch>

写一个luanch文件，直接启动所有程序

文件名：calib_usb_lot.launch

<?xml version="1.0"?>
<launch>
    <arg name="namespace_prefix" default="lobot_arm_usb_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" default="0.1"/>
    <arg name="marker_id" doc="The ID of the ArUco marker used" default="100"/>
    <arg name="eye" default="left"/> 

    <!-- start the usb camera -->
    <!--include file="$(find freenect_launch)/launch/freenect.launch" >
        <arg name="depth_registration" value="true" />
    </include-->
    <include file="$(find detection_color)/launch/usb_cam.launch" />

    <!-- start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <remap from="/camera_info" to="/usb_cam/camera_info" />
        <remap from="/image" to="/usb_cam/image_raw" />
        <param name="image_is_rectified" value="true"/>
        <param name="marker_size"        value="0.0967"/>
        <param name="marker_id"          value="200"/>

        <param name="reference_frame"    value="camera_link"/>
        <param name="camera_frame"       value="camera_link"/>

        <param name="marker_frame"       value="camera_marker" />
    </node>

    <!-- start the robot -->
    <include file="$(find probot_grasp)/launch/startup_robot.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="link5" />

        <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>

我们采集的二维码的ID是200，尺寸大小是9.6厘米。

        <param name="marker_size"        value="0.0967"/>
        <param name="marker_id"          value="200"/>

说明一点

在采集数据是，可以自动控制机械臂的位姿，也可以自己手动调节位姿。
由于我的程序无法自动调节机械臂位姿，也就是没法用Next pose。所以我在rviz中自己调节了。
数据采集最少17个点，我的原则是越多越好。

启动标定程序

roslaunch probot_grasp calib_usb_lot.launch

启动之后，除了rviz之外，还有两个窗口，如下：

正常来讲，在那个大的窗口上是可以打开
/aruco_tracker/result的，从而显示识别二维码的窗口。但是我这个没有，因此我们需要手动打开。
在工作空间下打开一个终端，输入：

rosrun image_view image_view image:=/aruco_tracker/result

然后就出现如下窗口了

首先，对于这个窗口，我没法使用next pose。因此我就不操作这个窗口了。

我们主要在在rviz下面，通过这个选择窗口，控制机械臂位姿，从容选择合适的位姿。

在选择合适的位姿后，合适的位姿就是摄像头可以识别出二维码ID。
然后点击take sample
点击一次，移动一次机械臂

采集第一个样本是这样的

然后依次移动机械臂运动，在点击take sample

我最终采集了35个位姿

然后点击compute

右侧计算出了最终的坐标转换关系。
然后点击save
这个时候，我们到终端看一下，可以找到转换矩阵的保存位置

这个时候，我们需要到home文件夹下面的,ros下面去寻找文件。由于.ros是隐藏文件，因此我们需要按Ctrl+H显示隐藏文件。

进入.ros下面
找到lobot_arm_usb_handeyecalibration_eye_on_base.yaml文件

这时，我们在机械臂的启动文件里添加启动摄像头和发布标定信息的节点
修改后的相机启动文件startup_robot.launch如下：

<?xml version="1.0"?>
<launch>
<!-- start action server node  -->
  <node pkg="probot_grasp" name="qidong_robot" type="myrobot_driver" />
<!-- start robot model and  rviz -->
  <include file="$(find lobot_arm_moveit_config)/launch/demo.launch"/>
<!-- srart the USB Serial port -->
  <node pkg="rosserial_python" name="qidong_chuankou" type="serial_node.py" args="/dev/ttyUSB0"/>

<!-- start USB camera -->
   <include file="$(find detection_color)/launch/usb_cam.launch" />

   <!-- start the calibration publish-->
 	<node name="link1_broadcaster" pkg="tf" type="static_transform_publisher" args="-0.0038246100436423597 -0.0023318449115560143 0.13466109184238712 0.9882889926593997 -0.09317833339710963 0.0527325627198183 -0.10872875425850478 base_link camera_link 100"/>

</launch>

至此，相机外参标定成功