CN112068168A - 一种基于视觉误差补偿的地质灾害未知环境组合导航方法 - Google Patents
一种基于视觉误差补偿的地质灾害未知环境组合导航方法 Download PDFInfo
- Publication number
- CN112068168A CN112068168A CN202010933698.8A CN202010933698A CN112068168A CN 112068168 A CN112068168 A CN 112068168A CN 202010933698 A CN202010933698 A CN 202010933698A CN 112068168 A CN112068168 A CN 112068168A
- Authority
- CN
- China
- Prior art keywords
- matrix
- positioning
- beidou navigation
- camera
- image
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 24
- 230000000007 visual effect Effects 0.000 title claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims abstract description 103
- 230000009466 transformation Effects 0.000 claims abstract description 31
- 238000012937 correction Methods 0.000 claims description 11
- 230000007704 transition Effects 0.000 claims description 9
- 238000013519 translation Methods 0.000 claims description 8
- 230000002159 abnormal effect Effects 0.000 claims description 7
- 238000001914 filtration Methods 0.000 claims description 7
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 6
- 239000000243 solution Substances 0.000 claims description 4
- 238000003491 array Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 239000012086 standard solution Substances 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000007619 statistical method Methods 0.000 claims description 2
- 230000004927 fusion Effects 0.000 abstract description 5
- 238000012423 maintenance Methods 0.000 abstract description 2
- 230000003287 optical effect Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 125000004432 carbon atom Chemical group C* 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/40—Correcting position, velocity or attitude
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
- G01S19/07—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing data for correcting measured positioning data, e.g. DGPS [differential GPS] or ionosphere corrections
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
- G01S19/10—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing dedicated supplementary positioning signals
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于视觉误差补偿的地质灾害未知环境组合导航方法,属于导航定位及信息融合应用技术领域。本发明利用单目视觉获取救援场景的关键帧信息,实时解算姿态变换矩阵,为救援平台提供高精度相对定位结果,实现了卫星导航定位失效状态下的定位维持,同时,基于一阶马尔科夫模型利用视觉姿态变换为卫星导航进行实时误差补偿,最终给出高精度、可信的定位结果。本发明方法简单易行,能够在地质灾害环境下为救援平台提供高精度的可信定位技术手段。
Description
技术领域
本发明涉及一种基于视觉误差补偿的地质灾害未知环境组合导航方法,属于导航定位及信息融合应用技术领域。
背景技术
目前,面向未知、复杂环境的高精度定位服务需求与日俱增。在山区、沟谷等地质灾害频发地带,受卫星信号遮挡、多径等影响,工程机械装备等车辆面临卫星导航定位失效的状况,致使车辆行驶和作业安全面临严重威胁。传统的基于单一传感的定位技术受适用环境的影响,其可靠性和精度均无法满足复杂环境下的高精度定位的需求,因此,亟需引入基于多源传感融合互补的高精度、可信定位技术手段。
导航与定位技术的发展同传感信源息息相关。卫星导航定位技术是依靠导航卫星的无线电导航***,可以为车辆等平台提供全天候、实时、连续的绝对位置信息和时间信息。但是,作为一种无线电信号,信号遮挡、环境干扰等也将直接导致接收机丧失定位能力。基于单目视觉的定位技术,通过连续图像序列的快速精确匹配,构建关键帧里程计,获取相对姿态,可为运动平台提供高精度的相对姿态变换。随着传感器成本的降低和多源融合技术的发展,运用多源信息融合的高精度定位技术将实现不同传感器的优势互补和误差修正。但是,现有技术中尚没有相关应用。
发明内容
有鉴于此,本发明提出了一种基于视觉误差补偿的地质灾害未知环境组合导航方法,该方法突破了单一传感器环境适用性能力限制,实现了高精度、可信的稳健定位。
为了实现上述目的,本发明采用的技术方案为:
一种基于视觉误差补偿的地质灾害未知环境组合导航方法,该方法利用单目视觉求解救援平台相对运动姿态,融合相对定位信息进行卫星导航定位误差补偿,最终得出高精度、可信定位结果;该方法具体包括如下步骤:
(1)基于连续序列图像特征跟踪获取候选关键帧,利用候选关键帧计算相机姿态变换矩阵;
(2)构建一阶马尔科夫模型,基于一阶马尔科夫模型进行视觉辅助北斗误差补偿;
(3)构建卫星导航信号观测信息的可信区间,基于信任函数对观测量权重取值。
进一步的,所述步骤(1)的具体方式如下:
(101)利用基于梯度的运动统计方法获取图像序列特征匹配,依次统计序列图像特征匹配跟踪数目,也就是视图中某一特征点在序列图像中连续匹配的数目,记录视图中最大的跟踪特征数目,选取连续序列中包含最多特征跟踪数目的视图作为候选的关键帧;
(102)相机的内参数为{fx,fy,s,cx,cy},其中,s表示相机的扭曲,fx,fy表示相机的焦距,cx,cy表示图像主点;相机的外参数为{R,t},其中,R为旋转矩阵,t为平移向量;
(103)选用针孔模型进行相机姿态变换求解,它由一个限定尺度的3×4投影矩阵表示;将3×4的投影矩阵P分解成一个3×3的上三角矩阵K和一个3×4的转移矩阵[R|t]的乘积:
其中,r11,r12,r13,r21,r22,r23,r31,r32,r33为旋转矩阵R的元素,tx,ty,tz为平移矩阵t的元素;
(104)假定X1=[x1,y1,1]T,X2=[x2,y2,1]T为两幅关键帧对应匹配点对的归一化坐标,定义描述图像姿态变换的基础矩阵基础矩阵F包含相机的内外参数信息,并对F各元素进行重组变换为列矢量形式,即:
f=[f11 f12 f13 f21 f22 f23 f31 f32 f33]T,
对于稳定存在的匹配点对(X1,X2),由对极几何约束X1 TFX2=0,有:
[x1x2,x1y2,x1,y1x2,y1y2,y1,x2,y2,1]·f=0
(105)选择8对匹配点,经代数变换构成线性方程组,满足:
Af=0
其中,
A为8×9的矩阵;
(106)根据矩阵的唯一性,所有解向量f参数存在比例因子;为了确定标准解,加入约束条件||f||=1,此条件满足f作为ATA的最小特征值所对应的特征向量;A的奇异值分解为A=UDVT,其中:
V=[v1,v2,v3,v4,v5,v6,v7,v8,v9],对应f=v9,从而求出基础矩阵F;
(107)定义本质矩阵E:
E=KTFK
利用奇异值分解求解第二幅图像外参数:
E=UDVT
其中U,V分别是3×3的正交阵,D为3×3的对角阵,其形式为:
对给定的一个本质矩阵E=Udiag(1,1,0)VT,由第一幅图像的相机投影矩阵P1=K[I,0]推导第二幅图像的投影矩阵P2,至此,获取关键帧间的相机姿态变换。
进一步的,所述步骤(2)的具体方式如下:
(201)通过北斗导航***给出连续定位观测量,获取运动平台的经度、纬度和高程,并转化为地图原点为起始点的平面地图坐标系下,定义连续时间t时刻北斗导航***给出的位置信息为Pt B,其中xt B表示t时刻北斗导航信号在平面地图坐标系给出的X轴坐标,yt B表示t时刻北斗导航信号在平面地图坐标系给出的Y轴坐标,表示t时刻北斗导航信号在平面地图坐标系给出的Z轴坐标;定义北斗导航观测量关键帧Ki B(i=1,2,3…),即要求与获取的图像关键帧Ki(i=1,2,3…)时间同步,对于图像关键帧中未获取有效姿态的关键帧则不用于后续校正处理;
(202)对于t1时刻与t2时刻对应进行姿态估计的两个相邻关键帧I1与I2,图像帧I1到图像帧I2的相机姿态变换矩阵为T12=[R12|t12],其中,R12为图像帧I1到图像帧I2的旋转矩阵,t12为图像帧I1到图像帧I2的平移矩阵;两个相邻关键帧I1与I2分别对应相应时刻北斗导航***给出的位置信息为P1 B与P2 B,则有如下估计模型:
对于tn时刻北斗导航定位***给出的直接定位结果Pn B,其置信概率如果Pc″<th″,th″为异常概率阈值,则定位结果Pn B视为异常值,予以滤除;依靠多步一阶马尔科夫估计模型,以北斗信号丢失前的有效定位结果作为初始值,给出信号缺失时间段的定位估计结果;完成滤除和插值后,对北斗定位结果进行视觉辅助校正,定义限定窗口大小为5的马尔科夫模型状态转移矩阵A和加权矩阵W,给出最终定位修正结果P公式:
P=WA
W=(w1,w2,w3,w4,w5)
其中,窗口大小为5,即定位离散时间序列T5={t1,t2,t3,t4,t5},P1 B,P2 B,P3 B,P4 B,P5 B为对应时刻北斗导航定位直接观测结果,T12,T23,T34,T45为对应时刻关键帧间相机姿态变换矩阵。
进一步的,所述步骤(3)的具体方式如下:
(301)定义包含n个元素的权重矩阵W为一识别框架,定义权重wi为可能取值的值域集合,要求对应权重的马尔科夫模型状态转移矩阵A是互不相容的,要求定义的识别框架矩阵W满足:
(304)定义信任区间[BEL(B),PL(B)],表示B的信任度区间,当北斗导航信号可观测时,其特殊含义如下:
[1,1]表示北斗导航直接观测信号稳定,信号值B为真;
[0,0]表示北斗导航直接观测信号丢失,其值B不可信;
[0,1]表示北斗导航直接观测信号存在部分遮挡,其值B不可完全取信;
(305)定义窗口大小为5,获取的北斗导航信号观测结果为P1 B,P2 B,P3 B,P4 B,P5 B,若无法直接通过北斗导航信号观测结果判定,则定义其信任区间获取上述置信概率 其中i=1,2,3,4;取由此,确定北斗导航信号直接观测结果的转移矩阵A所对应加权矩阵W中各权重元素的信任区间,权重元素取信任函数和似真度函数的均值。
本发明与现有技术相比所取得的有益效果为:
1、本发明利用高精度视觉定位信息对卫星导航进行误差修正,提高了现有单一卫星定位技术的精确性、鲁棒性。
2、本发明基于信任函数的观测量权重方法增加了卫星导航定位观测信息可信度量,提高了实际定位结果可信度。
3、本发明方法简单易行,能够在地质灾害环境下为救援平台提供高精度的可信定位技术手段。
附图说明
图1为本发明实施例中组合导航方法的原理示意图。
具体实施方式
为了更好的说明本发明的目的和优点,下面结合附图对本发明的技术方案作进一步说明。
一种基于视觉误差补偿的地质灾害未知环境组合导航方法,本方法所需的设备包括光学相机和北斗接收机。具体来说,如图1所示,通过光学相机获取序列图像,北斗接收机获取卫星观测量,对序列图像获取救援场景的关键帧信息,实时解算姿态变换矩阵,为救援平台提供高精度相对定位结果,同时基于一阶马尔科夫模型利用视觉姿态变换为卫星导航进行实时误差补偿,最终给出定位结果,包括如下步骤:
(1)基于特征跟踪的关键帧姿态变换矩阵解算,首先基于连续序列图像特征跟踪获取候选关键帧,然后,利用候选关键帧计算相机姿态变换矩阵;
(2)基于一阶马尔科夫模型的视觉辅助北斗误差补偿方法,首先构建一阶马尔科夫模型,然后,基于一阶马尔科夫模型进行视觉辅助北斗误差补偿;
(3)基于信任函数的观测量权重取值,首先构建卫星导航信号观测信息的可信区间,然后,基于信任函数对观测量权重取值。
进一步地,所述步骤(1)的具体方式为:
候选关键帧选取;关键帧的选择对相机姿态变换至关重要,如果相机关键帧的重建陷入错误的局部最小值,则后续优化困难,因此必须谨慎选择相机关键帧,为满足相机姿态变换要求,关键帧首先应该具有足够的匹配点,同时也应满足较大的基线需要,这里也可以理解为相机中心之间存在足够的计算距离或者两视图观测方向存在一定合理的角度,从而可以鲁棒地估计两帧的姿态变换。利用现有的GMS(Grad-based Motion Statistic)算法获取图像序列特征匹配,随后依次统计序列图像特征匹配跟踪数目,也就是视图中某一特征点在序列图像中连续匹配的数目,记录视图中最大的跟踪特征数目,选取连续序列中包含最多特征跟踪数目的视图作为候选的关键帧。
利用邻近候选的关键帧求解相机外参数计算相机姿态变换矩阵;假定存在候选邻近参考关键帧对应的匹配点对X1和X2,这时,可以给出相机的内参数矩阵K,并且内参数为:{fx,fy,s,cx,cy},分别表示相机的扭曲s,fx,fy表示工业相机的焦距,cx,cy表示为图像主点,一般状况下为图像的中心点,相机的内参数是固定的,一般由相机厂商给出。对于相机在空间中的运动,空间某一定点P的相机坐标是世界坐标系下某一空间点,在数学关系中,借助于旋转矩阵R、平移向量t来确定相机的运动数学模型,而相机的位姿又称为相机的外参数,包括{R,t}。选用针孔模型进行相机姿态变换求解,它由一个限定尺度的3×4投影矩阵表示。任何一个3×4的投影矩阵P都可以被分解成一个3×3的上三角矩阵K和一个3×4的转移矩阵[R|t]的乘积:
r11,r12,r13,r21,r22,r23,r31,r32,r33为旋转矩阵R的元素,tx,ty,tz为平移矩阵t的元素,假定X1=[x1,y1,1]T,X2=[x2,y2,1]T为两幅关键帧对应匹配点对的归一化坐标,定义描述图像姿态变换的基础矩阵基础矩阵F包含相机的内外参数信息,并对F各元素进行重组变换为列矢量形式,即f=[f11 f12 f13 f21 f22 f23 f31 f32 f33]T,对于稳定存在的匹配点对(X1,X2),由对极几何约束X1 TFX2=0,有:
[x1x2,x1y2,x1,y1x2,y1y2,y1,x2,y2,1]·f=0
选择8对匹配点,经代数变换构成如上式线性方程组,根据相差一个常数因子的情况下的唯一性原则,求解齐次方程确定F矩阵。将8个这样的方程组成线性方程组,满足:
Af=0
其中
A为8×9的矩阵。根据矩阵的唯一性,所有解向量f参数存在比例因子。为了确定标准解,加入约束条件||f||=1,此条件满足f作为ATA的最小特征值所对应的特征向量。A的奇异值分解为A=UDVT,其中:
V=[v1,v2,v3,v4,v5,v6,v7,v8,v9],对应f=v9,从而求出F。
基础矩阵F包含内外参数信息,其求解过程需要本质矩阵E完成,本质矩阵E是基础矩阵F的特殊形式,本质矩阵E可由F进行代数变换获得,进而求解关键帧图像的姿态变换。本质矩阵的定义为:
E=KTFK
利用奇异值分解求解第二幅图像外参数:
E=UDVT
其中U,V分别是3×3的正交阵,D为3×3的对角阵,其形式为:
对给定的一个本质矩阵:E=Udiag(1,1,0)VT,由第一幅图像的相机投影矩阵P1=K[I,0]推导第二幅图像的投影矩阵P2。至此可以获取关键帧间的相机姿态变换。
进一步地,所述步骤(2)的具体方式为:
通过北斗导航***给出连续定位观测量,获取运动平台经度、纬度和高程三维位置信息,并转化为地图原点为起始点的平面地图坐标系下,定义连续时间t时刻北斗导航***给出的位置信息为Pt B,其中xt B表示t时刻北斗导航信号在平面地图坐标系给出的X轴坐标,yt B表示t时刻北斗导航信号在平面地图坐标系给出的Y轴坐标,表示t时刻北斗导航信号在平面地图坐标系给出的Z轴坐标。定义北斗导航观测量关键帧Ki B(i=1,2,3…),即要求与获取的图像关键帧Ki(i=1,2,3…)时间同步,对于图像关键帧中未获取有效姿态的关键帧则不用于后续校正处理。
构建一阶马尔科夫估计模型;上述权利要求1视觉关键帧已获取对应的相机姿态,对于t1时刻与t2时刻对应进行姿态估计的两个相邻关键帧I1与I2,图像帧I1到图像帧I2的相机姿态变换矩阵为T12=[R12|t12],其中,R12为图像帧I1到图像帧I2的旋转矩阵,t12为图像帧I1到图像帧I2的平移矩阵。两个相邻关键帧I1与I2分别对应相应时刻北斗导航***给出的位置信息为P1 B与P2 B,则应有如下估计模型:为t1时刻北斗导航***给出的位置信息P1 B乘以转移矩阵估计所得的t2时刻的位置信息。对于理想状态下,应等于P2 B,由于北斗接收机和光学相机的实际适用性和噪声误差不可避免,一般不等于P2 B,甚至存在较大偏差。已知北斗导航***给出的连续离散观测信息并不存在明显的约束关系,即当前时刻的定位结果不受前一时刻的影响,同时也不会影响后续时刻的定位结果,因此上述估计模型符合一阶马尔科夫估计模型。假定存在离散时间序列Tn={t1,t2,t3,…,tn},对于tn时刻的位置结果估计,存在一阶马尔科夫估计模型:其中i=1,2,…,n-1;
一阶马尔科夫模型的视觉辅助北斗误差补偿;考虑估计模型的效率并尽可能消除时间累积误差的影响,采用有限时间段滑动窗口来控制定位结果的估计,具体方法如下:首先异常观测量定位结果滤除,对于tn时刻北斗导航定位***给出的直接定位结果Pn B,其置信概率如果Pc″<th″,th″为异常概率阈值,则定位结果Pn B视为异常值,滤除。北斗信号缺失段定位结果插值,依靠多步一阶马尔科夫估计模型,以北斗信号丢失前的有效定位结果作为初始值,给出信号缺失时间段的定位估计结果。完成滤除和插值后,可对北斗定位结果进行视觉辅助校正,定义限定窗口大小为5的马尔科夫模型状态转移矩阵A和加权矩阵W,给出最终定位修正结果P公式:
P=WA
W=(w1,w2,w3,w4,w5)
其中,窗口大小为5,即定位离散时间序列T5={t1,t2,t3,t4,t5},P1 B,P2 B,P3 B,P4 B,P5 B为对应时刻北斗导航定位直接观测结果,T12,T23,T34,T45为对应时刻关键帧间相机姿态变换矩阵。
进一步地,所述步骤(3)的具体方式为:
利用信任函数约束权重矩阵W大小,定义包含n个元素的权重矩阵W为一识别框架,定义权重wi为可能取值的值域集合,要求对应权重的马尔科夫模型状态转移矩阵A是互不相容的,要求定义的识别框架矩阵W满足:
定义信任区间[BEL(B),PL(B)],表示B的信任度区间,当北斗导航信号可观测时,其特殊含义如下:
[1,1]表示北斗导航直接观测信号稳定,信号值B为真;
[0,0]表示北斗导航直接观测信号丢失,其值B不可信;
[0,1]表示北斗导航直接观测信号存在部分遮挡,其值B不可完全取信;
定义上述窗口大小为5,获取的北斗导航信号观测结果为P1 B,P2 B,P3 B,P4 B,P5 B,若无法直接通过北斗导航信号观测结果判定,可定义其信任区间获取上述置信概率 其中i=1,2,3,4;可取此时,可确定北斗导航信号直接观测结果的转移矩阵A所对应加权矩阵W中各权重元素的信任区间,权重元素可取信任函数和似真度函数的均值。
本发明给出一种基于视觉误差补偿的地质灾害未知环境组合导航方法。在地质灾害环境下,工程机械车辆等运动救援平台的定位的准确性将直接影响救援平台的行驶和工作安全。卫星导航在地质灾害环境下,由于信号遮挡、多径,易引发卫星导航定位降效、失效,同时定位可信度较低,因此,亟需地质灾害环境下能够为救援平台提供高精度可信定位的技术手段。
本发明利用单目视觉获取救援场景的关键帧信息,实时解算姿态变换矩阵,为救援平台提供高精度相对定位结果,实现了卫星导航定位失效状态下的定位维持,其次,基于一阶马尔科夫模型利用视觉姿态变换为卫星导航进行实时误差补偿,最终给出高精度、可信的定位结果。
Claims (4)
1.一种基于视觉误差补偿的地质灾害未知环境组合导航方法,其特征在于,利用单目视觉求解救援平台相对运动姿态,融合相对定位信息进行卫星导航定位误差补偿,最终得出高精度、可信定位结果;具体包括如下步骤:
(1)基于连续序列图像特征跟踪获取候选关键帧,利用候选关键帧计算相机姿态变换矩阵;
(2)构建一阶马尔科夫模型,基于一阶马尔科夫模型进行视觉辅助北斗误差补偿;
(3)构建卫星导航信号观测信息的可信区间,基于信任函数对观测量权重取值。
2.根据权利要求1所述的一种基于视觉误差补偿的地质灾害未知环境组合导航方法,其特征在于,所述步骤(1)的具体方式如下:
(101)利用基于梯度的运动统计方法获取图像序列特征匹配,依次统计序列图像特征匹配跟踪数目,也就是视图中某一特征点在序列图像中连续匹配的数目,记录视图中最大的跟踪特征数目,选取连续序列中包含最多特征跟踪数目的视图作为候选的关键帧;
(102)相机的内参数为{fx,fy,s,cx,cy},其中,s表示相机的扭曲,fx,fy表示相机的焦距,cx,cy表示图像主点;相机的外参数为{R,t},其中,R为旋转矩阵,t为平移向量;
(103)选用针孔模型进行相机姿态变换求解,它由一个限定尺度的3×4投影矩阵表示;将3×4的投影矩阵P分解成一个3×3的上三角矩阵K和一个3×4的转移矩阵[R|t]的乘积:
其中,r11,r12,r13,r21,r22,r23,r31,r32,r33为旋转矩阵R的元素,tx,ty,tz为平移矩阵t的元素;
(104)假定X1=[x1,y1,1]T,X2=[x2,y2,1]T为两幅关键帧对应匹配点对的归一化坐标,定义描述图像姿态变换的基础矩阵基础矩阵F包含相机的内外参数信息,并对F各元素进行重组变换为列矢量形式,即:
f=[f11 f12 f13 f21 f22 f23 f31 f32 f33]T,
对于稳定存在的匹配点对(X1,X2),由对极几何约束X1 TFX2=0,有:
[x1x2,x1y2,x1,y1x2,y1y2,y1,x2,y2,1]·f=0
(105)选择8对匹配点,经代数变换构成线性方程组,满足:
Af=0
其中,
A为8×9的矩阵;
(106)根据矩阵的唯一性,所有解向量f参数存在比例因子;为了确定标准解,加入约束条件||f||=1,此条件满足f作为ATA的最小特征值所对应的特征向量;A的奇异值分解为A=UDVT,其中:
V=[v1,v2,v3,v4,v5,v6,v7,v8,v9],对应f=v9,从而求出基础矩阵F;
(107)定义本质矩阵E:
E=KTFK
利用奇异值分解求解第二幅图像外参数:
E=UDVT
其中U,V分别是3×3的正交阵,D为3×3的对角阵,其形式为:
对给定的一个本质矩阵E=Udiag(1,1,0)VT,由第一幅图像的相机投影矩阵P1=K[I,0]推导第二幅图像的投影矩阵P2,至此,获取关键帧间的相机姿态变换。
3.根据权利要求1所述的一种基于视觉误差补偿的地质灾害未知环境组合导航方法,其特征在于,所述步骤(2)的具体方式如下:
(201)通过北斗导航***给出连续定位观测量,获取运动平台的经度、纬度和高程,并转化为地图原点为起始点的平面地图坐标系下,定义连续时间t时刻北斗导航***给出的位置信息为Pt B,其中xt B表示t时刻北斗导航信号在平面地图坐标系给出的X轴坐标,yt B表示t时刻北斗导航信号在平面地图坐标系给出的Y轴坐标,表示t时刻北斗导航信号在平面地图坐标系给出的Z轴坐标;定义北斗导航观测量关键帧Ki B(i=1,2,3…),即要求与获取的图像关键帧Ki(i=1,2,3…)时间同步,对于图像关键帧中未获取有效姿态的关键帧则不用于后续校正处理;
(202)对于t1时刻与t2时刻对应进行姿态估计的两个相邻关键帧I1与I2,图像帧I1到图像帧I2的相机姿态变换矩阵为T12=[R12|t12],其中,R12为图像帧I1到图像帧I2的旋转矩阵,t12为图像帧I1到图像帧I2的平移矩阵;两个相邻关键帧I1与I2分别对应相应时刻北斗导航***给出的位置信息为P1 B与则有如下估计模型:
对于tn时刻北斗导航定位***给出的直接定位结果其置信概率如果Pc″<th″,th″为异常概率阈值,则定位结果视为异常值,予以滤除;依靠多步一阶马尔科夫估计模型,以北斗信号丢失前的有效定位结果作为初始值,给出信号缺失时间段的定位估计结果;完成滤除和插值后,对北斗定位结果进行视觉辅助校正,定义限定窗口大小为5的马尔科夫模型状态转移矩阵A和加权矩阵W,给出最终定位修正结果P公式:
P=WA
W=(w1,w2,w3,w4,w5)
其中,窗口大小为5,即定位离散时间序列T5={t1,t2,t3,t4,t5},P1 B,P2 B,P3 B,P4 B,P5 B为对应时刻北斗导航定位直接观测结果,T12,T23,T34,T45为对应时刻关键帧间相机姿态变换矩阵。
4.根据权利要求1所述的一种基于视觉误差补偿的地质灾害未知环境组合导航方法,其特征在于,所述步骤(3)的具体方式如下:
(301)定义包含n个元素的权重矩阵W为一识别框架,定义权重wi为可能取值的值域集合,要求对应权重的马尔科夫模型状态转移矩阵A是互不相容的,要求定义的识别框架矩阵W满足:
(304)定义信任区间[BEL(B),PL(B)],表示B的信任度区间,当北斗导航信号可观测时,其特殊含义如下:
[1,1]表示北斗导航直接观测信号稳定,信号值B为真;
[0,0]表示北斗导航直接观测信号丢失,其值B不可信;
[0,1]表示北斗导航直接观测信号存在部分遮挡,其值B不可完全取信;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010933698.8A CN112068168B (zh) | 2020-09-08 | 2020-09-08 | 一种基于视觉误差补偿的地质灾害未知环境组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010933698.8A CN112068168B (zh) | 2020-09-08 | 2020-09-08 | 一种基于视觉误差补偿的地质灾害未知环境组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112068168A true CN112068168A (zh) | 2020-12-11 |
CN112068168B CN112068168B (zh) | 2024-03-15 |
Family
ID=73664195
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010933698.8A Active CN112068168B (zh) | 2020-09-08 | 2020-09-08 | 一种基于视觉误差补偿的地质灾害未知环境组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112068168B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114659526A (zh) * | 2022-02-11 | 2022-06-24 | 北京空间飞行器总体设计部 | 基于序列图像状态表达的航天器自主导航鲁棒滤波算法 |
CN115128655A (zh) * | 2022-08-31 | 2022-09-30 | 智道网联科技(北京)有限公司 | 自动驾驶车辆的定位方法和装置、电子设备和存储介质 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080167814A1 (en) * | 2006-12-01 | 2008-07-10 | Supun Samarasekera | Unified framework for precise vision-aided navigation |
US20120116676A1 (en) * | 2010-11-10 | 2012-05-10 | Gm Global Technology Operations, Inc. | Method of Augmenting GPS or GPS/Sensor Vehicle Positioning Using Additional In-Vehicle Vision Sensors |
CN104729506A (zh) * | 2015-03-27 | 2015-06-24 | 北京航空航天大学 | 一种视觉信息辅助的无人机自主导航定位方法 |
US20150219767A1 (en) * | 2014-02-03 | 2015-08-06 | Board Of Regents, The University Of Texas System | System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates |
CN106780699A (zh) * | 2017-01-09 | 2017-05-31 | 东南大学 | 一种基于sins/gps和里程计辅助的视觉slam方法 |
FR3046006A1 (fr) * | 2015-12-18 | 2017-06-23 | Inst Mines-Telecom | Procede d'estimation de trajectoires utilisant des donnees mobiles |
CN107229063A (zh) * | 2017-06-26 | 2017-10-03 | 奇瑞汽车股份有限公司 | 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法 |
CN107833249A (zh) * | 2017-09-29 | 2018-03-23 | 南京航空航天大学 | 一种基于视觉引导的舰载机着陆过程姿态预估方法 |
CN111324126A (zh) * | 2020-03-12 | 2020-06-23 | 集美大学 | 一种视觉无人船及其视觉导航方法 |
-
2020
- 2020-09-08 CN CN202010933698.8A patent/CN112068168B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080167814A1 (en) * | 2006-12-01 | 2008-07-10 | Supun Samarasekera | Unified framework for precise vision-aided navigation |
US20120116676A1 (en) * | 2010-11-10 | 2012-05-10 | Gm Global Technology Operations, Inc. | Method of Augmenting GPS or GPS/Sensor Vehicle Positioning Using Additional In-Vehicle Vision Sensors |
US20150219767A1 (en) * | 2014-02-03 | 2015-08-06 | Board Of Regents, The University Of Texas System | System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates |
CN104729506A (zh) * | 2015-03-27 | 2015-06-24 | 北京航空航天大学 | 一种视觉信息辅助的无人机自主导航定位方法 |
FR3046006A1 (fr) * | 2015-12-18 | 2017-06-23 | Inst Mines-Telecom | Procede d'estimation de trajectoires utilisant des donnees mobiles |
CN106780699A (zh) * | 2017-01-09 | 2017-05-31 | 东南大学 | 一种基于sins/gps和里程计辅助的视觉slam方法 |
CN107229063A (zh) * | 2017-06-26 | 2017-10-03 | 奇瑞汽车股份有限公司 | 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法 |
CN107833249A (zh) * | 2017-09-29 | 2018-03-23 | 南京航空航天大学 | 一种基于视觉引导的舰载机着陆过程姿态预估方法 |
CN111324126A (zh) * | 2020-03-12 | 2020-06-23 | 集美大学 | 一种视觉无人船及其视觉导航方法 |
Non-Patent Citations (1)
Title |
---|
周阳林: "视觉传感器辅助的高精度组合导航定位技术研究", 中国博士学位论文全文数据库 基础科学辑, pages 008 - 39 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114659526A (zh) * | 2022-02-11 | 2022-06-24 | 北京空间飞行器总体设计部 | 基于序列图像状态表达的航天器自主导航鲁棒滤波算法 |
CN115128655A (zh) * | 2022-08-31 | 2022-09-30 | 智道网联科技(北京)有限公司 | 自动驾驶车辆的定位方法和装置、电子设备和存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112068168B (zh) | 2024-03-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
Sim et al. | Integrated position estimation using aerial image sequences | |
CN102472609B (zh) | 位置和姿势校准方法及设备 | |
CN111795686B (zh) | 一种移动机器人定位与建图的方法 | |
Sola et al. | Fusing monocular information in multicamera SLAM | |
CN113538410A (zh) | 一种基于3d激光雷达和uwb的室内slam建图方法 | |
CN105352509A (zh) | 地理信息时空约束下的无人机运动目标跟踪与定位方法 | |
CN113252033B (zh) | 基于多传感器融合的定位方法、定位***及机器人 | |
CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
CN112284384A (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN104281148A (zh) | 基于双目立体视觉的移动机器人自主导航方法 | |
CN112068168A (zh) | 一种基于视觉误差补偿的地质灾害未知环境组合导航方法 | |
CN111932674A (zh) | 一种线激光视觉惯性***的优化方法 | |
CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN112461228B (zh) | 一种相似环境下基于imu和视觉的二次回环检测定位方法 | |
US20150168562A1 (en) | Systems and Methods for Estimating Attitude Using Double Differenced GPS Carrier Phase Measurements | |
CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
CN115930977A (zh) | 特征退化场景的定位方法、***、电子设备和可读存介质 | |
CN111145267A (zh) | 基于imu辅助的360度全景视图多相机标定方法 | |
CN116380079A (zh) | 一种融合前视声呐与orb-slam3的水下slam方法 | |
CN113850864B (zh) | 一种面向室外移动机器人的gnss/lidar回环检测方法 | |
CN115830116A (zh) | 一种鲁棒视觉里程计方法 | |
CN115457022A (zh) | 基于实景三维模型正视影像的三维形变检测方法 | |
CN114705223A (zh) | 多移动智能体在目标跟踪中的惯导误差补偿方法及*** | |
CN114025320A (zh) | 一种基于5g信号的室内定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |