手眼标定-基于apriltags+UR5上眼在手外

参考网址：
（机器人手眼标定解读）https://blog.csdn.net/qq_16481211/article/details/79764730
（机器人运动学解读）https://blog.csdn.net/qq_16481211/article/details/79749282
（相机标定参数解读） https://blog.csdn.net/qq_16481211/article/details/79464786
（相机内部坐标系解读）https://blog.csdn.net/lql0716/article/details/71973318
（收录大量相关资料）https://xgyopen.github.io/2018/08/16/2018-08-16-imv-calibration-eye-hand/
（坐标变换的解读）https://wenku.baidu.com/view/8bba76d371fe910ef02df821.html
（一篇很好的手眼标定文章）https://blog.csdn.net/Xiongchao99/article/details/52850990

相机标定：

之前我们分析过matlab相机下的标定，主要可以得到相机的内参（相机本身固有），以及相机相对于世界坐标系的外参，这里的世界坐标系就是标定板上来确定的。在matlab上标定过程会出现x，y的显示，通过右手螺旋可以确定Z。如下图1,而在opencv上可通过图2确定
图 1：

来源链接：https://blog.csdn.net/shenxiaolu1984/article/details/50165635
图2：

手眼标定

通过上述分析，我们已经明确了相机的内参和外参，并且可以通过开始分享的链接了解其理论背景，那么问题来了，针对于实际的手眼标定过程，我们该如何进行呢？下面是自己构想并实施的一种思路。

首先，手眼标定的目的就是为了获得相机与机械手末端的坐标转换，那麽如果我们可以知道相机在机械手基坐标下的位姿，然后通过机械手正运动学的方式获取到机械手末端相对于机械手基坐标下的位姿，那麽便可以得到相机相对于机械手末端的位姿。
AprilTags在概念上类似于QR码，因为它们是一种二维条形码。 但是，它们被设计用于编码更小的数据有效载荷（4到12位之间），从而可以更长距离地检测它们。 此外，它们设计用于高定位精度 - 可以计算AprilTag相对于相机的精确3D位置。正是利用这种特性，我们选择把apriltags固定在机械臂末端上，那麽我们便可以得到相机相对于机械臂末端的位姿，然后通过正运动学得到机械臂末端在机械臂基座标下的位姿，这样我们便可以求得相机相对于机械臂基坐标的位姿。
注意事项：由于我们目前的项目只是想要UR5末端到达相机目标检测位置，所以并未加入手抓进行标定，只是标定了UR5末端与相机的位姿变换。

实际操作图如下，是一种眼在手外的模型，这里的tag位姿要保证和机械臂末端位姿一致：

简化后的系统模型：

1,TB求取
这里UR5在ROS上建立模型后，连接上实体机械臂会将每个关节的变换发布到TF树中。

注意事项：在ros中，我们通过设置Robot_Base看成world，所以上面两者的坐标是一致的。

具体代码部分：在连接实际UR5的.launch文件下，我的是myrobot2_bringup_ros_control.launch下，有

这样我们便可以直接获得TB，如下，找到发布的TF话题，可以找到，这里robot_base到tool0_controller的变换旋转四元数和平移向量，但这里的tool0_controller和我们实际的tag位置有差别，这一块有待改进，另一种思路是我们得到每个相邻关节的变换一步一步的从robot_base推演到ee_link(机械臂末端)。

 transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1562138718
        nsecs: 175382578
      frame_id: "base"
    child_frame_id: "tool0_controller"
    transform: 
      translation: 
        x: -0.00679725721916
        y: 0.779366792331
        z: 0.749814791947
      rotation: 
        x: -0.457573780798
        y: -0.112201201201
        z: -0.361785957808
        w: 0.804455123863

2,TA部分的求取
这里我们在ros上运行apriltags识别的程序，便会发布apriltags与camera的相对变换矩阵，我们可以直接查看此话题，找到两者的坐标变换。

也可以订阅此话题发布到tf树中，查看，这样可以在tf树中做变换找到TZ，但这里自己未尝试。

在我们得到TA和TB后，我们可以通过计算得出这里的TZ。这里只是提供了一种标定思路，其实这里存在很多缺陷，比如：单纯采集一个位姿且依赖apriltags算法的精度，具体的精度问题，还需要在后面的项目实践中进行验证。

---

7.7号更新

下午实验了一下，发现当布置成相机，apriltags（机械臂末端），机械臂基坐标在一个平面下，可以减少旋转量的计算，根据项目可以这样布置。如下图：

在rviz下，如下图所示

保证在一个平面的情况下，这样world与camera的旋转部分用欧拉角表示，rpy=（3.1415926,0,0）;也即绕x轴顺时针180度。这样我们只需要判断相机相对于机械臂末端的x，y，z平移量。
从tf树话题下找到了机械臂末端到tool0_controller的变换，以及camera到tool0的坐标变换，这里tool0_controller与tool0并不同，在z轴上有一定位移量，因为tool0_controller有位置发布者发布，猜测是ur_hardware发布。这里我把两者先看成一致，再在z轴上作调整。

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1562479328
        nsecs: 998094598
      frame_id: "simple_arm_base"
    child_frame_id: "simple_arm_tool0_controller"
    transform: 
      translation: 
        x: -0.111214832382
        y: 0.662424167298
        z: 0.57255140631
      rotation: 
        x: -9.88806525602e-05
        y: -0.0470761030224
        z: -0.998891237283
        w: 0.000356127304733


        frame_id: "usb_cam"
      pose: 
        pose: 
          position: 
            x: 0.148372258216
            y: -0.194709882548
            z: 0.585047242365
          orientation: 
            x: 0.997158322351
            y: -0.0491579432068
            z: 0.010261372842
            w: 0.0561558635635

计算过程：

验证：
在world坐标系下，相机的位姿：


发现在y轴上偏差大，于是进行微调




此时偏差在y轴上小了很多，现在看在z轴上偏差，这是我之前将simple_arm_tool0_controller与simple_arm_tool0看成一直和相机精度造成的，于是我们再在z轴上进行微调，


发现此时已经和实际的布局接近了，从而实现了对相机相对于世界坐标系的标定。