CN1361433A - 运载体的全融合定位方法 - Google Patents

Abstract

全融合定位方法,不但可用现有硬体,更适合用将来的图片级集成的硬体实现,包括步骤:馈入全球定位***天线来的信号及数据融合模块来的预测伪距和伪距率测量,变换和跟踪该信号,得出伪距和伪距率测量及伪距和伪距率的跟踪误差,输给数据融合模块;接收惯性测量组件运载体来的角速率和加速度信号/数据,解导航方程,获得参考导航参数,包括位置、速度、姿态,输给数据融合模块,融合伪距和伪距率测量及伪距和伪距率的跟踪误差、参考导航参数,获得预测伪距和伪距率测量、参考导航参数误差和惯性传感器误差的最优估计、及最优定位信息。

Description

运载体的全融合定位方法

本发明涉及运转体的定位方法，更具体说，是指一种运载体的全融合定位方法(Full Fusion Positioning Method)，其中来自全球定位***、惯性角速率传感器和惯性加速度计的信息，被融合处理，以便获得高性能的定位数据，满足一些对定位性能要求较高的应用场合，如长期的高精度、高的抗干扰和高动态性能。

仅管只是在最近几十年，运载体的定位***才广泛被人们所熟悉，但它可追溯到很早以前。世界上第一个运载体定位***是二轮指南战车，它是公元200-300年(据传说可能更早)由中国发明的一种自动稳定方向***。它要比磁罗盘问世早一千年，它的操作原理是基于一个基本的现象，当车轮改变方向时，外面车轮走过的距离要比内车轮的多，这个多走的距离是航向变化值的简单数学函数。当航向变化时，由外车轮驱动的齿轮，转动一个水平转台，以便精确地抵销航向的变化，这样无论指南战车的航向如何变化，安装在这个水平转台上的假人的伸开的手臂，总是保持在同样方向上，类似罗盘的指针。随着定位***和其它相关领域技术的不断进步，现已有不同种类的定位***。

随着定位***技术的不断进步，将会出现低成本、体积小、高精度的定位***，这种***具有广泛的商业应用价值。

运转体定位***将会进一步拓宽其应用范围，如用于小汽车、出租车、公共汽车、火车、机器人、采矿、建筑，以及象手持电话的911紧急呼叫的个人移动通信市场的寻呼和数据传送。

对先进的商业车辆跟踪***，汽车导航及路径导引***，以及智能车辆高速公路***来说，要求具有车辆定位能力，是这些***的基本要求。

在世界范围内，正在通过应用信息、通信、定位、控制等技术，发展智能高速公路***。

一般来说，运载体定位方法和***是航程推算方法***(DEADREELONING SYSTEM)，无线电定位***，以及混合***(HYBRIDSYSTEM)。本发明是一种混合的，基于信息融合的运转体定位方法。

基于惯性角速率和惯性角加速度计测量的航程推算***，可以提供运载体的位置{B姿态等信息，它通常包括惯性测量组(INERTIALMEASUREMENT UNIT)和处理机。惯性测量组件包括三个正交安装和多个歪斜配置的陀螺，三个正交安装和多个歪斜的加速度计，以及其它相应的硬体。陀螺用来测量运载体的角速率，加速度计用来测量载体的加速度。

这些部件为稳定一个导航坐标系，并融合载体的角运动，提供必要的信息，在平台式惯性***中，这个导航坐标系是通过框架来实现的，在捷联惯性***中，这个导航坐标系是一个解析平台。

导航处理机处理用来自惯性测量组件的载体加速度和角速率信息。给定初始位置和完成初始对准之后，***可以连续输出位置、速度、姿态等数据。

基于惯性角速率传感器和惯性加速度传感器的航程推算***，通常被称为惯性导航性定位***或惯性参考***。它与其它定位***相比，最大的优点是其完全的自主操作特性和具有较宽的频带。

然而惯性定位***成本昂贵且其误差随时间增加，这主要是由于其传感器误差，如陀螺漂移，加速度计偏置及刻度系数误差引起的。

通常，提高惯性定位***的精度的方法，是采用高精度的惯性传感器和采用外部传感器来辅助惯性定位***。

全球定位***(Global Positioning System，GPS)是一个基于卫星的、全球的、全天候的无线电定位和授时***，它可为无限量的、装有接收设备的用户提供定位服务。

用户须有接收机方可获得全球定位***的服务。一个传统的单天线GPS接收机，通过处理它的码跟踪环路和载波跟踪环路的伪距(Pseudo Range)和伪距率(Delta Range)测量，提供用户精度的三维位置、速度和时间信息，然而没有姿态信息。在正常的情况下，GPS信号的传播误差和卫星误差，包括选择可用性(Selective Availability)，限制了GPS的误差范围。然而GPS信号易受到有意或无意的干扰和欺编，而且在载体姿态机动时，GPS天线易被遮挡。另外，当GPS信号噪声比较低或载体在作高动态的机动时，GPS信号易于丢失，从而导致性能降低。

随着高性能GPS接收机的成本和体积的不断减少，多天线GPS接收机，采用干涉(Interferometric)相位技术，可同时提供位置和姿态信息。这种技术利用GPS载波相位在GPS多天线上的差分测量，获得高精度的相对位置测量，然后将其转换为姿态测量。这种技术的优点是姿态精度可保持长期的稳定性，且成本较低，但仍有带宽窄和易遮挡或干扰的特性，而且要获得三轴的载体姿态测量，须至少配置3个天线，并要求GPS天线之间尽可能分离，以便有足够的姿态分辨率。

由于单独的惯性定位***和单独的GPS接收机的这些内在缺点，需要一个单独的惯性定位***或一个单独的GPS，以满足一些性能要求较高的场合，如低成本、长期高精度，连续的输出等。

由于GPS接收机和惯性定位***具有互补的特性，因此很多应用场合，组合GPS/惯性定位***可以利用两个***各自的优点，能提供连续的导航输出，且具有两个独立分***所不能达到的高性能。自从GPS的概念在1973年提出以来，已有很多关于组合GPS/惯性定位***方面的文献，而且组合GPS/惯性定位***已投入了实际应用。

组合GPS/惯性定位***的优点可概括如下：

1)可以使用惯性定位数据辅助GPS信号跟踪环路，从而提高了其在噪声和动态环境下的跟踪能力。

2)当GPS信号暂时丢失之后，惯性定位***不但可提供定位数据，而且可以降低重新捕获GPS信号所需的搜索时间。

3)当GPS信号可以获得时，惯性定位***的误差和惯性传感器的误差可以被校正，这样当GPS信号丢失之后，惯性定位***可在一段时间内保持高精度的输出信息。

4)借助于载体机构，GPS可以对惯性定位***进行动基座对准，从而可取消惯性定位***在进入正常操作之前所需的静基座对准。

但是上述的性能优点，并非是任何水平的组合GPS/惯性定位***都可达到的。现列举以下几种组合GPS/惯性定位***：

1)使用GPS的位置和速度，重调惯性定位***，这是最简单的一种组合方式。

2)第二种方法被称为级联式(Cascaded Integration)组合GPS/惯性定位***。在多种组合方式中，组合GPS/惯性定位***的卡尔曼滤波器(Kalman Filter)，使用GPS接收机内部的卡尔曼滤波器的输出，作为它的测量，这种方法具有“滤波路驱动滤波器”的结构特点。

3)第三种方法称为紧耦合(Tightly-Coupled)GPS/惯性定位***，在这种组合方法中，组合卡尔曼滤波器直接处理GPS接收机的原始测量值(伪距和伪距率)，获得惯性定位***误差、惯性传感器误差、GPS接收机时钟误差的最优估计。惯性定位***数据用来辅助GPS接收机的信号跟踪环路，用以提高GPS接收机在噪声和高动态环境下的信号跟踪性能。

紧耦合GPS/惯性定位***是努力实现上述组合GPS/惯性定位***潜在优点的结果。尽管紧耦合GPS/惯性定位***已得到了实际应用，但是对于实现GPS/惯性定位***的最优组合来说，传统的紧耦合GPS/惯性定位***仍然有许多不足之处，这主要是由于两***之间数据交换不足所造成的潜在的不稳定。

给传统紧耦合GPS/惯性定位***带来潜在不稳定的原因在于：

1)惯性辅助的GPS信号跟踪环路具有窄的带宽，其时间常数大于GPS/惯性组合卡尔曼滤波器的更新周期，惯性辅助误差被缓慢地滤掉。GPS信号跟踪误差不但与时间相关，而且与组合卡尔曼滤波器中建模的惯性定位***的误差相关。因此，受GPS信号跟踪误差扰动的GPS伪距、伪距率测量的统计特性，与组合卡尔曼滤波对测量的要求是不相容的。

2)在传统的紧耦合GPS/惯性定位***中，存在一个正反馈回路：惯性定位数据下降会增大GPS接收机的跟踪误差，而GPS测量的精度会严重影响低精度惯性定位***调谐的组合卡尔曼滤波器的性能，这样，误差在增大的GPS测量被馈入组合卡尔曼滤波器处理，会进一步导致惯性辅助误差的增大。

除了上述的不稳定性问题，传统的紧耦合GPS/惯性定位***和方法不便于检测和隔离出现故障GPS卫星的信号，这是由于组合卡尔曼滤波器以集中的方式处理所有的信息。

本发明的主要目的是提供一种全融合的定位方法，其中来自GPS、惯性角速率传感器、惯性加速度计的信息，以全融合的方式进行处理以提高其性能，满足一些性能要求较高的应用场合，如长期的高精度，高动态能力，强的抗干扰能力。本发明不但可用现有的硬体实现，而且更适合用现在出现的图片级集成(Wafer ScaleIntegration)的硬体实现。

本发明的另一目的在于提供一种全融合的定位方法，其中采用了全数字、多层信号/数据处理流程。

本发明又一目的在于提供一种全融合的定位方法，其中传统的基于锁相环路的GPS信号跟踪方法，被一种在整个***中实现GPS信号跟踪方法取代。GPS信号的跟踪处理被实现在整个GPS/惯性定位***中，以便克服传统紧耦合GPS/惯性***的潜在不稳定性，增强GPS在高动态和强干抗环境合的信号跟踪能力。

本发明再一目的在于提供一全融合的定位方法，其中一个最大似然估计器和融合滤波器提供GPS的伪距和伪距率的跟踪误差，以便补偿GPS的伪距和伪距率测量中的相关噪声，同时融合之后的最优导航参数用来计算预测的GPS的伪距和伪距率，闭合GPS信号的跟踪环路。

本发明再一目的在于提供一全融合的定位方法，其中融合滤波器不但用来融合GPS和惯性传感器的数据，而且具有GPS信号跟踪环路的环路滤波功能。

本发明再一目的在于提供一全融合的定位方法，其中具有双重功能的融合滤波器使用并行分散卡尔曼滤波器，以便当GPS卫星出现故障时，滤波器能进行重构。

本发明再一目的在于提供一全融合的定位方法，其中采用多层的容差设计以提高***的可靠性。

本发明再一目的在于提供一全融合的定位方法，其中采用了多层的容差设计，便于实现GPS的完善性监测。

为实现上述目的，本发明的方法包括以下步骤：

1)馈入来自GPS天线的GPS信号，以及来自数据融合的预测伪距和伪距率变换和跟踪GPS信号，以获得伪距和伪距率测量及其跟踪误差，将其馈入上述数据融合模块。

2)接收来自惯性测量组件的运载体的角速率和加速度信号，求解惯性导航方程，获得参考的导航参数，包括位置、速度、姿态、将其馈入上述数据融合模块。

3)融合上述伪距和伪距率测量及其跟踪误差，以及参考的导航数据，获得预测的伪距和伪距率，参考导航参数和惯性传感器的误差，以及最优的导航参数。

图1是运载体的全融合定位方法框图，

图2是GPS信号的预处理框图，

图3是GPS信号预处理的数字信号处理模块框图，

图4是惯性测量组件的信号预处理框图，

图5是数据融合模块的一种方法，

图6是数据融合模块的另一方法。

本发明为GPS信号和惯性传感器信号的全融合定位方法，以便提供运载体的连续位置信息。如图1到图6所示，本发明的全融合定位方法包含下列步骤：

1)馈入来自GPS天线(5)的GPS信号，对其进行转换和预处理，以便获得GPS的测量数据，包括伪距、伪距率，将其送入数据融合模块(80)；

2)接收来自惯性测量组件(10)的运载体的角速率和加速度信号，求解惯性导航方程，获得参考的导航数据，包括位置、速度等，并将其送入数据融合模块(80)；

3)融合上述GPS的测量数据和参考惯性导航参数，以获得最优的定位信息。

为了提高***性能，在第1步骤中，GPS信号的跟踪处理采用开环方式，且完整的GPS信号跟踪环路在数据融合模块(80)中闭合，以提高对干扰和载体动态的容限。

为了提高***的性能，在第2步骤中，来自数据融合模块(80)的参考导航数据误差的最优估计用来补偿参考导航数据中的误差。

如图2所示，第1步骤进一步包含以下步骤：

(1-1)由GPS天线(5)接收的L段GPS RF(Radio Frequency)信号，被送入一RF/IF(Intermediate Frequency)转换器(21)，输入的RF信号和来自本地数控振荡器(24)的当地载波信号进行混频，混频之后的信号，经低通滤波被转换为中频信号，并输出给一中频基带变换器(22)。

GPS卫星发射L1波段的GPS RF粗捕获(Coarse Acquisition，C/A)码和精确码(Precision Code，P码)信号，其第i颗GPS卫星发射的L1波段信号是： $S_i^{L1}\left(t\right)=\sqrt{2P_c}\mathrm{CA}{\left(t\right)}_{i}D{\left(t\right)}_i\cos ({\mathrm{\ω}}_{1}t+\mathrm{\φ})+\sqrt{2P_p}P{\left(t\right)}_{i}D{\left(t\right)}_i\sin ({\mathrm{\ω}}_{1}t+\mathrm{\φ})$ 其中：ω₁是L1载波角频率；

φ是小的相位噪声和振荡器漂移部分；

P_c是C/A码信号的功率；

P_p是P码信号的功率；

CA(t)是C/A码；

P(t)是P码；

D(t)是导航数据。

GPS卫星也发射L2波段的RF P码信号。第I颗GPS卫星发射的L2信号可表示为： $S_i^{L2}\left(t\right)=\sqrt{2P_2}P{\left(t\right)}_{i}D{\left(t\right)}_i\cos ({\mathrm{\ω}}_{2}t+{\mathrm{\φ}}_2),$ 其中：ω₂是L2载波角频率；

P₂是P2 P码信号的功率；

φ₂是小的相位噪声和振荡器漂移部分；

P(t)是P码；

D(t)是导航数据。

这些信号以光速传播，被GPS天线(5)接收到的GPS信号可表示为：L1： $S_i^{L1}\left(t\right)=\sqrt{2P_c}C{A}_i\left(t{-\mathrm{\τ}}_i\right)D_i\left(t\right)\cos [({\mathrm{\ω}}_1+{\mathrm{\ω}}_{\mathrm{id}})t+\mathrm{\φ})]+\sqrt{2P_p}{P}_i\left(t\right)D_i\left(t\right)\sin [({\mathrm{\ω}}_1+{\mathrm{\ω}}_{\mathrm{id}})t+\mathrm{\φ}\left)\right]$ L2： $S_i^{L2}\left(t\right)=\sqrt{2P_2}{P}_i(t-{\mathrm{\τ}}_i)D_i\left(t\right)\cos [({\mathrm{\ω}}_2+{\mathrm{\ω}}_{\mathrm{id}})t+{\mathrm{\φ}}_2)],$ 其中：τ是码延时；

  ω_d是多普勒角频率；

  i表示第i颗GPS卫星。

GPS天线(5)接收到的GPS信号被馈入GPS预处理(20)的RF/IF转换器(21)。

(1-2)来自RF/IF转换器(21)的GPS IF信号被IF/基带转换器(22)接收，IF信号与来自本地数控振荡器(24)的信号进行混频，混频后的信号被放大、低通滤波，转换为基带信号。对C/A码信号进行滤波的低通滤波器的带宽是1.023MHz，对P码信号进行低通滤波的滤波器的带宽是10.23MHz。该基带信号被送入A/D转换器(23)。

(1-3)来自IF/基带转换器的基带信号是模拟信号，它被A/D转换器(23)接收，模拟的基带信号被采样，形成数字信号。其采样频率是伪随机信号频率的2倍(C/A码信号的频率是2.1518MHz，P码信号的频率是21.518MHz。)。输出的数字信号被馈入数字信号处理机(25)。A/D转换器输出的第i个卫星的数字L1 CA信号可表示为：r_i(n)＝A_iCA_i[(1+ζ_i)nT_S-ζ_iT_P]cos[(ω_b+ω_id)n+φ₀]+N(n)其中：A：信号幅度；

CA[…]：码速率为R，与GPS时间相比，延时为τ＝ξT_P的±1幅度的PRN信号，其码速率等于(1+ζ)R₀，这里，

，F_L是GPSRF信号载波频率，R₀是多普勒频移(Doppler Frequency)的码速率；

Ts是采样周期；

ω_b等于2πf_bT_s，是基带角频率f_b的数字角频率；

ω_d等于2πf_dT_s，是多普勒频移f_d的数字角频率；

φ₀是n＝o时的初始载波相位；

N(n)是基带频率的等效高斯(Gaussian)噪声。

(1-4)如图2和3所示，来自A/D变换器(23)的数字基带信号和来自数据融合模块(80)的预测的码延时和载波多普勒漂移，被数字信号处理机(25)接收，用来计算出伪距和伪距率测量，及其伪距和伪距率的跟踪误差。

由数控振荡器(24)产生的本地参考信号被输入到RF/IF变换器(21)、IF/基带转换器(22)、以及数字信号处理机(25)。

如图3所示，来自A/D转换器(23)的数字信号与来自正弦-余弦发生器(29)的本地同相(In-phase，I)和正交(Quadraphase，Q)信号在混频器(26)中进行混频，混频后的信号被送入相关器(27)。来自混频器(26)的经过混频的正交和同相信号和来自码发生器(30)的本地码，在相关器(27)中进行相关处理，相关处理的结果被输入最大似然估计器(Maximum Likelihood Estimator)(28)。

来自相关器(27)的相关计算结果，被最大似然估计器(28)进行采集。假定在一小时时间段内，码延时和载波多普勒是常值，最大似然估计器可做出码延时和载波多普勒频移的跟踪误差的最大似然估计，并将其转换为伪距和伪距率的跟踪误差，再输出给数据融合模块(80)。

来自数据融合模块(80)的预测的多普勒频移被馈入码振荡器(31)，用以计算码速率，产生的一定码速率PRN(Pseudo-random Noise)码被馈入码发生器(30)。

来自码发生器的一定码速率的PRN码，和来自数据融合模块(80)的预测的码延时被馈入码发生器(30)，用来产生本地准时码以及计算伪距、伪距率测量。产生的本地准时码被送入相关器(27)，并解调卫星星历数据，解调出的卫星星历被送入数据融合模块(80)，得出的伪距和伪距率测量被送入数据融合模块(80)。

来自数据融合模块(80)的载波多普勒频移被馈入正弦-余弦发生器(29)，用以产生本地的同相和正交信号，以及计算伪距率测量。产生的同相和正交信号被送入混频器(26)，得出的伪距率测量被送入数据融合模块(80)。

如图4所示，第2步骤有两种操作模式：

1)反馈补偿(Feedback Compensation)；

2)前馈补偿(Feedforward Compensation)。

运载体的角速率和加速度信息，可由下列两种惯性测量组件提供：

1)包含三个正交安装的陀螺和三个正交安装的加速度计的惯性测量组件，输出三轴角速率和加速度数据。

2)包含三个以上斜置安装的陀螺和三个以上斜置安装的加速度计，输出余度的角速率和加速度数据。

因而，第2步骤进一步包含：

2(A).惯性测量组件(50)的信号预处理采用反馈补偿方式，该惯性测量组件包含三个正交安装的陀螺和三个正交安装的加速度计，其输出的三轴角速率和加速度数据以及来自数据融合模块(80)的惯性传感器误差的最优估计，被输入惯性测量组件(50)的信号预处理部分的误差补偿模块(52)。

用惯性传感器误差的最优估计来补偿三轴角速率和加速度中的误差，经误差补偿后的三轴角速率被输出给姿态矩计算模块(53)，经误差补偿后的加速度被输出给坐标变换模块(54)。

来自误差补偿模块(52)的运载体角速率数据、来自地球和运载体角速率计算模块(26)的从当地导航坐标系(n系)到惯性坐标系(i系)的旋转向量、以及来自数据融合模块(80)的参考导航参数误差的最优估计，被输入到姿态矩阵计算模块(53)，用来更新机体坐标系到导航坐标系的姿态矩阵，同时补偿姿态矩阵的误差。得出的姿态矩阵被输出给坐标变换模块(54)和参考导航数据计算模块(56)。更新姿态矩阵的方法有欧拉(Euler)法、方向余弦(Direction Cosine)法、四元数(quaternion)法。

来自误差补偿模块(52)的加速度数据，是表达在机体坐标系内的，它和来自姿态计算模块(53)的姿态矩阵，被坐标转换模块(54)接收，并将表达在机体坐标系内的加速度转换为表达在导航坐标系内的加速度，并输出给参考导航参数计算模块(56)。

来自坐标变换模块(54)的表达在导航坐标系内的加速度、来自姿态矩阵计算模块(53)的姿态矩阵、以及来自数据融合模块(80)的参考导航参数误差的最优估计，被参考导航参数计算模块(56)接收，用来计算参考的位置、速度、姿态数据，并补偿位置、速度、姿态数据的误差。得出的参考导航参数包括位置、速度、姿态，输出给数据融合模块(80)和地球及运载体角速率计算模块(55)。

来自参考导航参数计算模块(56)的参考导航参数，被地球和运载体角速率计算模块(55)接收，用来计算当地导航坐标系到惯性坐标系的旋转角速率，并将其输出给姿态矩阵计算模块(53)。

2(b).惯性测量组件的预处理(50)采用前馈补偿方式，该惯性测量组件包含三个正交安装的陀螺和三个正交安装的加速度计，它输出的三轴角速率和加速度数据被馈入惯性测量组件进行预处理(50)。三轴角速率数据被馈入姿态矩阵计算模块(53)，三轴加速度被馈入坐标变换模块(54)。

来自惯性测量组件的运载体角速率数据、来自地球和运载体角速率计算模块(26)的从当地导航坐标系(n系)到惯性坐标系(i系)的旋转向量，被输入到姿态矩阵计算模块(53)，用来更新机体坐标系到导航坐标系的姿态矩阵，同时补偿姿态矩阵的误差。得出的姿态矩阵被输出给坐标变换模块(54)和参考导航数据计算模块(56)。更新姿态矩阵的方法有欧拉法、方向余弦法、四元数法。

来自惯性测量组件的加速度数据，是表达在机体坐标系内的，它和来自姿态计算模块(53)的姿态矩阵，被坐标转换模块(54)接收，并将表达在机体坐标系内的加速度转换为表达在导航坐标系内的加速度，并输出给参考导航参数计算模块(56)。

来自坐标变换模块(54)的表达在导航坐标系内的加速度、来自姿态矩阵计算模块(53)的姿态矩阵，被参考导航参数计算模块(56)接收，用来计算参考的位置、速度、姿态数据，得出的参考导航参数包括位置、速度、姿态，输出给数据融合模块(80)和地球及运载体角速率计算模块(55)。

来自参考导航参数计算模块(56)的参考导航参数，被地球和运载体角速率计算模块(55)接收，用来计算当地导航坐标系到惯性坐标系的旋转角速率，并将其输出给姿态矩阵计算模块(53)。

2(c).惯性测量组件的信号预处理(50)采用反馈补偿方式，该惯性测量组件包含三个以斜置安装的陀螺和三个以上斜置安装的加速度计，其输出的余度的角速率和加速度数据，被馈入FDIR模块，用来执行传感器故障的检测、隔离、重构，输出可靠三轴角速率和加速度数据、输出的三轴角速率和加速度数据、以及来自数据融合模块(80)的惯性传感器误差的最优估计，被输入到惯性测量组件(50)的信号预处理部分的误差补偿模块(52)。

用惯性传感器误差的最优估计来补偿三轴角速率和加速度中的误差，经误差补偿后的角速率被输出给姿态矩计算模块(53)，经误差补偿后的加速度被输出给坐标变换模块(54)。

来自误差补偿模块(52)的运载体角速率数据、来自地球和运载体角速率计算模块(26)的从当地导航坐标系(n系)到惯性坐标系(i系)的旋转向量、以及来自数据融合模块(80)的参考导航参数误差的最优估计，被输入到姿态矩阵计算模块(53)，用来更新机体坐标系到导航坐标系的姿态矩阵，同时补偿姿态矩阵的误差。得出的姿态矩阵被输出给坐标变换模块(54)和参考导航数据计算模块(56)。

更新姿态矩阵的方法有欧拉法、方向余弦法、四元数法。

来自误差补偿模块(52)的加速度数据，是表达在机体坐标系内的，它和来自姿态计算模块(53)的姿态矩阵，被坐标转换模块(54)接收，并将表达在机体坐标系内的加速度转换为表达在导航坐标系内的加速度，并输出给参考导航参数计算模块(56)。

来自坐标变换模块(54)的表达在导航坐标系内的加速度、来自姿态矩阵计算模块(53)的姿态矩阵、以及来自数据融合模块(80)的参考导航参数误差的最优估计，被参考导航参数计算模块(56)接收，用来计算参考的位置、速度、姿态数据，并补偿位置、速度、姿态数据的误差。得出的参考导航参数包括位置、速度、姿态，输出给数据融合模块(80)和地球及运载体角速率计算模块(55)。

来自参考导航参数计算模块(56)的参考导航参数，被地球和运载体角速率计算模块(55)接收，用来计算当地导航坐标系到惯性坐标系的旋转角速率，并将其输出给姿态矩阵计算模块(53)。

2(d).惯性测量组件的预处理(50)采用前馈补偿，该惯性测量组件包含三个以上斜置安装的陀螺和三个以上斜置安装的加速度计，其输出的余度的角速率和加速度数据，被馈入FDIR模块，用来执行传感器故障的检测、隔离、重构，输出可靠三轴角速率和加速度数据。三轴角速率数据被馈入姿态矩阵计算模块(53)，三轴加速度被馈入坐标变换模块(54)。

来自惯性测量组件的运载体角速率数据、来自地球和运载体角速率计算模块(26)的从当地导航坐标系(n系)到惯性坐标系(i系)的旋转向量，被输入姿态矩阵计算模块(53)，用来更新机体坐标系到导航坐标系的姿态矩阵，同时补偿姿态矩阵的误差。得出的姿态矩阵被输出给坐标变换模块(54)和参考导航数据计算模块(56)。

更新姿态矩阵的方法有欧拉法、方向余弦法、四元数法。

来自惯性测量组件的加速度数据，是表达在机体坐标系内的，它和来自姿态计算模块(53)的姿态矩阵，被坐标转换模块(54)接收，并将表达在机体坐标系内的加速度转换为表达在导航坐标系内的加速度，并输出给参考导航参数计算模块(56)。

来自坐标变换模块(54)的表达在导航坐标系内的加速度、来自姿态矩阵计算模块(53)的姿态矩阵，被参考导航参数计算模块(56)接收，用来计算参考的位置、速度、姿态数据，得出的参考导航参数包括位置、速度、姿态，输出给数据融合模块(80)和地球及运载体角速率计算模块(55)。

来自参考导航参数计算模块(56)的参考导航参数，被地球和运载体角速率计算模块(55)接收，用来计算当地导航坐标系到惯性坐标系的旋转角速率，并将其输出给姿态矩阵计算模块(53)。

如图5和6所示，第3步骤可用下列两个方案来实现：

3(a).基于集中式处理的卡尔曼滤波器(Centralized Kalman Filter)方案；

3(b).基于分散并行处理的分散式卡尔曼滤波(Decentralized Filter)方案，诸如联合(Federated)卡尔曼滤波。

如图5和6所示，第3(a)步骤进一步包含：

3(a)-1.如图5和6所示，如果惯性测量组件的预处理(50)以误差反馈补偿方式实现，则开关(90)与惯性测量组件的预处理(50)闭合。从惯性测量组件的预处理(50)输出的参考惯性导航参数，已被来自集中式滤波器(81)的参考惯性导航参数误差的最优估计进行误差补偿，并被减法器(82)接收，然后被馈入预测的伪距和伪距率计算模块(83)，为作为整个***全融合的定位数据输出。

如果惯性测量组件的预处理(50)以误差前馈补偿方式实现，则开关(90)与减法器(82)闭合，来自惯性测量组件的预处理(50)的参考导航参数和来自集中滤波器(81)的参考导航参数误差的最优估计，被减法器(82)接收，并用参考导航参数误差的最优估计来补偿参考导航参数的误差。补偿后的导航参数输出给预测的伪距和伪距率计算模块(83)，并作为整个***的全融合的定位数据输出。

来自每一个GPS卫星的跟踪通道的数字信号处理机(25)的卫星星历、来自减法器(90)的参考导航数据、来自集中滤波器(81)的GPS的接收机时钟偏置和时钟漂移的最优估计，被馈入预测的伪距和伪距率计算模块(83)。

每个通道的预测的伪距和伪距率可用下列参数计算得出：GPS卫星的位置和速度、惯性测量组件的位置和速度、估计的接收机时钟位置和时钟漂移、确定性的GPS卫星时钟校正、计算的信号的大气延时。

预测的伪距和伪距率，被输给集中滤波器(81)，并被转换为预测的GPS信号的码延时和载波多普勒频移，输出给每个通道的数字信号处理机(25)，用以闭合GPS信号的跟踪回路。

3(a)-2.参考导航参数误差包括：

三个位置误差、三个速度误差、三个姿态误差、以及惯性传感器误差和GPS接收机误差，在集中滤波器(81)中建模，其微分方程可表示为： $\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

来自每个通道的数字信号处理机(25)的测量的伪距和伪距率的跟踪误差、来自预测伪距和伪距率计算模块的预测的伪距和伪距率、卫星星历、以及参考的导航参数，一起被集中滤波器(81)接收，用来执行下列处理步骤：

更新***和测量方程的参数；

计算***方程的离散模型；

计算测量方程的线性模型；

计算状态估计及其方差阵的时间传播；

将测量的伪距和伪距率与预测的伪距和伪距率相减，并用伪距和伪距率的跟踪误差进行补偿，作为集中滤波器(81)的测量；

计算测量残差；

更新状态估计及其方差阵；

更新后的状态估计被输出给减法器(82)和惯性测量组件(50)进行预处理。

3(a)-3.来自集中滤波器(81)的测量残差被输入FDIR(FailureDetection Isolation and Recovery)(84)，用来执行测量残差的统计检验，以便检测隔离由GPS卫星故障导致的输入伪距和伪距率的故障。如果检测出故障，故障卫星的标志被输出给集中滤波器(81)，以便隔离故障GPS卫星或重调集中滤波器(81)。

3(b)-1.如图6所示，如果惯性测量组件的预处理(50)以误差反馈补偿方式实现，则开关(90)与惯性测量组件的预处理(50)闭合。从惯性测量组件的预处理(50)输出的参考惯性导航参数，已被来自集中式滤波器(81)的参考惯性导航参数误差的最优估计进行误差补偿，并被减法器(82)接收，然后被馈入预测的伪距和伪距率计算模块(83)，并作为整个***全融合的定位数据输出。

如果惯性测量组件的预处理(50)以误差前馈补偿方式实现，则开关(90)与减法器(82)闭合，来自惯性测量组件的预处理(50)的参考导航参数和来自集中滤波器(81)的参考导航参数误差的最优估计，被减法器(82)接收，并用参考导航参数误差的最优估计来补偿参考导航参数的误差。补偿后的导航参数输出给预测的伪距和伪距率计算模块(88)，并作为整个***的全融合的定位数据输出。

来自每一个GPS卫星的跟踪通道的数字信号处理机(25)的卫星星历、来自减法器(90)的参考导航数据、来自集中滤波器(81)的GPS的接收机时钟偏置和时钟漂移的最优估计，被馈入预测的伪距和伪距率计算模块(83)。

每个通道的预测的伪距和伪距率可用下列参数计算得出：GPS卫星的位置和速度、惯性测量组件的位置和速度、估计的接收机时钟位置和时钟漂移、确定性的GPS卫星时钟校正、计算的信号的大气延时。

每一个GPS卫星的跟踪通道的预测的伪距和伪距率，被输给通道的本地滤波器(85)，并被转换为预测的GPS信号的码延时和载波多普勒频移，输出给每个通道的数字信号处理机(25)，用以闭合GPS信号的跟踪回路。

3(b)-2.参考导航参数误差包括：三个位置误差、三个速度误差、三个姿态误差、以及惯性传感器误差和GPS接收机误差，在每个本地滤波器(85)中建模，其微分方程可表示为： $\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

来自每个通道的数字信号处理机(25)的测量的伪距和伪距率的跟踪误差、来自预测的伪距和伪距率计算模块的预测的伪距和伪距率、卫星星历、以及参考的导航参数，一起被相应的每个本地滤波器(85)接收，用来执行下列处理步骤：

更新***和测量方程的参数；

计算***方程的离散模型；

计算测量方程的线性模型；

计算状态估计及其方差阵的时间传播；

将测量的伪距和伪距率与预测的伪距和伪距率相减，并用伪距和伪距率的跟踪误差进行补偿，作为本地滤波器(85)的测量；

计算测量残差；

更新状态估计及其方差阵；

更新后的状态估计被输出给减法器(82)和惯性测量组件(50)进行预处理。

3(b)-3.来自每个本地滤波器的局部状态估计及方差阵被输出给主滤波器(86)，用于执行融合计算，得出全局最优的状态估计及其方差阵，并将其输出给FDIR(89)和开关(91)。

全局最优的状态估计，包括惯性导航参数误差、GPS接收机误差、惯性传感器误差的最优估计、及其方差阵，被反馈回每个本地滤波器以重调本地滤波器，并用来执行主滤波器(86)和每个本地滤波器之间的信息分享。

主滤波器(86)和每个局部滤波器(85)之间的通信和用在主滤波器(86)和局部滤波器(85)中的滤波方法，可以用不同的方式，以便获得不同的性能指标。

3(b)-4.来自每个本地滤波器(85)的局部状态估计及其方差阵，和来自主滤波器(86)的全局状态估计及其方差阵，被用来执行相容性检验，以便能检测出由故障卫星及输出信号造成的有故障的伪距和伪距率测量。结果检测出的故障、有故障的GPS卫星的标志，被FDIR(89)输出给主滤波(86)，以便重构主滤波器(86)，隔离有故障的卫星。

对GPS完善性(Integrity)监测来说，第3(b)的FDIR(89)的性能要优于第3(a)方法的FDIR(89)性能。因为第3(b)方案有并行滤波结构。

完善性是指***有能力向用户提供即时的报警信号来终止它的操作。人们关心的问题是有故障的GPS卫星有可能向用户发出错误的导航信息。当GPS的导航精度超出规定的报警范围时，要求报警时间在10秒之内。但不幸的是GPS的控制部分不能在这个时间限制范围内响应故障。通常GPS控制部分要花费15分钟到1小时的时间来确定GPS的故障、辨别出故障、确定校正方案、作出反应。

Claims (42)

1.一种全融合定位方法，包含下列步骤：

(a)馈入一GPS天线收到的GPS信号和来自数据融合模块预测的伪距和伪距率，变换、跟踪该GPS信号，获得GPS的伪距和伪距率测量及该伪距和伪距率测量的跟踪误差，将其输出给所述数据融合模块；

(b)接收来自一惯性测量组件的运载体的角速率和加速度信号，求解惯性导航方程，获得参考的导航参数，包括位置、速度、姿态，并将其输出给所述数据融合模块；

(c)融合上述伪距和伪距率测量、伪距和伪距率的跟踪误差以及上述参考的导航参数，得出上述预测的伪距、伪距率、上述导航参数误差和惯性传感器误差的最优估计以及最优的定位数据。

2.如权利要求1所述的全融合定位方法，其特征在于，所述第(a)步骤进一步包含：

(a-1)输出来自所述GPS天线接收到的GPS信号给一RF/IF转换器，该GPS信号是RF信号，并与来自一本地数控振荡器的本地GPS RF信号混频；对混频后的信号进行带通滤波，形成中频信号，将该中频信号输出给一RF基带转换器；

(a-2)用来自该本地数控振荡器的本地信号与该GPS IF信号进行混频，混频后的信号被放大、低通滤波，形成基带信号，将该基带信号输出给一A/D转换器；

(a-3)接收来自所述IF/基带转换器的基带信号，该基带信号是模拟信号；由所述A/D转换器对该基带信号进行采样，形成数字信号，并输出给一数字信号处理模块；

(a-4)来自所述A/D转换器的数字信号和来自数据融合模块的预测的码延时和载波多普勒频移，被所述数字信号处理模块接收，得出每一个跟踪卫星的伪距、伪距率测量，以及该伪距、伪距率测量的跟踪误差，并输出给所述数据融合模块。

3.如权利要求2所述的全融合定位方法，其特征在于，所述第(a-4)步骤进一步包含：

接收上述来自A/D转换器的数字信号，将其与来自一正弦-余弦信号发生器的本地同相(I)和正交(Q)信号进行混频，输出混频后的正交和同相信号给一相关模块；

由该相关模块接收混频后的同相和正交信号以及来自一码发生器的本地码，进行相关计算，并将相关计算的结果送入一最大似然估计器；

该最大似然估计器收集所述相关模块输出的N个采样值，做出码延时和载波多普勒漂移的跟踪误差的最大似然估计，并将其转换为伪距、伪距率的跟踪误差，输出给所述数据融合模块；

由一码振荡器接收来自所述数据融合模块的预测的载波多普勒频移，用来计算码速率，并将该码送入一码发生器；

由该码发生器接收来自该码发生器的给定速率的码，以便产生本地准时码，并将其输出给相关模块；计算伪距测量，将其输出给所述数据融合模块；执行卫星星历的解调，并将该卫星星历输出给所述数据融合模块；

由一正弦-余弦信号发生器接收来自数据融合模块的预测的载波多普勒频移，产生上述I和Q信号，将其输出给上述混频器；计算伪距率测量，并将其输出给数据融合模块。

4.一种全融合定位方法，包含以下步骤：

(a)馈入一GPS天线收到的GPS信号和来自数据融合模块预测的伪距和伪距率，变换、跟踪该GPS信号，获得GPS的伪距和伪距率测量和该伪距和伪距率测量的跟踪误差，将其输出给所述数据融合模块；

(b)接收来自一惯性测量组件的运载体的角速率和加速度信号，以及来自所述数据融合模块的参考导航参数误差和惯性传感器误差的最优估计，求解导航方程，得出该参考导航参数，包括位置、速度、姿态，并补偿该参考导航参数中的误差，将经过补偿后的参考导航数据送入所述数据融合模块。

(c)融合上述伪距和伪距率测量、伪距和伪距率的跟踪误差以及上述参考的导航参数，得出上述预测的伪距、伪距率、上述导航参数误差和惯性传感器误差的最优估计以及最优的定位数据。

5.如权利要求4所述的全融合定位方法，其特征在于，所述第(a)步骤进一步包含：

(a-1)输出来自所述GPS天线接收到的GPS信号给一RF/IF转换器，该GPS信号是RF信号，并与来自一本地数控振荡器的本地GPS RF信号混频；对混频后的信号进行带通滤波，形成中频信号，将该中频信号输出给一RF基带转换器；

(a-2)用来自该本地数控振荡器的本地信号与该GPS IF信号进行混频，混频后的信号被放大、低通滤波，形成基带信号，将该基带信号输出给一A/D转换器；

(a-3)接收来自所述IF/基带转换器的基带信号，该基带信号是模拟信号；由所述A/D转换器对该基带信号进行采样，形成数字信号，并输出给一数字信号处理模块；

(a-4)来自所述A/D转换器的数字信号和来自数据融合模块的预测的码延时和载波多普勒频移，被所述数字信号处理模块接收，得出每一个跟踪卫星的伪距、伪距率测量，以及该伪距、伪距率测量的跟踪误差，并输出给所述数据融合模块。

6.如权利要求5所述的全融合定位方法，其特征在于，所述第(a-4)步骤进一步包含：

接收上述来自A/D转换器的数字信号，将其与来自一正弦-余弦信号发生器的本地同相(I)和正交(Q)信号进行混频，输出混频后的正交和同相信号给一相关模块；

由该相关模块接收混频后的同相和正交信号以及来自一码发生器的本地码，进行相关计算，并将相关计算的结果送入一最大似然估计器；

该最大似然估计器收集所述相关模块输出的N个采样值，做出码延时和载波多普勒漂移的跟踪误差的最大似然估计，并将其转换为伪距、伪距率的跟踪误差，输出给所述数据融合模块；

由一码振荡器接收来自所述数据融合模块的预测的载波多普勒频移，用来计算码速率，并将该码送入一码发生器；

由该码发生器接收来自该码发生器的给定速率的码，以便产生本地准时码，并将其输出给相关模块；计算伪距测量，将其输出给所述数据融合模块；执行卫星星历的解调，并将该卫星星历输出给所述数据融合模块；

由一正弦-余弦信号发生器接收来自数据融合模块的预测的载波多普勒频移，产生上述I和Q信号，将其输出给上述混频器；计算伪距率测量，并将其输出给数据融合模块。

7.如权利要求4所述的全融合定位方法，其特征在于，所述第(b)步骤包含：

(b-1)输入来自所述惯性测量组件的三轴角速率和三轴加速度、以及来自所述数据融合的惯性传感器误差的最优估计，给一误差补偿模块，该惯性测量组件有三个正交安装的陀螺和三个正交安装的加速度计；

(b-2)用该惯性传感器误差的最优估计补偿输入的三轴角速率和三轴加速度，补偿后的三轴角速率被送入一姿态矩阵计算模块；

(b-3)由该姿态矩阵计算模块接收来自所述误差补偿模块的三轴角速率、来自地球和运载体角速率计算模块的从当地导航坐标系到惯性坐标系的旋转向量、来自所述数据融合模块的参考导航参数误差的最优估计，用来更新从机体坐标系到所述导航坐标系的姿态矩阵，并补偿该姿态矩阵中的误差，该姿态矩阵被送入上述坐标变换模块和一参考导航参数计算模块；

(b-4)由该坐标变换模块接收来自所述误差补偿模块的表达在机体坐标系中的加速度、以及来自所述姿态矩阵计算模块的姿态矩阵，将输入的表达机体坐标系中的加速度转换为表达在导航坐标系的加速度计算数据；

(b-5)由上述参考导航参数计算模块，接收坐标变换模块的表达在导航坐标系的加速度、来自所述姿态矩阵计算模块的姿态矩阵、来自所述数据融合模块的参考导航参数误差的最优估计，计算上述参考位置、速度、姿态，并补偿该位置和速度中的误差，将所述参考导航参数，包括该参考位置、速度、姿态输出给上述地球和运载体角速率计算模块和数据融合模块；

(b-6)由该地球和运载体角速率计算模块，接收来自所述参考导航参数计算模块的参考导航参数，计算上述从当地导航坐标系(n系)相对于惯性坐标系的旋转角速率，并将其输出给上述姿态矩阵计算模块。

8.如权利要求5所述的全融合定位方法，其特征在于，所述第(b)步骤包含：

(b-1)输入来自所述惯性测量组件的三轴角速率和三轴加速度、以及来自所述数据融合的惯性传感器误差的最优估计，给一误差补偿模块，该惯性测量组件有三个正交安装的陀螺和三个正交安装的加速度计；

(b-2)用该惯性传感器误差的最优估计补偿输入的三轴角速率和三轴加速度，补偿后的三轴角速率被送入一姿态矩阵计算模块；

(b-3)由该姿态矩阵计算模块接收来自所述误差补偿模块的三轴角速率、来自地球和运载体角速率计算模块的从当地导航坐标系到惯性坐标系的旋转向量、来自所述数据融合模块的参考导航参数误差的最优估计，用来更新从机体坐标系到所述导航坐标系的姿态矩阵，并补偿该姿态矩阵中的误差，该姿态矩阵被送入上述坐标变换模块和一参考导航参数计算模块；

(b-4)由该坐标变换模块接收来自所述误差补偿模块的表达在机体坐标系中的加速度、以及来自所述姿态矩阵计算模块的姿态矩阵，将输入的表达机体坐标系中的加速度转换为表达在导航坐标系的加速度计算数据；

(b-5)由上述参考导航参数计算模块，接收坐标变换模块的表达在导航坐标系的加速度、来自所述姿态矩阵计算模块的姿态矩阵、来自所述数据融合模块的参考导航参数误差的最优估计，计算上述参考位置、速度、姿态，并补偿该位置和速度中的误差，将所述参考导航参数，包括该参考位置、速度、姿态输出给上述地球和运载体角速率计算模块和数据融合模块；

(b-6)由该地球和运载体角速率计算模块，接收来自所述参考导航参数计算模块的参考导航参数，计算上述从当地导航坐标系(n系)相对于惯性坐标系的旋转角速率，并将其输出给上述姿态矩阵计算模块。

9.如权利要求6所述的全融合定位方法，其特征在于，所述第(b)步骤包含：

(b-1)输入来自所述惯性测量组件的三轴角速率和三轴加速度、以及来自所述数据融合的惯性传感器误差的最优估计，给一误差补偿模块，该惯性测量组件有三个正交安装的陀螺和三个正交安装的加速度计；

(b-2)用该惯性传感器误差的最优估计补偿输入的三轴角速率和三轴加速度，补偿后的三轴角速率被送入一姿态矩阵计算模块；

(b-3)由该姿态矩阵计算模块接收来自所述误差补偿模块的三轴角速率、来自地球和运载体角速率计算模块的从当地导航坐标系到惯性坐标系的旋转向量、来自所述数据融合模块的参考导航参数误差的最优估计，用来更新从机体坐标系到所述导航坐标系的姿态矩阵，并补偿该姿态矩阵中的误差，该姿态矩阵被送入上述坐标变换模块和一参考导航参数计算模块；

(b-4)由该坐标变换模块接收来自所述误差补偿模块的表达在机体坐标系中的加速度、以及来自所述姿态矩阵计算模块的姿态矩阵，将输入的表达机体坐标系中的加速度转换为表达在导航坐标系的加速度计算数据；

(b-5)由上述参考导航参数计算模块，接收坐标变换模块的表达在导航坐标系的加速度、来自所述姿态矩阵计算模块的姿态矩阵、来自所述数据融合模块的参考导航参数误差的最优估计，计算上述参考位置、速度、姿态，并补偿该位置和速度中的误差，将所述参考导航参数，包括该参考位置、速度、姿态输出给上述地球和运载体角速率计算模块和数据融合模块；

(b-6)由该地球和运载体角速率计算模块，接收来自所述参考导航参数计算模块的参考导航参数，计算上述从当地导航坐标系(n系)相对于惯性坐标系的旋转角速率，并将其输出给上述姿态矩阵计算模块。

10.如权利要求7所述的全融合定位方法，其特征在于，所述第(c)步骤包含：

(c-1)将所述参考导航参数输出给预测的伪距和伪距率计算模块，并作为全融合的定位数据输出；

(c-2)由预测伪距和伪距率计算模块，接收来自每个GPS信号跟踪通道的数字处理模块的卫星星历、上述参考导航参数以及GPS接收机时钟偏置和漂移的最优估计；

(c-3)使用GPS卫星的位置和速度、上述参考导航参数的位置和速度、估计的GPS接收机的时钟偏置和漂移速率、确定的GPS卫星时钟校正量、计算的GPS信号的大气延时，来计算上述预测的伪距和伪距率；

(c-4)输出该预测的伪距和伪距率给一集中处理式滤波器，并将该预测的伪距和伪距率转换成上述预测的码延时和载波多普勒漂移，并将其输出给所述每个GPS信号跟踪通道的数字信号处理模块，用来闭合该GPS信号跟踪回路；

(c-5)由所述集中处理滤波器对参考导航参数误差，包括三个位置误差、三个速度误差、三个姿态误差、惯性传感器误差和GPS接收机误差建模；

(c-6)由该集中处理滤波器接收并处理来自上述每个GPS信号跟踪通道的数字信号处理模块的伪距和伪距率测量、以及该伪距和伪距率的跟踪误差，来自上述预测的伪距和伪距率计算模块的预测的伪距和伪距率与卫星星历、以及参考的惯性导航参数，进一步包括下列步骤：

更新***和测量方程；

计算该***方程的离散模型的参数；

计算该测量方程的线性模型的参数；

计算状态估计及其方差阵的时间传播；

将上述测量的伪距和伪距率与预测的伪距和伪距率相比较，并用上述测量的伪距和伪距率的跟踪误差进行补偿，其结果作为所述集中处理滤波器的测量；

计算测量残差，更新上述状态估计及其方差阵，获得上述参考导航参数误差、惯性传感器误差、GPS接收机误差的最优估计。

11.如权利要求8所述的全融合定位方法，其特征在于，所述第(c)步骤包含：

(c-1)将所述参考导航参数输出给预测的伪距和伪距率计算模块，并作为全融合的定位数据输出；

(c-2)由预测伪距和伪距率计算模块，接收来自每个GPS信号跟踪通道的数字处理模块的卫星星历、上述参考导航参数以及GPS接收机时钟偏置和漂移的最优估计；

(c-3)使用GPS卫星的位置和速度、上述参考导航参数的位置和速度、估计的GPS接收机的时钟偏置和漂移速率、确定的GPS卫星时钟校正量、计算的GPS信号的大气延时，来计算上述预测的伪距和伪距率；

(c-4)输出该预测的伪距和伪距率给一集中处理式滤波器，并将该预测的伪距和伪距率转换成上述预测的码延时和载波多普勒漂移，并将其输出给所述每个GPS信号跟踪通道的数字信号处理模块，用来闭合该GPS信号跟踪回路；

(c-5)由所述集中处理滤波器对参考导航参数误差，包括三个位置误差、三个速度误差、三个姿态误差、惯性传感器误差和GPS接收机误差建模；

(c-6)由该集中处理滤波器接收并处理来自上述每个GPS信号跟踪通道的数字信号处理模块的伪距和伪距率测量、以及该伪距和伪距率的跟踪误差，来自上述预测的伪距和伪距率计算模块的预测的伪距和伪距率与卫星星历、以及参考的惯性导航参数，进一步包括下列步骤：

更新***和测量方程；

计算该***方程的离散模型的参数；

计算该测量方程的线性模型的参数；

计算状态估计及其方差阵的时间传播；

将上述测量的伪距和伪距率与预测的伪距和伪距率相比较，并用上述测量的伪距和伪距率的跟踪误差进行补偿，其结果作为所述集中处理滤波器的测量；

计算测量残差，更新上述状态估计及其方差阵，获得上述参考导航参数误差、惯性传感器误差、GPS接收机误差的最优估计。

12.如权利要求9所述的全融合定位方法，其特征在于，所述第(c)步骤包含：

(c-1)将所述参考导航参数输出给预测的伪距和伪距率计算模块，并作为全融合的定位数据输出；

(c-2)由预测伪距和伪距率计算模块，接收来自每个GPS信号跟踪通道的数字处理模块的卫星星历、上述参考导航参数以及GPS接收机时钟偏置和漂移的最优估计；

(c-3)使用GPS卫星的位置和速度、上述参考导航参数的位置和速度、估计的GPS接收机的时钟偏置和漂移速率、确定的GPS卫星时钟校正量、计算的GPS信号的大气延时，来计算上述预测的伪距和伪距率；

(c-4)输出该预测的伪距和伪距率给一集中处理式滤波器，并将该预测的伪距和伪距率转换成上述预测的码延时和载波多普勒漂移，并将其输出给所述每个GPS信号跟踪通道的数字信号处理模块，用来闭合该GPS信号跟踪回路；

(c-5)由所述集中处理滤波器对参考导航参数误差，包括三个位置误差、三个速度误差、三个姿态误差、惯性传感器误差和GPS接收机误差建模；

(c-6)由该集中处理滤波器接收并处理来自上述每个GPS信号跟踪通道的数字信号处理模块的伪距和伪距率测量、以及该伪距和伪距率的跟踪误差，来自上述预测的伪距和伪距率计算模块的预测的伪距和伪距率与卫星星历、以及参考的惯性导航参数，进一步包括下列步骤：

更新***和测量方程；

计算该***方程的离散模型的参数；

计算该测量方程的线性模型的参数；

计算状态估计及其方差阵的时间传播；

将上述测量的伪距和伪距率与预测的伪距和伪距率相比较，并用上述测量的伪距和伪距率的跟踪误差进行补偿，其结果作为所述集中处理滤波器的测量；

计算测量残差，更新上述状态估计及其方差阵，获得上述参考导航参数误差、惯性传感器误差、GPS接收机误差的最优估计。

13.如权利要求7所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

14.如权利要求8所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

15.如权利要求9所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

16.如权利要求10所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

17.如权利要求11所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

18.如权利要求12所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

19.如权利要求10所述的全融合定位方法，其特征在于，在所述第(c-6)步骤之后进一步包含一附加步骤：

(c-7)输入来自所述集中式滤波器的测量残差给一故障检测、隔离、重构模块(FDIR)，用以执行该测量残差的统计检验，以检测和隔离由于GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给中心滤波器，以便隔离该故障卫星或重调所述集中处理滤波器。

20.如权利要求11所述的全融合定位方法，其特征在于，在所述第(c-6)步骤之后进一步包含一附加步骤：

(c-7)输入来自所述集中式滤波器的测量残差给一故障检测、隔离、重构模块(FDIR)，用以执行该测量残差的统计检验，以检测和隔离由于GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给中心滤波器，以便隔离该故障卫星或重调所述集中处理滤波器。

21.如权利要求12所述的全融合定位方法，其特征在于，在所述第(c-6)步骤之后进一步包含一附加步骤：

(c-7)输入来自所述集中式滤波器的测量残差给一故障检测、隔离、重构模块(FDIR)，用以执行该测量残差的统计检验，以检测和隔离由于GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给中心滤波器，以便隔离该故障卫星或重调所述集中处理滤波器。

22.如权利要求16所述的全融合定位方法，其特征在于，在所述第(c-6)步骤之后进一步包含一附加步骤：

(c-7)输入来自所述集中式滤波器的测量残差给一故障检测、隔离、重构模块(FDIR)，用以执行该测量残差的统计检验，以检测和隔离由于GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给中心滤波器，以便隔离该故障卫星或重调所述集中处理滤波器。

23.如权利要求17所述的全融合定位方法，其特征在于，在所述第(c-6)步骤之后进一步包含一附加步骤：

(c-7)输入来自所述集中式滤波器的测量残差给一故障检测、隔离、重构模块(FDIR)，用以执行该测量残差的统计检验，以检测和隔离由于GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给中心滤波器，以便隔离该故障卫星或重调所述集中处理滤波器。

24.如权利要求18所述的全融合定位方法，其特征在于，在所述第(c-6)步骤之后进一步包含一附加步骤：

(c-7)输入来自所述集中式滤波器的测量残差给一故障检测、隔离、重构模块(FDIR)，用以执行该测量残差的统计检验，以检测和隔离由于GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给中心滤波器，以便隔离该故障卫星或重调所述集中处理滤波器。

25.如权利要求7所述的全融合定位方法，其特征在于，所述第(c)步骤包含：

(c-1)将所述参考导航参数输出给预测的伪距和伪距率计算模块，并作为全融合的定位数据输出；

(c-2)由预测伪距和伪距率计算模块，接收来自每个GPS信号跟踪通道的数字处理模块的卫星星历、上述参考导航参数以及GPS接收机时钟偏置和漂移的最优估计；

(c-3)使用GPS卫星的位置和速度、上述参考导航参数的位置和速度、估计的GPS接收机的时钟偏置和漂移速率、确正的GPS卫星时钟校正量、计算的GPS信号的大气延时，来计算上述每个GPS信号跟踪通道的预测的伪距和伪距率；

(c-4)输出为每一个GPS信号跟踪通道预测的伪距和伪距率给相应的上述每一个GPS信号跟踪通道所设置的本地滤波器，并将该预测的伪距和伪距率转换成上述预测的码延时和载波多普勒漂移，并将其输出给上述每个GPS信号跟踪通道的数字信号处理模块，用来闭合该GPS信号跟踪回路；

(c-5)由所述本地滤波器对参考导航参数误差，包括三个位置误差、三个速度误差、三个姿态误差、惯性传感器误差和GPS接收机误差建模；

(c-6)由上述本地滤波器接收并处理来自上述每个GPS信号跟踪通道的数字信号处理模块的伪距和伪距率测量、以及该伪距和伪距率的跟踪误差，来自上述预测的伪距和伪距率计算模块的预测的伪距和伪距率与卫星星历、以及参考的惯性导航参数，进一步包括下列步骤：

更新***和测量方程；

计算该***方程的离散模型的参数；

计算该测量方程的线性模型的参数；

计算状态估计及其方差阵的时间传播；

将上述测量的伪距和伪距率与预测的伪距和伪距率相比较，并用上述测量的伪距和伪距率的跟踪误差进行补偿，其结果作为所述本地滤波器的测量；

计算测量残差，更新上述状态估计及其方差阵，获得上述参考导航参数误差、惯性传感器误差、GPS接收机误差的本地最优估计；

(c-7)由一主滤波器接收并融合来自每个本地滤波器的本地最优的状态估计及其协方差，获得全局最优的状态估计。

26.如权利要求8所述的全融合定位方法，其特征在于，所述第(c)步骤包含：

(c-1)将所述参考导航参数输出给预测的伪距和伪距率计算模块，并作为全融合的定位数据输出；

(c-2)由预测伪距和伪距率计算模块，接收来自每个GPS信号跟踪通道的数字处理模块的卫星星历、上述参考导航参数以及GPS接收机时钟偏置和漂移的最优估计；

(c-3)使用GPS卫星的位置和速度、上述参考导航参数的位置和速度、估计的GPS接收机的时钟偏置和漂移速率、确正的GPS卫星时钟校正量、计算的GPS信号的大气延时，来计算上述每个GPS信号跟踪通道的预测的伪距和伪距率；

(c-4)输出为每一个GPS信号跟踪通道预测的伪距和伪距率给相应的上述每一个GPS信号跟踪通道所设置的本地滤波器，并将该预测的伪距和伪距率转换成上述预测的码延时和载波多普勒漂移，并将其输出给上述每个GPS信号跟踪通道的数字信号处理模块，用来闭合该GPS信号跟踪回路；

(c-5)由所述本地滤波器对参考导航参数误差，包括三个位置误差、三个速度误差、三个姿态误差、惯性传感器误差和GPS接收机误差建模；

(c-6)由上述本地滤波器接收并处理来自上述每个GPS信号跟踪通道的数字信号处理模块的伪距和伪距率测量、以及该伪距和伪距率的跟踪误差，来自上述预测的伪距和伪距率计算模块的预测的伪距和伪距率与卫星星历、以及参考的惯性导航参数，进一步包括下列步骤：

更新***和测量方程；

计算该***方程的离散模型的参数；

计算该测量方程的线性模型的参数；

计算状态估计及其方差阵的时间传播；

将上述测量的伪距和伪距率与预测的伪距和伪距率相比较，并用上述测量的伪距和伪距率的跟踪误差进行补偿，其结果作为所述本地滤波器的测量；

计算测量残差，更新上述状态估计及其方差阵，获得上述参考导航参数误差、惯性传感器误差、GPS接收机误差的本地最优估计；

(c-7)由一主滤波器接收并融合来自每个本地滤波器的本地最优的状态估计及其协方差，获得全局最优的状态估计。

27.如权利要求9所述的全融合定位方法，其特征在于，所述第(c)步骤包含：

(c-1)将所述参考导航参数输出给预测的伪距和伪距率计算模块，并作为全融合的定位数据输出；

(c-2)由预测伪距和伪距率计算模块，接收来自每个GPS信号跟踪通道的数字处理模块的卫星星历、上述参考导航参数以及GPS接收机时钟偏置和漂移的最优估计；

(c-3)使用GPS卫星的位置和速度、上述参考导航参数的位置和速度、估计的GPS接收机的时钟偏置和漂移速率、确正的GPS卫星时钟校正量、计算的GPS信号的大气延时，来计算上述每个GPS信号跟踪通道的预测的伪距和伪距率；

(c-4)输出为每一个GPS信号跟踪通道预测的伪距和伪距率给相应的上述每一个GPS信号跟踪通道所设置的本地滤波器，并将该预测的伪距和伪距率转换成上述预测的码延时和载波多普勒漂移，并将其输出给上述每个GPS信号跟踪通道的数字信号处理模块，用来闭合该GPS信号跟踪回路；

(c-5)由所述本地滤波器对参考导航参数误差，包括三个位置误差、三个速度误差、三个姿态误差、惯性传感器误差和GPS接收机误差建模；

(c-6)由上述本地滤波器接收并处理来自上述每个GPS信号跟踪通道的数字信号处理模块的伪距和伪距率测量、以及该伪距和伪距率的跟踪误差，来自上述预测的伪距和伪距率计算模块的预测的伪距和伪距率与卫星星历、以及参考的惯性导航参数，进一步包括下列步骤：

更新***和测量方程；

计算该***方程的离散模型的参数；

计算该测量方程的线性模型的参数；

计算状态估计及其方差阵的时间传播；

将上述测量的伪距和伪距率与预测的伪距和伪距率相比较，并用上述测量的伪距和伪距率的跟踪误差进行补偿，其结果作为所述本地滤波器的测量；

计算测量残差，更新上述状态估计及其方差阵，获得上述参考导航参数误差、惯性传感器误差、GPS接收机误差的本地最优估计；

(c-7)由一主滤波器接收并融合来自每个本地滤波器的本地最优的状态估计及其协方差，获得全局最优的状态估计。

28.如权利要求25所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含附加步骤：

(c-8)接收来自所述每个本地滤波器的局部状态估计及其方差阵，以及来自主滤波器的全局最优估计及其方差阵，用以执行相容性检验，以便用来检测和隔离由GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给主滤波器，以便隔离该故障卫星或重调该主滤波器。

29.如权利要求26所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含附加步骤：

(c-8)接收来自所述每个本地滤波器的局部状态估计及其方差阵，以及来自主滤波器的全局最优估计及其方差阵，用以执行相容性检验，以便用来检测和隔离由GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给主滤波器，以便隔离该故障卫星或重调该主滤波器。

30.如权利要求27所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含附加步骤：

(c-8)接收来自所述每个本地滤波器的局部状态估计及其方差阵，以及来自主滤波器的全局最优估计及其方差阵，用以执行相容性检验，以便用来检测和隔离由GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给主滤波器，以便隔离该故障卫星或重调该主滤波器。

31.如权利要求28所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含一附加步骤：

将来自所述主滤波器的全局最优的状态估计，包括惯性导航参数误差、GPS接收机误差和惯性传感器误差的最优估计及其方差阵反馈回所述每个本地滤波器，以便重调该本地滤波器，执行该主滤波器和每个本地滤波器之间的信息分享。

32.如权利要求29所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含一附加步骤：

将来自所述主滤波器的全局最优的状态估计，包括惯性导航参数误差、GPS接收机误差和惯性传感器误差的最优估计及其方差阵反馈回所述每个本地滤波器，以便重调该本地滤波器，执行该主滤波器和每个本地滤波器之间的信息分享。

33.如权利要求30所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含一附加步骤：

将来自所述主滤波器的全局最优的状态估计，包括惯性导航参数误差、GPS接收机误差和惯性传感器误差的最优估计及其方差阵反馈回所述每个本地滤波器，以便重调该本地滤波器，执行该主滤波器和每个本地滤波器之间的信息分享。

34.如权利要求25所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

35.如权利要求26所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

36.如权利要求27所述的全融合定位方法，其特征在于，所述第(b-1)步骤包括以下步骤：

(b-1-1)从一惯性测量组件接收余度角速率和加速度信号，该惯性测量组件包含三个以上的斜置配置的陀螺和加速度计；

(b-1-2)执行故障检测和隔离；

(b-1-3)从余度的角速率和加速度数据中，求解出三轴角速率和加速度数据；

(b-1-4)将三轴角速率和加速度数据及来自所述数据融合模块的传感器误差的最优估计，送入一误差补偿模块。

37.如权利要求34所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含附加步骤：

(c-8)接收来自所述每个本地滤波器的局部状态估计及其方差阵，以及来自主滤波器的全局最优估计及其方差阵，用以执行相容性检验，以便用来检测和隔离由GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给主滤波器，以便隔离该故障卫星或重调该主滤波器。

38.如权利要求37所述的全融合定位方法，其特征在于，在第(c-7)步骤之后，进一步包含一附加步骤：

将来自所述主滤波器的全局最优的状态估计，包括惯性导航参数误差、GPS接收机误差和惯性传感器误差的最优估计及其方差阵反馈回所述每个本地滤波器，以便重调该本地滤波器，执行该主滤波器和每个本地滤波器之间的信息分享。

39.如权利要求35所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含附加步骤：

(c-8)接收来自所述每个本地滤波器的局部状态估计及其方差阵，以及来自主滤波器的全局最优估计及其方差阵，用以执行相容性检验，以便用来检测和隔离由GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给主滤波器，以便隔离该故障卫星或重调该主滤波器。

40.如权利要求39所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含一附加步骤：

将来自所述主滤波器的全局最优的状态估计，包括惯性导航参数误差、GPS接收机误差和惯性传感器误差的最优估计及其方差阵反馈回所述每个本地滤波器，以便重调该本地滤波器，执行该主滤波器和每个本地滤波器之间的信息分享。

41.如权利要求36所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含附加步骤：

(c-8)接收来自所述每个本地滤波器的局部状态估计及其方差阵，以及来自主滤波器的全局最优估计及其方差阵，用以执行相容性检验，以便用来检测和隔离由GPS卫星故障所造成的错误的输入伪距和伪距率，当检测到故障之后，FDIR模块输出一个故障卫星标志给主滤波器，以便隔离该故障卫星或重调该主滤波器。

42.如权利要求41所述的全融合定位方法，其特征在于，在所述第(c-7)步骤之后，进一步包含一附加步骤：

将来自所述主滤波器的全局最优的状态估计，包括惯性导航参数误差、GPS接收机误差和惯性传感器误差的最优估计及其方差阵反馈回所述每个本地滤波器，以便重调该本地滤波器，执行该主滤波器和每个本地滤波器之间的信息分享。

Images