根据步骤（8. 3)计算得到的高度角、方位角确定可见卫星。根据可见卫星个 数，系统动态调整观测方程及组合滤波器相关矩阵的维数；
[0129] (8. 5)构建系统量测方程
[0130] Z(t) = H(t)X(t)+V(t)
[0131] 其中Z(t)为系统观测矢量，H(t)为系统观测矩阵，V(t)为系统观测噪声阵，X为 系统状态矢量。量测方程的维数及组合滤波器的维数根据可见星数量变化，结合图4,若可 见星数量为n，则观测向量Z的维数为2nX 1 ;系统观测矩阵H维数为2nX 17 ;系统观测噪 声方差R阵维数为2nX2n ;卡尔曼滤波增益阵维数为17X2n ;
[0132] 伪距观测方程如下：
[0134] 式中，^为伪距观测矢量，为伪距观测矩阵，^为伪距观测噪声阵，f为伪距 状态矢量，分别为：
[0138] 其中，…％为各通道伪距测量白噪声，P Ip P e]分别为步骤7中所求载体相 对每颗卫星伪距和步骤5中GNSS接收机输出的卫星伪距，馬j = 1,2,3)展开如下：
[0142]式中，&为伪距率观测矢量，为伪距率观测矩阵，$为伪距率观测噪声阵，f 为伪距率状态矢量，分别为：
[0146]其中，< <为各通道伪距率测量白噪声分别为步骤7中所求载体相 对每颗卫星伪距率和步骤5中GNSS接收机输出的卫星伪距率，/),,(/ =卜= 1,2,3)展开如 下：
[0150] 上式中，'之为系统观测矢量，H为系统观测矩阵，F为系统观测矩阵，f为状态向 量。
[0151] 步骤9,结合图5,根据步骤8构建的系统状态方程和量测方程进行全维滤波解算， 将所得滤波结果对系统进行校正，具体如下：
[0152] 当可见卫星数目大于四颗时系统将面临两种选择：通过选星算法选择四颗卫星作 为导航星；采用全维滤波方式使用全部可见星的观测量作为滤波器输入，全维滤波方式中 组合导航系统工作模式的切换如图5所示。全维滤波算法与选星算法的比较如下：
[0153] 选星算法可以在不增加滤波器系统维数的情况下提高导航精度。缺点是其他卫星 提供的有效信息会被浪费，从而降低系统的可观测性。全维滤波使用所有可见卫星进行组 合滤波，增强了系统的观测性；但观测维数的增加导致滤波时间加长。
[0154] 对于"DSP+FPGA"平台，数据的接收和发送均由FPGA完成，DSP与FPGA之间通过 EMIF连接，传输耗时可忽略不计。不妨设卡尔曼滤波器全维滤波时间为t k，数据的同步时 间为t。，惯导解算时间为tins，其他数据运算时间为Uct。若惯导的解算周期为T，那么需满 足下列公式成立：
[0155] T^tc+tk+tins+tother
[0156] 当处理器选择TI公司的TMS320C6713时，测得各项时间如下表所示：
[0157] 表一：时间测试
[0161] 若惯导的解算周期设为10ms，从上表可以看到，即使是进行全维滤波，总时间仍小 于解算周期，说明采用全维滤波完全可行。
[0162] 本发明利用惯性信息（三维位置、速度）结合本地时间、卫星星历等信息，实时计 算可视卫星的相对多普勒频移及变化率辅助GNSS接收机进行信号跟踪。针对惯性辅助间 隔内的多普勒频移误差积累问题，采用曲线拟合估计加加速度的方法对惯性辅助信息进行 数据递推以消除加速度残差。将GNSS接收机输出的伪距、伪距率与计算得到的伪距、伪距 率作差，并将差值作为观测量进行全维滤波，根据滤波结果校正当前系统。本发明方法可实 现基于惯性/卫星的伪距、伪距率组合导航，提高了导航精度和对复杂环境的适应性，应用 前景广阔。
【主权项】
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. I) GNSS接收机初始化，通过初始对准提供的载体位置，结合卫星的轨道分布预测搜 捕卫星，并为相关器各通道分配相关卫星C/A码，设置搜捕卫星； (3.2)根据Flash中保存的卫星历书信息确定各通道正在搜捕卫星的位置和速度，结 合步骤2中初始对准提供的载体位置、速度信息确定卫星的多普勒频移。4. 根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤4中 所述导航计算机接收SINS数据，采用四元数法进行导航解算，确定出载体的速度、位置、姿 态信息，具体如下： (4. 1)由FPGA接收惯导数据并将数据存入FIFO中，当接收到完整的一包数据后触发接 收中断，由导航计算机读取IMU数据； (4. 2)导航计算机进行SINS导航解算，采用四元数算法计算出载体的速度、位置、姿态 信息，具体如下： (4. 2. 1)捷联惯导系统姿态更新解算 四元数微分方程表达式为：其中，Ω为载体坐标系相对导航系下的角速率构成的反对称矩阵，Q为四元数； 通过龙格-库塔求解四元数微分方程，然后由四元数求得姿态矩阵，由姿态矩阵求解 三个姿态角； (4. 2. 2)捷联惯导系统速度解算 速度微分方程如下：其中，q是由四元数求得的姿态矩阵，fiC为地球自转角速率在η系的投影，<为η系 相对e系的旋转角速率，VnSn系下载体速度矢量，fb为加速度计测得的在载体坐标系下 的输出值，g n为当地重力加速度矢量； (4. 2. 3)捷联惯导系统位置解算 位置微分方程如下：上式中，L，λ和h分别是载体的炜度、经度和高度；VE、VjPV。分别是载体在东、北、天 方向上的速度分量；RM、Rn分别是地球的子午圈半径和卯酉圈半径； (4. 3)将SINS系统解算得到的地理坐标系下的位置、速度、加速度信息转换到ECEF坐 标系下，并装订成惯性辅助信息写入FIFO中，由FPGA串口发送给GNSS接收机。5.根据权利要求1所述的基于DSP+FPGA的超紧组合导航方法，其特征在于，步骤5中 所述导航处理器根据TIC锁存数据确定卫星的伪距和伪距率，并对导航星的伪距测量误差 进行补偿；同时解调出导航星发送的导航电文，并根据导航电文解算导航卫星的位置、速度 信息，具体为： (5. 1)导航处理器根据TIC锁存数据确定卫星的伪距和伪距率：在GPS的导航电文中， 根据Z计数确定每一帧开始时对应的GPS时间；在帧同步后，对数据位、码周期数、半个码片 滑动整数及小数部分分别进行计数得到测量时刻信号的发射时间，用发射时间减去接收时 间得到传播延时，从而实现伪距的测量； (5. 2)对导航星的伪距测量误差进行补偿，即对伪距补偿地球自转和对流层影响，地球 自转和对流层影响如下：上式第一项为地球自转影响，第二项为对流层效应影响，其中Px、Py分别为卫星ECEF坐 标系下X、y轴位置，Px、别为载体ECEF坐标系下X、y轴位置，we为地球自转角速率， light_speed为光速，Θ为卫星的高度角； 校正后，得到导航卫星伪距P 、伪距率/?信息，导航卫星的伪距P 公式为： Pgj= r T5 tU-Vp j 其中，r,为载体到第j颗卫星的真实无差距离，δ t u为钟差引起的距离误差，V p ]为伪 距测量白噪声； 导航卫星的伪距率公式如下： Pfg=η-^ru-V.- 其中，Stra为钟漂引起的距离率误差，为伪距率测量白噪声，ξ为载体到第j颗卫 星s]的真实无差距离变化率； (5. 3)导航电文是GPS信号中调制的数据，GPS接收机接收到卫星信号后，进行载波 解调和伪码解扩，得到50bps的数据码，按照导航电文的格式，即可将数据码编译成导航电 文，卫星位置和速度的计算公式参考I⑶-GPS-20