CN109029451A - 基于网络信标矫正的小型无人机自主定位方法 - Google Patents

Abstract

公开一种基于网络信标矫正的小型无人机自主定位方法，(1)基于摄像头、压力传感器、方向角传感器，确定无构图定位方法MLVO；(2)针对无构图定位算法中的累积误差问题，引入网络地图资源，选择具有缓变性和连续性的标志物作为导航信标，以其GPS全局坐标作为先验信息，预先查找任务区域中这类标志物的GPS坐标，存储至数据库中；设计基于网络信标矫正的自主定位算法，与第一步的MLVO算法构成回路，用以消除在无人机相对定位过程中的累积误差，改善相对定位算法的定位误差漂移的问题。本发明能基于无人机采集的视频序列初步计算无人机的位置，然后融合网络地图，消除前期定位过程中累积的误差漂移，提高整体定位精度与稳定性。

Description

基于网络信标矫正的小型无人机自主定位方法

技术领域

本发明主要涉及无人机的设计领域，特指一种基于网络信标校正的小型无人机自主定位方法。

背景技术

无人机最早诞生于上世纪初，由于其可替代人类执行危险、枯燥的任务，受到了军事和民用领域多方面的关注。近年来小型无人机以其轻巧的体积，敏捷的运动，以及可实现定点悬停、目标跟踪等功能，在单兵作战、抢险救灾、环境勘探和物流快递等领域表现出很大应用潜力。目前小型无人机还主要用于宽阔无遮挡的室外环境，主要依赖GPS模块定位，但随着其应用领域的不断拓展，未来小型无人机将更多的应用于室内、隧道或山区等GPS信号不稳定甚至缺失的环境中，这就需要其能够利用自带的传感器对周围的环境进行自主感知与定位。使其能够在无GPS信号覆盖的室内或狭小区域仍能开展作业，完成侦查救援、生命物资投递等任务。在单兵作战过程中，具备自主定位功能的小型无人机可穿梭于GPS信号受干扰或禁用区域，及时为作战人员提供最新情报，辅助部队完成任务。如何能够在无GPS信号覆盖的室内或狭小区域完成自主定位，是目前小型无人机定位技术领域中所面临的主要技术问题之一。

为了实现无人机的自主定位，多传感器融合是目前的主要方向之一，即通过融合机载视觉、陀螺仪、加速度计、压力计等传感器的数据，使得不同传感器的测量性能得到补充，定位过程更加可靠精准。但随着融合的传感器种类越来越多，小型无人机的载荷能力有限，为此，可以选择MEMS传感器作为机载传感器，这与小型无人机对载荷要求成本低廉、体积轻巧的需求不谋而合。如果将MEMS传感器充分利用，则将适用于众多的小型无人机平台，具有广阔前景。但目前MEMS传感器普遍精度较差，因此如何设计一个满足定位需求、计算速度快的自主定位方法，是目前将MEMS传感器用于小型无人机定位的主要瓶颈。

发明内容

本发明针对现有技术存在的不足，提出一种基于网络信标矫正的小型无人机自主定位方法，具体如下：

采用东北天坐标系作为定位过程中的世界坐标系，原点为初始时刻的机载摄像头中心，以符号″w″表示；令初始时刻的摄像头坐标系与世界坐标系三轴重合；

步骤一：基于摄像头、压力传感器、方向角传感器，确定无构图定位方法MLVO；包括算法初始化，摄像头姿态估计，平移量估计和关键帧设置四个模块；其中，姿态估计和平移量估计是方法框架的核心，具体步骤如下：

步骤1.1：设t时刻摄像头获取的图像记为I_t，首先判断I_t是否为首帧，即第一帧图像，如果是首帧图像，则转到步骤1.2，否则转到步骤1.3；

步骤1.2：如果图像I_t为首帧图像，则进行方法初始化，具体如下：将每个图像分解为N×N个子区域，对每个子区域采用FAST角点检测方法提取图像I_t的特征点，特征点坐标记为p_t，然后将首帧图像I_t设为关键帧，记为I_k，并将p_t作为在后续图像中待跟踪的特征点，读取下一帧图像；

步骤1.3：跟踪特征点；如果当前帧图像I_t不是首帧图像，则在当前帧图像中利用Lucas-Kanade金字塔光流法跟踪p_t的对应点，跟踪得到的坐标点记为p_k，转到步骤1.4；

步骤1.4：判断跟踪到的特征点数，如果有至少有T_p个特征点都被成功跟踪上，则转到步骤1.5，否则转到步骤1.7；

步骤1.5：摄像头姿态估计；该步骤分为基于视觉的摄像头姿态估计、卡尔曼滤波和优化后的摄像头姿态；下面分别对每部分方法过程展开介绍：

(1)基于视觉的摄像头姿态估计：

①基于步骤1.3跟踪得到的特征点对(p_t，p_k)，采用RANSAC-8点算法求解本征矩阵E，然后对本征矩阵进行奇异值分解，得到两个3×3阶的酉矩阵U和V；

②通过下式求解图像I_t相对图像I_k的旋转矩阵和平移向量的反对称矩阵

其中，的旋转矩阵，具体为即沿z轴坐标旋转正负90度的旋转矩阵；

③摄像头的姿态角可由下式求出：

其中，以及分别表示图像I_t对应的相机坐标系与图像I_k对应的相机坐标系之间的俯仰角、滚动角以及偏航角，atan为反正切，表示矩阵的第i行第j列的元素；姿态角转换成四元数后记为

④通过步骤①-③可得到t时刻摄像头相对于世界坐标系的姿态角四元数为即视觉估计值

其中，表示图像I_k对应的相机坐标系相对世界坐标系的姿态角四元素；

(2)卡尔曼滤波：

I.状态方程：t时刻的摄像头坐标系相对世界坐标系w的姿态角用四元数表示，其导数可以写成：

其中，为t时刻的摄像头坐标系相对世界坐标系的旋转角速度，表示四元数的乘法；由于角速度具有连续变化的特性，在采样时间间隔很小时，可假设其与前一时刻优化得到的角速度保持不变，即：

联立方程(7)和(8)，通过4阶龙格-库塔Runge-Kutta方法求出下一时刻的摄像头姿态角估计值

II.观测方程：，为了对前一步得到的估计值进行校正，通过式(9)融合方向角传感器和摄像头提供的图像信息提供相应的观测值：

其中，表示融合后摄像头t时刻的最优姿态角，表示方向角传感器t时刻的测量值，表示t时刻的方向角传感器测量值对应的欧拉角表示，为步骤⑤中计算得到的姿态角四元素对应的欧拉角表示；T_w为阈值参数；是通过式(7)、(8)得到的姿态角估计值，e(.)表示方向角传感器的误差函数，该误差函数通过预先传感器误差评估方法得到；

(3)优化后的摄像头姿态：联立方程(7)，(8)和(9)，通过卡尔曼滤波算法可估计出其代表摄像头t时刻相对初始时刻的姿态最优值；

步骤1.6：摄像头平移量估计；该步骤分为基于视觉的摄像头平移估计、卡尔曼滤波和优化后的摄像头平移量；下面分别对每部分计算过程展开介绍：

1)基于视觉的摄像头平移估计：

i.基于公式(1)得到的将其代入下式可得：

其中，表示t时刻摄像头坐标系相对k时刻摄像头坐标系的平移向量，(p_t，p_k)表示步骤1.3得到的图像I_t相对图像I_k的特征点对，(λ_t，λ_k)分别是特征点(p_t，p_k)在摄像头坐标系下的深度信息；以压力传感器的测量值作为其深度信息；

ii.遍历图中所有的特征点对，设有M组，将得到M组3×1的平移向量，对每组平移向量通过下式求解其与其他平移向量的曼哈顿Manhattan距离d_ij，即

iii.如果d_ij≤T_d，其中T_d表示算法预先设定的阈值，则对应观测向量的票数加1，以此类推，建立所有特征点对的平移向量得票数；

iv.将得票数最高的平移向量选出，假设为第1个平移向量则摄像头平移向量观测值可通过下式求得：

其中表示与向量的距离小于T_d的平移量，即M′表示对应的数量；

vi.通过步骤i-iv可基于视觉得到图像I_t对应的摄像头坐标系相对世界坐标系的平移向量

其中，表示图像I_k对应的摄像头坐标系相对世界坐标系的平移向量；

2)卡尔曼滤波：

1.状态方程：t时刻的摄像头坐标系相对世界坐标系的平移量可记为一个(3×1)的向量其导数可以写成：

其中表示t时刻摄像头相对世界坐标系的运动速度，由于速度具有连续变化的特性，因此在采样时间间隔很小时，假设其与前一时刻优化后的运动速度保持不变，即有：

联立方程(11)和(12)，通过4阶龙格-库塔方法便可求出下一时刻的摄像头平移量估计值

2.观测方程：以公式(13)计算得到的作为观测值；

3)优化后的摄像头平移量；联立方程(13)，(14)和(15)，通过卡尔曼滤波算法可估计出摄像头t时刻相对初始时刻的平移量最优值

通过步骤1.5和步骤1.6可以估计出摄像头t时刻的位置即为然后读取下一帧图像；

步骤1.7：当匹配点数不足时，设置当前帧图像为第k+1帧关键帧，记为I_k+1，将该图像分解为N×N个子区域，对每个子区域采用FAST角点检测方法提取图像I_k+1的特征点，替换之前得到的p_t，并将p_t作为在后续图像中待跟踪的特征点，读取下一帧图像；

步骤二：针对无构图定位算法中的累积误差问题，引入网络地图资源，选择具有缓变性和连续性的标志物作为导航信标，以其GPS全局坐标作为先验信息，预先查找任务区域中这类标志物的GPS坐标，存储至数据库中；设计基于网络信标矫正的自主定位算法，与第一步的MLVO算法构成回路，用以消除在无人机相对定位过程中的累积误差，改善相对定位算法的定位误差漂移的问题；

(1)坐标转换；以符号L₁，L₂，...L_n表示n个标志物，它们的GPS位置信息已预先通过网络地图获得，存至数据库中；为了使方法结果可统一处理，需预先将GPS坐标转换到世界坐标系中，转换公式为：

其中，X_Li是第i个标志物在世界坐标系w中的坐标，(λ_i，φ_i，h_i)表示第i个标志物的GPS坐标中的经度、纬度和高度；r_e表示地球长半轴，e为地球第一偏心率；

(2)飞机在飞行过程中一直开启摄像头进行标志物检测，检测算法为Adaboost算法，目标框中心点即为标志物在图像中的位置坐标；

(3)设飞机t时刻在世界坐标系下的位置估计值为(x_wt，y_wt，z_wt)；当飞机检测到标志物时，则以坐标(x_wt，y_wt，z_wt)为中心，r为半径搜索数据库中的所有标志物，并将搜索到的标志物加载到概率地图中，搜索公式如下，假设有S个标志物在搜索半径内：

其中，X′_Li表示满足上述搜索条件的第i个标志物在世界坐标系下的坐标，x_Li，y_Li，z_Li分别表示第i个标志物在世界坐标系三个坐标轴X、Y、Z下的坐标值；

(4)结合步骤(2)和步骤(3)，通过下式推算出飞机当前时刻在世界坐标系下的可能位置坐标为：

其中，(x_Li，y_Li，z_Li)^T表示通过步骤(3)得到的第i个标志物在世界坐标系下的坐标；(u_xi，u_yi)表示通过步骤(2)检测到的第i个标志物在图像中的位置坐标，单位是像素；(u_cx，u_cy)是图像的中心坐标；(k_x，k_y)表示单位水平像素和垂直像素的尺度，通过摄像头标定可以得到；z_wt为t时刻飞机的飞行高度；表示通过第i个标志物位置推算得到的飞机t时刻在世界坐标系下的坐标；

(5)无人机继续飞行，按照步骤(2)和步骤(3)接着检测标志物，在t+1时刻再次检测到标志物时，通过式(18)计算t+1时刻无人机在世界坐标系中的位置，记为：

(6)另一方面，随着无人机平台的继续运动，无人机可能的位置信息也在不断更新，t+1时刻无人机的在世界坐标系中的位置由下式获得：

其中，ΔX_t+1，t＝(Δx_t+1，t，Δy_t+1，t，Δz_t+1，t)^T是从t时刻到t+1时刻的无人机位移增量，由下式计算得到：

其中，和分别表示t+1时刻和t时刻摄像头相对世界坐标系的平移量，由步骤1.6得到；

(7)通过步骤(5)和步骤(6)都能估计得到无人机t+1时刻在世界坐标系中的坐标，分别为和按照式(21)估计各位置之间的距离，如果距离小于某一阈值，则通过式(22)对无人机当前位置信息进行更新，否则返回步骤(1)重新检测；

本发明通过设计一种融合算法，提供一种适用于小型无人机平台、计算速度快、对存储资源要求低、精度可靠且稳定的自主定位方法。与现有技术相比，本发明的优点在于：本发明的基于网络信标矫正的小型无人机自主定位方法，能基于无人机采集的视频序列初步计算无人机的位置，然后融合网络地图，消除前期定位过程中累积的误差漂移，提高整体定位精度与稳定性。

附图说明

图1是无构图机载定位方法的框架图，是具体实施方式中步骤一的算法框架；

图2是本发明标志物搜索示意图。

具体实施方式

为了进一步说明本发明，以下内容分两步骤分别阐述。

采用东北天坐标系作为定位过程中的世界坐标系，原点为初始时刻的机载摄像头中心(以符号″w″表示)。令初始时刻的摄像头坐标系与世界坐标系三轴重合。

步骤一：基于摄像头、压力传感器、方向角传感器，确定无构图定位方法(MLVO)，算法框架如图1所示。包括算法初始化，摄像头姿态估计，平移量估计和关键帧设置四个模块。其中，姿态估计和平移量估计是方法框架的核心，具体步骤如下：

步骤1.1：如图1所示，设t时刻摄像头获取的图像记为I_t，首先判断I_t是否为首帧(第一帧)图像，如果是首帧图像，则转到步骤1.2，否则转到步骤1.3。

步骤1.2：如果图像I_t为首帧图像，则进行方法初始化，具体如下：将每个图像分解为N×N个子区域，对每个子区域采用FAST角点检测方法(Rosten E，Drummond T.MachineLearning for High-Speed Corner Detection.Springer Berlin Heidelberg，2006)提取图像I_t的特征点，特征点坐标记为p_t，然后将首帧图像I_t设为关键帧，记为I_k，并将p_t作为在后续图像中待跟踪的特征点，读取下一帧图像。

步骤1.3：跟踪特征点。如果当前帧图像I_t不是首帧图像，则在当前帧图像中利用Lucas-Kanade金字塔光流法(Lucas B D，Kanade T.An iterative image registrationtechnique with an application to stereo vision.International Joint Conferenceon Artificial Intelligence.Morgan Kaufmann Publishers Inc.1981)跟踪p_t的对应点，跟踪得到的坐标点记为p_k，转到步骤1.4。

步骤1.4：判断跟踪到的特征点数，如果有至少有T_p个特征点都被成功跟踪上，则转到步骤1.5，否则转到步骤1.7。

步骤1.5：摄像头姿态估计。该步骤分为基于视觉的摄像头姿态估计、卡尔曼滤波和优化后的摄像头姿态。下面分别对每部分方法过程展开介绍：

(1)基于视觉的摄像头姿态估计：

①基于步骤1.3跟踪得到的特征点对(p_t，p_k)，采用RANSAC-8点算法(Ma Yi，etal.An invitation to 3D vision from images to geometric models.ThePhotogrammetric Record，2004，19(108)：415-416)求解本征矩阵E，然后对本征矩阵进行奇异值分解，得到两个3×3阶的酉矩阵U和V；

②通过下式求解图像I_t相对图像I_k的旋转矩阵和平移向量的反对称矩阵

其中，的旋转矩阵，具体为即沿z轴坐标旋转正负90度的旋转矩阵。

③摄像头的姿态角可由下式求出：

其中，以及分别表示图像I_t对应的相机坐标系与图像I_k对应的相机坐标系之间的俯仰角、滚动角以及偏航角，a tan为反正切，表示矩阵的第i行第j列的元素。姿态角转换成四元数后记为(陈志明等，全角度欧拉角与四元素转换研究，中国科技论文在线，2006)。

④通过步骤①-③可得到t时刻摄像头相对于世界坐标系的姿态角四元数为即视觉估计值

其中，表示图像I_k对应的相机坐标系相对世界坐标系的姿态角四元素。

(2)卡尔曼滤波：

I状态方程：t时刻的摄像头坐标系相对世界坐标系w的姿态角用四元数表示，其导数可以写成：

其中，为t时刻的摄像头坐标系相对世界坐标系的旋转角速度，表示四元数的乘法。由于角速度具有连续变化的特性，在采样时间间隔很小时，可假设其与前一时刻优化得到的角速度保持不变，即：

联立方程(7)和(8)，通过4阶龙格-库塔(Runge-Kutta)方法(高普云，吴志桥，任钧国。Runge-Kutta方法求解结构动力学方程。***仿真学报。2010，09期(9))求出下一时刻的摄像头姿态角估计值

II观测方程：，为了对前一步得到的估计值进行校正，通过式(9)融合方向角传感器和摄像头提供的图像信息提供相应的观测值：

其中，表示融合后摄像头t时刻的最优姿态角，表示方向角传感器t时刻的测量值，表示t时刻的方向角传感器测量值对应的欧拉角表示，为步骤⑤中计算得到的姿态角四元素对应的欧拉角表示。T_w为阈值参数。是通过式(7)、(8)得到的姿态角估计值，e(.)表示方向角传感器的误差函数(通过预先传感器误差评估方法得到，具体实施方法参见文献(Zhaoboxin，Industrial Robot，2015))。

(3)优化后的摄像头姿态：联立方程(7)，(8)和(9)，通过卡尔曼滤波算法(宋文尧，张牙，卡尔曼滤波，科学出版社，1991)可估计出其代表摄像头t时刻相对初始时刻的姿态最优值。

步骤1.6：摄像头平移量估计。该步骤分为基于视觉的摄像头平移估计、卡尔曼滤波和优化后的摄像头平移量。下面分别对每部分计算过程展开介绍：

1)基于视觉的摄像头平移估计：

i基于公式(1)得到的将其代入下式可得：

其中，表示t时刻摄像头坐标系相对k时刻摄像头坐标系的平移向量，(p_t，p_k)表示步骤1.3得到的图像I_t相对图像I_k的特征点对，(λ_t，λ_k)分别是特征点(p_t，p_k)在摄像头坐标系下的深度信息。在本发明中，以压力传感器的测量值作为其深度信息。

ii遍历图中所有的特征点对(设有M组)，将得到M组3×1的平移向量，对每组平移向量通过下式求解其与其他平移向量的曼哈顿(Manhattan)距离d_ij，即

iii如果d_ij≤T_d，其中T_d表示算法预先设定的阈值，则对应观测向量的票数加1，以此类推，建立所有特征点对的平移向量得票数。

iv将得票数最高的平移向量选出，假设为第1个平移向量则摄像头平移向量观测值可通过下式求得：

其中表示与向量的距离小于T_d的平移量，即M′表示对应的数量。

vi通过步骤i-iv可基于视觉得到图像I_t对应的摄像头坐标系相对世界坐标系的平移向量

其中，表示图像I_k对应的摄像头坐标系相对世界坐标系的平移向量。

2)卡尔曼滤波：

1.状态方程：t时刻的摄像头坐标系相对世界坐标系的平移量可记为一个(3×1)的向量其导数可以写成：

其中表示t时刻摄像头相对世界坐标系的运动速度，由于速度具有连续变化的特性，因此在采样时间间隔很小时，本发明假设其与前一时刻优化后的运动速度保持不变，即有：

联立方程(11)和(12)，通过4阶龙格-库塔方法(高普云，吴志桥，任钧国。Runge-Kutta方法求解结构动力学方程。***仿真学报。2010，09期(9))便可求出下一时刻的摄像头平移量估计值

2.观测方程：以公式(13)计算得到的作为观测值。

3)优化后的摄像头平移量。联立方程(13)，(14)和(15)，通过卡尔曼滤波算法(宋文尧，张牙，卡尔曼滤波，科学出版社，1991)可估计出摄像头t时刻相对初始时刻的平移量最优值

通过步骤1.5和步骤1.6可以估计出摄像头t时刻的位置即为然后读取下一帧图像。

步骤1.7：当匹配点数不足时，设置当前帧图像为第k+1帧关键帧，记为I_k+1，将该图像分解为N×N个子区域，对每个子区域采用FAST角点检测方法(Rosten E，DrummondT.Machine Learning for High-Speed Corner Detection.Springer BerlinHeidelberg，2006)提取图像I_k+1的特征点，替换之前得到的p_t，并将p_t作为在后续图像中待跟踪的特征点，读取下一帧图像。

步骤二：针对无构图定位算法中的累积误差问题，引入网络地图资源，选择具有缓变性和连续性的标志物作为导航信标，以其GPS全局坐标作为先验信息，预先查找任务区域中这类标志物的GPS坐标，存储至数据库中。设计基于网络信标矫正的自主定位算法，与第一步的MLVO算法构成回路，用以消除在无人机相对定位过程中的累积误差，改善相对定位算法的定位误差漂移的问题。

(1)坐标转换。以符号L₁，L₂，...L_n表示n个标志物，它们的GPS位置信息已预先通过网络地图获得，存至数据库中。为了使方法结果可统一处理，需预先将GPS坐标转换到世界坐标系中，转换公式为：

其中，X_Li是第i个标志物在世界坐标系w中的坐标，(λ_i，φ_i，h_i)表示第i个标志物的GPS坐标中的经度、纬度和高度。r_e表示地球长半轴，e为地球第一偏心率。

(2)飞机在飞行过程中一直开启摄像头进行标志物检测，检测算法为Adaboost算法(采用opencv_cascadetrain进行训练的步骤及注意事项，blog.csdn.net/xidianzhimeng/article/details/10470839)，目标框中心点即为标志物在图像中的位置坐标。

(3)设飞机t时刻在世界坐标系下的位置估计值为(x_wt，y_wt，z_wt)。当飞机检测到标志物时，则以坐标(x_wt，y_wt，z_wt)为中心，r为半径搜索数据库中的所有标志物，并将搜索到的标志物加载到概率地图中，如图2所示，搜索公式如下，假设有S个标志物在搜索半径内：

其中，X′_Li表示满足上述搜索条件的第i个标志物在世界坐标系下的坐标，x_Li，y_Li，z_Li分别表示第i个标志物在世界坐标系三个坐标轴X、Y、Z下的坐标值。

(4)结合步骤(2)和步骤(3)，通过下式推算出飞机当前时刻在世界坐标系下的可能位置坐标为：

其中，(x_Li，y_Li，z_Li)^T表示通过步骤(3)得到的第i个标志物在世界坐标系下的坐标；(u_xi，u_yi)表示通过步骤(2)检测到的第i个标志物在图像中的位置坐标，单位是像素；(u_cx，u_cy)是图像的中心坐标；(k_x，k_y)表示单位水平像素和垂直像素的尺度，通过摄像头标定(使用MATLAB工具箱TOOLBOX_calib标定摄像头过程，blog.csdn.net/moluoyu/article/details/51419024)可以得到；z_wt为t时刻飞机的飞行高度。表示通过第i个标志物位置推算得到的飞机t时刻在世界坐标系下的坐标。

(5)无人机继续飞行，按照步骤(2)和步骤(3)接着检测标志物，在t+1时刻再次检测到标志物时，通过式(18)计算t+1时刻无人机在世界坐标系中的位置，记为：

(6)另一方面，随着无人机平台的继续运动，无人机可能的位置信息也在不断更新，t+1时刻无人机的在世界坐标系中的位置由下式获得：

其中，ΔX_t+1，t＝(Δx_t+1，t，Δy_t+1，t，Δz_t+1，t)T是从t时刻到t+1时刻的无人机位移增量，由下式计算得到：

其中，和分别表示t+1时刻和t时刻摄像头相对世界坐标系的平移量，由步骤1.6得到。

(7)通过步骤(5)和步骤(6)都能估计得到无人机t+1时刻在世界坐标系中的坐标，分别为和按照式(21)估计各位置之间的距离，如果距离小于某一阈值，则通过式(22)对无人机当前位置信息进行更新，否则返回步骤(1)重新检测。

综上所述，本发明针对该定位过程中存在的定位误差漂移，引入网络标志物信息，对误差进行校正，方法具有较高的精度和稳定性。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通基数人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (1)

1.一种基于网络信标矫正的小型无人机自主定位方法，具体如下：

采用东北天坐标系作为定位过程中的世界坐标系，原点为初始时刻的机载摄像头中心，以符号″w″表示；令初始时刻的摄像头坐标系与世界坐标系三轴重合；

步骤一：基于摄像头、压力传感器、方向角传感器，确定无构图定位方法MLVO；包括算法初始化，摄像头姿态估计，平移量估计和关键帧设置四个模块；其中，姿态估计和平移量估计是方法框架的核心，具体步骤如下：

步骤1.1：设t时刻摄像头获取的图像记为I_t，首先判断I_t是否为首帧，即第一帧图像，如果是首帧图像，则转到步骤1.2，否则转到步骤1.3；

步骤1.2：如果图像I_t为首帧图像，则进行方法初始化，具体如下：将每个图像分解为N×N个子区域，对每个子区域采用FAST角点检测方法提取图像I_t的特征点，特征点坐标记为p_t，然后将首帧图像I_t设为关键帧，记为I_k，并将p_t作为在后续图像中待跟踪的特征点，读取下一帧图像；

步骤1.3：跟踪特征点；如果当前帧图像I_t不是首帧图像，则在当前帧图像中利用Lucas-Kanade金字塔光流法跟踪p_t的对应点，跟踪得到的坐标点记为p_k，转到步骤1.4；

步骤1.4：判断跟踪到的特征点数，如果有至少有T_p个特征点都被成功跟踪上，则转到步骤1.5，否则转到步骤1.7；

步骤1.5：摄像头姿态估计；该步骤分为基于视觉的摄像头姿态估计、卡尔曼滤波和优化后的摄像头姿态；下面分别对每部分方法过程展开介绍：

(1)基于视觉的摄像头姿态估计：

①基于步骤1.3跟踪得到的特征点对(p_t，p_k)，采用RANSAC-8点算法求解本征矩阵E，然后对本征矩阵进行奇异值分解，得到两个3×3阶的酉矩阵U和V；

②通过下式求解图像I_t相对图像I_k的旋转矩阵和平移向量T_t ^k的反对称矩阵

其中，具体为即沿z轴坐标旋转正负90度的旋转矩阵；

③摄像头的姿态角可由下式求出：

其中，以及分别表示图像I_t对应的相机坐标系与图像I_k对应的相机坐标系之间的俯仰角、滚动角以及偏航角，a tan为反正切，表示矩阵的第i行第j列的元素；姿态角转换成四元数后记为

④通过步骤①-③可得到t时刻摄像头相对于世界坐标系的姿态角四元数为即视觉估计值

其中，表示图像I_k对应的相机坐标系相对世界坐标系的姿态角四元素；

(2)卡尔曼滤波：

I.状态方程：t时刻的摄像头坐标系相对世界坐标系w的姿态角用四元数表示，其导数可以写成：

其中，为t时刻的摄像头坐标系相对世界坐标系的旋转角速度，表示四元数的乘法；由于角速度具有连续变化的特性，在采样时间间隔很小时，可假设其与前一时刻优化得到的角速度保持不变，即：

联立方程(7)和(8)，通过4阶龙格-库塔Runge-Kutta方法求出下一时刻的摄像头姿态角估计值

II.观测方程：，为了对前一步得到的估计值进行校正，通过式(9)融合方向角传感器和摄像头提供的图像信息提供相应的观测值：

其中，表示融合后摄像头t时刻的最优姿态角，表示方向角传感器t时刻的测量值，表示t时刻的方向角传感器测量值对应的欧拉角表示，为步骤⑤中计算得到的姿态角四元素对应的欧拉角表示；T_w为阈值参数；是通过式(7)、(8)得到的姿态角估计值，e(·)表示方向角传感器的误差函数，该误差函数通过预先传感器误差评估方法得到；

(3)优化后的摄像头姿态：联立方程(7)，(8)和(9)，通过卡尔曼滤波算法可估计出其代表摄像头t时刻相对初始时刻的姿态最优值；

步骤1.6：摄像头平移量估计；该步骤分为基于视觉的摄像头平移估计、卡尔曼滤波和优化后的摄像头平移量；下面分别对每部分计算过程展开介绍：

1)基于视觉的摄像头平移估计：

i.基于公式(1)得到的将其代入下式可得：

其中，T_t ^k表示t时刻摄像头坐标系相对k时刻摄像头坐标系的平移向量，(p_t，p_k)表示步骤1.3得到的图像I_t相对图像I_k的特征点对，(λ_t，λ_k)分别是特征点(p_t，p_k)在摄像头坐标系下的深度信息；以压力传感器的测量值作为其深度信息；

ii.遍历图中所有的特征点对，设有M组，将得到M组3×1的平移向量，对每组平移向量T_t ^k(i)，1≤i≤M，通过下式求解其与其他平移向量T_t ^k(j)，1≤j≤M的曼哈顿Manhattan距离d_ij，即

iii.如果d_ij≤T_d，其中T_d表示算法预先设定的阈值，则对应T_t ^k(i)观测向量的票数加1，以此类推，建立所有特征点对的平移向量得票数；

iv.将得票数最高的平移向量选出，假设为第1个平移向量T_t ^k(l)，则摄像头平移向量观测值T_t ^k可通过下式求得：

其中T_t ^k(l′)表示与向量T_t ^k(l)的距离小于T_d的平移量，即M′表示对应T_t ^k(l′)的数量；

vi.通过步骤i-iv可基于视觉得到图像I_t对应的摄像头坐标系相对世界坐标系的平移向量T_t ^w：

其中，表示图像I_k对应的摄像头坐标系相对世界坐标系的平移向量；

2)卡尔曼滤波：

1.状态方程：t时刻的摄像头坐标系相对世界坐标系的平移量可记为一个(3×1)的向量T_t ^w，其导数可以写成：

其中表示t时刻摄像头相对世界坐标系的运动速度，由于速度具有连续变化的特性，因此在采样时间间隔很小时，假设其与前一时刻优化后的运动速度保持不变，即有：

联立方程(11)和(12)，通过4阶龙格-库塔方法便可求出下一时刻的摄像头平移量估计值

2.观测方程：以公式(13)计算得到的作为观测值；

3)优化后的摄像头平移量；联立方程(13)，(14)和(15)，通过卡尔曼滤波算法可估计出摄像头t时刻相对初始时刻的平移量最优值T_t ^w*；

通过步骤1.5和步骤1.6可以估计出摄像头t时刻的位置即为T_t ^w*，然后读取下一帧图像；

步骤1.7：当匹配点数不足时，设置当前帧图像为第k+1帧关键帧，记为I_k+1，将该图像分解为N×N个子区域，对每个子区域采用FAST角点检测方法提取图像I_k+1的特征点，替换之前得到的p_t，并将p_t作为在后续图像中待跟踪的特征点，读取下一帧图像；

步骤二：针对无构图定位算法中的累积误差问题，引入网络地图资源，选择具有缓变性和连续性的标志物作为导航信标，以其GPS全局坐标作为先验信息，预先查找任务区域中这类标志物的GPS坐标，存储至数据库中；设计基于网络信标矫正的自主定位算法，与第一步的MLVO算法构成回路，用以消除在无人机相对定位过程中的累积误差，改善相对定位算法的定位误差漂移的问题；

(1)坐标转换；以符号L₁，L₂，...L_n表示n个标志物，它们的GPS位置信息已预先通过网络地图获得，存至数据库中；为了使方法结果可统一处理，需预先将GPS坐标转换到世界坐标系中，转换公式为：

其中，X_Li是第i个标志物在世界坐标系w中的坐标，(λ_i，φ_i，h_i)表示第i个标志物的GPS坐标中的经度、纬度和高度；r_e表示地球长半轴，e为地球第一偏心率；

(2)飞机在飞行过程中一直开启摄像头进行标志物检测，检测算法为Adaboost算法，目标框中心点即为标志物在图像中的位置坐标；

(3)设飞机t时刻在世界坐标系下的位置估计值为(x_wt，y_wt，z_wt)；当飞机检测到标志物时，则以坐标(x_wt，y_wt，z_wt)为中心，r为半径搜索数据库中的所有标志物，并将搜索到的标志物加载到概率地图中，搜索公式如下，假设有S个标志物在搜索半径内：

其中，X′_Li表示满足上述搜索条件的第i个标志物在世界坐标系下的坐标，x_Li，y_Li，z_Li分别表示第i个标志物在世界坐标系三个坐标轴X、Y、Z下的坐标值；

(4)结合步骤(2)和步骤(3)，通过下式推算出飞机当前时刻在世界坐标系下的可能位置坐标为：

其中，(x_Li，y_Li，z_Li)^T表示通过步骤(3)得到的第i个标志物在世界坐标系下的坐标；(u_xi，u_yi)表示通过步骤(2)检测到的第i个标志物在图像中的位置坐标，单位是像素；(u_cx，u_cy)是图像的中心坐标；(k_x，k_y)表示单位水平像素和垂直像素的尺度，通过摄像头标定可以得到；z_wt为t时刻飞机的飞行高度；表示通过第i个标志物位置推算得到的飞机t时刻在世界坐标系下的坐标；

(5)无人机继续飞行，按照步骤(2)和步骤(3)接着检测标志物，在t+1时刻再次检测到标志物时，通过式(18)计算t+1时刻无人机在世界坐标系中的位置，记为：

(6)另一方面，随着无人机平台的继续运动，无人机可能的位置信息也在不断更新，t+1时刻无人机的在世界坐标系中的位置由下式获得：

其中，ΔX_t+1，t＝(Δx_t+1，t，Δy_t+1，t，Δz_t+1，t)^T是从t时刻到t+1时刻的无人机位移增量，由下式计算得到：

其中，T_t ^w*和分别表示t+1时刻和t时刻摄像头相对世界坐标系的平移量，由步骤1.6得到；

(7)通过步骤(5)和步骤(6)都能估计得到无人机t+1时刻在世界坐标系中的坐标，分别为和按照式(21)估计各位置之间的距离，如果距离小于某一阈值，则通过式(22)对无人机当前位置信息进行更新，否则返回步骤(1)重新检测；