CN108549397A - 基于二维码和惯导辅助的无人机自主降落方法及系统 - Google Patents

Abstract

本发明提供一种基于二维码和惯导辅助的无人机自主降落方法，设置无人机、机载相机、机载计算机、GNSS定位装置、惯性测量元件和带有授权信息的二维码，无人机通过GNSS/INS组合导航移动到降落点附近，利用相机采集到的图像识别二维码，当从图像识别的二维码和二维码信息数据库匹配成功时，读取授权信息；无人机根据视觉和惯导组合导航方式确定二维码中心点在无人机机体坐标系下的位置以及无人机在二维码局部坐标系下的姿态，调整无人机位姿，控制降落点和降落姿态的偏差进行自动降落。本发明与现有技术相比，更易于操作、部署成本低、不易受环境干扰、稳定性好、安全性好，适用于物流货运、抢险救灾等诸多领域，易于推广。

Description

基于二维码和惯导辅助的无人机自主降落方法及系统

技术领域

本发明属于无人机定位定姿技术领域，特别涉及一种利用包含授权信息的二维码和惯导辅助的无人机自主降落技术方案。

背景技术

二维码，又称二维条码。是用某种特定的几何图形按一定规律在平面(二维方向上)分布的黑白相间的图形记录数据符号信息的；在代码编制上巧妙地利用构成计算机内部逻辑基础的“0”、“1”比特流的概念，使用若干个与二进制相对应的几何形体来表示文字数值信息，通过图象输入设备或光电扫描设备自动识读以实现信息自动处理：它具有条码技术的一些共性：每种码制有其特定的字符集；每个字符占有一定的宽度；具有一定的校验功能等。二维码具有信息含量高、可靠性高、安全性好等优点，被广泛应用于日常生产生活。

卫星导航(Satellite navigation)是指采用导航卫星对地面、海洋、空中和空间用户进行导航定位的技术。GNSS定位的基本原理是测量出已知位置的卫星到用户接收机之间的距离，综合至少四颗卫星的数据交会出接收机的具体位置。

惯性导航是是随着惯性传感器技术的发展而发展起来的一门导航技术，由于其完全自主、不受任何干扰、隐蔽性强等优点，使其在许多领域中都得到了广泛应用。其主要缺点是定位误差随时间累积，无法长时间独立工作，因而，以惯性导航为基本导航设备，综合利用其他导航手段进行组合导航的方法被广泛使用。视觉定位是利用相机拍摄得到的影像确定相机位置和姿态的一种定位方法，其中利用地面已知坐标控制点的单目视觉定位技术是依靠单个视觉传感器拍摄已知的控制点，进而确定视觉传感器的绝对位姿的定位技术。

随着无人机行业的发展以及小型无人机的普及，利用无人机进行货物运输和抢险救灾等应用变得越来越“触手可及”，而对于此类应用，无人机的精准降落是一个关键问题。高精度GNSS定位(如RTK需要布设和维护基站)运营成本较高，而在城市峡谷和森林等区域由于卫星信号弱可能导致无法定位。而视觉定位成本低廉，定位精度高，因此成为无人机自动降落系统的常用解决方案。

北京理工大学佘浩平等人提出一种运动平台上无人机自主精确着陆系统及着陆方法(CN 106527487 A)，该方法利用一种多层嵌套的二维码针对汽车等运动平台提出了一种无人机位姿估计并自动降落的技术。

深圳市科卫泰实业发展有限公司的李振宇提出一种无人机辅助降落方法(CN107450590 A)，该方法利用二维码和标志码结合的方式作为辅助无人机自主降落的手段。

以上两种方案存在以下不足：1)二维码多层嵌套，设计较为复杂，而且二维码不包含其他任何信息，不能发挥更有效的作用；2)降落过程仅仅依赖视觉，鲁棒性不强，容易在有外界干扰(如大风)的情况下出现二维码失锁从而无法降落的情况；3)未涉及确定降落目标点前对二维码目标的搜索。

发明内容

基于上述内容，本发明提供一种含有授权信息的二维码和惯导辅助的无人机自主降落系统和方法。无人机获得目标点坐标后自动起飞依靠GNSS/INS组合导航不断更新位置并确定与目标点的偏差，确定到达目标点附近后在当前区域内对二维码进行搜索，识别并提取二维码授权信息，与预先建立的二维码数据库进行比对，确定可以降落后开始启用视觉加惯导定位模式辅助无人机进行精准降落。

本发明提供一种基于二维码和惯导辅助的无人机自主降落方法，设置无人机、机载相机、机载计算机、GNSS定位装置、惯性测量元件和带有授权信息的二维码，

相机、GNSS定位模块、惯性测量单元和机载计算机安装在无人机上；

带有授权信息的二维码设置于降落点上；

降落过程为，无人机通过GNSS/INS组合导航移动到降落点附近后，开始利用相机采集到的图像识别二维码，当从图像识别的二维码和二维码信息数据库内预存的二维码匹配成功时，读取授权信息，确定搜索到二维码；无人机确定搜索到二维码之后，根据视觉和惯导组合导航方式确定二维码中心点在无人机机体坐标系下的位置以及无人机在二维码局部坐标系下的姿态，调整无人机位姿，控制降落点和降落姿态的偏差稳定在相应阈值内，进行自动降落。

而且，相机安装在无人机机体下方，摄像头朝下；或者通过云台安装在无人机上，支持摄像头自由旋转。

而且，二维码通过介质张贴在已知大地坐标的降落点上，或者由显示装置显示于降落点上。

而且，二维码由若干个黑或白色的单元组成，对应不同的授权编码信息，授权信息包括但不限于但不限于二维码的实际物理尺寸、二维码的几何规格、二维码在数据库中的编号、无人机降落后的动作和授权降落密码；二维码信息数据库储存在无人机的机载计算机中或者后台服务器端。

而且，无人机通过GNSS/INS组合导航移动到降落点附近后，开始利用相机采集到的图像识别二维码，实现方式为无人机以当前悬停点为圆心，以合适长度为半径并以固定角速度进行环绕飞行，逐渐加大飞行半径直到搜索到二维码。

或者，无人机通过GNSS/INS组合导航移动到降落点附近后，开始利用相机采集到的图像识别二维码，实现方式为无人机以当前悬停点为起点，以稳定速度竖直上升，直到搜索到二维码。

而且，二维码是方形时，利用相机采集到的图像识别二维码，实现方式如下，

1)对相机采集到的图像进行形态学处理消除斑点，降低噪声，然后进行灰度化和二值化；

2)在二值化后的图像中提取边缘轮廓信息，去除不合理的轮廓；

3)对剩下的轮廓进行多边形拟合，剔除不合理的多边形轮廓，并对剩下的四边形轮廓根据对应的二维码角点平面坐标进行纠正；

4)将纠正后的轮廓划分为若干小方块，根据每个小方块的灰度信息，判断是为黑色或白色，计算识别到的二维码和数据库内二维码之间的汉明距离，若匹配成功，则读取授权信息，确定搜索到二维码。

而且，视觉和惯导组合导航方式为松组合或紧组合。

本发明还提出一种基于二维码和惯导辅助的无人机自主降落系统，用于执行如上所述基于二维码和惯导辅助的无人机自主降落方法。

本发明的优点是：

利用含有授权信息的二维码可以增加无人机自主降落的安全性和可靠性，利用GNSS/INS组合测量系统确定无人机飞行时的位置可以保证无人机高动态飞行时的高频率导航结果输出，以及提高无人机定位的稳定性和可靠性。

利用单目视觉和惯导组合的方式来确定无人机降落时的位姿有如下几个优点：

1、补偿无人机自身单点定位精度不足的缺陷；

2、提高导航结果输出频率，便于无人机的高频控制；

3、在视觉定位失效的情况下使无人机快速复位，提高系统鲁棒性

4、利用惯导的陀螺仪数据辅助无人机的姿态确定。

本发明技术方案具有如下有益效果：

1、易于操作、部署成本低。二维码设计和维护成本低，只需张贴在平坦位置即可；

2、不易受环境干扰、稳定性好。GNSS/INS和Visual/INS两种组合模式保证了定位结果的稳定性和鲁棒性；

3、安全性好。本发明所使用的二维码含有特定的授权信息，使得无人机的降落稳定可控；

4、结合以上几点，本发明易于推广，适合物流货运、抢险救灾和科研监测等许多不同的场景，具有重要的市场价值。

附图说明

图1为本发明实施例系统示意图。

图2为本发明实施例二维码示意图。

图3为本发明实施例图像处理流程示意图。

图4为本发明实施例方法整体流程示意图。

具体实施方式

为了更清楚地说明现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面的描述仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。

本发明提出基于包含授权信息的二维码，利用无人机搭载的相机，惯性传感器和GNSS接收机，提供一种精度高、成本低、安全可靠的无人机自主降落系统。系统组成如图1所示，包括无人机1，机载相机2，惯性测量单元(IMU)3，GNSS定位装置4，机载计算机5，二维码6。

无人机1可采用旋翼无人机，所述机载相机2安装在无人机1下方，摄像头朝下，使相机坐标系Z轴与旋翼无人机1中心轴平行；

所述IMU3，GNSS定位装置4，机载计算机5均安装在无人机上方，IMU3用于测量无人机三轴角速度和加速度，GNSS接收机4用于卫星定位确定无人机的大地坐标。IMU3和GNSS定位装置合称为INS/GNSS组合测量系统。

本发明所述的INS/GNSS组合测量系统包含但不仅限于惯性测量单元、GNSS接收机和GNSS天线，还包括以INS/GNSS为核心但融合了里程计、磁强计、气压高程计传感器中任意一种或多种的集成式组合导航系统。

本发明所述的INS/GNSS组合测量系统不限于本具体实施方式采用的未集成在同一壳体内的“分体式”组合导航系统，也可以为GNSS接收机与INS集成的“一体式”INS/GNSS系统。板载计算机5用于处理传感器数据并向无人机控制器发送指令。机载相机2、惯性测量单元3和GNSS定位装置4分别连接机载计算机5。

所述二维码6是带有授权信息的二维码，设置于降落点上，二维码可以某一介质张贴在已知大地坐标的降落点上，或者由某种显示装置(如液晶屏幕)显示于降落点上。

优选地：

旋翼无人机包括但不限于可以直接对电机转速进行底层控制的无人机(另如：集合封装程度良好，开放SDK以指令形式控制的无人机)，包括但不限于四旋翼无人机(另如：六旋翼无人机)。

相机安装在无人机机体下方，摄像头朝下，相机包括各种形式的图像采集装置，包括鱼眼相机、全局快门相机、卷帘快门相机等。

GNSS定位模块、惯性测量单元和机载计算机安装在无人机上。

机载计算机包括但不限于搭载操作系统的ARM开发板(如：FPGA+ARM架构的芯片)。

包含有授权信息的黑白两色二维码张贴在已知大地坐标的地面或平台上。

二维码形状不唯一，可以为矩形，正方形，也可以为圆形等，尺寸大小已知。可视为由若干个黑白不同的单元组成。圆形二维码由互相嵌套的圆环分成圆心角相同的扇区。

为避免图像出现中心对称从而导致方向定义出现歧义，因此选择黑色方块时需注意最后所成二维码不能为中心对称图形而可以是轴对称图形。外围一圈设计为黑色，内部主体为白色，可选择固定数量m的方块填充为黑色，对应不同的编码信息。

根据二维码大小可以计算得到有M种编码方案，根据不同的编码方案在数据库中对这M种二维码分别赋予不同的编号和其他授权信息，使得无人机在降落时识别到其中某一二维码时通过与数据库进行比对确认该地区可以降落，反之若识别到的二维码不包含授权信息或与数据库不相符合，则拒绝降落。具体实施时，授权信息可优选包括但不限于二维码的实际物理尺寸、二维码的几何规格(例如提供方块数量)、二维码在数据库中的编号、无人机降落后的动作(返程还是飞向下一个目标点)和授权降落密码等。例如授权降落密码，是指当无人机送货到顾客家，需要顾客输入与二维码对应的授权密码才能打开货箱取出货物。用于显示二维码的介质包括但不限于普通纸张(另如：玻璃，油性纸张等)

二维码信息数据库储存在无人机机载计算机中或者后台服务器端。若存储在后台服务器端，则需要无人机和服务器进行远程通信以确定二维码是否匹配成功。

实施例中，所述二维码6张贴在已知大地坐标的降落点上，以正方形二维码为例，二维码由N×N大小的黑白方块组成，周围一圈为黑色，内部按不中心对称的原则适当填充黑色方块，对不同的填充方式形成的二维码赋予不同的授权信息，编码信息对应的存储在二维码信息数据库中，正方形二维码如图2所示。

本发明实施例提供的一种含有授权信息的二维码和惯导辅助的无人机自主降落方法，无人机以GNSS/INS组合导航结果移动到降落点附近后，开始利用相机采集到的图像搜索二维码，包括：无人机以当前悬停点为圆心，以合适长度为半径并以固定角速度进行环绕飞行，逐渐加大飞行半径直到搜索到二维码。这样，根据GNSS/INS组合导航确定无人机自身位置，到达目标点附近后基于提前布设好的二维码和惯导进行定位定姿，辅助无人机进行精准降落。

包括如下步骤：

1、标定机载相机内参，得到相机内参数矩阵和畸变系数

2、利用GNSS静态定位技术确定无人机待降落点的大地坐标；

3、按照预设的二维码大小建立二维码数据库，以正方形二维码为例，二维码数据库包含二维码的物理尺寸，二维码四个角点在二维码局部坐标系下的坐标，总的方块数量和中间区域黑色方块个数，并且每一种编码对应不同的授权信息。数据库中用(N-2)×(N-2)的二维矩阵表示二维码的编码信息(N为二维码整体一行或一列的方块数，周围一圈为黑色)，其中黑色小方块对应的矩阵元素值为0，白色小方块对应的矩阵元素值为1；

4、将二维码张贴在无人机待降落点，使二维码中心与降落点重合；

5、将降落点的大地坐标发送给无人机，无人机自动起飞飞向待降落点，具体步骤如下：

51、无人机依靠GNSS/INS组合导航获取自身位置；

52、无人机机载计算机根据无人机当前位置和待降落点的大地坐标差计算待降落点在无人机导航坐标系下的坐标；

53、无人机机载计算机按照该坐标计算电机控制量并将控制信号发送给电机不断减小偏差直到无人机稳定在待降落点附近；

6、无人机启用视觉模块对二维码进行搜索，根据具体情形，分别有两种方式进行搜索：

(1)环绕搜索方式，具体步骤如下：

61、无人机以当前悬停点为圆心，以合适长度为半径并以合适角速度进行环绕飞行；

62、飞行过程中无人机开启相机，对以固定频率采集到的图像进行形态学处理消除斑点，然后进行灰度化和二值化；

63、在二值化后的图像中提取边缘轮廓信息，去除不合理的轮廓；

64、对剩下的轮廓进行多边形拟合，剔除不合理的多边形轮廓，并对剩下的四边形轮廓根据对应的二维码角点平面坐标进行纠正；

65、提取纠正后的轮廓区域的图像灰度信息，若能在二维码数据库中找到匹配，则确定搜索到二维码。匹配方式为，将纠正后的轮廓划分为N×N的小方块，每个小方块的灰度信息，若有大于一半的部分为黑色(或白色)，则该小方块整体视为黑色(或白色)。计算识别到的二维码和数据库内二维码之间的汉明距离，若匹配成功，则读取授权信息，确定搜索到二维码。

66、若未能成功匹配则增大半径重复步骤61，直到搜索到二维码。

(2)具体步骤如下：

61、上升搜索方式，无人机以当前悬停点为起点，以合适速度竖直上升；

因为相机是垂直向下的，视角固定，无人机上升之后在地面上的搜索范围会变大，在相机分辨率有保障的情况下会增大二维码搜索成功的几率。因此理论上也可实现，执行和步骤(1)-62、63、64、65类似的步骤即可。

66、若未能成功搜索到二维码则继续上升直到搜索到二维码。

以上两种搜索方式在本发明方法实施时应视相机分辨率、像素大小、焦距和二维码规格等情况决定。

无人机确定搜索到二维码之后开始根据视觉和惯导组合导航结果确定二维码中心点在无人机机体坐标系下的位置以及无人机在二维码局部坐标系下的姿态，并将其作为无人机控制器的输入，根据控制器输出值调整无人机位姿使之与降落点和降落姿态的偏差稳定在较小的预设阈值内，随后开始自动降落。

视觉和惯导的组合需要提取图像中的二维码角点，对相机采集到的图像进行处理提取二维码四个角点的方式有两种：

一是对每次采集到的图像单独地进行二值化和轮廓提取的操作，再将图像中四边形轮廓内的区域与二维码数据库进行比对；二是在第一次匹配到二维码之后用描述子对四个角点进行描述，之后便不再进行二值化和轮廓提取，而是提取图像特征点，并用相同的描述子进行描述再与之前的二维码角点匹配，匹配之后即得到当前图像中对应的二维码角点坐标和描述子，下一帧图像再与之进行匹配。

具体实施时，所述描述子可采用的类型包括DoG(Difference of Guassians)，BRIEF(Binary Robust Independent Elementary Features)，SIFT(Scale-invariantfeature transform)等。

所述的视觉和惯导组合导航方法包括松组合和紧组合两种模式。松组合方式是相机根据二维码角点的大地坐标和相机相对于二维码局部坐标系位置确定相机的大地坐标和相机姿态，惯导根据加速度计和陀螺仪数据推算得到IMU的位置和姿态，根据IMU和相机的安装关系推算得到相机的位置和姿态，两者之间的差异作为卡尔曼滤波的观测更新估计位置和姿态误差以及IMU零偏误差等参数，得到更新后的定位结果，再将其转换为无人机相对于二维码局部坐标系的相对位姿。紧组合方式是相机观测到特征点(即二维码角点)后不单独进行定位，在观测值层面上与惯导组合进而确定相机相对于二维码局部坐标系的位姿。

实施例的具体步骤如下：

首先，通过高精度地图或者GNSS静态定位获得降落点的大地坐标。布设二维码使其中心与其重合，使二维码的局部坐标系与当地导航坐标系(NED，X轴指北，Y轴指东，Z轴垂直于XY平面指向地心)重合，并以此确定二维码四个角点在局部坐标系下的坐标。

然后无人机收到降落点的大地坐标后自动起飞，根据机载GNSS定位装置和IMU测量数据组合计算自身与目标点的位置偏差，将偏差转换为无人机控制系统的控制输入不断调整使无人机以稳定的速度飞向目标点。INS/GNSS组合导航解算方法可以为松组合解算模式、紧组合解算模式或PPK紧组合解算模式，其中，松组合解算模式下的GNSS解算方法包含实时动态差分定位(RTK)、精密单点定位(PPP)、单点定位(SPP)等解算模式。本具体实施中优选实时单点定位松组合解算方法。

当无人机到达目标点附近一定预设的阈值内并稳定一段时间后开始利用单目视觉在视野范围内对二维码进行搜索。以环绕飞行搜索为例，如果无人机在当前位置处没有检测到二维码，则以目标点为圆心，优选以固定的角速度，从预设的最小搜索半径开始，按大小逐渐递增的半径进行环绕飞行来搜索二维码。固定的角速度可以对无人机有比较稳定的控制，同时使得采集照片更为均匀。为提高搜索效率，避免环绕飞行时出现重叠搜索，可根据当前无人机对地高度及内参数确定最小搜索半径和半径递增梯度。从图像中得到的像素点的坐标表示在像素坐标系下，像素坐标系原点位于图像左上角，u轴向右，v轴向下。则有

其中，K为相机的内参数矩阵，其中f_x、f_y为焦距，c_x、c_y表示相机成像平面的原点到像素坐标系原点的平移。P＝[X Y Z]^T代表像素点对应的实际点在相机坐标系下的坐标。根据无人机当前离地高度可以计算相机当前高度，再由上式可以计算相机成像的边缘像素点在相机坐标系下的位置。取X、Y坐标的较小值为r，则以无人机当前位置为圆心，2r-m即为最小搜索半径，递增梯度为2r-m，m为二维码尺寸大小，即在以无人机当前位置为圆心，以2r-m为半径进行搜索，搜索不到时，增加为以(2r-m)+(2r-m)为半径进行搜索，如果还搜索不到，增加为以(2r-m)+(2r-m)+(2r-m)为半径进行搜索…。无人机环绕飞行时，板载计算机将相机得到的图像进行形态学处理，降低噪声并进行灰度化和二值化，然后提取图像中的轮廓信息，对提取出来的若干个轮廓进行多边形拟合，根据轮廓大小和点间距离剔除不符合要求的轮廓，留下二维码的待检测轮廓。根据四点透视变换纠正待检测轮廓，解码二维码，提取轮廓区域包含的像素信息，将该信息与数据库进行比对若比对成功则根据对应的授权信息确认是否降落，以及降落后无人机的动作。图像处理流程如图3所示。

若搜索半径增加到限值时仍未搜索到二维码，则减小一开始的最小搜索半径为上次迭代时取值的1/2，而梯度不变重新进行搜索，即再以无人机当前位置为圆心，以(2r-m)/2为半径进行搜索，搜索不到时，增加为以(2r-m)/2+2r-m为半径进行搜索，按此迭代，直至搜索到二维码。具体实施时，限值可根据需要预先设置。

无人机搜索到二维码之后，即启动视觉和惯导(Visual/INS)组合定位模块，视觉和惯导组合定位方式包括松组合和紧组合两种模式，具体实施时可任选其一。本实施例选用松组合模式，并推导相应的观测方程。步骤如下：

1)对相机拍摄到的图像降噪处理后进行二值化，提取二值图像中的边缘轮廓信息，将提取到的所有轮廓存放在向量中并逐一进行轮廓多边形拟合，剔除非四边形、凹多边形和周长过短或过长等不合理的轮廓。

2)然后在剩下的轮廓中根据四个角点的像素坐标和已知的二维码四个角点的局部坐标之间的对应关系得到透视投影变换的变换矩阵，再分别利用该矩阵将轮廓包含区域的图像变换成正方形图像。

3)根据该图像内的像素信息与数据库中的二维码进行比对，得到正确的轮廓之后再对角点的像素坐标进行细化得到亚像素级的坐标。

4)最后根据n点透视投影变换(PNP，Perspective-n-points)求解得到二维码局部坐标系相对于相机坐标系的旋转矩阵和平移向量，再进一步转换得到相机的大地坐标和相对二维码局部坐标系的姿态。相机估算出位置和姿态后即可与惯导估算得到的位置和姿态进行组合，修正惯导元件的零偏等误差，得到更鲁棒的导航定位结果。

5)将组合后的相机定位结果和二维码中心的位置偏差转换为二维码中心在无人机坐标系下的位置，即可将该位置作为无人机飞行控制器的输入，输出为无人机的电机转速，不断控制直到位置偏差稳定在较小的阈值内即可视为满足降落条件，开始自动降落。系统整体流程图如图4所示。

上式中，和分别表示二维码局部坐标系w到相机坐标系c的旋转矩阵和平移向量，P_w、P_c分别表示角点在二维码局部坐标系和相机坐标系下的位置。令P_w＝0，可得二维码局部坐标系的原点(即二维码中心点)在相机坐标系下的坐标将无人机机体坐标系b顺时针旋转90°得到中间坐标系b′，则有

其中，分别表示中间坐标系b′到相机坐标系c和无人机坐标系b到b′之间的旋转矩阵，前者为单位矩阵，表示b′系到c系的平移向量，该值为固定不变的常值。且，

其中，I表示单位矩阵；

则有，

其中，表示的逆。

因此二维码中心点在无人机坐标系下位置为：

无人机飞行控制器采用经典的PID控制。为便于实施参考起见，提供PID控制器的输出表达式为

ev(t)＝sp(t)-pv(t)(8)

其中，sp(t)为设定值，pv(t)为过程变量，即反馈值，ev(t)为相应差值；mv(t)是控制器的输出信号，K_p为比例系数，T_I和T_D分别为积分时间和微分时间。通过不断调整PID参数来降低超调量，缩短响应时间并减小稳态误差，使控制器达到较优状态。

Visual/INS松组合观测方程推导：

由惯导(位置速度姿态)误差微分方程可得到卡尔曼滤波的状态方程

x(t)＝[δrⁿ δvⁿ φ b_g b_a] (10)

x(t)为状态向量，包括位置误差、速度误差、姿态误差、陀螺和加速度计零偏等十五维。F(t)表示状态转移矩阵，w(t)为系统噪声，G(t)为噪声驱动矩阵。

而对于观测方程，视觉测量得到的是相机相对于二维码局部坐标系的位姿，INS推算得到的是IMU的位置，通过杆臂补偿可以换算得到相机位置，由相机和INS分别得到的相机位置求差即可作为位置误差的观测量z_rc。

上式中，为惯导推算得到的相机位置，为视觉测量得到的相机位置，为惯导推算得到的相机位置误差，为b系到n系的方向余弦矩阵，表示相机和惯导的安装位置偏差，即杆臂，表示向量的反对称矩阵，φ表示姿态误差，ε_rc表示测量噪声。文中(α×)表示向量α对应的反对称矩阵。

下面来看一下姿态误差的观测方程，IMU解算得到的姿态是b系到n系，通过将特征点局部坐标系与当地导航坐标系n系对齐，可以得到相机相对n系的姿态，由此可以建立观测方程。

惯导推算得到的相机姿态：

其中，为惯导推算得到的相机姿态，为相机坐标系相对于载体坐标系的姿态。为IMU姿态的真实值，为惯导解算得到的IMU姿态，二者之间的误差用φ表示，(φ×)表示误差相应的反对称矩阵。

相机解算得到的姿态：

其中，为相机解算得到的姿态，为真实的相机姿态，v表示量测噪声，(v×)表示量测噪声对应的反对称矩阵。

用ρ表示相机解算得到的相机姿态与惯导推算得到的相机姿态之间的偏差，(ρ×)表示偏差相应的反对称矩阵，则有

[I-(φ×)]＝[I-(ρ×)][I+(v×)] (15)

化简得到

ρ＝φ+v (16)

再由14式，

综上：

Z表示观测向量。

具体实施时，可采用软件方式实现自动流程运行。

上述实施例所述是用以具体说明本发明，文中虽通过特定的术语进行说明，但不能以此限定本发明的保护范围，熟悉此技术领域的人士可在了解本发明的精神与原则后对其进行变更或修改而达到等效目的，而此等效变更和修改，皆应涵盖于权利要求范围所界定范畴内。

Claims (9)

1.一种基于二维码和惯导辅助的无人机自主降落方法，其特征在于：设置无人机、机载相机、机载计算机、GNSS定位装置、惯性测量元件和带有授权信息的二维码，

相机、GNSS定位模块、惯性测量单元和机载计算机安装在无人机上；

带有授权信息的二维码设置于降落点上；

降落过程为，无人机通过GNSS/INS组合导航移动到降落点附近后，开始利用相机采集到的图像识别二维码，当从图像识别的二维码和二维码信息数据库内预存的二维码匹配成功时，读取授权信息，确定搜索到二维码；无人机确定搜索到二维码之后，根据视觉和惯导组合导航方式确定二维码中心点在无人机机体坐标系下的位置以及无人机在二维码局部坐标系下的姿态，调整无人机位姿，控制降落点和降落姿态的偏差稳定在相应阈值内，进行自动降落。

2.根据权利要求1所述基于二维码和惯导辅助的无人机自主降落方法，其特征在于：相机安装在无人机机体下方，摄像头朝下。或者无人机通过云台安装在无人机上，摄像头可以自由旋转。

3.根据权利要求1所述基于二维码和惯导辅助的无人机自主降落方法，其特征在于：二维码通过介质张贴在已知大地坐标的降落点上，或者由显示装置显示于降落点上。

4.根据权利要求1所述基于二维码和惯导辅助的无人机自主降落方法，其特征在于：二维码由若干个黑或白色的单元组成，对应不同的授权编码信息，授权信息包括但不限于但不限于二维码的实际物理尺寸、二维码的几何规格、二维码在数据库中的编号、无人机降落后的动作和授权降落密码；二维码信息数据库储存在无人机的机载计算机中或者后台服务器端。

5.根据权利要求1或2或3或4所述基于二维码和惯导辅助的无人机自主降落方法，其特征在于：无人机通过GNSS/INS组合导航移动到降落点附近后，开始利用相机采集到的图像识别二维码，实现方式为无人机以当前悬停点为圆心，以合适长度为半径并以固定角速度进行环绕飞行，逐渐加大飞行半径直到搜索到二维码。

6.根据权利要求1或2或3或4所述基于二维码和惯导辅助的无人机自主降落方法，其特征在于：无人机通过GNSS/INS组合导航移动到降落点附近后，开始利用相机采集到的图像识别二维码，实现方式为无人机以当前悬停点为起点，以稳定速度竖直上升，直到搜索到二维码。

7.根据权利要求1或2或3或4所述基于二维码和惯导辅助的无人机自主降落方法，其特征在于：二维码是方形时，利用相机采集到的图像识别二维码，实现方式如下，

1)对相机采集到的图像进行形态学处理消除斑点，降低噪声，然后进行灰度化和二值化；

2)在二值化后的图像中提取边缘轮廓信息，去除不合理的轮廓；

3)对剩下的轮廓进行多边形拟合，剔除不合理的多边形轮廓，并对剩下的四边形轮廓根据对应的二维码角点平面坐标进行纠正；

4)将纠正后的轮廓划分为若干小方块，根据每个小方块的灰度信息，判断是为黑色或白色，计算识别到的二维码和数据库内二维码之间的汉明距离，若匹配成功，则读取授权信息，确定搜索到二维码。

8.根据权利要求1或2或3或4所述基于二维码和惯导辅助的无人机自主降落方法，其特征在于：视觉和惯导组合导航方式为松组合或紧组合。

9.一种基于二维码和惯导辅助的无人机自主降落系统，其特征在于：用于执行如权利要求1至8所述基于二维码和惯导辅助的无人机自主降落方法。