CN108426576B - 基于标识点视觉导航与sins的飞行器路径规划方法及系统 - Google Patents

Abstract

本发明提供一种基于标识点视觉导航与SINS的飞行器路径规划方法及系统，采用基于标识点的视觉导航与捷联惯性导航结合的组合导航系统实现多旋翼飞行器在具有禁飞区等复杂地形环境中获得最短可导航轨迹的路径规划，其通过PC端地面控制系统实现多旋翼飞行器的远程控制，实时图像与飞行姿态获取，定位信息实时回传；采用基于标识点的视觉导航技术与惯性导航技术构成组合导航系统，实现多旋翼飞行器精确定位；通过比较实时获取图像和板载预存参考图像中标识点的相对位置实现飞行器基于标识点的视觉导航，具有成本低、精度高，可远程控制，可用于室内与室外、二维与三维空间环境等优点。经过测试验证，该路径规划系统与方法的精度达到±10cm。

Description

基于标识点视觉导航与SINS的飞行器路径规划方法及系统

技术领域

本发明涉及控制工程技术、导航技术、定位技术与路径规划技术领域，特别涉及一种基于标识点视觉导航与SINS的飞行器路径规划方法及系统。

背景技术

在移动机器人与飞行器、交通网络、3D路径规划与动态最短路径问题等领域，路径规划都是热门话题，而其技术基础就是精确定位。目前，惯性导航系统(InertialNavigation System，INS)、全球卫星定位系统(Global Positioning System，GPS)与视觉导航是应用最为广泛的导航系统。INS是利用惯性传感器(陀螺仪与加速度计)、基准方位及初始位置信息确定载体方位、位置与速度的自主式导航系统，其通常分为平台惯性导航系统(Platform Inertial Navigation System，PINS)与捷联惯性导航系统(Strap-downInertial Navigation System，SINS)两类。PINS将惯性传感器安装在稳定的平台上，并以其为基准测量载体运动参数；SINS直接将惯性传感器安装在载体上，利用计算机完成载体的跟踪保持。INS是一种不依赖外部信息的自主式导航系统，具有较强的自主性并可提供较为全面的导航信息。但是，由于定位误差随时间积累，其难以独立完成长时间高精度定位。GPS由分布在6个轨道平面的导航卫星、地面监控与用户设置等部分构成，其是一个实时卫星导航系统。它可以在全球范围内任何天气条件下实现高精度的三维定位、测速与测定姿态，且定位误差不随时间累积。但是，GPS接收机工作状态受载体机动性影响较大。此外，GPS卫星并没有完全覆盖地球，并且不能在室内环境中使用。由此可见，INS与GPS具有良好的互补性。视觉导航依靠载体获取的实时视觉图像，利用图像处理与模型识别等技术获取载体的运动信息与空间位置信息。作为一种被动导航系统，视觉导航不容易被其他信号干扰而导致导航中断。

传统路径规划方法如动态规划法、A*算法等需要搜索全部飞行空间，并行处理效果较差；Voronoi图、模拟退火算法、遗传算法与神经网络等算法无法适应优化目标的动态变化，路径规划时间长，常用于离线方式。当前较常用的路径规划方法在鲁棒性、处理速度、实时性与稳定性上存在不同缺陷，很多算法无法扩展到三维路径实时规划。此外，基于视觉的路径规划方法中，若飞行器在飞行过程中目标点丢失，通常将无法实现按预定规划路径飞行。

发明内容

为了解决背景技术中所述问题，本发明提供一种基于标识点视觉导航与SINS的飞行器路径规划方法及系统，采用基于标识点的视觉导航与捷联惯性导航结合的组合导航系统实现多旋翼飞行器在具有禁飞区等复杂地形环境中获得最短可导航轨迹的路径规划，具有成本低、精度高，可远程控制，可用于室内与室外、二维与三维空间环境等优点。经过测试验证，该路径规划系统与方法的精度达到±10cm。

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

一种基于标识点视觉导航与SINS的飞行器路径规划方法，包括如下步骤：

步骤1、在多旋翼飞行器起飞前，初始化起始点、目标点信息，加载已知的地理图像数据库与起始点特征；获取飞行器当前位置的定位信息；PC端地面控制系统计算起始点与目标点之间的最终最短可导航轨迹：离线预处理运行寻找由起始点至所有标识点以及由所有标识点至目标点可行飞行轨迹子算法与构建最短轨迹网络子算法；多旋翼飞行器在线运行连接起始点、所有标识点和目标点至最终的最短轨迹网络子算法与寻找最终最短可导航轨迹子算法；PC端地面控制系统向多旋翼飞行器发送起飞控制指令；

步骤2、多旋翼飞行器实时获取飞行姿态、下视图像并无线传输至PC端地面控制系统；

步骤3、运行于PC端地面控制系统的基于标识点视觉导航系统对飞行器位置进行实时定位；

步骤4、捷联惯性导航系统实时获取多旋翼飞行器的瞬时惯性导航数据，并通过SPI接口将数据传输至多旋翼飞行器的主飞行控制系统；经过基于标识点的视觉导航系统与捷联惯性导航系统校正后的精确定位数据发送至PC端地面控制系统；

步骤5、判断多旋翼飞行器是否丢失标识点：是，则返回执行步骤1，重新规划由当前标识点即丢失标识点的前一个标识点至目标点的路径；否，则执行步骤6；

步骤6、判断多旋翼飞行器是否到达目标点：是，则执行步骤7；否，则返回执行步骤2；

步骤7、多旋翼飞行器降落并结束飞行。

所述步骤1包括如下步骤：

步骤1-1、初始化起始点、目标点信息，加载已知的地理图像数据库与起始点特征；

步骤1-2、获取飞行器起飞位置的定位信息；

步骤1-3、在带有禁飞区的复杂地形中寻找可行的飞行轨迹，在不考虑标识点导航方向的情况下，设置起始点为S＝(x_S,x_S)，其速度向量为v_S；目标点为D＝(x_D,x_D)，其速度向量为v_D；Z_i与i分别代表禁飞区与禁飞区的编号，使用直线S(t)表示目标平面中任意两标识点间的一条最短轨迹，S(t)定义为：

式(1)中，t∈(0,T)；x(t)与y(t)为飞行器在时刻t的位置，该直线必须满足如下的起始和目的条件：

使用式(4)计算得到最后的轨迹为：

首先，算法计算起始点、所有标识点和目标点的位置与速度向量，起始点和目标点的速度向量分别为v_S和v_D。定义飞行器的位置和速度向量作为轨迹元素，然后，算法连接所有起始点、标识点和目标点间的可飞行轨迹，最后，算法使用式(1)计算所有可飞行轨迹的直线(轨迹边界)，并使用c(·)表示可飞行轨迹的距离，每个标识点可以为飞行器提供8个可行的飞行方向，且不同的飞行方向具有不同的飞行距离限制，因此，定义每个飞行方向的导航距离d如下：

d＝d_max×p (5)

式(5)中，d_max为标识点间的最大导航距离；p为标识点不同飞行方向的导航距离参数；

步骤1-4、为找到一条由起始点S到目标点D所有导航方向的可飞行最短轨迹，需要为所有标识点构建一个最短轨迹网络，使用Dijkstra算法计算标识点L_i和L_j之间的最短轨迹，通常情况下，Dijkstra算法用来计算从一个起始点至多个目标点间的最短轨迹，由于使用的标识点具有多个飞行方向，因此，Dijkstra算法必须在标识点L_i的每个飞行方向运行一次，为保证飞行安全以及保持下视相机稳定，假设飞行器飞越标识点时的加速度为0m/s²；

步骤1-5、为获得最终的最短可导航轨迹，应将起始点S和目标点D加入至为所有标识点构建的最短轨迹网络中。为了这个目的，算法需重新生成由起始点S至所有标识点以及由所有标识点至目标点D的轨迹。

步骤1-6、算法将计算得到的飞行轨迹长度与对应各个标识点飞行方向相应的允许导航距离比较，待从最短轨迹网络中删除与各个标识点飞行方向相应的允许导航距离不相符的飞行轨迹后，即获得最终的最短可导航轨迹。算法中，使用dist(·)表示飞行器由当前位置飞至下一个标识点或目标点的最大飞行距离。实际飞行中，飞行器由起始点至目标点不可能严格地沿着获得的每条最短轨迹飞行，因此，算法需要为起始点、所有标识点和目标点间的每条最短轨迹提供一个可行的飞行位置偏差。算法中使用欧氏距离计算距离d_i。d_i表示由前一标识点(或起始点)至飞行器以及由飞行器至下一标识点(或目标点)间的距离总和。飞行器根据d_i适当调整飞行位置。该距离d_i应满足下述关系：

(d_bi-α)＜d_i＜(d_bi+α),i＝1,2,...,n (6)

式(6)中，d_bi为一条最短的轨迹；α为设定的最大位置允许偏差(允许飞行区域)；i为起始点与目标点间最短轨迹的编号。

所述步骤3包括如下步骤：

步骤3-1、在基于标识点的视觉导航系统中，所有标识点的位置是固定且已知的，其直径为30cm。为了实现基于标识点的视觉导航，将每一个标识点分为8个飞行方向(每个方向45°，标识点正北与正南方向的角度分别为0°与180°)，每两个标识点可形成64个飞行轨迹；在任意时刻，为获得较高的定位精度，必须保证至少一个标识点存在于相机的视野中；

步骤3-2、使用尺度不变特征变换(Scale-invariant feature transform，SIFT)完成标识点特征提取与初始特征匹配；

尺度不变特征变换方法如下：

该方法主要由四部分构成：尺度空间极值检测、关键点定位、方向分配以及关键点描述。关键点检测的第一步是确定位置与尺度。在不同角度观测同一对象的条件下，该位置与尺度可以被重复分配。检测位置可通过在所有可能的尺度范围内搜索稳定特征来实现，例如：使用一个被称为尺度空间的连续函数。在各种合理假设的条件下，只有高斯函数成为唯一可能的尺度空间内核，因此，定义图像的尺度空间函数为L(x,y,σ)，其是由输入图像I(x,y)与可变尺度的高斯函数G(x,y,σ)卷积得到的，即：

L(x,y,σ)＝G(x,y,σ)*I(x,y) (7)

为了在尺度空间中有效地检测稳定关键点的位置，引入高斯差分函数的尺度空间极值。构建高斯差分函数D(x,y,σ)可以通过计算由一个恒定乘系数k分离后的相邻两个尺度之间的差分与图像的卷积而得出：

该高斯差分函数提供一个近似尺度归一化的高斯拉普拉斯算子σ²▽²G。σ²▽²G的最大值与最小值能生成最稳定的图像特征。通过将图中的像素点与当前尺度以及相邻尺度的26个3×3区域逐个比对以检测D(x,y,σ)最大值与最小值。D与σ²▽²G之间的函数关系，可以通过热扩散方程来理解：

因此，由式(10)可进一步推导出：

G(x,y,kσ)-G(x,y,σ)≈(k-1)σ²▽²G (11)

通过逐个比对尺度空间上的每个点与相邻尺度及相邻位置的点，即可获得特征点所处的位置和相应尺度。

一旦发现候选关键点，接下来将要为其附近数据的位置、规模和主曲率比完成拟合。目前，最为有效的方法是使用三维二次函数拟合方法确定最大值的插入位置。实验已证实，该方法显著提高了匹配精度与稳定性，其采用尺度空间函数D(x,y,σ)的二次Taylor展开式，并将其移位以使得原点为采样点：

式(12)中，在采样点处估计D及其导数；x＝(x,y,σ)^T为采样点位移偏移。极值

的位置可通过下式得到：

极值

的函数值

对于剔除低对比度的不稳定极值是有效的。将式(13)代入式(12)可得：

由于高斯差分函数在边缘处响应强烈，因此，对于稳定性而言，仅剔除不稳定的关键点是不够的。在高斯差分函数中，一个不确定的峰值在边缘处会产生一个较大的主曲率，但是，在垂直方向也会产生一个较小的主曲率。主曲率可以在2×2Hessian矩阵(H)中通过计算关键点处的位置和尺度得到：

H的特征值与D的主曲率成正比。令α为特征值的最大幅度，β为特征值的较小幅度。因此，可得：

Tr(H)＝D_xx+D_yy＝α+β (16)

Det(H)＝D_xxD_yy-(D_xy)²＝αβ (17)

令r为二者比率，即：α＝rβ。进而可得：

因此，为了检查主曲率比是否小于门限r，只需检查下式即可：

为了将关键点的尺度用于选择高斯平滑后的图像L，因此，所有计算都是在尺度不变的方式下进行。在该尺度下，对图像内的每个样本L(x,y)，计算其梯度模值m(x,y)与方向θ(x,y)：

在以关键点为中心的相邻区域内采样，并用直方图统计像素的梯度方向。在360°范围内，梯度直方图共有36个方向，其中每10°为一个方向。直方图中，每个样本的权重为其自身梯度模值以及高斯加权圆形窗，该高斯加权圆形窗具有关键点尺度1.5倍的σ值。直方图的主峰值作为该关键点的方向，达到主峰值80％的峰值作为主方向的辅辅方向，以增强其鲁棒性。

使用基于一种生物视觉模型的描述方法，允许使用不同的计算机制实现位置移动。首先，在关键点位置周围区域计算每个图像采样点的图像梯度模值和方向，并使用关键点尺度选择图像的高斯模糊程度。为了实现方向不变，将描述符的坐标和梯度方向相对关键点方向进行旋转。使用式(14)与式(15)计算所有梯度；使用高斯加权函数为每个采样点的梯度模值分配权重，其中，σ的取值为描述符窗口宽度的1/2。此高斯窗的目的是为了避免由窗口位置发生微小改变而引起特征描述符的突变，并降低远离描述符中心的梯度贡献(越靠近特征点的像素点，其梯度对最终的特征描述贡献越大；远离中心特征点的像素点梯度对特征描述影响较小)。在4×4的样本区域内创建方向直方图，其允许梯度位置发生显著改变，每个直方图均有8个方向。具有8方向的4×4直方图数组具有最好的效果，因此，最终获得4×4×8的128维的关键点特征描述。最后，将SIFT特征描述的长度归一化以消除光照变化的影响。为提高算法对非线性光照变化的鲁棒性，可将归一化的特征向量的每一维元素限制为不超过0.2(经验数据)，即如果某一维数据元素大于0.2，则令其等于0.2然后再次进行向量归一化。

步骤3-3、采用随机采样一致(Random Sample Consensus，RANSAC)方法完成初次滤波(剔除不可靠的特征)；

随机采样一致方法如下：

与常规的平滑技术不同，RANSAC方法使用尽可能小的可行初始数据集，且在可能的情况下使用一致数据来扩大该数据集，而不是使用尽可能多的数据得到初始数据集，然后再尝试消除无效数据点，其具有三个不确定参数：1)用于确定一个点是否与模型兼容的误差容限；2)需要尝试的子集数量；3)门限t，即找到正确模型所使用的兼容点个数。

数据和模型之间的偏差是误差与数据以及误差与模型间的函数(其中，部分误差与数据间的函数被用于实例化模型)。如果模型是数据点的简单函数，则可通过实际分析来建立合理的误差容限，但是，这种简单的方法往往在实际应用中是不可行的。这种情况下，只能通过经验来估计误差容限。样本偏差通常是由数据扰动、模型计算与隐含误差测量等因素造成的，因此，误差容限可以被设置为大于平均误差的一至两个标准偏差值。一个假设模型中，某个数据的期望标准偏差通常为该数据的函数，因此，不同的数据存在不同的误差容限。但是，与显著误差相比，误差容限的变化相对较小，因此，其可以被设置为唯一的误差容限。

何时结束选择新子集，可以由所需选择n个良好数据点子集的实验次数k决定。令w为任意选择的数据点在模型误差容限范围内的概率，则可得实验次数k的期望为：

式(22)中，b＝wⁿ，a＝(1-b)。其中，E(k)、n与w的关系可参阅文献。通常，在结束寻找新子集前，误差容限被设置为大于E(k)一至两个标准偏差。令k的标准偏差为SD(k)，其数学表达式可定义为：

由于几何级数2a/(1-a)³可表示为：

因此，式(23)可重新描述为：

通常，SD(k)近似于E(k)。从略微不同的角度来看，如果想保证概率z，即随机进行k次选择的数据集中(每次选择n个数据点)至少有一个是无差错的数据集，即：

由式(26)可知，如果wⁿ≠1，则k≈log(1-z)E(k)。

门限t的选择必须足够大以满足两个目的：为数据找到正确的模型；发现足够数量的相互一致数据点以满足最终平滑过程(该平滑过程为模型参数计算改进后的估计)的需要。为防止最终得到的一致集与一个非正确模型相兼容，假设y为任意给定的数据点在某个非正确模型误差容限范围内的概率，并希望y^t-n非常小。可以合理假设y小于w，w为给定的一个数据点在正确模型误差容限内的先验概率。假设y<0.5，则t-n等于5意味着不会发生与非正确模型相容的概率大于95％。为了满足最终平滑过程的需求，可使用最小二乘平滑方法。许多情况下，该方法可以调用形式化方法以确定得到期望精度所需的数据点数量。

步骤3-4、采用特征组匹配(Feature Group Matching，FGM)方法完成二次滤波；该方法的计算时间稍有增加，但其能滤除大部分的不正确匹配而得到较好的整体匹配结果；

特征组匹配方法如下：

定义特征组中包含n个特征，且n>1；建立特征C的位置为特征组的中心；P为距离特征C最近的特征点，且其允许分割特征C周围的空间来描述特征的空间关系；特征C与特征P之间的距离为d；M为以C为圆心的同心圆个数，每个同心圆的半径定义为：

ρ_i＝d·(i+a) (27)

式(1)中，i为同心圆编号，且1≤i≤M；a为常量，其用于避免在第一个同心圆中只有一个特征点P的情形，本文选择a＝0.1。此外，L为以C为圆心的半线个数(即将特征C为圆心的圆周分为L个扇形)。第一个扇形由特征点C与P连接线所在的半线开始，每个扇形角度范围定义为：

式(28)中，1≤j≤L；

这些几何要素将圆周空间分为M·L个区域。

区域划分后，需要为特征组找到合理的描述方式来代表所有特征点间的不同空间距离。假设特征组中有N个特征点，特征点的极坐标表述形式为X_k＝(ρ_k,θ_k)，1≤k≤N。定义归一化的特征组描述向量S具有M·L+L个元素。前M·L个元素代表扇形内的特征数量，其定义为：

后L个元素代表圆环内的特征数量，其定义为：

式(29)与式(30)中，由于每个特征被计算两次，因此，归一化因数为2N，且x_kij与x_ki的取值分别定义为：

式(31)与式(32)中，Δr与

可以用零均值的高斯函数描述。归一化后的向量S具有尺度不变性与旋转不变性，其被用于匹配两图像的特征组。匹配过程是依据欧氏距离以及正确或不正确匹配之间的辨别阈值实现的。

步骤3-5、剔除可能存在的不可靠特征点后，将基于标识点视觉导航系统得到的飞行器定位信息传输至PC端地面控制系统；并将飞行器定位信息用于校正捷联惯性导航系统。

用于实现基于标识点视觉导航与SINS的飞行器路径规划方法的系统，包括多旋翼飞行器的主飞行控制系统、捷联惯性导航系统和PC端地面控制系统，多旋翼飞行器的主飞行控制系统、捷联惯性导航系统重叠放置在多旋翼飞行器的旋翼支架交汇处，即多旋翼飞行器的理论质心位置；基于标识点的视觉导航系统运行于PC端地面控制系统；捷联惯性导航系统通过SPI接口与多旋翼飞行器的主飞行控制系统连接，多旋翼飞行器的主飞行控制系统与PC端地面控制系统建立无线连接

与现有技术相比，本发明的有益效果是：

1、本发明采用基于标识点的视觉导航与捷联惯性导航结合的组合导航系统实现多旋翼飞行器在具有禁飞区等复杂地形环境中获得最短可导航轨迹的路径规划，具有成本低、精度高，可远程控制，可用于室内与室外、二维与三维空间环境等优点。经过测试验证，该路径规划系统与方法的精度达到±10cm。

2、本发明是一种多旋翼飞行器在含有禁飞区等复杂地形环境中实现最短路径规划的方法与系统。其通过PC端地面控制系统实现多旋翼飞行器的远程控制，实时图像与飞行姿态获取，定位信息实时回传；采用基于标识点的视觉导航技术与惯性导航技术构成组合导航系统，实现多旋翼飞行器精确定位；通过比较实时获取图像和板载预存参考图像中标识点的相对位置实现飞行器基于标识点的视觉导航；采用尺度不变特征变换(Scale-invariant feature transform，SIFT)完成点特征提取与初始特征匹配；采用随机采样一致(Random Sample Consensus，RANSAC)方法与特征组匹配(Feature Group Matching，FGM)方法完成滤波；采用基于最短路径算法实现最终复杂地形的路径规划：首先，找出由起始点到所有标识点以及由所有标识点到目标点的所有可行轨迹。然后，构造所有标识点之间的最短轨迹网络，并将起始点、目标点与所有标识点连接。最后，找到最终的最短可导航轨迹；其中，所有标识点必须是可见且可区分的。此外，为了飞行器的飞行安全，算法为每条最短轨迹定义了最大位置允许偏差。本系统与方法具有成本低、精度高，实时性好，可远程控制，可用于室内与室外、二维与三维空间环境，若在飞行过程中丢失标识点可重新规划由当前标识点(丢失标识点的前一个标识点)至目标点的路径等优点。

附图说明

图1为本发明一种实施例的基于标识点视觉导航与SINS的飞行器路径规划系统总体结构框图；

图2为本发明一种实施例的基于标识点视觉导航与SINS的飞行器路径规划方法流程图；

图3为本发明一种实施例的路径规划方法中在未考虑标识点导航方向的情况下，寻找可行飞行轨迹子算法流程图；

图4为本发明一种实施例的路径规划方法中寻找可行飞行轨迹的主要原理示意图；

图5为本发明一种实施例的基于标识点的视觉导航使用的标识点模型、飞行方向及相应方向的最大飞行距离示意图；

图6为本发明一种实施例的路径规划方法中构建最短轨迹网络子算法流程图；

图7为本发明一种实施例的路径规划方法中连接起始点、所有标识点与目标点子算法流程图；

图8为本发明一种实施例的路径规划方法中寻找最终的最短可导航轨迹的子算法流程图；

图9为本发明一种实施例的每条最短轨迹的允许飞行区域示意图；

图10为本发明一种实施例的基于标识点的视觉导航特征组匹配方法描述示意图。

具体实施方式

以下结合附图对本发明提供的具体实施方式进行详细说明。

如图1所示，一种基于标识点视觉导航与SINS的飞行器路径规划的系统，包括多旋翼飞行器的主飞行控制系统、捷联惯性导航系统和PC端地面控制系统，多旋翼飞行器的主飞行控制系统、捷联惯性导航系统重叠放置在多旋翼飞行器的旋翼支架交汇处，即多旋翼飞行器的理论质心位置；基于标识点的视觉导航系统运行于PC端地面控制系统；捷联惯性导航系统通过SPI接口与多旋翼飞行器的主飞行控制系统连接，多旋翼飞行器的主飞行控制系统与PC端地面控制系统建立无线连接。

多旋翼飞行器的主飞行控制系统：根据PC端地面控制系统的控制指令工作，实时获取的下视图像，将获取的下视图像、得到的多旋翼飞行器的实时定位信息回传输至PC端地面控制系统；此外，依据采用路径规划方法的时间花费，将所提出的路径规划方法分为4个子算法，其中，飞行器起飞前，将连接起始点、所有标识点和目标点至最终的最短轨迹网络算法与寻找最终最短可导航轨迹算法运行于多旋翼飞行器；

基于标识点的视觉导航系统：该导航方法是基于已知的地理图像数据库与飞行器获取的当前图像间的匹配实现的，用于获取多旋翼飞行器的定位数据，运行于PC端地面控制系统，并辅助捷联惯性导航系统完成最终精确定位；

捷联惯性导航系统：用于获取多旋翼飞行器的瞬时惯性导航数据，通过SPI接口将数据传输至多旋翼飞行器的主飞行控制系统；经过基于标识点的视觉导航系统与捷联惯性导航系统校正后的精确定位数据发送至PC端地面控制系统；

PC端地面控制系统：用于对多旋翼飞行器远程控制，根据多旋翼飞行器飞行姿态、实时获取的下视图像、回传的实时定位信息，实现多旋翼飞行器位置估计；所述PC端地面控制系统根据基于标识点的视觉导航系统与捷联惯性导航系统实现多旋翼飞行器的精确定位；此外，路径规划方法中的其余2个子算法，即寻找由起始点至所有标识点以及由所有标识点至目标点可行飞行轨迹算法与构建最短轨迹网络算法离线预处理运行于PC端地面控制系统；

多旋翼飞行器的主飞行控制系统与PC端地面控制系统通过WIFI、3G网络、遥控等方式建立无线连接，实现多旋翼飞行器的远程控制。

多旋翼飞行器的主飞行控制系统采用ARM9处理器，并运行Linux 2.6.32操作系统。其主要用于实现多旋翼飞行器操控、姿态控制等；捷联贯性导航系统由一个气压计、9自由度的惯性测量单元(3轴陀螺仪、3轴加速度计与3轴磁力计)构成；基于标识点的视觉导航系统与捷联贯性导航系统相结合，构成组合导航系统。

如图2所示，一种基于标识点视觉导航与SINS的飞行器路径规划方法，包括如下步骤：

步骤1、在多旋翼飞行器起飞前，初始化起始点、目标点信息，加载已知的地理图像数据库与起始点特征；获取飞行器当前位置的定位信息；PC端地面控制系统计算起始点与目标点之间的最终最短可导航轨迹：离线预处理运行寻找由起始点至所有标识点以及由所有标识点至目标点可行飞行轨迹子算法与构建最短轨迹网络子算法；多旋翼飞行器在线运行连接起始点、所有标识点和目标点至最终的最短轨迹网络子算法与寻找最终最短可导航轨迹子算法；PC端地面控制系统向多旋翼飞行器发送起飞控制指令；

步骤2、多旋翼飞行器实时获取飞行姿态、下视图像并无线传输至PC端地面控制系统；

步骤3、运行于PC端地面控制系统的基于标识点视觉导航系统对飞行器位置进行实时定位；

步骤4、捷联惯性导航系统实时获取多旋翼飞行器的瞬时惯性导航数据，并通过SPI接口将数据传输至多旋翼飞行器的主飞行控制系统；经过基于标识点的视觉导航系统与捷联惯性导航系统校正后的精确定位数据发送至PC端地面控制系统；

步骤5、判断多旋翼飞行器是否丢失标识点：是，则返回执行步骤1，重新规划由当前标识点即丢失标识点的前一个标识点至目标点的路径；否，则执行步骤6；

步骤6、判断多旋翼飞行器是否到达目标点：是，则执行步骤7；否，则返回执行步骤2；

步骤7、多旋翼飞行器降落并结束飞行。

所述步骤1包括如下步骤：

步骤1-1、初始化起始点、目标点信息，加载已知的地理图像数据库与起始点特征；

步骤1-2、获取飞行器起飞位置的定位信息；

步骤1-3、在带有禁飞区的复杂地形中寻找可行的飞行轨迹，在不考虑标识点导航方向的情况下，设置起始点为S＝(x_S,x_S)，其速度向量为v_S；目标点为D＝(x_D,x_D)，其速度向量为v_D；Z_i与i分别代表禁飞区与禁飞区的编号，使用直线S(t)表示目标平面中任意两标识点间的一条最短轨迹，S(t)定义为：

式(1)中，t∈(0,T)；x(t)与y(t)为飞行器在时刻t的位置，该直线必须满足如下的起始和目的条件：

使用式(4)计算得到最后的轨迹为：

首先，算法计算起始点、所有标识点和目标点的位置与速度向量，起始点和目标点的速度向量分别为v_S和v_D。定义飞行器的位置和速度向量作为轨迹元素，然后，算法连接所有起始点、标识点和目标点间的可飞行轨迹，最后，算法使用式(1)计算所有可飞行轨迹的直线(轨迹边界)，并使用c(·)表示可飞行轨迹的距离，图3为在未考虑标识点导航方向的情况下，寻找可行飞行轨迹子算法流程图；图4为寻找可行飞行轨迹的主要原理示意图；图5为基于标识点的视觉导航使用的标识点模型、飞行方向及相应方向的最大飞行距离示意图。每个标识点可以为飞行器提供8个可行的飞行方向，且不同的飞行方向具有不同的飞行距离限制，因此，定义每个飞行方向的导航距离d如下：

d＝d_max×p (5)

式(5)中，d_max为标识点间的最大导航距离；p为标识点不同飞行方向的导航距离参数；

步骤1-4、为找到一条由起始点S到目标点D所有导航方向的可飞行最短轨迹，需要为所有标识点构建一个最短轨迹网络，图6为构建最短轨迹网络的算法过程示意图。使用Dijkstra算法计算标识点L_i和L_j之间的最短轨迹，通常情况下，Dijkstra算法用来计算从一个起始点至多个目标点间的最短轨迹，由于使用的标识点具有多个飞行方向，因此，Dijkstra算法必须在标识点L_i的每个飞行方向运行一次，为保证飞行安全以及保持下视相机稳定，假设飞行器飞越标识点时的加速度为0m/s²；

步骤1-5、为获得最终的最短可导航轨迹，应将起始点S和目标点D加入至为所有标识点构建的最短轨迹网络中。为了这个目的，算法需重新生成由起始点S至所有标识点以及由所有标识点至目标点D的轨迹。图7为连接起始点、所有的标识点和目标点至最终的最短轨迹网络的算法过程。

步骤1-6、算法将计算得到的飞行轨迹长度与对应各个标识点飞行方向相应的允许导航距离比较，待从最短轨迹网络中删除与各个标识点飞行方向相应的允许导航距离不相符的飞行轨迹后，即获得最终的最短可导航轨迹。图8描绘了寻找最终的最短可导航轨迹的算法过程。算法中，使用dist(·)表示飞行器由当前位置飞至下一个标识点或目标点的最大飞行距离。实际飞行中，飞行器由起始点至目标点不可能严格地沿着获得的每条最短轨迹飞行，因此，算法需要为起始点、所有标识点和目标点间的每条最短轨迹提供一个可行的飞行位置偏差。算法中使用欧氏距离计算距离d_i。d_i表示由前一标识点(或起始点)至飞行器以及由飞行器至下一标识点(或目标点)间的距离总和。飞行器根据d_i适当调整飞行位置。该距离d_i应满足下述关系：

(d_bi-α)＜d_i＜(d_bi+α),i＝1,2,...,n (6)

式(6)中，d_bi为一条最短的轨迹；α为设定的最大位置允许偏差(允许飞行区域)；i为起始点与目标点间最短轨迹的编号。图9为每条最短轨迹设定的最大位置允许偏差示意图。

所述步骤3包括如下步骤：

步骤3-1、在基于标识点的视觉导航系统中，所有标识点的位置是固定且已知的，其直径为30cm。为了实现基于标识点的视觉导航，将每一个标识点分为8个飞行方向(每个方向45°，标识点正北与正南方向的角度分别为0°与180°)，每两个标识点可形成64个飞行轨迹；在任意时刻，为获得较高的定位精度，必须保证至少一个标识点存在于相机的视野中；

步骤3-2、使用尺度不变特征变换(Scale-invariant feature transform，SIFT)完成标识点特征提取与初始特征匹配；

尺度不变特征变换方法如下：

该方法主要由四部分构成：尺度空间极值检测、关键点定位、方向分配以及关键点描述。关键点检测的第一步是确定位置与尺度。在不同角度观测同一对象的条件下，该位置与尺度可以被重复分配。检测位置可通过在所有可能的尺度范围内搜索稳定特征来实现，例如：使用一个被称为尺度空间的连续函数。在各种合理假设的条件下，只有高斯函数成为唯一可能的尺度空间内核，因此，定义图像的尺度空间函数为L(x,y,σ)，其是由输入图像I(x,y)与可变尺度的高斯函数G(x,y,σ)卷积得到的，即：

L(x,y,σ)＝G(x,y,σ)*I(x,y) (7)

为了在尺度空间中有效地检测稳定关键点的位置，引入高斯差分函数的尺度空间极值。构建高斯差分函数D(x,y,σ)可以通过计算由一个恒定乘系数k分离后的相邻两个尺度之间的差分与图像的卷积而得出：

该高斯差分函数提供一个近似尺度归一化的高斯拉普拉斯算子σ²▽²G。σ²▽²G的最大值与最小值能生成最稳定的图像特征。通过将图中的像素点与当前尺度以及相邻尺度的26个3×3区域逐个比对以检测D(x,y,σ)最大值与最小值。D与σ²▽²G之间的函数关系，可以通过热扩散方程来理解：

因此，由式(10)可进一步推导出：

G(x,y,kσ)-G(x,y,σ)≈(k-1)σ²▽²G (11)

通过逐个比对尺度空间上的每个点与相邻尺度及相邻位置的点，即可获得特征点所处的位置和相应尺度。

一旦发现候选关键点，接下来将要为其附近数据的位置、规模和主曲率比完成拟合。目前，最为有效的方法是使用三维二次函数拟合方法确定最大值的插入位置。实验已证实，该方法显著提高了匹配精度与稳定性，其采用尺度空间函数D(x,y,σ)的二次Taylor展开式，并将其移位以使得原点为采样点：

式(12)中，在采样点处估计D及其导数；x＝(x,y,σ)^T为采样点位移偏移。极值

的位置可通过下式得到：

极值

的函数值

对于剔除低对比度的不稳定极值是有效的。将式(13)代入式(12)可得：

由于高斯差分函数在边缘处响应强烈，因此，对于稳定性而言，仅剔除不稳定的关键点是不够的。在高斯差分函数中，一个不确定的峰值在边缘处会产生一个较大的主曲率，但是，在垂直方向也会产生一个较小的主曲率。主曲率可以在2×2Hessian矩阵(H)中通过计算关键点处的位置和尺度得到：

H的特征值与D的主曲率成正比。令α为特征值的最大幅度，β为特征值的较小幅度。因此，可得：

Tr(H)＝D_xx+D_yy＝α+β (16)

Det(H)＝D_xxD_yy-(D_xy)²＝αβ (17)

令r为二者比率，即：α＝rβ。进而可得：

因此，为了检查主曲率比是否小于门限r，只需检查下式即可：

为了将关键点的尺度用于选择高斯平滑后的图像L，因此，所有计算都是在尺度不变的方式下进行。在该尺度下，对图像内的每个样本L(x,y)，计算其梯度模值m(x,y)与方向θ(x,y)：

在以关键点为中心的相邻区域内采样，并用直方图统计像素的梯度方向。在360°范围内，梯度直方图共有36个方向，其中每10°为一个方向。直方图中，每个样本的权重为其自身梯度模值以及高斯加权圆形窗，该高斯加权圆形窗具有关键点尺度1.5倍的σ值。直方图的主峰值作为该关键点的方向，达到主峰值80％的峰值作为主方向的辅辅方向，以增强其鲁棒性。

使用基于一种生物视觉模型的描述方法，允许使用不同的计算机制实现位置移动。首先，在关键点位置周围区域计算每个图像采样点的图像梯度模值和方向，并使用关键点尺度选择图像的高斯模糊程度。为了实现方向不变，将描述符的坐标和梯度方向相对关键点方向进行旋转。使用式(14)与式(15)计算所有梯度；使用高斯加权函数为每个采样点的梯度模值分配权重，其中，σ的取值为描述符窗口宽度的1/2。此高斯窗的目的是为了避免由窗口位置发生微小改变而引起特征描述符的突变，并降低远离描述符中心的梯度贡献(越靠近特征点的像素点，其梯度对最终的特征描述贡献越大；远离中心特征点的像素点梯度对特征描述影响较小)。在4×4的样本区域内创建方向直方图，其允许梯度位置发生显著改变，每个直方图均有8个方向。具有8方向的4×4直方图数组具有最好的效果，因此，最终获得4×4×8的128维的关键点特征描述。最后，将SIFT特征描述的长度归一化以消除光照变化的影响。为提高算法对非线性光照变化的鲁棒性，可将归一化的特征向量的每一维元素限制为不超过0.2(经验数据)，即如果某一维数据元素大于0.2，则令其等于0.2然后再次进行向量归一化。

步骤3-3、采用随机采样一致(Random Sample Consensus，RANSAC)方法完成初次滤波(剔除不可靠的特征)；

随机采样一致方法如下：

与常规的平滑技术不同，RANSAC方法使用尽可能小的可行初始数据集，且在可能的情况下使用一致数据来扩大该数据集，而不是使用尽可能多的数据得到初始数据集，然后再尝试消除无效数据点，其具有三个不确定参数：1)用于确定一个点是否与模型兼容的误差容限；2)需要尝试的子集数量；3)门限t，即找到正确模型所使用的兼容点个数。

数据和模型之间的偏差是误差与数据以及误差与模型间的函数(其中，部分误差与数据间的函数被用于实例化模型)。如果模型是数据点的简单函数，则可通过实际分析来建立合理的误差容限，但是，这种简单的方法往往在实际应用中是不可行的。这种情况下，只能通过经验来估计误差容限。样本偏差通常是由数据扰动、模型计算与隐含误差测量等因素造成的，因此，误差容限可以被设置为大于平均误差的一至两个标准偏差值。一个假设模型中，某个数据的期望标准偏差通常为该数据的函数，因此，不同的数据存在不同的误差容限。但是，与显著误差相比，误差容限的变化相对较小，因此，其可以被设置为唯一的误差容限。

何时结束选择新子集，可以由所需选择n个良好数据点子集的实验次数k决定。令w为任意选择的数据点在模型误差容限范围内的概率，则可得实验次数k的期望为：

式(22)中，b＝wⁿ，a＝(1-b)。其中，E(k)、n与w的关系可参阅文献。通常，在结束寻找新子集前，误差容限被设置为大于E(k)一至两个标准偏差。令k的标准偏差为SD(k)，其数学表达式可定义为：

由于几何级数2a/(1-a)³可表示为：

因此，式(23)可重新描述为：

通常，SD(k)近似于E(k)。从略微不同的角度来看，如果想保证概率z，即随机进行k次选择的数据集中(每次选择n个数据点)至少有一个是无差错的数据集，即：

由式(26)可知，如果wⁿ≠1，则k≈log(1-z)E(k)。

门限t的选择必须足够大以满足两个目的：为数据找到正确的模型；发现足够数量的相互一致数据点以满足最终平滑过程(该平滑过程为模型参数计算改进后的估计)的需要。为防止最终得到的一致集与一个非正确模型相兼容，假设y为任意给定的数据点在某个非正确模型误差容限范围内的概率，并希望y^t-n非常小。可以合理假设y小于w，w为给定的一个数据点在正确模型误差容限内的先验概率。假设y<0.5，则t-n等于5意味着不会发生与非正确模型相容的概率大于95％。为了满足最终平滑过程的需求，可使用最小二乘平滑方法。许多情况下，该方法可以调用形式化方法以确定得到期望精度所需的数据点数量。

步骤3-4、采用特征组匹配(Feature Group Matching，FGM)方法完成二次滤波；该方法的计算时间稍有增加，但其能滤除大部分的不正确匹配而得到较好的整体匹配结果；

特征组匹配方法如下：

图10为特征组匹配方法示意图。定义特征组中包含n个特征，且n>1；建立特征C的位置为特征组的中心；P为距离特征C最近的特征点，且其允许分割特征C周围的空间来描述特征的空间关系；特征C与特征P之间的距离为d；M为以C为圆心的同心圆个数，每个同心圆的半径定义为：

ρ_i＝d·(i+a) (27)

式(1)中，i为同心圆编号，且1≤i≤M；a为常量，其用于避免在第一个同心圆中只有一个特征点P的情形，本文选择a＝0.1。此外，L为以C为圆心的半线个数(即将特征C为圆心的圆周分为L个扇形)。第一个扇形由特征点C与P连接线所在的半线开始，每个扇形角度范围定义为：

式(28)中，1≤j≤L；

这些几何要素将圆周空间分为M·L个区域。

区域划分后，需要为特征组找到合理的描述方式来代表所有特征点间的不同空间距离。假设特征组中有N个特征点，特征点的极坐标表述形式为X_k＝(ρ_k,θ_k)，1≤k≤N。定义归一化的特征组描述向量S具有M·L+L个元素。前M·L个元素代表扇形内的特征数量，其定义为：

后L个元素代表圆环内的特征数量，其定义为：

式(29)与式(30)中，由于每个特征被计算两次，因此，归一化因数为2N，且x_kij与x_ki的取值分别定义为：

式(31)与式(32)中，Δr与

可以用零均值的高斯函数描述。归一化后的向量S具有尺度不变性与旋转不变性，其被用于匹配两图像的特征组。匹配过程是依据欧氏距离以及正确或不正确匹配之间的辨别阈值实现的。

步骤3-5、剔除可能存在的不可靠特征点后，将基于标识点视觉导航系统得到的飞行器定位信息传输至PC端地面控制系统；并将飞行器定位信息用于校正捷联惯性导航系统。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (3)

1.基于标识点视觉导航与SINS的飞行器路径规划方法，其特征在于，包括如下步骤：

步骤1、在多旋翼飞行器起飞前，初始化起始点、目标点信息，加载已知的地理图像数据库与起始点特征；获取飞行器当前位置的定位信息；PC端地面控制系统计算起始点与目标点之间的最终最短可导航轨迹：离线预处理运行寻找由起始点至所有标识点以及由所有标识点至目标点可行飞行轨迹子算法与构建最短轨迹网络子算法；多旋翼飞行器在线运行连接起始点、所有标识点和目标点至最终的最短轨迹网络子算法与寻找最终最短可导航轨迹子算法；PC端地面控制系统向多旋翼飞行器发送起飞控制指令；

步骤2、多旋翼飞行器实时获取飞行姿态、下视图像并无线传输至PC端地面控制系统；

步骤3、运行于PC端地面控制系统的基于标识点视觉导航系统对飞行器位置进行实时定位；

步骤4、捷联惯性导航系统实时获取多旋翼飞行器的瞬时惯性导航数据，并通过SPI接口将数据传输至多旋翼飞行器的主飞行控制系统；经过基于标识点的视觉导航系统与捷联惯性导航系统校正后的精确定位数据发送至PC端地面控制系统；

步骤5、判断多旋翼飞行器是否丢失标识点：是，则返回执行步骤1，重新规划由当前标识点即丢失标识点的前一个标识点至目标点的路径；否，则执行步骤6；

步骤6、判断多旋翼飞行器是否到达目标点：是，则执行步骤7；否，则返回执行步骤2；

步骤7、多旋翼飞行器降落并结束飞行；

所述步骤3包括如下步骤：

步骤3-1、在基于标识点的视觉导航系统中，所有标识点的位置是固定且已知的，其直径为30cm；为了实现基于标识点的视觉导航，将每一个标识点分为8个飞行方向，每个方向45°，标识点正北与正南方向的角度分别为0°与180°，每两个标识点可形成64个飞行轨迹；在任意时刻，为获得较高的定位精度，必须保证至少一个标识点存在于相机的视野中；

步骤3-2、使用尺度不变特征变换算法Scale-invariant feature transform即SIFT完成标识点特征提取与初始特征匹配；

尺度不变特征变换方法如下：

定义图像的尺度空间函数为L(x,y,σ)，其是由输入图像I(x,y)与可变尺度的高斯函数G(x,y,σ)卷积得到的，即：

L(x,y,σ)＝G(x,y,σ)*I(x,y) (7)

为了在尺度空间中有效地检测稳定关键点的位置，引入高斯差分函数的尺度空间极值；构建高斯差分函数D(x,y,σ)，通过计算由一个恒定乘系数μ分离后的相邻两个尺度之间的差分与图像的卷积而得出：

该高斯差分函数提供一个近似尺度归一化的高斯拉普拉斯算子σ²▽²G；σ²▽²G的最大值与最小值能生成最稳定的图像特征；通过将图中的像素点与当前尺度以及相邻尺度的26个3×3区域逐个比对以检测D(x,y,σ)最大值与最小值；D与σ²▽²G之间的函数关系，通过热扩散方程来理解：

因此，由式(10)进一步推导出：

通过逐个比对尺度空间上的每个点与相邻尺度及相邻位置的点，即获得特征点所处的位置和相应尺度；

其采用尺度空间函数D(x,y,σ)的二次Taylor展开式，并将其移位以使得原点为采样点：

式(12)中，在采样点处估计D及其导数；x＝(x,y,σ)^T为采样点位移偏移；极值

的位置通过下式得到：

极值

的函数值

对于剔除低对比度的不稳定极值是有效的；将式(13)代入式(12)得：

主曲率在2×2Hessian矩阵H中通过计算关键点处的位置和尺度得到：

H的特征值与D的主曲率成正比；令α为特征值的最大幅度，β为特征值的较小幅度；因此，得：

Tr(H)＝D_xx+D_yy＝α+β (16)

Det(H)＝D_xxD_yy-(D_xy)²＝αβ (17)

令r为二者比率，即：α＝rβ；进而得：

只检查下式：

为了将关键点的尺度用于选择高斯平滑后的图像L，因此，所有计算都是在尺度不变的方式下进行；在该尺度下，对图像内的每个样本L(x,y)，计算其梯度模值m(x,y)与方向θ(x,y)：

在以关键点为中心的相邻区域内采样，并用直方图统计像素的梯度方向；在360°范围内，梯度直方图共有36个方向，其中每10°为一个方向；直方图中，每个样本的权重为其自身梯度模值以及高斯加权圆形窗，该高斯加权圆形窗具有关键点尺度1.5倍的σ值；直方图的主峰值作为该关键点的方向，达到主峰值80％的峰值作为主方向的辅方向，以增强其鲁棒性；

首先，在关键点位置周围区域计算每个图像采样点的图像梯度模值和方向，并使用关键点尺度选择图像的高斯模糊程度；为了实现方向不变，将SIFT算法的描述符的坐标和梯度方向相对关键点方向进行旋转；使用式(14)与式(15)计算所有梯度；使用高斯加权函数为每个采样点的梯度模值分配权重，其中，σ的取值为描述符窗口宽度的1/2；此高斯窗的目的是为了避免由窗口位置发生微小改变而引起特征描述符的突变，并降低远离描述符中心的梯度贡献，越靠近特征点的像素点，其梯度对最终的特征描述贡献越大；远离中心特征点的像素点梯度对特征描述影响较小；在4×4的样本区域内创建方向直方图，其允许梯度位置发生显著改变，每个直方图均有8个方向；具有8方向的4×4直方图数组具有最好的效果，因此，最终获得4×4×8的128维的关键点特征描述；最后，将SIFT特征描述的长度归一化以消除光照变化的影响；为提高算法对非线性光照变化的鲁棒性，将归一化的特征向量的每一维元素限制为不超过经验数据0.2；

步骤3-3、采用随机采样一致算法Random Sample Consensus即RANSAC方法完成初次滤波，剔除不可靠的特征；

随机采样一致方法如下：

令w为任意选择的数据点在模型误差容限范围内的概率，则可得实验次数k的期望为：

式(22)中，b＝wⁿ，a＝(1-b)；在结束寻找新子集前，误差容限被设置为大于E(k)一至两个标准偏差；令k的标准偏差为SD(k)，其数学表达式定义为：

由于几何级数2a/(1-a)³表示为：

因此，式(23)重新描述为：

SD(k)近似于E(k)；即：

由式(26)知，如果wⁿ≠1，则k≈log(1-z)E(k)；

步骤3-4、采用特征组匹配Feature Group Matching即FGM方法完成二次滤波；该方法的计算时间稍有增加，但其能滤除大部分的不正确匹配而得到较好的整体匹配结果；

特征组匹配方法如下：

定义特征组中包含n个特征，且n>1；建立特征C的位置为特征组的中心；P为距离特征C最近的特征点，且其允许分割特征C周围的空间来描述特征的空间关系；特征C与特征P之间的距离为d；M为以特征C为圆心的同心圆个数，则以特征C为特征组中心的每个同心圆半径定义为：

ρ_index＝d·(index+a) (27)

式(27)中，index为同心圆编号，且1≤index≤M；a为常量，其用于避免在第一个同心圆中只有一个特征点P的情形，本文选择a＝0.1；此外，L为以C为圆心的半线个数，即将特征C为圆心的圆周分为L个扇形；第一个扇形由特征点C与P连接线所在的半线开始，每个扇形角度范围定义为：

式(28)中，1≤j≤L；

这些几何要素将圆周空间分为M·L个区域；

区域划分后，为特征组找到合理的描述方式来代表所有特征点间的不同空间距离；设特征组中有N个特征点，特征点的极坐标表述形式为X_kk＝(ρ_kk,θ_kk)，1≤kk≤N；定义归一化的特征组描述向量S具有M·L+L个元素；前M·L个元素代表扇形内的特征数量，其定义为：

后L个元素代表圆环内的特征数量，其定义为：

式(29)与式(30)中，由于每个特征被计算两次，因此，归一化因数为2N，且x_jkkindex与x_kkindex的取值分别定义为：

式(31)与式(32)中，Δr与

用零均值的高斯函数描述；归一化后的向量S具有尺度不变性与旋转不变性，其被用于匹配两图像的特征组；匹配过程是依据欧氏距离以及正确或不正确匹配之间的辨别阈值实现的；

步骤3-5、剔除可能存在的不可靠特征点后，将基于标识点视觉导航系统得到的飞行器定位信息传输至PC端地面控制系统；并将飞行器定位信息用于校正捷联惯性导航系统。

2.根据权利要求1所述的基于标识点视觉导航与SINS的飞行器路径规划方法，其特征在于，所述步骤1包括如下步骤：

步骤1-1、初始化起始点、目标点信息，加载已知的地理图像数据库与起始点特征；

步骤1-2、获取飞行器起飞位置的定位信息；

步骤1-3、在带有禁飞区的复杂地形中寻找可行的飞行轨迹，在不考虑标识点导航方向的情况下，设置起始点为S＝(x_S,y_S)，其速度向量为v_S；目标点为D＝(x_D,y_D)，其速度向量为v_D；R_no与no分别代表禁飞区与禁飞区的编号，使用直线S(t)表示目标平面中任意两标识点间的一条最短轨迹，S(t)定义为：

式(1)中，t∈(0,T)；x(t)与y(t)为飞行器在时刻t的位置，该直线必须满足如下的起始和目的条件：

使用式(4)计算得到最后的轨迹为：

首先，算法计算起始点、所有标识点和目标点的位置与速度向量，起始点和目标点的速度向量分别为v_S和v_D；定义飞行器的位置和速度向量作为轨迹元素，然后，算法连接所有起始点、标识点和目标点间的可飞行轨迹，最后，算法使用式(1)计算所有可飞行轨迹的直线：轨迹边界，并使用c(·)表示可飞行轨迹的距离，每个标识点可以为飞行器提供8个可行的飞行方向，且不同的飞行方向具有不同的飞行距离限制，因此，定义每个飞行方向的导航距离d如下：

d＝d_max×p(5)

式(5)中，d_max为标识点间的最大导航距离；p为标识点不同飞行方向的导航距离参数；

步骤1-4、为找到一条由起始点S到目标点D所有导航方向的可飞行最短轨迹，为所有标识点构建一个最短轨迹网络，使用Dijkstra算法计算标识点L_ii和L_jj之间的最短轨迹，Dijkstra算法用来计算从一个起始点至多个目标点间的最短轨迹，由于使用的标识点具有多个飞行方向，因此，Dijkstra算法必须在标识点L_ii的每个飞行方向运行一次，为保证飞行安全以及保持下视相机稳定，设飞行器飞越标识点时的加速度为0m/s²；

步骤1-5、为获得最终的最短可导航轨迹，将起始点S和目标点D加入至为所有标识点构建的最短轨迹网络中；为了这个目的，算法需重新生成由起始点S至所有标识点以及由所有标识点至目标点D的轨迹；

步骤1-6、算法将计算得到的飞行轨迹长度与对应各个标识点飞行方向相应的允许导航距离比较，待从最短轨迹网络中删除与各个标识点飞行方向相应的允许导航距离不相符的飞行轨迹后，即获得最终的最短可导航轨迹；算法中，使用dist(·)表示飞行器由当前位置飞至下一个标识点或目标点的最大飞行距离；算法中使用欧氏距离计算距离d_i；d_i表示由前一标识点至飞行器以及由飞行器至下一标识点或目标点间的距离总和；飞行器根据d_i适当调整飞行位置；该距离d_i应满足下述关系：

(d_bi-α)＜d_i＜(d_bi+α),i＝1,2,...,n(6)

式(6)中，d_bi为一条最短的轨迹；α为设定的最大位置允许偏差，允许飞行区域；i为起始点与目标点间最短轨迹的编号。

3.用于实现权利要求1所述的基于标识点视觉导航与SINS的飞行器路径规划方法的系统，其特征在于，包括多旋翼飞行器的主飞行控制系统、捷联惯性导航系统和PC端地面控制系统，多旋翼飞行器的主飞行控制系统、捷联惯性导航系统重叠放置在多旋翼飞行器的旋翼支架交汇处，即多旋翼飞行器的理论质心位置；基于标识点的视觉导航系统运行于PC端地面控制系统；捷联惯性导航系统通过SPI接口与多旋翼飞行器的主飞行控制系统连接，多旋翼飞行器的主飞行控制系统与PC端地面控制系统建立无线连接。

Images