一种基于dsp+fpga的超紧组合导航方法
【技术领域】
[0001] 本发明涉及组合导航技术领域，特别是一种基于DSP+FPGA的超紧组合导航方法。 二、
【背景技术】
[0002] 卫星/惯性组合导航系统结合卫星导航、惯性导航的优点，具有定位精度高，稳定 性强等特点，因此在军事领域及民用领域都被广泛应用。组合导航的模式有很多种，主要分 为松组合、紧组合、深组合三类。基于位置和速度的松组合系统虽然具有结构简单，计算量 小等优点，但在GNSS接收机收星数少于4颗无法定位时，系统无法正常组合，导航精度随之 下降。基于伪距、伪距率的紧组合导航系统虽然可以在收星数少于四颗的情况下进行组合， 但由于GNSS接收机收星数的减少，降低了观测变量的维数，导致导航精度变差。此外，无论 是松组合导航系统还是紧组合导航系统，在高动态环境下都面临GNSS接收机性能下降甚 至失效的问题。 三、
【发明内容】
[0003] 本发明的目的是提供一种高精度的基于DSP+FPGA的超紧组合导航方法，能够有 效抑制导航精度的发散。
[0004] 实现本发明目的的技术解决方案为：一种基于DSP+FPGA的超紧组合导航方法，包 括以下步骤：
[0005] 步骤1，系统上电后完成硬件初始化工作，包括串口初始化、总线初始化、定时器初 始化；
[0006] 步骤2, SINS系统初始对准，即对SINS系统的初始速度、位置、姿态进行初始对 准；
[0007] 步骤3, GNSS接收机初始化，为相关器各通道分配C/A码，设置搜捕卫星，开始可见 卫星的捕获；
[0008] 步骤4,导航计算机接收SINS数据，采用四元数法进行导航解算，确定出载体的速 度、位置、姿态信息，并装订成惯性辅助信息发送给GNSS接收机；
[0009] 步骤5,导航处理器根据TIC锁存数据确定卫星的伪距和伪距率，并对导航星的伪 距测量误差进行补偿；同时解调出导航星发送的导航电文，并根据导航电文解算导航卫星 的位置、速度信息；
[0010] 步骤6, GNSS接收机将步骤3中已捕获卫星的通道转为跟踪状态，利用步骤4提供 的载体位置、速度信息，以及步骤5提供的卫星位置和速度，实时计算由于卫星和载体之间 相对运动造成的多普勒频移及变化率，并利用所得结果对载波跟踪环路进行反馈校正；
[0011] 步骤7,根据步骤5提供的导航星速度、位置信息，以及步骤4提供的载体速度、位 置信息，确定载体相对每颗导航星的伪距、伪距率信息；
[0012] 步骤8,构建系统状态方程和量测方程：状态变量包括SINS系统误差、GNSS系统误 差和惯性器件误差，SINS系统误差包括姿态误差、速度误差和位置误差；GNSS系统误差包 括钟差和钟漂；惯性器件误差包括随机误差；
[0013] 步骤9,根据步骤8构建的系统状态方程和量测方程进行全维滤波解算，将所得滤 波结果对系统进行校正。
[0014] 本发明与现有技术相比，其显著优点是：⑴由于采用GNSS输出的原始伪距、伪距 率信息，从而消除了量测输出的时间相关性，并且在收星数少于四颗的情况下仍然可以实 现组合导航；(2)采用全维滤波方式组合，利用全部观测信息，从而实现最佳精度导航；（3) 采用惯性信息辅助GNSS接收机跟踪环路，实现GNSS与INS之间的双向辅助；(4)对于惯性 高动态飞行过程中加加速度所造成的多普勒频移残差问题，根据载体飞行过程中的受力情 况对载体运动状态构建相应数学模型，实时计算观测时刻各通道多普勒频移及其变化率， 提高GNSS接收机的动态适应性。 四、
【附图说明】
[0015] 图1是本发明基于DSP+FPGA的超紧组合导航方法的流程图。
[0016]图2是本发明基于DSP+FPGA的超紧组合导航方法的系统硬件结构图。
[0017]图3是本发明基于DSP+FPGA的超紧组合导航方法的惯性辅助载波环数学模型图。
[0018] 图4是本发明基于DSP+FPGA的超紧组合导航方法的系统维数变化示意图。
[0019] 图5是本发明基于DSP+FPGA的超紧组合导航方法的系统原理结构图。 五、
【具体实施方式】
[0020] 下面结合附图及具体实施例对本发明作进一步详细说明。
[0021] 结合图1，本发明基于DSP+FPGA的超紧组合导航方法，步骤如下：
[0022] 步骤1，系统上电后完成硬件初始化工作，包括串口初始化、总线初始化、定时器初 始化；
[0023] 步骤2, SINS系统初始对准，即对SINS系统的初始速度、位置、姿态进行初始对准， 具体如下：
[0024] SINS初始对准，初始化速度、位置。采用动基座传递对准技术，实现姿态、位置、速 度的快速，精确初始化。
[0025] 步骤3, GNSS接收机初始化，为相关器各通道分配C/A码，设置搜捕卫星，开始可见 卫星的捕获，具体如下：
[0026] (3. 1)GNSS接收机初始化，通过初始对准提供的载体位置，结合卫星的轨道分布 预测搜捕卫星，并为相关器各通道分配相关卫星C/A码，设置搜捕卫星；二维搜索中，为了 提高捕获速度，由一次搜索1/2码片增加为一次搜索3个码片，即由即时相关峰一路相关 增加为超前相关峰、即时相关峰、滞后相关峰、延迟超前相关峰、延迟即时相关峰、延迟滞后 相关峰等六路相关峰。采用六路相关提高了二维搜索中码片搜索的速度，在扫频范围同为 ± 10KHZ的情况下，捕获速度将提高为原来一路相关方法的6倍。
[0027] (3. 2)根据Flash中保存的卫星历书信息计算各通道正在搜捕卫星的位置和速 度，结合步骤2中初始对准提供的载体位置、速度信息计算卫星的多普勒频移，从而缩小搜 索范围，加快捕获速度。
[0028] 步骤4,导航计算机接收SINS数据，采用四元数法进行导航解算，确定出载体的速 度、位置、姿态信息，并装订成惯性辅助信息发送给GNSS接收机，硬件图如图2所示，具体如 下：
[0029] (4. 1)由FPGA接收惯导数据并将数据存入FIFO中，当接收到完整的一包数据后触 发接收中断，由导航计算机读取IMU数据；
[0030] (4. 2)导航计算机进行SINS导航解算，采用四元数算法计算出载体的速度、位置、 姿态信息，具体如下：
[0031] (4. 2. 1)捷联惯导系统姿态更新解算，采用传统四元数算法。
[0032] 四元数微分方程表达式为：
[0034]其中，Q为载体坐标系相对导航系下的角速率构成的反对称矩阵，Q为四元数。
[0035] 通过龙格-库塔求解四元数微分方程，然后由四元数求得姿态矩阵，由姿态矩阵 求解三个姿态角；
[0036] (4. 2. 2)捷联惯导系统速度解算。
[0037] 速度微分方程如下：
[0039] 其中，O1是由四元数求得的姿态矩阵，< 为地球自转角速率在n系的投影，C为 n系相对e系的旋转角速率，，为n系下载体速度矢量，f b为加速度计测得的在载体坐标系 下的输出值，gn为当地重力加速度矢量；
[0040] (4. 2. 3)捷联惯导系统位置解算。
[0041] 位置微分方程如下：
[0043] 上式中，L，入和h分别是载体的炜度、经度和高度；VE、％和V。分别是载体在东、 北、天方向上的速度分量；R M、Rn分别是地球的子午圈半径和卯酉圈半径；
[0044] (4. 3)将SINS解算得到的地理坐标系下的位置、速度、加速度等信息转换到ECEF 坐标系下，装订成惯性辅助信息后写入FIFO中，由FPGA串口发送给GNSS接收机。
[0045] 步骤5,导航处理器根据TIC锁存数据确定卫星的伪距和伪距率，并对导航星的伪 距测量误差进行补偿；同时解调出导航星发送的导航电文，并根据导航电文解算导航卫星 的位置、速度信息，具体如下：
[0046] (5. 1)根据TIC锁存数据计算卫星的伪距和伪距率：具体测量时，由于接收机时间 可以预知或假定，因此只要知道信号的发射时间就可以求得伪距。在GPS的导航电文中，根 据Z计数确定每一帧开始时对应的GPS时间；在帧同步后，对数据位、码周期数、半个码片滑 动整数及小数部分分别进行计数得到测量时刻信号的发射时间，用发射时间减去接收时间 就得到传播延时，从而实现了伪距的测量。
[0047] (5. 2)对导航星的伪距测量误差进行补偿，即对伪距补偿地球自转和对流层影响， 地球自转和对流层影响如下：
[0048] 地球自转影响：
[0050] 其中，Px、Py分别为卫星ECEF坐标系下x、y轴位置，p x、py分别为载体ECEF坐标 系下x、y轴位置，we为地球自转角速率，light_speed为光速；
[0051] 对流层效应影响：
[0053] 其中，9为卫星的高度角。
[0054] 校正后，得到导航卫星伪距P、伪距率九,信息，导航卫星的伪距P公式为：
[0055] PGj=rr5tU-Vpj
[0056]其中，r,为载体到第j颗卫星的真实无差距离，S t u为钟差引起的距离误差，V p ] 为伪距测量白噪声。
[0057] 导航卫星的伪距率為r公式如下：
[0058]