CN1361430A - 增强的运动体定位和导航方法与系统 - Google Patents

Abstract

一增强的全耦合运动体定位和导航方法与系统可从根本上解决在只有GPS和只有惯性导航系统中遇到的各种问题,如GPS卫星信号失锁,GPS接收机容易被干扰和欺骗,惯性导航解随时间漂移,来自一惯性导航处理器的速度和加速度用于GPS卫星信号的码和载波相位跟踪来增强组合GPS/IMU的导航性能,甚至于在强干扰和高动态环境下也可得到高性能的导航解。增强的全耦合GPS/IMU运动体定位与导航系统包括一IMU(惯性测量单元)和一GPS处理器,它们都与一中心导航处理器相连,该中心导航处理器输出导航解给一输入/输出(I/O)接口。

Description

增强的运动体定位和导航方法与系统

全球定位系统接收设备包括一个接收天线，一个信号处理单元，以及相关的电子线路和必要的显示设备。全球定位系统接收机接收GPS卫星信号，並导出飞行体如飞机，飞弹，火箭的位置，速度和时间信息。全球定位系统定位是基于测距的原理。卫星的位置可以由卫星的星历表解算出来，而卫星到用户的距离可以由全球定位系统接收机测得的卫星信号时间延迟导出来。卫星信号以光速传播，卫星到GPS接收的几何距离等于卫星信号时间延迟乘以光速。这样测得的几何距离叫做伪距，它包含了很多的误差，例如接收机的时钟误差。因为卫星用的是原子钟，它的误差远远小于GPS接收机的时钟误差。在三维飞行体导航系统中，这个未知的用户接收机的时钟误差也要解出来。因此应用单一的全球定位系统导航需要至少四颗GPS卫星观测量。

全球定位系统的误差源有：信号传播误差，卫星时钟误差和星历表误差，以及选择可用性(Selective Availability)误差。用户距离误差(URE)就是由这些误差源引起的沿用户到卫星视线上的测距误差。全球定位系统误差长时间保持稳定，这就是全球定位系统能提供长时间稳定的导航解的原因。然而全球定位系统的缺点是它很容易被有意或无意地干扰或被欺骗，在飞行体作机动飞行时GPS接收机的天线也有可能被遮挡而导致接收不到卫星信号，当卫星信号噪声比低和飞行体作大机动飞行时也可能导致卫星信号的失锁。

一惯性导航系统由一惯性测量单元，一个处理器，和一套嵌入式的导航软件组成。定位解通过数值解由飞行体的比力和转动速率建立的Newton运动方程，比力和转动速率由惯性传感器测定。惯性传感器包括加速度计和陀螺仪，它们连同相关的硬体和电子线路一起组成惯性测量单元。

惯性导航系统包括框架式和捷联式两种。在框架式惯性导航系统中加速度计和陀螺仪被固定在一框架平台上以隔离传感器，从而保证观测量和导航计算都处于一稳定的导航座标系中。可能的导航座标系包括地心惯性座标系(Earth CenteredIntertail-ECI)，地心地固座标系(Earth Centered Earth Fixed-ECEF)，北东下局部座标系(North East Down-NED)，游移方位局部座标系。在捷联惯性导航系统中，惯性传感器被钢性地连接在飞行体上，而一座标系转换矩阵则用于将表达为机体的加速度和转动观测量转换为导航座标系中相应的量，並在一稳定的导航座标系中作导航计算。这个座标系转换矩阵又被称为分析平台。框架式惯性导航系统可以比捷联式导航系统更精密，更易于校正。捷联式导航系统受制于高机动环境，例如高速率的转动。高机动可能影响惯性传感器的性能。随著新的陀螺仪和加速度计的出现，捷联式导航系统越来越占主导地位，因为它的成本低，同时性能可靠。

从原理上讲，惯性导航系统允许纯粹自动操作，在进行起点位置初始化和姿态初始对准后惯性导航系统输出连续的飞行体的位置，速度和姿态信息。除了自动化操作外，惯性导航系统的优点还包括全导航解和宽的带宽。然而惯性导航系统价格昂贵，其导航解並随时间漂移。其含义是惯性导航系统的导航误差随时间不断地增加，这种误差特性主要由惯性传感器误差源引起，例如陀螺仪漂移，加速度计偏差，和刻度因子误差。

单独的惯性导航系统和单独的全球定位系统都存在各自的缺点，这些缺点表明在一些限制条件下，例如低成本，高精度，连续导航解输出，抗干扰能力，和适应大机动等，独立的一个导航系统不能满足任务要求。

对于全球定位系统与惯性系统组成的组合导航系统来说，这两个导航系统互相辅助，利用惯性导航系统的短时导航高精度弥补全球定位系统的导航误差，利用全球定位系统的长时间的高精度导航弥补惯性导航系统的误差随时间增长的缺点。

全球定位系统和惯性导航系统的一种简单的组合方式就是应用全球定位系统接收机的位置和速度直接重调惯性系统。第二种组合方式为全球定位系统和惯性系统的级联组合。在这种组合方式中全球定位系统导出的位置和速度做为观测量，一组合Kalman滤波器融合来自惯性导航系统和全球定位接收机的导航解。第三种全球定位系统和惯性导航系统的组合方式为应用一扩展的Kalman滤波器处理全球定位系统接收机的伪距和伪距率等原始观测量以及惯性导航系统的信息，从而得到最优的惯性导航系统误差，惯性传感器误差和GPS接收机钟偏移的估计。

目前的全球定位系统和惯性导航系统的组合系统存在的缺点有：

1在目前的全球定位系统和惯性导航系统的组合系统中仅有GPS接收机的位置和速度或伪距和伪距率被用来做组合滤波。实际上GPS接收机测得的载波相位具有更高的精度，然而它没有被充分地被利用起来。

2用惯性导航系统辅助GPS接收机跟踪环路的一个隐患是这可能引起传统的GPS/INS组合导航系统的不稳定，其原因是在GPS/INS组合系统中存在一个正反馈回路，精确度不高的惯性辅助信息会增大GPS信号跟踪误差。因为组合Kalman滤波器是根据已知惯性系统的性能精调的，GPS观测量会严重地影响组合Kalman滤波器的性能，因而增大后的GPS跟踪误差又反馈回惯性系统引起惯性系统的性能进一步变坏。

3传统的紧密型GPS/INS组合导航系统中，低精度的惯性传感器不能用于GPS接收机的载波相位跟踪辅助，因为载波跟踪回路要求高精度的外来输入的速度辅助。

本发明的第一个目的是提供一全耦合的定位和导航方法与系统，使得来自惯性导航处理器的速度和加速度用于辅助GPS接收机的码和相位跟踪环，从而提高GPS/INS组合导航系统的性能。

本发明的进一步目的是提供一全耦合的定位和导航方法与系统，使得来自惯性导航处理器的速度和加速度用于辅助GPS接收机的码和相位跟踪环，从而提高GPS/INS组合导航系统的在强干扰和高动态环境下的性能。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，使得来自经一组合Kalman滤波器校正的惯性导航处理器的速度和加速度用于辅助GPS接收机的码和相位跟踪环，从而提高GPS/INS组合导航系统的性能。

本发明的另一目的是提供一全耦合的定位和导航方法与系统，使得来自经一组合Kalman滤波器校正的惯性导航处理器的速度和加速度用于辅助GPS接收机的码和相位跟踪环，从而提高GPS/INS组合导航系统的在强干扰和高动态环境下的性能。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，使得来自惯性导航处理器的速度和加速度用于辅助GPS接收机的码和相位跟踪环，从而避免在GPS接收机中常常遇到的卫星信号失锁和载波信号周跳的发生。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，使得来自经一组合Kalman滤波器校正的惯性导航处理器的速度和加速度用于辅助GPS接收机的码和相位跟踪环，从而避免在GPS接收机中常常遇到的卫星信号失锁和载波信号周跳的发生。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，它允许惯性导航系统通过提供更精确的位置信息来辅助GPS的卫星信号整周模糊度求解。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，它允许GPS/INS组合的导航解用于辅助GPS的卫星信号整周模糊度求解。因为GPS/INS组合系统提供精确的位置信息和误差的协方差矩阵。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，它允许卫星信号载波相位信息与GPS接收机的码伪距和delta距离一起用于组合Kalman滤波，从而提高组合定位与导航系统的精度。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，它利用一实时Kalman滤波器来最优地融合GPS接收机的原始观测量和惯性导航系统的导航解，从而导出最优的组合导航解。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，它应用一稳健的实时Kalman滤波器来消除组合导航系统中可能存在的不稳定现象。

本发明的另一个目的是提供一全耦合的定位和导航方法与系统，因为使用GPS接收机的观测量来辅助惯性导航系统，它使得低精度的惯性传感器可用于组合系统中来获得高精度的导航解。

第一图是一方块图，说明一全耦合定位与导航方法和系统，在这个全耦合定位与导航方法和系统中，一中心导航处理器用于融合GPS观测量和惯性观测量。

第二图是一根据上述本发明的优选实现方案的全球定位系统处理过程的方块图，说明使用中心导航处理器作GPS接收机的外来辅助。

第三图是一根据上述本发明的优选实现方案的全球定位系统信号处理过程的方块图，说明使用中心导航处理器作GPS接收机的数字信号处理的辅助。

第四图是一根据上述本发明的优选实现方案的中心组合导航处理过程的方块图。

第五图是一根据上述本发明的优选实现方案的惯性导航系统的处理过程方块图，该惯性导航系统接收来自Kalman滤波器的导航状态误差校正用于校正导航解。

第六图是一根据上述本发明的优选实现方案的稳健的Kalman滤波器的方块图

第七图是一根据上述本发明的优选实现方案的全球定位系统卫星信号载波相位模糊度解的方块图，其中惯性导航系统用于模糊度求解的辅助。

本发明是一全耦合的定位与导航处理方法和系统，它能从根本上解决在仅有GPS的导航系统和仅有惯性导航系统中所遇到的问题，例如卫星信号丢失，易被干扰和欺骗，以及惯性导航系统的导航解随时间漂移等。本发明还包括一Kalman滤波器，它用于融合来自GPS接收机的原始观测数据和惯性传感器观测数据。本发明进一步用一稳健的Kalman滤波器来融合来自GPS接收机的原始观测数据和惯性传感器观测数据。这个稳健的Kalman滤波器可从根本上解决在很多组合导航系统中遇到的不稳定性。

本发明支持航空和航太的高精度导航，以及飞弹，制导武器系统的导航与制导。它也可用于地面运动体的跟踪和导航应用。

如第一图所示一全耦合GPS/IMU定位与导航系统包括一IMU10，一GPS处理器20，一与它们相连接的中心导航处理器30。导航解通过一输入/输出接口40输出。全耦合GPS/IMU组合定位与导航系统包含如下步骤：

(1)从GPS处理器20接收GPS观测量，它们是伪距，载波相位和多卜勒频率，並把它们送给中心导航处理器30。从惯性测量单元10接收惯性观测量，它们是机体的角速率和比力，並把它们送给中心导航处理器30的惯性导航处理器31。

(2)用Kalman滤波器33融合惯性导航系统的输出和GPS观测量得到惯性校正量。

(3)将Kalman滤波器33输出的惯性校正量反馈给惯性导航系统处理器31来校正惯性导航系统的导航解。

(4)将来自惯性导航系统31的速度和加速度信息注入GPS处理器20的微处理器254来辅助GPS卫星信号的码和载波相位的跟踪。

(5)将GPS处理器20的微处理器254的输出，惯性导航系统处理器31的输出和Kalman滤波器33的输出一起注入一载波相位整周模糊解模块32来确定GPS卫星信号载波相位整周模糊值。

(6)从载波相位整周模糊解模块32输出载波相位整周模糊值给Kalman滤波器更进一步地提高定位和导航精度。

(7)从惯性导航处理器31输出导航解给输入/输出接口40。其它的机载航空电子系统可以方便地从该输入/输出接口40获得实时的导航数据。

GPS卫星在L波段射频1上广播粗捕获码(C/A)和精确码(P)，该GPS卫星信号为： $S_{11}\left(t\right)=\sqrt{2P_c}\mathrm{CA}\left(t\right)D\left(t\right)\cos ({\mathrm{\ω}}_{1}t+\mathrm{\φ})+\sqrt{2P_p}P\left(t\right)D\left(t\right)\sin ({\mathrm{\ω}}_{1}t+\mathrm{\φ})$ GPS卫星在L波段射频2上广-蝶踳T码(P)，该GPS卫星信号为： $S_{12}\left(t\right)=\sqrt{2P_2}P\left(t\right)D\left(t\right)\cos ({\mathrm{\ω}}_{2}t+{\mathrm{\φ}}_2)$

其中：ω₁是L波段上射频1的载波频率；φ是一小量，它是相位噪声和晶体振荡器漂移的和；P_c是C/A码信号功率；P_p是P码信号功率；D(t)是调制的导航数据；CA(t)是C/A码；P(t)是P码；ω₂是L波段射频2的载波频率；P₂是P码信号功率；φ₂是一小量，它是相位噪声和晶体振荡器的漂移的和。

GPS接收机的GPS天线31接收到的卫星信号为： $S_{11}\left(t\right)=\sqrt{2P_c}\mathrm{CA}(t-\mathrm{\τ})D\left(t\right)\cos [({\mathrm{\ω}}_1+{\mathrm{\ω}}_d)t+\mathrm{\φ})\left]\sqrt{2P_p}P\left(t\right)D\left(t\right)\sin \right[({\mathrm{\ω}}_1+{\mathrm{\ω}}_d)t+\mathrm{\φ}\left)\right]$ $S_{12}\left(t\right)=\sqrt{2P_2}P(t-\mathrm{\τ})D\left(t\right)\cos [({\mathrm{\ω}}_2+{\mathrm{\ω}}_d)t+{\mathrm{\φ}}_2)]$

其中：τ是码延迟；ω_d是多卜勒频移。

参考第二图，接收到的GPS信号由一前置放大器电路22放大。被放大了的GPS信号送给GPS处理器20的下变频器23。下变频器23转换射频(RF)信号为中频(IF)信号，中频信号再由IF采样和A/D转换器24转换为正交的I(in-phase)和Q(quadraphase)分量数据。在IF采样和A/D转换器24中，中频信号先由一低通滤波器滤波，然后被采样，最后才由模拟信号转换为数字数据。数字数据输入一信号处理器25，信号处理器25从这些数据中解调出导航数据，它们是GPS卫星星历表，大气层参数，卫星钟参数和时间信息。信号处理器25还从这些数字数据中导出伪距，载波相位和多卜勒频率。在GPS处理器20中，一振荡器电路26提供时钟信号给下变频器23，IF采样和A/D转换器24和信号处理器25。

伪距观测量从GPS接收机的码跟踪环路导出，如第三图所示，码跟踪环路包括一相关器252，一累加器253，一微处理器254，一码的数控振荡(numericalcontrolledoscillator-NCO)器257和一码产生器256。多卜勒频移和载波相位由GPS卫星信号的载波跟踪环路得到。载波跟踪环路包括一多卜勒消去器251，一相关器252，一累加器253，一微处理器254和一载波数控振荡器255。

来自IF采样和A/D转换器24的I和Q数据由多卜勒移去器251处理，从而移去调制在GPS信号上的多卜勒频率。多卜勒频率移去器由一数字单边带调制器实现。基于频率数输入，载波NCO255以其时钟速率积累相位。每次当积累器满时产生一新的周期。来自振荡器电路26的时钟和来自微处理器254的delta频率一起驱动载波NC0255。载波NCO255输出参考信号的I和Q分量(即Iref和Qref)给多卜勒移去器251。

经多卜勒移去处理后的GPS卫星信号送入相关器252与本地产生的伪随机码作相关处理。累加器253紧跟相关器252之后，它对相关器252的输出作相关处理並滤波。累加处理即将T时间间隔中的相关后的数据累加起来，T通常取为C/A码的一个码片长。累加后的数据(I3和Q3)送给微处理器254，接著倒空累加器，这即为信号的累加-倒空滤波。

用于相关器252中作相关运算的本地码由码产生器256产生，而码产生器256则由来自振荡器电路26的时钟和来自微处理器254的delta延迟驱动。码产生器256用于产生C/A码和P码。码NCO257由振荡器电路26和微处理器254驱动，码NCO257输出的时钟再驱动累加器253。码NCO257还驱动码产生器256。

微处理器有两种工作模式：(1)接收来自累加器253的数据並对其作滤波处理，信号搜捕处理，锁住检测，数据恢复和观测量处理。(2)从累加器253接收数据和从导航处理器30接收速度和加速度信息並进行环路滤波处理信号搜捕处理，锁住检测，数据恢复和观测量处理。第二种工作的模式称为速度加速度辅助的载波相位跟踪和码相位跟踪。

当GPS信号跟踪误差大于信号跟踪环路的跟踪带宽时卫星信号会丢失。跟踪环路丢失对卫星信号的跟踪主要由低的信号噪声比(SNR)和接收到的卫星信号多卜勒频移引起。前者主要由输入噪声和干扰产生，而后者则主要由飞行体的高速运动引起。一般来说，扩展跟踪环路的带宽可以该善锁相环在高机动环境中的跟踪性能。这种方法同时引入更多的噪声从而降低了GPS接收机的抗干扰的能力。利用校正后的惯性导航系统解辅助GPS接收机的跟踪环路既可提扩展跟踪环的跟踪带宽，同时又能提高其抗干扰的能力。

参考微处理器的第二种工作模式，被校正后的惯性导航系统的速度和加速度信息辅助GPS锁相环的目的是在足够短的时间内对中频信号的相位作精确的估计。这个中频信号的载波相位可以近似为：

       θ₁(t)＝θ₁₀+ω₁₀t+γ₁₀t²+δ₁₀t³+…

于是问题变为估计上述等式的参数。将描述飞行体运动的速度和加速度投影到卫星视线(LOS)方向上，于是中频信号的载波相位可以由视线方向上的速度和加速度表示为： $\widehat{\mathrm{\θ}}\left(t\right)=b_1{V}^{\mathrm{LOS}}t+b_2{A}^{\mathrm{LOS}}{t}^2+b_3{a}^{\mathrm{LOS}}{t}^3+\·\·\·$ 其中：(b₁，b₂，b₃)是与载波频率和光速有关的常量，它们为： $b_1=\frac{4{\mathrm{\πf}}_c}{c},b_2=\frac{2{\mathrm{\πf}}_c}{c},b_3=\frac{4{\mathrm{\πf}}_c}{3c}$

V^LOS，A^LOS和a^LOS分别对应于沿接收机到卫星的距离率，距离加速度和距离加加速度，因此辅助的GPS接收机的卫星信号跟踪性能和抗干扰的能力依赖于距离率V^LOS和距离加速度A^LOS的估计的精确度。视线方向上的距离率V^LOS和距离加速度A^LOS可以由惯性导航系统中的速度和加速度信息计算出来，並把它们注入微处理器254的环路滤波器中。

信号处理器25的码跟踪环路跟踪输入的直接序列扩展频谱信号的码相位。码跟踪环提供卫星信号时延的估计，该信号时延在微处理器254中被用来计算飞行体到卫星的距离估计，它即为伪距(pseudorange)。来自中心导航处理器30的速度和加速度信息转换为视线方向上的速度和加速度信息(V^LOS和A^LOS)。视线上的速度和加速度用来辅助确定信号码的时延，从而增强GPS接收机的动态性能和抗干扰能力。

参考第4图，中心导航处理器30接收来自IMU10的观测量和来自GPS处理器20的微处理器254的观测量，並融合这些数据来导出高精度的导航信息，它们是三维姿态，三维位置和三维速度。这些数据由中心导航处理器30的惯性导航处理器31输出，並送给输入/输出接口40。其它机载航空电子系统可以从该输入/输出接口40获得导航数据。如前所述，速度和加速度也被反馈回GPS处理器20的微处理254来辅助GPS卫星信号的码和相位跟踪。

GPS处理器20的微处理器254输出伪距，多卜勒频移，GPS卫星星历表，以及大气参数给卡尔曼滤波器。在卡尔曼滤波器中来自惯性导航系统处理器31和GPS处理器20的微处理器254的数据被融合，从而导出惯性导航系统的位置误差，速度误差和姿态误差。惯性导航系统处理器31处理惯性测量值，它们是机体角数率和比力，和来自卡尔曼滤波器33的位置误差，速度误差和姿态误差，从而导出校正后的导航解。导航解包括三维位置，三维速度和三维姿态。这些导航数据一方面送入卡尔曼滤波器33用于同GPS数据作组合，另一方面送给输入/输出接口40，以便于其他机载航空电子系统获得导航信息。

参考第5图，惯性导航系统处理器31包含一IMU输入/输出接口311，一IMU误差补偿模块312，一坐标转换模块313，一姿态位置速度计算模块314，一转换矩阵计算模块315和一地球和运动体转动速率计算模块316。

IMU输入/输出接口311接收来自IMU10的机体角速率和比力信号，並将它们转换为数字数据。这些数据实际上带有测量误差。带有测量误差的这些数据送给IMU误差补偿模块312。IMU误差补偿模块312同时从卡尔曼滤波器33接收传感器误差估计，並对IMU数据作IMU传感器误差消除处理。校正后的惯性数据被送给坐标转换313和转换矩阵计算模块315。更深而言之，校正后的机体转动角速率送给转换矩阵计算模块315，而校正后的比力送给坐标转换模块313。

转换矩阵计算模块315接收来自IMU误差计算模块312的机体转动角速率和来自地球和运动体速率计算模块316的地球和飞行体速率，並进行转换矩阵的计算。转换矩阵又被送给坐标转换模块313和姿态位置速度计算模块314。因为四元素方法具有高效率的数值计算和稳定性等特征，它被用在转换矩阵计算模块315中的姿态更新算法中以描述刚体的转动。从机体系到导航系的四元素微分方程为： $\stackrel{\·}{q}=1/2\left[{\mathrm{\Ω}}_b\right]q-1/2\left[{\mathrm{\Ω}}_n\right]q$

其中q^T＝[q₀ q₁ q₂ q₃]是四元素参量的四个分量；Ω_b是ω_ib ^b的反对称矩阵，ω_ib ^b由陀螺仪测得，它是在机体系中机体相对于惯性系的旋转速率向量。 $\left[{\mathrm{\Ω}}_b\right]=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bx}}& 0\end{array}\right]$ ${\mathrm{\ω}}_{\mathrm{ib}}^b={[{\mathrm{\ω}}_{\mathrm{bx}},{\mathrm{\ω}}_{\mathrm{by}},{\mathrm{\ω}}_{\mathrm{bz}}]}^T$ Ω_n是向量ω_in ⁿ的反对称矩阵，ω_in ⁿ是在导航系中导航坐标系到惯性系的旋转速率向量。 $\left[{\mathrm{\Ω}}_n\right]=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nx}}& -{\mathrm{\ω}}_{\mathrm{ny}}& -{\mathrm{\ω}}_{\mathrm{nz}}\\ {\mathrm{\ω}}_{\mathrm{nx}}& 0& {\mathrm{\ω}}_{\mathrm{nz}}& -{\mathrm{\ω}}_{\mathrm{ny}}\\ {\mathrm{\ω}}_{\mathrm{ny}}& -{\mathrm{\ω}}_{\mathrm{nz}}& 0& {\mathrm{\ω}}_{\mathrm{nx}}\\ {\mathrm{\ω}}_{\mathrm{nz}}& {\mathrm{\ω}}_{\mathrm{ny}}& -{\mathrm{\ω}}_{\mathrm{nx}}& 0\end{array}\right]$ ${\mathrm{\ω}}_{\mathrm{in}}^b=[{\mathrm{\ω}}_{\mathrm{nx}},{\mathrm{\ω}}_{\mathrm{ny}},{\mathrm{\ω}}_{\mathrm{nz}}{]}^T$ 如果导航坐标系是北东下(NED)坐标系，则有： ${\mathrm{\ω}}_{\mathrm{in}}^n=\left[\begin{array}{c}({\mathrm{\ω}}_e+\stackrel{\·}{\mathrm{\λ}})\cos L\\ -\stackrel{\·}{L}\\ -({\mathrm{\ω}}_e+\stackrel{\·}{\mathrm{\λ}})\sin L\end{array}\right]$

其中ω_e是地球旋转速率，L是地理纬度，λ是地理经度。

坐标转换模块313从IMU误差计算模块312接收比力，从转换矩阵计算模块315接收转换矩阵来进行坐标变换计算，並将转换到由转换矩阵表征的坐标系中的比力送给姿态位置速度计算模块314。

姿态位置速度计算模块314从坐标转换计算模块313接收转换后的比力，从转换矩阵计算模块315接收转换矩阵，並进行姿态位置和速度更新。描述在地球表面或近地面的质点运动的方程为：

     (t)＝a-(2ω_ie+ω_en)×V-ω_ie×ω_en×r

其中：a和V是导航坐标系中飞行体相对地球的加速度和速度，ω_ie是地球旋转向量，ω_en是导航系相对于地球的角速率，r是飞行体相对地球中心的位置向量。

因为加速度计不区分飞行体加速度和质量之间的引力，由加速度计测量得到的比力可以表达为：

          f＝a-g(r)其中，g(r)是地球引力和飞行体离心力之和。由此有：

    (t)＝f-(2ω_ie+ω_en)×V+g(r)其中， ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}{\mathrm{\ω}}_e\cos L\\ 0\\ -{\mathrm{\ω}}_e\sin L\end{array}\right]$ ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}\stackrel{\·}{\mathrm{\λ}}\cos L\\ -\stackrel{\·}{L}\\ -\mathrm{\λ}\sin L\end{array}\right]$ 飞行体速度由下式更新 $\stackrel{\·}{V^n}=C_b^n{f}^b+{\mathrm{MV}}^n+g^n$ 其中，C_b ⁿ是从机体系到导航系的方向余弦矩阵，并有： $V^n=\left[\begin{array}{c}{\mathrm{\ν}}_n\\ {\mathrm{\ν}}_e\\ {\mathrm{\ν}}_d\end{array}\right],f^b=\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right],g^n=\left[\begin{array}{c}0\\ 0\\ g_d\end{array}\right],$ $M=\left[\begin{array}{ccc}0& -({2\mathrm{\ω}}_e+\stackrel{\·}{\mathrm{\λ}})\sin L& \stackrel{\·}{L}\\ ({2\mathrm{\ω}}_e+\stackrel{\·}{\mathrm{\λ}})\sin L& 0& ({2\mathrm{\ω}}_e+\stackrel{\·}{\mathrm{\λ}})\cos L\\ -\stackrel{\·}{L}& -({2\mathrm{\ω}}_e+\stackrel{\·}{\mathrm{\λ}})\cos L& 0\end{array}\right]$ 由WGS-84椭球体的一般公式可以导出 $g_d=g_0[1-2(1+f+m)\frac{h}{a}+(\frac{5}{2}m-f){\sin }^{2}L]$ $(m={\mathrm{\ω}}_{\mathrm{ie}}^2{a}^{2}b/\mathrm{GM})$

其中，g₀是中赤道的重力加速度，f是椭球体扁平率，h是高度，a是长半轴，b是短半轴，GM是地球重力常数。用于地理坐标更新的差分方程为： $\stackrel{\·}{L}=\frac{V_n}{R_M+h},\stackrel{\·}{\mathrm{\λ}}=\frac{V_e}{(R_N+h)\cos L},\stackrel{\·}{h}=-{\mathrm{\ν}}_d$

其中，R_M是经线方向上曲率半径，R_N是主垂线半径。

由卡尔曼滤波器33计算得到的位置和速度误差被姿态位置速度计算模块314用来校正。第一种方法是将卡尔曼滤波器33得到的姿态误差送给姿态位置速度计算模块314来进行直接的姿态校正。第二种方法是在进行姿态计算之前，将由卡尔曼滤波器33得到的姿态误差送给转换矩阵计算模块315进行姿态校正。校正后的惯性解送给卡尔曼滤波器33，用它们来生成卡尔曼滤波器的观测值。校正后的惯性解同时送给载波相位整周模糊解模块32来辅助确定GPS卫星载波相位整周模糊值。校正后的速度和加速度送给GPS处理器20的微处理器254来辅助GPS卫星信号的载波相位和码跟踪。姿态，位置和速度信息也送给输入/输出接口40。它为其它机载航空电子系统提供导航数据。

由姿态位置速度计算模块314得到的姿态，位置和速度送给地球和运动体速率计算模块316来计算地球旋转速率和飞行体旋转速率。这些速率值再送给转换矩阵计算模块315。

在统计特性已知情况下，卡尔曼滤波器产生最优解。这些最优解是无偏的，在线性无偏估计值中它们具有最小的方差。估计的质量只有当数学模型正确的情况下能得到保证，任何与模型之间的失配都可能导致滤波器性能变坏。

在增强的GPS/IMU组合定位与导航系统中，一稳健的卡尔曼滤波器可用于位置姿态计算。该稳健的卡尔曼滤波器足够的稳定，以保证可在不同的动态环境下工作。如果动态环境改变了，或者是传感器发生故障，例如GPS卫星信号故障或惯性传感器故障，该滤波器能检测，识别和隔离故障。

一稳健滤波器能对一组过程和观测模型提供次优解。纯粹的卡尔曼滤波器不够稳健是因为它是通过精调后对某一特殊的过程和观测模型提供最优解。滤波器的完整性是用于保证从误差协方差预测的导航性能接近真实估计的误差特性。另外，滤波器的发散通常由变化的过程或观测模型或传感器故障引起。

本发明用残差监视的方法来获得稳健的卡尔曼滤波器，该滤波器用于GPS原始数据和惯性数据的融合。当合适的冗余可用时，残差监视方法的一个好处就是当滤波器模型正确时残差序列的统计特性分布是已知的。这样，可以很容易地用特性分布试验于观测量残差来生成故障检测方案。同样的统计特性可用于评价滤波器的调谐，当检测到发散时调整协方差的大小。第6图给出了一包含残差监视功能的稳健的卡尔曼滤波器的实现。

如图六所示，一GPS误差补偿模块337从GPS处理器20搜集GPS原始观测量，包括伪距，载波相位，和多卜勒频移，从状态向量更新模块339接收位置和速度校正来进行GPS误差补偿。校正后的GPS原始数据送给预处理模块335。

预处理模块335从GPS处理器30接收GPS卫星星历表，从GPS误差补偿模块337接收校正的GPS原始数据，包括伪距，载波相位和多卜勒频移，从惯性导航处理器31接收惯性导航解，並进行状态传输矩阵的计算，同时将它和前述状态向量送给一协方差传播模块332。预处理模块335计算观测矩阵，並根据计算得到的观测矩阵和观测模型计算当前观测向量。观测矩阵及当前观测向量送给一观测量残差计算模块338。

状态向量预测模块336从预处理模块335接收状态传输矩阵和前一时刻状态向量，並进行当前状态预测。预测的当前向量送给观测量残差计算模块338。

观测量残差计算模块338从状态向量预测模块336接收预测的当前状态向量，从预处理模块335接收观测矩阵和当前观测向量。通过预测的当前状态向量与观测矩阵之乘积与当前观测向量相减得到观测量残差。观测量残差送给残差监视模块331和状态向量更新模块339。

残差监视模块331进行残差判别，判别的准则是观测量残差的平方被残差方差相除后的值是否大于一给定门限值。如果观测量残差的平方被残差方差相除后的值大于该给定门限，则当前的观测量可能导致卡尔曼滤波器的发散。如果这样，残差监视模块331计算一新的系统协方差，或拒绝接收当前的观测量。如果观测量残差的平方被残差方差相除后的值小于该给定门限，则当前的观测量应用于卡尔曼滤波，尔不需改变系统当前的协方差来获取当前的导航解。系统协方差送给协方差传播模块332。

协方差传播模块332从残差监视模块331接收系统协方差，从预处理模块335接收状态传输矩阵，以及前一时刻的估计误差的协方差，並计算当前估计误差的协方差。计算得到的估计误差的协方差送给最优增益计算模块333。

最优增益计算模块333从协方差传播模块332接收估计误差的协方差，並计算最优增益。该最优增益送给协方差更新模块333，同时也送给状态向量更新模块339。协方差更新模块334更新估计误差的协方差，並将它送给协方差传播模块332。

状态向量更新模块339从最优增益计算模块333接收最优增益，从观测量残差计算模块接收观测量残差，並计算状态向量的当前估计值，它们包括位置，速度和姿态误差。这些误差值送给GPS误差补偿模块337和惯性导航处理器31。

应用GPS载波相位观测量可以获得比应用伪距观测量更高的定位精度，这是因为GPSL1广播频率为1575.42MHz，其对应的载波波长为19厘米，然尔C/A码的一个码片长约300米。然尔高精度的GPS载波相位定位是基于载波相位模糊值已经已知的前提下。载波相位模糊值依赖于GPS接收机和GPS卫星。在理想的假设下，即没有观测误差，接收机和卫星的位置都精确已知，则该相位模糊可以由一简单的算术方法得到。因为各种误差的存在，如卫星星历表误差，卫星钟偏差，大气传播延迟误差，多路径效应，接收机钟差，以及接收机噪声，不可能得到精确的从GPS接收机到卫星的精确几何距离。该不精确几何距离叫做伪距。

IMU辅助载波相位模糊度求解和周跳检测的好处在于来自校正过的惯性导航解的精确飞行体的位置和速度信息可用于辅助确定初始的整周模糊值和收索空间。另外，惯性导航系统辅助信号跟踪加强了接收机锁定GPS卫星信号的能力，这样就减少了信号失锁和周跳发生的可能性。

参考第4图，载波相位整周模糊解模块32从惯性导航处理器31接收位置和速度数据，从GPS处理器20的微处理器254接收接收载波相位和多普勒频移观测量，从卡尔曼滤波器33接收协方差矩阵，来求解GPS卫星信号整周模糊值。在整周模糊值确定后，它被送给卡尔曼滤波器来进一步提高GPS观测量精度和组合导航解的精度

IMU辅助GPS卫星信号载波相位整周模糊解由第7图给出，它包括一几何距离计算模块321，最小方差调整模块322，一卫星钟模型323，一电离层模型324，一对流层模型325，一卫星预报模块326，和一收索空间确定模块327。

GPS卫星信号载-i相位模糊度的一个基本特征是当保持卫星信号跟踪不间断的情况下，它不随时间改变。载波相位观测量模型为： $\mathrm{\φ}=\frac{1}{\mathrm{\λ}}\mathrm{\ρ}+\mathrm{f\Δ\δ}+N+\frac{d_{\mathrm{eph}}}{\mathrm{\λ}}-\frac{d_{\mathrm{iono}}}{\mathrm{\λ}}+\frac{d_{\mathrm{trop}}}{\mathrm{\λ}}+\mathrm{\ϵ}$

其中，Φ是测得的载波相位；λ是信号波长；ρ是接收机到卫星之间的真实几何距离；f是信号频率；Δδ＝δ^S-δ_R是钟误差；δ^S是卫星钟偏移；δ_R是接收机钟误差；N是载波相位整周模糊值；d_eph是星历表误差引入的距离测量误差；d_iono是由电离层引入的传播误差；d_trop是由对流层引入的传播误差；ε是相位测量噪声。

当双频数据可用时，双频载波相位观测量可用来消除几乎所有的电离层误差。而且IMU辅助载波相位模糊解用于由双频载波相位观测量形成的宽道信号。宽道信号可以表达为：

                         Φ_w＝Φ_L1-Φ_L2其中，Φ_L1是L1通道载波相位观测值；Φ_L2是L2通道载波相位观测值。相应的宽道信号频率和相位模糊值为：

                     f_w＝f_L1-f_L2，N_w＝N_L1-N_L2

载波相位模糊度解的问题更进一步由于卫星信号失锁带来的周跳尔变得复杂化。为保证高精度的导航解，周跳必须检测出来並被修复。有三种原因带来周跳。第一是由于树木，建筑物，桥梁和山脉等对卫星信号的遮挡，这是最为常见的一种引起周跳的原因。第二种引起周跳的原因是低的信噪比(SNR)，它主要来自于坏的电离层环境，多路径影响，接收机高动态运动或低卫星仰角。第三种引起周跳的原因是接收机振荡器。在本发明中，IMU辅助也用于周跳的检测和修复。

卫星预报模块326从GPS处理器20接收可见GPS卫星的星历表数据，並进行卫星位置计算。计算出来的卫星位置信息送给几何距离计算模块321。几何距离计算模块321同时接收来自惯性导航处理器31的飞行体精确位置信息，並由此计算从GPS接收机到卫星之间的几何距离。这个几何距离与由GPS处理器20的码跟踪环路导出的伪距不一样。该计算出来的几何距离送入最小方差调整模块322。

对流层模型325从GPS处理器得到时间标志；並应用一内置的对流层传播延迟模型计算GPS卫星信号的对流层时延，计算出来的对流层时延送给最小方差调整模块322。

电离层模型324从GPS处理器得到时间标志和电离层参数；並由一内置的电离层传播延迟模型计算由电离层引入的时延，计算出来的电离层时延送给最小方差调整模块322。

卫星钟模型323接收GPS卫星钟参数；並计算卫星钟校正量，计算出来的卫星钟校正量送给最小方差调整模块322。

收索空间确定模块327从卡尔曼滤波器33接收观测向量的协方差矩阵，基于该协方差矩阵，收索空间确定模块327导出观测量误差，並确定GPS卫星载波相位整周模糊收索空间。该载波相位整周模糊收索空间也送给最小方差调整模块322。

最小平方调整模块322从几何距离计算模块321接收从飞行体到GPS卫星之间的几何距离，从对流层模型325接收对流层时延，从电离层模型324接收电离层时延，从卫星钟模型323接收卫星钟校正量，由此计算初始的收索原点。最小平方调整模块322也从收索空间确定模块327接收载波相位整周模糊收索空间，然后用一标准的最小平方调整来确定这个载波相位模糊值。

由此可见，本发明提供一增强的全耦合GPS/IMU定位与导航方法和系统，它能切实可行的解决只有GPS的导航系统和只有惯性导航系统中遇到的各种问题，如丢失GPS信号，GPS接收机易于被干扰和欺骗，以及惯性导航系统的导航解随时间漂移，由此可获得如下的特征和优点：

(1)从惯性导航处理器来的速度和加速度用于辅助GPS卫星信号的码跟踪和载波相位跟踪，由此来增强GPS/IMU组合系统的性能，特别是在将电磁干扰和高动态环境下的导航性能。

(2)从惯性导航处理器来的速度和加速度由卡尔曼滤波器进行校正，並用于辅助GPS卫星信号的码跟踪和载波相位跟踪，由此来增强GPS/IMU组合系统的性能，特别是在将强电磁干扰和高动态环境下的导航性能。

(3)从惯性导航处理器来的速度和加速度用于辅助GPS卫星信号的码跟踪和载波相位跟踪，防止在GPS接收机中会遇到的卫星信号失锁和载波相位周跳等现象。

(4)从惯性导航处理器来的速度和加速度由卡尔曼滤波器进行校正，並用于辅助GPS卫星信号的码跟踪和载波相位跟踪，以防止在GPS接收机中会遇到的卫星信号失锁和载波相位周跳等现象。

(5)惯性导航系统通过提供更精确的位置信息来辅助GPS卫星信号整周模糊解。

(6)GPS/IMU组合系统通过提供更精确的位置信息和误差协方差矩阵来辅助GPS卫星信号整周模糊解。

(7)GPS卫星信号的载波相位观测值连同伪距，多普勒数据一起用于卡尔曼滤波以提高组合导航解的精度。

(8)卡尔曼滤波器以实时方式来实现，最优地溶合GPS原始观测数据和惯性导航解，並估计组合导航解。

(9)稳健的卡尔曼滤波器以实时方式来实现来消除组合解中可能存在的不稳定性。

(10)由于GPS观测量辅助，低精度的惯性传感器可用于获得高精度的组合解。

Claims (18)

1. (1)一增强运动体定位与导航系统包括：

   一GPS处理器用于提供GPS观测量，包括伪距，载波相位和多卜勒频移；

   一惯性测量单元(IMU)用于提供惯性观测量，包括机体角速率和比力；

   一中心导航处理器，它与GPS处理器和该IMU相连接，並包括一惯性导航系统(INS)处理器，一卡尔曼滤波器，和一载波相位整周模糊求解模块；和

   一输入/输出(I/O)接口，它与该中心导航处理器相连；

   其中该GPS观测量送给该中心导航处理器，该惯性观测量注入该INS处理器；

   该INS处理器的输出与该GPS观测量在该卡尔曼滤波器中作融合处理；该卡尔曼滤波器的输出反馈给该INS滤波器来校正由该中心导航处理器输出到该I/O接口的INS导航解；

   该INS处理器提供速度和加速度数据给该GPS处理器的微处理器来辅助GPS卫星信号的码和载波相位跟踪；

   该GPS处理器的该微处理器的输出，该INS处理器的输出和该卡尔曼滤波器的输出一起注入该载波相位整周模糊求解模块中来确定GPS卫星信号载波相位整周模糊值；

   该载波相位整周模糊求解模块输出载波相位整周值给该卡尔曼滤波器来进一步提高定位导航精度；和

   该INS处理器输出导航数据给该I/O接口。
2. (2)如申请专利范围第1项所述之一增强运动体定位与导航系统，其中该GPS处理器的该微处理器输出伪距，多卜勒频移，GPS卫星星历以及大气数据给该卡尔曼滤波器。
3. (3)如申请专利范围第2项所述之一增强运动体定位与导航系统，其中该INS处理器处理该惯性观测量，它们是机体角速率和比力，和来自该卡尔曼滤波器的该位置误差，速度误差和姿态误差来导出该校正的导航解。
4. (4)如申请专利范围第3项所述之一增强运动体定位与导航系统，其中该INS处理器包括一IMUI/O接口，一IMU误差补偿模块，一坐标转换计算模块，一姿态位置速度计算模块，一转换矩阵计算模块和一地球和运动体速率计算模块，其中该IMUI/O接口从该IMU接收该机体角速率和比力信号，该机体角速率和比力信号被处理和转换为数字数据，它们实际上带有惯性传感器观测误差，这些带有误差的数字数据送给该IMU误差补偿模块，其中该IMU误差补偿模块从卡尔曼滤波器接收传感器误差估计量並从该IMU数据中消除传感器误差，该校正后的惯性数据送入该坐标转换计算模块和该转换矩阵计算模块，其中该机体角速率送入该转换矩阵计算模块，该比力送入坐标转换计算模块，该转换矩阵计算模块从IMU误差计算模块接收该机体角速率和从该地球和运动体速率计算模块接收地球和运动体速率来进行转换矩阵的计算，该转换矩阵送给该坐标转换计算模块和姿态位置速度计算模块该转换矩阵计算模块中的姿态更新算法应用一四元素方法，该四元素方法具有计算高效和性能稳定的特点，其中该坐标转换模块接收从该IMU误差计算模块接收比力，从该转换矩阵计算模块接收该转换矩阵，並进行该坐标转换，该坐标转换模块将转换成由该转换矩阵表征的该坐标系中的比力送给该姿态位置速度计算模块，该姿态位置速度计算模块接收来自坐标转换计算模块的该比力和来自该转换矩阵计算模块的该转换矩阵，並更新该姿态，位置和速度。
5. (5)如申请专利范围第4项所述之一增强运动体定位与导航系统，在计算出位置和速度以后，由该卡尔曼滤波器计算得到的该位置和速度误差用在该姿态位置速度计算模块中来校正该惯性解。
6. (6)如申请专利范围第5项所述之一增强运动体定位与导航系统，其中由该卡尔曼滤波器计算出来的姿态误差送给该姿态位置速度计算模块来进行姿态校正。
7. (7)如申请专利范围第5项所述之一增强运动体定位与导航系统，其中由该卡尔曼滤波器计算出来的姿态误差送给该转换矩阵模块来进行姿态校正。
8. (8)如申请专利范围第5项所述之一增强运动体定位与导航系统，其中由姿态位置速度计算模块得来的校正后的惯性解送给该卡尔曼滤波器以形成该卡尔曼滤波器的该观测量，同时校正后的惯性解送给该载波相位整周模糊求解模块来辅助确定该GPS卫星信号载波相位模糊值，校正后的速度和加速度送给该GPS处理器的该微处理器来辅助该GPS卫星信号的载波相位和码跟踪，由该姿态位置速度计算模块计算出来的姿态，位置和速度送给地球和运动体速率计算模块来计算地球转动速率和飞行体转动速率，地球转动速率和飞行体转动速率送给该转换矩阵计算模块，该姿态位置和速度信息送给该I/O接口，该I/O接口为其它机载航空电子系统提供导航数据。
9. (9)如申请专利范围第6项所述之一增强运动体定位与导航系统，其中由姿态位置速度计算模块得来的校正后的惯性解送给该卡尔曼滤波器以形成该卡尔曼滤波器的该观测量，同时校正后的惯性解送给该载波相位整周模糊求解模块来辅助确定该GPS卫星信号载波相位模糊值，校正后的速度和加速度送给该GPS处理器的该微处理器来辅助该GPS卫星信号的载波相位和码跟踪，由该姿态位置速度计算模块计算出来的姿态，位置和速度送给地球和运动体速率计算模块来计算地球转动速率和飞行体转动速率，地球转动速率和飞行体转动速率送给该转换矩阵计算模块，该姿态位置和速度信息送给该I/O接口，该I/O接口为其它机载航空电子系统提供导航数据。
10. (10)如申请专利范围第7项所述之一增强运动体定位与导航系统，其中由姿态位置速度计算模块得来的校正后的惯性解送给该卡尔曼滤波器以形成该卡尔曼滤波器的该观测量，同时校正后的惯性解送给该载波相位整周模糊求解模块来辅助确定该GPS卫星信号载波相位模糊值，校正后的速度和加速度送给该GPS处理器的该微处理器来辅助该GPS卫星信号的载波相位和码跟踪，由该姿态位置速度计算模块计算出来的姿态，位置和速度送给地球和运动体速率计算模块来计算地球转动速率和飞行体转动速率，地球转动速率和飞行体转动速率送给该转换矩阵计算模块，该姿态位置和速度信息送给该I/O接口，该I/O接口为其它机载航空电子系统提供导航数据。
11. (11)如申请专利范围第1-10项中任意一项所述之一增强运动体定位与导航系统，其中该卡尔曼滤波器是一稳健的卡尔曼滤波器，它用于对一大组过程和观测量模型提供次优的估计，並融合GPS观测量和该惯性传感器观测量。
12. (12)如申请专利范围第11项所述之一增强运动体定位与导航系统，该稳健的卡尔曼滤波器包括一GPS误差补偿模块，它用于收集来自该GPS处理器的该GPS观测量的该伪距，载波相位和多卜勒频移，和来自一状态向量更新模块的该位置和速度校正来进行GPS误差补偿以形成校正的GPS原始数据，包括伪距，载波相位和多卜勒频移，这些校正后的GPS原始数据送给一预处理模块，其中该预处理模块，从该GPS处理器接收GPS卫星星历数据，从该GPS误差补偿模块接收校正后的GPS原始数据，从该INS处理器接收INS解，並进行状态传输矩阵的计算，並将该状态传输矩阵和该状态向量送给一状态预测模块，该计算出来的状态传输矩阵也送给一协方差传播模块，该协方差传播模块计算一观测矩阵，並根据计算的观测矩阵和一观测模型计算一当前观测向量，该观测矩阵和该当前观测向量送给一观测量残差计算模块，该状态向量预测模块从预处理模块接收该状态传输矩阵和该状态向量来计算当前时刻状态的预测值，该预测的当前状态向量送给该观测量残差计算模块，该观测量残差计算模块同时从预处理模块接收该观测矩阵和该当前观测向量，並将该观测矩阵和该预测的观测量之间的乘积与当前观测向量相减来计算得到观测量残差，该观测量残差送给一残差监视模块和该状态向量更新模块，该残差监视模块对来自该观测量残差计算模块的该观测量残差进行判别，该协方差传播模块从该残差监视模块接收系统的协方差，从预处理模块接收状态传输矩阵，並和估计误差的协方差一起来计算该估计误差的当前协方差，並将它送给一最优增益计算模块，该最优增益计算模块从该协方差计算模块接收估计误差的该当前协方差，並计算最优增益，该最优增益送给一协方差更新模块和该状态向量更新模块，该协方差更新模块更新该估计误差的该协方差，並将其送给该协方差传播模块，该状态向量更新模块从最优增益计算模块接收该最优增益，从该观测量残差计算模块接收该观测量残差，並计算状态向量的当前估计值，包括位置，速度和姿态误差，这些误差量送给该GPS误差补偿模块和该INS处理器。
13. (13)如申请专利范围第1-10项中任意一项所述之一增强运动体定位与导航系统，其中该载波相位整周模糊求解模块从该INS处理器接收位置和速度数据，从该INS处理器的该微处理器接收载波相位和多卜勒频移，从该卡尔曼滤波器接收协方差矩阵来确定该GPS卫星信号整周模糊值。在整周模糊值确定下来后，该载波相位模糊值送给该卡尔曼滤波器来进一步提高该GPS原始数据的精度。
14. (14)如申请专利范围第13项所述之一增强运动体定位与导航系统，其中该载波相位整周模糊求解模块包括一几何距离计算模块，一最小平方调整模块，一卫星钟模型，一电离层模型，一对流层模型，一卫星预报模块，和一搜索空间确定模块，其中该卫星预报模块从该GPS处理器接收可见卫星的星历表並计算卫星的位置，该卫星的位置送给该几何距离计算模块，几何距离计算模块同时从该INS处理器接收飞行体的精确的位置信息，並计算从卫星到飞行体之间的几何距离，该几何距离送给最小平方调整模块，该对流层模型从GPS处理器接收时间标誌，並用一嵌入的对流层延迟模型来计算GPS卫星信号的对流层时延，该计算出来的对流层时延送给该最小平方调整模块，该电离层模型从该GPS处理器接收时间标誌和由GPS卫星广播的电离层参数来计算电离层时延，该电离层时延送给该最小平方调整模块该卫星钟模型接收GPS卫星的时钟参数来计算卫星时钟校正量，该卫星时钟校正量也送给该最小平方调整模块，该搜索空间确定模块从卡尔曼滤波器接收该观测向量的协方差矩阵，基于该协方差矩阵该搜索空间确定模块导出该观测量，並确定GPS卫星信号载波相位整周模糊度搜索空间，该搜索空间送给该最小平方调整模块，该最小平方调整模块从该几何距离计算模块接收从该飞行体到GPS卫星的该几何距离，从该对流层模型接收该对流层时延，从该电离层模型接收该电离层时延，从该卫星时钟模型接收该卫星时钟校正量，並计算出一初始搜索原点，该最小平方调整模块同时也从搜索空间确定模块接收一搜索空间，並用一标准的最小平方调整模块算法来确定该载波相位模糊值。
15. (15)一增强的运动体定位与导航方法包括如下步骤：

    (a)接收一组GPS卫星信号来导出运动体的位置和速度信息和一组GPS原始观测量，包括伪距，载波相位和多卜勒频移；

    (b)将该GPS原始观测量从一GPS处理器送给一中心导航处理器；

    (c)从一惯性测量单元(IMU)接收一组惯性观测量，包括机体角速率和比力；

    (d)从该IMU将该惯性观测量送给该中心导航处理器的一惯性系统(INS)处理器来计算一惯性导航解，包括运动体的位置，速度，加速度和姿态；

    (e)用一卡尔曼滤波器融合来自该INS处理器的惯性导航解和来自该GPS处理器的该GPS原始观测量导出一组INS校正量和GPS校正量；

    (f)将该INS校正量从该卡尔曼滤波器送回给该INS处理器来校正该惯性导航解；和

    (g)将该惯性导航解从该INS处理器送给一I/O接口，来为其它机载航空电子系统提供导航数据。
16. (16)如申请专利范围第15项所述之一增强运动体定位与导航方法，在第(f)步后，还包括将该运动体的速度和加速度从该INS处理器送给该GPS处理器的一微处理器来辅助一组GPS码跟踪环路和GPS载波相位跟踪环路，该GPS处理器的该微处理器输出该GPS原始观测量包括该伪距，该载波相位和该多卜勒频移。
17. (17)如申请专利范围第15项所述之一增强运动体定位与导航方法，在第(f)步后包括如下步骤：

    将来自该GPS处理器的该微处理器的该GPS原始观测量，来自该INS处理器的该惯性导航解，来自该卡尔曼滤波器的该惯性校正量和该GPS校正量送给一载波相位整周模糊求解模块来确定一组GPS卫星信号载波相位整周模糊数；

    将来自该载波相位整周模糊求解模块的该GPS卫星信号载波整周数送给该卡尔曼滤波器来导出进一步提高了的运动体导航解。
18. (18)如申请专利范围第16项所述之一增强运动体定位与导航方法，在第(f)步后包括如下步骤：

    将来自该GPS处理器的该微处理器的该GPS原始观测量，来自该INS处理器的该惯性导航解，来自该卡尔曼滤波器的该惯性校正量和该GPS校正量送给一载波相位整周模糊求解模块来确定一组GPS卫星信号载波相位整周模糊数；

    将来自该载波相位整周模糊求解模块的该GPS卫星信号载波整周数送给该卡尔曼滤波器来导出进一步提高了的运动体导航解。

Images