视觉机械臂自主抓取全流程解析

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

    整个抓取过程大致分为以下几步（含图中九步）：相机标定——相机和机械臂进行手眼标定——读取摄像头信息——识别处目标物体并得出位姿——物体在相机中的二维坐标转化成相对于相机光心的三维坐标——再转换成相对于机械臂底座的世界坐标——计算机机械臂末端执行器抓取物体时的位姿——通过已知的目标位置和末端执行器的位姿信息来计算机械臂上每一个电机应转动的角度——根据有无障碍物、路径距离要求、时间要求规划处合适的路径——驱动机械臂各个关节一步一步到达合适的位置——末端执行器抓取物体——投放到指定位置
    根据以上步骤，对视觉机械臂自主抓取全流程做出更加详细的介绍。
    相机标定
    什么是相机标定？为什么要进行相机标定呢？由于相机采用小孔成像的原理来采集图像，为了使物体成像更加清晰，就采用了透镜的方式。但是，因为透镜本身在成像的过程中会不可避免的引入畸变和切向畸变（比如一条直线成像后会显示成一条弧线）。为了使得现实中的图像能够真实的呈现在图片中，有利于之后机械臂的抓取，则需要对相机进行畸变校正。一般情况下，只要是对于成像精度较高的场景都需要标定，比如三维结构光成像、人脸识别、视觉测量等。
    现在对于相机标定常用的方法是张正友相机标定法，这种方法是利用棋盘格标定每个角点像素坐标的相对关系以及在已知真实坐标相对关系的条件下计算出来的。在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也是不变的。 
    Eye-To-Hand（摄像头被安装在一个固定不动的位置，而标定板被拿在机械臂上）:

    这种方式的手眼标定是通过两次运动解得相机坐标系（Camera）与机械臂末端坐标系（Base）之间的坐标关系。
    1、机械臂末端（End）在机器人（Base）坐标系下的坐标是可以通过机械臂各个关节的角度值直接解算出来的。   
    2、摄像头与机械臂末端之间的坐标转换关系不变，也就是说，TE-C始终不变；标定板与机械臂底座之间的坐标转换关系不变，也就是说，TB一O也是不变的。

    实际上，求解的只是一个固定的6自由度的位置姿态关系，可以用3×3的旋转矩阵和3×1的平移矩阵表示，也可以用其他方法表示如用四元数表示角度关系等。
    求解（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=Rz*Ry*Rx
    （opencv的旋转方式是zyx，所以旋转矩阵R=Rz*Ry*Rx）

    其中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），则其指数映射满足罗德里格斯公式：



    手眼标定步骤
    1.事先准备：机械臂可以按照正常的位姿移动，并且能够正常的输出机械末端姿态；机械臂能够稳定搭载相机；相机能够正确稳定地检测到物体，并且能够正确输出位置参数。
    2.通过机械臂上面的相机，对待测物体进行位姿检测，并且输出参数。
    3.输出机械臂末端的位置参数。
    4.重复2、3步骤，采集十组以上有效参数。
    5.将采集的有效参数放入代码中进行计算。
    6.输出手眼转换矩阵。
    读取出摄像头信息并确定目标物体的位姿
    前两部完成之后就要通过摄像头读取信息了，这个步骤不难，比较难的是确定目标物体的姿态。它首先要进行图像分割（实例分割、语义分割、部件分割），把图像中的每个个体单独分出来。然后进行物体识别，来确定哪个物体才是目标物体。之后是确定物体的姿态，比如一个瓶子，它是立着的，还是倒着的，还是竖着的。最后确定抓取的位置，是抓取瓶子的盖子下面，还是抓取瓶子的中间部分，还是底部。此处的图像还氛围二维的灰度图、RGB图、三维的点云图、三角面片、Volumetric、Multi-View等。
    图像分割
    图像分割是计算机视觉研究中的一个经典难题，已经称为图像理解领域关注的一个热点，图像分割是图像分析的第一步，是计算机视觉的基础，是图像理解的重要组成部分，同时也是图像处理中最困难的问题之一。所谓图像分割是只根据灰度、彩色、空间纹理、几何形状等特征把图像分成若干个互不相交的区域，使得这些特征在同一区域内表现出一致性或相似性，而在不同区域间表现出明显的不同。简单的说就是在一幅图像中，把目标从背景中分离出来。对于灰度图像来说，区域内部的像素一般具有灰度相似性，而在区域的边界上一般具有灰度不连续性。图像分割的方法主要分为以下几类：基于阈值的分割的方法、基于边缘的分割方法以及基于特定理论的分割方法。下图中就是基于边缘的分割方法，正方体和香蕉的分割效果很好，易拉罐分割效果次之。










    总结
    视觉机械大致有11步，前文图中九步加上相机标定、手眼标定。里面有很多知识也是借鉴别人，能力有限，不可能完成所有的理论知识，以上仅供参考。