CN110208843A - 一种基于增广伪距信息辅助的容错导航方法 - Google Patents

一种基于增广伪距信息辅助的容错导航方法 Download PDF

Info

Publication number
CN110208843A
CN110208843A CN201910422237.1A CN201910422237A CN110208843A CN 110208843 A CN110208843 A CN 110208843A CN 201910422237 A CN201910422237 A CN 201910422237A CN 110208843 A CN110208843 A CN 110208843A
Authority
CN
China
Prior art keywords
subfilter
satellite
navigation
matrix
state
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
Application number
CN201910422237.1A
Other languages
English (en)
Other versions
CN110208843B (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201910422237.1A priority Critical patent/CN110208843B/zh
Publication of CN110208843A publication Critical patent/CN110208843A/zh
Application granted granted Critical
Publication of CN110208843B publication Critical patent/CN110208843B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • 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
    • 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
    • G01S19/49—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 whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Computer Security & Cryptography (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种基于增广伪距信息辅助的容错导航方法，属于组合导航技术领域。该导航方法包括：导航系统状态方程的建立、导航系统量测方程的建立和基于卡尔曼滤波的组合导航系统信息融合。本发明利用卫星接收机接收到的星历数据、罗兰C接收机接收到的伪卫星数据结合惯性导航系统给出的位置和速度，进行卡尔曼滤波，同时卡尔曼滤波下的多解分离算法开始工作，检测卫星故障情况。若发现故障，及时进行故障隔离，利用隔离后的数据重置卡尔曼滤波器，系统再次进入卡尔曼滤波和监测工作状态。本发明将多解分离算法运用到卡尔曼滤波中，增加了卡尔曼滤波定位的连续性、稳定性和鲁棒性。

Description

一种基于增广伪距信息辅助的容错导航方法
技术领域
本发明涉及一种基于增广伪距信息辅助的容错导航方法，属于组合导航技术领域。
背景技术
随着科学技术的迅猛发展，导航系统在多个领域，尤其在航空、航天等领域内应用的不断深入，组合导航技术成为一个必然的选择。全球卫星导航系统能够进行全球、全天候和实时导航,有较高的定位和测速精度。由于全球卫星导航系统和SINS(捷联惯性导航系统)具有较强的互补性,采用卫星/SINS组合导航系统能够综合两者的优点,实现优势互补,使系统适应复杂多变的环境。但是当卫星信号丢失无法获得足够的导航信息或者卫星出现故障并排除故障后无法提供足够的导航信息时，卫星/SINS组合导航系统此时只能工作在纯惯性导航模式，导航精度不高。同时，系统数学模型和噪声统计不准确、噪声突变等情况也会导致卡尔曼滤波收敛性变差，甚至导致滤波发散，极大影响组合系统导航精度。
发明内容
本发明提出了一种基于增广伪距信息辅助的容错导航方法，在考虑卫星/SINS组合导航系统导航过程中卫星出现故障或者特殊环境下无法获得足够导航卫星时，利用罗兰C导航提供增广伪距结合卫星导航信号进行卫星的故障检测、排除，并与惯性导航组合滤波，实现对载体的高精度定位导航。
本发明为解决其技术问题采用如下技术方案：
一种基于增广伪距信息辅助的容错导航方法，其特征在于包括以下步骤：
(1)罗兰C辅助惯性卫星容错导航信息的获取
根据卫星导航接收机得到当前时刻k可见卫星的位置和伪距，根据罗兰C导航接收机获得当前时刻k罗兰C导航台的位置和伪距，根据陀螺仪和加速度计获得当前时刻k载体的位置和速度和姿态信息；
(2)组合导航状态方程的建立
选择东北天地理坐标系作为导航坐标系，组合导航系统的状态方程为SINS的误差方程，表示为：
其中，状态向量包括三向数字平台误差角三向速度误差δve,δvn,δvu，位置误差δL,δλ,δh，三向陀螺常值漂移误差εbx,εby,εbz，三向一阶马尔可夫漂移误差εrx,εry,εrz，三向加速度计零偏误差▽x,▽y,▽z，接收机时钟偏差δtu和时钟漂移δtru；X(t)为系统状态向量，表示系统状态向量的导数，Φ(t)表示组合导航系统状态方程的一步状态转移矩阵；Γ(t)表示组合导航系统状态方程的系统误差矩阵；W(t)为组合导航系统状态方程的系统误差白噪声矢量，包括陀螺仪白噪声ωgxωgyωgzωrxωryωrz和加速度计白噪声ωaxωayωaz；
(3)组合导航系统量测方程的建立
根据步骤(1)获取的当前时刻k罗兰C数据，卫星数据和惯导数据建立组合导航量测方程：
Zρ(t)＝ρIi-ρGi＝Hρ(t)X(t)+Vρ(t)
其中，Zρ(t)为n维矢量，由各颗卫星的测量伪距、各罗兰C测量伪距和计算伪距之差组成，n为可见卫星数a和罗兰C导航台数b总和，i(1≤i≤n)为(伪)卫星编号；Vρ(t)为量测噪声矩阵，测量伪距ρGi由卫星导航接收机和罗兰C导航接收机给出，计算伪距ρIi由惯导位置信息与卫星、罗兰C位置坐标解算获得；已知卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的位置(xsi,ysi,zsi),则相应于惯性导航系统给出的(转换为ECEF坐标系)位置所对应于某颗(伪)卫星i的计算伪距为xI为惯性导航系统测得的相对于ECEF坐标系的载体的横坐标，yI为惯性导航系统测得的相对于ECEF坐标系的载体的纵坐标，zI为惯性导航系统测得的相对于ECEF坐标系的载体的z坐标，xsi为卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的横坐标，ysi为卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的纵坐标，zsi为卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的z坐标，0n×6为n×6维全0矩阵，0n×9为n×9维0矩阵，为从地球直角坐标系向大地坐标系转换的方向余弦阵，ai1＝(RN+h)[-ei1sinLcosλ-ei2sinLsinλ]+[RN(1-f)2+h]ei3cosL,ai2＝(RN+h)[ei2cosLcosλ-ei1cosLsinλ]，ai3＝ei1cosLcosλ+ei2cosLsinλ+ei3sinL,1≤i≤n,RN为卯酉圈曲率半径，f＝1/298.257， x为载体位置真值的横坐标，y为载体位置真值的纵坐标，z为载体位置真值的z坐标，λ为惯导输出的载体的经度，L为惯导输出的载体的纬度，h为惯导输出的载体的高度，为n×2维矩阵，第一列全为1，对应时钟偏差δtu，第二列全为0；
(4)滤波初始化，根据步骤(1)获得的当前时刻k的量测数据将罗兰C个数b与3比较，若b≥3，则转置步骤(12)；若0＜b＜3，则转置步骤(11)，反之转置步骤(5)；
(5)综合全部可见卫星a(卫星编号为1～a)的量测信息和惯导解算信息进行卡尔曼主滤波，分别计算k-1时刻到k时刻的状态一步预测值Xk|k-1＝Φk,k-1Xk-1|k-1、一步预测均方误差值和k时刻的滤波增益(对应主滤波器F00的滤波增益K00)、估计均方误差值Pk|k＝(I-KkHk)Pk|k-1(对应主滤波器的协方差阵P00)、状态估值Xk|k＝Xk|k-1+Kk(Zk-HkXk|k-1)(对应主滤波器的状态量X00)；其中，Φk,k-1为k-1时刻到k时刻的系统一步转移矩阵(20×20阶)，Xk-1|k-1为k-1的状态估计，Γk-1为系统噪声矩阵(20×9阶)，Pk-1为k-1的均方误差矩阵，Hk为k时刻的量测矩阵，Pk|k-1为一步预测均方误差矩阵，Kk为k时刻的滤波增益，Zk为系统的量测值，Qk-1为k-1时刻系统噪声的方差矩阵，Rk为k时刻量测噪声的方差矩阵；
(6)弃用编号为1～a可见卫星的一颗卫星信息，利用剩余a-1颗卫星的量测信息和惯导解算信息进行卡尔曼子滤波，共进行a次卡尔曼子滤波(每次弃用不同的卫星)，如步骤(5)一样计算各子滤波器F01～F0a的状态向量X01～X0a、协方差矩阵P01～P0a和滤波增益矩阵K01～K0a；
(7)根据步骤(5)输出的主滤波器状态向量X00和步骤(6)输出子滤波器的状态向量X01～X0a构造子滤波器i的水平位置检测统计量其中：X00(7)为主滤波器状态向量的纬度误差状态，X00(8)为主滤波器状态向量的经度误差状态，X0i(7)为子滤波器i状态向量的纬度误差状态，X0i(8)为子滤波器i状态向量的经度误差状态，根据步骤(6)输出的协方差矩阵P01～P0a构造各子滤波器的检测门限其中PFA为误警率，P00(7,7)为主滤波器协方差矩阵纬度误差协方差，P0i(7,7)为子滤波器i协方差矩阵纬度误差协方差，P00(8,8)为主滤波器协方差矩阵经度误差协方差，P0i(8,8)为子滤波器i协方差矩阵经度误差协方差，erf-1()为高斯误差函数的反函数，a为可见卫星数；比较d0i和Ti，若d0i＞Ti，则转置步骤(8),反之转置步骤(16)；
(8)根据步骤(6)中的子滤波器进行各子滤波器下属的子-子滤波器的卡尔曼滤波，每个子-子滤波器除弃用相应子滤波器弃用的卫星量测外，还弃用另一个卫星量测，再结合惯导解算信息进行卡尔曼滤波，每个子滤波器共进行a-1次其下属的子-子滤波，输出对应子-子滤波器F12～Fa,a-1的状态向量X12～Xa,a-1、协方差矩阵P12～Pa,a-1和滤波增益矩阵K12～Ka,a-1；
(9)根据步骤(6)输出的状态向量X01～X0a和步骤(8)输出的状态向量X12～Xa,n-1构造各子-子滤波器的水平位置检测统计量其中：X0i(7)为子滤波器i状态向量的纬度误差状态，X0i(8)为子滤波器状态向量的经度误差状态，Xij(7)为子滤波器i下属子-子滤波器j状态向量的纬度误差状态，Xij(8)为子滤波器i下属子-子滤波器j状态向量的经度误差状态，根据步骤(8)输出的协方差矩阵P12～Pa,n-1构造各子滤波器的检测门限其中P0i(7,7)为子滤波器i协方差矩阵纬度误差协方差，Pij(7,7)为子滤波器i下属子-子滤波器j协方差矩阵纬度误差协方差，P0i(8,8)为子滤波器i协方差矩阵经度误差协方差，Pij(8,8)为子滤波器i下属子-子滤波器j协方差矩阵经度误差协方差；比较各子滤波器下属的子-子滤波器的水平位置检测统计量dij和检测门限Tij，若该子滤波器Fi下的a-1个检测统计量均小于检测门限，则判断该编号为i的卫星为故障星，转置步骤(15),反之转置步骤(17)；
(10)综合a个可见卫星和b个罗兰C导航台的量测信息，结合惯导信息进行卡尔曼主滤波，计算主滤波器的状态一步预测值Xk|k-1、一步预测均方误差值Pk|k-1、滤波增益K00、协方差阵P00和状态量X00；
(11)弃用编号为1～a可见卫星中的一颗卫星，利用剩余a-1颗卫星和b个罗兰C导航台的量测信息，结合惯导信息进行卡尔曼子滤波，共进行a次子滤波(每次弃用不同的卫星)，计算各子滤波器F01～F0a的状态向量X01～X0a、协方差矩阵P01～P0a和滤波增益矩阵K01～K0a；
(12)根据步骤(10)输出的主滤波器状态向量X00和步骤(11)输出子滤波器的状态向量X01～X0a构造子滤波器i的水平位置检测统计量根据步骤(11)输出的协方差矩阵P01～P0a构造各子滤波器的检测门限比较d0i和Ti，若d0i＞Ti，则转置步骤(13),反之转置步骤(16)；
(13)根据步骤(11)中的子滤波器进行各子滤波器下属的子-子滤波器的卡尔曼滤波，每个子-子滤波器除弃用相应子滤波器弃用的卫星量测外，还弃用另一个卫星量测，再结合惯导解算信息进行卡尔曼滤波，每个子滤波器进行a-1次其下属的子-子滤波，输出对应子-子滤波器F12～Fa,a-1的状态向量X12～Xa,a-1、协方差矩阵P12～Pa,a-1和滤波增益矩阵K12～Ka,a-1；
(14)根据步骤(11)输出的状态向量X01～X0a和步骤(13)输出的状态向量X12～Xa,a-1构造各子-子滤波器的水平位置检测统计量根据步骤(13)输出的协方差矩阵P12～Pa,a-1构造各子滤波器的检测门限比较各子滤波器下属的子-子滤波器的水平位置检测统计量dij和检测门限Tij，若该子滤波器Fi下的a-1个检测统计量均小于检测门限，则判断该编号为i的卫星为故障星，转置步骤(15),反之转置步骤(17)；
(15)将子滤波器Fi对应的量测信息结合惯导解算信息重置卡尔曼滤波，得到排除故障星后的结果，进行滤波修正，输出组合导航解算结果，转置步骤(19)；
(16)将主滤波器F00对应的量测信息结合惯导解算信息重置卡尔曼滤波，输出组合导航解算结果，转置步骤(19)；
(17)输出惯导定位结果,转置步骤(19)；
(18)输出罗兰C导航定位结果；
(19)进行下一时刻的导航解算。
本发明的有益效果如下：
1、本发明将多解分离算法运用于组合导航卡尔曼滤波中，通过罗兰C辅助增加了冗余量，使可见卫星少于5颗时也能进行故障检测和排除。
2、相比于传统RAIM(接收机自主完好性监测)法，本发明不仅对于缓变的斜坡故障监测具有更好的实时性，而且在检测到故障时刻的导航误差更小。
3、本发明可用于多系统容错导航对复合故障的检测，提高导航定位精度和系统的容错性能。
4、本发明适用性广，不受载体的限制。
附图说明
图1是本发明的一种基于增广伪距信息辅助的容错导航方法的系统图。
图2(a)是故障排除前后的经度误差曲线图，图2(b)是故障排除前后的纬度误差曲线图，图2(c)是故障排除前后的高度误差曲线图。
图3(a)是故障排除前后组合导航东向速度误差曲线图，图3(b)是故障排除前后组合导航北向速度误差曲线图，图3(c)是故障排除前后组合导航天向速度误差曲线图。
图4是故障排除前后组合导航水平误差曲线图。
图5(a)是子滤波器F01检测函数值曲线图，图5(b)是子滤波器F02检测函数值曲线图，图5(c)是子滤波器F03检测函数值曲线图，图5(d)是子滤波器F04检测函数值曲线图，图5(e)是子滤波器F05检测函数值曲线图。
图6(a)是子滤波器F01下属的子-子滤波器F12检测函数值曲线图，图6(b)是子滤波器F01下属的子-子滤波器F13检测函数值曲线图，图6(c)是子滤波器F01下属的子-子滤波器F14检测函数值曲线图，图6(d)是子滤波器F01下属的子-子滤波器F15检测函数值曲线图。
图7(a)是子滤波器F02下属的子-子滤波器F21检测函数值曲线图，图7(b)是子滤波器F02下属的子-子滤波器F23检测函数值曲线图，图7(c)是子滤波器F02下属的子-子滤波器F24检测函数值曲线图，图7(d)是子滤波器F02下属的子-子滤波器F25检测函数值曲线图。
图8(a)是子滤波器F03下属的子-子滤波器F31检测函数值曲线图，图8(b)是子滤波器F03下属的子-子滤波器F32检测函数值曲线图，图8(c)是子滤波器F03下属的子-子滤波器F34检测函数值曲线图，图8(d)是子滤波器F03下属的子-子滤波器F35检测函数值曲线图。
图9(a)是子滤波器F04下属的子-子滤波器F41检测函数值曲线图，图9(b)是子滤波器F04下属的子-子滤波器F42检测函数值曲线图，图9(c)是子滤波器F04下属的子-子滤波器F43检测函数值曲线图，图9(d)是子滤波器F04下属的子-子滤波器F45检测函数值曲线图。
图10(a)是子滤波器F05下属的子-子滤波器F51检测函数值曲线图，图10(b)是子滤波器F05下属的子-子滤波器F52检测函数值曲线图，图10(c)是子滤波器F05下属的子-子滤波器F53检测函数值曲线图，图10(d)是子滤波器F05下属的子-子滤波器F54检测函数值曲线图。
具体实施方式
下面结合附图的示例，进一步阐述本发明，应理解附图描述的实施方式是范例性的，仅用于解释本发明，而不用于限制本发明的范围。
本发明提供一种基于增广伪距信息辅助的容错导航方法，在卫星出现故障或者特殊环境下无法获得足够导航卫星时，利用罗兰C导航提供增广伪距结合卫星导航信号进行卫星的故障检测、排除，并与惯性导航组合滤波，用于提高导航系统的容错性和定位精度，算法系统图如图1所示，仿真结果图如图2至图10所示，滤波方法包括以下步骤：
(1)罗兰C辅助惯性卫星容错导航信息的获取
根据卫星导航接收机得到当前时刻k可见卫星的位置和伪距，根据罗兰C导航接收机获得当前时刻k罗兰C导航台的位置和伪距，根据陀螺仪和加速度计获得当前时刻k载体的位置和速度和姿态信息；
(2)组合导航状态方程的建立
选择东北天地理坐标系作为导航坐标系，组合导航系统的状态方程为SINS的误差方程，表示为：
其中，状态向量包括三向数字平台误差角三向速度误差δve,δvn,δvu，位置误差δL,δλ,δh，三向陀螺常值漂移误差εbx,εby,εbz，三向一阶马尔可夫漂移误差εrx,εry,εrz，三向加速度计零偏误差▽x,▽y,▽z，接收机时钟偏差δtu和时钟漂移δtru；X(t)为系统状态向量，表示系统状态向量的导数，Φ(t)表示组合导航系统状态方程的一步状态转移矩阵；Γ(t)表示组合导航系统状态方程的系统误差矩阵；W(t)为组合导航系统状态方程的系统误差白噪声矢量，包括陀螺仪白噪声ωgxωgyωgzωrxωryωrz和加速度计白噪声ωaxωayωaz；
(3)组合导航系统量测方程的建立
根据步骤(1)获取的当前时刻k罗兰C数据，卫星数据和惯导数据建立组合导航量测方程：
Zρ(t)＝ρIi-ρGi＝Hρ(t)X(t)+Vρ(t)
其中，Zρ(t)为n维矢量，由各颗卫星的测量伪距、各罗兰C测量伪距和计算伪距之差组成，n为可见卫星数a和罗兰C导航台数b总和，i(1≤i≤n)为(伪)卫星编号；Vρ(t)为量测噪声矩阵，测量伪距ρGi由卫星导航接收机和罗兰C导航接收机给出，计算伪距ρIi由惯导位置信息与卫星、罗兰C位置坐标解算获得；已知卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的位置(xsi,ysi,zsi),则相应于惯性导航系统给出的(转换为ECEF坐标系)位置所对应于某颗(伪)卫星i的计算伪距为xI为惯性导航系统测得的相对于ECEF坐标系的载体的横坐标，yI为惯性导航系统测得的相对于ECEF坐标系的载体的纵坐标，zI为惯性导航系统测得的相对于ECEF坐标系的载体的z坐标，xsi为卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的横坐标，ysi为卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的纵坐标，zsi为卫星导航和罗兰C导航第i颗(伪)卫星t时刻在地球固定坐标系(ECEF)上的z坐标，0n×6为n×6维全0矩阵，0n×9为n×9维0矩阵，为从地球直角坐标系向大地坐标系转换的方向余弦阵，ai1＝(RN+h)[-ei1sinLcosλ-ei2sinLsinλ]+[RN(1-f)2+h]ei3cosL,ai2＝(RN+h)[ei2cosLcosλ-ei1cosLsinλ]，ai3＝ei1cosLcosλ+ei2cosLsinλ+ei3sinL,1≤i≤n,RN为卯酉圈曲率半径，f＝1/298.257， x为载体位置真值的横坐标，y为载体位置真值的纵坐标，z为载体位置真值的z坐标，λ为惯导输出的载体的经度，L为惯导输出的载体的纬度，h为惯导输出的载体的高度，为n×2维矩阵，第一列全为1，对应时钟偏差δtu，第二列全为0；
(4)滤波初始化，根据步骤(1)获得的当前时刻k的量测数据将罗兰C个数b与3比较，若b≥3，则转置步骤(12)；若0＜b＜3，则转置步骤(11)，反之转置步骤(5)；
(5)综合全部可见卫星a(卫星编号为1～a)的量测信息和惯导解算信息进行卡尔曼主滤波，分别计算k-1时刻到k时刻的状态一步预测值Xk|k-1＝Φk,k-1Xk-1|k-1、一步预测均方误差值和k时刻的滤波增益(对应主滤波器F00的滤波增益K00)、估计均方误差值Pk|k＝(I-KkHk)Pk|k-1(对应主滤波器的协方差阵P00)、状态估值Xk|k＝Xk|k-1+Kk(Zk-HkXk|k-1)(对应主滤波器的状态量X00)；其中，Φk,k-1为k-1时刻到k时刻的系统一步转移矩阵(20×20阶)，Xk-1|k-1为k-1的状态估计，Γk-1为系统噪声矩阵(20×9阶)，Pk-1为k-1的均方误差矩阵，Hk为k时刻的量测矩阵，Pk|k-1为一步预测均方误差矩阵，Kk为k时刻的滤波增益，Zk为系统的量测值，Qk-1为k-1时刻系统噪声的方差矩阵，Rk为k时刻量测噪声的方差矩阵；
(6)弃用编号为1～a可见卫星的一颗卫星信息，利用剩余a-1颗卫星的量测信息和惯导解算信息进行卡尔曼子滤波，共进行a次卡尔曼子滤波(每次弃用不同的卫星)，如步骤(5)一样计算各子滤波器F01～F0a的状态向量X01～X0a、协方差矩阵P01～P0a和滤波增益矩阵K01～K0a；
(7)根据步骤(5)输出的主滤波器状态向量X00和步骤(6)输出子滤波器的状态向量X01～X0a构造子滤波器i的水平位置检测统计量其中：X00(7)为主滤波器状态向量的纬度误差状态，X00(8)为主滤波器状态向量的经度误差状态，X0i(7)为子滤波器i状态向量的纬度误差状态，X0i(8)为子滤波器i状态向量的经度误差状态，根据步骤(6)输出的协方差矩阵P01～P0a构造各子滤波器的检测门限其中PFA为误警率，P00(7,7)为主滤波器协方差矩阵纬度误差协方差，P0i(7,7)为子滤波器i协方差矩阵纬度误差协方差，P00(8,8)为主滤波器协方差矩阵经度误差协方差，P0i(8,8)为子滤波器i协方差矩阵经度误差协方差，erf-1()为高斯误差函数的反函数，a为可见卫星数；比较d0i和Ti，若d0i＞Ti，则转置步骤(8),反之转置步骤(16)；
(8)根据步骤(6)中的子滤波器进行各子滤波器下属的子-子滤波器的卡尔曼滤波，每个子-子滤波器除弃用相应子滤波器弃用的卫星量测外，还弃用另一个卫星量测，再结合惯导解算信息进行卡尔曼滤波，每个子滤波器共进行a-1次其下属的子-子滤波，输出对应子-子滤波器F12～Fa,a-1的状态向量X12～Xa,a-1、协方差矩阵P12～Pa,a-1和滤波增益矩阵K12～Ka,a-1；
(9)根据步骤(6)输出的状态向量X01～X0a和步骤(8)输出的状态向量X12～Xa,n-1构造各子-子滤波器的水平位置检测统计量其中：X0i(7)为子滤波器i状态向量的纬度误差状态，X0i(8)为子滤波器状态向量的经度误差状态，Xij(7)为子滤波器i下属子-子滤波器j状态向量的纬度误差状态，Xij(8)为子滤波器i下属子-子滤波器j状态向量的经度误差状态，根据步骤(8)输出的协方差矩阵P12～Pa,n-1构造各子滤波器的检测门限其中P0i(7,7)为子滤波器i协方差矩阵纬度误差协方差，Pij(7,7)为子滤波器i下属子-子滤波器j协方差矩阵纬度误差协方差，P0i(8,8)为子滤波器i协方差矩阵经度误差协方差，Pij(8,8)为子滤波器i下属子-子滤波器j协方差矩阵经度误差协方差；比较各子滤波器下属的子-子滤波器的水平位置检测统计量dij和检测门限Tij，若该子滤波器Fi下的a-1个检测统计量均小于检测门限，则判断该编号为i的卫星为故障星，转置步骤(15),反之转置步骤(17)；
(10)综合a个可见卫星和b个罗兰C导航台的量测信息，结合惯导信息进行卡尔曼主滤波，计算主滤波器的状态一步预测值Xk|k-1、一步预测均方误差值Pk|k-1、滤波增益K00、协方差阵P00和状态量X00；
(11)弃用编号为1～a可见卫星中的一颗卫星，利用剩余a-1颗卫星和b个罗兰C导航台的量测信息，结合惯导信息进行卡尔曼子滤波，共进行a次子滤波(每次弃用不同的卫星)，计算各子滤波器F01～F0a的状态向量X01～X0a、协方差矩阵P01～P0a和滤波增益矩阵K01～K0a；
(12)根据步骤(10)输出的主滤波器状态向量X00和步骤(11)输出子滤波器的状态向量X01～X0a构造子滤波器i的水平位置检测统计量根据步骤(11)输出的协方差矩阵P01～P0a构造各子滤波器的检测门限比较d0i和Ti，若d0i＞Ti，则转置步骤(13),反之转置步骤(16)；
(13)根据步骤(11)中的子滤波器进行各子滤波器下属的子-子滤波器的卡尔曼滤波，每个子-子滤波器除弃用相应子滤波器弃用的卫星量测外，还弃用另一个卫星量测，再结合惯导解算信息进行卡尔曼滤波，每个子滤波器进行a-1次其下属的子-子滤波，输出对应子-子滤波器F12～Fa,a-1的状态向量X12～Xa,a-1、协方差矩阵P12～Pa,a-1和滤波增益矩阵K12～Ka,a-1；
(14)根据步骤(11)输出的状态向量X01～X0a和步骤(13)输出的状态向量X12～Xa,a-1构造各子-子滤波器的水平位置检测统计量根据步骤(13)输出的协方差矩阵P12～Pa,a-1构造各子滤波器的检测门限比较各子滤波器下属的子-子滤波器的水平位置检测统计量dij和检测门限Tij，若该子滤波器Fi下的a-1个检测统计量均小于检测门限，则判断该编号为i的卫星为故障星，转置步骤(15),反之转置步骤(17)；
(15)将子滤波器Fi对应的量测信息结合惯导解算信息重置卡尔曼滤波，得到排除故障星后的结果，进行滤波修正，输出组合导航解算结果，转置步骤(19)；
(16)将主滤波器F00对应的量测信息结合惯导解算信息重置卡尔曼滤波，输出组合导航解算结果，转置步骤(19)；
(17)输出惯导定位结果,转置步骤(19)；
(18)输出罗兰C导航定位结果；
(19)进行下一时刻的导航解算。

Claims (5)

1.一种基于增广伪距信息辅助的容错导航方法，其特征在于包括以下步骤：
(1)罗兰C辅助惯性卫星容错导航信息的获取；
(2)组合导航状态方程的建立；
(3)组合导航系统量测方程的建立；
(4)滤波初始化，根据步骤(1)获得的当前时刻k的量测数据将罗兰C个数b与3比较，若b≥3，则转置步骤(12)；若0＜b＜3，则转置步骤(11)，反之转置步骤(5)；
(5)综合全部卫星a量测信息和惯导解算信息进行卡尔曼主滤波，分别计算k-1时刻到k时刻的状态一步预测值Xk|k-1＝Φk,k-1Xk-1|k-1、一步预测均方误差值和k时刻的滤波增益估计均方误差值Pk|k＝(I-KkHk)Pk|k-1、态估值Xk|k＝Xk|k-1+Kk(Zk-HkXk|k-1)；其中，Φk,k-1为k-1时刻到k时刻的系统一步转移矩阵，Xk-1|k-1为k-1的状态估计，Γk-1为系统噪声矩阵(20×9阶)，Pk-1为k-1的均方误差矩阵，Hk为k时刻的量测矩阵，Pk|k-1为一步预测均方误差矩阵，Kk为k时刻的滤波增益，Zk为系统的量测值，Qk-1为k-1时刻系统噪声的方差矩阵，Rk为k时刻量测噪声的方差矩阵；
(6)弃用编号为1～a全部卫星中的一颗卫星信息，利用剩余a-1颗卫星的量测信息和惯导解算信息进行卡尔曼子滤波，共进行a次卡尔曼子滤波，如步骤(5)一样计算各子滤波器F01～F0a的状态向量X01～X0a、协方差矩阵P01～P0a和滤波增益矩阵K01～K0a；
(7)根据步骤(5)输出的主滤波器状态向量X00和步骤(6)输出子滤波器的状态向量X01～X0a构造子滤波器i的水平位置检测统计量其中：X00(7)为主滤波器状态向量的纬度误差状态，X00(8)为主滤波器状态向量的经度误差状态，X0i(7)为子滤波器i状态向量的纬度误差状态，X0i(8)为子滤波器i状态向量的经度误差状态，根据步骤(6)输出的协方差矩阵P01～P0a构造各子滤波器的检测门限其中PFA为误警率，P00(7,7)为主滤波器协方差矩阵纬度误差协方差，P0i(7,7)为子滤波器i协方差矩阵纬度误差协方差，P00(8,8)为主滤波器协方差矩阵经度误差协方差，P0i(8,8)为子滤波器i协方差矩阵经度误差协方差，erf-1()为高斯误差函数的反函数，a为可见卫星数；比较d0i和Ti，若d0i＞Ti，则转置步骤(8),反之转置步骤(16)；
(8)根据步骤(6)中的子滤波器进行各子滤波器下属的子-子滤波器的卡尔曼滤波，每个子-子滤波器除弃用相应子滤波器弃用的卫星量测外，还弃用另一个卫星量测，再结合惯导解算信息进行卡尔曼滤波，每个子滤波器共进行a-1次其下属的子-子滤波，输出对应子-子滤波器F12～Fa,a-1的状态向量X12～Xa,a-1、协方差矩阵P12～Pa,a-1和滤波增益矩阵K12～Ka,a-1；
(9)根据步骤(6)输出的状态向量X01～X0a和步骤(8)输出的状态向量X12～Xa,n-1构造各子-子滤波器的水平位置检测统计量其中：X0i(7)为子滤波器i状态向量的纬度误差状态，X0i(8)为子滤波器状态向量的经度误差状态，Xij(7)为子滤波器i下属子-子滤波器j状态向量的纬度误差状态，Xij(8)为子滤波器i下属子-子滤波器j状态向量的经度误差状态，根据步骤(8)输出的协方差矩阵P12～Pa,n-1构造各子滤波器的检测门限其中P0i(7,7)为子滤波器i协方差矩阵纬度误差协方差，Pij(7,7)为子滤波器i下属子-子滤波器j协方差矩阵纬度误差协方差，P0i(8,8)为子滤波器i协方差矩阵经度误差协方差，Pij(8,8)为子滤波器i下属子-子滤波器j协方差矩阵经度误差协方差；比较各子滤波器下属的子-子滤波器的水平位置检测统计量dij和检测门限Tij，若该子滤波器Fi下的a-1个检测统计量均小于检测门限，则判断该编号为i的卫星为故障星，转置步骤(15),反之转置步骤(17)；
(10)综合a个卫星和b个罗兰C导航台的量测信息，结合惯导信息进行卡尔曼主滤波，计算主滤波器的状态一步预测值Xk|k-1、一步预测均方误差值Pk|k-1、滤波增益K00、协方差阵P00和状态量X00；
(11)弃用编号为1～a全部卫星中的一颗卫星，利用剩余a-1颗卫星和b个罗兰C导航台的量测信息，结合惯导信息进行卡尔曼子滤波，共进行a次子滤波，计算各子滤波器F01～F0a的状态向量X01～X0a、协方差矩阵P01～P0a和滤波增益矩阵K01～K0a；
(12)根据步骤(10)输出的主滤波器状态向量X00和步骤(11)输出子滤波器的状态向量X01～X0a构造子滤波器i的水平位置检测统计量根据步骤(11)输出的协方差矩阵P01～P0a构造各子滤波器的检测门限比较d0i和Ti，若d0i＞Ti，则转置步骤(13),反之转置步骤(16)；
(13)根据步骤(11)中的子滤波器进行各子滤波器下属的子-子滤波器的卡尔曼滤波，每个子-子滤波器除弃用相应子滤波器弃用的卫星量测外，还弃用另一个卫星量测，再结合惯导解算信息进行卡尔曼滤波，每个子滤波器进行a-1次其下属的子-子滤波，输出对应子-子滤波器F12～Fa,a-1的状态向量X12～Xa,a-1、协方差矩阵P12～Pa,a-1和滤波增益矩阵K12～Ka,a-1；
(14)根据步骤(11)输出的状态向量X01～X0a和步骤(13)输出的状态向量X12～Xa,a-1构造各子-子滤波器的水平位置检测统计量根据步骤(13)输出的协方差矩阵P12～Pa,a-1构造各子滤波器的检测门限比较各子滤波器下属的子-子滤波器的水平位置检测统计量dij和检测门限Tij，若该子滤波器Fi下的a-1个检测统计量均小于检测门限，则判断该编号为i的卫星为故障星，转置步骤(15),反之转置步骤(17)；
(15)将子滤波器Fi对应的量测信息结合惯导解算信息重置卡尔曼滤波，得到排除故障星后的结果，进行滤波修正，输出组合导航解算结果，转置步骤(19)；
(16)将主滤波器F00对应的量测信息结合惯导解算信息重置卡尔曼滤波，输出组合导航解算结果，转置步骤(19)；
(17)输出惯导定位结果,转置步骤(19)；
(18)输出罗兰C导航定位结果；
(19)进行下一时刻的导航解算。
2.根据权利要求1所述的一种基于增广伪距信息辅助的容错导航方法，其特征在于，步骤(1)的具体过程如下：
根据卫星导航接收机得到当前时刻k全部卫星的位置和伪距，根据罗兰C导航接收机获得当前时刻k罗兰C导航台的位置和伪距，根据陀螺仪和加速度计获得当前时刻k载体的位置和速度和姿态信息。
3.根据权利要求1所述的一种基于增广伪距信息辅助的容错导航方法，其特征在于，步骤(2)的具体过程如下：
选择东北天地理坐标系作为导航坐标系，组合导航系统的状态方程为SINS的误差方程，表示为：
其中，X(t)为系统状态向量，表示系统状态向量的导数，Φ(t)表示组合导航系统状态方程的一步状态转移矩阵；Γ(t)表示组合导航系统状态方程的系统误差矩阵；W(t)为组合导航系统状态方程的系统误差白噪声矢量，包括陀螺仪白噪声ωgxωgyωgzωrxωryωrz和加速度计白噪声ωaxωayωaz。
4.根据权利要求3所述的一种基于增广伪距信息辅助的容错导航方法，其特征在于，步骤(3)的具体过程如下：
根据步骤(1)获取的当前时刻k罗兰C数据，卫星数据和惯导数据建立组合导航量测方程：
Zρ(t)＝ρIi-ρGi＝Hρ(t)X(t)+Vρ(t)
其中，Zρ(t)为n维矢量，由各颗卫星的测量伪距、各罗兰C测量伪距和计算伪距之差组成，n为接收机获得的全部卫星数a和罗兰C导航台数b总和，i为卫星编号；Vρ(t)为量测噪声矩阵，测量伪距ρGi由卫星导航接收机和罗兰C导航接收机给出，计算伪距ρIi由惯导位置信息与卫星、罗兰C位置坐标解算获得；0n×6为n×6维全0矩阵，0n×9为n×9维0矩阵，为从地球直角坐标系向大地坐标系转换的方向余弦阵，ai1＝(RN+h)[-ei1sin L cosλ-ei2sin L sinλ]+[RN(1-f)2+h]ei3cos L,ai2＝(RN+h)[ei2cos L cosλ-ei1cos L sinλ]，ai3＝ei1cos L cosλ+ei2cos L sinλ+ei3sin L,1≤i≤n,RN为卯酉圈曲率半径，f＝1/298.257， x为载体位置真值的横坐标，y为载体位置真值的纵坐标，z为载体位置真值的z坐标，λ为惯导输出的载体的经度，L为惯导输出的载体的纬度，h为惯导输出的载体的高度，为n×2维矩阵，第一列全为1，对应时钟偏差δtu，第二列全为0。
5.根据权利要求4所述的一种基于增广伪距信息辅助的容错导航方法，其特征在于，所述计算伪距为xI为惯性导航系统测得的相对于ECEF坐标系的载体的横坐标，yI为惯性导航系统测得的相对于ECEF坐标系的载体的纵坐标，zI为惯性导航系统测得的相对于ECEF坐标系的载体的z坐标，xsi为卫星导航和罗兰C导航第i颗卫星t时刻在地球固定坐标系上的横坐标，ysi为卫星导航和罗兰C导航第i颗卫星t时刻在地球固定坐标系上的纵坐标，zsi为卫星导航和罗兰C导航第i颗卫星t时刻在地球固定坐标系上的z坐标。
CN201910422237.1A 2019-05-21 2019-05-21 一种基于增广伪距信息辅助的容错导航方法 Active CN110208843B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910422237.1A CN110208843B (zh) 2019-05-21 2019-05-21 一种基于增广伪距信息辅助的容错导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910422237.1A CN110208843B (zh) 2019-05-21 2019-05-21 一种基于增广伪距信息辅助的容错导航方法

Publications (2)

Publication Number Publication Date
CN110208843A true CN110208843A (zh) 2019-09-06
CN110208843B CN110208843B (zh) 2022-07-22

Family

ID=67787912

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910422237.1A Active CN110208843B (zh) 2019-05-21 2019-05-21 一种基于增广伪距信息辅助的容错导航方法

Country Status (1)

Country Link
CN (1) CN110208843B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111027204A (zh) * 2019-12-05 2020-04-17 中国人民解放军63620部队 航天发射光、雷、遥与导航卫星测量数据融合处理方法
CN113376664A (zh) * 2021-05-25 2021-09-10 南京航空航天大学 一种无人蜂群协同导航多故障检测方法
WO2022037340A1 (zh) * 2020-08-20 2022-02-24 华为技术有限公司 故障检测方法、装置及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107643534A (zh) * 2017-09-11 2018-01-30 东南大学 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN109373999A (zh) * 2018-10-23 2019-02-22 哈尔滨工程大学 基于故障容错卡尔曼滤波的组合导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107643534A (zh) * 2017-09-11 2018-01-30 东南大学 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN109373999A (zh) * 2018-10-23 2019-02-22 哈尔滨工程大学 基于故障容错卡尔曼滤波的组合导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
吴美平 等: "惯导/GNSS/罗兰C/航姿系统组合导航方案", 《中国惯性技术学报》 *
林雪原 等: "GPS/罗兰C/SINS/AHRS组合导航系统及试验", 《电子科技大学学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111027204A (zh) * 2019-12-05 2020-04-17 中国人民解放军63620部队 航天发射光、雷、遥与导航卫星测量数据融合处理方法
CN111027204B (zh) * 2019-12-05 2023-07-28 中国人民解放军63620部队 航天发射光、雷、遥与导航卫星测量数据融合处理方法
WO2022037340A1 (zh) * 2020-08-20 2022-02-24 华为技术有限公司 故障检测方法、装置及系统
CN113376664A (zh) * 2021-05-25 2021-09-10 南京航空航天大学 一种无人蜂群协同导航多故障检测方法

Also Published As

Publication number Publication date
CN110208843B (zh) 2022-07-22

Similar Documents

Publication Publication Date Title
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
Sun et al. Robust IMU/GPS/VO integration for vehicle navigation in GNSS degraded urban areas
CN105737828B (zh) 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法
CN110208843A (zh) 一种基于增广伪距信息辅助的容错导航方法
CN104457754A (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN106405670A (zh) 一种适用于捷联式海洋重力仪的重力异常数据处理方法
CN102252677A (zh) 一种基于时间序列分析的变比例自适应联邦滤波方法
CN104697520B (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
Fu et al. In-motion alignment for a velocity-aided SINS with latitude uncertainty
Sun et al. In-motion attitude and position alignment for odometer-aided SINS based on backtracking scheme
CN104931994A (zh) 一种基于软件接收机的分布式深组合导航方法及系统
CN106199668A (zh) 一种级联式gnss/sins深组合导航方法
CN108151765B (zh) 一种在线实时估计补偿磁强计误差的定位测姿方法
Zuo et al. A GNSS/IMU/vision ultra-tightly integrated navigation system for low altitude aircraft
Wu et al. An experimental evaluation of autonomous underwater vehicle localization on geomagnetic map
Wang et al. Application of gravity passive aided strapdown inertial navigation in underwater vehicles
RU2277696C2 (ru) Интегрированная инерциально-спутниковая навигационная система
CN112697154A (zh) 一种基于矢量分配的自适应多源融合导航方法
Maklouf et al. Performance evaluation of GPS\INS main integration approach
CN115523920B (zh) 一种基于视觉惯性gnss紧耦合的无缝定位方法
Dahmane et al. Controlling the degree of observability in GPS/INS integration land-vehicle navigation based on extended Kalman filter
Iqbal et al. A review of sensor system schemes for integrated navigation
TN et al. Navigation of autonomous underwater vehicle using extended kalman filter

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