[发明专利]一种基于双目视觉的机械臂目标定位抓取方法

权利要求书

1.一种基于双目视觉的机械臂目标定位抓取方法，其特征在于包括以下步骤：

步骤1、安装双目视觉相机和机械臂，其中双目视觉相机水平放置，并确保双目视觉相机与机械臂底座处在同一水平面上，水平拍摄物体，识别区域为机械臂工作半径范围，要求双目视觉相机位置与识别区域的距离为0.8-20.0米并确保识别区域不被遮挡；

步骤2、矫正双目视觉相机内参，双目视觉相机内参是与双目视觉相机自身特性有关的参数，可以此确定其焦距和像素大小，根据双目视觉相机提供的校准程序得到双目视觉相机中左、右摄像机的左、右内参，通过式(1)进行描述，

其中，表示左摄像机图像横轴方向上以像素为单位的焦距，为左摄像机图像纵轴方向上以像素为单位的焦距，为左摄像机光轴与图像中心在横轴方向上以像素为单位的差距，为左摄像机光轴与图像中心在纵轴方向上以像素为单位的差距，为右摄像机图像横轴方向上以像素为单位的焦距，为右摄像机图像纵轴方向上以像素为单位的焦距，为右摄像机光轴与图像中心在横轴方向上以像素为单位的差距，为右摄像机光轴与图像中心在纵轴方向上以像素为单位的差距；

步骤3、获取目标物体的深度信息，运行双目视觉相机，根据颜色的直观特性在由A.R.Smith在1978年创建的HSV颜色空间中对图像进行预处理，利用目标物体的颜色特征根据其在饱和度区间和色调区间的阈值来分割图像，同时将识别到的目标区域位置与深度图像进行匹配获得目标区域的深度信息，并通过公式(2)计算目标物体距离双目视觉相机的深度信息d，

其中，n表示识别区域像素点个数，d_i表示识别区域每个像素点的深度信息；

步骤4、建立图像坐标系和双目视觉相机坐标系，获取目标物体在双目视觉相机坐标系中的空间位置，目标物体在双目视觉相机坐标系中的三维空间坐标为(X,Y,Z)，在图像坐标系中的图像物理坐标系(x,y)和图像像素坐标系(u,v)，通过公式(3)计算得到，

其中，d_x表示每个像素在x轴上的物理尺寸，d_y表示每个像素在y轴上的物理尺寸，由式(3)得到从图像物理坐标到图像像素坐标变换的矩阵形式，通过公式(4)进行描述，

根据双目视觉相机成像原理得到，

其中，f表示摄像机焦距，可根据双目视觉相机内参得知，由式(4)和(5)得到图像坐标系与双目视觉相机坐标系三维空间坐标转换的矩阵形式，通过公式(6)进行描述，

其中，Z表示由步骤3获得的目标物体距离双目视觉相机的深度信息d，最终获得目标物体在双目视觉相机坐标系中的三维空间坐标为(X,Y,Z)；

步骤5、计算目标物体在机械臂坐标中的空间位置，在步骤1中，安装双目视觉相机时，它相对于机械臂的位置关系是确定的，其中机械臂基座的坐标系与基于双目视觉相机的坐标系x轴相互平行但方向相对，y轴相互平行但方向相向，z轴相互平行且方向一致，已知基于双目视觉相机的坐标系原点在机械臂基座坐标系的笛卡尔坐标为(x₁,y₁,z₁)，目标物体在双目视觉相机坐标系的坐标为(X,Y,Z),则目标物体在相对于机械臂基座的三维坐标(x₂,y₂,z₂)，通过公式(7)进行描述，

其中，θ表示机械臂基座坐标系相对于双目视觉相机坐标系在z轴上的旋转角，根据已知双目视觉相机与机械臂的安装位置关系，可得θ＝π；

步骤6、计算机械臂末端的抓取姿态，具体包括以下子步骤：

(a)计算目标物体在机械臂基座坐标系中的方位角α，方位角表示从x轴正方向开始，逆时针方向旋转与目标物体到坐标原点连线之间的夹角，其中机械臂基座指向双目视觉相机方向为x轴正方向，x轴正方向逆时针旋转90度为机械臂基座y轴正方向，

若目标物体位于第一象限，

若目标物体位于第二象限，

若目标物体位于第三象限，

若目标物体位于第四象限，

若x＝0,y>0时，α＝π/2，x＝0,y<0时，α＝3π/2，

若y＝0,x>0时，α＝0，y＝0,x<0时，α＝π；

(b)计算极角β和机械臂末端旋转角ω，极角β表示正z轴到目标物体矢量的角度，它表示物体的倾斜程度，旋转角ω表示机械臂末端抓取物体时旋转的角度，由于目标物体竖直放置，则：

β＝π/2 (12)

ω＝π/2 (13)

(c)欧拉角到四元数的转换，由于机械臂末端姿态控制参数为四元数和三维坐标点，其中三维坐标点为目标物体在相对于机械臂基座的三维坐标(x₂,y₂,z₂)，而四元数需要根据计算得到的欧拉角转换得到，上述计算得到的方位角、极角、机械臂末端旋转角的欧拉旋转方式为ZYZ，因此转换后的四元数q(x′,y′,z′,w′)，通过公式(14)进行描述，

其中，ψ＝β/2,ξ＝ω/2,φ＝α/2；

步骤7、发送控制信息，将目标物体的三维空间坐标及抓取时机械臂末端的姿态作为控制信息发送至机械臂控制器，同时通过返回机械臂末端的状态信息，来判断是否完成对目标物体的抓取。