一种基于自然特征的飞行机械臂视觉伺服抓取方法与流程

本发明涉及无人机领域，尤其涉及一种基于自然特征的飞行机械臂视觉伺服抓取方法。

背景技术：

随着机器人技术的迅速发展，在工业制造、军事行动、民用生活中得到了广泛的应用。无论军事应用、民用，多旋翼无人机运输有着很重要的作用，它们可垂直起飞、降落、悬停。逐渐地，多旋翼无人机不仅局限于运输方面，复合无人机将成为新一代空中操作机器人，将代替人类完成危险、复杂环境的空中操作任务。

近年来，科研人员希望无人机能如同人一样用手臂去抓取环境中的物体，因此诞生了飞行机械臂。飞行机械臂是一个复杂的欠驱动系统，同时搭载的多自由度机械臂在控制上有别于工业机械臂，目前全球都处于初步研究中。由于在大部分环境中飞行机械臂的运动和抓取必须在机器人本体保持静止或稳定状态下才能进行，这极大地限制了机器人的运动，同时在机械臂运动时会对无人机产生干扰，而视觉伺服可以很好的抑制各种干扰，实现精准的抓取。目前国内关于飞行机械臂只有较少的相关文献，虽有少部分相关无人机机械臂的发明专利，但难以找到飞行机械臂视觉伺服抓取相关的。通常视觉伺服的标靶是人工标记物，需要提前预设，无法与环境进行友好的交互，不符合真实场景。大部分研究者只考虑在机械臂的行程范围内进行抓取，而实际中无人机可能距离目标物的距离大于阈值，单纯通过机械臂的运动进行抓取，无法完成任务。本发明正是根据这种需求提出了基于自然特征的视觉伺服和多任务的联合视觉伺服策略。

《一种无人机及其基于视觉的抓取方法》(公开号：cn107139178a)。该发明提供了一种无人机，包括无人机机体、机械臂、舵机控制关节和摄像头，所述机械臂固连于所述无人机机体的下方，所述舵机控制关节连接于所述无人机机体的上方前部，所述舵机控制关节为单自由度，所述舵机控制关节与所述摄像头连接，对机械臂各关节进行了标定，本发明提供了一种无人机基于视觉的抓取方法，用pd控制器进行舵机控制关节的控制，使得在俯仰、偏航方向上，在无人机或目标运动中，摄像机能够准确对准目标物，进行俯仰、偏航追踪。该方法摄像头装在机身，摄像头的活动范围小，并不是对机械臂进行视觉伺服，而是利用相机和目标物的位姿关系，解算机械臂的逆运动学，无法做到实时性。同时机械臂的自由度低，只能在一个平面内运动，无法到达任意位姿。

《一种无人机机动目标定位跟踪中的视觉伺服控制方法》(公开号：cn105353772a)。该发明通过建立大地坐标系、机体坐标系、摄像机坐标系、图像坐标系、机体大地过渡坐标系，通过上述建立的坐标系之间的关系，根据目标的成像序列，进行目标的定位及目标跟踪的姿态角给定值和航线跟踪的姿态角给定值的计算，完成视觉伺服控制。仅仅使用单一固定的摄像头，其优点是无需跟踪云台和激光测距设备，有效地降低了载荷的体积和成本，提高了侦察的隐蔽性。但是摄像头无法自由活动。限制了其可视范围。该发明只涉及到无人机的飞行控制，未涉及飞行机械臂的伺服抓取。

《一种基于力反馈装置和vr感知的飞行机械臂系统及控制方法》(公开号：cn109164829a)。该发明包括无人机部分和地面站部分，无人机部分包括了无人机机体、双目相机、机载电脑、机械臂、控制系统和视觉标签。控制系统包括飞行主控和机械臂主控，机载电脑和控制系统安装在无人机机体上；地面站部分包括vr头显、力反馈装置和地面站主机，机载电脑将从地面站主机接收到的飞行控制指令以及机械臂控制指令发送至控制系统。本发明通过vr感知及显示技术，以第一视角实时观察到无人机前方的立体场景，更为精准地观察、定位目标物体，降低了后续机械臂控制的难度。采用力反馈设备控制机械臂运动，运用飞行机械臂抓取技术，实时、精确的调整机械臂位置。该发明通过vr设备人为的提供反馈，无法进行自主反馈抓取目标物。本发明未涉及飞行机械臂视觉伺服抓取问题。

《一种基于动态重心补偿的旋翼飞行机械臂系统及算法》(公开号：cn108248845a)。该系统包括旋翼飞行平台、图像传感器、连接架、机械臂系统、系统控制器和地面站控制装置；所述旋翼飞行平台包括旋翼飞行器和飞行控制器；所述图像传感器安装在所述旋翼飞行平台的前下方；所述连接架是一块机械板，用于固定连接所述旋翼飞行平台、图像传感器和机械臂系统；所述机械臂系统安装在旋翼飞行平台的正下方；所述系统控制器安装在旋翼飞行平台的正上方；所述地面站控制装置与所述系统控制器之间采用无线方式进行通讯。该发明设计了一种重心补偿控制器，主要是针对机械臂对无人机的扰动问题。该发明并未涉及如何进行高效、准确的抓取问题。

技术实现要素：

为解决上述问题，本发明提出了一种基于自然特征的飞行机械臂视觉伺服抓取方法，通过前端自然特征点提取，被伺服的目标物无需任何的人工标记物，仅仅需要提前拍摄含有目标物的期望图像；后端视觉伺服解算采用基于图像的视觉伺服作为基础，通过机器人学的速度变换关系，可以将相机的伺服速度传递到无人机和机械臂进行速度控制。另外，通过将摄像头设置在机械臂上，并且在远距离时做无人机的视觉伺服任务，在近距离时做机械臂的视觉伺服任务，实现多任务的联合视觉伺服控制，能够进行高效、准确的抓取，具体发明内容如下：

一种基于自然特征的飞行机械臂视觉伺服抓取方法，其特征在于：包括无人机本体，在所述无人机本体上设置有机械臂底座，在所述机械臂底座上设置有机械臂；所述机械臂中设置多个旋转关节，每个所述旋转关节中设置一个舵机；在所述机械臂的末端设置有夹爪和摄像头；在所述无人机本体上设置有控制系统，所述控制系统控制所述无人机、机械臂和夹爪的运动，所述摄像头与所述控制系统电性连接；所述飞行机械臂视觉伺服抓取方法包括如下步骤：

步骤1：拍摄含有目标物的期望图像，人工框取目标物中任意矩形区域，记录下框取区域在期望图像角点的像素位置，用于寻找框取区域在实时图像的位置，区域中检测到的特征点记为期望特征点；

步骤2：所述摄像头抓捕实时图像，所述控制系统将所述摄像头抓捕到的实时图像进行特征点检测和匹配；

步骤3：通过实时图像与期望特征点的匹配，可以获得图像之间的单应矩阵变换关系，紧接着利用单应矩阵的仿射变换，可以求出期望图像的角点在实时图像中的角点位置；

步骤4：利用获取的实时角点位置与期望的角点位置信息作差，得到所述摄像头的伺服速度；

步骤:5：通过机器人学的速度变换关系，可以将所述摄像头的伺服速度传递到所述无人机和所述机械臂进行速度控制。

优选的，所述摄像头的中心轴与所述夹爪的中心轴平行。所以，所述夹爪的夹取更精确。

优选的，所述特征点采用orb特征点。

优选的，改进后的orb特征点算法如下：

步骤2.1：输入图像；

步骤2.2：高斯图像金字塔；

步骤2.3：四叉树网格划分；

步骤2.4：带方向fast角点提取；

步骤2.5：冗余角点剔除；

步骤2.6：特征点描述子。

优选的，所述机械臂能够提供俯仰角和偏航角。使得目标物始终在视野正中心。

优选的，所述机械臂包括第一关节、第二关节、第三关节、第四关节、第五关节和第六关节；所述机械臂的末端为第六关节，当无人机距离目标物的距离大于阈值时，只控制无人机的x、y、z轴方向的线速度、沿着z轴方向旋转的角速度以及机械臂第五关节的速度；当无人机距离目标物的距离小于阈值时，只控制机械臂的速度。多任务伺服，使机械臂抓取的范围更广泛，更精确。

优选的，所述步骤5中将所述摄像头的伺服速度传递到所述无人机和所述机械臂的计算过程如下：

飞行机械臂定义的惯性坐标系o、机体坐标系b、机械臂基座坐标系a、机械臂末端执行器坐标系e、相机光心坐标系c、目标物中心坐标系t、期望抓取位置坐标系d，o^b＝[xyz]^t和r^b表示无人机坐标系相对于惯性坐标系的位置和旋转，以欧拉角的形式表示无人机的方向，q＝[q1q2q3q4q5q6]^t表示机械臂各关节的关节角。接下来对飞行机械臂整体的微分运动学推导进行简要阐述，飞行机械臂中机械臂末端的速度等于无人机运动产生的速度和飞行机械臂运动产生的速度之和：

v^e＝v^e_a+v^e_b(1)

其中v^e表示机械臂末端最终速度，v^e_a表示机械臂运动产生的机械臂末端速度，v^e_b表示无人机运动产生的机械臂末端速度。

根据机械臂的微分运动学，可以得到：

其中j(q)表示机械臂的几何雅可比矩阵，表示机械臂正运动学求解得到的齐次矩阵中的旋转分量，表示各关节的角速度，j2表示化简后的变量。

机械臂末端执行器速度转到无人机坐标系下表示为：

其中表示无人机基座相对机械臂末端的旋转分量，表示机械表末端相对无人机基座的平移分量，s(*)表示向量转反对称矩阵，v^b是无人机的运动速度，j1表示化简后的变量。

将式(2)和式(3)进行合并得：

多旋翼无人机本身是一个欠驱动控制系统，可控量为三个自由度的线速度分量和沿着飞行器z轴的旋转角速度。剩下两个轴上旋转角速度通过飞行器自身的惯性测量装置获得作为稳定飞行的内部反馈环进行调节，因此式(4)可以写成：

v^e＝j^v＝jcovco+jucvuc(5)

其中juc为联合雅克比矩阵j中对应不可控分量的列向量，jco为联合雅克比矩阵j中去掉不可控部分的雅克比矩阵，为可控分量。

由于实际飞行过程中，多旋翼无人机始终保持平稳状态，即俯仰角和翻滚角接近于零，因此不可控部分几乎不对系统造成影响，式(5)可近似写成：

v^e＝j^v＝jcovco+jucvuc≈jcovco(6)

设定速度分配矩阵为w∈r^10*n,n取决于取决于需要控制量的个数，vu∈r^n*1为控制量的矢量，针对实际问题vco

提取对应的vu，式(6)可进一步写成：

v^e＝jcowvu＝juvu(7)

当无人机距离目标物的距离大于阈值时，式(7)中的速度分配矩阵可以写成：

当无人机距离目标物的距离小于阈值时，式(7)中的速度分配矩阵可以写成：

本发明的有益效果如下：

1、本发明针对视觉伺服在普通场景下无法使用人工特征问题，提出基于自然特征的视觉伺服抓取方法，传统视觉伺服需要贴附人工标记物，而实际应用中，受条件限制，无法提前贴好人工标记物，而自然特征的方法只需提前框取目标物区域，便可实现目标物的视觉伺服抓取。

2、本发明针对机械臂视觉伺服超行程问题，提出多任务的联合视觉伺服控制策略，当无人机距离目标物的距离大于阈值(该阈值可根据实际需求进行制定)时，通过速度分配矩阵将伺服的速度传递到无人机进行视觉伺服，为了保证摄像头始终能看到目标物，利用机械臂的关节进行俯仰角和偏航角伺服；当无人机进入机械臂的工作空间，仅对机械臂进行视觉伺服操作，使抓取更高效、准确。

本发明可以应用于远距离运输搬运任务，尤其可以应用在灾难救援现场或复杂地形环境等人们很难到达的地方，解决无人工标记的视觉伺服问题，因此其具有很大的应用价值。

附图说明

图1为本发明飞行机械臂结构图；

图2为本发明基于自然特征的视觉伺服算法的飞行机械臂抓取的算法框图；

图3为本发明改进orb特征点检测算法框图；

图4为本发明机械臂相关坐标系示意图。

图中各附图标记所指代的技术特征如下：

1、螺旋桨；2、螺旋桨电机；3、无人机本体；4、电池；5、机械臂底座；6、第一舵机；7、第二舵机；8、第三舵机；9、机械臂连接支架；10、第四舵机；11、第五舵机；12、第六舵机；13、摄像头；14、夹爪舵机；15、夹爪。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

如图1所示，飞行机械臂包括无人机本体3和机械臂，在所述无人机本体3上设置有机械臂底座5、若干均匀分布的螺旋桨1、控制螺旋桨旋转的螺旋桨电机2；所述机械臂底座5与无人机本体3之间形成一个容纳空间，所述电池4容纳在所述容纳空间内，所述机械臂固定安装在所述机械臂底座5的下表面，所述机械臂中设置多个旋转关节，每个旋转关节对应一个自由度，每个所述旋转关节设置一个舵机，在所述机械臂的末端设置有夹爪15和摄像头13，在所述机械臂上还设置有机械臂连接支架9，在所述无人机本体上设置有控制系统，所述控制系统控制所述无人机3、机械臂和夹爪15的运动，所述摄像头13与所述控制系统电性连接，所述机械臂通过所述旋转关节调节所述机械臂末端的夹爪15来夹持目标物。其中，所述摄像头13的中心轴与所述夹爪15的中心轴同轴，是为了能更准确、高效的抓取目标物。所述夹爪15的中心轴是指夹爪15的夹紧方向的轴线。所述摄像头为彩色摄像头。

所述机械臂具有六个自由度，每个自由度均由电机、减速齿轮组和编码器构成所述舵机。减速齿轮组安装在电机的输出轴上，所述减速齿轮组中设置多个相互啮合的齿轮，减速齿轮组中的出轴齿轮上设置用于连接舵机中的舵盘的旋转轴。所述舵机还包括舵机本体、舵机底座、副舵盘、主舵盘、设置在所述副舵盘和所述主舵盘之间沿所述舵机本体周向分布的舵机外侧壁，所述舵机本体设置在所述舵机底座上，所述副舵盘和主舵盘分别设置在所述舵机本体相对设置的顶壁和底壁的外侧。所述主舵盘和副舵盘设置相互对用的安装孔，所述安装孔用于固定传动连接件，从而使得副舵盘跟随主舵盘同步运动。

所述舵机包括第一舵机6、第二舵机7、第三舵机8、第四舵机10、第五舵机11、第六舵机12和夹爪舵机14，所述第一舵机6的副舵盘与所述机械臂底座5相连接，所述第一舵机6的主舵盘与所述第二舵机7的舵机外侧壁连接从而带动第二舵7转动。所述第二舵机7的主舵盘与延长臂杆的一端固定侧相连从而带动延长臂杆转动，所述延长臂的另一端与所述第三舵机8的舵机底座连接，延长臂杆转动的同时带动第三舵机8转动，所述第三舵8的主舵盘和副舵盘分别与两个互相平行的连杆的一端相连，两个互相平行的连杆的另一端分别固定在所述第四舵机10的舵机外侧壁上，通过连杆实现第三舵机8的主舵盘和副舵盘带动第四舵机10转动。所述第四舵机10的主舵盘与所述第五舵机11的舵机底座相连接从而带动第五舵机11转动，所述第五舵机11的主舵盘和副舵盘分别与两个互相平行的连杆的一端相连，两个互相平行的连杆的另一端与所述第六舵机12的舵机外侧壁相连，通过连杆实现第五舵机的主舵盘和副舵盘带动第六舵机12转动。每个舵机对应一个关节，所以，所述飞行臂包括第一关节、第二关节、第三关节、第四关节、第五关节以、第六关节以及夹爪关节。

本发明主要由基于自然特征的视觉伺服算法和多任务的联合视觉伺服策略两部分组成，下面就这两方面展开分别展开论述：

如图2所示，所述飞行机械臂视觉伺服抓取方法主要包括前端自然特征点提取和后端视觉伺服解算，主要包括如下步骤：

步骤1：拍摄含有目标物的期望图像，人工框取目标物中任意矩形区域，记录下框取区域在期望图像角点的像素位置，用于寻找框取区域在实时图像的位置，区域中检测到的特征点记为期望特征点；

步骤2：所述摄像头抓捕实时图像，所述控制系统将所述摄像头抓捕到的实时图像进行特征点检测和匹配；

步骤3：通过实时图像与期望特征点的匹配，可以获得图像之间的单应矩阵变换关系，紧接着利用单应矩阵的仿射变换，可以求出期望图像的角点在实时图像中的角点位置；

步骤4：利用获取的实时角点位置与期望的角点位置信息作差，得到所述摄像头的伺服速度；

步骤:5：通过机器人学的速度变换关系，可以将所述摄像头的伺服速度传递到所述无人机和所述机械臂进行速度控制。

所谓前端自然特征点的提取是指，被伺服的目标物无需任何的人工标记物，仅仅需要提前拍摄含有目标物的期望图像，人工框取目标物中任意矩形区域，记录下框取区域在期望图像角点的像素位置，用于寻找框取区域在实时图像的位置，区域中检测到的特征点记为期望特征点；通过彩色摄像头捕抓的图像，然后进行特征点检测和匹配，通过实时图像的与期望特征点的匹配，可以获得图像之间的单应矩阵变换关系，紧接着利用单应矩阵的仿射变换，可以求出期望图像的角点在实时图像中的角点位置。

后端视觉伺服解算采用基于图像的视觉伺服作为基础，利用前端获取的实时角点位置与期望的角点位置信息作差，作为基于图像的视觉伺服的前端输入，最终可以得到相机的伺服速度。通过机器人学的速度变换关系，可以将相机的伺服速度传递到无人机3和机械臂进行速度控制。

上述特征点采用orb特征点，对其进行改进，算法流程图见图3，引入高斯图像金字塔，增加特征点的尺度不变性，由于特征点分布不均与，特征点聚集在前景物体上，不利于特征点匹配，因此引入四叉树结构网格化提取特征点，通过降低阈值来保证图像的每个小方格区域都有特征点被检测到。

多任务的联合视觉伺服策略如下：

如图4所示，飞行机械臂定义的惯性坐标系o、机体坐标系b、机械臂基座坐标系a、机械臂末端执行器坐标系e、相机光心坐标系c、目标物中心坐标系t、期望抓取位置坐标系d，o^b＝[xyz]^t和r^b表示无人机坐标系相对于惯性坐标系的位置和旋转，以欧拉角的形式表示无人机的方向，q＝[q1q2q3q4q5q6]^t表示机械臂各关节的关节角。接下来对飞行机械臂整体的微分运动学推导进行简要阐述，飞行机械臂中机械臂末端的速度等于无人机运动产生的速度和飞行机械臂运动产生的速度之和：

v^e＝v^e_a+v^e_b(1)

其中v^e表示机械臂末端最终速度，v^e_a表示机械臂运动产生的机械臂末端速度，v^e_b表示无人机运动产生的机械臂末端速度。

根据机械臂的微分运动学，可以得到：

其中j(q)表示机械臂的几何雅可比矩阵，表示机械臂正运动学求解得到的齐次矩阵中的旋转分量，表示各关节的角速度，j2表示化简后的变量。

机械臂末端执行器速度转到无人机坐标系下表示为：

其中表示无人机基座相对机械臂末端的旋转分量，表示机械表末端相对无人机基座的平移分量，s(*)表示向量转反对称矩阵，v^b是无人机的运动速度，j1表示化简后的变量。

将式(2)和式(3)进行合并得：

多旋翼无人机本身是一个欠驱动控制系统，可控量为三个自由度的线速度分量和沿着飞行器z轴的旋转角速度。剩下两个轴上旋转角速度通过飞行器自身的惯性测量装置获得作为稳定飞行的内部反馈环进行调节，因此式(4)可以写成：

v^e＝j^v＝jcovco+jucvuc(5)

其中juc为联合雅克比矩阵j中对应不可控分量的列向量，jco为联合雅克比矩阵j中去掉不可控部分的雅克比矩阵，为可控分量。

由于实际飞行过程中，多旋翼无人机始终保持平稳状态，即俯仰角和翻滚角接近于零，因此不可控部分几乎不对系统造成影响，式(5)可近似写成：

v^e＝j^v＝jcovco+jucvuc≈jcovco(6)

机械臂是固定在无人机的正下方，两者的运动对双方有着力和力矩的干扰，如果采用无人机悬停在机械臂的可操作范围内进行视觉伺服抓取，可以很大程度的减少相互之间干扰问题，但是在远距离机械臂超出工作空间时，机械臂无法抓取，实际任务中也不可能刚好飞行机械臂落在可抓取范围内，因此本专利需要将视觉伺服的运动传递到无人机，在远距离时做无人机的视觉伺服任务。摄像头安装在机械臂末端，随着无人机伺服靠近目标物时，目标物会逐渐脱离相机的视野，如果机械臂提供俯仰角和偏航角实时伺服目标物，则能够保证目标物始终在视野正中心。因此本专利提出速度分配率实现多任务的联合视觉伺服控制，设定速度分配矩阵为w∈r^10*n,n取决于取决于需要控制量的个数，vu∈r^n*1为控制量的矢量，针对实际问题vco

提取对应的vu，式(6)可进一步写成：

v^e＝jcowvu＝juvu(7)

将飞行机械臂的视觉伺服过程分为远距离和近距离。远距离时机械臂和无人机可以同时作用，但是机械臂的大幅度运动及无人机重心偏移会导致无人机飞行不稳定，因此本专利采用无人机主导的视觉伺服，无人机的x、y、z轴方向的线速度、沿着z轴方向旋转的角速度以及机械臂第五个关节的速度，式(7)中的速度分配矩阵可以写成：

近距离为无人机进入机械臂的可行工作空间，无人机悬停，仅机械臂运动，速度分配矩阵可以写成：

以上对本发明所提供的一种基于自然特征的飞行机械臂视觉伺服抓取方法实施例进行了详细阐述。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明的原理的前提下，还可以本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。