让机械臂自动进行手眼标定---以Dobot Magician和Realsense为例（附示例代码）
你需要看这篇文章吗？
这篇文章描述的是如何自动化手眼标定（眼在手外，eye-to-hand）过程，以建立机械臂坐标系和摄像头坐标系之间的转换关系。示例的硬件环境是Dobot Magician 机械手、Intel RealSense D415 深度摄像头。如果您是想了解相机内外参数标定，请移步文章末尾参考文献 ，那里有一些博主认为不错的教程。
原文链接：https://www.three.ml/2018/08/hand-eye-calibration
如需转载，请勿删除。
前言
博主最近想用机械臂做一些视觉相关的东西，由于之前没有接触过相关的知识，加上网上很多相关的博客和知乎回答都说的不够直白，所以想现学现卖，写一篇Tutorial，供需要进行相关工作的同学参考。如果文中有错误或者概念混淆，希望大家留言指出。相关的代码在Github上供大家参考。
首先，我们用一张图来描述具体的场景：
我们的任务是让机械臂能根据摄像头捕捉到的图像，精准地夹起桌面上的小木块。首先，我们可以通过机械臂API（或者运动学方程）获得当前机械臂末端在机械臂坐标系下的位置。同时，我们也可以获得机械臂末端、小木块在深度摄像头捕获图像中的坐标以及深度信息。为了让机械手知道小木块的位置，并运行到小木块的上方进行夹取，我们需要建立起摄像头图像坐标系到机械臂坐标系的转换关系。
什么是标定？
相机标定
照例说一下什么是标定，简单的说，标定就是获得不同坐标系之间的转换关系。相机的标定分为内参和外参标定。相机内参，顾名思义描述相机的内部属性，如相机的焦距、像素点大小、相机畸变属性，这里就不详细叙述了。外参描述相机的空间位置，也就是相机坐标相对于（人为规定的）世界坐标系的旋转和平移。通过内参和外参，我们可以准确地从相机捕获的图像的坐标转换到外部世界的坐标。当然，因为RGB图像是二维的，世界坐标是三维的，我们需要再加入图像的深度信息，深度信息可以通过深度摄像头或者一些三维重建方法获得，笔者对这个不是很了解，OpenCV似乎有相关方法，熟悉的朋友可以发信或留言告诉我。另外，需要指出的是，对于需要转换的点均在世界中的一个二维平面，这个问题就退化为二维到二维的转换，不需要深度信息。
手眼标定
手眼标定可以理解为相机标定+机械臂标定。机械臂标定就是把机械臂坐标系转换为世界坐标系，相机标定已经把图像坐标系转换到了摄像头坐标系，再转换到了世界坐标系，因此机械臂坐标系到图像坐标系之间的转换关系我们也可以确定了。在实际的标定过程中，我们可以忽略到世界坐标系转换这个步骤，直接将摄像头坐标系转换到机械臂坐标系。如果不清楚各个坐标系，可以参考这篇博文。另外，这篇博文将机器人手眼标定这一过程比作智障儿童抓玩具，讲解的比较清晰。
方法
由于笔者使用的是RealSense D415深度摄像头，能直接利用Intel的Realsense SDK 2.0来将图像中的点还原到摄像头坐标系下的三维坐标，标定的过程一下就简化了下来。因为SDK直接能利用相机的内参和深度信息，将图像坐标系下的点还原到相机坐标系下。因此，只需要将机械臂坐标系和相机坐标系之间的转换关系求得即可。如果使用的是其他摄像头，需要自己根据相机的内参将图像坐标系转换到相机坐标系，具体的转换方法可参考可以参考这篇博文。
关于机械臂控制，不同型号的机械臂应该有不同的坐标系设置。以Dobot Magician为例，这款机械臂提供笛卡尔坐标系，该坐标系规定机械臂的地盘中心为坐标的原点，并且末端的位置直接可以通过机械臂提供的API读出，如图所示：
下一步，我们需要的是获取机械臂末端在图像坐标系下的坐标。为了能让标定过程自动化，我们需要机器人能在图像中自动找到机械手的末端坐标。在这里，我们使用了ArUco标签，ArUco是科尔多瓦大学人工视觉应用小组（A.V.A）设计开发的一个开源增强现实库，OpenCV里也内置有关于ArUco标签的识别方法，我们通过对机械臂末端粘贴ArUco标签，并在捕获的图像中识别出标签，并计算标签的中心点，来获得机械臂末端的坐标，具体实现步骤会在下一小节中详解。
到目前，我们已经可以获得机械臂末端在图像坐标系中的坐标和在机械臂坐标系中的坐标，只需通过一个旋转矩阵 R R R和平移矩阵 T T T，即可获得两个坐标系之间的转换关系。如下式：
P a r m = R ⋅ P i m a g e + T P_{arm}= R\cdot P_{image} + T Parm=R⋅Pimage+T
[ x a r m 1 x a r m 2 . . . x a r m n y a r m 1 y a r m 2 . . . y a r m n z a r m 1 z a r m 2 . . . z a r m n ] = R ⋅ [ x i m a g e 1 x i m a g e 2 . . . x i m a g e n y i m a g e 1 y i m a g e 2 . . . y i m a g e n z i m a g e 1 z i m a g e 2 . . . z i m a g e n ] + T \begin{bmatrix} x_{arm_1} & x_{arm_2} & ...& x_{arm_n} \\ y_{arm_1} & y_{arm_2} & ...& y_{arm_n} \\ z_{arm_1} & z_{arm_2} & ...& z_{arm_n} \end{bmatrix} = R\cdot \begin{bmatrix} x_{image_1} & x_{image_2} & ...& x_{image_n} \\ y_{image_1} & y_{image_2} & ... & y_{image_n} \\ z_{image_1} & z_{image_2} & ... & z_{image_n} \end{bmatrix} + T ⎣⎡xarm1yarm1zarm1xarm2yarm2zarm2.........xarmnyarmnzarmn⎦⎤=R⋅⎣⎡ximage1yimage1zimage1ximage2yimage2zimage2.........ximagenyimagenzimagen⎦⎤+T
将R和T矩阵合并，得到下式：
[ x a r m 1 x a r m 2 . . . x a r m n y a r m 1 y a r m 2 . . . y a r m n z a r m 1 z a r m 2 . . . z a r m n ] = [ R T ] ⋅ [ x i m a g e 1 x i m a g e 2 . . . x i m a g e n y i m a g e 1 y i m a g e 2 . . . y i m a g e n z i m a g e 1 z i m a g e 2 . . . z i m a g e n 1 1 . . . 1 ] \begin{bmatrix} x_{arm_1} & x_{arm_2} & ...& x_{arm_n} \\ y_{arm_1} & y_{arm_2} & ...& y_{arm_n} \\ z_{arm_1} & z_{arm_2} & ...& z_{arm_n} \end{bmatrix} = \left [ \begin{array}{c:c} \begin{matrix} R \end{matrix}& \begin{matrix} T \end{matrix} \end{array} \right ] \cdot \begin{bmatrix} x_{image_1} & x_{image_2} & ...& x_{image_n} \\ y_{image_1} & y_{image_2} & ... & y_{image_n} \\ z_{image_1} & z_{image_2} & ... & z_{image_n} \\ 1&1&...&1 \end{bmatrix} ⎣⎡xarm1yarm1zarm1xarm2yarm2zarm2.........xarmnyarmnzarmn⎦⎤=[RT]⋅⎣⎢⎢⎡ximage1yimage1zimage11ximage2yimage2zimage21............ximagenyimagenzimagen1⎦⎥⎥⎤
剩下的任务就是求图像坐标到机械臂坐标的转换关系$ C_{itoa} = \left [
\begin{array}{c:c}
\begin{matrix}
R
\end{matrix}&
\begin{matrix}
T
\end{matrix}
\end{array}
\right ]$，为一个 4 ∗ 4 4* 4 4∗4的矩阵，可以通过等式左边点乘 P i m a g e − 1 {P_{image}}^{-1} Pimage−1而得到，即： C i t o a = P a r m ⋅ P i m a g e − 1 C_{itoa} = P_{arm} \cdot {P_{image}}^{-1} Citoa=Parm⋅Pimage−1。
同理， C a t o i = P i m a g e ⋅ P a r m − 1 C_{atoi} = P_{image} \cdot {P_{arm}}^{-1} Catoi=Pimage⋅Parm−1。
实现详解
我们先来看一下想要达到的效果：
[Video]
找出图像中的Marker
关于RealSense SDK中的Python Wrapper的pipeline写法，官方有写简单的例程，在这里就不详述。
我们需要实现的是从摄像头捕获的图像中实时找出ArUco标签，ArUco标签的生成可以使用这个线上生成网站，在线生成用A4纸打印出来再自行裁剪就可以了，由于程序里面不检测Marker具体的编号，所以ID可以在范围内随便填写。如图，使用cv2.aruco下的detectMarkers()函数，检测出了ID为100的ArUco标签，并使用drawDetectedMarkers()函数，在标签周围画了绿色框。这时，我们可以获得正方形Marker四个顶点在图像上的坐标，使用np.average()对四个顶点坐标求平均值，我们就获得了Marker中心点的坐标。我们继续使用get_distance()函数来获得该点对应的深度信息。由于Intel RealSense SDK 2.0的文档和示例不是很全面，笔者通过查阅Python Wrapper的代码加上Github上搜索，才搞清楚了RealSense deproject的用法，代码如下：
# get intrinsic of color image
color_intrinsics = color_frame.profile.as_video_stream_profile().intrinsics
if len(corners)!=0:
point = np.average(corners[0][0], axis=0)
depth = depth_frame.get_distance(point[0], point[1])
point = np.append(point,depth)
if depth!=0:
global center
x=point[0]
y=point[1]
z=point[2]
## see rs2 document:
## https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0#point-coordinates
## and example: https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0#point-coordinates
x,y,z=rs.rs2_deproject_pixel_to_point(color_intrinsics, [x, y], z)
center=[x,y,z]
color_image = aruco.drawDetectedMarkers(color_image, corners)
####控制机械臂运动到标定点
标定点选择
首先，关于标定点的个数，当然是越多越好，因为根据上述方法中的等式我们知道，求解旋转平移矩阵的过程涉及到矩阵的求伪逆，点的个数越多，误差造成的影响越小。并且，标定点应尽量大地覆盖机械臂末端的整个运动空间，笔者使用了机械臂工作空间中的一个矩形的8个顶点作为标定点，实际效果不错。注意，下面代码中标定点的第四维是末端夹具的旋转角度，我们设置为0，这样保证了Marker始终朝向摄像头。根据前文等式3，我们对标定点矩阵加上一行1。
# Calibration points
default_cali_points = [[180,-120,135,0],[270,-120,135,0],
[180,120,135,0],[270,120,135,0],
[270,120,-5,0],[180,120,-5,0],
[180,-120,-5,0],[270,-120,-5,0]]
np_cali_points = np.array(default_cali_points)
arm_cord = np.column_stack((np_cali_points[:,0:3],np.ones(np_cali_points.shape[0]).T)).T
机械臂控制
关于机械臂控制，值得注意的地方是，最好在标定前对机械臂进行复位操作，这样做的理由是保证机械臂的坐标准确。以Dobot Magician为例，有时机械臂运动受到物体遮挡，机械臂坐标系会发生少量偏移，造成标定时的误差。由于不同型号的机械臂的控制差异较大，这里就不对具体步骤进行说明，有兴趣的同学可以查阅代码。
计算转换关系
我们得到两个转换矩阵：
image_to_arm = np.dot(arm_cord, np.linalg.pinv(centers))
arm_to_image = np.linalg.pinv(image_to_arm)
Sanity Test
为了验证我们得到的矩阵的正确性，我们可以将预设的标定点和从图像中复原的标定点进行对比。以下是笔者一次标定的测试数据，：
Sanity Test:
-------------------
Image_to_Arm
-------------------
Expected: [180, -120, 135]
Result: [ 180.45854171 -118.05179826 134.8207605 ]
Expected: [270, -120, 135]
Result: [ 269.0604216 -121.47545384 134.57466783]
Expected: [180, 120, 135]
Result: [179.87300481 118.17882766 135.12789621]
Expected: [270, 120, 135]
Result: [270.60174502 121.34495196 135.4708232 ]
Expected: [270, 120, -5]
Result: [270.2242233 121.52053742 -5.31904548]
Expected: [180, 120, -5]
Result: [179.28116379 118.88137143 -5.28169954]
Expected: [180, -120, -5]
Result: [ 180.41191063 -118.9554326 -4.6571776 ]
Expected: [270, -120, -5]
Result: [ 270.08898915 -121.44300379 -4.73622512]
-------------------
Arm_to_Image
-------------------
Expected: [-0.08698806 -0.05967812 0.46800002]
Result: [-0.08882234 -0.06007397 0.46825837]
Expected: [-0.08947706 -0.01211117 0.39100003]
Result: [-0.0880777 -0.0119517 0.38999574]
Expected: [ 0.13517193 -0.05967413 0.47400004]
Result: [ 0.13688449 -0.05949768 0.47400009]
Expected: [ 0.13890341 -0.01143393 0.39500001]
Result: [ 0.13762914 -0.0113754 0.39573746]
Expected: [0.1377386 0.10304449 0.46900001]
Result: [0.13630977 0.10266108 0.46899168]
Expected: [0.13450451 0.05438121 0.54800004]
Result: [0.13556512 0.0545388 0.54725431]
Expected: [-0.08915272 0.05390601 0.54100001]
Result: [-0.09014171 0.0539625 0.54151259]
Expected: [-0.09075091 0.10191404 0.46300003]
Result: [-0.08939707 0.10208478 0.46324995]
我们可以看到，标定的误差已经比较小了，误差的来源有：1. 机械臂的机械误差 2. Marker识别的误差。
小结
本文简述了机械臂手眼标定的过程，由于使用了RealSense深度摄像头，手眼标定就不需对摄像头内参进行标定。希望能给从事相关研究、项目的同学给予帮助。笔者在这领域也是新手，如果文中有错误，请指出。谢谢！
参考文献&相关工具
- 本文Github Repo
- [Aruco Marker 在线生成器](http://keystone.umd.edu/html/markergen.html"Aruco Marker 在线生成器")
- 计算机视觉：相机成像原理：世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换
- 深入浅出地理解机器人手眼标定
- Dobot Magician 使用手册&开发文档
- ArUco: a minimal library for Augmented Reality applications based on OpenCV
- ArUco Marker 在线生成
- RealSense Example