一种旋翼飞行双机械臂目标定位抓取系统及方法与流程

本发明属于无人机技术领域，尤其涉及一种旋翼飞行双机械臂目标定位抓取系统及方法。

背景技术：

目前，业内常用的现有技术是这样的：

随着无人机技术得到快速发展，多旋翼无人机也开始走进人们的视野，多旋翼无人机有着垂直起降和悬停的能力，操作简单、体积小巧、飞行灵活，所以其在航拍检测、农林、安全执法、电网巡检、边防巡逻等领域得到了越来越广泛的应用。但这些都仅仅停留在观测的层面，并未有效的与环境互动，对于危险物品排查及运输、灾难救援、快速补给等一些危险或者特殊任务，现有无人机很难做到。因此，在旋翼无人机上安装灵活的机械手臂和“眼睛”(视觉传感器)使得其具备与环境相互作用的能力，极大的拓展了其应用领域，具备应用价值。

2017年，包，仓等人在发明专利《一种带有机械臂的飞行机器人及其控制方法》(公开号：cn107309872a)中公开了一种机械臂的空中作业控制方法，机械臂安装在无人机的下方。该发明专利虽然实现了旋翼无人机与机械臂的结合，解决旋翼无人机与环境交互的问题，但是其所述的系统机械臂为单臂系统，且自由度少，在抓取时单臂容易引起无人机不稳定，且单臂抓取局限性大，应用范围受到限制。

综上所述，现有技术存在的问题是：

(1)现有技术中，旋翼飞行机械臂均为单臂系统，难以稳定重心，自由度较少，且抓取对象容易受到限制；

(2)现有技术中，大多旋翼飞行机械臂系统未配备多视觉传感器，不能实时与环境进行交互，大都为人工控制抓取，未形成基于视觉和机械臂规划的识别与抓取系统；

(3)现有技术中，旋翼飞行机械臂系统未有采用深度卷积网络对环境中目标进行检测识别，因此大多都是识别对象单一，不具备识别多样性，且识别精度低，鲁棒性差。

(4)现有技术中，旋翼飞行机械臂系统在硬件上均未设计电源与通信集中模块，在硬件软件连接与调试过程中，存在大量繁琐的线路连接操作，且难以安装，极易产生接线失误导致重要模块损坏，同时可能导致重心不稳。

旋翼飞行机械臂系统，由于机械臂的抓取运动会使得飞行器存在重心不稳的问题，且要使得旋翼飞行机械臂实现与环境的自主交互，则必须配备有先进的多视觉传感器和先进的环境感知和目标识别定位算法，以及改良的机械臂系统和运动规划算法，以此提高旋翼飞行机械臂系统的自主性、稳定性及鲁棒性，使得旋翼飞行机械臂系统有更加广泛的实际应用，具备现实价值。

技术实现要素：

针对现有技术存在的问题，本发明提供了一种旋翼飞行双机械臂目标定位抓取系统及方法。

本发明是这样实现的，一种旋翼飞行双机械臂目标定位抓取系统，包括旋翼飞行器，双机械臂系统，电源与通信集成电路模块，机载计算机，飞行控制器，视觉传感器；

所述旋翼飞行器机架为四或六旋翼，包括机架平台、旋翼飞行器机架及与旋翼飞行器机架相连的四个或六个机臂；

所述旋翼飞行器机架还包括飞行平台动力系统：四或六组或无刷电机、无刷电调以及旋翼桨，每组电机安装在机架机臂上且与其对应的电调连接，旋翼桨安装在对应的电机上；gps定位模块，为无人机提供定位服务；

双机械臂系统安装在旋翼飞行器机架的下方定制固定板上；

自制电源与通信集成电路模块通过四个支撑棒安装在旋翼飞行器机架正上方，其为所有的硬件模块供电、实现模块之间相互通信；

机载计算机和飞行控制器安装在机架与自制集成电路模块中间位置，所有的软件算法程序在机载计算机中运行；

飞行控制器通过通信模块给所有飞行器硬件模块发送任务指令；

视觉传感器安装在机架前下端边缘位置，实时获取环境rgb视频流。

本发明所述的双机械臂系统由两个单机械臂组合而成，是由两个4自由度的机械臂构成的机械臂系统，且两个单机械臂在机械结构上完全一致。所述双机械臂系统，安装在旋翼飞行器下方定制固定板上，两个单机械臂平行安装在固定板左右两端；

其中，每一个单机械臂的机械结构包括一个左右转向机构、两个上下收放机构以及一个夹持机构，4个自由度，共有4个舵机，舵机与舵机之间通过连接件连接，第一个舵机连接在固定板上，第四个舵机与机械爪子连接组成夹持机构。

本发明的进一步改进，所述电源与通信集成电路模块包括：11v直流稳压电路给飞行器自动收放脚架供电，电调电机及信号接口服务于飞行平台动力系统，5vubec稳压电路和电量检测电路服务于飞行控制系统，19v直流稳压电路和ft4232通信电路服务于机载计算机平台，12v直流稳压电路给双机械臂系统供电，飞行控制系统mavlink协议通过ft4232通信电路及uart与机载计算机建立通信连接，双机械臂通过dynamixelprotocol2.0协议和ft4232通信电路与机载计算机建立通信。

所述的目标定位抓取方法包括以下步骤：

步骤1：通过视觉传感器实时获取环境视频流信息，采用mobilenet-ssd网络识别目标，锁定待抓取目标物；

步骤2：根据步骤1锁定抓取目标后，采用图像传感器获取目标的点云3d数据和apriltag视觉定位切换的分段方法,精确获取目标的三维坐标；

步骤3：根据步骤2中的三维信息，引入fcl碰撞检测机制，根据末端位姿信息进行机械臂逆向运动学运算，采用rrt_connect路径规划算法得到双臂运动轨迹，实现协同规划抓取。

进一步地，步骤1中mobilenet-ssd训练网络实现待抓取目标检测，步骤如下：收集待抓取物体的数据集，并对数据集进行初始化处理，标注数据；划分训练集和测试集，制作lmdb文件；通过mobilenet-ssd训练模型，输出训练文件；通过视觉传感器实时获取环境视频流信息，在机载计算机上运行上述训练模型识别并锁定待抓取目标。

进一步地，步骤二中获取目标3d坐标，先采用深度传感器获取目标3d信息，当飞行器运动至接近目标物时切换至apriltag视觉定位获取3d信息，其距离阈值为1(米)，步骤2所采用图像传感器和apriltag视觉定位切换分段的具体步骤如下：

(1)视觉传感器实时获取视频流，通过步骤2方法确定目标并给定目标框；

(2)通过视觉传感器获取目标rgb图像，获取目标像素坐标(x,y)，得到深度图像，ir图像，从而得到目标深度值z；

(3)基于棋盘格，采用张正友标定法对视觉传感器进行标定，获取其内参矩阵c和外参矩阵t；

(4)基于相机参数，根据相机坐标系和图像坐标系之间的转换关系得到图像中目标相对于相机坐标系的三维坐标，坐标转换关系为：

式中，(x,y)表示图像像素坐标系；(x,y,z)表示相机坐标系；c表示相机内参矩阵；t表示相机外部参数矩阵。

(5)深度相机获取的三维信息传递给飞行器，旋翼飞行器不断飞行运动至接近目标，设置循环，判断距离是否满足z≤1(单位：米)，若否，则继续使用该图像传感器获取3d坐标；若是，则转至第(6)步采用apriltag视觉定位方法获取目标更精确3d坐标；

(6)通过图像传感器，获取图像特征，计算像素梯度得到幅值图像，得到梯度方向；

(7)将相似的梯度方向和幅值集群到一个组件，即每个节点代表一个像素，使用加权最小二乘法，提取场景中的直线，将方形区域同态映射为正方形，与tag库进行匹配，取0-15号tag标签作为实验对象，大小君取为10cm和15cm两种，调用apriltag视觉库得到目标精确3d坐标。

(8)将获取的3d坐标实时通过权力要求1中所述的电源与通信集成电路模块传送给飞行控制器和机载计算机。

其中：当旋翼无人机在空中搜索目标对象时，此时目标与无人机距离较远，而深度相机的深度测距范围适合在1m-10m之间，所以此时选取深度相机获取目标3d坐标，当无人机接近目标时且在1m内时，采用apriltag视觉库获取目标3d坐标。

进一步地，步骤三中引入fcl碰撞检测机制，根据末端位姿信息进行机械臂逆向运动学运算，采用rrt_connect路径规划算法得到双臂运动轨迹，实现协同规划抓取的实现步骤如下：

(1)创建机械臂规划群，建立4自由度机械臂的部位连接关系，建立关节空间笛卡尔坐标系；

(2)创建机器人目标位姿，将获取的3d坐标即机械臂末端位姿信息，作为机械臂系统的输入；

(3)把终端位姿作为输入，采用rrt_connect算法采样输出关节空间集合；

(4)引入fcl碰撞检测机制，结合输出关节空间集合，得到一条不与环境中任何障碍碰撞的最优轨迹。

本发明的另一目的在于提供一种旋翼飞行双机械臂目标定位抓取计算机程序，所述旋翼飞行双机械臂目标定位抓取计算机程序实现所述的旋翼飞行双机械臂目标定位抓取方法。

本发明的另一目的在于提供一种终端，所述终端至少搭载实现所述旋翼飞行双机械臂目标定位抓取方法的控制器。

本发明的另一目的在于提供一种计算机可读存储介质，包括指令，当其在计算机上运行时，使得计算机执行所述的旋翼飞行双机械臂目标定位抓取方法。

综上所述，本发明的优点及积极效果为：

本发明设计为多旋翼带双机械臂系统，双机械臂对称置于无人机下方，具有更好的稳定性，双臂的应用前景也更为广泛。

本发明利用较先进的深度学习、计算机视觉算法和机械臂规划算法，形成了从目标识别，精确视觉定位到机械臂轨迹规划抓取的完整方法与系统，具有实际意义。

本发明设计了电源与通信集成电路模块，进一步保障无人机各个硬件模块之间的通信与供电，提升无人机整体可靠性。

为了更好地说明本发明方法的技术有效性，将该专利所使用算法进行数据验证。在目标识别算法中以键盘、背包、椅子等物品作为深度网络训练集，测试得到目标检测识别平均精度均值为71.3％map，检测识别速度约38帧，达到实时检测效果，且检测识别类别不限，样本大小不限，相比于现有技术中的特征点匹配检测等算法有较大的优势；

在目标定位算法上，经过测试，深度传感器三维定位适用于距离约1-10m有效范围，该范围内的定位精度约为0.10m,属于粗略定位，因此当距离范围处于1m内时，切换至apriltag精确定位，其定位精度约为0.03m，在误差允许范围之内，此类定位方法有效，相比于目前飞行器常用的单一视觉定位，定位更灵活精确，鲁棒性更高；

在机械臂运动规划算法上，经过测试，其平均规划即算法解算时长约为3.5-4.5s，规划算法求解得到的可行解与目标位置的误差不超过0.015m，从开始规划解算到机械臂运动规划至目标位置实施抓取共计时长一般不超过6s，其中，机械臂运动速度可根据实际情况进行微调，且双机械臂几乎为同时规划，其解算精度在可接受范围之内，具有较好的技术效果。

附图说明

图1是本发明实施例提供的旋翼飞行双机械臂目标定位抓取系统示意图。

图中：1、旋翼飞行器机架；2、双机械臂系统；3、飞行控制器；4、自制电源与通信集成电路模块；5、机载计算机；6、视觉传感器。

图2是本发明实施例提供的双机械臂系统结构示意图。

图3是本发明实施例提供的系统电源与通信集成电路模块的详细连接关系图。

图4是本发明实施例提供的图3中11v直流稳压电路图。

图5是本发明实施例提供的图3中电调分电机及信号接口图。

图6是本发明实施例提供的图3中5vubec稳压电路图。

图7是本发明实施例提供的图3中电量监测电路图。

图8是本发明实施例提供的图3中19v直流稳压电路图。

图9是本发明实施例提供的图3中12v直流稳压电路图。

图10是本发明实施例提供的系统总体实现原理图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

现有技术中，旋翼飞行机械臂均为单臂系统，难以稳定重心，且抓取对象容易受到限制；大多旋翼飞行机械臂系统未配备多视觉传感器，不能实时与环境进行环境，大都为人工控制抓取，未形成基于视觉和机械臂规划的识别与抓取系统。

下面结合具体分析对本发明的应用坐进一步描述。

如图1所示，本发明实施例提供的旋翼飞行双机械臂目标定位抓取系统，包括：旋翼飞行器机架1、双机械臂系统2、飞行控制器3、自制电源与通信集成电路模块4、机载计算机5、视觉传感器6；

旋翼飞行器机架1，在机架的下方安装有双机械臂系统，另外电源与通信集成电路模块、机载计算机、飞行控制器安装在机架上方，按照电源与通信集成电路4在上，机载计算机5在中，飞行控制器3在下进行安装，视觉传感器安装在机架下方边缘位置。

所述旋翼飞行器机架1为四(或六)旋翼，包括机架平台：飞行器机架及与机架相连的四个或六个机臂；飞行平台动力系统：四(六)组或无刷电机、无刷电调以及旋翼桨，每组电机安装在机架机臂上且与其对应的电调连接，旋翼桨安装在对应的电机上；gps定位模块，为无人机提供定位服务。

双机械臂系统2安装在旋翼飞行器机架的下方定制固定板上；

自制电源与通信集成电路模块通过四个支撑棒安装在旋翼飞行器机架正上方，其为所有的硬件模块供电、实现模块之间相互通信；

机载计算机和飞行控制器安装在机架与自制集成电路模块中间位置，所有的软件算法程序在机载计算机中运行；

飞行控制器通过通信模块给所有飞行器硬件模块发送任务指令；

视觉传感器realsenser2006安装在机架前下端边缘位置，实时获取环境rgb视频流。

如图2所示双机械臂系统有两个单机械臂组合而成，是由两个是由两个4自由度的机械臂构成的双机械臂系统，且两个单机械臂在机械结构上完全一致。所述双机械臂系统，安装在旋翼飞行器下方定制固定板上，两个单机械臂平行安装在固定板左右两端，其中，每一个单机械臂的机械结构包括一个左右转向机构、两个上下收放机构以及一个夹持机构，4个自由度，共有4个舵机，舵机与舵机之间通过连接件连接，第一个舵机连接在固定板上，第四个舵机与机械爪子连接组成夹持机构。

如图3所示电源与通信集成电路模块连接关系，其中，图4表示11v直流稳压电路给飞行器自动收放脚架供电，图5表示电调电机及信号接口电路服务于飞行平台动力系统，图6表示5vubec稳压电路和图7表示的电量检测电路服务于飞行控制系统，图8中的19v直流稳压电路配合ft4232通信电路服务于机载计算机平台，图9中的12v直流稳压电路给双机械臂系统供电，飞行控制系统mavlink协议通过ft4232通信电路及uart与机载计算机建立通信连接，双机械臂通过dynamixelprotocol2.0协议和ft4232通信电路与机载计算机建立通信。

如图10所示为一种旋翼飞行双机械臂目标定位抓取系统硬件和软件结合的框架，其中本发明所述的目标定位抓取方法包括以下步骤：

步骤1：通过视觉传感器实时获取环境视频流信息，采用mobilenet-ssd网络识别目标，锁定待抓取目标物；

步骤2：根据步骤1锁定抓取目标后，采用图像传感器获取目标的点云3d数据和apriltag视觉定位切换的分段方法,精确获取目标的三维坐标；

步骤3：根据步骤2中的三维信息，引入fcl碰撞检测机制，根据末端位姿信息进行机械臂逆向运动学运算，采用rrt_connect路径规划算法得到双臂运动轨迹，实现协同规划抓取。

其中步骤1，mobilenet-ssd训练网络实现待抓取目标检测，步骤如下：收集待抓取物体的数据集，并对数据集进行初始化处理，标注数据；划分训练集和测试集，制作lmdb文件；通过mobilenet-ssd训练模型，输出训练文件；通过视觉传感器实时获取环境视频流信息，在机载计算机上运行上述训练模型识别并锁定待抓取目标。

以键盘、书籍、鼠标、手机等物品为例测试本发明中目标检测识别网络，检测识别结果中可以看出键盘可信度为98％，鼠标可信度为80％，另外可测试得到目标检测识别平均精度均值为71.3％map，检测识别速度约38帧，达到实时检测效果，且检测识别类别不限，样本大小不限，相比于现有技术中的特征点匹配检测等算法有较大的优势。

步骤二中获取目标3d坐标，先采用深度传感器获取目标3d信息，当飞行器运动至接近目标物时切换至示apriltag视觉定位获取3d信息，其距离阈值为1(米)，步骤2所采用图像传感器和apriltag视觉定位切换分段的具体步骤如下：

(1)视觉传感器实时获取视频流，通过步骤2方法确定目标并给定目标框；

(2)通过视觉传感器获取目标rgb图像，获取目标像素坐标(x,y)，得到深度图像，ir图像，从而得到目标深度值z。

其中一个仿真中，简单检测识别书本并实时获取其目标中心某像素点三维位置信息反馈显示，图中右边显示框中三个值分别(x,y,z)，即目标位置相对于摄像传感器的坐标。

(3)基于棋盘格，采用张正友标定法对视觉传感器进行标定，获取其内参矩阵c和外参矩阵t；

(4)基于相机参数，根据相机坐标系和图像坐标系之间的转换关系得到图像中目标相对于相机坐标系的三维坐标，坐标转换关系为：

式中，(x,y)表示图像像素坐标系；(x,y,z)表示相机坐标系；c表示相机内参矩阵；t表示相机外部参数矩阵。

(5)深度相机获取的三维信息传递给飞行器，旋翼飞行器不断飞行运动至接近目标，设置循环，判断距离是否满足z≤1(单位：米)，若否，则继续使用该图像传感器获取3d坐标；若是，则转至第(6)步采用apriltag视觉定位方法获取目标更精确3d坐标。

(6)通过图像传感器，获取图像特征，计算像素梯度得到幅值图像，得到梯度方向；

(7)将相似的梯度方向和幅值集群到一个组件，即每个节点代表一个像素，使用加权最小二乘法，提取场景中的直线，将方形区域同态映射为正方形，与tag库进行匹配，取0-15号tag标签作为实验对象，大小都取为10cm和15cm两种，调用apriltag视觉库得到目标精确3d坐标。

(8)将获取的3d坐标实时通过权力要求1中所述的电源与通信集成电路模块传送给飞行控制器和机载计算机。

其中：当旋翼无人机在空中搜索目标对象时，此时目标与无人机距离较远，而深度相机的深度测距范围适合在1m-20m之间，所以此时选取深度相机获取目标3d坐标，当无人机接近目标时且在1m内时，采用apriltag视觉库获取目标3d坐标。

步骤三中引入fcl碰撞检测机制，根据末端位姿信息进行机械臂逆向运动学运算，采用rrt_connect路径规划算法得到双臂运动轨迹，实现协同规划抓取的实现步骤如下：

(1)创建机械臂规划群，建立4自由度机械臂的部位连接关系，建立关节空间笛卡尔坐标系，创建urdf文件；

(2)中间步骤部分，创建机器人目标位姿，将获取的3d坐标即机械臂末端位姿信息，作为机械臂系统的输入；

(3)把终端位姿作为输入，采用rrt_connect算法采样输出关节空间集合；

(4)引入fcl碰撞检测机制，结合输出关节空间集合，得到一条不与环境中任何障碍碰撞的最优轨迹，实现运动规划。

为证明与解释相关步骤中的细节，特举例说明。

首先创建urdf文件，该文件中定义关节与关节之间的关系，并且建立机械臂的基准坐标系，以及各个关节初始化位置，例如在urdf文件中定义base_link关节的初始化坐标为(0.003889,0.029746,-0.075699)，末端位姿初始状态四元数表示为(0,1,0,0)。

打开moveit配置助手，创建碰撞免检矩阵、创建规划群，创建虚拟关节等，最后生成仿真环境。

取待抓取目标位置坐标为(0.06,0.21,0.75)(单位：m)，然后启动程序开始运行本发明专利机械臂运动规划方法，经过约3s的规划机械臂生成从初始位置到末端位置的一系列关节运动信息，得到规划所示的仿真结果。

列举一个规划结果中关节的运动信息，用数据表示：

time_from_start:positions:[-0.00010606236812165936,0.10214788717763565,0.07238222171577066]，

velocities:[-0.0003995765630509095,0.384829250979326,0.27269086945146415]，

accelerations:[-0.0010391832412044797,1.0008297415883136,0.7091901972422131]。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用全部或部分地以计算机程序产品的形式实现，所述计算机程序产品包括一个或多个计算机指令。在计算机上加载或执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(dsl)或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输)。所述计算机可读取存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，dvd)、或者半导体介质(例如固态硬盘solidstatedisk(ssd))等。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。