CN111123304B - 一种视觉导航完好性监测计算方法 - Google Patents

一种视觉导航完好性监测计算方法 Download PDF

Info

Publication number
CN111123304B
CN111123304B CN201911189662.7A CN201911189662A CN111123304B CN 111123304 B CN111123304 B CN 111123304B CN 201911189662 A CN201911189662 A CN 201911189662A CN 111123304 B CN111123304 B CN 111123304B
Authority
CN
China
Prior art keywords
vector
visual
observation
fault
positioning
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.)
Active
Application number
CN201911189662.7A
Other languages
English (en)
Other versions
CN111123304A (zh
Inventor
王志鹏
朱衍波
杜聪
方堃
李强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN201911189662.7A priority Critical patent/CN111123304B/zh
Priority to US16/713,288 priority patent/US10782418B1/en
Publication of CN111123304A publication Critical patent/CN111123304A/zh
Application granted granted Critical
Publication of CN111123304B publication Critical patent/CN111123304B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
    • G08G5/0017—Arrangements for implementing traffic-related aircraft activities, e.g. arrangements for generating, displaying, acquiring or managing traffic information
    • G08G5/0021—Arrangements for implementing traffic-related aircraft activities, e.g. arrangements for generating, displaying, acquiring or managing traffic information located in the aircraft
    • 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
    • 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/13—Receivers
    • G01S19/23—Testing, monitoring, correcting or calibrating of receiver elements
    • 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/13—Receivers
    • G01S19/20—Integrity monitoring, fault detection or fault isolation of space segment
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
    • G08G5/0047—Navigation or guidance aids for a single aircraft
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
    • G08G5/0073—Surveillance aids
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
    • G08G5/0073—Surveillance aids
    • G08G5/0086—Surveillance aids for monitoring terrain
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
    • G08G5/02—Automatic approach or landing aids, i.e. systems in which flight data of incoming planes are processed to provide landing data
    • G08G5/025—Navigation or guidance aids

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Security & Cryptography (AREA)
  • Electromagnetism (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种视觉导航完好性监测计算方法，该方法通过运用合适的视觉定位模型、数学算法和丰富的导航测量量，提高了定位的准确性和定位结果的可用性，同时解决了在复杂环境中无法保证GNSS可用性，导致卫星完好性算法性能不足的问题，有助于实现飞机精确进近和自动着陆，对保证航空飞行的安全需求有重大意义。

Description

一种视觉导航完好性监测计算方法
技术领域
本发明涉及卫星导航技术领域，具体涉及一种视觉导航完好性监测计算方法。
背景技术
随着卫星导航技术的发展和广泛应用，人们越来越认识到全球卫星导航系统在完好性方面的重要性，有调查报告显示，在航空应用中，大多数致命事故发生在最后的进近和着陆阶段，所以虽然飞行的进近和着陆阶段是最短的，但也是保障生命安全最关键的部分。
全球定位系统接收机自主完好性监测是通过检查卫星定位观测的一致性来提供导航系统的完好性，然而，在飞行路径的进近和着陆阶段，由于卫星定位观测不充分，现有接收机自主完好性监测方法的性能通常不能满足严格的航空应用对完好性和可用性的要求。因此，开发视觉导航设备辅助全球卫星定位系统已经成为航空卫星导航研究领域的一个热点。
目前，一些学校和研究所比如北京航空航天大学、西安导航技术研究所等对卫星导航的完好性进行了理论和技术的研究，由GPS到差分GPS，再到其他导航系统，如惯性导航系统和组合导航系统等，对卫星导航完好性的研究越来越多。同时，卫星定位完好性监测方法已经较为成熟，但在所有环境或运行条件下却无法保证卫星定位信号的永久可用性，导致卫星完好性算法性能不足。因此，本发明提出了一种视觉辅助导航系统的完好性监测研究方法。
发明内容
本发明为了解决现有技术中复杂环境或运行条件下无法保证卫星定位的永久可用性，导致卫星完好性算法性能不足的问题，提供了一种视觉导航完好性监测计算方法，包括以下步骤：
(1)根据特定的视觉定位场景，建立合适的视觉定位模型，计算视觉定位解；
(2)修正视觉系统参数的误差，计算误差状态向量；
(3)建立视觉观测模型，计算视觉系统观测矩阵H；
(4)引入视觉故障偏差，分析故障偏差矢量b对视觉观测量带来的变化；
(5)组合视觉系统观测方程与卫星定位方程，计算组合导航定位方程；
(6)计算不同故障模式下的保护级大小。
进一步地，步骤(1)所述视觉定位模型在特定的视觉定位场景中可分为两类：第一类是以人工设置的地面标志作为着陆参考，第二类是以机场跑道作为着陆参考；
所述以人工设置的地面标志为设置“H”或者“工”型标志；
所述以机场跑道作为着陆参考为在机场环境附近下，借用机场跑道轮廓线，使摄像机能够可靠地识别机场跑道目标。
进一步地，在导航坐标系下，构建以下视觉定位方程：
Figure GDA0003183520990000021
当得知地标在摄像机下的视线矢量
Figure GDA0003183520990000022
并且已知摄像机在机体坐标系下的位置qb＝[x1 y2 z3]T和着陆地标的位置矢量
Figure GDA0003183520990000023
就可以解算飞机在三维世界中的位置pn＝[x3 y3 z3]T，实现对飞机的定位；
其中，
Figure GDA0003183520990000024
表示在导航坐标系下第m个地标点的位置矢量，
Figure GDA0003183520990000025
表示在导航坐标系下第m个目标的视线矢量，pn表示在导航坐标系下的飞机位置矢量，qn表示摄像机光心到机体质心的距离，又称为摄像机与飞机体的最小角度偏移矢量，qb表示摄像机在机体坐标系下的位置，可由摄像机标定得到。
进一步地，所述修正视觉系统参数的误差，计算误差状态向量的具体步骤如下：
误差状态向量基于先前描述的数学参数存在有误差，可以表示为：
Figure GDA0003183520990000026
其中，δpn为飞机位置矢量的三维误差向量：
Figure GDA0003183520990000027
为地标的位置误差：
Figure GDA0003183520990000028
用波浪号～表示标称值；
当着陆地标进入和离开摄像机的视线场范围时，地标数量实际上随时间变化，但是，在任何给定时间，跟踪的地标数量必须有合理的最大值，以保持计算效率。
进一步地，所述计算视觉系统的观测矩阵H的具体步骤如下：
第m个地标的视觉观测方程表示为：
Figure GDA0003183520990000029
其中，矢量zm(ti)为着陆地标点m的像素位置，
Figure GDA00031835209900000210
为像素坐标系与摄像机坐标系的转换矩阵，
Figure GDA00031835209900000211
表示像素位置对应于从相机焦点向外延伸的视线矢量，
Figure GDA00031835209900000212
为相机在sz方向上与光学中心的距离，
Figure GDA00031835209900000213
摄像机到目标m的视线矢量的归一化形式，v(ti)是均值为零，协方差为R的独立加性高斯白噪声，满足以下关系：
Figure GDA0003183520990000031
假设一个高为H，宽为W的矩形(M*N)像素网格，像素坐标系与摄像机坐标的转化关系为
Figure GDA0003183520990000032
Figure GDA0003183520990000033
为已知，
Figure GDA0003183520990000034
与
Figure GDA0003183520990000035
的关系如下：
Figure GDA0003183520990000036
对非线性方程
Figure GDA0003183520990000037
线性化，令非线性函数为
Figure GDA0003183520990000038
Figure GDA0003183520990000039
计算一阶泰勒展开式，可以得到视觉系统观测矩阵H，其中，视觉系统观测矩阵H中的元素由下列方程求得的偏导数给出如下表达式：
Figure GDA00031835209900000310
又因为：
Figure GDA00031835209900000311
所以h对pn求偏导数如下：
Figure GDA00031835209900000312
又因为视觉系统观测矩阵H与
Figure GDA00031835209900000313
无关，所以H对
Figure GDA00031835209900000315
求偏导数的值为零。
进一步地，所述故障偏差矢量b对视觉观测量矢量z带来的变化的分析过程如下：
状态向量x的线性化视觉观测模型表达式如下：
z＝Hx+v+b
其中，z是视觉观测量矢量，状态向量x是n维的列向量；v是m维观测噪音矢量，服从零均值的高斯噪声，其协方差矩阵为对角矩阵R，表示为R＝σ2Im*m；b是m维故障偏置矢量，在无故障条件下，b为零向量，但在第i个测量量中发生单个故障时的偏差，b中第i个元素依旧表示为b
当视觉观测量矢量发生变化时，视觉观测误差δz表示为：δz＝Hδx
所以得到z＝Hx+v+b的最小二乘解为：
δx＝(HTH)-1HTδz
若不考虑噪声矢量，由状态向量x的线性化观测模型可得到：
Figure GDA00031835209900000314
其中，
Figure GDA0003183520990000041
又因为故障偏差矢量b对像素点(x，y)的每一个分量x和y都有影响，所以将故障偏差矢量b用极坐标表示为：bi＝||b||cosθ，bj＝||b||sinθ。那么bi＝||b||cosθ就是故障偏差矢量b对分量x的影响，bj＝||b||sinθ就是故障偏差矢量b对分量y的影响，所以位置误差的范数为：
Figure GDA0003183520990000042
又因为：
Figure GDA0003183520990000043
代入极坐标公式，可得到故障偏差矢量b对水平位置误差的影响公式为：
Figure GDA0003183520990000044
因此，可以看出当角度值θ变化时，水平位置误差也会发生相应的变化，特别是当θ取一些特殊角度值时，可以简化水平位置误差δx的公式。
进一步地，计算组合导航定位方程的过程为：
假设所述卫星定位方程为z＝Gx，将视觉系统观测矩阵H与卫星观测矩阵G进行组合，得到新的组合扩展观察矩阵H′，表达式如下：
Figure GDA0003183520990000045
H′是m*n线性观察矩阵
当有限数量的卫星仍可用时，可以利用所述有限数量的卫星观测量增强视觉导航设备；
如果，所述视觉系统是由两个摄像机组成的双目视觉系统或是三个摄像机组成的系统，则可将所述视觉系统中的单个摄像机传感的转化矩阵M1，M2，Mi组合成M，则M可以表示为：
Figure GDA0003183520990000046
进一步地，所述保护级的计算方法分为三种假设：假设1、假设2和假设3；
所述假设1为观测量不存在故障的情况；所述假设2为观测量发生单个故障的情况；所述假设3为观测量发生双故障的情况。
进一步地，在所述假设1情况下，保护级可以通过无故障捡漏因子Kff和水平位置的标准差σx来计算的，其中，无故障检漏因子Kff由无故障概率Pff确定，因此，无故障保护级的计算公式表示为：
HPL1＝Kff(k)*σx
Kff：无故障检漏因子；σx：飞机水平方向的标准差；
在所述假设2情况下，可能的现象有单个视觉观测量发生故障或者单个卫星定位观测量发生故障；
当只有单个观测量发生故障，在故障偏差矢量b中只含有一个偏差成分，这时的水平位置误差δx可以如下表示：
Figure GDA0003183520990000051
其中，
Figure GDA0003183520990000052
表示最小二乘估计值，
Figure GDA0003183520990000053
设每个观测量的斜率为Hslope，表达式如下：
Figure GDA0003183520990000054
其中，
Figure GDA0003183520990000055
其中，D为测试统计量，p表示奇偶矢量，P表示奇偶变换矩阵，p＝Pb；因此，水平定位误差保护级HPL2为：
HPL2＝Hslopemax×sbias
其中，
Figure GDA0003183520990000056
σ为噪声方差，λ为非中心化参数，当无故障时，HPL2的值最小；当观测量有单个故障时，就会导致HPL2的值增大；
在所述假设3情况下，可能的现象有两个视觉观测量发生故障、两个卫星定位观测量故障或者一个视觉观测量和一个卫星定位观测量故障。
当两个观测量发生故障，第i个观测量和第j个观测量的同时发生故障的故障偏差矢量为b(i)和b(j)，可以表示为：
b＝[0...0 b(i) b(j)0...0 0...0]T
因此，定位误差表达式如下：
Figure GDA0003183520990000057
水平定位误差表达式如下：
δx2＝(b(i)A1i+b(j)A1j)2+(b(i)A2i+b(j)A2j)2
因此，计算斜率Hslope的公式为：
Figure GDA0003183520990000058
因此，HPL3的计算公式为：
HPL3＝Hslopemax×sbias
综合所述假设1、假设2和假设3，将所有情况下的误差包络在保护级中，得到总的保护级计算公式如下：
Figure GDA0003183520990000061
进一步地，所述第m个地标的视觉测量方程：
Figure GDA0003183520990000062
又称为伪距定位方程。
与现有技术相比，本发明的有益效果是：
本发明提供了一种视觉导航完好性监测计算方法，该方法针对不同的实际情况提出了合适的视觉定位测量模型和数学算法，丰富了导航观测量，提高了定位的准确度和完好性监测性能，提高了定位结果的可用性，解决了现有技术中在复杂环境或情况下无法保证卫星定位可用性，导致卫星完好性算法性能不足的缺点。
附图说明
为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中
图1为本实施例中视觉完好性监测流程图；
图2为本实施例中人工目标的定位模型；
图3为本实施例中机场跑道的定位模型；
图4为本实施例中保护级计算流程图。
具体实施方式
通过参考示范性实施例，本发明的目的和功能以及用于实现这些目的和功能的方法将得以阐明。然而，本发明并不受限于以下所公开的示范性实施例；可以通过不同形式来对其加以实现。说明书的实质仅仅是帮助相关领域技术人员综合理解本发明的具体细节。
在下文中，将参考附图描述本发明的实施例。在附图中，相同的附图标记代表相同或类似的部件，或者相同或类似的步骤。下面通过具体的实施例对本发明的一种视觉导航完好性监测计算方法进行说明：
本方法通过引入视觉导航着陆系统的视觉观测量，以提高导航完好性监测性能，如图1所示，为本实施例中视觉完好性监测流程图，首先，根据摄像机拍摄的图像，对图像进行预处理，检测和保留带有着陆地标的图像，将其用于视觉系统的定位；其次，为确保定位精度，同时减少系统参数带来的误差，需要对误差进行修正估计；然后，通过构造视觉定位方程，得到视觉观测模型，计算观测矩阵，用于进一步计算飞机的位置姿态信息；最后，进行故障检测和排除，当检验统计量小于阈值时，系统没有发生故障，可直接输出定位结果，否则，当检验统计量大于等于阈值时，系统发生故障，可对发生的故障进行排除，然后再分别计算不同故障发生情况下的保护级，进一步对系统的可用性做出判断。
该方法的具体步骤如下：
(1)根据特定的视觉定位场景，建立合适的定位模型，计算视觉定位解；
通过对飞机视觉着陆场景的研究，视觉定位模型可归纳为两种情景：一时采用特征明显的人工目标作为着陆标志，例如“H”或者“工”型标志，如图2所示；二是在机场环境附近，借用机场跑道轮廓线，使摄像机能够可靠地识别机场跑道目标，如图3所示；
如图2和图3所示，在导航坐标系下，根据矢量的相加运算，将飞机位置矢量pn、摄像机与飞机体的小角度偏移矢量qn和视线矢量
Figure GDA0003183520990000071
相加，得到着陆地标矢量
Figure GDA0003183520990000072
以下等式成立：
Figure GDA0003183520990000073
所以，当得知地标在摄像机下的视线矢量
Figure GDA0003183520990000074
并且已知摄像机在机体坐标系下的位置qb＝[x1 y2 z3]T和着陆地标的位置矢量
Figure GDA0003183520990000075
就可以解算飞机在三维世界中的位置pn＝[x3 y3 z3]T，实现对飞机的定位。
其中，pn表示在导航坐标系下的飞机位置矢量；
Figure GDA0003183520990000076
表示在导航坐标系下第m个地标点的位置矢量；
Figure GDA0003183520990000077
表示机体坐标系转换到导航坐标系的方向余弦矩阵；
Figure GDA0003183520990000078
表示在导航坐标系下第m个目标的视线矢量；
Figure GDA0003183520990000079
表示在机体坐标系下第m个目标的视线矢量，
Figure GDA00031835209900000710
qn表示摄像机光心到机体质心的距离；
qb表示摄像机在机体坐标系下的位置，可有摄像机的标定得到。
(2)修正视觉系统参数的误差，计算误差状态向量；
因为系统参数容易存在误差，所以在计算视觉观测量时，为了减少线性化误差带来的影响，对视觉参数进行误差估计修正是必不可少的；
误差状态向量基于先前描述的数学参数存在有误差，可以表示为：
Figure GDA00031835209900000711
其中，δpn为飞机位置矢量的三维误差向量，可以表示为：
Figure GDA0003183520990000081
Figure GDA0003183520990000082
为地标的位置误差，可以表示为：
Figure GDA0003183520990000083
其中，用波浪号“～”表示标称值。
要注意的是，当着陆地标进入和离开摄像机的视线场范围时，地标数量实际上随时间变化，但是，在任何给定时间，跟踪的地标数量必须有合理的最大值，以保持计算效率。
(3)建立视觉测量模型，计算视觉系统的观测矩阵H；
如图2所示，第m个地标的视觉测量方程表示为以下公式：
Figure GDA0003183520990000084
该公式又称为伪距定位方程，是根据着陆地标点以及其对应图像中的像素点得到的；
其中，矢量zm(ti)为着陆地标点m的像素位置；
Figure GDA0003183520990000085
为像素坐标系与摄像机坐标系的转换矩阵
Figure GDA0003183520990000086
表示像素位置对应于从相机焦点向外延伸的视线矢量；
Figure GDA0003183520990000087
为相机在sZ方向上与光学中心的距离；
Figure GDA0003183520990000088
摄像机到目标m的视线矢量的归一化形式；
v(ti)是均值为零，协方差为R的独立加性高斯白噪声，满足以下方程：
Figure GDA0003183520990000089
Figure GDA00031835209900000810
是已知的，假设一个高为H，宽为W的矩形(M*N)像素网格，像素坐标系与摄像机坐标的转化关系为
Figure GDA00031835209900000811
满足以下公式：
Figure GDA00031835209900000812
为了计算方便，将非线性方程
Figure GDA00031835209900000813
线性化，令非线性函数
Figure GDA00031835209900000814
计算一阶Taylor级数展开，可以得到测量模型观测矩阵H，其中，H中的元素由下列方程求得的偏导数给出：
Figure GDA00031835209900000815
又因为：
Figure GDA0003183520990000091
因此，h对pn求偏导数为：
Figure GDA0003183520990000092
因为H与
Figure GDA0003183520990000093
无关，所以H对
Figure GDA0003183520990000094
求偏导数的值为零。
(4)引入视觉故障偏差分析故障偏差矢量b对视觉观测量带来的变化；
状态向量x的线性化观测模型表达式为以下公式：
z＝Hx+v+b
其中，z是视觉观测量矢量，状态向量x是n维的列向量；v是m维观测噪音矢量，服从零均值的高斯噪声，其协方差矩阵为对角矩阵R，表示为R＝σ2Im*m；b是m维故障偏置矢量，在无故障条件下，b为零向量，但在第i个测量量中发生单个故障时的偏差，b中第i个元素依旧表示为b
当视觉测量量变化时，图像测量误差表示为：
δz＝Hδx
所以得到z＝Hx+v+b的最小二乘解为：
δx＝(HTH)-1HTδz
若不考虑噪声矢量，由状态向量x的线性化观测模型可得到：
Figure GDA0003183520990000095
其中，
Figure GDA0003183520990000096
又因为故障偏差矢量b对像素点(x，y)的每一个分量x和y都有影响，所以将故障偏差矢量b用极坐标表示为：
bi＝|||b||cosθ
bj＝||b||sinθ
所以，bi＝||b||cosθ就是故障偏差矢量b对分量x的影响；
bj＝||b||sinθ就是故障偏差矢量b对分量y的影响。
所以，位置误差的范数由以下公式表示：
Figure GDA0003183520990000097
又根据以下公式：
Figure GDA0003183520990000098
向上式中带入极坐标公式，可得故障偏差矢量b对水平位置误差的影响，具体公式如下：
Figure GDA0003183520990000101
所以，当角度值θ变化时，水平位置误差也会发生相应的变化，特别是θ取一些特殊角度值时，水平位置误差δx的公式会变得更简洁。
(5)组合视觉系统测量方程与卫星定位方程，计算组合导航定位方程；
假设所述卫星定位方程为z＝Gx，将视觉系统观测矩阵H与卫星观测矩阵G进行组合，得到新的组合扩展观察矩阵H′，表达式如下：
Figure GDA0003183520990000102
H′是m*n线性观察矩阵；
当有限数量卫星，例如两个或者三个卫星仍然可用时，可以利用这些有限卫星测量量来增强视觉导航设备；
另外，如果视觉系统是两个摄像机组成的双目视觉系统或者三个摄像机组成的系统，则可将单个摄像机传感器的转换矩阵M1、M2、Mi组合成M，具体M可以表达如以下等式：
Figure GDA0003183520990000103
(6)计算不同故障模式下的保护级大小；
保护级计算方法分为三种假设：假设1、假设2和假设3。
假设1：观测量不存在故障的情况，保护级可以通过无故障增益Kff和水平位置的标准差来计算的，其中，无故障检漏因子Kff由无故障概率Pff确定，因此，无故障保护级的计算公式如下：
HPL1＝Kff(k)*σx
HPL1表示无故障保护级；Kff：无故障检漏因子；σx：飞机水平方向的标准差。
假设2：观测量发生单个故障的情况，可能的现象有单个视觉观测量发生故障或者单个GNSS观测故障。
当只有单个观测量发生故障，在故障偏差矢量b中只含有一个偏差成分，这时的水平位置误差δx可以如下表示：
Figure GDA0003183520990000104
其中，
Figure GDA0003183520990000105
表示最小二乘估计值，
Figure GDA0003183520990000106
设每个观测量的斜率为Hslope，表达式如下：
Figure GDA0003183520990000111
其中，
Figure GDA0003183520990000112
其中，D为测试统计量，p表示奇偶矢量，P表示奇偶变换矩阵，p＝Pb；因此，水平定位误差保护级HPL2为：
HPL2＝Hslopemax×sbias
其中，
Figure GDA0003183520990000113
σ为噪声方差，λ为非中心化参数，当无故障时，HPL2的值最小；当观测量有单个故障时，就会导致HPL2的值增大；
假设3：观测量发生双故障的情况，可能的现象有两个视觉观测量发生故障、两个GNSS观测量故障或者一个视觉观测量和一个GNSS观测量故障。
当两个观测量发生故障，第i个观测量和第j个观测量的同时发生故障的故障偏差矢量为b(i)和b(j)，可以表示为：
b＝[0...0 b(i) b(j)0...0 0...0]T
因此，定位误差表达式如下：
Figure GDA0003183520990000114
水平定位误差表达式如下：
δx2＝(b(i)A1i+b(j)A1j)2+(b(i)A2i+b(j)A2j)2
因此，计算斜率Hslope的公式为：
Figure GDA0003183520990000115
因此，HPL3的计算公式为：
HPL3＝Hslopemax×sbias
综合所述假设1、假设2和假设3，将所有情况下的误差包络在保护级中，得到总的保护级计算公式如下：
Figure GDA0003183520990000116
考虑到所有情况下，保护级的值越精确，在给定风险值的情况下越能够保护用户抵抗所有偏差，系统可用性就越高。
本实施例表示通过提出的视觉导航完好性监测算法，能有效地排除故障，提高导航定位的准确性和可用性。
显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (8)

1.一种视觉导航完好性监测计算方法，其特征在于，包括以下步骤：
(1)根据特定的视觉定位场景，建立合适的视觉定位模型，计算视觉定位解；
(2)修正视觉系统参数的误差，计算误差状态向量；
(3)建立视觉观测模型，计算视觉系统观测矩阵H；
所述计算视觉系统的观测矩阵H的具体步骤如下：
第m个地标的视觉观测方程表示为：
Figure FDA0003258487540000011
其中，矢量zm(ta)为着陆地标点m的像素位置，
Figure FDA0003258487540000012
为像素坐标系与摄像机坐标系的转换矩阵，
Figure FDA0003258487540000013
表示像素位置对应于从相机焦点向外延伸的视线矢量，
Figure FDA0003258487540000014
为相机在sz方向上与光学中心的距离，
Figure FDA0003258487540000015
摄像机到目标m的视线矢量的归一化形式，v(ta)是均值为零，协方差为R的独立加性高斯白噪声，满足以下关系：
Figure FDA0003258487540000016
假设一个高为E，宽为W的矩形(M*N)像素网格，像素坐标系与摄像机坐标的转化关系为
Figure FDA0003258487540000017
为已知，
Figure FDA0003258487540000018
与
Figure FDA0003258487540000019
的关系如下：
Figure FDA00032584875400000110
对非线性方程
Figure FDA00032584875400000111
线性化，令非线性函数为
Figure FDA00032584875400000112
计算一阶泰勒展开式，可以得到视觉系统观测矩阵H，其中，视觉系统观测矩阵H中的元素由下列方程求得的偏导数给出如下表达式：
Figure FDA00032584875400000113
又因为：
Figure FDA00032584875400000114
其中，pn表示导航坐标系下的飞机位置矢量，
Figure FDA00032584875400000115
表示在导航坐标下第m个地标点的位置矢量，
Figure FDA00032584875400000116
表示在导航坐标系第m个目标的视线矢量，qn表示摄像机光心到机体质心的距离，
Figure FDA00032584875400000117
表示机体坐标系转换到导航坐标系的方向余弦矩阵，qb表示相机在机体坐标系下的位置；
所以h对pn求偏导数如下：
Figure FDA0003258487540000021
又因为视觉系统观测矩阵H与
Figure FDA0003258487540000022
无关，所以H对
Figure FDA0003258487540000023
求偏导数的值为零
(4)引入视觉故障偏差，分析故障偏差矢量b对视觉观测量带来的变化；
(5)组合视觉系统观测方程与卫星定位方程，计算组合导航定位方程；
(6)计算不同故障模式下的保护级大小。
2.根据权利要求1所述的一种视觉导航完好性监测计算方法，其特征在于，步骤(1)所述视觉定位模型在特定的视觉定位场景中可分为两类：第一类是以人工设置的地面标志作为着陆参考，第二类是以机场跑道作为着陆参考；
所述以人工设置的地面标志为设置“H”或者“工”型标志；
所述以机场跑道作为着陆参考为在机场环境附近下，借用机场跑道轮廓线，使摄像机能够可靠地识别机场跑道目标。
3.根据权利要求1所述的一种视觉导航完好性监测计算方法，其特征在于，所述计算视觉定位解的具体步骤如下：
在导航坐标系下，构建以下视觉定位方程：
Figure FDA0003258487540000024
当得知地标在摄像机下的视线矢量
Figure FDA0003258487540000025
并且已知摄像机在机体坐标系下的位置qb＝[x1y2 z3]T和着陆地标的位置矢量
Figure FDA0003258487540000026
就可以解算飞机在三维世界中的位置pn＝[x3 y3 z3]T，实现对飞机的定位；
其中，
Figure FDA0003258487540000027
表示在导航坐标系下第m个地标点的位置矢量，
Figure FDA0003258487540000028
表示在导航坐标系下第m个目标的视线矢量，pn表示在导航坐标系下的飞机位置矢量，qn表示摄像机光心到机体质心的距离，又称为摄像机与飞机体的最小角度偏移矢量，qb表示摄像机在机体坐标系下的位置，可由摄像机标定得到。
4.根据权利要求1所述的一种视觉导航完好性监测计算方法，其特征在于，所述修正视觉系统参数的误差，计算误差状态向量的具体步骤如下：
误差状态向量基于先前描述的数学参数存在有误差，可以表示为：
Figure FDA0003258487540000029
其中，δpn为飞机位置矢量的三维误差向量：
Figure FDA00032584875400000210
pn表示在导航坐标系下的飞机位置矢量；
Figure FDA00032584875400000211
为地标的位置误差：
Figure FDA00032584875400000212
Figure FDA00032584875400000213
表示在导航坐标系下第m个地标点的位置矢量，用波浪号～表示标称值；
当着陆地标进入和离开摄像机的视线场范围时，地标数量实际上随时间变化，但是，在任何给定时间，跟踪的地标数量必须有合理的最大值，以保持计算效率。
5.根据权利要求1所述的一种视觉导航完好性监测计算方法，其特征在于，所述故障偏差矢量b对视觉观测量矢量Z带来的变化的分析过程如下：
状态向量x的线性化视觉观测模型表达式如下：
z＝Hx+v+b
其中，z是视觉观测量矢量，状态向量x是n维的列向量；v是m维观测噪音矢量，服从零均值的高斯噪声，其协方差矩阵为对角矩阵R，表示为R＝σ2Im*m；b是m维故障偏置矢量，在无故障条件下，b为零向量，但在第i个测量量中发生单个故障时的偏差，b中第i个元素依旧表示为b
当视觉观测量矢量发生变化时，视觉观测误差δz表示为：δz＝Hδx
所以得到z＝Hx+v+b的最小二乘解为：
δx＝(HTH)-1HTδz
若不考虑噪声矢量，由状态向量x的线性化观测模型可得到：
Figure FDA0003258487540000031
其中，
Figure FDA0003258487540000032
又因为故障偏差矢量b对像素点(x，y)的每一个分量x和y都有影响，所以将故障偏差矢量b用极坐标表示为：bi＝||b||cosθ，bj＝||b||sinθ；那么bi＝||b||cosθ就是故障偏差矢量b对分量x的影响，bj＝||b||sinθ就是故障偏差矢量b对分量y的影响，所以位置误差的范数为：
Figure FDA0003258487540000033
又因为：
Figure FDA0003258487540000034
代入极坐标公式，可得到故障偏差矢量b对水平位置误差的影响公式为：
Figure FDA0003258487540000035
因此，可以看出当角度值θ变化时，水平位置误差也会发生相应的变化，特别是当θ取一些特殊角度值时，可以简化水平位置误差δx的公式。
6.根据权利要求1所述一种视觉导航完好性监测计算方法，其特征在于，计算组合导航定位方程的过程为：
假设所述卫星定位方程为z＝Gx，将视觉系统观测矩阵H与卫星观测矩阵G进行组合，得到新的组合扩展观察矩阵H′，表达式如下：
Figure FDA0003258487540000041
H′是m*n线性观察矩阵
当有限数量的卫星仍可用时，可以利用所述有限数量的卫星观测量增强视觉导航设备；
如果，所述视觉系统是由两个摄像机组成的双目视觉系统或是三个摄像机组成的系统，则可将所述视觉系统中的单个摄像机传感的转化矩阵M1，M2，Mi组合成M，则M可以表示为：
Figure FDA0003258487540000042
7.根据权利要求1所述的一种视觉导航完好性监测计算方法，其特征在于，所述保护级的计算方法分为三种假设：假设1、假设2和假设3；
所述假设1为观测量不存在故障的情况；所述假设2为观测量发生单个故障的情况；所述假设3为观测量发生双故障的情况。
8.根据权利要求6所述的一种视觉导航完好性监测计算方法，其特征在于，在所述假设1情况下，保护级可以通过无故障捡漏因子Kff和水平位置的标准差σx来计算的，其中，无故障检漏因子Kff由无故障概率Pff确定，因此，无故障保护级的计算公式表示为：
HPL1＝Kff(k)*σx
Kff：无故障检漏因子；σx：飞机水平方向的标准差；
在所述假设2情况下，可能的现象有单个视觉观测量发生故障或者单个卫星定位观测量发生故障；
当只有单个观测量发生故障，在故障偏差矢量b中只含有一个偏差成分，这时的水平位置误差δx可以如下表示：
Figure FDA0003258487540000043
其中，
Figure FDA0003258487540000044
表示最小二乘估计值，
Figure FDA0003258487540000045
设每个观测量的斜率为Hslope，表达式如下：
Figure FDA0003258487540000046
其中，
Figure FDA0003258487540000047
bi表示故障偏差矢量b对分量x的影响，bj表示故障偏差矢量b对分量y的影响；
其中，D为测试统计量，p表示奇偶矢量，P表示奇偶变换矩阵，p＝Pb；因此，水平定位误差保护级HPL2为：
HPL2＝Hslopemax×sbias
其中，
Figure FDA0003258487540000051
σ为噪声方差，λ为非中心化参数，n为计数变量，当无故障时，HPL2的值最小；当观测量有单个故障时，就会导致HPL2的值增大；
在所述假设3情况下，可能的现象有两个视觉观测量发生故障、两个卫星定位观测量故障或者一个视觉观测量和一个卫星定位观测量故障；
当两个观测量发生故障，第i个观测量和第j个观测量的同时发生故障的故障偏差矢量为b(i)和b(j)，可以表示为：
b＝[0...0 b(i) b(j)0...0 0...0]T
因此，定位误差表达式如下：
Figure FDA0003258487540000054
水平定位误差表达式如下：
δx2＝(b(i)A1i+b(j)A1j)2+(b(i)A2i+b(j)A2j)2
因此，计算斜率Hslope的公式为：
Figure FDA0003258487540000055
因此，HPL3的计算公式为：
HPL3＝Hslopemax×sbias
综合所述假设1、假设2和假设3，将所有情况下的误差包络在保护级中，得到总的保护级计算公式如下：
Figure FDA0003258487540000053
CN201911189662.7A 2019-11-28 2019-11-28 一种视觉导航完好性监测计算方法 Active CN111123304B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201911189662.7A CN111123304B (zh) 2019-11-28 2019-11-28 一种视觉导航完好性监测计算方法
US16/713,288 US10782418B1 (en) 2019-11-28 2019-12-13 Calculation method for visual navigation integrity monitoring

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911189662.7A CN111123304B (zh) 2019-11-28 2019-11-28 一种视觉导航完好性监测计算方法

Publications (2)

Publication Number Publication Date
CN111123304A CN111123304A (zh) 2020-05-08
CN111123304B true CN111123304B (zh) 2021-12-24

Family

ID=70496923

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911189662.7A Active CN111123304B (zh) 2019-11-28 2019-11-28 一种视觉导航完好性监测计算方法

Country Status (2)

Country Link
US (1) US10782418B1 (zh)
CN (1) CN111123304B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113137962A (zh) * 2021-03-31 2021-07-20 广州宸祺出行科技有限公司 一种基于ar的遮挡环境下的视觉定位方法及系统
CN113483759B (zh) * 2021-06-29 2023-10-17 北京航空航天大学 一种GNSS、INS、Vision组合导航系统完好性保护级计算方法
CN114877871B (zh) * 2022-05-06 2024-01-09 中国人民解放军国防科技大学 未标定视频卫星进行深空目标观测的姿态凝视控制方法
CN116380057B (zh) * 2023-06-05 2023-08-29 四川腾盾科技有限公司 一种gnss拒止环境下无人机自主着陆定位方法
CN117928494B (zh) * 2024-03-18 2024-05-24 中国人民解放军战略支援部队航天工程大学 一种光学卫星切片影像的几何定位测量方法、系统和设备

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102901971A (zh) * 2012-09-27 2013-01-30 电子科技大学 基于奇偶矢量法的双星故障识别方法
US8610623B2 (en) * 2010-06-23 2013-12-17 Astrium Gmbh RAIM algorithm
CN103454650A (zh) * 2013-08-20 2013-12-18 北京航空航天大学 一种视觉辅助卫星完好性监测方法
CN103592656A (zh) * 2013-10-17 2014-02-19 航天恒星科技有限公司 一种适用于星载导航接收机的自主完好性监测方法
CN109031356A (zh) * 2018-08-22 2018-12-18 北京理工大学 特征斜率加权的最小二乘残差接收机自主完好性监测方法
CN110068840A (zh) * 2019-05-15 2019-07-30 北京航空航天大学 一种基于伪距测量特征值提取的araim故障检测方法
CN110260885A (zh) * 2019-04-15 2019-09-20 南京航空航天大学 一种卫星/惯性/视觉组合导航系统完好性评估方法

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4253239B2 (ja) * 2003-10-07 2009-04-08 富士重工業株式会社 画像認識を用いた航法装置
ES2498720T3 (es) * 2010-12-01 2014-09-25 European Space Agency Método y aparato para determinar un parámetro de indicación de integridad que indica la integridad de la información de posicionamiento determinada en un sistema de posicionamiento global
ES2788750T3 (es) * 2011-09-16 2020-10-22 Saab Ab Procedimiento para mejorar la precisión de un sistema de navegación en base a radio
US8898011B2 (en) * 2012-09-12 2014-11-25 Raytheon Company Method for maintaining integrity against erroneous ephemeris for a differential GPS based navigation solution supporting fast system startup
EP2987001A4 (en) * 2013-04-16 2017-01-11 Bae Systems Australia Limited Landing system for an aircraft
US9182237B2 (en) * 2013-12-06 2015-11-10 Novatel Inc. Navigation system with rapid GNSS and inertial initialization
US20160327653A1 (en) * 2014-02-03 2016-11-10 Board Of Regents, The University Of Texas System System and method for fusion of camera and global navigation satellite system (gnss) carrier-phase measurements for globally-referenced mobile device pose determination
US10684375B2 (en) * 2015-01-05 2020-06-16 Samsung Electronics Co., Ltd Method of multiple satellite measurement failure detection and isolation for GNSS
US20180340788A1 (en) * 2015-10-19 2018-11-29 Nokia Technologies Oy A navigation apparatus and associated methods
US10338233B2 (en) * 2017-04-12 2019-07-02 Coherent Technical Services, Inc. Assured validation of carrier-phase integer ambiguities for safety-of-life applications
KR102463176B1 (ko) * 2017-10-16 2022-11-04 삼성전자주식회사 위치 추정 장치 및 방법

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8610623B2 (en) * 2010-06-23 2013-12-17 Astrium Gmbh RAIM algorithm
CN102901971A (zh) * 2012-09-27 2013-01-30 电子科技大学 基于奇偶矢量法的双星故障识别方法
CN103454650A (zh) * 2013-08-20 2013-12-18 北京航空航天大学 一种视觉辅助卫星完好性监测方法
CN103592656A (zh) * 2013-10-17 2014-02-19 航天恒星科技有限公司 一种适用于星载导航接收机的自主完好性监测方法
CN109031356A (zh) * 2018-08-22 2018-12-18 北京理工大学 特征斜率加权的最小二乘残差接收机自主完好性监测方法
CN110260885A (zh) * 2019-04-15 2019-09-20 南京航空航天大学 一种卫星/惯性/视觉组合导航系统完好性评估方法
CN110068840A (zh) * 2019-05-15 2019-07-30 北京航空航天大学 一种基于伪距测量特征值提取的araim故障检测方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
视觉辅助的飞行载体组合引导着陆关键技术研究;李丰阳;《中国优秀硕士学位论文全文数据库工程科技II辑》;20180615;第9-74页 *

Also Published As

Publication number Publication date
US10782418B1 (en) 2020-09-22
CN111123304A (zh) 2020-05-08

Similar Documents

Publication Publication Date Title
CN111123304B (zh) 一种视觉导航完好性监测计算方法
US7769543B2 (en) Fault detection and reconfiguration of an automated refueling boom
US7937190B2 (en) Apparatus for an automated aerial refueling boom using multiple types of sensors
CN110196443A (zh) 一种飞行器的容错组合导航方法及系统
Williamson et al. Sensor fusion applied to autonomous aerial refueling
CN101858748A (zh) 高空长航无人机的多传感器容错自主导航方法
Dumble et al. Efficient terrain-aided visual horizon based attitude estimation and localization
CN103454650A (zh) 一种视觉辅助卫星完好性监测方法
Watanabe et al. Vision-integrated navigation system for aircraft final approach in case of GNSS/SBAS or ILS failures
Hajiyev et al. Two-stage Kalman filter for fault tolerant estimation of wind speed and UAV flight parameters
Wen et al. Object-detection-aided GNSS and its integration with lidar in highly urbanized areas
Das et al. An experimental study on relative and absolute pose graph fusion for vehicle localization
CN113358135A (zh) 一种用光电测量数据修正飞行器位置的方法
Chu et al. Performance comparison of tight and loose INS-Camera integration
Khanafseh et al. Autonomous airborne refueling of unmanned air vehicles using the global positioning system
CN112179347B (zh) 一种基于光谱红移误差观测方程的组合导航方法
Zhao et al. Distributed filtering-based autonomous navigation system of UAV
US20230026395A1 (en) System and method for computing positioning protection levels
Liu et al. Error bounds of the GNSS/INS integrated system against GNSS fault for integrity monitoring
CN115096309A (zh) 融合定位方法、装置、电子设备及存储介质
CN112113567B (zh) 机载综合着陆导航方法
Samadzadegan et al. Vision-based pose estimation for autonomous outdoor navigation of aerial vehicles
Al Hage et al. Bounding localization errors with Student distribution for road vehicles
Liu et al. 3D LiDAR Aided GNSS NLOS Correction with Direction of Arrival Estimation Using Doppler Measurements
CN117168499B (zh) 一种高频动态目标基准位置估测方法及计算机可读介质

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