一种面向高危环境的智能无人机抓取目标的系统和方法与流程

本发明属于图像处理与无人机技术领域，涉及高危环境下无人机的定位导航和目标的检测定位与精准抓取技术。

背景技术：

这些年来，随着旅游业的迅猛发展，景区的卫生清理工作关系到景区经济效益、环境效益的均衡发展，是景区亟待解决的问题之一。但是，在部分险峻的景区，譬如悬崖峭壁上，卫生清理工作就变得十分危险。此外，在一些架空电力线路上，异物的清理工作也是十分艰巨的任务。

随着无人机技术的不断进步，其操纵的强灵活性及其功能的高扩展性逐渐受到大众青睐，与人们冒着风险回收垃圾相比，利用无人机无疑是最好的选择。传统的无人机系统利用预先编写的程序与固定流程来执行动作，其自主能力有限，难以感知外部环境的变化进行响应动作，与人和工作环境之间进行友好交互。随着计算机技术和人工智能技术的飞速发展，计算机处理能力和传感器精度的提高，无人机在功能和技术层次上有了很大提升，具有感受、思考、决策和动作能力的智能无人机系统成为研究领域的热点。智能无人机系统能够在复杂环境下，完成自主感知推演、规划和控制，在电力线巡检、灾害救援等应用领域，开展对周围环境的物体检测跟踪、障碍物感知、定位路径规划、动作。其相对于通用无人机平台的一个显著不同点在于其选取最佳角度进行物品抓取功能，可使该无人机系统在拾取险峻景区的垃圾和清理架空电力线的异物等应用中发挥重要作用。

然而，无人机在高危环境下完成任务时依然存在许多问题，gps信号丢失、定位不准等，这使得无人机需要借助其他传感器进行自身定位，同时提高自主飞行能力，才能避免在各种复杂的高危环境下飞行时撞击障碍或坠毁。此外，由于抓取目标的姿态各异以及机械臂抓取时对无人机平衡性造成的影响等，需要设计更好的方案来实现稳定抓取。

技术实现要素：

本发明的目的在于提供一种面向高危环境的智能无人机抓取目标的系统和方法，旨在代替目前高危环境下的人工作业，降低人力成本，提高作业效率和安全性。

为达到上述目的，本发明采取了以下技术方案：

一种智能无人机抓取目标的系统，包括多旋翼无人机，搭载在无人机上的机载处理器、深度相机、单目相机、惯性测量单元、机载控制器、激光雷达，以及机械臂和重心补偿单元；

所述机载控制器用于控制无人机起飞、移动、降落；

所述机载处理器、单目相机、惯性测量单元、激光雷达相连接，用于对无人机自身进行定位；

所述深度相机和机载处理器相连接，用于检测出目标及目标的摆放角度并且计算出目标相对于无人机的坐标位置；

所述机械臂、重心补偿单元和机载处理器相连接，用于实现多自由度的目标抓取；

所述机械臂为相互铰接的三段式结构，包括三个自由度，一端固定在无人机的中心点处，其末端执行器为可旋转的机械爪结构，用于抓取目标；

所述重心补偿单元用于消除机械臂抓取目标时导致无人机飞行不稳定以及抓取不稳定等影响，其安装在无人机中间，包括牵引导轨，滑台，配重和第四舵机，所述牵引导轨中心点和无人机中心重合，所述第四舵机用于控制滑台在牵引导轨上的移动，配重安放在滑台上面，用配重的重量来平衡无人机重心的位置。

进一步的，所述机械臂的三段之间利用三个舵机相连接，第一舵机连接机械臂的第一段和无人机机身，控制旋转角度θ1，第二舵机连接机械臂的第一段和第二段，控制旋转角度θ2，第三舵机连接机械臂第二段和末端，控制末端执行器的旋转角度θ3；前两个自由度保证机械臂可以在2d平面上移动，最后一个自由度用于实现对目标进行精准地旋转抓握，三自由度机械臂的动力学公式如下：

x0＝l1cosθ1+l2cos(θ1+θ2)

y0＝l1sinθ1+l2sin(θ1+θ2)

θ3＝θ

其中，l1和l2是第一段与第二段的长度，θ1、θ2、θ3是每个关节的旋转角度，(x0,y0)是在以机械臂固定点为原点的坐标系下机械臂末端执行器的坐标，θ为目标的摆放角度。

进一步的，所述重心补偿单元中的配重为用于向无人机提供电能的电池。

本发明还提供一种面向高危环境的智能无人机抓取目标的方法，应用于上述技术方案中的系统中，所述方法包括如下步骤：

步骤100，依靠机载单目相机和惯性测量单元对复杂环境下无人机自身进行定位，实现精准地导航飞行，并通过激光雷达测量无人机的飞行高度，保证无人机和地面保持一定的安全高度距离；

步骤200，利用机载深度相机拍摄实时拍摄当前环境下的图片，获得rgb图像和对应的深度图像，结合rgb图像进行目标的检测识别和定位，并检测目标的摆放角度θ，同时结合深度图像获得目标相对于无人机的相对位置坐标；

步骤300，利用机载控制器控制无人机飞行到靠近目标的位置，实时检测目标的位置坐标和无人机机械臂末端执行器的坐标，当两者的距离小于一定阈值时，机载处理器控制机械臂旋转，并控制重心补偿单元对无人机的重心进行调节，当机械臂末端执行器旋转到与目标摆放角度θ相同时，对目标进行抓取；

步骤400，利用无人机机械臂携带目标返回回收点，机械臂丢下目标。

进一步的，步骤s100中无人机自身进行定位的具体实现方式如下，

步骤s101，机载控制器控制无人机飞行，利用单目相机对周围环境的图像进行采集，单目相机移动之后进行三角化测量像素的距离，以此得到无人机周围的环境的三维点云数据，通过串口或者usb连接到机载处理器上；

步骤s102，机载处理器获取每时刻的三维点云相对于无人机的三维坐标，并匹配相邻时刻的三维点云数据，计算对应的坐标差，通过优化算法估计相邻两个时刻无人机的位移和姿态变化；

步骤s103，以无人机初始位置作为原点建立三维坐标系，利用相邻两时刻无人机的姿态位移变化累计估计无人机相对于坐标原点的位置移动和姿态变化，并利用无人机自身携带的imu数据对累计误差进行校正。

进一步的，步骤200中采用rotation-squeezedet进行目标的检测识别和定位，所述rotation-squeezedet是一种改进的squeezedet网络模型，其利用旋转的边界框来标定目标，表达式为r'＝(cx,cy,h,w,θ)，其中cx，cy分别表示该边界框左上角的像素坐标，h，w分别表示该边界框的高度和宽度，θ表示旋转边界框的旋转角度。

进一步的，步骤s200中获得目标相对于无人机的相对位置坐标的具体实现方式如下，

检测到目标之后，通过深度图像得到目标的距离信息，将rgb图像与深度图像在同一坐标系中进行对齐，得到rgb图像与三维点云数据；接着，利用检测到的目标结果，从整个图像的点云中提取包含目标的点云子区域；之后，通过计算目标点云子区域的中心来计算目标的在深度相机坐标系下的坐标(xt,yt,zt)，公式如下：

式中，(xp,yp,zp)为旋转边界框内所有点云坐标的集合，式中的上标i表示在目标边界框内从左上角开始的第i个有效点云坐标，k表示旋转边界框内的有效点云个数；

最后，利用已知的深度相机的内参，求出目标关于无人机的相对位置坐标。

进一步的，机载处理器控制机械臂旋转，并控制重心补偿单元对无人机的重心进行调节的具体实现方式如下，

令机械臂末端执行器的坐标(x0,y0)等于步骤s200中解算出来的目标的坐标，由此坐标逆向计算出前第一舵机和第二舵机应该旋转的角度θ1、θ2，θ3与检测出来的目标旋转角度θ相等；机载处理器内设置有4个比例-积分-微分控制器(pid控制器)，分别用于控制舵机旋转的速度，通过返回的当前实际角度θ1cur、θ2cur、θ3cur和理想角度θ1、θ2、θ3相比较，确定每个舵机旋转的速度v1、v1、v3，以确保最终第一、第二、第三舵机控制的角度到达理想的角度值，同时结合实际的角度值θ1cur、θ2cur利用重心解算原理，求解出配重的理想位移pb，然后通过检测配重距离中心的实际位移pbcur，利用第四pid控制器来实现第四舵机旋转速度的控制。

本发明所述的面向高危环境的智能无人机抓取目标的系统，其完成的效果是：首先，无人机起飞，在飞行过程中将拍到的图像进行实时处理，检测其中是否存在感兴趣的目标，如果存在，定位目标，无人机接近目标，利用机械臂对目标实施抓取，在成功抓取目标后，携带目标飞行返回回收点，丢下目标后，继续巡视。本发明通过结合无人机技术和图像处理技术，将无人机应用于高危环境下的异物清理工作中，利用多种传感器，采用同时定位与制图算法、路径规划算法、基于深度学习的目标检测等算法实现目标的检测识别和定位，同时，设计了一套完备的机械体系，实现对目标的稳定抓取。相对于传统的人力清理，本发明自动化程度高，效率高，安全系数高。可广泛应用于高危环境下的异物清理工作中。

附图说明

图1为本发明系统模块示意图。

图2为本发明系统工作流程。

图3为本发明实施例中机械臂及其坐标系示意图。

图4为本发明实施例中机械臂工作空间气流强度示意图。

图5为本发明实施例中机械臂及重心补偿单元控制流程图。

具体实施方式

为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图对本发明进一步详细说明。

本发明主要属于无人机技术领域，涉及空中机器人的基本问题，包括无人机自身定位、路径规划等，同时还涉及图像处理方面的技术，包括目标检测，目标定位，此外，抓取目标所用的机械臂还涉及了一些机械动力学方面的技术。

如图1所示，本发明提供的一种智能无人机抓取目标的系统，包括多旋翼无人机，搭载在无人机上的机载处理器、深度相机、单目相机、惯性测量单元、机载控制器、激光雷达，以及机械臂和重心补偿单元；

所述机载控制器用于控制无人机起飞、移动、降落；

所述机载处理器、单目相机、惯性测量单元、激光雷达相连接，用于对无人机自身进行定位；

所述深度相机和机载处理器相连接，用于检测出目标及目标的摆放角度并且计算出目标相对于无人机的坐标位置；

所述机械臂、重心补偿单元和机载处理器相连接，用于实现多自由度的目标抓取；

所述机械臂为相互铰接的三段式结构，包括三个自由度，一端固定在无人机的中心点处，其末端执行器为可旋转的机械爪结构，用于抓取目标；

所述重心补偿单元用于消除机械臂抓取目标时导致无人机飞行不稳定以及抓取不稳定等影响，其安装在无人机中间，包括牵引导轨，滑台，配重和第四舵机，所述牵引导轨中心点和无人机中心重合，所述第四舵机用于控制滑台在牵引导轨上的移动，配重安放在滑台上面，用配重的重量来平衡无人机重心的位置。

如图2所示，本发明提供的一种面向高危环境的智能无人机抓取目标的方法，包含如下步骤：

步骤s100:依靠机载单目相机和惯性测量单元(imu)对复杂环境下无人机自身进行定位，实现精准地导航飞行，并通过激光雷达测量无人机的飞行高度，保证无人机和地面保持一定的安全高度距离；

该步骤主要利用机载单目相机采集周围环境的数据，通过机载处理器处理来增量式地计算里程计对无人机进行定位，同时由于增量累积法的误差会逐渐增大，因此采用imu和单目视觉计算出来的点云融合来获取里程计，从而实现对无人机定位。具体包括：

步骤s101:无人机移动飞行，利用单目相机对周围环境的图像进行采集，单目相机移动之后可以进行三角化测量像素的距离，以此可以得到无人机周围的环境的三维点云数据，通过串口或者usb连接到机载处理器上。

步骤s102:机载处理器获取每时刻的三维点云相对于无人机的三维坐标，并匹配相邻时刻的三维点云数据，计算对应的坐标差，通过优化算法估计相邻两个时刻无人机的位移和姿态变化。

步骤s103:以无人机初始位置作为原点建立三维坐标系，利用相邻两时刻无人机的姿态位移变化累计估计无人机相对于坐标原点的位置移动和姿态变化，并利用无人机自身携带的imu数据对累计误差进行校正。

需要注意的是，无人机在高度的测量上不是利用单目相机和惯性测量单元来实现的，而是通过机载的tfmini激光雷达实现，保证无人机和地面保持一定的安全高度距离。

步骤s200:利用机载深度相机拍摄实时拍摄当前环境下的图片，获得rgb图像和对应的深度图像，结合rgb图像进行目标的检测识别和定位，并检测目标的摆放角度θ，同时结合深度图像获得目标相对于无人机的相对位置坐标；

该步骤主要是利用机载深度相机拍摄当前环境下的图片，并通过机载处理器利用目标检测算法实时检测是否存在目标，如果检测出目标，就对目标进行定位，解算目标相对于无人机的坐标，具体包括：

步骤s201：利用深度相机采集图像，可以得到rgb图像和对应的深度图像，同时，机载处理器利用基于深度学习的目标检测算法对采集到的彩色图像(rgb图像)图像进行处理。本系统采用rotation-squeezedet作为检测模型进行目标检测，rotation-squeezedet是一种改进的squeezedet网络模型，结合了rotationregionproposalnetworks(rrpn)中的rotationanchors(r-anchors)，即利用旋转的边界框来标定目标。squeezedet是将边界框定位和分类器通过单一网络结合起来的单通道检测模型，其中的边界框即为目标的边界框，在squeezedet的网络模型中，边界框为平行于图像边界的一个矩形，可以表示为r＝(cx,cy,h,w)，其中cx，cy分别表示该边界框左上角的像素坐标，h，w分别表示该边界框的高度和宽度。这种平行于图像边界的矩形框，容易框入大量的背景信息，且不含有目标角度信息，所以，结合rrpn中采取的旋转边界框的方法，我们对squeezedet中的边界框进行了修改，用r'＝(cx,cy,h,w,θ)，来表示旋转边界框，其中θ表示旋转边界框的旋转角度。利用改进后的检测模型，就可以检测出含有旋转角度信息的目标边界。

步骤s202:在步骤s201中检测到目标，通过深度图像得到目标的距离信息，之后利用机载处理器计算出目标相对于无人机的具体位置。首先，将彩色图像(rgb图像)与深度图像在同一坐标系中进行对齐，得到rgb图像与三维点云数据；接着，利用检测到的目标结果，从整个图像的点云中提取包含目标的点云子区域；之后，通过计算目标点云子区域的中心来计算目标的在深度相机坐标系下的坐标(xt,yt,zt)，公式如下：

式中，(xp,yp,zp)为目标边界框内所有点云坐标的集合，式中的上标i表示在目标边界框内从左上角开始的第i个有效点云坐标，k表示目标边界框内的有效点云个数。

得到目标在相机坐标系下的坐标之后，利用已知的深度相机的内参，可以求出目标关于无人机的相对位置坐标，该部分为现有技术，本发明不予撰述。

步骤s300:利用机载控制器控制无人机飞行到靠近目标的位置，实时检测目标的位置坐标和无人机机械臂末端执行器的坐标，当两者的距离小于一定阈值时，机载处理器控制机械臂旋转，并控制重心补偿单元对无人机的重心进行调节，当机械臂末端执行器旋转到与目标摆放角度θ相同时，对目标进行抓取；

该步骤在步骤s200检测到目标并且准确定位到目标的基础上，利用无人机飞行到靠近目标的位置，此时，机载处理器控制机械臂旋转，抓取目标。具体包括：

步骤s301:机载处理器利用单目相机采集到的数据，辅以惯性测量单元，计算视觉里程计，控制无人机靠近目标，同时，实时检测目标的位置坐标和无人机机械臂末端的坐标，无人机飞行至两者坐标距离小于3cm，悬停，准备抓取目标；

步骤s302:在步骤s301中无人机接近目标时，末端执行器与目标的距离小于3cm，此时，利用机载处理器控制机械臂移动至末端执行器和目标的坐标重合，同时控制末端执行器旋转至与目标的摆放角度相一致，之后抓取目标。

机械臂有三段，都是由3d打印的pla材料制成，类似于人的手臂，前两段用来延伸，后面一段是末端执行器，是两爪的机械爪，通过张合来实现抓取和放下，而且末端执行器可以实现任意角度的旋转，因此可以根据物体的摆放角度来调整抓取角度，实现更加稳定的抓取。机械臂的三段之间利用舵机相连接，第一舵机连接机械臂的第一段和无人机机身，固定在无人机的中心点处，控制旋转角度θ1，第二舵机连接机械臂的第一段和第二段，控制旋转角度θ2，第三舵机连接机械臂第二段和末端执行器，控制末端执行器的旋转角度θ3；因此，机械臂具有三个自由度，前两个自由度保证机械臂可以在2d平面上移动，在最后一个自由度上，利用目标检测给出的目标旋转角度，可以实现精准地旋转抓握，很大程度的提高抓取准确性和稳定性。本系统三自由度机械臂示意图如附图3所示，其动力学公式如下：

x0＝l1cosθ1+l2cos(θ1+θ2)

y0＝l1sinθ1+l2sin(θ1+θ2)

θ3＝θ

其中，l1和l2是第一段与第二段的长度，θ1、θ2、θ3是每个关节的旋转角度，(x0,y0)是在以机械臂固定点为原点的坐标系下机械臂末端执行器的坐标。由于θ3只与目标旋转角度θ有关，即步骤s200中检测出来的目标角度。因此我们的机械臂相当于仅有两个自由度。

由于无人机旋翼产生的风力的影响，无人机过于接近目标可能会导致目标受风力影响移动。本系统考虑了无人机转子产生的气流影响，对机械臂的工作空间进行了一定的限制。通过查阅资料，我们得到了许多不同无人机的高保真计算流体动力学(cfd)模拟结果。从这些结果的观察中，每个无人机转子产生的流量在外围区域迅速减小，具体示意图如附图4所示。该图表示的是以无人机中心点为原点，无人机下方部分区域的气流强度，图中，浅灰色区域表示弱流动影响区域或无流动影响区域，深灰色区域表示强流动影响区域。由此结果，本系统规定在机械臂进行抓取或者丢弃目标的工作状态下，机械臂末端执行器需要保持在弱流动影响区域，以此来减小气流的影响。具体方案如下，首先，建立三维坐标系，本系统采用四旋翼无人机，包括两个前翼(右侧前翼和左侧前翼)和两个后翼(右侧后翼和左侧后翼)，以无人机中心为原点，两个前翼连线的中点和原点的连线作为x轴，由原点向两个前翼连线的中点方向为x轴正方向，右侧前翼和右侧后翼连线的中点与原点的连线作为y轴，由原点向右为y轴正方向，垂直无人机平面向上为z轴。在机械臂进行抓取或者丢弃目标的工作状态下，机械臂末端执行器需要保持在距离zoy平面的垂直距离大于30cm的弱流动影响区域；在无人机进行丢下目标的工作状态下，机械臂末端执行器保持在距离zoy平面的垂直距离小于5cm的弱流动影响区域。

此外，需要注意的是，机械臂在移动抓取目标时，整个系统的重心会发生改变，会导致无人机飞行不稳定以及抓取不稳定等影响，因此，本系统设计了一个重心补偿单元，用来平衡机械臂移动导致的无人机重心的偏移。重心补偿单元安装在无人机中间，由3d打印的pla材料制成，包括牵引导轨，滑台，配重和第四舵机，导轨长60cm，固定在上述坐标系中的x轴上，牵引导轨中点位于无人机中心，在牵引导轨中点处，即无人机中心的位置，安置第四舵机，通过第四舵机旋转来控制滑台的移动，配重安放在滑台上面，用配重的重量来平衡重心的位置，滑台中心的初始位置位于上述坐标系中x轴上的(-15)cm点处。其工作原理就是机载处理器在机械臂移动时，控制第四舵机工作，驱动滑台在牵引导轨上移动相对应的距离，以此来平衡系统的重心。无人机的电池为5200mah4s-35c电池，重0.525kg，可以用作为重心补偿单元的配重。

机械臂以及重心补偿单元的工作由机载处理器控制，整个抓取过程由四个比例-积分-微分控制器(proportionintegrationdifferentiationcontroller，缩写pid控制器)来调节和控制，控制示意图如附图5所示。为了使机械臂末端执行器和目标的坐标重合，令机械臂末端执行器的坐标(x0,y0)等于步骤s202中解算出来的目标的坐标，由此坐标可以逆向计算出前两个舵机(即第一舵机和第二舵机)应该旋转的角度θ1、θ2，θ3与检测出来的目标旋转角度θ相等。在控制舵机旋转的过程中，由于舵机的旋转可能存在超过或者未达到预设的角度值，所以，我们利用三个pid控制器(即第一、二、三pid控制器)去控制舵机旋转的速度，通过返回的当前实际角度θ1cur、θ2cur、θ3cur和理想角度θ1、θ2、θ3相比较，确定每个舵机旋转的速度v1、v1、v3，以确保最终前三个舵机的控制的角度到达理想的角度值，同时结合实际的角度值θ1cur、θ2cur利用重心解算的知识，求解出配重的理想位移pb，然后通过检测配重距离中心的实际位移pbcur，利用第四pid控制器来实现第四舵机旋转速度的控制。所有pid控制器的参数均经过良好的调优，保证了控制的稳定性。

s400：利用无人机携带目标返回回收点，机械臂丢下目标；

该步骤在步骤s300中成功抓取目标的基础上，携带目标，返回事先设定的回收点，在回收点处，用机械臂丢下目标，完成整个工作。具体包括：

步骤s401:利用单目相机和惯性测量单元相结合，计算视觉里程计，规划无人机返回回收点的路径，机载控制器控制无人机飞回回收点；

步骤s402：机载处理器控制机械臂垂直向下，松开末端执行器，丢下抓取的目标。

综上所述，本发明所述的一种面向高危环境的智能无人机抓取系统，主要是通过单目相机和惯性测量单元计算视觉里程计，以此来对无人机进行定位，控制无人机的飞行轨迹；在飞行过程中，利用机载的深度相机实时拍摄当前环境下的图像，同时，通过机载处理器，利用基于深度学习的目标检测算法进行目标的检测；在确定存在目标后，对目标进行精准定位，得到目标相对于无人机的位置坐标；之后，无人机靠近目标，利用机械臂，结合之前目标检测时得到的目标摆放角度，稳定地抓取目标；最后携带目标返回回收点，丢下目标。本发明通过结合无人机技术、图像处理技术以及一些机械学方面的技术，将无人机应用于高危环境下的异物清理工作。同时，利用多种传感器，采用同时定位与制图算法、路径规划算法、目标检测算法和目标定位算法，实现对感兴趣目标的精确检测、精准定位和稳定抓取。相对于传统的人工清理异物的方法，本发明自动化程度高，效率高，安全系数高。可广泛应用于高危环境下的异物清理工作中。