一种利用单目视觉和机载传感器旋翼无人机自主抓取方法与流程

本发明涉及旋翼无人机的技术领域，更具体地，涉及一种利用单目视觉和机载传感器旋翼无人机自主抓取方法。

背景技术：

旋翼飞行器由于其相对固定翼飞行器具有可垂直起落、低空悬停、机动转向快速的优点，目前已经广泛应用在军事、农业、航空拍摄等领域，而旋翼飞行器对一般物体进行自主抓取等一系列和物理世界实现自主交互的技术近年来已成为研究的热点，这类技术在利用旋翼飞行器进行高空作业，物品配送等方面具有广阔的应用前景。

旋翼飞行器自主抓取系统主要通过设计飞行器的视觉检测算法，定位算法，飞行器和机载机械臂控制算法以及相应的抓取策略来实现，根据视觉算法和定位算法检测得到目标物体相对于飞行器的位置，结合抓取策略，调整云台摄像机的角度以及飞行器的飞行方向和速度，机械臂各个关节的转动角度进而使飞行器能有效的实现自主抓取一般物体。

国内外众多研究院所，高校也同样在旋翼飞行器自主抓取方面做了大量的探索。目前的技术方案主要集中于人工远程遥控抓取和基于特定物体的视觉伺服的自主抓取。前者需要通过人工调整来改变飞行器及机械臂的状态，主要研究的是飞行器与负载结合的动力学模型，力求在任务执行的过程中保持飞行器的稳定，继而实现整个抓取过程，这种技术方案相对容易实现，但在实际操作中仍需要经验丰富的操作人员。后者主要是将视觉检测，视觉定位和控制算法结合起来，能够在带有标识的特定物体和特定环境的情况下的实现快速、高效地抓取，但是该方案实现的前提条件比较严格，在现实中场景中难以灵活运用。

技术实现要素：

本发明为克服上述现有技术所述的至少一种缺陷，提供一种利用单目视觉和机载传感器旋翼无人机自主抓取方法，其将机器学习方法引入到系统的视觉模块，降低视觉伺服方案中对特定物体，特定环境的依赖程度的方法。同时提供了将检测，定位和控制集于一体的决策策略，从而提高旋翼飞行器实现完全自主的物体抓取任务的成功率。

本发明的技术方案是：一种利用单目视觉和机载传感器旋翼无人机自主抓取方法，其中，包括以下步骤：

S1.飞行器起飞，通过云台摄像机回传的图像，飞行器在预设高度检测视野范围内检测被抓取目标在图像中的位置；

S2.若飞行器检测到目标，飞行器进入抵近阶段；飞行器终止目标检测模块，启用视觉跟踪模块持续跟踪目标，同时根据机载惯性测量单元，超声波，气压计等传感器回传的信息，实时计算飞行器和目标之间的相对位置，控制飞行器向目标所在位置运动；

S3.当飞行器抵近目标，飞行器进入抓取阶段；飞行器维持悬停状态，同时启用抓取点检测模块，实时检测被抓取目标合适的抓取位置，计算抓取位置和飞行器之间的相对距离，控制机械臂实施抓取任务；

S4.飞行器完成抓取任务，终止视觉跟踪模块和抓取点检测模块，飞行器上升至安全高度，完成自主抓取任务。

所述的目标检测和跟踪分别采用Edge Boxes和KCF算法。

与现有技术相比，有益效果是：本发明所提出的方法计算量小，实时性高，可在机载嵌入式系统TK1实现。通过云台摄像头采集图像，使用Edge Boxes算法从图像中检测识别出目标并使用KCF算法对目标进行持续跟踪。结合机载传感器和图像坐标计算得到目标的位置，根据位置信息将无人机自主抓取问题分成抵近阶段和抓取阶段，并设计相应的无人机飞行控制方案。在抓取阶段，设计了一种利用预训练的模板检测目标合适的抓取点的方法，同时设计相应的机械臂控制方案。

(1)结合机载传感器和图像坐标计算得到目标的位置，根据位置信息将无人机自主抓取问题分成抵近阶段和抓取阶段，并设计相应的无人机飞行控制方案。当无人机离目标较远时进入抵近阶段，并为抵近阶段设计了相应的非线性上层控制算法，使无人机朝向目标移动。

(2)在抓取阶段，使用机器学习的方法设计了一种基于物体边缘和根模板匹配的抓取点检测方法，并利用该方法在抓取阶段检测目标合适的抓取位置，同时设计了相应的机械臂控制方案控制机械臂到达抓取位置，从而提高抓取的成功率。

附图说明

图1为本发明实施例中的一种基于单目视觉的旋翼无人机自主抓取方法的系统流程示意图。

图2为本发明实施例中的系统模块结构示意图。

图3为本发明实施例中的飞行器外观结构示意图。

图4为本发明实施例中的系统抓取策略示意图。

图5为本发明实施例中的飞行器与被抓取目标之间的相对位置关系示意图。

图6为本发明实施例中的飞行器所挂载的机械臂结构示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

1、被抓取目标的检测及跟踪。

目标检测和跟踪分别采用Edge Boxes和KCF算法。首先，飞行搭载的云台摄像机首先获取到实时图像，Edge Boxes算法是典型非机器学习图像处理算法，用于将图像中可能存在物体的前景区域框选出来，在实际应用中可保留最高得分的备选框作为目标在当前图像中的位置。KCF算法则是基于机器学习的视觉跟踪算法，用于将Edge Boxes算法框选出的区域进行跟踪，从而得到目标在连续多帧图像中的位置(u,v)。当飞行器成功检测并跟踪到目标物体后，根据图4的抓取策略示意图，飞行器根据目标的相对位置分别进入抵近阶段和抓取阶段。

2、抵近阶段的飞行器的控制和目标和飞行器的相对位置估计

在抵近阶段，基于抓取目标在机体坐标系下高度不变的假设，目标和飞行器之间的相对位置可简化到二维平面，目标和飞行器之间的位置关系如图5所示。其中，飞行器和机载云台摄像机在世界坐标系下的偏航角，滚转角和俯仰角可通过其搭载的IMU传感器得到。由于需要得到的是机体坐标系下目标和飞行器之间的相对位置，因而需要做以下的坐标变换，用Rgp表示从云台坐标系到机体坐标系的转换矩阵，使用K表示云台摄像头的内参矩阵，A＝[u v 1]表示抓取目标在二维图像中的位置，则抓取目标在机体坐标系下的位置T＝[xb yb zb]可表示为：

其中P为如下置换矩阵：

飞行器的实际高度h可由飞行器搭载的超声波传感器得到，则抓取目标与飞行器之间的实际距离为：

z＝0

在得到计算抓取目标和飞行器之间的距离之后，飞行器在抵近目标时可以利用已知的各项信息：飞行器与抓取目标的直线距离εd，飞行器当前速度和期望速度的误差εv，飞行器当前偏航角和期望偏航角的误差εσ，飞行器的当前速度vxc和vyc，得到飞行器x，y方向的期望速度vpx和vpy，飞行器的期望角速度ωp：

ωp＝k2εσ

其中k1和k2为小于0的常数，该计算公式根据不断获取得到的相对位置和速度信息计算飞行器在某个时刻的期望飞行速度，并将该期望速度传输给飞行控制系统，使飞行器响应该期望速度。飞行器采用以上控制方法可以满足李雅普诺夫稳定判据，系统是渐进稳定的。由于引入了速度误差，距离误差，飞行器对期望速度的响应将更加准确。

3、抓取阶段的目标抓取点检测和机械臂的控制

当飞行器抵近抓取目标时，飞行器进入抓取阶段。为提高抓取成功率，而不是让飞行器盲目抓取物体，本发明从仿生视角出发，提出了一种基于物体边缘和根模板匹配的抓取点检测方法。人类在拾取一般物体的时候，通常都是先观察物体的轮廓，边缘的特征，根据这些信息以及基于生活常识的先验知识判断抓取位置，进而用手或是其它工具进行抓取。在此基础上，本发明预先采集并训练多种飞行器可承载的物体的抓取点样本根模板(root)及其边缘模板(side)，在实际实施中使用以下步骤检测目标抓取点：

(1)使用边缘模板检测其边缘特征G(t)，t表示被抓取的目标。对检测出的边缘进行筛选组合，得到抓取位置集合{z|z∈combination(G(t))}并使用以下评估函数剔除不合理的组合方式。

公式表示被抓取目标的第k个位置zk左右两侧的边缘模板匹配得分减去两侧边缘的形心(xm,ym)，(xn,yn)之间的偏移量。考虑到运算的实时性，每个目标只保留最多5个得分最高的边缘组合方式。

(2)将剩余的组合方式使用以下根模板匹配公式进行匹配检测，选择匹配得分最高的边缘组合及其在原图像的位置作为抓取目标的抓取点的位置，并使用步骤2的方法计算得到抓取位置和抓取装置(机械臂)的相对距离。

scorer(zk)＝max(R(zk))

采用上述的抓取点检测方法可以使飞行器自主判断合理的抓取位置，同时在边缘检测的方法中避免一般边缘检测算法经常出现的阈值问题，即检测的结果受限于阈值的选取，从而提高算法的泛用性。

得到抓取位置和飞行器的相对位置后，根据图6所示的三自由度机械臂结构，假设被抓取目标的抓取点的位置在机体坐标系表示为B＝[x y z]，则可使用以下控制方式计算机械臂的三个关节J1，J2，J3的转动角度来抓取目标物体：

其中L1、L2和L3分别为三段机械臂的臂长，为关节J1到关节J2的直线距离，为关节J2到机械臂末端抓取装置的直线距离，h为飞行器高度。

本发明将非机器学习方法和机器学习方法融合并应用于目标检测、跟踪和抓取点检测中，降低了已有技术方案中对特定环境、特定物体的依赖程度；同时，在抓取任务执行的不同阶段采用不同的策略和控制方法，为视觉检测模块提供良好的感知环境，提高飞行器自主检测的鲁棒性和准确率，从而提高飞行器自主抓取目标物体的成功率。所提出的方法计算量小，实时性高，可在机载嵌入式系统TK1实现。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。