CN117268395A - 一种无人机地图匹配位置跳变抑制方法 - Google Patents

Abstract

本发明提供一种无人机地图匹配位置跳变抑制方法，该方法包括：判断是否存在视觉跳变信息，若存在，则采用改进的视觉导航自适应滤波算法进行视觉导航，其中，采用sigmoid函数对视觉导航自适应滤波算法中的量测噪声矩阵进行拟合并确定比例系数。该方法保证了组合导航系统的稳定性，支撑了无人机卫星拒止条件下的自主导航能力。

Description

一种无人机地图匹配位置跳变抑制方法

技术领域

本发明属于无人机视觉导航技术领域，涉及一种无人机地图匹配位置跳变抑制方法。

背景技术

传统的无人机地图匹配一般采用预先制备基准图，这种方式虽然可以有效降低误匹配率，但是具有成本高、需要提前规划航线等缺点。使用商业卫星地图进行匹配导航更加便利，但是具有着匹配率低，容易误匹配的缺点。传统组合导航方法在发生误匹配时无法判断导致位置修正错误，因此需要对组合导航策略进行规划。

发明内容

本发明旨在至少解决现有技术或相关技术中存在的技术问题之一。

为此，本发明提供了一种无人机地图匹配位置跳变抑制方法。

本发明的技术解决方案如下：

根据一方面，提供一种无人机地图匹配位置跳变抑制方法，该方法包括：

判断是否存在视觉跳变信息，若存在，则采用改进的视觉导航自适应滤波算法进行视觉导航，其中，采用sigmoid函数对视觉导航自适应滤波算法中的量测噪声矩阵R_k进行拟合并确定比例系数。

进一步地，通过下式获取量测噪声矩阵R_k：

其中，a为比例系数，x为自变量。

进一步地，所述判断是否存在视觉跳变信息，包括：

同时对单帧位置跳变、相似地形区域连续跳变以及自适应滤波算法中量测信息有效性进行判断，若三种情况下有一种出现跳变，即判定为存在视觉跳变信息。

进一步地，通过下述方式对单帧位置跳变进行判断：

在连续两帧匹配时间间隔ΔT内进行如下式(1)和(2)判断，若式(1)和(2)均满足则认为匹配信息正确，否则，判断当前时刻地图匹配信息错误，存在跳变；

(Lat₂-Lat₁)×R_m≤(v_n×ΔT) (1)

(Lon₂-Lon₁)×R_m×cos(Lat₂)≤(v_e×ΔT) (2)

式中，Lat₁为上一帧地图匹配得到的纬度信息，Lon₁为经度信息，当前帧地图匹配得到的纬度信息为Lat₂，经度信息为Lon₂，无人机北向速度、东向速度分别为v_n、v_e，R_m、R_n分别为地球子午圈曲率半径长度与卯酉圈曲率半径长度。

进一步地，通过下述方式对相似地形区域连续跳变进行判断：

根据惯导标定器件误差获取在一个舒拉周期内惯导位置误差发散峰值Δp_max，其中Δp_max＝[ΔL_max ΔH_max Δλ_max]^T，ΔL_max为最大纬度误差、ΔH_max为最大高度误差、Δλ_max为最大经度误差；

根据Δp_max获取在连续n帧间隔时间T_n内，系统最大发散误差Δp为Δp＝Δp_max/T_n；

计算T_n内匹配定位结果差值：Δp_map＝p_k-p_k-n；p_k为当前k时刻的匹配导航位置，p_k-n为T_n前时刻即k-n时刻的匹配导航位置；

分别将Δp与Δp_map转换至载体坐标系b系下，得到Δp^b、Δp^b与/>均为三维向量，第一维表示纬度误差，第二维为高度误差，第三维为经度误差；

分别比较Δp^b与中的第一维与第三维，若/>则认为匹配结果发生跳变，下一时刻匹配结果继续与上一次成功匹配的位置p_k-1进行比较。

进一步地，通过咔方检验的方式对量测信息有效性进行判断。

进一步地，所述通过咔方检验的方式对量测信息有效性进行判断具体包括：

将故障检测函数λ_k记为：

其中，λ_k是服从自由度为m的卡方分布；m为测量Z_k的维数；r_k为卡尔曼滤波器的残差；A_k为方差。

设计跳变判定准则为：

其中，T_D为预先设置的门限。

进一步地，所述方法还包括：在每次视觉导航自适应滤波算法中均对量测信息进行咔方检验。

根据另一方面，提供一种计算机设备，包括存储器、处理器及存储在所述存储器中并可在所述处理器上运行的计算机程序，其中，所述处理器执行所述计算机程序时实现上述方法的步骤。

上述技术方案针对无人机系统在卫星拒止条件下的自主导航问题，提出基于惯性辅助的地图匹配位置跳变抑制方法，首先全面准确地完成了视觉跳变信息的判断，并在存在跳变信息的条件下通过对组合导航量测噪声阵的线性化拟合设计实现了惯性/地图匹配组合导航，保证了组合导航系统的稳定性，支撑了无人机卫星拒止条件下的自主导航能力。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的位置跳变示意；

图2为本发明实施例提供的地形相似区域位置跳变示意；

(a)实时图像；(b)基准图；

图3为本发明实施例提供的Sigmoid函数图像。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

如图1-3所示，在本发明的一个实施例中，提供一种无人机地图匹配位置跳变抑制方法，该方法包括：

判断是否存在视觉跳变信息，若存在，则采用改进的视觉导航自适应滤波算法进行视觉导航，其中，采用sigmoid函数对视觉导航自适应滤波算法中的量测噪声矩阵R_k进行拟合并确定比例系数。

举例来说，若不存在跳变信息，则直接采用现有卡尔曼滤波方法进行视觉导航，若存在，则需要对滤波算法中的量测噪声矩阵进行改进，由此能够保证组合导航系统的稳定性。

可见，本发明实施例针对无人机系统在卫星拒止条件下的自主导航问题，提出基于惯性辅助的地图匹配位置跳变抑制方法，首先全面准确地完成了视觉跳变信息的判断，并在存在跳变信息的条件下通过对组合导航量测噪声阵的线性化拟合设计实现了惯性/地图匹配组合导航，保证了组合导航系统的稳定性，支撑了无人机卫星拒止条件下的自主导航能力。

在上述实施例中，为了准确判断是否存在跳变信息，所述判断是否存在视觉跳变信息，包括：

同时对单帧位置跳变、相似地形区域连续跳变以及自适应滤波算法中量测信息有效性进行判断，若三种情况下有一种出现跳变，即判定为存在视觉跳变信息。

也即一旦单帧位置跳变、相似地形区域连续跳变以及自适应滤波算法中量测信息有效性中有一种出现跳变，即认为存在视觉跳变，也即，两种和三种都出现也是存在视觉跳变。

在上述实施例中，为了获取单帧位置跳变抑制方法，通过下述方式对单帧位置跳变进行判断：

在连续两帧匹配时间间隔ΔT内进行如下式(1)和(2)判断，若式(1)和(2)均满足则认为匹配信息正确，否则，判断当前时刻地图匹配信息错误，存在跳变；

(Lat₂-Lat₁)×R_m≤(v_n×ΔT) (1)

(Lon₂-Lon₁)×R_m×cos(Lat₂)≤(v_e×ΔT) (2)

式中，Lat₁为上一帧地图匹配得到的纬度信息，Lon₁为经度信息，当前帧地图匹配得到的纬度信息为Lat₂，经度信息为Lon₂，无人机北向速度、东向速度分别为v_n、v_e，R_m、R_n分别为地球子午圈曲率半径长度与卯酉圈曲率半径长度。

也即，无人机在巡航段通过商业地图匹配方式进行视觉导航的过程中，位置跳变的主要原因为错误匹配，如图1所示。其中纬度位置跳变100m，直接使用该帧数据参与组合将导致系统误差发散，严重影响无人机控制系统的稳定性。因此，本实施例进行单帧位置跳变判断，以采用改进滤波算法处理视觉信息，保证系统稳定性。本实施例方法对视觉导航的误匹配导致的位置跳变具有很好地抑制作用，在无人机自主导航领域具有十分广阔的应用前景。

在上述实施例中，为了获取相似地形区域连续跳变抑制方法，通过下述方式对相似地形区域连续跳变进行判断：

根据惯导标定器件误差获取在一个舒拉周期内惯导位置误差发散峰值Δp_max，其中Δp_max＝[ΔL_max ΔH_max Δλ_max]^T，ΔL_max为最大纬度误差、ΔH_max为最大高度误差、Δλ_max为最大经度误差；

根据Δp_max获取在连续n帧间隔时间T_n内，系统最大发散误差Δp为Δp＝Δp_max/T_n；

计算T_n内匹配定位结果差值：Δp_map＝p_k-p_k-n；p_k为当前k时刻的匹配导航位置，p_k-n为T_n前时刻即k-n时刻的匹配导航位置；

分别将Δp与Δp_map转换至载体坐标系b系下，得到Δp^b、Δp^b与/>均为三维向量，第一维表示纬度误差，第二维为高度误差，第三维为经度误差；

分别比较Δp^b与中的第一维与第三维，即北向、东向位置误差；若则认为匹配结果发生跳变，下一时刻匹配结果继续与上一次成功匹配的位置p_k-1进行比较。

具体来说，在无人机飞行过程中一些区域地形结构特征不明显，导致由于地形相似发生误匹配，如图2，图2为无人造建筑的自然区域。这些区域的基准图相似，因此在位置跳变后，将发生由错误位置开始进行连续的误匹配。这种情况下采用单帧位置跳变(基于无人机最大航速的位置跳变)判别策略无效。针对这一问题，本实施例提出了一种基于惯导辅助的误匹配抑制策略，本实施例利用惯性导航系统虽然误差随时间发散，但是能够保证短时间间隔内的导航精度，因此提出上述判断方法。

本实施例提出的方法针对的是地面特征稀疏场景，由于地形较为相似可能发生连续多帧的误匹配，依靠惯导短时间内精度高的特点对误匹配进行判断。

根据本发明一种实施例，为了准确对量测信息进行有效性判断，通过咔方检验的方式对量测信息有效性进行判断。

本发明实施例中，所述通过咔方检验的方式对量测信息有效性进行判断具体包括：

将故障检测函数λ_k记为：

其中，λ_k是服从自由度为m的卡方分布；m为测量Z_k的维数；r_k为卡尔曼滤波器的残差；A_k为方差。

设计跳变判定准则为：

其中，T_D为预先设置的门限。

具体来说，经过单帧位置跳变和相似地形区域连续跳变判断策略后，大范围的跳变可以有效剔除，对于一些误差较小的误匹配，本实施例通过一种咔方检验的方式对量测信息有效性进行判断：

组合导航卡尔曼滤波器的残差为：

式中，Z_k为系统观测量，即视觉导航得到的地理位置信息。H_k为系统量测矩阵，由视觉导航位置与惯性导航位置间的关联确定，为k时刻系统误差向量估计值，误差变量数量及定义由惯导系统误差建模确定。

式中预报值为：

式为卡尔曼滤波一步预测公式，表示由k-1时刻的状态估计预测k时刻的状态估计。

可以证明，当系统观测信息即视觉导航的位置信息正常时卡尔曼滤波器的残差r_k是零均值高斯白噪声，而方差为：

式中，P_k/k-1为系统一步预测均方误差，R_k为系统量测噪声矩阵，根据观测信息噪声量级确定。

当观测信息故障即视觉导航解算的位置信息出现跳变时，残差r_k的均值就不再为零了。因此，通过对残差r_k的均值的检验即可确定视觉导航是否跳变。

将故障检测函数记为：

式中λ_k是服从自由度为m的卡方分布，即λ_k:χ²(m)。m为测量Z_k的维数。跳变判定准则为：

其中预先设置的门限T_D由多次试验数据仿真验证确定，当卡方检验算法监测到视觉导航发生跳变时，按照改进的滤波算法处理视觉信息。

而且在每次视觉导航自适应滤波算法中均按照上述实施例方法对量测信息进行咔方检验。

在上述实施例中，通过下式获取量测噪声矩阵R_k：

其中，a为比例系数，x为自变量。

具体来说，无人机地图匹配导航获取到实时图像中心位置经纬度坐标，通过惯导姿态信息与对地高度信息转换为无人机实时位置，因此视觉导航精度与无人机飞行高度相关。在不同高度下观测信息精度不高，同时在上述实施例判断出的视觉跳变信息若不处理也会对组合导航系统的稳定性产生影响，针对这一问题，本实施例提出了基于惯性导航信息的自适应量测噪声阵计算方法，由卡尔曼滤波算法增益K_k的计算公式：

其中量测噪声矩阵R_k体现了观测量是否准确，直接影响对估计误差的修正作用。而随着无人机对地距离的减小，测量精度提高，此时应减小R_k矩阵，保证视觉观测量对惯导系统的修正作用。因此，针对视觉匹配导航精度随无人机相对地面距离变化的问题，本实施例通过采用sigmoid函数对R_k进行拟合并确定比例系数，如图3所示，sigmoid函数如下式

其中，a为比例系数，由不同型号无人机飞行速度确定。当判断到视觉导航位置跳变时，依据纯惯性导航结果计算跳变距离，根据跳变程度按照式计算其具体的R_k值，减小视觉导航对惯性导航的修正作用。通过式将R_k拟合到理想范围之间，对无人机巡航阶段组合导航精度提升具有重要意义。

根据另一实施例，还提供一种计算机设备，包括存储器、处理器及存储在所述存储器中并可在所述处理器上运行的计算机程序，其中，所述处理器执行所述计算机程序时实现上述方法的步骤。

如上针对一种实施例描述和/或示出的特征可以以相同或类似的方式在一个或更多个其它实施例中使用，和/或与其它实施例中的特征相结合或替代其它实施例中的特征使用。

应该强调，术语“包括/包含”在本文使用时指特征、整件、步骤或组件的存在，但并不排除一个或更多个其它特征、整件、步骤、组件或其组合的存在或附加。

本发明以上的方法可以由硬件实现，也可以由硬件结合软件实现。本发明涉及这样的计算机可读程序，当该程序被逻辑部件所执行时，能够使该逻辑部件实现上文所述的装置或构成部件，或使该逻辑部件实现上文所述的各种方法或步骤。本发明还涉及用于存储以上程序的存储介质，如硬盘、磁盘、光盘、DVD、flash存储器等。

这些实施例的许多特征和优点根据该详细描述是清楚的，因此所附权利要求旨在覆盖这些实施例的落入其真实精神和范围内的所有这些特征和优点。此外，由于本领域的技术人员容易想到很多修改和改变，因此不是要将本发明的实施例限于所例示和描述的精确结构和操作，而是可以涵盖落入其范围内的所有合适修改和等同物。

本发明未详细说明部分为本领域技术人员公知技术。

Claims (9)

1.一种无人机地图匹配位置跳变抑制方法，其特征在于，所述方法包括：

判断是否存在视觉跳变信息，若存在，则采用改进的视觉导航自适应滤波算法进行视觉导航，其中，采用sigmoid函数对视觉导航自适应滤波算法中的量测噪声矩阵R_k进行拟合并确定比例系数。

2.根据权利要求1所述的一种无人机地图匹配位置跳变抑制方法，其特征在于，通过下式获取量测噪声矩阵R_k：

其中，a为比例系数，x为自变量。

3.根据权利要求1或2所述的一种无人机地图匹配位置跳变抑制方法，其特征在于，所述判断是否存在视觉跳变信息，包括：

同时对单帧位置跳变、相似地形区域连续跳变以及自适应滤波算法中量测信息有效性进行判断，若三种情况下有一种出现跳变，即判定为存在视觉跳变信息。

4.根据权利要求3所述的一种无人机地图匹配位置跳变抑制方法，其特征在于，通过下述方式对单帧位置跳变进行判断：

在连续两帧匹配时间间隔ΔT内进行如下式(1)和(2)判断，若式(1)和(2)均满足则认为匹配信息正确，否则，判断当前时刻地图匹配信息错误，存在跳变；

(Lat₂-Lat₁)×R_m ≤(v_n×ΔT) (1)

(Lon₂-Lon₁)×R_m×cos(Lat₂)≤(v_e×ΔT)(2)

式中，Lat₁为上一帧地图匹配得到的纬度信息，Lon₁为经度信息，当前帧地图匹配得到的纬度信息为Lat₂，经度信息为Lon₂，无人机北向速度、东向速度分别为v_n、v_e，R_m、R_n分别为地球子午圈曲率半径长度与卯酉圈曲率半径长度。

5.根据权利要求3所述的一种无人机地图匹配位置跳变抑制方法，其特征在于，通过下述方式对相似地形区域连续跳变进行判断：

根据惯导标定器件误差获取在一个舒拉周期内惯导位置误差发散峰值Δp_max，其中Δp_max＝[ΔL_maxΔH_maxΔλ_max]^T，ΔL_max为最大纬度误差、ΔH_max为最大高度误差、Δλ_max为最大经度误差；

根据Δp_max获取在连续n帧间隔时间T_n内，系统最大发散误差Δp为Δp＝Δp_max/T_n；

计算T_n内匹配定位结果差值：Δp_map＝p_k-p_k-n；p_k为当前k时刻的匹配导航位置，p_k-n为T_n前时刻即k-n时刻的匹配导航位置；

分别将Δp与Δp_map转换至载体坐标系b系下，得到Δp^b与/>均为三维向量，第一维表示纬度误差，第二维为高度误差，第三维为经度误差；

分别比较Δp^b与中的第一维与第三维，若/>则认为匹配结果发生跳变，下一时刻匹配结果继续与上一次成功匹配的位置p_k-1进行比较。

6.根据权利要求3所述的一种无人机地图匹配位置跳变抑制方法，其特征在于，通过咔方检验的方式对量测信息有效性进行判断。

7.根据权利要求6所述的一种无人机地图匹配位置跳变抑制方法，其特征在于，所述通过咔方检验的方式对量测信息有效性进行判断具体包括：

将故障检测函数λ_k记为：

其中，λ_k是服从自由度为m的卡方分布；m为测量Z_k的维数；r_k为卡尔曼滤波器的残差；A_k为方差。

设计跳变判定准则为：

其中，T_D为预先设置的门限。

8.根据权利要求6或7所述的一种无人机地图匹配位置跳变抑制方法，其特征在于，所述方法还包括：在每次视觉导航自适应滤波算法中均对量测信息进行咔方检验。

9.一种计算机设备，包括存储器、处理器及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-8所述方法的步骤。