目录

简介

视觉机械臂是智能机器人的一个重要分支，它主要包括控制芯片、驱动电路、机械臂、相机等部分。自主抓取是指，在没有人为干预的情况下，视觉机械臂系统通过摄像头获取到目标物体的位置，并且通过驱动机械臂来完成对于目标物体的抓取任务。

整个抓取过程大致分为以下几步（含图中九步）：相机标定——→相机和机械臂进行手眼标定——→①读取摄像头信息——→②识别出目标物体并得出位姿——→③物体在相机中的二维坐标转化成相对于相机光心的三维坐标——→④再转换成相对于机械臂底座的世界坐标——→⑤计算机械臂末端执行器抓取物体时的位姿——→⑥通过已知的目标位置和末端执行器的位姿信息来计算机械臂上每个电机应转动的角度——→⑦根据有无障碍物、路径距离要求、时间要求规划出合适的路径——→⑧驱动机械臂各个关节一步一步到达合适的位置——→⑨末端执行器抓取物体——→投放到指定位置

下面我将根据以上这些步骤，对视觉机械臂自主抓取全流程做出更加详细的介绍。

相机标定

什么是相机标定？为什么要进行相机标定呢？由于相机采用小孔成像的原理来采集图像，为了使物体成像更加清晰，就采用了透镜的方式。但是，因为透镜本身在成像的过程中会不可避免的引入径向畸变和切向畸变（比如一条直线成像后会显示成一条弧线）。为了使得现实中的图像能够真实的呈现在图片当中，有利于之后机械臂的抓取，则需要对相机进行畸变校正。一般情况下，只要是对于成像精度较高的场景都需要进行标定，比如三维结构光成像、人脸识别、视觉测量等。

现在对于相机标定最常用的方法是张正友相机标定法，这种方法是利用棋盘格标定板每个角点像素坐标的相对关系以及在已知真实坐标相对关系的条件下计算出来的。在OpenCv当中已经有了对应的库，在使用前只需设置一下每个格子的大小以及格子的行列数（减去一）即可进行标定。相机标定的合格指标一共有四个，分别是x轴方向、y轴方向、远近以及旋转角度。

 捕捉到多张有效图片之后，进度条会变成绿色，点击CALIBRATE可直接计算出关系矩阵。以下就是我采用奥比中光Astro Pro设备得到的标定结果。

 我们采用逆向计算的方法通过物体的像素坐标获取到图像的真实空间位置（世界坐标），在下图的推到过程中会涉及到以下四个坐标系：

OW-XWYWZW：世界坐标系，描述相机位置，单位m

Oc-XcYcZc：相机坐标系，光心为原点，单位m

xy：图像坐标系，光心为图像中点，单位mm

uv：像素坐标系，原点为图像左上角，单位pixel

P：世界坐标系中的一点，现实空间中的点

p：点P在图像中的成像点，在图像坐标系中的坐标为(x,y)，在像素坐标系中的坐标为(u,v)

f：相机焦距，等于o与Oc的距离，f=||o-Oc||

世界坐标系是为了更好的描述相机的空间位置，原点（OW）的选择在双目深度相机当中一般会是其中一个相机的中心或者两个相机的中点。在视觉机械臂的标定中，原点（OW）一般被确定为机械臂底座的中心。

世界坐标和相机坐标的变换过程属于刚体变换：即物体不会发生形变，只是进行了平移和旋转的变换。其中R表示旋转矩阵，T表示偏移向量（R、T被称为外参）。围绕着不同的坐标轴，旋转不同的角度都是有对应矩阵的，如下图所示：

 从相机坐标系到图像坐标系的变换，是从3D向2D的转换，主要思想就是相似三角形原理，对应原理图如下：

图中P点就是现实世界的坐标，p点就是在相机中的成像坐标，f就是透镜的焦距。

图像坐标与像素坐标的转换关系如下：

图像坐标系的原点是相机光轴和成像平面的交点，单位是mm，属于物理单位。像素坐标系的原点是成像平面的左上角，单位是pixel。

将以上这几个公式进行整理，结果如下：

相机的内参和外参已在图中标明，由于未知数有很多个，所以需要多组数据才能求解。

手眼标定

首先讲一下什么是手眼标定？为什么要手眼标定？因为一个物体与相机的相对位置关系和这个物体与机械臂之间的关系是不一样的，所以在相机确定了物体的位置之后，还要把此时的位置转换成相对于机械臂的位置，这样机械臂才能进行抓取。这个位置转换需要一个矩阵，这个矩阵就是由手眼标定得出的。

手眼标定的核心公式只有一个，AX=XB，这里的X就是指手（机械臂末端）与眼（摄像头）之间的坐标转换关系。视觉机械臂的相机和机械臂有两种结合方式，一种是眼在手上（Eye-In-Hand），一种是眼在手外（Eye-To-Hand），所以手眼标定也分两种。

Eye-In-Hand（摄像头被安装在机械臂上）：

这种方式的手眼标定是通过两次运动解得相机坐标系（Camera）与机械臂末端坐标系（End）之间的坐标关系。

注意：

1、机械臂末端（End）在机器人（Base）坐标系下的坐标是可以通过机械臂各个关节的角度值直接解算出来的。

2、摄像头与机械臂末端之间的坐标转换关系不变，也就是说，TE→C始终不变；标定板与机械臂底座之间的坐标转换关系不变，也就是说，TB→O也是不变的。

把TB→O按照前后两次运动展开，有：

TB→E1TE1→C1TC1→O=TB→E2TE2→C2TC2→O

(TB→E2)-1TB→E1TE1→C1TC1→O=TE2→C2TC2→O

(TB→E2)-1TB→E1TE1→C1=TE2→C2TC2→O(TC1→O)-1

TE2→E1TE1→C1=TE2→C2TC2→C1

记

A = TE2→E1 =  (TB→E2)-1TB→E1

B = TC2→C1 = TC2→O(TC1→O)-1

X = TE→C = TE1→C1 = TE2→C2

就得到了

AX = XB

Eye-To-Hand（摄像头被安装在一个固定不动的位置，而标定板被拿在机械臂手上）：

这种方法的手眼标定是通过两次运动解得相机坐标系（Camera）与机器人坐标系（Base）之间的坐标关系。

注意：
1、机械臂末端（End）在机器人（Base）坐标系下的坐标是可以通过机械臂各个关节的角度值直接解算出来的。

2、摄像头与机械臂底座之间的坐标转换关系不变，也就是说，TB→C始终不变；标定板与机械臂末端之间的坐标转换关系不变，也就是说，TE→O始终不变。

把TB→O按照前后两次运动展开，有：

TE1→BTB→CTC→O1=TE2→BTB→CTC→O2

(TE2→B)-1TE1→BTB→CTC→O1=TB→CTC→O2

(TE2→B)-1TE1→BTB→C=TB→CTC→O2(TC→O1)-1

记

A = (TE2→B)-1TE1→B

B = TC→O2(TC→O1)-1

X = TB→C

就得到了

AX = XB

实际上，求解的只是一个固定的6自由度的位置姿态关系，可以用3X3的旋转矩阵和3X1的平移矩阵表示，也可以用其他方法表示如用四元数表示角度关系等。

求解（Eye-In-Hand）

坐标系之间的转换关系说明：

baseHend：表示机械手坐标系到基础坐标系的转换关系，可以由机器人系统中得出。（已知）

endHcam：表示相机坐标系到机械手坐标系的转换关系；这个转化关系在机械手移动过程中是不变的；（未知，待求）

objHcam：表示相机坐标系到标定板坐标系的转换关系（相机外參），可以由相机标定求出；（相当于已知）

baseHobj：表示标定板坐标系到基础坐标系的变换，这个是最终想要得到的结果；只要机械手和标定板的相对位置不变，这个变换矩阵不发生变化。

基础坐标系(求解baseHend)

符合右手定则的XYZ三个坐标轴：

原点：机器人底座的中心点

X轴正向：指向机器人的正前方

Z轴正向：指向机器人的正上方

Y轴正向：由右手定则确定

六个自由度

三个位置：x、y、z（第六轴法兰盘圆心相对于原点的偏移量）

三个角：Rx、Ry、Rz（第六轴法兰盘的轴线角度，由初始姿态即竖直向上绕x轴旋转Rx度，再绕Y轴旋转Ry度，再绕Z轴旋转Rz度得到）

旋转方式

绕定轴X-Y-Z旋转(判断机械臂输出四元数与代码得到的四元数是否相等得到)

旋转矩阵的计算方法如下：R = R z ∗R y ∗R x

（opencv的旋转方式是 z y x，所以旋转矩阵R=Rx*Ry*Rz）

 R=Rx∗Ry∗Rz=X1Y1Z3

其中c为cos，s为sin。

所以：

其中t=[x,y,z]^T,从示教器读取Rx、Ry、Rz分别是绕x、y、z轴的旋转角度。

欧拉角：分别绕x、y、z轴旋转的角度，不一样的旋转次序，得到的R不一样；

李代数：维度是3，是绕一个轴转动一定的角度。欧拉角可以理解成李代数在x、y、z轴上的分解旋转。

camHobj相机到标定板

思路大致如下：

已知双目相机的内参、畸变系数、外参（Pr=R∗Pl+t),

对左右相机的两张图片调用OpenCV中的findChessboardCorners函数，找到内角点（如果结果不好，继续提取亚像素点）；

将左右相机的像素点对应起来，得到匹配的2d点；

使用空间异面直线的方法，用对应的2d点计算出以右相机为世界坐标系的3维坐标Pcam；计算出每个角点以棋盘格为世界坐标的3维坐标Pcal；

通过解方程Pcam=camHcal*Pcal求解出外参；

这个部分可以用张正友相机标定Opencv实现

求解AX=XB

李代数到李群的转换满足指数映射关系，假设[w]∈so(3)，而exp[w]∈SO(3)，则其指数映射满足罗德里格斯公式：

其中∣∣ω∣∣^2=ω12+ω22+ω32

利用李群知识求解AX=XB

展开得到：

θA∗θX=θX∗θB，θA∗bX+bA=θX∗bB+bX​

采用“两步法”求解上述方程，先解算旋转矩阵，再求得平移向量。

求解旋转矩阵

假设AX=XB，这里的都是旋转矩阵(SO(3))，并非变换矩阵(SE(3))。

变换可得到：A =XBX^T

两边取对数：log(A)=log(XBX^T)(对数映射)

令logA=[α],logB=[β]，则上式可以化为[α]=X[β]X^T=[Xβ]从而α=Xβ

存在多组观测值时，求解该方程可以转化为下面最小二乘拟合问题：

很显然，上述问题是典型的绝对定向问题，因而求解上式与绝对定向相同，其解为

 其中，

当只有两组A、B时，即有A1,A2,B1,B2

α1=logA1,α2=logA2,β1=logB1,β2=logB2，θX=MN^−1  

其中，M=(α1 α2 α1×α2)，N=(β1 β2 β1×β2)（×表示叉乘）

即可求得旋转矩阵。

求解平移向量

θA∗bX+bA=θX∗bB+bX 移项化简得到：(I−θA)∗bX=bA−(θX∗bB)计作：c*X=d

又c不一定是可逆，所以两边同时乘以c^T。即为：c^T∗c∗X=c^T∗d

所以：X=（c^T∗c)^−1∗(c^T∗d)

当有多组数据时：(c1^T∗c1+c2^T∗c2+…+cn^T∗cn)∗X=c1^T∗d1+c2^T∗d2+…+cn^T∗dn

即可求得X，即平移向量

手眼标定步骤

1. 事先准备：机械臂可以按照正常的位姿移动，并且能够正常的输出机械臂末端姿态；机械臂能够稳定搭载相机；相机能够正确稳定地检测到物体，并且能够正确输出位置参数。
2. 通过机械臂上面的相机，对待测物体进行位姿检测，并且输出参数。
3. 输出机械臂末端的位置参数。
4. 重复2、3步骤，采集十组以上有效参数。
5. 将采集的有效参数放入代码中进行计算。
6. 输出手眼转换矩阵。

代码：见https://blog.csdn.net/hellohake/article/details/104808149

读取出摄像头信息并确定目标物体的位姿

前两步完成之后就要通过摄像头读取信息了，这个步骤不难，比较难的是确定目标物体的姿态。它首先要进行图像分割（实例分割、语义分割、部件分割），把图像中的每个个体单独分出来。然后进行物体识别，来确定哪个物体才是目标物体。之后是确定物体的姿态，比如一个瓶子，它是立着的，还是倒着的，是横着的，还是竖着的。最后确定抓取的位置，是抓取瓶子的盖子下面，还是抓取瓶子的中间部分，还是底部。此处的图像还分为二维的灰度图、RGB图，三维的点云图、三角面片、Volumetric、Multi-View等。

图像分割

图像分割是计算机视觉研究中的一个经典难题，已经成为图像理解领域关注的一个热点，图像分割是图像分析的第一步，是计算机视觉的基础，是图像理解的重要组成部分，同时也是图像处理中最困难的问题之一。所谓图像分割是指根据灰度、彩色、空间纹理、几何形状等特征把图像划分成若干个互不相交的区域，使得这些特征在同一区域内表现出一致性或相似性，而在不同区域间表现出明显的不同。简单的说就是在一副图像中，把目标从背景中分离出来。对于灰度图像来说，区域内部的像素一般具有灰度相似性，而在区域的边界上一般具有灰度不连续性。图像分割的方法主要分为以下几类：基于阈值的分割方法、基于区域的分割的方法、基于边缘的分割方法以及基于特定理论的分割方法。下图中就是基于边缘的分割方法，正方体和香蕉的分割效果很好，易拉罐分割效果次之。

               ​​​​

三维对象实例分割

三维对象实例分割是指从输入的三维点云中检测某一类的点集实例对象。与二维对象实例分割类似，双阶段方法需要区域建议，而单阶段方法则需要自由建议。三维对象实例分割的典型功能曲线图如图所示。

三维对象实例分割在机器人抓取任务中非常重要。但目前的方法主要是利用二维实例分割方法来获得目标对象的三维点云，这利用了RGB-D图像的优势。目前，三维对象实例分割仍是一个快速发展的领域，如果其性能和速度大大提高，在未来将得到广泛的应用。

姿态估计

先简单介绍三种方法：

A.通过深度神经网络处理，来生成几个候选的抓取位置。这些候选的抓握姿态被编码到一个堆叠的多通道图中。使用一个四层卷积神经网络对每个候选抓握姿态进行评估，以获得一个分数。

B.对整个三维空间进行均匀采样生成候选抓取，利用三维CNN和候选抓取姿态的抓取可达性预测抓取稳定性，得到抓取成功概率。

C.还有一种名为6-自由度GraspNet的算法，该算法使用变分自动编码器对抓取建议进行采样，并使用抓取评估器模型对采样的抓取进行参考。

第一幅图可以看到，算法生成了很多的抓取位置，然后以评分的方式选择其中一种。第二幅图中，通过算法进行模型匹配，来生成最佳的抓取位置。

机械臂正逆运动学求解

首先，大致说一下什么是正逆学求解。机械臂的运动主要是靠电机的旋转来实现末端的移动，一个旋转，一个空间的位置移动。输入每个电机旋转的角度j1,j2…,jn，输出是机械臂末端执行器的位置(x,y,z)与姿态(α,β,γ)，这就是机械臂运动学正解。反之，为逆解。

机械臂还有一个常用的参数叫自由度。机械臂机架上有n个电机，就是n自由度机械臂，也叫n轴机械臂，记为n DOF。（末端执行器上的不算，因为它不影响机械臂的运动学结果。）

UR5机械臂的Gazebo仿真图

运动学正解

正运动学是已知关节六个角度求变换矩阵T

 所以末端坐标系6到基座固定坐标系0的变换矩阵

那么求正解就很简单了，只要输入六个关节角度θi，就得到末端坐标在基坐标系的变换矩阵T。ur机械臂的视教板上末端点的坐标是用六个值[x, y, z, rx, ry, rz]表示的。前三个值[x, y, z]是三维笛卡尔坐标，表示空间位置，后三个值[rx, ry, rz]是坐标旋转向量，表示空间姿态。我们得到的变换矩阵T怎么变成六值坐标[x, y, z, rx, ry, rz]呢？设

T的左上角的3×3矩阵是旋转矩阵，旋转矩阵和旋转向量之间可以通过罗德里格斯（Rodrigues）变换进行转换。opencv里有相应的函数调用。算法也比较简单，不用opencv的函数自己写代码也不难。T的右上角3×1就是空间位置[x, y, z]。这样有变换矩阵T得到六值坐标，完成了正解。

运动学逆解

逆解相对复杂一些，根据机械臂末端的空间位置和姿态，来求解可能的关节角度。求逆解的方法有解析法，迭代法和几何法等。下面我们推导一下ur的逆解。

首先计算变换矩阵T过程中的一些中间矩阵

 ，其中c23=cos(θ2+θ3)，s23=sin(θ2+θ3)。

 由得到。计算

 ，，得到

 等式两边矩阵的行列应该分别相等，由第三行第四列得到，可解得，有两个解。这里注意写程序的时候，求解这里的反正切是用atan2()这类的函数，返回之在(-π,+π]。而反余弦的返回值在[0,π]，从而保证在2π范围每个解是唯一的。

由第三行第三列得，可解得，两个解。由第三行第二列得到，可解得。接着由

 ，计算

 ，得出等式左边等于

 由，两边平方，令，。同样由，令，两式相加得到

 ，则，有两个解。

 把θ3带入和，得

 ，，其中t2=tanθ2。两式消去c2，得到

 最后得到，从而得到θ4。综合有两个解的情况，ur机械臂逆解总共有2x2x2=8组解。

总结

视觉机械大致有11步，前文图中九步加上相机标定、手眼标定。里面有很多知识也是借鉴别人的，毕竟一个的能力有限，不可能完成这么多理论知识。自己在学习前也是找了很多资料，没有发现一篇能把视觉机械臂全流程讲详细的帖子。所以我学习并理解了全流程之后，自己整理了一篇文章，仅供大家参考。因为视觉机械臂本身涉及的内容比较多，所以我对于每一步的讲解也不是非常的透彻。如果想更详细的了解其中的某个或某些步骤，可以自行网上检索。总结不易，希望能帮到每一个热爱学习的人。只要你想成功，全世界都会为你让路！！！