CN104297773A - 一种高精度北斗三频sins深组合导航*** - Google Patents

Abstract

本发明涉及一种高精度北斗三频SINS深组合导航***，考虑了北斗接收机导航的误差消除方法和模糊度解算方法，以及北斗导航与捷联惯导SINS的互相辅助的组合导航问题，利用TCAR方法，准确高效的解算出整周模糊度，从而进行高精度的载波相位测量；通过三个频率的数据信号，根据电离层折射误差计算公式构成的方程组，可以解出电离层折射误差，提高定位精度。本发明重点解决了深组合导航***对SINS的精度依赖问题，通过北斗三频的数据融合提高了接收机的导航精度，降低了对SINS自身姿态、速度、位置的精度要求，提高了北斗三频接收机和SINS组合导航***的抗干扰能力和综合精度，可以提高武器装备和重要民用载运工具的性能。

Description

一种高精度北斗三频SINS深组合导航***

技术领域

本发明涉及一种高精度北斗三频SINS深组合导航***，属于组合导航领域。

背景技术

中国北斗卫星导航***正按着三步走的发展战略稳步推进，目前已经具备了区域导航能力。完成三步走战略后，将具有全球导航能力。北斗卫星能够同时播发三个频率的信号，分别为B1(1561.1MHz)、B2(1207.14MHz)、B3(1268.52MHz)。随着北斗卫星导航技术的成熟，在军民两个领域得到了广泛的应用。

卫星导航相比传统的惯性导航技术，具有以下特点：全球连续覆盖，实现了全球导航定位的无缝连接；精度高且不依赖于接收机所处的位置；定位速度快，实时性好；采用扩频技术，抗干扰、抗多径、保密性好。综上所述，北斗***能提供全球覆盖、全天候的连续高精度定位、导航和授时。但是在载体高动态或信号遮挡的情形下，卫星导航接收机跟踪环路容易失锁或无法正常接收卫星信号，导致导航工作中断。

惯性导航技术是一种不依赖外界信号源的自主导航技术，具有完全自主、高度隐蔽、信息全面、实时性好、轻小型、高可靠等特点，但是惯性导航由于固有的惯性器件误差，导航误差会随着时间积累，影响导航的精度和准确度。

由此可见，卫星导航与惯性导航具有很强的互补性，能够相互弥补不足，两者进行组合后可充分发挥各自特点，发挥各自所长，得到了极大地发展。目前，组合导航方式根据信息融合的程度，可以分为以下三种：第一，基于位置、速度松散组合方式(松组合)；第二，基于伪距、伪距率紧组合方式(紧组合)；第三，基于伪距、伪距率，具有惯性辅助GNSS的深组合方式(深组合)。深组合是在硬件级进行一体化的组合方式。其主要思路是将SINS和卫星接收机在同相和正交相位信号(I和Q)级别上进行深度耦合，在接收机内部实现SINS和卫星导航量测信息的深度最优融合，有效提高接收机在弱信号、高动态、射频干扰环境下的性能。SINS辅助反馈所包含的载体动态信息不仅可以减小卫星导航接收机码环和载波环跟踪载体的动态，从而减小码环和载波环的等效带宽，提高整个***在高动态环境下的抗干扰能力，还可以辅助GNSS接收机快速重新捕获信号，提高灵敏度，增强GNSS的可用性和连续性。深组合相比松组合、紧组合，具有更高的抗干扰能力、动态性能和可靠性。

目前，国内深组合导航技术是基于单频点的卫星导航与SINS构成的深组合导航技术。这种深组合形式，能够发挥深组合固有的优势，但是对于消除接收机的电离层误差、载波相位整周模糊度的解算能力有限，导致定位、测速精度较低。

如果采用单频卫星导航/SINS深组合导航***，由于卫星导航的精度有限，会相应的提高对SINS的要求，而较高精度的SINS相比低精度的SINS，具有体积、重量、功耗等方面的不利因素，具体包括以下几方面：

a)单频接收机由于观测数据单一，无法通过联立方程组消除电离层折射引起的误差，因此无法得到较高的定位精度；

b)同样，观测数据单一，无法解算载波相位整周模糊度，因此不能得到高精度的载波相位观测值；

c)单频接收机的定位精度，限制了对SINS的辅助作用，也限制了组合导航对接收机捕获跟踪的辅助作用；

d)为了适应高动态的工作环境，避免接收机的失锁，需要高精度的SINS进行辅助，而高精度的SINS具有体积重量大、功耗高、成本高等特点，不利于工程应用。

因此，相对于单频北斗卫星导航/SINS深组合导航***，需要一种深组合导航***能够结合三频接收机的高精度、抗干扰优势，降低对SINS的精度要求，可以避免体积、重量、功耗等一系列问题，同时具有较高的组合导航精度和可靠性，在工程实用性方面具有独特的优势，同时精度指标可以满足当前大多数的应用场合。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种高精度高动态北斗三频/SINS深组合导航***，解决了深组合导航***对SINS的精度依赖问题，通过北斗三频的数据融合提高了接收机的导航精度，降低了对SINS自身姿态、速度、位置的精度要求，提高了北斗三频接收机/SINS组合导航***的抗干扰能力和综合精度，可以提高武器装备和重要民用载运工具的性能。

本发明的技术解决方案：一种高精度北斗三频SINS深组合导航***，其特征在于：包含北斗三频天线、数据预处理模块、误差消除与数据融合模块、定位解算模块、数据同步模块、捷联解算模块、组合滤波算法模块、惯性测量单元IMU；北斗三频天线接收来自北斗导航卫星B1、B2、B3三个频点的信号，数据处理模块对B1、B2、B3三个频点的信号分别进行数据采集、数字下变频、载波剥离、伪码剥离获得数据码和数据接收时间，通过数据码解析得到B1、B2、B3三个频点的信号的伪距观测量送至误差消除与数据融合模块，误差消除与数据融合模块中通过卫星信号在电离层中的传播关系得到的伪距观测量方程组，解算该方程组得到电离层误差，并予以消除，得到修正后的伪距观测量ρ_G以及伪距率观测量定位解算模块将修正后的伪距观测量ρ_G进行最小二乘法定位解算，获得钟差信号用于修正接收机时钟，定期输出PPS秒脉冲同步信号送至数据同步模块；惯性测量单元IMU输出感测的视速度增量信号和角度增量信号，数据同步模块对感测的视速度增量信号和角度增量信号通过数据插值，完成与卫星信号的数据同步，同步后的视速度增量信号和角度增量信号送至捷联解算模块，捷联解算模块采用单子样四元数算法实现捷联解算，得到载体当前的姿态、位置、速度，结合卫星星历计算得到的卫星位置、速度得到伪距ρ_I、伪距率组合滤波算法模块根据捷联解算模块得到的ρ_I和与误差消除与数据融合模块输出的ρ_G和的差值作为组合Kalman滤波量测方程的观测量，完成***状态量的误差估计，以修正捷联解算模块输出的姿态、位置、速度；修正后的姿态、速度、位置作为***的输出提供给用户，同时输出的速度、位置和IMU提供的加速度送至数据预处理模块，辅助载波剥离和伪码剥离。

所述的一种高精度北斗三频SINS深组合导航***的组合滤波算法模块中，Kalman滤波方程中状态量为：载体的三维速度误差、三维姿态误差、三维位置误差及接收机时钟偏差和时钟漂移，观测量为伪距误差和伪距率误差。

本发明与现有技术相比具有如下优点：

(1)本发明对卫星导航信号在电离层中的传播过程中折射率变化引起的传播路径距离误差及相位误差进行了理论分析，结合三频改正技术与伪距码测量相结合，可进一步提高伪距码测量的精度；

(2)高精度的北斗三频接收机的定位输出，深组合导航可以降低对SINS的精度要求，可以选用体积小、重量轻的惯导产品，同时降低了***的功耗和成本。

(3)本发明相对于北斗单频卫星导航/SINS深组合导航***，可以消除电离层折射引起的传播距离误差和伪码测距误差，能够可靠地计算出载波相位整周模糊度，从而提高卫星导航的定位测速精度。

附图说明

图1为高精度北斗三频/SINS深组合导航***总体框图；

图2为SINS辅助接收机捕获跟踪原理框图；

图3为低动态测试的一种高精度北斗三频SINS深组合导航***速度精度测试结果；

图4为低动态测试的一种高精度北斗三频SINS深组合导航***位置精度测试结果；

图5为高动态测试的一种高精度北斗三频SINS深组合导航***角度精度测试结果；

图6为高动态测试的一种高精度北斗三频SINS深组合导航***速度精度测试结果；

图7为高动态测试的一种高精度北斗三频SINS深组合导航***位置精度测试结果。

(五)具体实施方式

以下结合附图，进一步说明本发明一种高精度北斗三频SINS深组合导航***的实现步骤，如图1：

第一步：通过北斗三频天线接收B1、B2、B3三频卫星导航信号；

第二步：将第一步得到的B1、B2、B3信号经过带通滤波、低噪声放大、模拟混频和双路AD正交采样。AD采集得到的信号经过复相位旋转数字下变频、解调解扩，得到数据码以及卫星时钟校正参数、星历参数和历书参数。利用三频信号消除电离层误差的高阶项，以修正伪距测量值；利用三频信号观测量计算得到载波相位模糊度，以实现对伪距的平滑。

其中，ρ_s，k、ρ_s，k-1分别为k、k-1时刻的载波平滑伪距，M为平滑时间常数，λ为载波波长，分别为k、k-1时刻的载波相位。

第三步：根据接收机提供的秒脉冲(PPS)信号，对IMU数据进行时间打标和数据插值，实现数据同步。

第四步：对同步后的IMU数据进行捷联解算得到载体的位置、速度，联合导航卫星的位置、速度得到计算伪距、伪距率，计算伪距、伪距率与卫星导航解算得到的平滑伪距和伪距率做差，得到对应的伪距、伪距率残差观测量。

第五步：将伪距、伪距率残差观测量代入kalman滤波模型，估计SINS的状态误差(位置、速度、姿态)。

第六步：将第五步得到状态误差用于SINS的误差补偿，姿态补偿采用四元数法进行，位置和速度补偿采用直接扣除法。

第七步：将第六步补偿后得到的SINS位置、速度以及IMU的加速度用于辅助接收机的环路捕获和跟踪。

通过卫星导航信号在电离层中的传播关系，利用北斗三频信号消除电离层误差的高阶项，可以提高伪距测量的精度；根据三维观测信号构成双差观测模型，计算载波相位模糊度，最终实现伪距载波平滑。高精度的卫星导航输出与SINS的输出，通过Kalman滤波估计***的状态误差，用于补偿SINS的输出误差。补偿后的SINS输出用于辅助接收机的捕获跟踪，以避免接收机在高动态情形下的失锁和工作失常。

北斗三频/SINS深组合导航***是建立在北斗三频接收机和SINS基础上的，是在硬件级进行一体化的组合方式。其主要思路是将北斗接收机和SINS在同相和正交相位信号(I和Q)级别上进行深度耦合，在接收机内部实现SINS和北斗导航量测信息的深度最优融合，有效提高接收机在弱信号、高动态、射频干扰环境下的性能。具体实施方式：采用标量跟踪结构的北斗三频接收机利用三频信号的观测量消除电离层折射误差高阶项以得到高精度的伪距测量值，采用TCAR方法和矩阵变换法可以进行整周模糊度的解算，从而得到高精度的载波相位测量以实现伪距平滑。以伪距、伪距率为观测量建立组合Kalman滤波模型，对SINS的输出误差进行估计，利用误差估计值对SINS的输出进行闭环补偿修正，姿态修正采用四元数法，修正后的SINS输出位置、速度、加速度辅助接收机内部码和载波跟踪环路，防止接收机在高动态情形下失锁或缩短失锁后的重捕时间。

利用北斗三频消除电离层误差的具体过程如下：

导航卫星信号在电离层中的传播与频率有关，其单一频率的相位传播折射率n_s与测距码群波传播率n_g有如下关系：

$n_g=n_s+f\frac{{\mathrm{dn}}_s}{\mathrm{df}}---\left(1\right)$

式(1)中，f为信号载波频率。

在离子化的大气中，折射率的弥散公式可简化为：

n_S＝1+a₁/f²+a₂/f³+...    (2)

式(2)中，a₁、a₂为简写后的系数。于是，可得

n_g＝1-a₁/f²-a₂/f³+...    (3)

当卫星信号穿过电离层时，由折射率变化引起的传播路径距离误差及相位误差为：δρ_s＝∫(n_S-1)ds、δρ_g＝∫(n_g-1)ds和结合(2)和(3)式有：

$\left\{\begin{array}{c}{\mathrm{\δ\ρ}}_s=\∫(a_1/f^2)\mathrm{ds}+\∫(a_2/f^3)\mathrm{ds}\\ {\mathrm{\δ\ρ}}_g=\∫(a_1/f^2)\mathrm{ds}-2\∫(a_2/f^3)\mathrm{ds}\end{array}\right.---\left(4\right)$

利用三个载波进行电离层误差的高阶项消除，令A₁＝∫a₁ds，A₂＝∫a₂ds，可得

δρ_s(f_i)＝A₁/f_i ²+A₂/f_i ³，i＝1，2，3    (5)

由式(5)可以导出无电离层误差二阶项的组合：

f₁ ³(f₃-f₂)δρ_s(f₁)+f₂ ³(f₁-f₃)δρ_s(f₂)+f₃ ³(f₂-f₁)δρ_s(f₃)＝0    (6)

因为由式(6)可得

令i＝f₁ ²(f₃-f₂)，j＝f₂ ²(f₁-f₃)，k＝f₃ ²(f₂-f₁)，则有无电离层误差的三频载波相位组合观测方程

式(8)中，分别为B1、B2、B3三频信号的载波相位。取三频信号同步观测所得观测站至卫星的距离分别为ρ₁、ρ₂、ρ₃，而消除电离层误差的传播路径为ρ₀，则有：

$\left\{\begin{array}{c}{\mathrm{\ρ}}_1={\mathrm{\ρ}}_0+A_1/{f_1}^2+A_2/{f_1}^3\\ {\mathrm{\ρ}}_2={\mathrm{\ρ}}_0+A_1/{f_2}^2+A_2/{f_2}^3\\ {\mathrm{\ρ}}_3={\mathrm{\ρ}}_0+A_1/{f_3}^2+A_2/{f_3}^3\end{array}\right.---\left(9\right)$

利用方程组(9)，可以计算得到A₁、A₂。分别代入式(5)，可得δρ_s(f₁)、δρ_s(f₂)和δρ_s(f₃)。然后，利用式(4)，可得δρ_g(f₁)、δρ_g(f₂)和δρ_g(f₃)。这样，就可以进行电离层延迟距离误差修正。通过三频观测数据，可将电离层误差修正至毫米级，基本上可消除电离层的影响，对北斗定位中的其它误差的分离起到很大的帮助作用，同时三频修正技术与伪距码测量相结合，可进一步提高伪距码测量的精度。

利用北斗三频，使用单历元解算模糊度TCAR解算方法过程如下：

TCAR方法由以下几个步骤组成：

(1)建立双差观测模型

式(10)中，为双差伪距观测值；为双差卫-地真实距离；为码观测噪声；λ_i为载波波长；为双差载波相位观测值；为整周模糊度；为载波相位噪声。

(2)计算甚宽巷模糊度(f₂-f₃)

对就近取整，其精度由下式决定：

利用式(11)和(12)可求解出的估值：

(3)计算宽巷模糊度(f₁-f₂)

就近取整，则可得的估值

(4)计算任一频率的模糊度f₃

(5)取整后的进行载波相位伪距平滑。

利用伪距进行定位的过程如下：

伪距方程为

${\mathrm{\ρ}}_{\mathrm{cj}}=\sqrt{(x_j-x_u)+(y_j-y_u)+(z_j-z_u)}+b_u+v_j---\left(17\right)$

式(17)中，v_j为修正后的伪距残差，(x_u，y_u，z_u)和(x_j，y_j，z_j)分别表示用户和卫星j(j＝1，2，3，......)在ECEF坐标系中的位置坐标，ρ_cj为经过伪距改正的卫星j到用户的伪距，b_u为用户钟差产生的等效距离误差，观测卫星数为n，接收机位置和时钟偏差由概略位置坐标、概略钟差和改正量两部分组成

$x_u={\hat{x}}_u+{\mathrm{\Δx}}_u---\left(18\right)$

$y_u={\hat{y}}_u+{\mathrm{\Δy}}_u---\left(19\right)$

$z_u={\hat{z}}_u+{\mathrm{\Δz}}_u---\left(20\right)$

$b_u={\hat{b}}_u+{\mathrm{\Δb}}_u---\left(21\right)$

对伪距观测方程在处进行泰勒级数展开，忽略高次项得到

${\mathrm{\ρ}}_{\mathrm{cj}}=R_j+{\hat{b}}_u+v_j-\frac{x_j-{\hat{x}}_u}{R_j}{\mathrm{\Δx}}_u-\frac{y_j-{\hat{y}}_u}{R_j}{\mathrm{\Δy}}_u-\frac{z_j-{\hat{z}}_u}{R_j}{\mathrm{\Δz}}_u+{\mathrm{\Δb}}_u---\left(22\right)$

定位方程常数项为接收机到卫星j的观测矢量为

$l_j=\frac{x_j-{\hat{x}}_u}{R_j},m_j=\frac{y_j-{\hat{y}}_u}{R_j},n_j=\frac{z_j-{\hat{z}}_u}{R_j}---\left(23\right)$

式(23)中，R_j为卫星j到概略位置的距离，故伪距残差可表示为

Δρ_j＝l_jΔx_u+m_jΔy_u+n_jΔz_u-Δb_u    (24)

式(24)中，有4个未知量Δx_u、Δy_u、Δz_u、Δb_u(Δb_u＝c·Δt)，当卫星颗数≥4颗时采用最小二乘法将它们解出来。

根据多普勒计算得到伪距率

${\mathrm{\ρ}}_{\mathrm{cj}}=f_d\frac{c}{f}---\left(25\right)$

式(25)中，f_d为多普勒频率，f_d为载波频率。

对于捷联惯导而言，选择地理系作为导航系，捷联计算过程如下：

(1)导航系相对于载体系的角速度

${\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}={\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bx}}-{\mathrm{\ω}}_{\mathrm{in}}^{\mathrm{bx}}-{\mathrm{fog}}_0^x$

${\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}={\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{by}}-{\mathrm{\ω}}_{\mathrm{in}}^{\mathrm{by}}-{\mathrm{fog}}_0^y---\left(26\right)$

${\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}={\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bz}}-{\mathrm{\ω}}_{\mathrm{in}}^{\mathrm{bz}}-{\mathrm{fog}}_0^z$

式(26)中，分别为惯组三个轴的输出，分别为导航系相对惯性在载体系的投影，为三个轴陀螺的零位。

(2)通过四元数更新姿态

$\left[\begin{array}{c}{\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right]\·\mathrm{\Δt}---\left(27\right)$

式(27)中，Δt为捷联解算周期，Δθ_x、Δθ_y、Δθ_z为对应的三个轴的角度增量。令 $\mathrm{\Δ\θ}=\sqrt{{\mathrm{\Δ\θ}}_x^2+{\mathrm{\Δ\θ}}_y^2+{\mathrm{\Δ\θ}}_z^2},$ 有

$\left[\begin{array}{c}{q}_{\mathrm{0,2}}\\ q_{\mathrm{1,2}}\\ q_{\mathrm{2,2}}\\ q_{\mathrm{3,2}}\end{array}\right]=\left[\begin{array}{c}\cos (0.5\·\mathrm{\Δ\θ})\\ \sin (0.5\·\mathrm{\Δ\θ})\frac{{\mathrm{\Δ\θ}}_z}{\mathrm{\Δ\θ}}\\ \sin (0.5\·\mathrm{\Δ\θ})\frac{{\mathrm{\Δ\θ}}_y}{\mathrm{\Δ\θ}}\\ \sin (0.5\·\mathrm{\Δ\θ})\frac{{\mathrm{\Δ\θ}}_z}{\mathrm{\Δ\θ}}\end{array}\right]---\left(28\right)$

式(28)中，[q_0，2、q_1，2、q_2，2、q_3，2]^T为四元素增量。设i时刻的四元数为[q_0，1 q_1，1 q_2，1 q_3，1]，则i+1时刻的四元数为

$\left[\begin{array}{c}{\mathrm{qq}}_0\\ {\mathrm{qq}}_1\\ {\mathrm{qq}}_2\\ {\mathrm{qq}}_3\end{array}\right]=\left[\begin{array}{cccc}{q}_{\mathrm{0,2}}& -q_{\mathrm{1,2}}& -q_{\mathrm{2,2}}& -q_{\mathrm{3,2}}\\ q_{\mathrm{1,2}}& q_{\mathrm{0,2}}& q_{\mathrm{3,2}}& -q_{\mathrm{2,2}}\\ q_{\mathrm{2,2}}& -q_{\mathrm{3,2}}& q_{\mathrm{0,2}}& q_{\mathrm{1,2}}\\ q_{\mathrm{3,2}}& q_{\mathrm{2,2}}& -q_{\mathrm{1,2}}& q_{\mathrm{0,2}}\end{array}\right]\left[\begin{array}{c}{q}_{\mathrm{0,1}}\\ q_{\mathrm{1,1}}\\ q_{\mathrm{2,1}}\\ q_{\mathrm{3,1}}\end{array}\right]---\left(29\right)$

四元数归一化

$\left[\begin{array}{c}{\mathrm{qq}}_0\\ {\mathrm{qq}}_1\\ {\mathrm{qq}}_2\\ {\mathrm{qq}}_3\end{array}\right]=\left[\begin{array}{c}{\mathrm{qq}}_0\\ {\mathrm{qq}}_1\\ {\mathrm{qq}}_2\\ {\mathrm{qq}}_3\end{array}\right]\·\frac{1}{E}---\left(30\right)$

式(30)中， $E=\sqrt{{\mathrm{qq}}_0^2+{\mathrm{qq}}_1^2++{\mathrm{qq}}_2^2+{\mathrm{qq}}_3^2}.$

T阵更新

$\left[\begin{array}{cccc}{\mathrm{qq}}_{\mathrm{0,0}}& {\mathrm{qq}}_{\mathrm{0,1}}& {\mathrm{qq}}_{\mathrm{0,2}}& {\mathrm{qq}}_{\mathrm{0,3}}\\ {\mathrm{qq}}_{\mathrm{1,0}}& {\mathrm{qq}}_{\mathrm{1,1}}& {\mathrm{qq}}_{\mathrm{1,2}}& {\mathrm{qq}}_{\mathrm{1,3}}\\ {\mathrm{qq}}_{\mathrm{2,0}}& {\mathrm{qq}}_{\mathrm{2,1}}& {\mathrm{qq}}_{\mathrm{2,2}}& {\mathrm{qq}}_{\mathrm{2,3}}\\ {\mathrm{qq}}_{\mathrm{3,0}}& {\mathrm{qq}}_{\mathrm{3,1}}& {\mathrm{qq}}_{\mathrm{3,2}}& {\mathrm{qq}}_{\mathrm{3,3}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{qq}}_0\\ {\mathrm{qq}}_1\\ {\mathrm{qq}}_2\\ {\mathrm{qq}}_3\end{array}\right]\left[\begin{array}{cccc}{\mathrm{qq}}_0& {\mathrm{qq}}_1& {\mathrm{qq}}_2& {\mathrm{qq}}_3\end{array}\right]---\left(31\right)$

则有T_0，0＝qq_0，0+qq_1，1-qq_2，2-qq_3，3，T_0，1＝2(qq_1，2-qq_0，3)，T_0，2＝2(qq_1，3-qq_0，2)，T_1，0＝2(qq_1，2+qq_0，3)，T_1，1＝qq_0，0-qq_1，1+qq_2，2-qq_3，3，T_1，2＝2(qq_2，3-qq_0，1)，T_2，0＝2(qq_1，3-qq_0，2)，T_2，1＝2(qq_2，3+qq_0，1)，T_2，2＝qq_0，0-qq_1，1-qq_2，2+qq_3，3

同时，有

式(32)中，θ、γ分别为航向角、俯仰角和横滚角。

根据更新后的T阵，得到姿态更新值为：

(3)速度更新

$\left[\begin{array}{c}{V}_{\mathrm{en}}^{\mathrm{nx}}\\ V_{\mathrm{en}}^{\mathrm{ny}}\\ V_{\mathrm{en}}^{\mathrm{nz}}\end{array}\right]=\left[\begin{array}{c}{V}_{\mathrm{en}}^{\mathrm{nx}}\\ V_{\mathrm{en}}^{\mathrm{ny}}\\ V_{\mathrm{en}}^{\mathrm{nz}}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{V}}_{\mathrm{en}}^{\mathrm{nx}}\\ {\stackrel{\·}{V}}_{\mathrm{en}}^{\mathrm{ny}}\\ {\stackrel{\·}{V}}_{\mathrm{en}}^{\mathrm{nz}}\end{array}\right]\mathrm{\Δt}---\left(34\right)$

式(34)中，分别为载体三个方向的速度，分别为载体三个方向的速度变化率，计算过程为

$\left[\begin{array}{c}{\stackrel{\·}{V}}_{\mathrm{en}}^{\mathrm{nx}}\\ {\stackrel{\·}{V}}_{\mathrm{en}}^{\mathrm{ny}}\\ {\stackrel{\·}{V}}_{\mathrm{en}}^{\mathrm{nz}}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{ib}}^{\mathrm{nx}}\\ f_{\mathrm{ib}}^{\mathrm{ny}}\\ f_{\mathrm{ib}}^{\mathrm{nz}}\end{array}\right]+\left[\begin{array}{ccc}0& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_{\mathrm{en}}^{\mathrm{nx}}}{R_N}\tan L& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_{\mathrm{en}}^{\mathrm{nx}}}{R_N})\\ -({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_{\mathrm{en}}^{\mathrm{nx}}}{R_N}\tan L)& 0& \frac{V_{\mathrm{en}}^{\mathrm{ny}}}{R_M}\\ 2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_{\mathrm{en}}^{\mathrm{nx}}}{R_N}& -\left(\frac{V_{\mathrm{en}}^{\mathrm{ny}}}{R_M}\right)& 0\end{array}\right]\left[\begin{array}{c}{V}_{\mathrm{en}}^{\mathrm{nx}}\\ V_{\mathrm{en}}^{\mathrm{ny}}\\ V_{\mathrm{en}}^{\mathrm{nz}}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

(4)位置更新

$\left[\begin{array}{c}L\\ \mathrm{\λ}\\ h\end{array}\right]=\left[\begin{array}{c}L\\ \mathrm{\λ}\\ h\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{L}\\ \stackrel{\·}{\mathrm{\λ}}\\ \stackrel{\·}{h}\end{array}\right]\mathrm{\Δt}---\left(35\right)$

式(35)中，L、λ、h分别为纬度、经度、高度，分别为纬度、经度、高度的变化率。

$\left[\begin{array}{c}\stackrel{\·}{L}\\ \stackrel{\·}{\mathrm{\λ}}\\ \stackrel{\·}{h}\end{array}\right]=\left[\begin{array}{c}\frac{V_{\mathrm{en}}^{\mathrm{ny}}}{R_M}\\ \frac{V_{\mathrm{en}}^{\mathrm{nx}}}{R_N\cos L}\\ V_{\mathrm{en}}^{\mathrm{nz}}\end{array}\right]---\left(36\right)$

以卫星导航得到的伪距和伪距率与SINS得到的伪距误差Z_ρ(t)和伪距率误差作为组合Kalman滤波的观测量，建立观测方程

$Z\left(t\right)=\left[\begin{array}{c}{Z}_{\mathrm{\ρ}}\left(t\right)\\ Z_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{H}_{\mathrm{\ρ}}\left(t\right)\\ H_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\ρ}}\left(t\right)\\ V_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)\end{array}\right]=H\left(t\right)\·X\left(t\right)+V\left(t\right)---\left(37\right)$

式(37)中，H_ρ(t)、为n×11维的伪距量测矩阵和伪距率量测矩阵；V_ρ(t)、为伪距量测噪声阵和伪距率量测噪声阵；n为可见星的颗数。

以东北天地理坐标系作为导航坐标系，建立***的状态方程为：

$\stackrel{\·}{X}\left(t\right)=\left[\begin{array}{c}{\stackrel{\·}{X}}_I\left(t\right)\\ {\stackrel{\·}{X}}_G\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_I\left(t\right)& 0\\ 0& F_G\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_I\left(t\right)\\ X_G\left(t\right)\end{array}\right]+\left[\begin{array}{cc}{G}_I\left(t\right)& 0\\ 0& G_G\left(t\right)\end{array}\right]\left[\begin{array}{c}{W}_I\left(t\right)\\ W_G\left(t\right)\end{array}\right]=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(38\right)$

式(38)中，下标I表示惯导的参数变量，下标G表示卫星导航***的参数变量。选取11个参数作为状态变量， ${\left[X_I\left(t\right)X_G\left(t\right)\right]}^T={\left[\begin{array}{cccccccc}{\mathrm{\δV}}_{\mathrm{en}}^{\mathrm{nx}}& {\mathrm{\δV}}_{\mathrm{en}}^{\mathrm{ny}}& {\mathrm{\δV}}_{\mathrm{en}}^{\mathrm{nz}}& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\mathrm{\δt}}_u& {\mathrm{\δt}}_{\mathrm{ru}}\end{array}\right]}^T$ 分别为载体的三维速度误差、三维姿态误差、三维位置误差及接收机时钟偏差和时钟漂移； $F\left(t\right)=\left[\begin{array}{cc}{F}_I\left(t\right)& 0\\ 0& F_G\left(t\right)\end{array}\right],$ 是11×11维的状态转移矩阵；G(t)是***噪声驱动矩阵；W(t)是噪声驱动向量。

以状态方程和观测方程组成的Kalman滤波基本方程，进行状态误差估计。利用估计的状态误差，对捷联惯导解算的速度、位置、姿态进行修正。

在接收机进行捕获时加入INS速度信息辅助，预报下一个时刻的多普勒频移，预先估计接收信号的频率范围，在此范围内搜索，就可以缩短接收机搜索捕获的时间，提高接收机的性能。

卫星和载体运动产生的多普勒频率为：

$f_{\mathrm{rec}}+f_s=\frac{1}{\mathrm{\λ}}\·({\stackrel{\→}{V}}^{\mathrm{rec}}-{\stackrel{\→}{V}}^s)\·{\stackrel{\→}{e}}^d=\frac{1}{\mathrm{\λ}}\·{\stackrel{\→}{V}}^{\mathrm{rec}}{\stackrel{\→}{e}}^d-\frac{1}{\mathrm{\λ}}\·{\stackrel{\→}{V}}^s{\stackrel{\→}{e}}^d---\left(39\right)$

式(39)中，λ为载波的波长；为载体的速度；为卫星的速度；为卫星到用户连线方向的单位矢量。可由卫星历书估算得到，可通过SINS提供。

同时，估计的载波多普勒频率可以辅助接收机进行跟踪。根据惯导输出的加速度信息预测出下一时刻输入信号载波频率的变化量，将此变化量直接加至本地载波的输出，从而消除环路动态应力。载体相对于卫星的加速度为

$a_{\mathrm{us}}^E={\stackrel{\→}{a}}_u^E\·\frac{{\stackrel{\→}{r}}_u^E-{\stackrel{\→}{r}}_s^E}{|{\stackrel{\→}{r}}_u^E-{\stackrel{\→}{r}}_s^E|}---\left(40\right)$

式(40)中，为惯导给出的加速度在地心地固坐标系下的值，根据多普勒频移公式，卫星多普勒频移的增量为：

${\mathrm{\Δf}}_i=\frac{a_{\mathrm{us}}^E\·T}{c}\·f_{L1}---\left(41\right)$

式(41)中，T为环路更新周期。将Δf_i加至i通道载波跟踪已通过PLL计算得到的载波频率上，产生本地载波，同时也加至本地载波中心频率以修正中心频率。

采用模拟源产生高动态的卫星信号和惯导信号，验证北斗三频SINS深组合导航***的效果。

如图2所示为预处理模块到定位解算模块的处理流程：SINS输出的位置、速度经过误差补偿后和IMU提供的加速度信息送至多普勒和码移计算模块，多普勒和码移计算模块预测出下一时刻的多普勒和码移量分别送至伪码PRN发生器、载波发生器，PRN发生器产生本地伪码用于码跟踪，载波发生器产生本地载波用于载波跟踪，码跟踪&载波跟踪模块实现信号跟踪后，卫星信号再经过信号解调、位同步、帧同步、伪距观测量计算和误差消除后，最后送至定位解算模块实现定位解算。

如图3、图4所示，利用北斗三频信号进行低动态测试(速度＜500m/s)，卫星导航的定位精度有10m提高到了5m，测速精度由0.2m/s提高到了0.08m/s。在高动态情况下(速度2000m/s，加加速度30m/s^3)，没有SINS辅助接收机捕获跟踪时，接收机发生失锁而无法正常工作；当采用低精度SINS辅助接收机时，接收机环路能够稳定跟踪，正常工作。

如图5、6、7，为***的高动态情形下的测试结果，姿态测量误差＜0.1°，测速误差＜0.1m/s，定位误差＜10m。

上面的实施方式中，北斗三频是指北斗二代三频B1(1561.1MHz)、B2(1207.14MHz)、B3(1268.52MHz)，SINS采用小型化的光纤惯导或MEMS惯导。本发明旨在保护北斗二代三频与SINS深组合导航的***技术方案。本领域的技术人员可以在本发明的启示下，采用北斗三频卫星导航与航位推算技术(DR)构成的组合导航***，或GPS/BD多频与SINS构成的深组合导航***，或北斗双频/SINS的深组合导航，不属于本发明的保护范围。

Claims (2)

1.一种高精度北斗三频SINS深组合导航***，其特征在于：包含北斗三频天线(1)、数据预处理模块(2)、误差消除与数据融合模块(3)、定位解算模块(4)、数据同步模块(5)、捷联解算模块(6)、组合滤波算法模块(7)、惯性测量单元IMU(8)；北斗三频天线(1)接收来自北斗导航卫星B1、B2、B3三个频点的信号，数据处理模块(2)对B1、B2、B3三个频点的信号分别进行数据采集、数字下变频、载波剥离、伪码剥离获得数据码和数据接收时间，通过数据码解析得到B1、B2、B3三个频点的信号的伪距观测量送至误差消除与数据融合模块(3)，误差消除与数据融合模块(3)中通过卫星信号在电离层中的传播关系得到的伪距观测量方程组，解算该方程组得到电离层误差，并予以消除，得到修正后的伪距观测量ρ_G以及伪距率观测量定位解算模块(4)将修正后的伪距观测量ρ_G进行最小二乘法定位解算，获得钟差信号用于修正接收机时钟，定期输出PPS秒脉冲同步信号送至数据同步模块(5)；惯性测量单元IMU(8)输出感测的视速度增量信号和角度增量信号，数据同步模块对感测的视速度增量信号和角度增量信号通过数据插值，完成与卫星信号的数据同步，同步后的视速度增量信号和角度增量信号送至捷联解算模块(6)，捷联解算模块(6)采用单子样四元数算法实现捷联解算，得到载体当前的姿态、位置、速度，结合卫星星历计算得到的卫星位置、速度得到伪距ρ_I、伪距率组合滤波算法模块(7)根据捷联解算模块(6)得到的ρ_I和与误差消除与数据融合模块(3)输出的ρ_G和的差值作为组合Kalman滤波量测方程的观测量，完成***状态量的误差估计，以修正捷联解算模块输出的姿态、位置、速度；修正后的姿态、速度、位置作为***的输出提供给用户，同时输出的速度、位置和IMU提供的加速度送至数据预处理模块(2)，辅助载波剥离和伪码剥离。

2.根据权利要求1所述的一种高精度北斗三频SINS深组合导航***，其特征在于：所述组合滤波算法模块(7)中，Kalman滤波方程中状态量为：载体的三维速度误差、三维姿态误差、三维位置误差及接收机时钟偏差和时钟漂移，观测量为伪距误差和伪距率误差。