基于双目视觉的绳驱动飞行机械臂目标识别与抓取

谭荣凯,杨浩秦,王尧尧

(南京航空航天大学 a. 机电学院; b. 材料科学与技术学院,江苏 南京 210016)

0 引言

目前无人机和多自由度机械臂所组成的空中操作系统由于其特有的优势,受到越来越广泛的关注。它们可以主动地和周围环境进行交互,在一些特殊的场景代替人类进行工作,有着非常广阔的应用前景。为了进一步提高飞行机械臂对周围环境的感知能力,对空中机械臂系统增加了视觉反馈系统,辅助机械臂进行作业。

空中机械臂的视觉系统采用的相机主要有3类:单目相机、双目相机和深度相机。单目相机体积小、价格低,但是其无法获取目标的深度信息;深度相机通过红外可直接获取目标的深度信息,但是其体积大,价格高,不适合用于无人机的实时监测;双目相机通过左右图像的视差来计算目标的深度信息,其体积和价格介于单目相机和深度相机之间,可以满足无人机实时监测的要求。在机械臂工作过程中,视觉系统承担的主要工作为目标追踪与姿态估计。目标追踪算法经过多年的研究,已经有了长足的发展,传统的算法比如均值漂移算法、基于Kalman滤波的目标追踪等以及目前时兴的深度学习算法。深度学习可以识别许多种类的目标,精度较高,但是深度学习依靠大量的计算资源,且需要大量的样本进行训练,而传统的目标追踪算法所需资源较少,适用于无人机的实时追踪任务。

近些年来,有很多学者在无人机的视觉领域作了很多工作。韩国首尔大学的 SEO等[1]基于单目相机开发了一种基于图像的圆柱体检测算法,该算法利用3D空间中透视投影圆的几何特征,实现了圆柱体的抓取。宾夕法尼亚大学的THOMAS等[2]从快速移动的猛禽获得灵感,基于单目相机在图像空间中研究了无人机的动力学模型,在视觉反馈的基础上生成运动轨迹,为无人机的抓取作了铺垫。日本立命馆大学的LEEWIWATWONG等[3]研究了一种新的图像区域分离方法,通过霍夫变换来实现对棒状物体的识别,从而控制空中机械臂系统自动与棒状物体对齐。塞尔维亚大学的RAMON-SORIA等[4]使用深度相机,利用人工神经网络(ANN)来检测目标,然后通过对齐算法来获取目标的姿态。东南大学王营华等[5]在目标上贴ArUco标签,在标签大小已知的情况下,利用n点透视算法求解摄像头位姿,进而利用摄像头位姿对空中机械臂系统进行分级控制,实现对目标的抓取。哈尔滨工业大学LUO等[6]利用ORB特征获取目标的特征点,在已知目标物真实大小的情况下,求解目标的姿态信息。

目前许多姿态估计算法采用深度学习的方式或者采用特征点的方式,计算量较大,对无人机机载计算机性能要求较高,且实时性难以保证。本文通过颜色特征结合目标的几何特征对目标进行追踪和姿态估计,采用的目标特征较少且计算方便,适合无人机等实时性要求较高的平台。

本文做的工作主要如下:

1)提出了一种轻量化的目标姿态估计算法,通过改进Camshift算法,结合目标几何特征对目标进行姿态估计,可用于实时性要求较高的飞行作业平台;

2)对机械臂和相机进行了系统建模,求解各坐标系之间的转换关系,通过逆运动学运算求解已知目标姿态时各关节的抓取角度。

1 系统整体建模

1.1 绳驱动机械臂建模

机械臂采用绳索驱动的形式。绳驱动机械臂将电机和减速器安装在机械臂的底座处,通过绳索传递运动与力,而传统机械臂将电机和减速器安装在机械臂的关节处,在臂的运动过程中,机械臂的质量会导致飞行器本身的质心发生迁移,降低飞行器的控制精度和作业质量,而绳驱动机械臂将电机安装在底座位置可以降低机械臂惯性对飞行器控制的不利影响。

使用D-H参数法对绳驱动机械臂进行建模,建立绳驱动连杆坐标系如图1所示。设关节i绕zi轴正方向的转动角度为θi,绕xi轴正方向的转动角度为αi-1,连杆沿zi轴正方向的移动距离为di,连杆长度为Ai。

图1 绳驱动机械臂坐标系

由于机械臂只在平面内进行运动,所以3个关节绕xi轴正方向的转动角度αi-1以及连杆沿zi轴正方向的移动距离di均为0,各连杆长度为:A0=10cm,A1=35cm,A2=35cm,AT=25cm。

(1)

式中:c1=cosθ1;c12=cos(θ1+θ2);c123=cos(θ1+θ2+θ3);s1=sinθ1;s12=sin(θ1+θ2);s123=sin(θ1+θ2+θ3)。

通过立体视觉可以获得目标在空间的位姿,将位姿作为机械臂末端执行器的期望位姿,通过运动学逆解,求解机械臂各关节的角度,再通过控制机械臂关节运动,即可实现抓取任务。可用末端执行器在底部坐标系x,y方向上的坐标以及相当于z轴的旋转来描述其在世界坐标系下的位姿,其变换矩阵如下式:

(2)

式中:c为cos;s为sin。

联立式(1)和式(2),通过代数法进行逆运动学求解,求解过程如下所示。

(3)

通过对机械臂关节角进行分析以及对上式的联立可得下式,其中X、Y为中间变量。

(4)

最终可得:

(5)

由于机械臂采用绳驱动的方式,使得机械臂关节在运动过程中会对其他关节产生影响,即关节之间存在耦合效应,所以关节期望的转角θa与实际电机的转角θm并不一致,两者之间存在一个系数I,即θm=Iθa,限于篇幅,在此直接给出I如下所示。

(6)

1.2 双目相机建模

双目相机相对于单目相机的优势是可以获取物体的深度信息,双目相机的左右两个摄像头可以同时采集信息,通过比较物体在两幅图像中的位置差异,可以获取物体的深度信息。本文所用的双目相机基线长度可变,通过调节基线长度可以实现探测深度的改变,实验中相机采用的基线长度为3cm,可探测深度范围为15cm～80cm之间,满足机械臂抓取要求。

图2所示为成像过程中涉及的4个坐标系,分别为世界坐标系{OW}、相机坐标系{OC}、左右成像坐标系{OL}、{OR}和位于成像坐标系左上角的像素坐标系,通过各个坐标系之间的转换关系,可以获取二维平面上的物体在三维空间中的位置关系。

图2 坐标系之间的关系

像素坐标系与相机坐标系之间的转换关系如下:

(7)

式中K称为相机的内参数矩阵,通过标定获得。

相机坐标系与世界坐标系之间的转换关系如下:

(8)

(9)

2 目标追踪与姿态识别

Camshift算法的主要流程如下:给定d维空间R的n个样本点x= [xi] ,i= 1,…,n,选定其中某一点xi,做一个半径为r的圆,其中有k个样本点落入圆中,记这k个点的集合为{Sk},落在圆内的所有点都与xi构成一个向量,将这些向量相加,所得的结果称为meanshift向量。如图3(a)所示,P1即为选定的点,P1指向P2的向量即这个时刻meanshift向量,然后P2作为新的选定点继续做圆,如图3(b)所示。最终meanshift向量可以收敛到概率密度最大的地方,如图3(c)所示。

图3 Camshift算法

meanshift向量通过搜索框搜索概率密度最大区域来确定目标对象,但其固有的缺陷,算法的收敛速度与选取的窗口有关,而且窗口一旦选定,窗口的大小不能改变。当目标在视野中发生变化,比如变大或变小时,窗口不能自适应的去调整大小。

为了能让搜索窗口的大小随着目标的变化自适应调整大小,本文利用目标的颜色直方图模型将图像转变为颜色概率分布图,同meanshift向量一样,初始化一个搜索窗口的位置和大小,根据搜索窗口得到的结果自适应调整下一帧搜索窗口的位置和大小,最终得到图片中颜色概率密度分布最大的区域。

想要将图像转变为颜色概率分布图,需要对图像进行反向投影。首先将图像从RGB颜色空间转换到HSV空间,可以降低光照的影响,对其中的H分量做直方图,在直方图中包含了不同的H分量值出现的概率,然后将图像中每个像素的值用其颜色出现的概率替换,就得到了颜色概率分布图。

RGB到HSV空间的转换关系如下[7]:

(10)

(11)

(12)

(13)

假设以绿色作为目标颜色,Camshift算法利用自适应的搜索框对感兴趣目标颜色区域(ROI)进行实时检测并返回ROI的相关信息,如ROI的外接矩形的4个顶点位置以及其中心位置。如此可以利用Camshift完成目标追踪的功能,并利用检测目标区域的相关信息进行姿态估计。算法返回的ROI外接矩形框的相关信息如图4所示。

图4 目标追踪与姿态估计

故而可以得到P0—P44个点在二维图像中的位置信息。设pi(i=0～4)代表P0—P4在像素坐标系中的坐标,PWi(i=0～4)为世界坐标系中的坐标。通过式(7)和式(8)可得:

(14)

由此可求得PWi,[XWiYWiZWi] 为PWi的坐标。故圆柱体的半径r等价于中心点到任意点间的距离:

r=|PW0PW3=

(15)

由于圆柱体的几何特性,在求解圆柱体姿态时只考虑圆柱体的法向量。从P1—P4中任取两点,例如P1、P2,设P0指向P1的向量为n1,P0指向P2的向量为n2,则圆柱体的法向量n可表示为

n=n1×n2

(16)

3 实验结果

实验所用的机械臂为本课题组自行设计的绳驱动机械臂,用STM32F103单片机进行控制,相机采用免驱动可变基线双目相机,机载计算机为NVIDIA JESTON NX,目标为圆柱形物体。主要控制流程为相机检测到目标物体后,通过机载计算机实时计算目标的大小与姿态信息,通过逆运动学解算后得到机械臂各关节的抓取角度,再将此抓取角度信息传递给单片机,单片机控制机械臂完成抓取过程,实验平台如图5所示。

图5 实验平台

图6所示为机载计算机通过算法估计的圆柱体目标的半径值,直线为圆柱体半径真实值为27.6mm,曲线为视觉算法估计的半径值,误差为[-0.21,+0.25]mm 。

图6 圆柱体半径估计

图7所示为绳驱动飞行机械臂的整个抓取过程。理想状态下,无人机从预定位置起飞,在预定的轨迹上行进,当双目相机检测到目标后通过Camshift算法对目标进行实时追踪并引导无人机靠近目标。当目标处于机械臂的工作空间后,通过机载计算机实时计算目标在三维空间中的姿态信息,然后通过逆运动学解算出机械臂各关节期望运动角度,机载计算机将数据发送到单片机中,然后单片机控制机械臂抓取,抓取完成后无人机退回到预定位置。

图7 绳驱动飞行机械臂抓取过程

4 结语

1)针对飞行机械臂实时性要求较高的情况,提出了一种轻量化的目标姿态估计算法。

2)建立了双目相机模型和绳驱动机械臂运动学模型,得到了目标在相机坐标系与世界坐标系的转换关系,通过逆运动学求解了在已知目标位姿情况下绳驱动机械臂的抓取角度。

3)进行了目标抓取实验,验证了姿态估计算法的可行性。