CN112068168A - 一种基于视觉误差补偿的地质灾害未知环境组合导航方法 - Google Patents

Abstract

本发明公开了一种基于视觉误差补偿的地质灾害未知环境组合导航方法，属于导航定位及信息融合应用技术领域。本发明利用单目视觉获取救援场景的关键帧信息，实时解算姿态变换矩阵，为救援平台提供高精度相对定位结果，实现了卫星导航定位失效状态下的定位维持，同时，基于一阶马尔科夫模型利用视觉姿态变换为卫星导航进行实时误差补偿，最终给出高精度、可信的定位结果。本发明方法简单易行，能够在地质灾害环境下为救援平台提供高精度的可信定位技术手段。

Description

一种基于视觉误差补偿的地质灾害未知环境组合导航方法

技术领域

本发明涉及一种基于视觉误差补偿的地质灾害未知环境组合导航方法，属于导航定位及信息融合应用技术领域。

背景技术

目前，面向未知、复杂环境的高精度定位服务需求与日俱增。在山区、沟谷等地质灾害频发地带，受卫星信号遮挡、多径等影响，工程机械装备等车辆面临卫星导航定位失效的状况，致使车辆行驶和作业安全面临严重威胁。传统的基于单一传感的定位技术受适用环境的影响，其可靠性和精度均无法满足复杂环境下的高精度定位的需求，因此，亟需引入基于多源传感融合互补的高精度、可信定位技术手段。

导航与定位技术的发展同传感信源息息相关。卫星导航定位技术是依靠导航卫星的无线电导航***，可以为车辆等平台提供全天候、实时、连续的绝对位置信息和时间信息。但是，作为一种无线电信号，信号遮挡、环境干扰等也将直接导致接收机丧失定位能力。基于单目视觉的定位技术，通过连续图像序列的快速精确匹配，构建关键帧里程计，获取相对姿态，可为运动平台提供高精度的相对姿态变换。随着传感器成本的降低和多源融合技术的发展，运用多源信息融合的高精度定位技术将实现不同传感器的优势互补和误差修正。但是，现有技术中尚没有相关应用。

发明内容

有鉴于此，本发明提出了一种基于视觉误差补偿的地质灾害未知环境组合导航方法，该方法突破了单一传感器环境适用性能力限制，实现了高精度、可信的稳健定位。

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

一种基于视觉误差补偿的地质灾害未知环境组合导航方法，该方法利用单目视觉求解救援平台相对运动姿态，融合相对定位信息进行卫星导航定位误差补偿，最终得出高精度、可信定位结果；该方法具体包括如下步骤：

(1)基于连续序列图像特征跟踪获取候选关键帧，利用候选关键帧计算相机姿态变换矩阵；

(2)构建一阶马尔科夫模型，基于一阶马尔科夫模型进行视觉辅助北斗误差补偿；

(3)构建卫星导航信号观测信息的可信区间，基于信任函数对观测量权重取值。

进一步的，所述步骤(1)的具体方式如下：

(101)利用基于梯度的运动统计方法获取图像序列特征匹配，依次统计序列图像特征匹配跟踪数目，也就是视图中某一特征点在序列图像中连续匹配的数目，记录视图中最大的跟踪特征数目，选取连续序列中包含最多特征跟踪数目的视图作为候选的关键帧；

(102)相机的内参数为{f_x,f_y,s,c_x,c_y}，其中，s表示相机的扭曲，f_x,f_y表示相机的焦距，c_x,c_y表示图像主点；相机的外参数为{R,t}，其中，R为旋转矩阵，t为平移向量；

(103)选用针孔模型进行相机姿态变换求解，它由一个限定尺度的3×4投影矩阵表示；将3×4的投影矩阵P分解成一个3×3的上三角矩阵K和一个3×4的转移矩阵[R|t]的乘积：

其中，r₁₁,r₁₂,r₁₃,r₂₁,r₂₂,r₂₃,r₃₁,r₃₂,r₃₃为旋转矩阵R的元素，t_x,t_y,t_z为平移矩阵t的元素；

(104)假定X₁＝[x₁,y₁,1]^T,X₂＝[x₂,y₂,1]^T为两幅关键帧对应匹配点对的归一化坐标，定义描述图像姿态变换的基础矩阵

基础矩阵F包含相机的内外参数信息，并对F各元素进行重组变换为列矢量形式，即：

f＝[f₁₁ f₁₂ f₁₃ f₂₁ f₂₂ f₂₃ f₃₁ f₃₂ f₃₃]^T，

对于稳定存在的匹配点对(X₁,X₂)，由对极几何约束X₁ ^TFX₂＝0，有：

[x₁x₂,x₁y₂,x₁,y₁x₂,y₁y₂,y₁,x₂,y₂,1]·f＝0

(105)选择8对匹配点，经代数变换构成线性方程组，满足：

Af＝0

其中，

A为8×9的矩阵；

(106)根据矩阵的唯一性，所有解向量f参数存在比例因子；为了确定标准解，加入约束条件||f||＝1，此条件满足f作为A^TA的最小特征值所对应的特征向量；A的奇异值分解为A＝UDV^T，其中：

V＝[v₁,v₂,v₃,v₄,v₅,v₆,v₇,v₈,v₉]，对应f＝v₉，从而求出基础矩阵F；

(107)定义本质矩阵E：

E＝K^TFK

利用奇异值分解求解第二幅图像外参数：

E＝UDV^T

其中U,V分别是3×3的正交阵，D为3×3的对角阵，其形式为：

对给定的一个本质矩阵E＝Udiag(1,1,0)V^T，由第一幅图像的相机投影矩阵P₁＝K[I,0]推导第二幅图像的投影矩阵P₂，至此，获取关键帧间的相机姿态变换。

进一步的，所述步骤(2)的具体方式如下：

(201)通过北斗导航***给出连续定位观测量，获取运动平台的经度、纬度和高程，并转化为地图原点为起始点的平面地图坐标系下，定义连续时间t时刻北斗导航***给出的位置信息为P_t ^B，其中

x_t ^B表示t时刻北斗导航信号在平面地图坐标系给出的X轴坐标，y_t ^B表示t时刻北斗导航信号在平面地图坐标系给出的Y轴坐标，

表示t时刻北斗导航信号在平面地图坐标系给出的Z轴坐标；定义北斗导航观测量关键帧K_i ^B(i＝1,2,3…)，即要求与获取的图像关键帧K_i(i＝1,2,3…)时间同步，对于图像关键帧中未获取有效姿态的关键帧则不用于后续校正处理；

(202)对于t₁时刻与t₂时刻对应进行姿态估计的两个相邻关键帧I₁与I₂，图像帧I₁到图像帧I₂的相机姿态变换矩阵为T¹²＝[R¹²|t¹²]，其中，R¹²为图像帧I₁到图像帧I₂的旋转矩阵，t¹²为图像帧I₁到图像帧I₂的平移矩阵；两个相邻关键帧I₁与I₂分别对应相应时刻北斗导航***给出的位置信息为P₁ ^B与P₂ ^B，则有如下估计模型：

其中，

为t₁时刻北斗导航***给出的位置信息P₁ ^B乘以转移矩阵估计所得的t₂时刻的位置信息；

(203)假定存在离散时间序列T_n＝{t₁,t₂,t₃,…,t_n}，对于t_n时刻的位置结果估计，存在一阶马尔科夫估计模型：

其中i＝1,2,…,n-1；

对于t_n时刻北斗导航定位***给出的直接定位结果P_n ^B，其置信概率

如果P_c″＜th″，th″为异常概率阈值，则定位结果P_n ^B视为异常值，予以滤除；依靠多步一阶马尔科夫估计模型，以北斗信号丢失前的有效定位结果作为初始值，给出信号缺失时间段的定位估计结果；完成滤除和插值后，对北斗定位结果进行视觉辅助校正，定义限定窗口大小为5的马尔科夫模型状态转移矩阵A和加权矩阵W，给出最终定位修正结果P公式：

P＝WA

W＝(w¹,w²,w³,w⁴,w⁵)

其中，窗口大小为5，即定位离散时间序列T₅＝{t₁,t₂,t₃,t₄,t₅}，P₁ ^B,P₂ ^B,P₃ ^B,P₄ ^B,P₅ ^B为对应时刻北斗导航定位直接观测结果，T¹²,T²³,T³⁴,T⁴⁵为对应时刻关键帧间相机姿态变换矩阵。

进一步的，所述步骤(3)的具体方式如下：

(301)定义包含n个元素的权重矩阵W为一识别框架，定义权重wⁱ为可能取值的值域集合，要求对应权重的马尔科夫模型状态转移矩阵A是互不相容的，要求定义的识别框架矩阵W满足：

权重wⁱ是权重矩阵W的元素，

表示权重矩阵选取为空；

(302)假设存在北斗导航信号直接观测结果B，定义信任函数：

其中B是W的子集，信任函数表示B的所有子集的可能性度量之和，即表示对集合B的总信任；

(303)定义似真度函数：

表示不否定B的信任度，是所有与B相交的集合概率赋值之和，其中

为非B；

(304)定义信任区间[BEL(B),PL(B)]，表示B的信任度区间，当北斗导航信号可观测时，其特殊含义如下：

[1,1]表示北斗导航直接观测信号稳定，信号值B为真；

[0,0]表示北斗导航直接观测信号丢失，其值B不可信；

[0,1]表示北斗导航直接观测信号存在部分遮挡，其值B不可完全取信；

(305)定义窗口大小为5，获取的北斗导航信号观测结果为P₁ ^B,P₂ ^B,P₃ ^B,P₄ ^B,P₅ ^B，若无法直接通过北斗导航信号观测结果判定，则定义其信任区间

获取上述置信概率

其中i＝1,2,3,4；取

由此，确定北斗导航信号直接观测结果的转移矩阵A所对应加权矩阵W中各权重元素的信任区间，权重元素取信任函数和似真度函数的均值。

本发明与现有技术相比所取得的有益效果为：

1、本发明利用高精度视觉定位信息对卫星导航进行误差修正，提高了现有单一卫星定位技术的精确性、鲁棒性。

2、本发明基于信任函数的观测量权重方法增加了卫星导航定位观测信息可信度量，提高了实际定位结果可信度。

3、本发明方法简单易行，能够在地质灾害环境下为救援平台提供高精度的可信定位技术手段。

附图说明

图1为本发明实施例中组合导航方法的原理示意图。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图对本发明的技术方案作进一步说明。

一种基于视觉误差补偿的地质灾害未知环境组合导航方法，本方法所需的设备包括光学相机和北斗接收机。具体来说，如图1所示，通过光学相机获取序列图像，北斗接收机获取卫星观测量，对序列图像获取救援场景的关键帧信息，实时解算姿态变换矩阵，为救援平台提供高精度相对定位结果，同时基于一阶马尔科夫模型利用视觉姿态变换为卫星导航进行实时误差补偿，最终给出定位结果，包括如下步骤：

(1)基于特征跟踪的关键帧姿态变换矩阵解算，首先基于连续序列图像特征跟踪获取候选关键帧，然后，利用候选关键帧计算相机姿态变换矩阵；

(2)基于一阶马尔科夫模型的视觉辅助北斗误差补偿方法，首先构建一阶马尔科夫模型，然后，基于一阶马尔科夫模型进行视觉辅助北斗误差补偿；

(3)基于信任函数的观测量权重取值，首先构建卫星导航信号观测信息的可信区间，然后，基于信任函数对观测量权重取值。

进一步地，所述步骤(1)的具体方式为：

候选关键帧选取；关键帧的选择对相机姿态变换至关重要，如果相机关键帧的重建陷入错误的局部最小值，则后续优化困难，因此必须谨慎选择相机关键帧，为满足相机姿态变换要求，关键帧首先应该具有足够的匹配点，同时也应满足较大的基线需要，这里也可以理解为相机中心之间存在足够的计算距离或者两视图观测方向存在一定合理的角度，从而可以鲁棒地估计两帧的姿态变换。利用现有的GMS(Grad-based Motion Statistic)算法获取图像序列特征匹配，随后依次统计序列图像特征匹配跟踪数目，也就是视图中某一特征点在序列图像中连续匹配的数目，记录视图中最大的跟踪特征数目，选取连续序列中包含最多特征跟踪数目的视图作为候选的关键帧。

利用邻近候选的关键帧求解相机外参数计算相机姿态变换矩阵；假定存在候选邻近参考关键帧对应的匹配点对X₁和X₂，这时，可以给出相机的内参数矩阵K，并且内参数为：{f_x,f_y,s,c_x,c_y}，分别表示相机的扭曲s，f_x,f_y表示工业相机的焦距，c_x,c_y表示为图像主点，一般状况下为图像的中心点，相机的内参数是固定的，一般由相机厂商给出。对于相机在空间中的运动，空间某一定点P的相机坐标是世界坐标系下某一空间点，在数学关系中，借助于旋转矩阵R、平移向量t来确定相机的运动数学模型，而相机的位姿又称为相机的外参数，包括{R,t}。选用针孔模型进行相机姿态变换求解，它由一个限定尺度的3×4投影矩阵表示。任何一个3×4的投影矩阵P都可以被分解成一个3×3的上三角矩阵K和一个3×4的转移矩阵[R|t]的乘积：

r₁₁,r₁₂,r₁₃,r₂₁,r₂₂,r₂₃,r₃₁,r₃₂,r₃₃为旋转矩阵R的元素，t_x,t_y,t_z为平移矩阵t的元素，假定X₁＝[x₁,y₁,1]^T,X₂＝[x₂,y₂,1]^T为两幅关键帧对应匹配点对的归一化坐标，定义描述图像姿态变换的基础矩阵

基础矩阵F包含相机的内外参数信息，并对F各元素进行重组变换为列矢量形式，即f＝[f₁₁ f₁₂ f₁₃ f₂₁ f₂₂ f₂₃ f₃₁ f₃₂ f₃₃]^T，对于稳定存在的匹配点对(X₁,X₂)，由对极几何约束X₁ ^TFX₂＝0，有：

[x₁x₂,x₁y₂,x₁,y₁x₂,y₁y₂,y₁,x₂,y₂,1]·f＝0

选择8对匹配点，经代数变换构成如上式线性方程组，根据相差一个常数因子的情况下的唯一性原则，求解齐次方程确定F矩阵。将8个这样的方程组成线性方程组，满足：

Af＝0

其中

A为8×9的矩阵。根据矩阵的唯一性，所有解向量f参数存在比例因子。为了确定标准解，加入约束条件||f||＝1，此条件满足f作为A^TA的最小特征值所对应的特征向量。A的奇异值分解为A＝UDV^T，其中：

V＝[v₁,v₂,v₃,v₄,v₅,v₆,v₇,v₈,v₉]，对应f＝v₉，从而求出F。

基础矩阵F包含内外参数信息，其求解过程需要本质矩阵E完成，本质矩阵E是基础矩阵F的特殊形式，本质矩阵E可由F进行代数变换获得，进而求解关键帧图像的姿态变换。本质矩阵的定义为：

E＝K^TFK

利用奇异值分解求解第二幅图像外参数：

E＝UDV^T

其中U,V分别是3×3的正交阵，D为3×3的对角阵，其形式为：

对给定的一个本质矩阵：E＝Udiag(1,1,0)V^T，由第一幅图像的相机投影矩阵P₁＝K[I,0]推导第二幅图像的投影矩阵P₂。至此可以获取关键帧间的相机姿态变换。

进一步地，所述步骤(2)的具体方式为：

通过北斗导航***给出连续定位观测量，获取运动平台经度、纬度和高程三维位置信息，并转化为地图原点为起始点的平面地图坐标系下，定义连续时间t时刻北斗导航***给出的位置信息为P_t ^B，其中

x_t ^B表示t时刻北斗导航信号在平面地图坐标系给出的X轴坐标，y_t ^B表示t时刻北斗导航信号在平面地图坐标系给出的Y轴坐标，

表示t时刻北斗导航信号在平面地图坐标系给出的Z轴坐标。定义北斗导航观测量关键帧K_i ^B(i＝1,2,3…)，即要求与获取的图像关键帧K_i(i＝1,2,3…)时间同步，对于图像关键帧中未获取有效姿态的关键帧则不用于后续校正处理。

构建一阶马尔科夫估计模型；上述权利要求1视觉关键帧已获取对应的相机姿态，对于t₁时刻与t₂时刻对应进行姿态估计的两个相邻关键帧I₁与I₂，图像帧I₁到图像帧I₂的相机姿态变换矩阵为T¹²＝[R¹²|t¹²]，其中，R¹²为图像帧I₁到图像帧I₂的旋转矩阵，t¹²为图像帧I₁到图像帧I₂的平移矩阵。两个相邻关键帧I₁与I₂分别对应相应时刻北斗导航***给出的位置信息为P₁ ^B与P₂ ^B，则应有如下估计模型：

为t₁时刻北斗导航***给出的位置信息P₁ ^B乘以转移矩阵估计所得的t₂时刻的位置信息。对于理想状态下，

应等于P₂ ^B，由于北斗接收机和光学相机的实际适用性和噪声误差不可避免，

一般不等于P₂ ^B，甚至存在较大偏差。已知北斗导航***给出的连续离散观测信息并不存在明显的约束关系，即当前时刻的定位结果不受前一时刻的影响，同时也不会影响后续时刻的定位结果，因此上述估计模型

符合一阶马尔科夫估计模型。假定存在离散时间序列T_n＝{t₁,t₂,t₃,…,t_n}，对于t_n时刻的位置结果估计，存在一阶马尔科夫估计模型：

其中i＝1,2,…,n-1；

一阶马尔科夫模型的视觉辅助北斗误差补偿；考虑估计模型的效率并尽可能消除时间累积误差的影响，采用有限时间段滑动窗口来控制定位结果的估计，具体方法如下：首先异常观测量定位结果滤除，对于t_n时刻北斗导航定位***给出的直接定位结果P_n ^B，其置信概率

如果P_c″＜th″，th″为异常概率阈值，则定位结果P_n ^B视为异常值，滤除。北斗信号缺失段定位结果插值，依靠多步一阶马尔科夫估计模型，以北斗信号丢失前的有效定位结果作为初始值，给出信号缺失时间段的定位估计结果。完成滤除和插值后，可对北斗定位结果进行视觉辅助校正，定义限定窗口大小为5的马尔科夫模型状态转移矩阵A和加权矩阵W，给出最终定位修正结果P公式：

P＝WA

W＝(w¹,w²,w³,w⁴,w⁵)

其中，窗口大小为5，即定位离散时间序列T₅＝{t₁,t₂,t₃,t₄,t₅}，P₁ ^B,P₂ ^B,P₃ ^B,P₄ ^B,P₅ ^B为对应时刻北斗导航定位直接观测结果，T¹²,T²³,T³⁴,T⁴⁵为对应时刻关键帧间相机姿态变换矩阵。

进一步地，所述步骤(3)的具体方式为：

利用信任函数约束权重矩阵W大小，定义包含n个元素的权重矩阵W为一识别框架，定义权重wⁱ为可能取值的值域集合，要求对应权重的马尔科夫模型状态转移矩阵A是互不相容的，要求定义的识别框架矩阵W满足：

权重wⁱ是权重矩阵W的元素，

表示权重矩阵选取为空。

假设存在北斗导航信号直接观测结果B，定义信任函数：

其中B是W的子集，信任函数表示B的所有子集的可能性度量之和，即表示对集合B的总信任。

定义似真度函数：

表示不否定B的信任度，是所有与B相交的集合概率赋值之和，其中

为非B，

定义信任区间[BEL(B),PL(B)]，表示B的信任度区间，当北斗导航信号可观测时，其特殊含义如下：

[1,1]表示北斗导航直接观测信号稳定，信号值B为真；

[0,0]表示北斗导航直接观测信号丢失，其值B不可信；

[0,1]表示北斗导航直接观测信号存在部分遮挡，其值B不可完全取信；

定义上述窗口大小为5，获取的北斗导航信号观测结果为P₁ ^B,P₂ ^B,P₃ ^B,P₄ ^B,P₅ ^B，若无法直接通过北斗导航信号观测结果判定，可定义其信任区间

获取上述置信概率

其中i＝1,2,3,4；可取

此时，可确定北斗导航信号直接观测结果的转移矩阵A所对应加权矩阵W中各权重元素的信任区间，权重元素可取信任函数和似真度函数的均值。

本发明给出一种基于视觉误差补偿的地质灾害未知环境组合导航方法。在地质灾害环境下，工程机械车辆等运动救援平台的定位的准确性将直接影响救援平台的行驶和工作安全。卫星导航在地质灾害环境下，由于信号遮挡、多径，易引发卫星导航定位降效、失效，同时定位可信度较低，因此，亟需地质灾害环境下能够为救援平台提供高精度可信定位的技术手段。

本发明利用单目视觉获取救援场景的关键帧信息，实时解算姿态变换矩阵，为救援平台提供高精度相对定位结果，实现了卫星导航定位失效状态下的定位维持，其次，基于一阶马尔科夫模型利用视觉姿态变换为卫星导航进行实时误差补偿，最终给出高精度、可信的定位结果。

Claims (4)

1.一种基于视觉误差补偿的地质灾害未知环境组合导航方法，其特征在于，利用单目视觉求解救援平台相对运动姿态，融合相对定位信息进行卫星导航定位误差补偿，最终得出高精度、可信定位结果；具体包括如下步骤：

(1)基于连续序列图像特征跟踪获取候选关键帧，利用候选关键帧计算相机姿态变换矩阵；

(2)构建一阶马尔科夫模型，基于一阶马尔科夫模型进行视觉辅助北斗误差补偿；

(3)构建卫星导航信号观测信息的可信区间，基于信任函数对观测量权重取值。

2.根据权利要求1所述的一种基于视觉误差补偿的地质灾害未知环境组合导航方法，其特征在于，所述步骤(1)的具体方式如下：

(101)利用基于梯度的运动统计方法获取图像序列特征匹配，依次统计序列图像特征匹配跟踪数目，也就是视图中某一特征点在序列图像中连续匹配的数目，记录视图中最大的跟踪特征数目，选取连续序列中包含最多特征跟踪数目的视图作为候选的关键帧；

(102)相机的内参数为{f_x,f_y,s,c_x,c_y}，其中，s表示相机的扭曲，f_x,f_y表示相机的焦距，c_x,c_y表示图像主点；相机的外参数为{R,t}，其中，R为旋转矩阵，t为平移向量；

(103)选用针孔模型进行相机姿态变换求解，它由一个限定尺度的3×4投影矩阵表示；将3×4的投影矩阵P分解成一个3×3的上三角矩阵K和一个3×4的转移矩阵[R|t]的乘积：

其中，r₁₁,r₁₂,r₁₃,r₂₁,r₂₂,r₂₃,r₃₁,r₃₂,r₃₃为旋转矩阵R的元素，t_x,t_y,t_z为平移矩阵t的元素；

(104)假定X₁＝[x₁,y₁,1]^T,X₂＝[x₂,y₂,1]^T为两幅关键帧对应匹配点对的归一化坐标，定义描述图像姿态变换的基础矩阵

基础矩阵F包含相机的内外参数信息，并对F各元素进行重组变换为列矢量形式，即：

f＝[f₁₁ f₁₂ f₁₃ f₂₁ f₂₂ f₂₃ f₃₁ f₃₂ f₃₃]^T，

对于稳定存在的匹配点对(X₁,X₂)，由对极几何约束X₁ ^TFX₂＝0，有：

[x₁x₂,x₁y₂,x₁,y₁x₂,y₁y₂,y₁,x₂,y₂,1]·f＝0

(105)选择8对匹配点，经代数变换构成线性方程组，满足：

Af＝0

其中，

A为8×9的矩阵；

(106)根据矩阵的唯一性，所有解向量f参数存在比例因子；为了确定标准解，加入约束条件||f||＝1，此条件满足f作为A^TA的最小特征值所对应的特征向量；A的奇异值分解为A＝UDV^T，其中：

V＝[v₁,v₂,v₃,v₄,v₅,v₆,v₇,v₈,v₉]，对应f＝v₉，从而求出基础矩阵F；

(107)定义本质矩阵E：

E＝K^TFK

利用奇异值分解求解第二幅图像外参数：

E＝UDV^T

其中U,V分别是3×3的正交阵，D为3×3的对角阵，其形式为：

对给定的一个本质矩阵E＝Udiag(1,1,0)V^T，由第一幅图像的相机投影矩阵P₁＝K[I,0]推导第二幅图像的投影矩阵P₂，至此，获取关键帧间的相机姿态变换。

3.根据权利要求1所述的一种基于视觉误差补偿的地质灾害未知环境组合导航方法，其特征在于，所述步骤(2)的具体方式如下：

(201)通过北斗导航***给出连续定位观测量，获取运动平台的经度、纬度和高程，并转化为地图原点为起始点的平面地图坐标系下，定义连续时间t时刻北斗导航***给出的位置信息为P_t ^B，其中

x_t ^B表示t时刻北斗导航信号在平面地图坐标系给出的X轴坐标，y_t ^B表示t时刻北斗导航信号在平面地图坐标系给出的Y轴坐标，

表示t时刻北斗导航信号在平面地图坐标系给出的Z轴坐标；定义北斗导航观测量关键帧K_i ^B(i＝1,2,3…)，即要求与获取的图像关键帧K_i(i＝1,2,3…)时间同步，对于图像关键帧中未获取有效姿态的关键帧则不用于后续校正处理；

(202)对于t₁时刻与t₂时刻对应进行姿态估计的两个相邻关键帧I₁与I₂，图像帧I₁到图像帧I₂的相机姿态变换矩阵为T¹²＝[R¹²|t¹²]，其中，R¹²为图像帧I₁到图像帧I₂的旋转矩阵，t¹²为图像帧I₁到图像帧I₂的平移矩阵；两个相邻关键帧I₁与I₂分别对应相应时刻北斗导航***给出的位置信息为P₁ ^B与

则有如下估计模型：

其中，

为t₁时刻北斗导航***给出的位置信息P₁ ^B乘以转移矩阵估计所得的t₂时刻的位置信息；

(203)假定存在离散时间序列T_n＝{t₁,t₂,t₃,…,t_n}，对于t_n时刻的位置结果估计，存在一阶马尔科夫估计模型：

其中i＝1,2,…,n-1；

对于t_n时刻北斗导航定位***给出的直接定位结果

其置信概率

如果P_c″＜th″，th″为异常概率阈值，则定位结果

视为异常值，予以滤除；依靠多步一阶马尔科夫估计模型，以北斗信号丢失前的有效定位结果作为初始值，给出信号缺失时间段的定位估计结果；完成滤除和插值后，对北斗定位结果进行视觉辅助校正，定义限定窗口大小为5的马尔科夫模型状态转移矩阵A和加权矩阵W，给出最终定位修正结果P公式：

P＝WA

W＝(w¹,w²,w³,w⁴,w⁵)

其中，窗口大小为5，即定位离散时间序列T₅＝{t₁,t₂,t₃,t₄,t₅}，P₁ ^B,P₂ ^B,P₃ ^B,P₄ ^B,P₅ ^B为对应时刻北斗导航定位直接观测结果，T¹²,T²³,T³⁴,T⁴⁵为对应时刻关键帧间相机姿态变换矩阵。

4.根据权利要求1所述的一种基于视觉误差补偿的地质灾害未知环境组合导航方法，其特征在于，所述步骤(3)的具体方式如下：

(301)定义包含n个元素的权重矩阵W为一识别框架，定义权重wⁱ为可能取值的值域集合，要求对应权重的马尔科夫模型状态转移矩阵A是互不相容的，要求定义的识别框架矩阵W满足：

权重wⁱ是权重矩阵W的元素，

表示权重矩阵选取为空；

(302)假设存在北斗导航信号直接观测结果B，定义信任函数：

其中B是W的子集，信任函数表示B的所有子集的可能性度量之和，即表示对集合B的总信任；

(303)定义似真度函数：

表示不否定B的信任度，是所有与B相交的集合概率赋值之和，其中

为非B；

(304)定义信任区间[BEL(B),PL(B)]，表示B的信任度区间，当北斗导航信号可观测时，其特殊含义如下：

[1,1]表示北斗导航直接观测信号稳定，信号值B为真；

[0,0]表示北斗导航直接观测信号丢失，其值B不可信；

[0,1]表示北斗导航直接观测信号存在部分遮挡，其值B不可完全取信；

(305)定义窗口大小为5，获取的北斗导航信号观测结果为P₁ ^B,P₂ ^B,P₃ ^B,P₄ ^B,P₅ ^B，若无法直接通过北斗导航信号观测结果判定，则定义其信任区间

获取上述置信概率

其中i＝1,2,3,4；取

由此，确定北斗导航信号直接观测结果的转移矩阵A所对应加权矩阵W中各权重元素的信任区间，权重元素取信任函数和似真度函数的均值。

Images