基于双目视觉的无人机自主着陆导航信息实时解算方法与流程

本发明属于无人机视觉导航、计算机视觉处理技术领域，涉及一种基于双目视觉的无人机自主着陆导航信息实时解算方法。

背景技术：

近些年来，随着科技的快速发展，无人机发展迅速，运用区域及涉及领域越来越广泛，且一步一步的朝着自主化、智能化发展。无人机自主着陆作为其自主化、智能化发展的最重要的一环，受到广泛关注。目前，无人机自主着陆常用的导航方式为gps导航、gps/ins信息融合导航等。然而，gps信号依赖卫星进行信息传输，极易受到干扰；而ins导航单独使用时，容易累计误差。故需要探索新的技术，在不依赖gps的情况下，为无人机提供精确导航信息，完成自主着陆。

目前，基于视觉的无人机自主导航着陆技术发展迅速，该技术采用视觉传感器采集图像进行处理，来获取导航信息，辅助无人机进行自主着陆。该技术具备抗干扰性好，精度高、易装备等优点，受到广泛关注。根据视觉传感器的个数，主要分为基于单目视觉的无人机自主着陆和基于多目视觉的无人机自主着陆。基于单目视觉的无人机自主着陆是基于一个视觉传感器来进行视觉图像处理，从而获得无人机相对位置与自身姿态信息进行导航，但此方法精度较低；基于多目视觉的无人机自主着陆是基于两个或多个视觉传感器来进行视觉图像处理，从而获得无人机相对位置与自身姿态信息进行导航，此法精度较高，但却需要多目视觉图像进行匹配，获得视差，从而进行三维重建，故较为耗时，不符合无人机自主着陆对图像处理算法实时性的要求。

参考文献：

【1】bouguetjy.pyramidalimplementationofthelucaskanadefeaturetrackerdescriptionofthealgorithm[j].opencvdocuments,1999,22(2):363-381.

【2】lepetitv,moreno-noguerf,fuap.epnp:anaccurateo(n)solutiontothepnpproblem[j].internationaljournalofcomputervision,2009,81(2):155-166.

技术实现要素：

针对上述现有技术中存在的问题，本发明的目的在于，提供一种基于双目视觉的无人机自主着陆导航信息实时解算方法。

为实现上述目的，本发明采用如下技术方案：

基于双目视觉的无人机自主着陆导航信息实时解算方法，包括以下步骤：

步骤一，获取包含有目标的双目视觉图像，双目视觉图像包括左目视觉图像和右目视觉图像；分别在左目视觉图像和右目视觉图像中提取目标图像，并对提取出的目标图像进行灰度化处理，分别得到左目目标灰度图像和右目目标灰度图像；

步骤二，分别对左目目标灰度图像和右目灰度目标图像，利用亚像素级shi-tomasi角点提取算法进行处理，均得到四个关键特征角点，根据四个关键特征角点的横纵坐标值的大小对四个关键特征角点进行排序，分别形成关键特征角点集合current_left1和关键特征角点集合current_right1；

步骤三，利用关键特征角点集合current_left1和关键特征角点集合current_right1，利用双目视觉原理求取目标中心o在无人机机体坐标系下的三维位置坐标(xs,ys,zs)，利用p4p求解方法求取目标中心o在无人机机体坐标系下的三维位置坐标(xp,yp,zp)和无人机的姿态信息；求三维位置坐标(xs,ys,zs)和三维位置坐标(xp,yp,zp)的均值作为最终求得的目标中心o在无人机机体坐标系下的三维位置坐标(x,y,z)；

步骤四，获取下一帧包含有目标的双目视觉图像，双目视觉图像包括本帧左目视觉图像和本帧右目视觉图像；分别对本帧左目视觉图像和本帧右目视觉图像进行灰度化处理，分别得到本帧左目灰度图像和本帧右目灰度图像；

步骤五，将关键特征角点集合current_left1更新为上一帧关键特征角点集合last_left，将关键特征角点集合current_right1更新为上一帧关键特征角点集合last_right；

步骤六，利用金字塔l-k光流法，基于上一帧关键特征角点集合last_left和本帧左目灰度图像，预测得到本帧的关键特征角点集合current_left2；基于上一帧关键特征角点集合last_left和本帧右目灰度图像，预测得到本帧的关键特征角点集合current_right2；

步骤七，分别针对关键特征角点集合current_left2和关键特征角点集合current_right2中的每一个关键特征角点，在以关键特征角点为中心的周围设定区域内，进行亚像素级shi-tomasi角点检测，检测得到新的角点，以检测得到的新的角点替换区域中心位置的关键特征角点，使得关键特征角点集合current_left2更新为关键特征角点集合current_left3，使得关键特征角点集合current_right2更新为关键特征角点集合current_right3；

步骤八，计算关键特征角点集合current_left3中的关键特征角点与上一帧关键特征角点集合last_left中的关键特征角点之间的相关系数，根据计算得到的相关系数对关键特征角点集合current_left3进行筛选，得到关键特征角点集合current_left4；

计算关键特征角点集合current_right3中的关键特征角点与上一帧关键特征角点集合last_right中的关键特征角点之间的相关系数，根据计算得到的相关系数对关键特征角点集合current_right3进行筛选，得到关键特征角点集合current_right4；

步骤九，若关键特征角点集合current_left4与关键特征角点集合current_right4中的关键特征角点数目都为4，则进入步骤三，其中的关键特征角点集合current_left1和关键特征角点集合current_right1分别用关键特征角点集合current_left4和关键特征角点集合current_right4代替；

若关键特征角点集合current_left4与关键特征角点集合current_right4中的关键特征角点数目都小于4，进入步骤十；

若关键特征角点集合current_left4中的键特征角点数目小于4，关键特征角点集合current_right4中的关键特征角点数目等于4，进入步骤十一；

若关键特征角点集合current_left4中的键特征角点数目等于4，关键特征角点集合current_right4中的关键特征角点数目小于4，进入步骤十二；

步骤十，分别在步骤四中得到的本帧左目灰度图像和本帧右目灰度图像中，确定左目目标再搜索区域和右目目标再搜索区域；

构建模板图像，基于归一化相关性匹配法，分别在左目目标再搜索区域和右目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像和重新确定的右目目标灰度图像；

分别针对重新确定的左目目标灰度图像和重新确定的右目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_left5和关键特征角点集合current_right5；执行步骤三，将其中的关键特征角点集合current_left1和关键特征角点集合current_right1分别用键特征角点集合current_left5和关键特征角点集合current_right5代替；

步骤十一，在步骤四中得到的本帧左目灰度图像中，确定左目目标再搜索区域；构建模板图像，基于归一化相关性匹配法，在左目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像；针对重新确定的左目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_left5；执行步骤三，将关键特征角点集合current_left1和关键特征角点集合current_right1分别用关键特征角点集合current_left5和关键特征角点集合current_right4代替；

步骤十二，在步骤四中得到的本帧右目灰度图像中，确定右目目标再搜索区域；构建模板图像，基于归一化相关性匹配法，在右目目标再搜索区域内进行目标搜索，得到重新确定的右目目标灰度图像；针对重新确定的右目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_right5；执行步骤三，将关键特征角点集合current_left1和关键特征角点集合current_right1分别用关键特征角点集合current_left4和关键特征角点集合current_right5代替。

具体地，所述步骤三中的利用双目视觉原理求取目标中心o在无人机机体坐标系下的三维位置坐标(xs,ys,zs)，具体包括以下步骤：

求左目目标灰度图像中目标中心o^l在左目摄像机坐标系下的三维坐标

其中，为左目目标灰度图像中目标中心o^l的像素坐标，为关键特征角点集合current_left1中的关键特征角点角点的像素坐标，dx为图像单个像素对应的实际尺寸，d表示左目摄像机和右目摄像机的视差值，表示左目摄像机的摄像头的光心坐标位置；t为左目摄像机和右目摄像机光心间的直线距离，一般称为基线，f为左目摄像机和右目摄像机的焦距；

求右目目标灰度图像中目标中心o^l在右目摄像机坐标系下的三维坐标

其中，为右目目标灰度图像中目标中心o^r的像素坐标，为关键特征角点集合current_right1中的关键特征角点角点的像素坐标，表示右目摄像机的摄像头的光心坐标位置；

利用旋转矩阵ml将目标中心o^l在左目摄像机坐标系下的三维坐标变换为目标中心o^l在无人机机体坐标系下的三维坐标利用利用旋转矩阵mr将目标中心o^r在左目摄像机坐标系下的三维坐标变换为目标中心o^r在无人机机体坐标系下的三维坐标采用的公式如下：

则目标中心o在无人机机体坐标系下的三维位置坐标(xs,ys,zs)为：

具体地，所述步骤三中的利用p4p求解方法求取目标中心o在无人机机体坐标系下的三维位置坐标(xp,yp,zp)和无人机的姿态信息；具体包括以下步骤：

步骤3.2.1：基于张正友标定法对左目摄像机和右目摄像机进行标定，得到左目摄像机的内外参数和右目摄像机的内外参数；

步骤3.2.2：基于左目摄像机的内外参数、关键特征角点集合current_left1中的关键特征角点的像素坐标以及关键特征角点集合current_left1中的关键特征角点的世界坐标，利用p4p求解方法，获得左目目标灰度图像中目标中心o^l在左目摄像机坐标系下的三维坐标以及无人机的姿态信息：俯仰角θl，滚转角φl，偏航角ψl；

步骤3.2.3：基于右目摄像机的内外参数、关键特征角点集合current_right1中的关键特征角点的像素坐标以及关键特征角点集合current_right1中的关键特征角点的世界坐标，利用p4p求解方法，获得右目目标灰度图像中目标中心o^r在右目摄像机坐标系下的三维坐标以及无人机的姿态信息：俯仰角θr，滚转角φr，偏航角ψr；

步骤3.2.4：利用旋转矩阵ml将目标中心o^l在左目摄像机坐标系下的三维坐标变换为目标中心o^l在无人机机体坐标系下的三维坐标利用利用旋转矩阵mr将目标中心o^r在左目摄像机坐标系下的三维坐标变换为目标中心o^r在无人机机体坐标系下的三维坐标即：

步骤3.2.5：根据目标中心o^l在无人机机体坐标系下的三维坐标和目标中心o^r在无人机机体坐标系下的三维坐标求得目标中心o在无人机机体坐标系下的三维位置坐标(xp,yp,zp)为：

根据步骤3.2.2得到的无人机的姿态信息和根据步骤3.2.3得到的无人机的姿态信息，求得最终的无人机的姿态信息：俯仰角θ＝(θl+θr)/2，滚转角φ＝(φl+φr)/2，偏航角ψ＝(ψl+ψr)/2。

具体地，所述步骤十中的分别在步骤四中得到的本帧左目灰度图像和本帧右目灰度图像中，确定左目目标再搜索区域和右目目标再搜索区域；包括以下步骤：

在步骤四中得到的本帧左目灰度图像中，确定左目目标再搜索区域的方法如下：

step1：计算上一帧的关键特征角点集合last_left中关键特征角点的横坐标的最大值与最小值之差为w1^l，纵坐标的最大值与最小值之差为

step2：在本帧左目灰度图像中，确定左目目标再搜索区域，左目目标再搜索区域是中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_left中的关键特征角点；

在步骤四中得到的本帧右目灰度图像中，确定右目目标再搜索区域，方法如下：

step1：确定上一帧的关键特征角点集合last_right中关键特征角点的横坐标的最大值与最小值之差为w1^r，纵坐标的最大值与最小值之差为

step2：在本帧右目灰度图像中，确定右目目标再搜索区域，右目目标再搜索区域是中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_right中的关键特征角点；

具体地，所述步骤十中的构建模板图像，基于归一化相关性匹配法，分别在左目目标再搜索区域和右目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像和重新确定的右目目标灰度图像；包括以下步骤：

在左目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像，方法如下：

step1：确定模板图像，模板图像用a表示，模板图像a是是在上一帧左目目标灰度图像中，中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_left中的关键特征角点，宽的取值范围为高的取值范围为其中，w1^l为上一帧的关键特征角点集合last_left中关键特征角点的横坐标的最大值与最小值之差，为上一帧的关键特征角点集合last_left中关键特征角点的纵坐标的最大值与最小值之差；

step2：对左目目标再搜索区域进行预处理，得到包含有多个轮廓的轮廓集，预处理包括中值滤波去噪、图像自适应阈值化处理和canny边缘检测；

step3：针对轮廓集中的每一个轮廓设置最小矩形框，最小矩形框的长为构成该轮廓的像素点中的横坐标的最大值与最小值之差，最小矩形框的宽为构成轮廓的像素点中的纵坐标的最大值与最小值之差；轮廓位于最小矩形框的内部；每一个轮廓对应的最小矩形框所在区域图像构成疑似目标图像集；

step4：将模板图像a与疑似目标图像集中的图像统一尺寸，计算模板图像a与疑似目标图像集中的每一个疑似目标图像的相关系数，计算公式如下:

其中，a(x′,y′)为模板图像a在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，i(x′,y′)为图像i在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，r(x,y)为模板图像a与图像i的相关系数，图像i为疑似目标图像集中的任意一个疑似目标图像；

step5：在疑似目标图像集中，选出相关系数最大值对应的疑似目标图像作为重新确定的左目目标灰度图像；

在右目目标再搜索区域内进行目标搜索，得到重新确定的右目目标灰度图像；方法如下：

step1：确定模板图像，模板图像用b表示，模板图像b在上一帧右目目标灰度图像中，中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_right中的关键特征角点，宽的取值范围为高的取值范围为w1^r为上一帧的关键特征角点集合last_right中关键特征角点的横坐标的最大值与最小值之差，为上一帧的关键特征角点集合last_right中关键特征角点的纵坐标的最大值与最小值之差；

step2：对右目目标再搜索区域进行预处理，得到包含有多个轮廓的轮廓集，预处理包括中值滤波去噪、图像自适应阈值化处理和canny边缘检测；

step3：针对轮廓集中的每一个轮廓设置最小矩形框，最小矩形框的长为构成该轮廓的像素点中的横坐标的最大值与最小值之差，最小矩形框的宽为构成轮廓的像素点中的纵坐标的最大值与最小值之差；轮廓位于最小矩形框的内部；每一个轮廓对应的最小矩形框所在区域图像构成疑似目标图像集；

step4：将模板图像b与疑似目标图像集中的图像统一尺寸，计算模板图像b与疑似目标图像集中的每一个疑似目标图像的相关系数，计算公式如下：

其中，b(x′,y′)为模板图像b在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，i(x′,y′)为图像i在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，r(x,y)即为模板图像b与图像i的归一化相关系数，图像i为疑似目标图像集中的任意一个疑似目标图像；

step5：在疑似目标图像集中，选出相关系数最大值对应的疑似目标图像作为重新确定的右目目标灰度图像。

具体地，所述步骤十一中的构建模板图像，采用的方法如下：

模板图像用c表示，模板图像c是在本帧右目灰度图像中，中心点为宽为高为的矩形区域；其中为关键特征角点集合current_right4中的关键特征角点，宽的取值范围为高的取值范围为其中为关键特征角点集合current_right4中关键特征角点的横坐标的最大值与最小值之差，为关键特征角点集合current_right4中关键特征角点的纵坐标的最大值与最小值之差。

具体地，所述步骤十二中的构建模板图像，采用的方法如下：

模板图像用d表示，模板图像d是在本帧左目灰度图像中，中心点为宽为高为的矩形区域；其中为关键特征角点集合current_left4中的关键特征角点，宽的取值范围为高的取值范围为其中为关键特征角点集合current_left4中角点的横坐标的最大值与最小值之差，为关键特征角点集合current_left4中角点的纵坐标的最大值与最小值之差。

与现有技术相比，本发明具有以下技术效果：

1、本发明以4个关键特征角点来表征着陆地标图像，通过金字塔l-k光流法跟踪这4个关键特征角点，来达到跟踪着陆地标的目的，避免直接跟踪整幅地标图像，可大大提高算法的实时性；

2、通过金字塔l-k光流法跟踪到4个关键特征角点后，在每个关键特征角点周围20*20区域内重新进行亚像素级角点检测，保证每一帧所需的用于解算导航信息的角点的精度；

3、将本帧跟踪到的关键特征角点与上一帧的关键特征角点，进行局部相似性匹配检验，保证本帧跟踪到的关键特征角点的准确性；

4、本发明对提取到的4个关键特征角点进行排序之后，关键特征角点一一对应，随后求取视差，计算着陆地标的三维位置信息；此方法不需对整幅图像进行匹配，求取视差，故大大节省时间，提高算法的实时性；

5、本发明对4个关键特征角点进行排序之后，结合关键特征角点对应的世界坐标，基于p4p问题求解算法，计算求取目标中心o在无人机机体坐标系下的三维位置坐标以及无人机的姿态信息，解算得到的三维位置信息与基于双目视觉原理计算得到的三维位置信息加和平均之后，最为最终的三维位置信息，提高算法精度。

附图说明

图1是本发明采用的着陆地标图；

图2是着陆地标上4个关键特征角点示意图；

图3是本发明的方法流程图；

图4是利用提取的关键特征角点进行导航信息解算的流程图；

图5是双目视觉原理示意图。

下面结合附图和具体实施方式对本发明的方案作进一步详细地解释和说明。

具体实施方式

本发明的基于双目视觉的无人机自主着陆导航信息实时解算方法，参见图3，具体包括以下步骤：

步骤一，利用无人机上的机载双目摄像机对着陆区域进行拍摄，获取包含有目标的双目视觉图像，机载双目摄像机包括左目摄像机和右目摄像机，因此双目视觉图像包括左目视觉图像和右目视觉图像；

在左目视觉图像提取目标图像，并对提取出的目标图像进行灰度化，得到左目目标灰度图像；在右目视觉图像提取目标图像，并对提取出的目标图像进行灰度化，得到右目目标灰度图像。此处的目标为着陆地标，目标中心为o。

步骤二，分别对左目目标灰度图像和右目灰度目标图像，利用亚像素级shi-tomasi角点提取算法进行处理，均得到四个关键特征角点，针对左目目标灰度图像得到的四个关键特征角点，根据四个关键特征角点的横纵坐标值的大小进行排序，形成关键特征角点集合current_left1，针对右目目标灰度图像得到的四个关键特征角点，根据四个关键特征角点的横纵坐标值的大小进行排序，形成关键特征角点集合current_right1，参见图2。

关键特征角点集合current_left1中的四个关键特征角点的排列顺序为：纵坐标最小、横坐标最大、纵坐标最大和横坐标最小。

关键特征角点集合current_right1中的四个关键特征角点的排列顺序为：纵坐标最小、横坐标最大、纵坐标最大和横坐标最小。

步骤三，利用关键特征角点集合current_left1和关键特征角点集合current_right1，利用双目视觉原理和p4p求解方法求取无人机自主着陆导航信息，即目标中心o在无人机机体坐标系下的三维位置坐标以及无人机的姿态信息。

步骤3.1，利用关键特征角点集合current_left1和关键特征角点集合current_right1，基于双目视觉原理求取目标中心o在无人机机体坐标系下的三维位置坐标(xs,ys,zs)；具体过程如下：

如图5所示，x^l和x^r分别为限定范围内空间中某点p在左目摄像机和右目摄像机中成像的横坐标，f为左目摄像机和右目摄像机的焦距，d表示p点到双目成像平面的垂直距离，分别表示左目摄像机的摄像头和右目摄像机的摄像头的光心坐标位置，t为左目摄像机和右目摄像机光心间的直线距离，一般称为基线。设d表示左目摄像机和右目摄像机的视差值，可由下式得到：

d＝x^l-x^r

根据相似三角形定理及相关数学推导可以得到公式(1)：

其中：

其中dx为图像单个像素对应的实际尺寸，假设已经校准完毕，由于左目摄像机和右目摄像机的主光线平行，根据平行线定义，可知二者相交于无穷远处，可表示为则由式(1)可得三维坐标为：

参见图2，目标灰度图像中目标中心o为四个关键特征角点构成的正方形的中心，因此可利用关键特征角点集合current_left1求得左目目标灰度图像中目标中心o^l的像素坐标利用关键特征角点集合current_right1求得右目灰度目标图像中目标中心o^r的像素坐标其中，为关键特征角点集合current_left1中的关键特征角点的像素坐标，为关键特征角点集合current_right1中的关键特征角点的像素坐标。

根据公式(2)和左目目标灰度图像中目标中心o^l的像素坐标得到目标中心o^l在左目摄像机坐标系下的三维坐标为：

同理，根据公式(2)和右目灰度目标图像中目标中心o^r的像素坐标得到目标中心o^r在右目摄像机坐标系下的三维坐标为：

由于左目摄像机和右目摄像机安装在无人机上的位置固定，故可知左目摄像机相对于无人机机体坐标系的旋转矩阵为ml，右目摄像机相对于无人机机体坐标系的旋转矩阵为mr；

利用旋转矩阵ml可将目标中心o^l在左目摄像机坐标系下的三维坐标变换为目标中心o^l在无人机机体坐标系下的三维坐标利用利用旋转矩阵mr可将目标中心o^r在左目摄像机坐标系下的三维坐标变换为目标中心o^r在无人机机体坐标系下的三维坐标即：

根据目标中心o^l在无人机机体坐标系下的三维坐标和目标中心o^r在无人机机体坐标系下的三维坐标求得目标中心o在无人机机体坐标系下的三维位置坐标(xs,ys,zs)为：

步骤3.2：利用关键特征角点集合current_left1和关键特征角点集合current_right1，基于p4p求解方法求取目标中心o在无人机机体坐标系下的三维位置坐标以及无人机的姿态信息，具体过程如下：

步骤3.2.1：基于张正友标定法对左目摄像机和右目摄像机进行标定，得到左目摄像机的内外参数和右目摄像机的内外参数；

步骤3.2.2：基于左目摄像机的内外参数、关键特征角点集合current_left1中的关键特征角点的像素坐标以及关键特征角点集合current_left1中的关键特征角点的世界坐标，利用p4p求解方法，获得左目目标灰度图像中目标中心o^l在左目摄像机坐标系下的三维坐标以及无人机的姿态信息：俯仰角θl，滚转角φl，偏航角ψl；具体求解方法可参考文献[2]。

步骤3.2.3：基于右目摄像机的内外参数、关键特征角点集合current_right1中的关键特征角点的像素坐标以及关键特征角点集合current_right1中的关键特征角点的世界坐标，利用p4p求解方法，获得右目目标灰度图像中目标中心o^r在右目摄像机坐标系下的三维坐标以及无人机的姿态信息：俯仰角θr，滚转角φr，偏航角ψr；

步骤3.2.4：利用旋转矩阵ml可将目标中心o^l在左目摄像机坐标系下的三维坐标变换为目标中心o^l在无人机机体坐标系下的三维坐标利用利用旋转矩阵mr可将目标中心o^r在左目摄像机坐标系下的三维坐标变换为目标中心o^r在无人机机体坐标系下的三维坐标即：

步骤3.2.5：根据目标中心o^l在无人机机体坐标系下的三维坐标和目标中心o^r在无人机机体坐标系下的三维坐标求得目标中心o在无人机机体坐标系下的三维位置坐标(xp,yp,zp)为：

根据步骤3.2.2得到的无人机的姿态信息和根据步骤3.2.3得到的无人机的姿态信息，求得最终的无人机的姿态信息：俯仰角θ＝(θl+θr)/2，滚转角φ＝(φl+φr)/2，偏航角ψ＝(ψl+ψr)/2；

步骤3.3：将步骤3.1获得的目标中心o在无人机机体坐标系下的三维位置坐标(xs,ys,zs)和步骤3.2获得的目标中心o在无人机机体坐标系下的三维位置坐标(xp,yp,zp)，进行加和平均，以提高精度，得到最终的目标中心o在无人机机体坐标系下的三维位置坐标为：姿态信息为：俯仰角θ，滚转角φ，偏航角ψ。

步骤四，利用无人机上的机载双目摄像机对着陆区域进行拍摄，获取下一帧包含有目标的双目视觉图像，双目视觉图像包括本帧左目视觉图像和本帧右目视觉图像；对本帧左目视觉图像进行灰度化，得到本帧左目灰度图像，对本帧右目视觉图像进行灰度化，得到本帧右目灰度图像。

步骤五，将利用上一帧左目视觉图像得到的关键特征角点集合current_left1更新为上一帧关键特征角点集合last_left，将利用上一帧右目视觉图像得到的关键特征角点集合current_right1更新为上一帧关键特征角点集合last_right。

步骤六，利用金字塔l-k光流法，基于上一帧关键特征角点集合last_left和本帧左目灰度图像，预测得到本帧的关键特征角点集合current_left2，基于上一帧关键特征角点集合last_left和本帧右目灰度图像，预测得到本帧的关键特征角点集合current_right2。具体求解方法可参考文献[1]。

步骤七，针对关键特征角点集合current_left2中的每一个关键特征角点，在以关键特征角点为中心的周围20*20的区域内，进行亚像素级shi-tomasi角点检测，检测得到新的角点，以检测得到的新的角点替换区域中心位置的关键特征角点，从而将关键特征角点集合current_left2更新为关键特征角点集合current_left3；同理，将关键特征角点集合current_right2更新为关键特征角点集合current_right3；

步骤八，计算关键特征角点集合current_left3中的关键特征角点与上一帧关键特征角点集合last_left中的关键特征角点之间的相关系数，根据计算得到的相关系数对关键特征角点集合current_left3进行筛选，得到关键特征角点集合current_left4；

计算关键特征角点集合current_right3中的关键特征角点与上一帧关键特征角点集合last_right中的关键特征角点之间的相关系数，根据计算得到的相关系数对关键特征角点集合current_right3进行筛选，得到关键特征角点集合current_right4，具体过程如下：

记为关键特征角点集合current_left3中的第i个关键特征角点，为上一帧的关键特征角点集合last_left中对应的一个关键特征角点，分别以关键特征角点和为中心的20*20大小的区域通过归一化相关系数匹配计算公式，计算两个关键特征角点的相关系数，计算公式如下：

其中，t(x′,y′)为图像t在像素点(x′,y′)处的灰度值，i(x′,y′)为图像i在(x′,y′)处的灰度值；图像t为以关键特征角点为中心的20*20大小的区域形成的图像，构成该图像的像素点用(x′,y′)表示；图像i为以特征角点为中心的20*20大小的区域形成的图像，构成该图像的像素点用(x′,y′)表示；为图像t与图像i的归一化相关系数，也就是关键特征角点集合current_left3中角点与上一帧的关键特征角点集合last_left中对应角点的相关系数。

以计算得到的相关系数对关键特征角点集合current_left3进行筛选，去除跟踪效果较差的角点，即当针对关键特征角点计算得到的满足则认为该关键特征角点局部相似，跟踪效果较好，则保留；反之，则认为跟踪效果较差，将该关键特征角点由关键特征角点集合current_left3中去除，经过上述处理后得到筛选后的关键特征角点集合current_left4；threshold基于实际情况设定，threshold＝0.75。

同理，可由关键特征角点集合current_right3与上一帧的关键特征角点集合last_right，得到筛选后的关键特征角点集合current_right4。

步骤九，若关键特征角点集合current_left4与关键特征角点集合current_right4中的关键特征角点数目都为4，则进入步骤三，其中的关键特征角点集合current_left1和关键特征角点集合current_right1分别用关键特征角点集合current_left4和关键特征角点集合current_right4代替；

若关键特征角点集合current_left4与关键特征角点集合current_right4中的关键特征角点数目都小于4，进入步骤十；

若关键特征角点集合current_left4中的键特征角点数目小于4，关键特征角点集合current_right4中的关键特征角点数目等于4，进入步骤十一；

若关键特征角点集合current_left4中的键特征角点数目等于4，关键特征角点集合current_right4中的关键特征角点数目小于4，进入步骤十二；

步骤十，分别在步骤四中得到的本帧左目灰度图像和本帧右目灰度图像中，确定左目目标再搜索区域和右目目标再搜索区域；

基于归一化相关性匹配法分别在左目目标再搜索区域和右目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像和重新确定的右目目标灰度图像；

分别针对重新确定的左目目标灰度图像和重新确定的右目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_left5和关键特征角点集合current_right5；执行步骤三，将其中的关键特征角点集合current_left1和关键特征角点集合current_right1分别用键特征角点集合current_left5和关键特征角点集合current_right5代替；

步骤10.1，在步骤四中得到的本帧左目灰度图像中，确定左目目标再搜索区域；在步骤四中得到的本帧右目灰度图像中，确定右目目标再搜索区域；具体确定方法如下：

在步骤四中得到的本帧左目灰度图像中，确定左目目标再搜索区域的方法如下：

step1：计算上一帧的关键特征角点集合last_left中关键特征角点的横坐标的最大值与最小值之差为w1^l，纵坐标的最大值与最小值之差为

step2：在本帧左目灰度图像中，确定左目目标再搜索区域，左目目标再搜索区域是中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_left中的关键特征角点；

在步骤四中得到的本帧右目灰度图像中，确定右目目标再搜索区域，方法如下：

step1：确定上一帧的关键特征角点集合last_right中关键特征角点的横坐标的最大值与最小值之差为w1^r，纵坐标的最大值与最小值之差为

step2：在本帧右目灰度图像中，确定右目目标再搜索区域，右目目标再搜索区域是中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_right中的关键特征角点；

步骤10.2，基于归一化相关性匹配法在左目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像；在右目目标再搜索区域内进行目标搜索，得到重新确定的右目目标灰度图像；具体过程如下：

在左目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像，方法如下：

step1：确定模板图像，模板图像用a表示，模板图像a是在上一帧左目目标灰度图像中，中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_left中的关键特征角点，宽的取值范围为高的取值范围为

step2：对左目目标再搜索区域进行预处理，得到包含有多个轮廓的轮廓集，预处理包括中值滤波去噪、图像自适应阈值化处理和canny边缘检测；

step3：针对轮廓集中的每一个轮廓设置最小矩形框，最小矩形框的长为构成该轮廓的像素点中的横坐标的最大值与最小值之差，最小矩形框的宽为构成轮廓的像素点中的纵坐标的最大值与最小值之差；轮廓位于最小矩形框的内部；每一个轮廓对应的最小矩形框所在区域图像构成疑似目标图像集；

step4：将模板图像a与疑似目标图像集中的图像统一尺寸(256*256)，计算模板图像a与疑似目标图像集中的每一个疑似目标图像的相关系数，计算公式如下:

其中，t(x′,y′)为模板图像a在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，i(x′,y′)为图像i在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，r(x,y)为模板图像a与图像i的相关系数，图像i为疑似目标图像集中的任意一个疑似目标图像；

step5：在疑似目标图像集中，选出相关系数最大值对应的疑似目标图像作为重新确定的左目目标灰度图像。

在右目目标再搜索区域内进行目标搜索，得到重新确定的右目目标灰度图像；方法如下：

step1：确定模板图像，模板图像用b表示，模板图像b在上一帧右目目标灰度图像中，中心点为宽为高为的矩形区域；其中，其中为上一帧的关键特征角点集合last_right中的关键特征角点，宽的取值范围为高的取值范围为

step2：对右目目标再搜索区域进行预处理，得到包含有多个轮廓的轮廓集，预处理包括中值滤波去噪、图像自适应阈值化处理和canny边缘检测；

step3：针对轮廓集中的每一个轮廓设置最小矩形框，最小矩形框的长为构成该轮廓的像素点中的横坐标的最大值与最小值之差，最小矩形框的宽为构成轮廓的像素点中的纵坐标的最大值与最小值之差；轮廓位于最小矩形框的内部；每一个轮廓对应的最小矩形框所在区域图像构成疑似目标图像集；

step4：将模板图像b与疑似目标图像集中的图像统一尺寸(256*256)，计算模板图像b与疑似目标图像集中的每一个疑似目标图像的相关系数，计算公式如下:

中，b(x′,y′)为模板图像b在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，i(x′,y′)为图像i在像素点(x′,y′)处的灰度值，构成该图像的像素点用(x′,y′)表示，r(x,y)即为模板图像b与图像i的归一化相关系数，图像i为疑似目标图像集中的任意一个疑似目标图像。

step5：在疑似目标图像集中，选出相关系数最大值对应的疑似目标图像作为重新确定的右目目标灰度图像。

步骤10.3，针对重新确定的左目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_left5；针对重新确定的右目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_right5；

步骤10.4：执行步骤三，将其中的关键特征角点集合current_left1和关键特征角点集合current_right1分别用关键特征角点集合current_left5和关键特征角点集合current_right5代替；

步骤十一，在步骤四中得到的本帧左目灰度图像中，确定左目目标再搜索区域；构建模板图像，基于归一化相关性匹配法，在左目目标再搜索区域内进行目标搜索，得到重新确定的左目目标灰度图像；针对重新确定的左目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_left5；

具体实现方法同步骤十，但其中模板图像的确定方法不同，此时的模板图像用c表示，模板图像c是在本帧右目灰度图像中，中心点为宽为高为的矩形区域；其中为关键特征角点集合current_right4中的关键特征角点，宽的取值范围为高的取值范围为其中为关键特征角点集合current_right4中关键特征角点的横坐标的最大值与最小值之差，为关键特征角点集合current_right4中关键特征角点的纵坐标的最大值与最小值之差。

执行步骤三，将关键特征角点集合current_left1和关键特征角点集合current_right1分别用关键特征角点集合current_left5和关键特征角点集合current_right4代替；

步骤十二，在步骤四中得到的本帧右目灰度图像中，确定右目目标再搜索区域；构建模板图像，基于归一化相关性匹配法，在右目目标再搜索区域内进行目标搜索，得到重新确定的右目目标灰度图像；针对重新确定的右目目标灰度图像，采用亚像素级shi-tomasi角点提取方法提取4个关键特征角点，提取到的4个关键特征角点形成关键特征角点集合current_right5；

具体实现方法同步骤十，但其中模板图像的确定方法不同，此时的模板图像用d表示，模板图像d是在本帧左目灰度图像中，中心点为宽为高为的矩形区域；其中为关键特征角点集合current_left4中的关键特征角点，宽的取值范围为高的取值范围为其中为关键特征角点集合current_left4中角点的横坐标的最大值与最小值之差，为关键特征角点集合current_left4中角点的纵坐标的最大值与最小值之差。

执行步骤三，将关键特征角点集合current_left1和关键特征角点集合current_right1分别用关键特征角点集合current_left4和关键特征角点集合current_right5代替。