CN105068102A - 一种基于dsp+fpga的超紧组合导航方法 - Google Patents

Abstract

本发明公开了一种基于DSP+FPGA的超紧组合导航方法。步骤如下：组合导航系统初始对准；GNSS接收机初始化；SINS导航解算，将惯性辅助信息发送给GNSS接收机；导航星的伪距测量误差补偿；载体相对每颗导航星的伪距、伪距率解算；构建系统状态方程和量测方程；进行全维滤波解算，并根据滤波结果对系统进行校正；利用卫星的星历信息解算出的卫星位置和速度以及组合导航输出的载体位置和速度，计算由于载体和卫星之间的相对运动所造成的载波多普勒频移并对跟踪环路进行反馈校正，实现惯性信息辅助GNSS跟踪环路。本发明方法可实现基于惯性/卫星的伪距、伪距率组合导航，提高了导航精度和对复杂环境的适应性，应用前景广阔。

Description

一种基于DSP+FPGA的超紧组合导航方法

一、技术领域

本发明涉及组合导航技术领域，特别是一种基于DSP+FPGA的超紧组合导航方法。

二、背景技术

卫星/惯性组合导航系统结合卫星导航、惯性导航的优点，具有定位精度高，稳定性强等特点，因此在军事领域及民用领域都被广泛应用。组合导航的模式有很多种，主要分为松组合、紧组合、深组合三类。基于位置和速度的松组合系统虽然具有结构简单，计算量小等优点，但在GNSS接收机收星数少于4颗无法定位时，系统无法正常组合，导航精度随之下降。基于伪距、伪距率的紧组合导航系统虽然可以在收星数少于四颗的情况下进行组合，但由于GNSS接收机收星数的减少，降低了观测变量的维数，导致导航精度变差。此外，无论是松组合导航系统还是紧组合导航系统，在高动态环境下都面临GNSS接收机性能下降甚至失效的问题。

三、发明内容

本发明的目的是提供一种高精度的基于DSP+FPGA的超紧组合导航方法，能够有效抑制导航精度的发散。

实现本发明目的的技术解决方案为：一种基于DSP+FPGA的超紧组合导航方法，包括以下步骤：

步骤1，系统上电后完成硬件初始化工作，包括串口初始化、总线初始化、定时器初始化；

步骤2，SINS系统初始对准，即对SINS系统的初始速度、位置、姿态进行初始对准；

步骤3，GNSS接收机初始化，为相关器各通道分配C/A码，设置搜捕卫星，开始可见卫星的捕获；

步骤4，导航计算机接收SINS数据，采用四元数法进行导航解算，确定出载体的速度、位置、姿态信息，并装订成惯性辅助信息发送给GNSS接收机；

步骤5，导航处理器根据TIC锁存数据确定卫星的伪距和伪距率，并对导航星的伪距测量误差进行补偿；同时解调出导航星发送的导航电文，并根据导航电文解算导航卫星的位置、速度信息；

步骤6，GNSS接收机将步骤3中已捕获卫星的通道转为跟踪状态，利用步骤4提供的载体位置、速度信息，以及步骤5提供的卫星位置和速度，实时计算由于卫星和载体之间相对运动造成的多普勒频移及变化率，并利用所得结果对载波跟踪环路进行反馈校正；

步骤7，根据步骤5提供的导航星速度、位置信息，以及步骤4提供的载体速度、位置信息，确定载体相对每颗导航星的伪距、伪距率信息；

步骤8，构建系统状态方程和量测方程：状态变量包括SINS系统误差、GNSS系统误差和惯性器件误差，SINS系统误差包括姿态误差、速度误差和位置误差；GNSS系统误差包括钟差和钟漂；惯性器件误差包括随机误差；

步骤9，根据步骤8构建的系统状态方程和量测方程进行全维滤波解算，将所得滤波结果对系统进行校正。

本发明与现有技术相比，其显著优点是：(1)由于采用GNSS输出的原始伪距、伪距率信息，从而消除了量测输出的时间相关性，并且在收星数少于四颗的情况下仍然可以实现组合导航；(2)采用全维滤波方式组合，利用全部观测信息，从而实现最佳精度导航；(3)采用惯性信息辅助GNSS接收机跟踪环路，实现GNSS与INS之间的双向辅助；(4)对于惯性高动态飞行过程中加加速度所造成的多普勒频移残差问题，根据载体飞行过程中的受力情况对载体运动状态构建相应数学模型，实时计算观测时刻各通道多普勒频移及其变化率，提高GNSS接收机的动态适应性。

四、附图说明

图1是本发明基于DSP+FPGA的超紧组合导航方法的流程图。

图2是本发明基于DSP+FPGA的超紧组合导航方法的系统硬件结构图。

图3是本发明基于DSP+FPGA的超紧组合导航方法的惯性辅助载波环数学模型图。

图4是本发明基于DSP+FPGA的超紧组合导航方法的系统维数变化示意图。

图5是本发明基于DSP+FPGA的超紧组合导航方法的系统原理结构图。

五、具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

结合图1，本发明基于DSP+FPGA的超紧组合导航方法，步骤如下：

步骤1，系统上电后完成硬件初始化工作，包括串口初始化、总线初始化、定时器初始化；

步骤2，SINS系统初始对准，即对SINS系统的初始速度、位置、姿态进行初始对准，具体如下：

SINS初始对准，初始化速度、位置。采用动基座传递对准技术，实现姿态、位置、速度的快速，精确初始化。

步骤3，GNSS接收机初始化，为相关器各通道分配C/A码，设置搜捕卫星，开始可见卫星的捕获，具体如下：

(3.1)GNSS接收机初始化，通过初始对准提供的载体位置，结合卫星的轨道分布预测搜捕卫星，并为相关器各通道分配相关卫星C/A码，设置搜捕卫星；二维搜索中，为了提高捕获速度，由一次搜索1/2码片增加为一次搜索3个码片，即由即时相关峰一路相关增加为超前相关峰、即时相关峰、滞后相关峰、延迟超前相关峰、延迟即时相关峰、延迟滞后相关峰等六路相关峰。采用六路相关提高了二维搜索中码片搜索的速度，在扫频范围同为±10KHZ的情况下，捕获速度将提高为原来一路相关方法的6倍。

(3.2)根据Flash中保存的卫星历书信息计算各通道正在搜捕卫星的位置和速度，结合步骤2中初始对准提供的载体位置、速度信息计算卫星的多普勒频移，从而缩小搜索范围，加快捕获速度。

步骤4，导航计算机接收SINS数据，采用四元数法进行导航解算，确定出载体的速度、位置、姿态信息，并装订成惯性辅助信息发送给GNSS接收机，硬件图如图2所示，具体如下：

(4.1)由FPGA接收惯导数据并将数据存入FIFO中，当接收到完整的一包数据后触发接收中断，由导航计算机读取IMU数据；

(4.2)导航计算机进行SINS导航解算，采用四元数算法计算出载体的速度、位置、姿态信息，具体如下：

(4.2.1)捷联惯导系统姿态更新解算，采用传统四元数算法。

四元数微分方程表达式为：

$\stackrel{\·}{Q}=\frac{1}{2}\mathrm{\Ω}Q$

其中，Ω为载体坐标系相对导航系下的角速率构成的反对称矩阵，Q为四元数。

通过龙格-库塔求解四元数微分方程，然后由四元数求得姿态矩阵，由姿态矩阵求解三个姿态角；

(4.2.2)捷联惯导系统速度解算。

速度微分方程如下：

${\stackrel{\·}{V}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×V^n+g^n$

其中，是由四元数求得的姿态矩阵，为地球自转角速率在n系的投影，为n系相对e系的旋转角速率，Vⁿ为n系下载体速度矢量，f^b为加速度计测得的在载体坐标系下的输出值，gⁿ为当地重力加速度矢量；

(4.2.3)捷联惯导系统位置解算。

位置微分方程如下：

$\left\{\begin{array}{c}\stackrel{\·}{L}=\frac{V_N}{R_M+h}\\ \stackrel{\·}{\mathrm{\λ}}=\frac{V_E\sec L}{R_N+h}\\ \stackrel{\·}{h}=v_U\end{array}\right.$

上式中，L，λ和h分别是载体的纬度、经度和高度；V_E、V_N和V_U分别是载体在东、北、天方向上的速度分量；R_M、R_N分别是地球的子午圈半径和卯酉圈半径；

(4.3)将SINS解算得到的地理坐标系下的位置、速度、加速度等信息转换到ECEF坐标系下，装订成惯性辅助信息后写入FIFO中，由FPGA串口发送给GNSS接收机。

步骤5，导航处理器根据TIC锁存数据确定卫星的伪距和伪距率，并对导航星的伪距测量误差进行补偿；同时解调出导航星发送的导航电文，并根据导航电文解算导航卫星的位置、速度信息，具体如下：

(5.1)根据TIC锁存数据计算卫星的伪距和伪距率：具体测量时，由于接收机时间可以预知或假定，因此只要知道信号的发射时间就可以求得伪距。在GPS的导航电文中，根据Z计数确定每一帧开始时对应的GPS时间；在帧同步后，对数据位、码周期数、半个码片滑动整数及小数部分分别进行计数得到测量时刻信号的发射时间，用发射时间减去接收时间就得到传播延时，从而实现了伪距的测量。

(5.2)对导航星的伪距测量误差进行补偿，即对伪距补偿地球自转和对流层影响，地球自转和对流层影响如下：

地球自转影响：

$(\frac{P_x-p_x}{P_y}-\frac{P_y-p_y}{P_x})*we*light\_speed$

其中，P_x、P_y分别为卫星ECEF坐标系下x、y轴位置，p_x、p_y分别为载体ECEF坐标系下x、y轴位置，we为地球自转角速率，light_speed为光速；

对流层效应影响：

$\frac{2.47}{sin\mathrm{\θ}+0.0121}*light\_speed$

其中，θ为卫星的高度角。

校正后，得到导航卫星伪距ρ_Gj、伪距率信息，导航卫星的伪距ρ_Gj公式为：

ρ_Gj＝r_j-δt_u-v_ρj

其中，r_j为载体到第j颗卫星的真实无差距离，δt_u为钟差引起的距离误差，v_ρj为伪距测量白噪声。

导航卫星的伪距率公式如下：

${\stackrel{\·}{\mathrm{\ρ}}}_{Gj}={\stackrel{\·}{r}}_j-{\mathrm{\δt}}_{ru}-v_{\stackrel{\·}{\mathrm{\ρ}}j}$

其中，δt_ru为钟漂引起的距离率误差，为伪距率测量白噪声，为载体到第j颗卫星S^j的真实无差距离变化率；

(5.3)导航电文是GPS信号中调制的数据，GPS接收机接收到卫星信号后，进行载波解调和伪码解扩，得到50bps的数据码，按照导航电文的格式，即可将数据码编译成导航电文，卫星位置和速度的计算公式参考ICD-GPS-200C。

步骤6，GNSS接收机将步骤3中已捕获卫星的通道转为跟踪状态，利用步骤4提供的载体位置、速度信息，以及步骤5提供的卫星位置和速度，实时计算由于卫星和载体之间相对运动造成的多普勒频移及变化率，并利用所得结果对载波跟踪环路进行反馈校正，载波跟踪环路的数学模型如图3所示：

(6.1)计算多普勒频移，设第i通道所跟踪的卫星在ECEF中的位置为步骤(4.2.3)中递推得到的载体位置P′_u＝(X_u,Y_u,Z_u)，则相对位置的单位视线矢量Rⁱ为：

$R^i=\frac{(X_S^i-X_u,Y_S^i-Y_u,Z_S^i-Z_u)}{\sqrt{{(X_S^i-X_u)}^2+{(Y_S^i-Y_u)}^2+{(Z_S^i-Z_u)}^2}}$

式中，(X_u,Y_u,Z_u)为载体的真实无差位置，为由卫星星历确定的第j颗卫星在ECEF坐标系中的位置；

观测时刻GNSS接收机第i通道所跟踪的卫星信号其载波多普勒频移及变化率为：

$f_{dop}^i=\frac{f_{L1}}{c}\·(V_u^{\′}-V_S)\·R^i$

${\stackrel{\·}{f}}_{dop}^i=\frac{f_{L1}}{c}\·({\mathrm{\α}}_u^{\′}-{\mathrm{\α}}_s)\·R^i$

式中，是载波多普勒频移，是载波多普勒频移变化率，c是光速，f_L1是信号载波频率，V_S、α_S分别是观测时刻载体的三维速度、加速度；V′_u、α′_u分别是观测时刻接收机i通道卫星的三维速度、加速度；

(6.2)对辅助信息进行数据拟合，接收机载波环是一个由鉴相器、环路滤波器和数控振荡器构成的负反馈闭环控制系统。环路滤波器若选择二阶滤波器，则传递函数F(s)可表达成：

$F\left(s\right)=\frac{{\mathrm{\τ}}_{2}s+1}{{\mathrm{\τ}}_{1}s}$

式中，F(s)为滤波器传递函数，τ₁和τ₂是滤波器系数，s为拉普拉斯变换算子；

常规载波跟踪环的闭环传递函数H(s)和误差传递函数H_e(s)分别为：

$H\left(s\right)=\frac{{\mathrm{\θ}}_e\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{2{\mathrm{\ξw}}_n^{2}s+w_n^2}{s^2+2{\mathrm{\ξw}}_n^{2}s+w_n^2}$

$H_e\left(s\right)=\frac{\mathrm{\δ}\mathrm{\θ}\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{s^2}{s^2+2{\mathrm{\ξw}}_n^{2}s+w_n^2}$

式中，w_n表示特征频率且K表示环路增益，ξ表示阻尼系数且θ_r(s)、θ_e(s)分别是滤波器输入相位、输出相位，δθ(s)为相位误差，且δθ(s)＝θ_e(s)-θ_γ(s)；

由此可知加入惯性辅助后的闭环传递函数和误差传递函数分别为

$H\left(s\right)=\frac{{\mathrm{\θ}}_e\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{(1-k)s^2+(\mathrm{\τ}s+1)(2{\mathrm{\ξw}}_{n}s+w_n^2)}{(\mathrm{\τ}s+1)(s^2+2{\mathrm{\ξw}}_{n}s+w_n^2)}$

$H_e\left(s\right)=\frac{\mathrm{\δ}\mathrm{\θ}\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{(\mathrm{\τ}s+k)s^2}{(\mathrm{\τ}s+1)(s^2+2{\mathrm{\ξw}}_{n}s+w_n^2)}$

其中，k表示刻度系数误差，τ表示滞后时间常数；

当载体在视线矢量上作匀加加速度运动时，根据拉式变换有：

${\mathrm{\θ}}_r\left(s\right)=\frac{{\mathrm{\θ}}_0}{s}+\frac{\mathrm{\ν}}{s^2}+\frac{A}{s^3}+\frac{J}{s^4}$

式中，θ₀为鉴相器输出的相位误差，ν、A和J分别是载体与卫星之间的相对速度、加速度和加加速度在视线矢量上的投影；

根据终值定理，得到环路的稳态误差为

$E\left(\mathrm{\∞}\right)=\underset{s\→0}{\lim }s\mathrm{\δ}\mathrm{\θ}\left(s\right)=\underset{s\→0}{\lim }{\mathrm{s\θ}}_r\left(s\right)H_e\left(s\right)$

根据以上公式可知：二阶锁相环不能跟踪载体的加加速度。而当载体作匀加速运动时，有惯性辅助时的稳态误差为无惯性辅助时的稳态误差为若刻度误差取k＝0.001，那么稳态误差将减小1000倍。由此可见，惯性辅助载波环路有效增大了环路的等效带宽，且辅助效果由惯性辅助信息的精度决定。

由于分布式组合导航系统串口传输速率的限制，惯性辅助信息更新率相对跟踪环路更新率较低，所以在惯性信息更新的间隔中，载体机动引起的加加速度会造成相位误差。

当辅助间隔取40ms，加加速度取20g/s时，可以算得加速度残留达到

$\mathrm{\Δ}a=\stackrel{\·}{a}\·T_s=20\×0.04\×9.78=7.824(m/s^2)$

若跟踪环路为2阶，滤波带宽取20Hz，阻尼系数取0.7071，那么此时动态应力造成的相位误差为

从上我们可以看出：当载体作加加速度运动时，较长的辅助间隔内会累积较大的相位误差，可能导致环路失锁，因此需要对辅助信息进行插值或拟合。加加速度反映了载体的受力变化情况。其中发动机的工作由火控系统控制，飞行阻力与大气稀薄程度和速度有关。通过对发动机推力和飞行阻力变化的合理建模，可以在一定范围内对加加速度进行拟合，减小其带来的不利影响。剩下的误差则由环路滤波器消除。

步骤7，根据步骤5提供的导航星速度、位置信息，以及步骤4提供的载体速度、位置信息，确定载体相对每颗导航星的伪距、伪距率信息，流程如图4所示。具体解算流程为：

根据导航星的速度、位置信息，以及SINS的速度、位置信息，计算出载体相对每颗导航星的伪距、伪距率信息，其中载体到第j颗卫星的伪距ρ_Ij为：

ρ_Ij＝r_j+e_j1δx+e_j2δy+e_j3δz

其中，δx、δy、δz分别为载体在地球坐标系中的位置误差在x轴、y轴、z轴分量，r_j为载体到第j颗卫星的真实无差距离，e_j1、e_j2、e_j3分别为载体与第j颗卫星x轴、y轴、z轴的方向余弦；

载体到第j颗卫星的伪距率为：

${\stackrel{\·}{\mathrm{\ρ}}}_{Ij}={\stackrel{\·}{r}}_j+e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}$

其中，分别为载体在地球坐标系中的速度误差在x轴、y轴、z轴分量，为载体到第j颗卫星的真实无差距离变化率。

步骤8，构建系统状态方程和量测方程：状态变量包括SINS系统误差、GNSS系统误差和惯性器件误差，SINS系统误差包括姿态误差、速度误差和位置误差；GNSS系统误差包括钟差和钟漂；惯性器件误差包括随机误差，具体为：

(8.1)组合导航系统通过卡尔曼滤波器对系统中的误差量进行估计，并将估计值作为校正量反馈到系统中，实现组合校正。为了对误差量进行补偿，需要建立相应的数学模型，包括SINS系统误差、GNSS系统误差和惯性器件误差三部分。SINS系统误差主要包括姿态误差、速度误差和位置误差；GNSS系统误差包括钟差和钟漂；惯性器件误差为简单起见，只考虑随机误差。由此建立的超紧组合导航系统状态方程如下：

上式中，φ_E、φ_N、φ_U分别为东、北、天方向平台失准角，δV_E、δV_N、δV_U分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，δt_u为与时钟等效的距离误差，δt_ru为与时钟频率等效的距离率误差，R_M为椭球子午圈上各点的曲率半径，R_N为卯酉圈上各点的曲率半径，w_ie为地球转动角速率，f_E、f_N、f_U分别是惯导系统的比力在导航系下东、北、天方向上的分量，ε_E、ε_N、ε_U分别为地理坐标系内陀螺的漂移在东、北、天方向上的投影，▽_E、▽_N、▽_U分别为地理坐标系内加速计的漂移在东、北、天方向上的投影，β_tru为反相关时间；L和h分别是载体的纬度和高度；V_E、V_N和V_U分别是载体在东、北、天方向上的速度分量；

(8.2)以导航系统的姿态误差、速度误差、位置误差及伪距、伪距率误差为状态变量，建立惯性/卫星组合导航系统的状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中，X为系统状态矢量，表示系统状态矢量的导数，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，具体如下：

系统状态矢量：

$X={\[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,\mathrm{\δ}L,\mathrm{\δ}\mathrm{\λ},\mathrm{\δ}h,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\δt}}_u,{\mathrm{\δt}}_{ru}\]}_{17\×1}^T$

上式中共17个状态变量，φ_E,φ_N,φ_U分别为东、北、天方向姿态误差角；δV_E,δV_N，δV_U分别为东、北、天方向速度误差；δL,δλ,δh分别为纬度误差、经度误差、高度误差；ε_x,ε_y，ε_z分别为载体系下陀螺三个轴向的随机漂移；▽_x,▽_y,▽_z分别为载体系下加速度计三个轴向的偏差；δt_u和δt_ru分别为钟差引起的等效距离误差和钟漂引起的距离率误差；

状态转移矩阵：

$F={\left[\begin{array}{cccc}{\left(F_{ins}\right)}_{9\×9}& {\left(F_{sg}\right)}_{9\×6}& 0& 0\\ 0_{6\×9}& 0_{6\×6}& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& -{\mathrm{\β}}_{tru}\end{array}\right]}_{17\×17},F_{sg}={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]}_{9\×6}$

其中F_ins由步骤(8.1)中误差方程构成，为载体坐标系到导航坐标系的姿态转换矩阵，

系统噪声驱动矩阵：

$G={\left[\begin{array}{cccc}{C}_b^n& 0_{3\×3}& 0& 0\\ 0_{3\×3}& C_b^n& 0& 0\\ 0_{9\×3}& 0_{9\×3}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}_{17\×8}$

系统噪声矢量：

$W={\left[\begin{array}{cccccccc}{w}_{gx}& w_{gy}& w_{gz}& w_{ax}& w_{ay}& w_{az}& w_{tu}& w_{tru}\end{array}\right]}_{8\×1}^T$

假设系统噪声为零均值高斯白噪声，协方差阵为E(WW^T)＝Q，Q为系统过程噪声方差阵；w_gx、w_gy、w_gz分别为陀螺仪x轴、y轴、z轴方向的随机白噪声，w_ax、w_ay、w_az分别为加速度计x轴、y轴、z轴方向的随机白噪声，w_tu、w_tru分别为伪距随机和伪距率随机白噪声；

(8.3)计算每个通道对应的每颗导航星的高度角，方位角信息，方法如下：首先由将ECEF坐标系中载体到相应导航星的观测向量转换到以载体位置为原点的地理坐标系中，计算公式如下：

G＝S*E

其中

$\begin{array}{cc}G=\left[\begin{array}{c}\mathrm{\Δ}e\\ \mathrm{\Δ}n\\ \mathrm{\Δ}u\end{array}\right]& E=\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right]\end{array}$

其中，E＝[ΔxΔyΔz]^T为ECEF坐标系内载体到该卫星的观测向量，G＝[ΔeΔnΔu]^T为向量E在以载体位置为原点的地理坐标系三个方向上的投影；

$S=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\sin L\cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right],\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]-\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

其中，[XYZ]^T为卫星在ECEF坐标系中的位置，[xyz]^T为载体在ECEF坐标系中位置，L,λ分别为载体的纬度、经度；

最终方位角和高度角由以下计算公式得出：

$\mathrm{\θ}=\arcsin (\mathrm{\Δ}u/\sqrt{{\left(\mathrm{\Δ}e\right)}^2+{\left(\mathrm{\Δ}n\right)}^2+{\left(\mathrm{\Δ}u\right)}^2})$

α＝arctan(Δe/Δn)

上式中，θ,α分别为高度角、方位角，其中θ∈(0,π/2),α∈(0,2π)；

(8.4)根据步骤(8.3)计算得到的高度角、方位角确定可见卫星。根据可见卫星个数，系统动态调整观测方程及组合滤波器相关矩阵的维数；

(8.5)构建系统量测方程

Z(t)＝H(t)X(t)+V(t)

其中Z(t)为系统观测矢量，H(t)为系统观测矩阵，V(t)为系统观测噪声阵，X为系统状态矢量。量测方程的维数及组合滤波器的维数根据可见星数量变化，结合图4，若可见星数量为n，则观测向量Z的维数为2n×1；系统观测矩阵H维数为2n×17；系统观测噪声方差R阵维数为2n×2n；卡尔曼滤波增益阵维数为17×2n；

伪距观测方程如下：

${\tilde{Z}}_{\mathrm{\ρ}}={\tilde{H}}_{\mathrm{\ρ}}\tilde{X}+{\tilde{V}}_{\mathrm{\ρ}}=\left[\begin{array}{cccc}{0}_{n\×6}& {\tilde{H}}_{\mathrm{\ρ}n1}& 0_{n\×6}& {\tilde{H}}_{\mathrm{\ρ}n2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\mathrm{\ρ}}$

式中，为伪距观测矢量，为伪距观测矩阵，为伪距观测噪声阵，为伪距状态矢量，分别为：

$\begin{array}{cc}{\tilde{Z}}_{\mathrm{\ρ}}={\left[\begin{array}{ccc}{\mathrm{\δ\ρ}}^1& ...& {\mathrm{\δ\ρ}}^n\end{array}\right]}_{1\×n}^T& {\tilde{V}}_{\mathrm{\ρ}}={\left[\begin{array}{ccc}{v}_{\mathrm{\ρ}}^1& ...& v_{\mathrm{\ρ}}^1\end{array}\right]}_{1\×n}^T\end{array}$

δρ_j＝ρ_Ij-ρ_Gj＝e_j1δx+e_j2δy+e_j3δz+δt_u+ν_ρj

${\tilde{H}}_{\mathrm{\ρ}n1}={\left[\begin{array}{ccc}{\tilde{a}}_{11}& {\tilde{a}}_{12}& {\tilde{a}}_{13}\\ .& .& .\\ .& .& .\\ .& .& .\\ {\tilde{a}}_{n1}& {\tilde{a}}_{n2}& {\tilde{a}}_{n3}\end{array}\right]}_{n\×3},{\tilde{H}}_{\mathrm{\ρ}n2}={\left[\begin{array}{cc}1& 0\\ .& .\\ .& .\\ .& .\\ 1& 0\end{array}\right]}_{n\×2}$

其中，为各通道伪距测量白噪声，ρ_Ij、ρ_Gj分别为步骤7中所求载体相对每颗卫星伪距和步骤5中GNSS接收机输出的卫星伪距，展开如下：

$\left\{\begin{array}{c}{\tilde{a}}_{i1}=(R_N+h)\[-e_{i1}\sin L\cos \mathrm{\λ}-e_{i2}\sin L\sin \mathrm{\λ}\]\\ +\[R_N{(1-f)}^2+h\]e_{i3}\cos L\\ {\tilde{a}}_{i2}=(R_N+h)\[e_{i2}\cos L\cos \mathrm{\λ}-e_{i1}\cos L\sin \mathrm{\λ}\]\\ {\tilde{a}}_{i3}=e_{i1}\cos L\cos \mathrm{\λ}+e_{i2}\cos L\sin \mathrm{\λ}+e_{i3}\sin L\end{array}\right.$

伪距率观测方程如下所示：

${\tilde{Z}}_{\stackrel{\·}{\mathrm{\ρ}}}={\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}}\tilde{X}+{\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}=\left[\begin{array}{cccc}{0}_{n\×3}& {\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n1}& 0_{n\×9}& {\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}$

式中，为伪距率观测矢量，为伪距率观测矩阵，为伪距率观测噪声阵，为伪距率状态矢量，分别为：

$\begin{array}{cc}{\tilde{Z}}_{\stackrel{\·}{\mathrm{\ρ}}}={\left[\begin{array}{ccc}\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^1& ...& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}_{1\×n}^T& {\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}={\left[\begin{array}{ccc}{\mathrm{\ν}}_{\stackrel{\·}{\mathrm{\ρ}}}^1& ...& {\mathrm{\ν}}_{\stackrel{\·}{\mathrm{\ρ}}}^1\end{array}\right]}_{1\×n}^T\end{array}$

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j={\stackrel{\·}{\mathrm{\ρ}}}_{Ij}-{\stackrel{\·}{\mathrm{\ρ}}}_{Gj}=e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}+{\mathrm{\δt}}_{ru}+{\mathrm{\ν}}_{\stackrel{\·}{\mathrm{\ρ}}j}$

${\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n1}={\left[\begin{array}{ccc}{\tilde{b}}_{21}& {\tilde{b}}_{22}& {\tilde{b}}_{23}\\ .& .& .\\ .& .& .\\ .& .& .\\ {\tilde{b}}_{n1}& {\tilde{b}}_{n2}& {\tilde{b}}_{n3}\end{array}\right]}_{n\×3},{\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n1}={\left[\begin{array}{cc}0& 1\\ .& .\\ .& .\\ .& .\\ 0& 1\end{array}\right]}_{n\×2}$

其中，为各通道伪距率测量白噪声分别为步骤7中所求载体相对每颗卫星伪距率和步骤5中GNSS接收机输出的卫星伪距率，展开如下：

$\left\{\begin{array}{c}{\tilde{b}}_{i1}=-e_{i1}\sin \mathrm{\λ}+e_{i2}\cos \mathrm{\λ}\\ {\tilde{b}}_{i2}=-e_{i1}\sin L\cos \mathrm{\λ}-e_{i2}\sin L\sin \mathrm{\λ}+e_{i3}\cos L\\ {\tilde{b}}_{i3}=e_{i1}\cos L\cos \mathrm{\λ}+e_{i2}\cos L\sin \mathrm{\λ}+e_{i3}\sin L\end{array}\right.$

综合伪距、伪距率差观测方程，得到系统观测方程如下：

$\tilde{Z}=\left[\begin{array}{c}{\tilde{Z}}_{\mathrm{\ρ}}\\ {\tilde{Z}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]=\left[\begin{array}{c}{\tilde{H}}_{\mathrm{\ρ}}\\ {\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]\tilde{X}+\left[\begin{array}{c}{\tilde{V}}_{\mathrm{\ρ}}\\ {\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]=\tilde{H}\tilde{X}+\tilde{V}$

上式中，为系统观测矢量，为系统观测矩阵，为系统观测矩阵，为状态向量。

步骤9，结合图5，根据步骤8构建的系统状态方程和量测方程进行全维滤波解算，将所得滤波结果对系统进行校正，具体如下：

当可见卫星数目大于四颗时系统将面临两种选择：通过选星算法选择四颗卫星作为导航星；采用全维滤波方式使用全部可见星的观测量作为滤波器输入，全维滤波方式中组合导航系统工作模式的切换如图5所示。全维滤波算法与选星算法的比较如下：

选星算法可以在不增加滤波器系统维数的情况下提高导航精度。缺点是其他卫星提供的有效信息会被浪费，从而降低系统的可观测性。全维滤波使用所有可见卫星进行组合滤波，增强了系统的观测性；但观测维数的增加导致滤波时间加长。

对于“DSP+FPGA”平台，数据的接收和发送均由FPGA完成，DSP与FPGA之间通过EMIF连接，传输耗时可忽略不计。不妨设卡尔曼滤波器全维滤波时间为t_k，数据的同步时间为t_c，惯导解算时间为t_ins，其他数据运算时间为t_other。若惯导的解算周期为T，那么需满足下列公式成立：

T≥t_c+t_k+t_ins+t_other

当处理器选择TI公司的TMS320C6713时，测得各项时间如下表所示：

表一：时间测试

时间项	tc	tins	tother
				耗时	0.5ms	0.38ms	0.1ms

表二：不同收星数下的滤波时间

收星数	4	6	8	12
					tk	5.03ms	5.5ms	6.1ms	8.9ms

若惯导的解算周期设为10ms，从上表可以看到，即使是进行全维滤波，总时间仍小于解算周期，说明采用全维滤波完全可行。

本发明利用惯性信息(三维位置、速度)结合本地时间、卫星星历等信息，实时计算可视卫星的相对多普勒频移及变化率辅助GNSS接收机进行信号跟踪。针对惯性辅助间隔内的多普勒频移误差积累问题，采用曲线拟合估计加加速度的方法对惯性辅助信息进行数据递推以消除加速度残差。将GNSS接收机输出的伪距、伪距率与计算得到的伪距、伪距率作差，并将差值作为观测量进行全维滤波，根据滤波结果校正当前系统。本发明方法可实现基于惯性/卫星的伪距、伪距率组合导航，提高了导航精度和对复杂环境的适应性，应用前景广阔。

Claims (9)

1.一种基于DSP+FPGA的超紧组合导航方法，其特征在于，包括以下步骤：

步骤1，系统上电后完成硬件初始化工作，包括串口初始化、总线初始化、定时器初始化；

步骤2，SINS系统初始对准，即对SINS系统的初始速度、位置、姿态进行初始对准；

步骤3，GNSS接收机初始化，为相关器各通道分配C/A码，设置搜捕卫星，开始可见卫星的捕获；

步骤4，导航计算机接收SINS数据，采用四元数法进行导航解算，确定出载体的速度、位置、姿态信息，并装订成惯性辅助信息发送给GNSS接收机；

步骤5，导航处理器根据TIC锁存数据确定卫星的伪距和伪距率，并对导航星的伪距测量误差进行补偿；同时解调出导航星发送的导航电文，并根据导航电文解算导航卫星的位置、速度信息；

步骤6，GNSS接收机将步骤3中已捕获卫星的通道转为跟踪状态，利用步骤4提供的载体位置、速度信息，以及步骤5提供的卫星位置和速度，实时计算由于卫星和载体之间相对运动造成的多普勒频移及变化率，并利用所得结果对载波跟踪环路进行反馈校正；

步骤7，根据步骤5提供的导航星速度、位置信息，以及步骤4提供的载体速度、位置信息，确定载体相对每颗导航星的伪距、伪距率信息；

步骤8，构建系统状态方程和量测方程：状态变量包括SINS系统误差、GNSS系统误差和惯性器件误差，SINS系统误差包括姿态误差、速度误差和位置误差；GNSS系统误差包括钟差和钟漂；惯性器件误差包括随机误差；

步骤9，根据步骤8构建的系统状态方程和量测方程进行全维滤波解算，将所得滤波结果对系统进行校正。

2.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤2中所述SINS系统初始对准，即采用动基座传递对准方式，实现姿态、位置、速度的初始化。

3.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤3中所述GNSS接收机初始化，为相关器各通道分配C/A码，设置搜捕卫星，开始可见卫星的捕获，具体如下：

(3.1)GNSS接收机初始化，通过初始对准提供的载体位置，结合卫星的轨道分布预测搜捕卫星，并为相关器各通道分配相关卫星C/A码，设置搜捕卫星；

(3.2)根据Flash中保存的卫星历书信息确定各通道正在搜捕卫星的位置和速度，结合步骤2中初始对准提供的载体位置、速度信息确定卫星的多普勒频移。

4.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤4中所述导航计算机接收SINS数据，采用四元数法进行导航解算，确定出载体的速度、位置、姿态信息，具体如下：

(4.1)由FPGA接收惯导数据并将数据存入FIFO中，当接收到完整的一包数据后触发接收中断，由导航计算机读取IMU数据；

(4.2)导航计算机进行SINS导航解算，采用四元数算法计算出载体的速度、位置、姿态信息，具体如下：

(4.2.1)捷联惯导系统姿态更新解算

四元数微分方程表达式为：

$\stackrel{\·}{Q}=\frac{1}{2}\mathrm{\Ω}Q$

其中，Ω为载体坐标系相对导航系下的角速率构成的反对称矩阵，Q为四元数；

通过龙格-库塔求解四元数微分方程，然后由四元数求得姿态矩阵，由姿态矩阵求解三个姿态角；

(4.2.2)捷联惯导系统速度解算

速度微分方程如下：

${\stackrel{\·}{V}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×V^n+g^n$

其中，是由四元数求得的姿态矩阵，为地球自转角速率在n系的投影，为n系相对e系的旋转角速率，Vⁿ为n系下载体速度矢量，f^b为加速度计测得的在载体坐标系下的输出值，gⁿ为当地重力加速度矢量；

(4.2.3)捷联惯导系统位置解算

位置微分方程如下：

$\left\{\begin{array}{c}\stackrel{\·}{L}=\frac{V_N}{R_M+h}\\ \stackrel{\·}{\mathrm{\λ}}=\frac{V_E\sec L}{R_N+h}\\ \stackrel{\·}{h}=v_U\end{array}\right.$

上式中，L，λ和h分别是载体的纬度、经度和高度；V_E、V_N和V_U分别是载体在东、北、天方向上的速度分量；R_M、R_N分别是地球的子午圈半径和卯酉圈半径；

(4.3)将SINS系统解算得到的地理坐标系下的位置、速度、加速度信息转换到ECEF坐标系下，并装订成惯性辅助信息写入FIFO中，由FPGA串口发送给GNSS接收机。

5.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤5中所述导航处理器根据TIC锁存数据确定卫星的伪距和伪距率，并对导航星的伪距测量误差进行补偿；同时解调出导航星发送的导航电文，并根据导航电文解算导航卫星的位置、速度信息，具体为：

(5.1)导航处理器根据TIC锁存数据确定卫星的伪距和伪距率：在GPS的导航电文中，根据Z计数确定每一帧开始时对应的GPS时间；在帧同步后，对数据位、码周期数、半个码片滑动整数及小数部分分别进行计数得到测量时刻信号的发射时间，用发射时间减去接收时间得到传播延时，从而实现伪距的测量；

(5.2)对导航星的伪距测量误差进行补偿，即对伪距补偿地球自转和对流层影响，地球自转和对流层影响如下：

$(\frac{P_x-p_x}{P_y}-\frac{P_y-p_y}{P_x})*we*light\_speed,\frac{2.47}{sin\mathrm{\θ}+0.0121}*light\_speed$

上式第一项为地球自转影响，第二项为对流层效应影响，其中P_x、P_y分别为卫星ECEF坐标系下x、y轴位置，p_x、p_y分别为载体ECEF坐标系下x、y轴位置，we为地球自转角速率，light_speed为光速，θ为卫星的高度角；

校正后，得到导航卫星伪距ρ_Gj、伪距率信息，导航卫星的伪距ρ_Gj公式为：

ρ_Gj＝r_j-δt_u-v_ρj

其中，r_j为载体到第j颗卫星的真实无差距离，δt_u为钟差引起的距离误差，v_ρj为伪距测量白噪声；

导航卫星的伪距率公式如下：

${\stackrel{\·}{\mathrm{\ρ}}}_{Gj}={\stackrel{\·}{r}}_j-{\mathrm{\δt}}_{ru}-v_{\stackrel{\·}{\mathrm{\ρ}}j}$

其中，δt_ru为钟漂引起的距离率误差，为伪距率测量白噪声，为载体到第j颗卫星S^j的真实无差距离变化率；

(5.3)导航电文是GPS信号中调制的数据，GPS接收机接收到卫星信号后，进行载波解调和伪码解扩，得到50bps的数据码，按照导航电文的格式，即可将数据码编译成导航电文，卫星位置和速度的计算公式参考ICD-GPS-200C。

6.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤6中所述GNSS接收机将步骤3中已捕获卫星的通道转为跟踪状态，利用步骤4提供的载体位置、速度信息，以及步骤5提供的卫星位置和速度，实时计算由于卫星和载体之间相对运动造成的多普勒频移及变化率，并利用所得结果对载波跟踪环路进行反馈校正，具体如下：

(6.1)计算多普勒频移，设第i通道所跟踪的卫星在ECEF中的位置为步骤(4.2.3)中递推得到的载体位置P′_u＝(X_u,Y_u,Z_u)，则相对位置的单位视线矢量Rⁱ为：

$R^i=\frac{(X_S^i-X_u,Y_S^i-Y_u,Z_S^i-Z_u)}{\sqrt{{(X_S^i-X_u)}^2+{(Y_S^i-Y_u)}^2+{(Z_S^i-Z_u)}^2}}$

式中，(X_u,Y_u,Z_u)为载体的真实无差位置，为由卫星星历确定的第j颗卫星在ECEF坐标系中的位置；

观测时刻GNSS接收机第i通道所跟踪卫星信号的载波多普勒频移及变化率为：

$f_{dop}^i=\frac{f_{L1}}{c}\·(V_u^{\′}-V_S)\·R^i$

${\stackrel{\·}{f}}_{dop}^i=\frac{f_{L1}}{c}\·({\mathrm{\α}}_u^{\′}-{\mathrm{\α}}_s)\·R^i$

式中，是载波多普勒频移，是载波多普勒频移变化率，c是光速，f_L1是信号载波频率，V_S、α_S分别是观测时刻载体的三维速度、加速度；V′_u、α'_u分别是观测时刻接收机i通道卫星的三维速度、加速度；

(6.2)对辅助信息进行数据拟合，接收机载波环是由鉴相器、环路滤波器和数控振荡器构成的负反馈闭环控制系统，滤波器若选择二阶滤波器，则传递函数F(s)表达成：

$F\left(s\right)=\frac{{\mathrm{\τ}}_{2}s+1}{{\mathrm{\τ}}_{1}s}$

式中，F(s)为滤波器传递函数，τ₁和τ₂是滤波器系数，s为拉普拉斯变换算子；

常规载波跟踪环的闭环传递函数H(s)和误差传递函数H_e(s)分别为：

$H\left(s\right)=\frac{{\mathrm{\θ}}_e\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{2{\mathrm{\ξw}}_n^{2}s+w_n^2}{s^2+2{\mathrm{\ξw}}_n^{2}s+w_n^2}$

$H_e\left(s\right)=\frac{\mathrm{\δ}\mathrm{\θ}\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{s^2}{s^2+2{\mathrm{\ξw}}_n^{2}s+w_n^2}$

式中，w_n表示特征频率且K表示环路增益，ξ表示阻尼系数且θ_r(s)、θ_e(s)分别是滤波器输入相位、输出相位，δθ(s)为相位误差，且δθ(s)＝θ_e(s)-θ_γ(s)；

由此可知加入惯性辅助后的闭环传递函数和误差传递函数分别为

$H\left(s\right)=\frac{{\mathrm{\θ}}_e\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{(1-k)s^2+(\mathrm{\τ}s+1)(2{\mathrm{\ξw}}_{n}s+w_n^2)}{(\mathrm{\τ}s+1)(s^2+2{\mathrm{\ξw}}_{n}s+w_n^2)}$

$H_e\left(s\right)=\frac{\mathrm{\δ}\mathrm{\θ}\left(s\right)}{{\mathrm{\θ}}_r\left(s\right)}=\frac{(\mathrm{\τ}s+k)s^2}{(\mathrm{\τ}s+1)(s^2+2{\mathrm{\ξw}}_{n}s+w_n^2)}$

其中，k表示刻度系数误差，τ表示滞后时间常数；

当载体在视线矢量上作匀加加速度运动时，根据拉式变换有：

${\mathrm{\θ}}_r\left(s\right)=\frac{{\mathrm{\θ}}_0}{s}+\frac{\mathrm{\ν}}{s^2}+\frac{A}{s^3}+\frac{J}{s^4}$

式中，θ₀为鉴相器输出的相位误差，ν、A和J分别是载体与卫星之间的相对速度、加速度和加加速度在视线矢量上的投影；

根据终值定理，得到环路的稳态误差E(∞)为

$E\left(\mathrm{\∞}\right)=\underset{s\→0}{\lim }s\mathrm{\δ}\mathrm{\θ}\left(s\right)=\underset{s\→0}{\lim }{\mathrm{s\θ}}_r\left(s\right)H_e\left(s\right)$

根据以上公式可知：二阶锁相环不能跟踪载体的加加速度，而当载体作匀加速运动时，有惯性辅助时的稳态误差为无惯性辅助时的稳态误差为

7.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤7中所述根据步骤5提供的导航星速度、位置信息，以及步骤4提供的载体速度、位置信息，确定载体相对每颗导航星的伪距、伪距率信息，具体为：

根据导航星的速度、位置信息，以及SINS的速度、位置信息，计算出载体相对每颗导航星的伪距、伪距率信息，其中载体到第j颗卫星的伪距ρ_Ij为：

ρ_Ij＝r_j+e_j1δx+e_j2δy+e_j3δz

其中，δx、δy、δz分别为载体在地球坐标系中的位置误差在x轴、y轴、z轴分量，r_j为载体到第j颗卫星的真实无差距离，e_j1、e_j2、e_j3分别为载体与第j颗卫星x轴、y轴、z轴的方向余弦；

载体到第j颗卫星的伪距率为：

${\stackrel{\·}{\mathrm{\ρ}}}_{Ij}={\stackrel{\·}{r}}_j+e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}$

其中，分别为载体在地球坐标系中的速度误差在x轴、y轴、z轴分量，为载体到第j颗卫星的真实无差距离变化率。

8.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤8中所述构建系统状态方程和量测方程：

(8.1)组合导航系统通过卡尔曼滤波器对系统中的误差量进行估计，并将估计值作为校正量反馈到系统中，实现组合校正；状态变量包括SINS系统误差、GNSS系统误差和惯性器件误差，SINS系统误差包括姿态误差、速度误差和位置误差；GNSS系统误差包括钟差和钟漂；惯性器件误差包括随机误差；由此建立的超紧组合导航系统状态方程如下：

上式中，φ_E、φ_N、φ_U分别为东、北、天方向平台失准角，δV_E、δV_N、δV_U分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，δt_u为与时钟等效的距离误差，δt_ru为与时钟频率等效的距离率误差，R_M为椭球子午圈上各点的曲率半径，R_N为卯酉圈上各点的曲率半径，w_ie为地球转动角速率，f_E、f_N、f_U分别是惯导系统的比力在导航系下东、北、天方向上的分量，ε_E、ε_N、ε_U分别为地理坐标系内陀螺的漂移在东、北、天方向上的投影，▽_E、▽_N、▽_U分别为地理坐标系内加速计的漂移在东、北、天方向上的投影，β_tru为反相关时间；L和h分别是载体的纬度和高度；V_E、V_N和V_U分别是载体在东、北、天方向上的速度分量；

(8.2)以导航系统的姿态误差、速度误差、位置误差及钟差、钟漂误差为状态变量，建立惯性/卫星组合导航系统的状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中，X为系统状态矢量，表示系统状态矢量的导数，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，具体如下：

系统状态矢量：

$X={\[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,\mathrm{\δ}L,\mathrm{\δ}\mathrm{\λ},\mathrm{\δ}h,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\δt}}_u,{\mathrm{\δt}}_{ru}\]}_{17\×1}^T$

上式中共17个状态变量，φ_E,φ_N,φ_U分别为东、北、天方向姿态误差角；δV_E,δV_N，δV_U分别为东、北、天方向速度误差；δL,δλ,δh分别为纬度误差、经度误差、高度误差；ε_x,ε_y，ε_z分别为载体系下陀螺三个轴向的随机漂移；▽_x,▽_y,▽_z分别为载体系下加速度计三个轴向的偏差；δt_u和δt_ru分别为钟差引起的等效距离误差和钟漂引起的距离率误差；

状态转移矩阵：

$F={\left[\begin{array}{cccc}{\left(F_{ins}\right)}_{9\×9}& {\left(F_{sg}\right)}_{9\×6}& 0& 0\\ 0_{6\×9}& 0_{6\×6}& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& -{\mathrm{\β}}_{tru}\end{array}\right]}_{17\×17},F_{sg}={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]}_{9\×6}$

其中F_ins由步骤(8.1)中误差方程构成，为载体坐标系到导航坐标系的姿态转换矩阵，

系统噪声驱动矩阵：

$G={\left[\begin{array}{cccc}{C}_b^n& 0_{3\×3}& 0& 0\\ 0_{3\×3}& C_b^n& 0& 0\\ 0_{9\×3}& 0_{9\×3}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}_{17\×8}$

系统噪声矢量：

$W={\left[\begin{array}{cccccccc}{w}_{gx}& w_{gy}& w_{gz}& w_{ax}& w_{ay}& w_{az}& w_{tu}& w_{tru}\end{array}\right]}_{8\×1}^T$

假设系统噪声为零均值高斯白噪声，协方差阵为E(WW^T)＝Q，Q为系统过程噪声方差阵；w_gx、w_gy、w_gz分别为陀螺仪x轴、y轴、z轴方向的随机白噪声，w_ax、w_ay、w_az分别为加速度计x轴、y轴、z轴方向的随机白噪声，w_tu、w_tru分别为伪距随机和伪距率随机白噪声；

(8.3)计算每个通道对应的每颗导航星的高度角，方位角信息，方法如下：首先由将ECEF坐标系中载体到相应导航星的观测向量转换到以载体位置为原点的地理坐标系中，计算公式如下：

G＝S*E

其中

$\begin{array}{cc}G=\left[\begin{array}{c}\mathrm{\Δ}e\\ \mathrm{\Δ}n\\ \mathrm{\Δ}u\end{array}\right]& E=\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right]\end{array}$

其中，E＝[ΔxΔyΔz]^T为ECEF坐标系内载体到该卫星的观测向量，G＝[ΔeΔnΔu]^T为向量E在以载体位置为原点的地理坐标系三个方向上的投影；

$S=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\sin L\cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right],\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]-\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

其中，[XYZ]^T为卫星在ECEF坐标系中的位置，[xyz]^T为载体在ECEF坐标系中位置，L,λ分别为载体的纬度、经度；

最终方位角和高度角由以下计算公式得出：

$\mathrm{\θ}=\arcsin (\mathrm{\Δ}u/\sqrt{{\left(\mathrm{\Δ}e\right)}^2+{\left(\mathrm{\Δ}n\right)}^2+{\left(\mathrm{\Δ}u\right)}^2})$

α＝arctan(Δe/Δn)

上式中，θ,α分别为高度角、方位角，其中θ∈(0,π/2),α∈(0,2π)；

(8.4)根据步骤(8.3)计算得到的高度角、方位角确定可见卫星，根据可见卫星个数，系统动态调整观测方程及组合滤波器相关矩阵维数；

(8.5)构建系统量测方程

Z(t)＝H(t)X(t)+V(t)

其中，Z(t)为系统观测矢量，H(t)为系统观测矩阵，V(t)为系统观测噪声阵，X为系统状态矢量；量测方程的维数及组合滤波器的维数根据可见星数量变化，若可见星数量为n，则观测向量Z的维数为2n×1；系统观测矩阵H维数为2n×17；系统观测噪声方差R阵维数为2n×2n；卡尔曼滤波增益阵维数为17×2n；

伪距观测方程如下：

${\tilde{Z}}_{\mathrm{\ρ}}={\tilde{H}}_{\mathrm{\ρ}}\tilde{X}+{\tilde{V}}_{\mathrm{\ρ}}=\left[\begin{array}{cccc}{0}_{n\×6}& {\tilde{H}}_{\mathrm{\ρ}n1}& 0_{n\×6}& {\tilde{H}}_{\mathrm{\ρ}n2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\mathrm{\ρ}}$

式中，为伪距观测矢量，为伪距观测矩阵，为伪距观测噪声阵，为伪距状态矢量，分别为：

${\tilde{Z}}_{\mathrm{\ρ}}={\left[\begin{array}{ccc}{\mathrm{\δ\ρ}}^1& ...& {\mathrm{\δ\ρ}}^n\end{array}\right]}_{1\×n}^T,{\tilde{V}}_{\mathrm{\ρ}}={\left[\begin{array}{ccc}{v}_{\mathrm{\ρ}}^1& ...& v_{\mathrm{\ρ}}^1\end{array}\right]}_{1\×n}^T$

δρ_j＝ρ_Ij-ρ_Gj＝e_j1δx+e_j2δy+e_j3δz+δt_u+ν_ρj

${\tilde{H}}_{\mathrm{\ρ}n1}={\left[\begin{array}{ccc}{\tilde{a}}_{11}& {\tilde{a}}_{12}& {\tilde{a}}_{13}\\ .& .& .\\ .& .& .\\ .& .& .\\ {\tilde{a}}_{n1}& {\tilde{a}}_{n2}& {\tilde{a}}_{n3}\end{array}\right]}_{n\×3},{\tilde{H}}_{\mathrm{\ρ}n2}={\left[\begin{array}{cc}1& 0\\ .& .\\ .& .\\ .& .\\ 1& 0\end{array}\right]}_{n\×2}$

其中， $\begin{array}{ccc}{v}_{\mathrm{\ρ}}^1& ...& v_{\mathrm{\ρ}}^n\end{array}$ 为各通道伪距测量白噪声，ρ_Ij、ρ_Gj分别为步骤7中所求载体相对每颗卫星伪距和步骤5中GNSS接收机输出的卫星伪距，展开如下：

$\left\{\begin{array}{c}{\tilde{a}}_{i1}=(R_N+h)\[-e_{i1}\sin L\cos \mathrm{\λ}-e_{i2}\sin L\sin \mathrm{\λ}\]\\ +\[R_N{(1-f)}^2+h\]e_{i3}\cos L\\ {\tilde{a}}_{i2}=(R_N+h)\[e_{i2}\cos L\cos \mathrm{\λ}-e_{i1}\cos L\sin \mathrm{\λ}\]\\ {\tilde{a}}_{i3}=e_{i1}\cos L\cos \mathrm{\λ}+e_{i2}\cos L\sin \mathrm{\λ}+e_{i3}\sin L\end{array}\right.$

伪距率观测方程如下所示：

${\tilde{Z}}_{\stackrel{\·}{\mathrm{\ρ}}}={\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}}\tilde{X}+{\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}=\left[\begin{array}{cccc}{0}_{n\×3}& {\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n1}& 0_{n\×9}& {\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}$

式中，为伪距率观测矢量，为伪距率观测矩阵，为伪距率观测噪声阵，为伪距率状态矢量，分别为：

$\begin{array}{cc}{\tilde{Z}}_{\stackrel{\·}{\mathrm{\ρ}}}={\left[\begin{array}{ccc}\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^1& ...& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}_{1\×n}^T& {\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}={\left[\begin{array}{ccc}{\mathrm{\ν}}_{\stackrel{\·}{\mathrm{\ρ}}}^1& ...& {\mathrm{\ν}}_{\stackrel{\·}{\mathrm{\ρ}}}^1\end{array}\right]}_{1\×n}^T\end{array}$

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j={\stackrel{\·}{\mathrm{\ρ}}}_{Ij}-{\stackrel{\·}{\mathrm{\ρ}}}_{Gj}=e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}+{\mathrm{\δt}}_{ru}+{\mathrm{\ν}}_{\stackrel{\·}{\mathrm{\ρ}}j}$

${\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n1}={\left[\begin{array}{ccc}{\tilde{b}}_{21}& {\tilde{b}}_{22}& {\tilde{b}}_{23}\\ .& .& .\\ .& .& .\\ .& .& .\\ {\tilde{b}}_{n1}& {\tilde{b}}_{n2}& {\tilde{b}}_{n3}\end{array}\right]}_{n\×3},{\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}n1}={\left[\begin{array}{cc}0& 1\\ .& .\\ .& .\\ .& .\\ 0& 1\end{array}\right]}_{n\×2}$

其中，为各通道伪距率测量白噪声分别为步骤7中所求载体相对每颗卫星伪距率和步骤5中GNSS接收机输出的卫星伪距率，展开如下：

$\left\{\begin{array}{c}{\tilde{b}}_{i1}=-e_{i1}\sin \mathrm{\λ}+e_{i2}\cos \mathrm{\λ}\\ {\tilde{b}}_{i2}=-e_{i1}\sin L\cos \mathrm{\λ}-e_{i2}\sin L\sin \mathrm{\λ}+e_{i3}\cos L\\ {\tilde{b}}_{i3}=e_{i1}\cos L\cos \mathrm{\λ}+e_{i2}\cos L\sin \mathrm{\λ}+e_{i3}\sin L\end{array}\right.$

综合伪距、伪距率差观测方程，得到系统观测方程如下：

$\tilde{Z}=\left[\begin{array}{c}{\tilde{Z}}_{\mathrm{\ρ}}\\ {\tilde{Z}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]=\left[\begin{array}{c}{\tilde{H}}_{\mathrm{\ρ}}\\ {\tilde{H}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]\tilde{X}+\left[\begin{array}{c}{\tilde{V}}_{\mathrm{\ρ}}\\ {\tilde{V}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]=\tilde{H}\tilde{X}+\tilde{V}$

上式中，为系统观测矢量，为系统观测矩阵，为系统观测矩阵，为状态向量。

9.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤9中所述根据步骤8构建的系统状态方程和量测方程进行全维滤波解算，将所得滤波结果对系统进行校正，设卡尔曼滤波器全维滤波时间为t_k，数据的同步时间为t_c，惯导解算时间为t_ins，其他数据运算时间为t_other，若惯导的解算周期为T，那么需满足下列公式成立：

T≥t_c+t_k+t_ins+t_other

。