CN103487820A - 一种车载捷联/卫星紧组合无缝导航方法 - Google Patents

Abstract

本发明公开了一种车载捷联/卫星紧组合无缝导航方法，主要包括如下步骤：系统初始化；GPS输出报文、SINS输出惯性测量单元数据，并且进行同步；解析GPS报文，获得粗伪距、多普勒频移、卫星位置参数，并对参数进行补偿，得到精伪距、伪距率、卫星位置参数；同时对惯性测量单元数据进行导航解算，得到车辆的姿态、速度和位置参数；判断可见卫星数目是否大于等于4颗，若大于等于4颗，选择其中4颗卫星进行紧组合滤波解算；若卫星数小于4颗，运用智能组合算法；输出车辆导航参数，如果继续导航则返回至步骤S2。本发明通过紧组合导航和智能组合算法实现不同可见卫星状态的无缝切换，为车辆无缝导航提供可靠保证。

Description

一种车载捷联/卫星紧组合无缝导航方法

技术领域

本发明涉及导航方法，尤其是一种车载捷联/卫星紧组合无缝导航方法。

背景技术

为了获得移动载体的实时位置和姿态信息，人们提出和采用了多种导航方式。在这些导航方式里，以捷联惯性导航系统(Strap down Inertial Navigation System,简称SINS)和全球卫星定位系统(Global Positioning System,简称GPS)应用最为广泛。SINS与GPS系统的组合方式很多，主要有松组合、紧组合、深组合三大类。松组合方式直接利用GPS接收机输出的速度和位置信息与SINS组合。深组合方式利用SINS信息来辅助GPS进行卫星的跟踪捕获。紧组合方式利用了GPS的原始信息和SINS组合，由于避开了二次滤波，紧组合在一定程度上提高了导航精度。伪距、伪距率组合的方式是紧组合模式的典型代表。它利用GPS输出的原始测量数据，测码伪距和多普勒频移，与由SINS输出的位置和速度信息计算得到的SINS伪距、伪距率进行比较，将差值作为滤波器的观测量，经过滤波器的最优估计，给出校正SINS的补偿量。对于精度与稳定性不高的惯导系统，紧组合的无缝导航模式能够很好的弥补其不足，大幅提高组合系统的性能。早在1990年左右国外就已经进行了紧组合系统的实验研究，并在1997年左右实现了在工程中的应用，从2001年开始，美国绝大部分主战飞机上已将采用紧组合技术的EGI(Embedded-GPS/SINS)接收机逐步取代单GPS接收机，并最终淘汰单GPS接收机，我国目前在该方面的实验应用仍多以松组合方式为主。

SINS/GPS紧组合导航系统在我国仍处于研制试验阶段，主要的研究力量集中在高校和科研院所。紧组合导航没有一个全面的性能评估，工程应用转化的研究较少。这有两方面的原因，第一是由于SINS/GPS紧组合技术涉及到GPS接收机内部的技术细节，需要伪距、伪距率、卫星星历的原始观测输出以及时钟特性稳定等多种条件，而目前国内基本采用现成的GPS接收机模块，二次开发困难；第二是紧组合技术的计算量比较大，国内导航系统的硬件平台计算能力难以满足需求，需要研究既简化运算又满足导航精度的软件算法，或者提高硬件平台的计算能力。这在一定程度上都制约了紧组合的深入研究，因此目前SINS/GPS组合导航多采用松组合方式，紧组合的研究发展缓慢。

发明内容

发明目的：本发明要提供一种车载捷联/卫星紧组合无缝导航方法，以解决上述问题。

技术方案：一种车载捷联/卫星紧组合无缝导航方法，包括如下步骤：

S1、系统初始化；

S2、GPS输出报文、SINS输出惯性测量单元数据，并且进行同步；

S3、解析GPS报文，获得粗伪距、多普勒频移、卫星位置参数，并对参数进行补偿，得到精伪距、伪距率、卫星位置参数；同时对惯性测量单元数据进行导航解算，得到车辆的姿态、速度和位置参数；

S4、判断可见卫星数目是否大于等于4颗，若大于等于4颗，选择其中4颗卫星进行紧组合滤波解算；若卫星数小于4颗，运用智能组合算法；

S5、输出车辆导航参数，如果继续导航则返回至步骤S2；

S6、导航结束，退出。

上述步骤中，当出现GPS信号阻塞或者失锁的状况时，SINS/GPS组合导航切换至纯捷联模式，即单独使用SINS进行导航，输出车辆的姿态、速度和位置信息。

步骤S3中对GPS参数进行补偿具体为：对GPS信号在电离层传播折射过程进行数学建模，利用GPS双频接收机两个频段L1和L2的观测量估计出电离层延迟，从而得到更加精确的卫星伪距、伪距率和卫星位置。

步骤S4中，若可见卫星大于4颗，则选择4颗卫星进行紧组合滤波解算，根据可见卫星的数量构建状态方程和观测方程进行紧组合滤波解算。

步骤S4中，所述智能组合算法包括可见卫星的无缝隙切换、卫星双频恢复技术和路面车辆的运动约束。

所述可见卫星的无缝隙切换是指根据当前可见卫星的数量调整观测向量的维数，更改观测方程，在不同的可见卫星数之间实现观测模型的无缝隙切换。

所述卫星双频恢复技术是指采用之前信号较好时的GPS卫星双频信号计算出的电离层延迟误差值对现有单频卫星的伪距进行补偿，相当于此时将单频卫星恢复为先前双频信号时的精度水平。

所述路面车辆的运动约束包括：当地面平整、车辆无侧移时，设定车辆速度在前进方向上分量最大，车辆的侧向和天向的速度分量为0，等价于增加新的观测量，保证系统导航的连续性。

所述紧组合滤波解算为：根据可见卫星的数量构建状态方程和观测方程进行紧组合滤波解算，其中状态变量由17个参数组成，

X＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δλ δh ▽b_x ▽b_y ▽b_z ε_bx ε_by ε_bz δt_u δt_ru]^T，其中，δV_E,δV_N,δV_U是东北天三个方向上的速度误差，

捷联的三个失准角，δL,δλ,δh是捷联的三个位置误差由地球坐标系描述，▽_bx,▽_by,▽_bz是加表三个轴向的零偏误差，ε_bx,ε_by,ε_bz是陀螺的三个轴向漂移，δt_u是时钟误差等效的距离，δt_ru是时钟频率误差等效的距离变化率；

状态方程为：

$\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]$

其中X_I(t)＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δλ δh ▽b_x ▽b_y ▽b_z ε_bx ε_by ε_bz]是关于SINS的状态向量，X_G(t)＝[δt_u δt_ru]是关于GPS的状态向量，F_I(t)是惯导系统误差状态方程状态矩阵，而F_G(t)是对GPS相关的两个状态量建模后得到的状态矩阵，G_I(t)和G_G(t)是噪声的输入矩阵，W_I(t),W_G(t)分别是关于SINS和GPS的系统噪声向量，伪距、伪距率观测方程：

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

其中，观测向量 $Z\left(t\right)={\left[\begin{array}{cc}{\mathrm{\δ\ρ}}_j& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j\end{array}\right]}^T,$ j=1,2,3...n，其中

分别为伪距和伪距率观测量，

分别对应伪距、伪距率的观测矩阵，V_ρ(t),V_ρ(t)分别对应伪距、伪距率的观测噪声向量。

所述同步方法包括利用GPS接收机的1PPS秒脉冲信号进行同步的硬件同步法，基于数据融合的软件同步法和基于SINS数据存储同步法。

所述路面车辆的运动约束算法为：当地面平整、车辆无侧移时，令

$\left\{\begin{array}{c}{V}_x^b=0\\ V_z^b=0\end{array}\right.---\left(64\right)$

取b系的捷联输出的速度与上式的速度作差，得

$\left\{\begin{array}{c}{\mathrm{\δV}}_x^b=V_{\mathrm{SINSx}}^b-V_x^b=V_{\mathrm{SINSx}}^b\\ {\mathrm{\δV}}_z^b=V_{\mathrm{SINSz}}^b-V_z^b=V_{\mathrm{SINSz}}^b\end{array}\right.---\left(65\right)$

令n系到b系的转换矩阵为

则有

$V^b=C_n^b{V}^n---\left(66\right)$

对式(66)求微分得

$\begin{array}{c}{\mathrm{\δV}}^b=C_n^b{\mathrm{\δV}}^n+\left({\mathrm{\δC}}_n^b\right)V^n=C_n^b\mathrm{\δ}{V}^n-({\mathrm{\ω}}_{\mathrm{en}}^n\×)C_n^b{V}^n\\ =C_n^b{\mathrm{\δV}}^n-{\mathrm{\ω}}_{\mathrm{en}}^n\×V^b=C_n^b\mathrm{\δ}{V}^n+V^b\×{\mathrm{\ω}}_{\mathrm{en}}^n\\ =C_n^b\mathrm{\δ}{V}^n+C_n^b{V}^n\×{\mathrm{\ω}}_{\mathrm{en}}^n\end{array}---\left(67\right)$

式中：

是地球坐标系相对东北天坐标系的旋转角速度，由于地球半径很大而载体一般运动速度相对较小，运动约束中低动态时可忽略此项；此外考虑到SINS的计算坐标系和东北天坐标系存在失准角故可由式(66)得

$(V_{\mathrm{SINS}}^{n^{\′}}\×)=(V^{n^{\′}}\×)=\left[\begin{array}{ccc}0& -V_U& V_N\\ V_U& 0& -V_E\\ -V_N& V_E& 0\end{array}\right]---\left(69\right)$

由公式(65)、(67)、(68)得：

由(70)得量测方程

式(70)(71)只对载体坐标系的侧向与天向成立；

得到量测方程为：

$Z_V=\left[\begin{array}{c}{\left(C_n^b{V}_{\mathrm{SINS}}^{n^{\′}}\right)}_x\\ {\left(C_n^b{V}_{\mathrm{SINS}}^{n^{\′}}\right)}_z\end{array}\right]=H_{V}X+V_V---\left(72\right)$

H_V＝[A_2*3 B_2*3 0_2*9]   (73)

$A=\left[\begin{array}{ccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)\end{array}\right]---\left(74\right)$

$B=\left[\begin{array}{ccc}{V}_U{C}_n^b\left(\mathrm{1,2}\right)-V_N{C}_n^b\left(\mathrm{1,3}\right)& V_E{C}_n^b\left(\mathrm{1,3}\right)-V_U{C}_n^b\left(\mathrm{1,2}\right)& V_N{C}_n^b\left(\mathrm{1,1}\right)-V_E{C}_n^b\left(\mathrm{1,2}\right)\\ V_U{C}_n^b\left(\mathrm{3,2}\right)-V_N{C}_n^b\left(\mathrm{3,3}\right)& V_E{C}_n^b\left(\mathrm{3,3}\right)-V_U{C}_n^b\left(\mathrm{3,2}\right)& V_N{C}_n^b\left(\mathrm{3,1}\right)-V_E{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right]---\left(75\right)$

式(71)方程不能用两边同时右乘姿态矩阵的方式得到简化式；

联立式(63)、(72)得：

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

添加运动约束后，公式(76)构成了适用于载体为车辆时的SINS/GPS紧组合导航系统的量测方程。

有益效果：本发明提出了一种车载捷联/卫星紧组合无缝导航方法，通过紧组合导航和智能组合算法实现不同可见卫星状态的无缝切换，为车辆无缝导航提供可靠保证。

附图说明

图1为本发明SINS/GPS紧组合无缝导航原理图；

图2为本发明基于1PPS的硬件同步原理图；

图3为本发明软件同步方法的时间分析图；

图4为本发明跑车路线示意图；

图5为本发明跑车试验可见卫星数目；

图6为本发明卫星双频恢复后的可用卫星数目；

图7a至图7i为本发明跑车智能组合导航结果曲线。

具体实施方式

如图1至图7i所示，本发明的主要步骤为：

S1、系统参数初始化；

S2、GPS输出报文、SINS输出IMU数据，并且依靠GPS输出的1PPS秒信号进行同步；GPS输出报文采用伪距报文#RANGE以及星历报文#GPSEPHEM，输出频率为20Hz。所述的SINS输出的IMU数据包括陀螺仪的角速率和加速度计的加速度信息，输出频率为200Hz，并且依靠GPS输出的秒脉冲信号1PPS进行SINS和GPS数据的同步。

S3、解析GPS报文，获得粗伪距、多普勒频移、卫星位置参数，并对参数进行补偿，得到精伪距、伪距率、卫星位置参数；同时对IMU数据进行导航解算，得到车辆的姿态、速度和位置参数；所述对GPS参数进行补偿，即对GPS信号在电离层传播折射过程进行数学建模，并利用GPS双频接收机两个频段L1和L2的观测量估计出电离层延迟，从而得到更加精确的卫星伪距、伪距率和卫星位置。

S4、判断可见卫星数目是否大于等于4颗若大于等于4颗则选择4颗卫星进行紧组合滤波解算；若卫星数小于4颗则运用智能组合算法；

根据可见卫星的数量构建状态方程和观测方程进行紧组合滤波解算，其中状态变量由17个参数组成，

X＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δλ δh ▽b_x ▽b_y ▽b_z ε_bx ε_by ε_bz δt_u δt_ru]^T，其中，δV_E,δV_N,δV_U是东北天三个方向上的速度误差，

捷联的三个失准角，δL,δλ,δh是捷联的三个位置误差由地球坐标系描述，▽_bx,▽_by,▽_bz是加表三个轴向的零偏误差，ε_bx,ε_by,ε_bz是陀螺的三个轴向漂移，δt_u是时钟误差等效的距离，δt_ru是时钟频率误差等效的距离变化率，

状态方程为：

$\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]$

其中X_I(t)＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δλ δh ▽b_x ▽b_y ▽bz ε_bx ε_by ε_bz]是关于SINS的状态向量，X_G(t)＝[δt_u δt_ru]是关于GPS的状态向量。F_I(t)是惯导系统误差状态方程状态矩阵，而F_G(t)是对GPS相关的两个状态量建模后得到的状态矩阵，G_I(t)和G_G(t)是噪声的输入矩阵，W_I(t),W_G(t)分别是关于SINS和GPS的系统噪声向量。

伪距、伪距率观测方程：

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

其中，观测向量 $Z\left(t\right)={\left[\begin{array}{cc}{\mathrm{\δ\ρ}}_j& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j\end{array}\right]}^T,$ j=1,2,3...n，其中分别为伪距和伪距率观测量，

分别对应伪距、伪距率的观测矩阵，V_ρ(t),V_ρ(t)分别对应伪距、伪距率的观测噪声向量。所述的智能组合算法应用环境指的是由于受树木建筑等遮挡的影响，卫星信号质量较差并且可见星数目较少，但又不同于隧道环境的完全不能接收到卫星信号的情况。智能组合算法集成了三项功能，分别是：可见卫星的无缝隙切换、卫星双频恢复技术、路面车辆的运动约束。所述的可见卫星的无缝隙切换指的是根据当前可见卫星的数量（小于4颗的情况），调整观测向量的维数更改观测方程，在不同的可见卫星数之间实现观测模型的无缝隙切换，提高系统对于应用环境的适应能力。所述的卫星双频恢复技术是在GPS信号受遮挡时，采用之前信号较好时的GPS卫星双频信号计算出的电离层延迟误差值对现有单频卫星的伪距进行补偿，相当于此时将单频卫星恢复为先前双频信号时的精度水平，提高了信号质量，增加了可观测度，使得伪距、伪距率和卫星位置更加精确。为了尽可能提高系统的性能，可以利用特定载体的运动特性来增加外部观测量信息，提高系统的可观测度以增加滤波精度。车辆速度在前进方向上分量最大，当地面平整、车辆无侧移时，可以认为车辆的侧向和天向的速度分量为0，这等价于增加新的观测量，滤波估值的精度和快速性都会有较大提高。当卫星信号质量下降甚至失效时，这两个信息尽可能保证系统导航的连续性。

S5、输出车辆导航参数，并重复上述S2-S4步骤或退出导航。

卫星的导航电文指的是包含导航信息的数据码，每一颗卫星都会发射相应的导航电文，包括卫星星历，卫星工作状态，时钟修正参数，轨道摄动参数等信息。GPS导航系统以24颗卫星计算，有24份完整的导航电文，每一份电文都包含25个主帧，每一主帧包含5个子帧。25个主帧的前三个子帧都是相同的，主要含有卫星星历和时钟修正量的信息。每个主帧的后两个子帧主要包括卫星历书信息，25个主帧的第四和第五子帧加起来组成包含所有卫星的、完整的卫星历书。所以卫星历书相对比较庞大，需要12.5min才能传输完毕，而且只有在地面注入站注入新的数据后才更新，一般用在接收机暖启动，卫星信号捕获等方面。

接收机根据卫星发送的导航电文，提取主帧的前三个子帧。其中第二和第三子帧是计算卫星位置和速度所需要的卫星星历数据。本发明所使用的NOVATEL接收机将这三个子帧放在一起形成包含星历数据和星钟修正等参数的报文，其报头是#GPSEPHEM。

卫星星历包含如下参数：

t_oe：卫星星历数据的参考时刻，其他星历参数都相对于此时刻而言；

M₀：参考时刻的平近点角；

Δn：卫星平均角速度的修正项；

e_s：卫星椭圆轨道的偏心率；

卫星椭圆轨道半长轴的平方根；

Ω_e：卫星轨道的升交点赤经；

升交点赤经的变化率；

i₀：参考时刻卫星轨道平面相对地球赤道平面的倾角；

idot：卫星轨道平面倾角的变化率；

ω：卫星轨道的近地点角距；

C_uc,C_us：升交点角距的调和修正项；

C_rc,C_rs：卫星地心向径的调和修正项；

C_ic,C_is：卫星轨道倾角的调和修正项；

利用卫星星历内的这些参数，可以计算出t_oe时刻所有可观测卫星，在某一时刻t的ECEF坐标系下的卫星坐标与卫星当前速度。

GPS接收机输出伪距信息，其报文的报头里含有当前观测时刻t的值，且是在GPST下描述。对于紧组合系统还需要观测时刻t(需要传播时间、星钟、相对论效应和单频接收机补偿项等补正)对应各卫星的坐标。下面给出卫星坐标的计算方法。

令

t_k＝t-t_oe   (1)

t_k是归一化的相对时间，需对上式进行归一化，即当t_k＞302400时，t_k＝t_k-604800；当t_k＜-302400时，t_k＝t_k+604800，这里604800是一个星期的秒数。

卫星的平均角速度n为

n＝n₀+Δn   (2)

其中

是卫星的理论平均角速度，μ＝3.986005×10¹⁴是地心引力常数。观测时刻的平近点角M为

M＝M₀+n*t_k   (3)

由平近点角可计算观测时刻的偏近点角，它的计算方程是一个超越方程

E＝M+e_s*sinE   (4)

对于上式，卫星轨道的离心率e_s一般不大（小于0.02），若采用迭代法求解，必能全局收敛，令E的初值E₀＝M，迭代十次后一般能有ΔE/E＜10^-10的精度。

根据偏近点角计算卫星的地心向径r

r＝a*(1-e_s*cosE)   (5)

根据偏近点角计算卫星的真近点角f

$\tan (f/2)=(\sqrt{1+e_s}/\sqrt{1-e_s}*\tan (E/2))---\left(6\right)$

上式在E＝π(180°)附近时计算过程中的值会很大，可以采用一个简易的方法，即给E或者tan(E/2)设定阈值。或利用下式计算

$\left\{\begin{array}{c}\sin f=\frac{\sqrt{1-e_s^2}\·\sin E}{1-e_s\cos E}\\ \cos f=\frac{\cos E-e_s}{1-e_s\cos E}\end{array}\right.---\left(7\right)$

根据真近点角计算升交点角距φ得

φ＝f+ω   (8)

根据星历中的摄动修正项和升交点角距来修正卫星轨道参数

升交点角距修正值δμ

δμ＝C_uc*cos(2φ)+C_us*sin(2φ)   (9)

地心向径修正值δr

δr＝C_rc*cos(2φ)+C_rs*sin(2φ)   (10)

轨道倾角修正值δi

δi＝C_ic*cos(2φ)+C_is*sin(2φ)   (11)

更新当前观测时刻卫星的轨道参数，升交点角距φ_k，地心向径r_k，轨道倾角i_k

φ_k＝φ+δμ   (12)

r_k＝r+δr   (13)

i_k＝i₀+idot*t_k+δi   (14)

在卫星轨道所在的平面里，建立原点为地心，Z轴取向北为正，X轴取椭圆轨道长轴的坐标系，得卫星位置坐标P为

$P=\left[\begin{array}{c}{r}_k*\cos \left({\mathrm{\φ}}_k\right)\\ r_k*\sin \left({\mathrm{\φ}}_k\right)\\ 0\end{array}\right]---\left(15\right)$

计算在归一化时刻t_k的升交点赤经Ω_k

${\mathrm{\Ω}}_k={\mathrm{\Ω}}_e+\stackrel{\·}{\mathrm{\Ω}}*t_k-{\mathrm{\ω}}_{\mathrm{ie}}*t---\left(16\right)$

其中，ω_ie为地球自转角速率，ω_ie＝7.2921151467×10^-5(rad/s)

将(15)式表述的卫星轨道坐标系的坐标变换到ECEF坐标系

$P_{\mathrm{ECEF}}=R(-{\mathrm{\Ω}}_k)*R(-i)*P=\left[\begin{array}{c}{x}_k*\cos {\mathrm{\Ω}}_k-y_k*\cos i_k*\sin {\mathrm{\Ω}}_k\\ x_k*\sin {\mathrm{\Ω}}_k+y_k*\cos i_k*\cos {\mathrm{\Ω}}_k\\ y_k*\sin i_k\end{array}\right]---\left(17\right)$

由(17)计算得到的卫星坐标是观测时间t时的值，无论是紧组合还是GPS的定位实际需要的卫星坐标，是由(t-Δt_j)时刻的ECEF坐标系下的卫星坐标再变换到t时刻的ECEF坐标系下的坐标值，这揭示了卫星坐标所含的一种误差。Δt_j是第j颗卫星信号传播到接收机的传播时间，在这个时间内，没有考虑两种运动，第一种是卫星自身的线运动，因此实际所需求的卫星坐标是(t-Δt_j)时刻的卫星坐标，第二种地球的自转角运动，Δt_j的时间段内ECEF坐标系发生了旋转。Δt_j的值大约为70毫秒左右，若将t时刻的卫星数据当作t时刻GPS的数据，则卫星的线速度为每秒千米级，由卫星线运动导致的卫星坐标误差10²m。实际应用中，伪距、伪距率、卫星速度、卫星坐标与方向向量都是(t-Δt_j)时刻的数据。

卫星速度计算的过程与坐标计算的过程相似，这里不再展开，需要注意的是应该利用星钟误差导数项与广义相对论效应的导数项来较正卫星速率。

（3）时间同步对于SINS/GPS组合导航系统，尤其是紧组合导航系统有重要影响。当载体处于运动状态或捷联系统IMU灵敏度特别高时，SINS与GPS系统的数据同步必须达到一定精度，否则导航的结果偏差较大。本发明分别给出了三种信息同步方法：

（a）基于GPS的1PPS信号的硬件同步法；

（b）基于数据融合的软件同步法；

（c）基于SINS数据存储的同步方法。

如图2所示，基于GPS的1PPS信号的硬件同步法：主要利用了GPS接收机非常精准的1PPS秒脉冲信号，并且当GPS因某种原因未输出1PPS信号或没有GPS伪距信息时，SINS系统在DSP控制下也能正常输出数据，组合系统进入纯捷联工作方式。SINS需要的时钟频率为200Hz，DSP利用记数的方法对现有的50M晶振进行分频，达到设定值时计数器清零，然后在上升沿触发AD转换器，对IMU的数据进行模数转换。在同步系统中，1PPS信号到来时由DSP赋予SINS一个中断，采样此时的SINS数据，同时将计数器清零。基于1PPS信号的硬件同步法在1PPS信号到来时进行数据对齐，精度很高，且1PPS信号消失时不影响SINS系统采样，所以容错性好。但是这种方法在数据传输和处理时的延迟问题上没有效果。在载体低动态的情况下，影响较小，仍可以采用此方法，当载体处于运动变化快速的高动态时，被忽略的数据处理和传输的延迟将带来较大影响，需要考虑其他同步方法。

如图3所示，基于数据融合的软件同步法：在硬件同步的基础上，考虑到在数据在1PPS的t时刻，SINS和GPS数据可以严格同步，DSP开始接受GPS数据和SINS数据。SINS数据量较小，DSP处理所耗时间极小，GPS传输的是#RANGE报文以及可能出现的星历更新报文#EPHEM，数据量要大的多。令GPS数据处理时间段为ΔT_G,则组合滤波的时间即为t+ΔT_G，此时SINS的数据则为mT_SINS，由GPS数据处理带来的SINS和GPS数据时间不同步为(mT_SINS-t-ΔT_G)，在高动态运动条件下，此项必须考虑。

处理上述问题有两种方法：

（a）对GPS数据进行外推

令ΔT_SINS＝(mT_SINS-t)，有

X_GPS(t+ΔT_SINS)＝X_GPS(t)+Δ_GPS   (18)

Δ_GPS＝A·[X_SINS(t+ΔT_SINS)-X_SINS(t)]   (19)

式中，A是待设定的X_SINS到X_GPS的状态转移阵。需要说明的是在载体动态运动情况下，不能利用GPS的数据进行外推，所以必须依靠SINS数据来对GPS数据进行外推处理。

（b）对SINS数据进行插值

利用mT_SINS到nT_SINS的SINS数据对1PPS(t)时的SINS数据进行(n-m+1)阶的插值。

即有

$X_{\mathrm{SINS}}\left(t\right)=a_0+a_1\·\mathrm{\Δ}{T}_S+a_2\·\mathrm{\Δ}{T}_S^2+...+a_{m-n}\·\mathrm{\Δ}{T}_S^{m-n}---\left(20\right)$

式中，a_i(i＝0,1...m-n)是(n-m+1)阶保持器的系数，由X_SINS(jT_SINS),(j＝n,n+1,...,m)决定。

基于SINS数据存储的同步方法：硬件同步的方法保证了在1PPS下GPS与SINS数据到达DSP的严格同步性。而软件同步的方法则考虑到GPS数据量较大，尤其是星历更新时，DSP很难在5ms内完成GPS数据接收并进入组合过程，因此SINS数据和GPS数据由于信息处理的时间而存在数据不同步。基于SINS数据存储的同步方法：具有原理简单、无需复杂计算、同步精度高的特点。

图3中，在1PPS(t)时刻DSP可以并行的采集GPS数据以及SINS数据，因此只需在mT_SINS时刻对1PPS(t)时刻的GPS/SINS组合系统滤波处理即可。考虑到，1PPS(t)时刻到mT_SINS时刻之间，DSP仍不断地采集IMU数据并对组合系统的状态矩阵和导航参数进行更新，若将1PPS(t)的滤波估计值补偿到mT_SINS时刻，必将引起新的误差。

基于数据存储的时间同步处理方法是，保留1PPS(t)时刻的系统状态和到mT_SINS时刻SINS输出的所有IMU数据，假定在(m+k)T_SINS到来之前完成以下工作：在副本中还原1PPS(t)时刻的系统状态，然后根据1PPS(t)时刻SINS的IMU信息和GPS的伪距、伪距率、卫星坐标信息来进行组合系统的滤波估计并对副本中的导航量进行补偿，期间保存(m+1)T_SINS到(m+k)T_SINS之间的IMU输出值，然后利用1PPS(t)时刻到(m+k)T_SINS时刻的IMU数据进行导航更新和状态阵更新，最后将结果返回到(m+k)T_SINS时刻的系统中，(m+k+1)T_SINS时刻后的IMU数据可以正常处理。此方法保证了SINS/GPS组合系统参与组合的SINS数据与GPS数据严格地是1PPS时刻。

与上述两种软件同步的方法相比，基于SINS数据存储的时间同步方法无需对高维的状态量进行插值计算，这避免了软件同步中外推和插值系数不准确而带来的误差，从原理上保证了GPS与SINS数据的同步性。

（1）SINS/GPS紧组合导航系统中，需要GPS系统部分输出三种物理量：卫星位置与速度、伪距和伪距率、当前的GPS时或者时间脉冲信号。卫星坐标用于计算量测矩阵中的方向余弦向量；伪距和伪距率用于计算紧组合系统的观测量，伪距率观测量由多普勒频移和卫星速度计算；时间信号用于实现SINS和GPS数据采集时间同步，这三种物理量的准确性直接影响导航的精度。伪距和伪距率可以由接收机直接输出，但伪距的误差一般很大，常常达上万米，尚需对其补偿修正。

接收机输出的星历报文除了导航电文的第二和第三子帧，即除卫星星历外，还包含了第一子帧的部分信息。属于第一子帧的信息有，群延迟估计(tgd,estimated groupdelay difference,seconds)，星钟零偏(af0,clock aging parameter,seconds)，星钟频率漂移(af1,clock aging parameter,s/s)，星钟频率漂移的变化率(af2,clockaging parameter,s/s/s)。群延迟估计用于计算单频接收机星钟误差，星钟漂移的三个系数则能够补偿星钟在参考时刻t_oc相对于GPS时的偏差和星钟相对于实际频率的频率差，从而计算星钟的钟差，这在计算卫星坐标和卫星速度中是必要的。

这里有一组#GPSEPHEM的报文，即输出的星历报文，

#GPSEPHEMA,COM1,5,78.5,FINESTEERING,1735,363600.000,00040000,1214,11526;22,363570.0,0,15,15,1735,1735,367184.0,2.656020434e+07,5.037709841e-09,-1.099211063e+00,6.4210073324e-03,-2.045163384e+00,4.762783647e-06,1.105293632e-05,1.47875000e+02,9.50937500e+01,5.029141903e-08,1.732259989e-07,9.2578839302e-01,-5.714523747e-11,4.6175285269e-01,-8.32748973e-09,15,367184.0,-1.722946763e-08,1.64041e-04,1.36424e-12,0.00000,TRUE,1.458601990e-04,4.00000000e+00*05eb1b7f

报头里(1735,363600.000)是GPS时，即测试时间为2013年4月12号下午1点左右，报头后面(报文里以分号区别开)的(22)则说明是22号卫星的星历数据，紧随其后的共22个参数都是来自卫星星历。后面的(15,367184.0)是两组第一子帧的IODC(时钟修正参数的保龄期)和t_oc(计算钟差所用到的星钟参考时刻)，在两者之后的是(tgd,af0,af1,af2)，其中

t_oc＝367184.0

tgd=-1.722946763e-08，等效距离为tgd*c≈-5.16(m)，单频接收机中需要用到

af0=1.64041e-04，等效距离为af0*c≈4.92*10⁴(m)。

星钟误差的零偏值很大，若在紧组合系统中利用测码伪距作为观测量，须对星钟做出补偿或对伪距进行等效距离的补偿，然后才能够得到精确的伪距观测量。

星钟误差补偿量的计算公式为：

Δt_si＝a_f0+a_f1*(t-t_oc)+a_f2*(t-t_oc)²+Δt_r   (21)

其中，a_f0表示星历报文中的af0，a_f1表示af1，a_f2表示af2，t表示观测时刻，紧组合系统中，就是伪距报文中报头的秒信息，Δt_r为相对论效应补偿项。

$\mathrm{\Δ}{t}_r={\mathrm{Fe}}_s\sqrt{a_s}\sin \left(E\right)---\left(22\right)$

式中

C为光速，C=299792458m/s。伪距率补偿量为

$\mathrm{\Δ}{\stackrel{\·}{t}}_{\mathrm{si}}=a_{f1}+2a_{f2}*(t-t_{\mathrm{oc}})+{\mathrm{Fe}}_s\sqrt{a_s}\cos \left(E\right)\stackrel{\·}{E}---\left(23\right)$

由式(23)补偿后，不考虑站钟频差的情况下，伪距率的测量精度已达到0.001m/s。

电离层延迟误差估计与补偿：另外，对于频率确定的电磁波来说，电离层的传播延迟取决于传播路径上的电子密度，即传播路径上的总电子数。因此，在中午时分，卫星接近地平线时，电离层折射带来的影响可能超过150m，而夜间卫星在天顶位置时，电离层折射的延迟影响将小于5m；太阳黑子活动剧烈，或者其他超新星爆发时，电离层延迟都将大大增加。

本发明所采用的接收机为GPS双频接收机，双频接收机对卫星进行观测时，得到两组测量伪距ρ₁，ρ₂和电离层延迟δρ₁，δρ₂，设接收机的频率分别为f₁,f₂，则有如下关系

$\left\{\begin{array}{c}\mathrm{\δ}{\mathrm{\ρ}}_1=\frac{A}{{f_1}^2}\\ \mathrm{\δ}{\mathrm{\ρ}}_2=\frac{A}{{f_2}^2}\\ {\mathrm{\ρ}}_1={\mathrm{\ρ}}_0+\mathrm{\δ}{\mathrm{\ρ}}_1\\ {\mathrm{\ρ}}_2={\mathrm{\ρ}}_0+\mathrm{\δ}{\mathrm{\ρ}}_2\end{array}\right.---\left(24\right)$

由(24)得

$\mathrm{\δ}{\mathrm{\ρ}}_1=({\mathrm{\ρ}}_1-{\mathrm{\ρ}}_2)\left(\frac{{f_2}^2}{{f_2}^2-{f_1}^2}\right)---\left(25\right)$

ρ₀＝ρ₁-δρ₁   (26)

通过（26）可以补偿伪距中的电离层延迟误差，从而进一步提高伪距、伪距率和卫星位置的精度。

（1）SINS/GPS紧组合系统建模

设状态向量

X＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δλ δh ▽b_x ▽b_y ▽b_z ε_bx ε_by ε_bz δt_u δt_ru](27)

状态量从第1至17维分别是：

δV_E,δV_N,δV_U是在东北天三个方向上的速度误差，

捷联的三个失准角(平台姿态角)，δL,δλ,δh是捷联的三个位置误差由地球坐标系描述，是加表三个轴向的零偏误差，ε_bx,ε_by,ε_bz是陀螺的三个轴向漂移，δt_u是时钟误差等效的距离和δt_ru是时钟频率误差等效的距离变化率，共17维。

捷联误差方程与模型建立：

（a）速度误差方程

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\δ}}{V}_E=f_N{\mathrm{\φ}}_U-f_U{\mathrm{\φ}}_N+(\frac{V_N}{R_E}\tan L-\frac{V_U}{R_E})\mathrm{\δ}{V}_E+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E}\tan L)\mathrm{\δ}{V}_N-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E})\mathrm{\δ}{V}_U\\ +(2{\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_E}{\sec }^{2}L)\mathrm{\δL}+\frac{V_E{V}_U-V_E{V}_N\tan L}{{R_E}^2}\mathrm{\δh}+{\▿}_E\\ \stackrel{\·}{\mathrm{\δ}}{V}_N=f_U{\mathrm{\φ}}_E-f_E{\mathrm{\φ}}_U-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E}\tan L)\mathrm{\δ}{V}_E-\frac{V_U}{R_N}{\mathrm{\δV}}_N-\frac{V_N}{R_N}\mathrm{\δ}{V}_U\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E}{\sec }^{2}L)V_E\mathrm{\δL}+(\frac{V_N{V}_U}{{R_N}^2}+\frac{V_E^2\tan L}{{R_E}^2})\mathrm{\δh}+{\▿}_N\\ \stackrel{\·}{\mathrm{\δ}}{V}_U=f_E{\mathrm{\φ}}_N-f_N{\mathrm{\φ}}_E+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E})\mathrm{\δ}{V}_E+\frac{{2V}_N}{R_N}\mathrm{\δ}{V}_N-2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_E\mathrm{\δL}\\ -(\frac{{V_N}^2}{{R_N}^2}+\frac{V_E^2}{{R_E}^2})\mathrm{\δh}+{\▿}_U\end{array}\right.---\left(28\right)$

上述方程中，f_E,f_N,f_U分别为在东北天三个方向上的比力值，R_E为垂直子午面的法线平面内的曲率半径，R_N为参考椭球体子午面内的曲率半径，ω_ie为地球自转角速率，L为纬度。在方程28-30中，有部分符号没有定义。所有的符号都要解释其含义。

（b）姿态误差方程

（c）位置误差方程

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_N}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{R_E}\sec L+\frac{V_E}{R_E}\sec L\tan \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{V}_U\end{array}\right.---\left(30\right)$

GPS系统建模：

SINS/GPS紧组合的观测量选择了伪距和伪距率，根据伪距、伪距率的数学模型GPS部分需要选取两个相关的状态量，即站钟的钟差等效距离误差δt_u和钟频差等效速率误差δt_ru。其模型描述为

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{t}}_u=\mathrm{\δ}{t}_{\mathrm{ru}}+{\mathrm{\ω}}_{\mathrm{tu}}\\ \mathrm{\δ}{\stackrel{\·}{t}}_{\mathrm{ru}}=-{\mathrm{\β}}_{\mathrm{tru}}\mathrm{\δ}{t}_{\mathrm{ru}}+{\mathrm{\ω}}_{\mathrm{tru}}\end{array}\right.---\left(31\right)$

上式表明，GPS中时钟偏差是白噪声激励的一阶微分方程，时钟频率偏差是一阶马尔可夫过程，β_tru为反相关时间。

紧组合系统的状态方程：

$\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]---\left(32\right)$

(32)式中，F_I(t)是惯导系统误差状态方程状态矩阵，而F_G(t)是对GPS相关的两个状态量建模后得到的状态矩阵，G_I(t)和G_G(t)是噪声的输入矩阵。δt_u建模后其激励为白噪声，δt_ru建模为一阶马尔可夫过程。如下所示

$F_G=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\β}}_{\mathrm{tru}}\end{array}\right],G_G=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],W_G\left(t\right)=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{tu}}\\ {\mathrm{\ω}}_{\mathrm{tru}}\end{array}\right]---\left(33\right)$

量测方程：

(a)伪距量测方程

令载体位置在ECEF坐标系下坐标为(x_I,y_I,z_I)，由捷联给出；第j颗卫星在ECEF坐标系中的位置是(x_sj,y_sj,z_sj)，由卫星星历求得。得惯导给出的载体位置到第j颗卫星的伪距为

${\mathrm{\ρ}}_{\mathrm{Ij}}=\sqrt{{(x_I-x_{\mathrm{sj}})}^2+{(y_I-y_{\mathrm{sj}})}^2+{(z_I-z_{\mathrm{sj}})}^2}---\left(34\right)$

对于GPS，其伪距和真实距离关系如下式

ρ_Gj＝r_j-δt_u-v_ρj   (35)

式中，r_j表示卫星到载体(其实是接收机)的真实距离，令载体的真实位置在ECEF下坐标为(x,y,z)，得r_j的表达式为

$r_j=\sqrt{{(x-x_{\mathrm{sj}})}^2+{(y-y_{\mathrm{sj}})}^2+{(z-z_{\mathrm{sj}})}^2}---\left(36\right)$

δt_u是站钟误差，表示站钟钟差等效距离误差，v_ρj表示其他公共误差和独有误差，如电离层误差、对流层误差和多路径误差等测量误差。在紧组合仿真中，v_ρj认为是一个白噪声形式的误差，钟差误差则多个通道统一设置。在紧组合样机实验中，则必须对电离层延迟等诸多误差做补偿，补偿的方法第三章里有详细论述。将r_j在(x_I,y_I,z_I)处一阶泰勒展开

$\begin{array}{c}{r}_j\≈{\mathrm{\ρ}}_{\mathrm{Ij}}+\frac{{\∂r}_j}{\∂x}{|}_{x=x_I}(x-x_I)+\frac{{\∂r}_j}{\∂y}{|}_{y=y_I}(y-y_I)+\frac{{\∂r}_j}{\∂z}{|}_{z=z_I}(z-z_I)\\ ={\mathrm{\ρ}}_{\mathrm{Ij}}-\frac{x_I-x_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\mathrm{\δx}-\frac{y_I-y_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\mathrm{\δy}-\frac{z_I-z_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\mathrm{\δz}\end{array}---\left(37\right)$

这里，令δx＝xI-x,δy＝yI-y,δz＝zI-z，与实际滤波估计量符号相一致。

令 $\begin{array}{c}(x_I-x_{\mathrm{sj}})/{\mathrm{\ρ}}_{\mathrm{Ij}}=e_{j1}\\ (y_I-y_{\mathrm{sj}})/{\mathrm{\ρ}}_{\mathrm{Ij}}=e_{j2}\\ (z_I-z_{\mathrm{sj}})/{\mathrm{\ρ}}_{\mathrm{Ij}}=e_{j3}\end{array}---\left(38\right)$

整理后得到

r_j＝ρ_Ij-e_j1δx-e_j2δy-e_j3δz   (39)

ρ_Gj＝ρ_Ij-e_j1δx-e_j2δy-e_j3δz-δt_u-v_ρj   (40)

将式(34)式(40)作差，得到伪距观测量的方程

δρ_j＝ρ_Ij-ρ_Gj＝e_j1δx+e_j2δy+e_j3δz+δt_u+v_ρj   (41)

将上式展开，取(j＝1,2,3,4)四颗卫星，写成矩阵形式有

$\left[\begin{array}{c}\mathrm{\δ}{\mathrm{\ρ}}_1\\ \mathrm{\δ}{\mathrm{\ρ}}_2\\ \mathrm{\δ}{\mathrm{\ρ}}_3\\ \mathrm{\δ}{\mathrm{\ρ}}_4\end{array}\right]=\left[\begin{array}{cccc}{e}_{11}& e_{12}& e_{13}& 1\\ e_{21}& e_{22}& e_{23}& 1\\ e_{31}& e_{32}& e_{33}& 1\\ e_{41}& e_{42}& e_{43}& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\δx}\\ \mathrm{\δy}\\ \mathrm{\δz}\\ \mathrm{\δ}{t}_u\end{array}\right]+\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ρ}1}\\ {\mathrm{\υ}}_{\mathrm{\ρ}2}\\ {\mathrm{\υ}}_{\mathrm{\ρ}3}\\ {\mathrm{\υ}}_{\mathrm{\ρ}4}\end{array}\right]---\left(42\right)$

式(42)是在ECEF坐标系下表述的，紧组合系统中，SINS部分的位置误差状态量是由地球坐标系描述的，故需将ECEF坐标系转为地球坐标系。根据ECEF坐标系(x,y,z)与地球坐标系(L,λ,h)的转换关系

$\begin{array}{c}x=(R_n+h)\cos L\cos \mathrm{\λ}\\ y=(R_n+h)\cos L\sin \mathrm{\λ}\\ z=[R_n(1-e^2)+h]\sin L\end{array}---\left(43\right)$

对上式求全微分得

$\begin{array}{c}\mathrm{\δx}=\mathrm{\δ}h\cos L\cos \mathrm{\λ}-(R_n+h)\sin L\cos \mathrm{\λ\δL}-(R_n+h)\cos L\sin \mathrm{\λ\δ\λ}\\ \mathrm{\δy}=\mathrm{\δ}h\cos L\sin \mathrm{\λ}-(R_n+h)\sin L\sin \mathrm{\λ\δL}+(R_n+h)\cos L\cos \mathrm{\λ\δ\λ}\\ \mathrm{\δz}=\mathrm{\δ}h\sin L+[R_n(1-e^2)+h]\cos \mathrm{L\δL}\end{array}---\left(44\right)$

其中，R_n是卯酉圈半径，e是地球椭球体的第一离心率，大地坐标系选择WGS-84，将式(44)带入式(42)可得，伪距观测量的量测方程

Z_ρ(t)＝H_ρ(t)X(t)+V_ρ(t)   (45)

$H_{\mathrm{\ρ}1}={\left(a_{\mathrm{ji}}\right)}_{4*3},H_{\mathrm{\ρ}2}={\left[\begin{array}{cccc}1& 1& 1& 1\\ 0& 0& 0& 0\end{array}\right]}^T---\left(47\right)$

$\begin{array}{c}{a}_{j1}=(R_n+h)[-e_{j1}\sin L\cos \mathrm{\λ}-e_{j2}\sin L\sin \mathrm{\λ}]+[R_n*(1-e^2)+h]e_{j3}\cos L\\ a_{j2}=(R_n+h)[e_{j2}\cos L\cos \mathrm{\λ}-e_{j1}\cos L\sin \mathrm{\λ}]\\ a_{j3}=e_{j1}\cos L\cos \mathrm{\λ}+e_{j2}\cos L\sin \mathrm{\λ}+e_{j3}\sin L\end{array}---\left(48\right)$

（b）伪距率观测量的量测方程

GPS接收机不直接输出伪距率，实验中用多普勒频移代替，对ρ_Gj求导得接收机的伪距率数学模型为

${\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Gj}}={\stackrel{\·}{r}}_j-{\mathrm{\δt}}_{\mathrm{ru}}-v_{\stackrel{\·}{\mathrm{\ρ}}j}={\mathrm{Dopp}}_j*{\mathrm{\λ}}_j---\left(49\right)$

其中，Dopp_j是第j颗卫星的多普勒频移，λ_j是L1的波长取为0.1903m，

是伪距率测量的噪声以与其他影响伪距测量的误差残差的导数的和，δt_ru是站钟钟漂的等效速率，且r_j＝ρ_Ij-e_j1δx-e_j2δy-e_j3δz，故

${\stackrel{\·}{r}}_j={\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}-{\stackrel{\·}{e}}_{j1}\mathrm{\δx}-{\stackrel{\·}{e}}_{j2}\mathrm{\δy}-{\stackrel{\·}{e}}_{j3}\mathrm{\δz}-e_{j1}\mathrm{\δ}\stackrel{\·}{x}-e_{j2}\mathrm{\δ}\stackrel{\·}{y}-e_{j3}\mathrm{\δ}\stackrel{\·}{z}---\left(49\right)$

这里

分别是e_j1，e_j2，e_j3的关于时间的导数，在高动态、高精度导航中，式(49)由于考虑了位置误差项，因此相比没有考虑此项的模型要准确，精度也要高。上式中

$\begin{array}{c}{\stackrel{\·}{e}}_{j1}=d\left(\frac{x_I-x_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\right)/\mathrm{dt}=\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})-(x_I-x_{\mathrm{sj}}){\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\\ {\stackrel{\·}{e}}_{j2}=d\left(\frac{y_I-y_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\right)/\mathrm{dt}=\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})-(y_I-y_{\mathrm{sj}}){\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\\ {\stackrel{\·}{e}}_{j3}=d\left(\frac{z_I-z_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\right)/\mathrm{dt}=\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}})-(z_I-z_{\mathrm{sj}}){\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\end{array}---\left(50\right)$

ρ_Ij的导数为

$\begin{array}{c}{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}=d\left(\sqrt{{(x_I-x_{\mathrm{sj}})}^2+{(y_I-y_{\mathrm{sj}})}^2+{(z_I-z_{\mathrm{sj}})}^2}\right)/\mathrm{dt}\\ =\frac{(x_I-x_{\mathrm{sj}})({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})+(y_I-y_{\mathrm{sj}})({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})+(z_I-z_{\mathrm{sj}})({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}})}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\\ =e_{j1}({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})+e_{j2}({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})+e_{j3}({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}})\end{array}---\left(51\right)$

式(51)既是得出数学模型而获得得量测方程的依据，也是获得SINS伪距率的观测量的依据。由式(51)知，计算SINS伪距率需要获得卫星的速度、方向向量以及ECEF坐标系下的用户速度。

式(51)代入式(50)得导数项

$\begin{array}{c}{\stackrel{\·}{e}}_{j1}=d\left(\frac{x_I-x_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\right)/\mathrm{dt}=\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})-(x_I-x_{\mathrm{sj}}){\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\\ =\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})-(x_I-x_{\mathrm{sj}})(e_{j1}({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})+e_{j2}({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})+e_{j3}({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}}))}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\\ {\stackrel{\·}{e}}_{j2}=d\left(\frac{y_I-y_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\right)/\mathrm{dt}=\frac{{\mathrm{\ρ}}_{\mathrm{Ij}({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})-(y_I-y_{\mathrm{sj}}){\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}}}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\\ =\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})-(y_I-y_{\mathrm{sj}})(e_{j1}({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})+e_{j2}({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})+e_{j3}({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}}))}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\\ {\stackrel{\·}{e}}_{j3}=d\left(\frac{z_I-z_{\mathrm{sj}}}{{\mathrm{\ρ}}_{\mathrm{Ij}}}\right)/\mathrm{dt}=\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}})-(z_I-z_{\mathrm{sj}}){\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\\ =\frac{{\mathrm{\ρ}}_{\mathrm{Ij}}({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}})-(z_I-z_{\mathrm{sj}})(e_{j1}({\stackrel{\·}{x}}_I-{\stackrel{\·}{x}}_{\mathrm{sj}})+e_{j2}({\stackrel{\·}{y}}_I-{\stackrel{\·}{y}}_{\mathrm{sj}})+e_{j3}({\stackrel{\·}{z}}_I-{\stackrel{\·}{z}}_{\mathrm{sj}}))}{{{\mathrm{\ρ}}_{\mathrm{Ij}}}^2}\end{array}---\left(52\right)$

得

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j={\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}-{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Gj}}={\stackrel{\·}{e}}_{j1}\mathrm{\δx}+{\stackrel{\·}{e}}_{j2}\mathrm{\δy}+{\stackrel{\·}{e}}_{j3}\mathrm{\δz}+e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}+{\mathrm{\δt}}_{\mathrm{ru}}+v_{\stackrel{\·}{\mathrm{\ρ}}j}---\left(53\right)$

考虑到

都是高阶小量，实际应用中可以舍去，若在高动态情况下需要计算位置项对伪距率差的影响，则只需对多组方向余弦求平均的方法来求得方向余弦的导数，舍去高阶量化简式(53)得：

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j={\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}-{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Gj}}=e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}+\mathrm{\δ}{t}_{\mathrm{ru}}+v_{\stackrel{\·}{\mathrm{\ρ}}j}---\left(54\right)$

式(54)忽略了载体内GPS与SINS两个系统之间的距离，(x,y,z)是载体在ECEF坐标系下的坐标真值，则

为ECEF坐标系下的接收机的速度真值，(δx,δy,δz)是载体在ECEF坐标系下的位置误差，转换为地球坐标系后也当作为SINS系统解算中的位置误差项，

是载体在ECEF坐标系下的速度误差，转换为“东、北、天坐标系”后也当作SINS系统解算中的速度误差项。

是SINS在ECEF坐标系下的速度，且式(4.36)描述的方程满足如下关系

${\stackrel{\·}{x}}_I=\stackrel{\·}{x}+\mathrm{\δ}\stackrel{\·}{x},{\stackrel{\·}{y}}_I=\stackrel{\·}{y}+\mathrm{\δ}\stackrel{\·}{y},{\stackrel{\·}{z}}_I=\stackrel{\·}{z}+\mathrm{\δ}\stackrel{\·}{z}---\left(55\right)$

SINS中速度误差状态量都是以东北天坐标系(地理坐标系)描述的，因此需要将ECEF坐标系的速度误差统一到东北天坐标系描述。转换公式为

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}\\ \mathrm{\δ}\stackrel{\·}{y}\\ \mathrm{\δ}\stackrel{\·}{z}\end{array}\right]{=G}_T^E\left[\begin{array}{c}{\mathrm{\δV}}_E\\ {\mathrm{\δV}}_N\\ {\mathrm{\δV}}_U\end{array}\right]---\left(56\right)$

$C_T^E=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& -\sin L\cos \mathrm{\λ}& \cos \mathrm{Lcis\λ}\\ \cos \mathrm{\λ}& -\sin L\sin & \cos L\sin \mathrm{\λ}\\ 0& \cos L& \sin L\end{array}\right]---\left(57\right)$

由此得

$\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=-\mathrm{\δ}{v}_E\sin \mathrm{\λ}-\mathrm{\δ}{v}_N\sin L\cos \mathrm{\λ}+\mathrm{\δ}{v}_U\cos L\cos \mathrm{\λ}\\ \mathrm{\δ}\stackrel{\·}{y}=\mathrm{\δ}{v}_E\cos \mathrm{\λ}-\mathrm{\δ}{v}_N\sin L\sin \mathrm{\λ}+\mathrm{\δ}{v}_U\cos L\sin \mathrm{\λ}\\ \mathrm{\δ}\stackrel{\·}{z}=\mathrm{\δ}{v}_N\cos L+{\mathrm{\δv}}_U\sin \mathrm{\λ}\end{array}---\left(58\right)$

可以得到伪距率组合的量测方程

$Z_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)=H\stackrel{\·}{\mathrm{\ρ}}\left(t\right)X\left(t\right)+V_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)---\left(59\right)$ 式中， $H_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)=\left[\begin{array}{ccccc}{H}_{\stackrel{\·}{\mathrm{\ρ}}1}& 0_{4*3}& H_{\stackrel{\·}{\mathrm{\ρ}}2}& 0_{4*6}& H_{\stackrel{\·}{\mathrm{\ρ}}3}\end{array}\right]$

其中

$H_{\stackrel{\·}{\mathrm{\ρ}}1}={\left(b_{\mathrm{ji}}\right)}_{4*3},H_{\stackrel{\·}{\mathrm{\ρ}}2}={\left(c_{\mathrm{ji}}\right)}_{4*3}{H}_{\stackrel{\·}{\mathrm{\ρ}}3}={\left[\begin{array}{cccc}1& 1& 1& 1\\ 0& 0& 0& 0\end{array}\right]}^T---\left(60\right)$

$\begin{array}{c}{b}_{j1}=-e_{j1}\sin \mathrm{\λ}+e_{j2}\cos \mathrm{\λ}\\ b_{j2}=-e_{j2}\sin L\cos \mathrm{\λ}-e_{j1}\sin L\sin \mathrm{\λ}+e_{j3}\cos L\\ b_{j3}=e_{j1}\cos L\cos \mathrm{\λ}+e_{j2}\cos L\sin \mathrm{\λ}+e_{j3}\sin L\end{array}---\left(61\right)$

综上得量测方程：

$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\ρ}}\\ H_{\stackrel{\·}{\mathrm{\ρ}}}\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]---\left(63\right)$

（c）车辆运动约束的添加

为了尽可能提高系统的性能，可以利用特定载体的运动特性来增加观测量，增加系统的可观测度以增加滤波的精度和速度。

现实中的载体运动不仅遵循牛顿定律还总是满足其他特定的规律。跑车实验的载体速度在载体坐标系的Y方向上分量最大，当地面平整、车辆无侧移时，可以认为载体坐标系的X和Z轴的速度分量为0。这等价于增加新的观测量，因此提高了系统的可观测度，滤波估值的精度和快速性都会有较大提高。并且当外部观测量失效时，这两组信息仍然能对系统产生作用，尽可能保持当前精度。令

$\left\{\begin{array}{c}{V}_x^b=0\\ V_z^b=0\end{array}\right.---\left(64\right)$

取b系的捷联输出的速度与上式的速度作差，得

$\left\{\begin{array}{c}{\mathrm{\δV}}_x^b=V_{\mathrm{SINSx}}^b-V_x^b=V_{\mathrm{SINSx}}^b\\ \mathrm{\δ}{V}_z^b=V_{\mathrm{SINSz}}^b-V_z^b=V_{\mathrm{SINSz}}^b\end{array}\right.---\left(65\right)$

令n系到b系的转换矩阵为

则有

$V^b=C_n^b{v}^n---\left(66\right)$

对式(66)求微分得

$\begin{array}{c}{\mathrm{\δV}}^b=C_n^b{\mathrm{\δV}}^n+\left({\mathrm{\δC}}_n^b\right)V^n=C_n^b\mathrm{\δ}{V}^n-({\mathrm{\ω}}_{\mathrm{en}}^n\×)C_n^b{V}^n\\ =C_n^b{\mathrm{\δV}}^n-{\mathrm{\ω}}_{\mathrm{en}}^n\×V^b=C_n^b\mathrm{\δ}{V}^n+V^b\×{\mathrm{\ω}}_{\mathrm{en}}^n\\ =C_n^b\mathrm{\δ}{V}^n+C_n^b{V}^n\×{\mathrm{\ω}}_{\mathrm{en}}^n\end{array}---\left(67\right)$

式中

是地球坐标系相对东北天坐标系的旋转角速度，由于地球半径很大而载体一般运动速度相对较小，故

是一个数值很小的量，运动约束中低动态时可忽略此项。此外考虑到SINS的计算坐标系和东北天坐标系存在失准角故可由式(66)得：

$(V_{\mathrm{SINS}}^{n^{\′}}\×)=(V^{n^{\′}}\×)=\left[\begin{array}{ccc}0& -V_U& V_N\\ V_U& 0& -V_E\\ -V_N& V_E& 0\end{array}\right]---\left(69\right)$

由式(65)、式(67)、式(68)得

由(70)得量测方程

注：式(70)(71)只对载体坐标系的侧向与天向成立。

综上，得到量测方程为

$Z_V=\left[\begin{array}{c}{\left(C_n^b{V}_{\mathrm{SINS}}^{n^{\′}}\right)}_x\\ {\left(C_n^b{V}_{\mathrm{SINS}}^{n^{\′}}\right)}_z\end{array}\right]=H_{V}X+V_V---\left(72\right)$

H_V＝[A_2*3 B_2*3 0_2*9]   (73)

$A=\left[\begin{array}{ccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)\end{array}\right]---\left(74\right)$

$B=\left[\begin{array}{ccc}{V}_U{C}_n^b\left(\mathrm{1,2}\right)-V_N{C}_n^b\left(\mathrm{1,3}\right)& V_E{C}_n^b\left(\mathrm{1,3}\right)-V_U{C}_n^b\left(\mathrm{1,2}\right)& V_N{C}_n^b\left(\mathrm{1,1}\right)-V_E{C}_n^b\left(\mathrm{1,2}\right)\\ V_U{C}_n^b\left(\mathrm{3,2}\right)-V_N{C}_n^b\left(\mathrm{3,3}\right)& V_E{C}_n^b\left(\mathrm{3,3}\right)-V_U{C}_n^b\left(\mathrm{3,2}\right)& V_N{C}_n^b\left(\mathrm{3,1}\right)-V_E{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right]---\left(75\right)$

注：式(71)方程不能用两边同时右乘姿态矩阵的方式得到简化式。

联立式(63),(72)得

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

添加运动约束后，式(76)构成了适用于载体为车辆时的SINS/GPS紧组合导航系统的量测方程。

所述的智能组合算法指的是在以下环境中使用的算法：受树木建筑等遮挡的影响，卫星信号质量较差并且可见星数目较少，但又不同于隧道环境的完全不能接收到卫星信号的情况；路况复杂，弯道多，路面起伏较大。智能组合算法包含以下可见卫星的无缝隙切换、卫星双频恢复及运动约束算法。

无缝隙切换指的是在由于GPS信号受遮挡导致可见卫星数不足4颗时，紧组合滤波观测模型需要根据可见卫星的数量进行适当调整，如只有3颗卫星时，式（76）中观测向量Z(t)中的伪距观测向量δρ维数为3维，伪距率观测向量

维数为3维；如只有2颗卫星时，伪距观测向量δρ维数为2维，伪距率观测向量

维数为2维，以此类推，这样可以提高组合系统对外部环境的适应性。

卫星双频恢复技术指的是：在GPS信号遮挡影响可见星数量及信号质量时，按照上文中提到的“电离层延迟误差估计与补偿”方法依据GPS接收机L1和L2双频测量信号估计出电离层延迟δρ，当GPS某些卫星变为单频信号后则利用之前估计的δρ对单频信号的伪距进行补偿，这样在一定程度上相当于恢复为双频卫星信号。

运动参数：车载试验，实验时间为1800秒，跑车在学校内进行，试验路线如图4所示，其中可见星数目的情况如图5所示，由于学校道路旁数木和楼宇较多因此可见卫星数较少，其中并未出现四颗星以上和零颗星的情况，少于四颗星时采用智能组合算法。

利用双频恢复后，可用星数目如图6所示，可知经双频恢复后，可用卫星的数目得到增加，采用智能组合算法的紧组合的结果如图7所示，虽然可见卫星数有变化，但是从误差曲线上来看没有太多的跳动，误差相对比较稳定并且保持一定的精度，因此该方法在GPS信号易受遮挡的环境下为车辆无缝导航提供了可靠的保证。通过伪距、伪距率进行捷联与卫星的紧组合导航，针对GPS信号较差（可见卫星小于4颗时）的情况下采用智能组合算法，在不同可见卫星数之间可以进行无缝隙切换，应用卫星双频恢复技术，同时增加对车辆的运动约束，可以提高GPS信号较差时紧组合的可观测度本发明与现有技术相比的优点在于：

总之，本发明能够弥补松组合方式在GPS可见卫星数小于4颗时不能定位的不足，无需利用GPS输出位置信息，而采用伪距、伪距率组合方式，对于GPS信号较差或遮挡时的容错性较好；本发明利用GPS双频接收机的L1、L2两个频段的测量信息估计电离层延迟，从而提高伪距、伪距率和卫星位置的计算精度；本发明在GPS信号受遮挡且可见卫星数小于4颗时能够根据可见卫星数调整观测量模型进行组合滤波，并且运用卫星双频恢复及车辆运动约束技术提高了车辆紧组合的可观测度，从而改善车辆在GPS信号较差时的环境适应性。

Claims (10)

1.一种车载捷联/卫星紧组合无缝导航方法，其特征在于，包括如下步骤：

S1、系统初始化；

S2、GPS输出报文、SINS输出惯性测量单元数据，并且进行同步；

S3、解析GPS报文，获得粗伪距、多普勒频移、卫星位置参数，并对参数进行补偿，得到精伪距、伪距率、卫星位置参数；同时对惯性测量单元数据进行导航解算，得到车辆的姿态、速度和位置参数；

S4、判断可见卫星数目是否大于等于4颗，若大于等于4颗，选择其中4颗卫星进行紧组合滤波解算；若卫星数小于4颗，运用智能组合算法；

S5、输出车辆导航参数，如果继续导航则返回至步骤S2；

S6、导航结束，退出。

2.如权利要求1所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，步骤S3中对GPS参数进行补偿具体为：对GPS信号在电离层传播折射过程进行数学建模，利用GPS双频接收机两个频段L1和L2的观测量估计出电离层延迟，得到更加精确的卫星伪距、伪距率和卫星位置。

3.如权利要求1所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，步骤S4中，若可见卫星大于4颗，则选择4颗卫星进行紧组合滤波解算，根据可见卫星的数量构建状态方程和观测方程进行紧组合滤波解算。

4.如权利要求1所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，步骤S4中，所述智能组合算法包括可见卫星的无缝隙切换、卫星双频恢复技术和路面车辆的运动约束。

5.如权利要求4所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，所述可见卫星的无缝隙切换是指根据当前可见卫星的数量调整观测向量的维数，更改观测方程，在不同的可见卫星数之间实现观测模型的无缝隙切换。

6.如权利要求4所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，所述卫星双频恢复技术是指采用之前信号较好时的GPS卫星双频信号计算出的电离层延迟误差值对现有单频卫星的伪距进行补偿，相当于此时将单频卫星恢复为先前双频信号时的精度水平。

7.如权利要求4所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，所述路面车辆的运动约束包括：当地面平整、车辆无侧移时，设定车辆速度在前进方向上分量最大，车辆的侧向和天向的速度分量为0，等价于增加新的观测量，保证系统导航的连续性。

8.如权利要求3所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，所述紧组合滤波解算为：根据可见卫星的数量构建状态方程和观测方程进行紧组合滤波解算，其中状态变量由17个参数组成，X＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δλ δh ▽b_x ▽b_y ▽b_z ε_bx ε_by ε_bz δt_u δt_ru]^T，其中，δV_E,δV_N,δV_U是东北天三个方向上的速度误差，捷联的三个失准角，δL,δλ,δh是捷联的三个位置误差由地球坐标系描述，▽_bx,▽_by,▽_bz是加表三个轴向的零偏误差，ε_bx,ε_by,ε_bz是陀螺的三个轴向漂移，δt_u是时钟误差等效的距离，δt_ru是时钟频率误差等效的距离变化率；

状态方程为：

$\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]$

其中X_I(t)＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δλ δh ▽b_x ▽b_y ▽b_z ε_bx ε_by ε_bz]是关于SINS的状态向量，X_G(t)＝[δt_u δt_ru]是关于GPS的状态向量，F_I(t)是惯导系统误差状态方程状态矩阵，而F_G(t)是对GPS相关的两个状态量建模后得到的状态矩阵，G_I(t)和G_G(t)是噪声的输入矩阵，W_I(t),W_G(t)分别是关于SINS和GPS的系统噪声向量，伪距、伪距率观测方程：

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

其中，观测向量 $Z\left(t\right)={\left[\begin{array}{cc}{\mathrm{\δ\ρ}}_j& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j\end{array}\right]}^T,$ j=1,2,3...n，其中分别为伪距和伪距率观测量，

分别对应伪距、伪距率的观测矩阵，V_ρ(t),V_ρ(t)分别对应伪距、伪距率的观测噪声向量。

9.如权利要求1所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，所述同步方法包括利用GPS接收机的1PPS秒脉冲信号进行同步的硬件同步法，基于数据融合的软件同步法和基于SINS数据存储同步法。

10.如权利要求7所述的车载捷联/卫星紧组合无缝导航方法，其特征在于，路面车辆的运动约束算法为：当地面平整、车辆无侧移时，令

$\left\{\begin{array}{c}{V}_x^b=0\\ V_z^b=0\end{array}\right.---\left(64\right)$

其中，

分别为b系的侧向和天向的理论真实速度分量，

取b系的捷联输出的速度与上式的速度作差，得

$\left\{\begin{array}{c}{\mathrm{\δV}}_x^b=V_{\mathrm{SINSx}}^b-V_x^b=V_{\mathrm{SINSx}}^b\\ {\mathrm{\δV}}_z^b=V_{\mathrm{SINSz}}^b-V_z^b=V_{\mathrm{SINSz}}^b\end{array}\right.---\left(65\right)$

其中，

分别为捷联测得的x方向和z方向的速度分量，

令n系到b系的转换矩阵为

则有

$V^b=C_n^b{V}^n---\left(66\right)$

其中，Vn为n系（地理坐标系）下的速度矢量，

对式(66)求微分得

$\begin{array}{c}{\mathrm{\δV}}^b=C_n^b{\mathrm{\δV}}^n+\left({\mathrm{\δC}}_n^b\right)V^n=C_n^b\mathrm{\δ}{V}^n-({\mathrm{\ω}}_{\mathrm{en}}^n\×)C_n^b{V}^n\\ =C_n^b{\mathrm{\δV}}^n-{\mathrm{\ω}}_{\mathrm{en}}^n\×V^b=C_n^b\mathrm{\δ}{V}^n+V^b\×{\mathrm{\ω}}_{\mathrm{en}}^n\\ =C_n^b\mathrm{\δ}{V}^n+C_n^b{V}^n\×{\mathrm{\ω}}_{\mathrm{en}}^n\end{array}---\left(67\right)$

式中：

是地球坐标系相对东北天坐标系的旋转角速度，由于地球半径很大而载体一般运动速度相对较小，运动约束中低动态时可忽略此项；此外考虑到SINS的计算坐标系和东北天坐标系存在失准角故可由式(66)得

$(V_{\mathrm{SINS}}^{n^{\′}}\×)=(V^{n^{\′}}\×)=\left[\begin{array}{ccc}0& -V_U& V_N\\ V_U& 0& -V_E\\ -V_N& V_E& 0\end{array}\right]---\left(69\right)$

由公式(65)、(67)、(68)得：

由(70)得量测方程

式(70)(71)只对载体坐标系的侧向与天向成立；

得到量测方程为：

$Z_V=\left[\begin{array}{c}{\left(C_n^b{V}_{\mathrm{SINS}}^{n^{\′}}\right)}_x\\ {\left(C_n^b{V}_{\mathrm{SINS}}^{n^{\′}}\right)}_z\end{array}\right]=H_{V}X+V_V---\left(72\right)$

H_V＝[A_2*3 B_2*3 0_2*9]   (73)

$A=\left[\begin{array}{ccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)\end{array}\right]---\left(74\right)$

$B=\left[\begin{array}{ccc}{V}_U{C}_n^b\left(\mathrm{1,2}\right)-V_N{C}_n^b\left(\mathrm{1,3}\right)& V_E{C}_n^b\left(\mathrm{1,3}\right)-V_U{C}_n^b\left(\mathrm{1,2}\right)& V_N{C}_n^b\left(\mathrm{1,1}\right)-V_E{C}_n^b\left(\mathrm{1,2}\right)\\ V_U{C}_n^b\left(\mathrm{3,2}\right)-V_N{C}_n^b\left(\mathrm{3,3}\right)& V_E{C}_n^b\left(\mathrm{3,3}\right)-V_U{C}_n^b\left(\mathrm{3,2}\right)& V_N{C}_n^b\left(\mathrm{3,1}\right)-V_E{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right]---\left(75\right)$

式(71)方程不能用两边同时右乘姿态矩阵的方式得到简化式；

联立式(63)、(72)得：

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

添加运动约束后，公式(76)构成了适用于载体为车辆时的SINS/GPS紧组合导航系统的量测方程。

Images