[发明专利]视觉伺服机械臂物品抓取最佳位姿检测方法

权利要求书

1.视觉伺服机械臂抓取物品最佳位姿检测方法，其特征在于：包括以下步骤，

步骤一，读取照片；

步骤二，提取SURF特征点；

步骤三，根据SURF特征点生成图像的特征向量；

步骤四，初步建立一个含野值的匹配对；

步骤五，预防仿射变化，去除不满足变化的野值；

步骤六，获取目标物的多边形框；

步骤七，二维坐标系下最佳位姿解算；

步骤八，伺服控制机械臂抓取物品；

在步骤一中，采用相机成像模型进行照片拍摄和相机标定；所述相机成像模型由世界坐标系O_w-X_wY_wZ_w、相机坐标系O_c-X_cY_cZ_c、像素坐标系O_p-uv和图像坐标系O_i-X_iY_i构成；其中P是相机坐标系中的一点，坐标是(X_c,Y_c,Z_c)；P’是在图像中的成像点，在图像坐标系的坐标是(x,y)，在像素坐标系是(u,v)；相机标定是为了确定相机坐标系、图像坐标系、像素坐标系与真实坐标系之间的关系；

在步骤二中，包括以下子步骤，

子步骤一，建立积分图像；通过简单的加减法去计算任一区域的像素之和，矩形ABCD内所有像素的灰度值之和计算公式为：

Σ＝A-B-C+D

子步骤二，建立Hessian矩阵；一个像素点(x,y)，设函数f(x,y)的Hessian矩阵定义为：

可以看出H矩阵由函数f的二阶偏导数构成，每一个像素点都可以解算出一个H矩阵；式中：L_xx(x,σ)为函数f与尺度为σ的高斯二阶导数的卷积其定义如下：

对于L_xy(x,σ)和L_yy(x,σ)，定义相似；

子步骤三，建立尺度空间；SUFR特征极值点的检测算法和提取是基于尺度空间理论；通过改变使用高斯滤波器的尺度，不是改变图像本身来构成对不同尺度的响应；

子步骤四，提取特征点；利用非极值去除掉确定特征点，在海森行列式图像的每个像素点与3维邻域的26个点进行大小比较，设定一个阈值，用3维线性插值法得到亚像素级的特征点后，去掉特征值小于阈值的点，得出更稳定的点；

子步骤五，选取特征点的方向；以特征点为中心，检测兴趣点确定尺度s，统计扇形内所有点的Haar小波响应，并给靠近特征点的像素赋予较大的权重；统计该区域所有Haar小波响应之和，形成矢量，即扇形的方向；遍历所有的扇形区域，选择最长矢量方向作为该特征点的方向；

子步骤六，构造SURF的特征点描述子；以特征点为中心，构造出以20s为边长的正方形窗口，s为特征点所在的尺度；将其分为16个4×4的子区域，每个子区域统计包含有25个像素元的水平x和垂直y的Haar小波特征之和分别为ΣH_x和ΣH_y；统计出每个子区域的4维描述子V＝[∑H_x,∑H_y,∑|H_x|,∑|H_y|]，得出特征向量长度为64维的特征描述子；

在步骤七中，

将图像在像素坐标系O_p-uv中位置关系转化为图像的物理坐标O_i-X_iY_i，其目的是在图像的物理坐标系中可以进行几何关系的计算，转化公式为矩阵形式(4)：

其中(u,v)是在水平方向上和垂直方向的像素，设O_i(u₀,v₀)是图像的坐标系中心的像素，dx和dy是每个像素在x轴和y轴的方向上的实际物理尺寸，由相机的标定可以知u₀,v₀,dx,dy；SURF不变特征点算法匹配到的物品矩形框CDEF中利用图像算法可求出物品的质心O₄(u_s,v_s)和CDEF的像素坐标，根据几何关系进而可以求出物品的抓持点A(u₁,v₁)和B(u₂,v₂)的像素坐标，其匹配到的物品矩形框；

将物品的抓持点A(u₁,v₁)和B(u₂,v₂)的像素坐标带入公式(4)推导可得A和B的物理坐标，如公式(5)：

以及公式(6)：

所求出A(x₁,y₁)和B(x₂,y₂)的图像物理坐标，可以解出夹角α即最佳位姿角，理论上伺服给机械手抓的旋转角度也应为α，但由于机械臂的系统误差使得旋转角度小于或大于α，故取旋转角度为β；取机械手抓相对物品顺时针旋转为正，逆时针旋转为负，利用在坐标系中由两点之间的几何关系可求其斜率，可得公式(7)：

由公式(5)(6)(7)可得公式(8)：

求出公式(8)中的角度α，取值范围[0,90°]，若为正则机械手抓顺时针旋转，负则逆时针旋转；α是物品在二维平面的位姿角也是机械手抓旋转的夹持角，其夹持点为A和B两点；由公式(8)可知，dx,dy是相机内参数，故α只与夹持点A和B两点的像素点的差值有关，简化了图像中复杂的计算，从而提高视觉伺服效果。