一种嵌入式组合导航系统及其方法
【专利摘要】本发明公开了一种嵌入式组合导航系统及其方法。该系统包括传感器模块、数据采集模块、数据处理解算模块和外围通讯接口模块，其中传感器模块包括光纤惯导IMU和GNSS接收机，数据采集模块为FPGA芯片，数据处理解算模块为DSP芯片，传感器模块与数据处理解算模块通过数据采集模块的高速总线连接；组合导航方法为：光纤惯导IMU和GNSS接收机的量测数据通过FPGA的总线传输至DSP进行数据处理解算，建立SINS/GNSS组合导航系统数学模型进行多级容错组合导航，根据子系统状态判断，进行导航决策匹配，将得到的实时速度、位置及姿态信息通过外围通讯接口模块发送至外围设备。该系统具有高集成化、接口丰富、精度高及容错性能好的优点。
【专利说明】-种嵌入式组合导航系统及其方法 -【技术领域】
[0001] 本发明属于组合导航领域，特别是一种嵌入式组合导航系统及其方法。 二【背景技术】
[0002] 随着航空技术对导航系统性能的要求越来越高，在工程应用上，组合导航系统逐 步代替了纯惯性导航系统。卫星/惯性组合导航结合了惯性导航系统与全球卫星导航系统 的优点，其通过后者对前者进行辅助修正，抑制了单一惯性系统的发散，实现了长航时下高 精度的组合导航，已被广泛应用于军事领域，如制导武器的导航系统，在民用领域也得到了 广泛应用，譬如车载导航系统等。
[0003] 对于现有的传统分布式组合导航系统而言，SINS子系统、GNSS子系统及组合导航 计算机之间采用设备间外围接口通讯的低级方式，组合导航系统性能易受各子系统间数据 通讯品质制约，同时存在体积大、设计分散、集成度较低等问题，从而影响了导航系统的使 用效能。
[0004] 此外，对于W SINS为主、GNSS辅助定位的组合导航系统而言，从本质上SINS及 GNSS子系统仍然是独立工作的，并且卡尔曼滤波是量测驱动的，在高动态及恶劣环境等情 况下各子系统出现的极短异常均会降低组合系统的导航精度，使系统稳定性变差；同时， 系统数学模型和噪声统计不准确、噪声突变等情况均可能导致常规卡尔曼滤波的收敛性变 差、精度下降，甚至导致滤波发散，同样极大地影响了组合系统的导航精度。 H
【发明内容】
[0005] 本发明的目的在于提供一种容错性能好、精度高的嵌入式组合导航系统及其方 法，能够在高动态及恶劣环境下进行稳定精确的导航。
[0006] 实现本发明目的的技术解决方案为：一种嵌入式组合导航系统，包括传感器模块、 数据采集模块、数据处理解算模块和外围通讯接口模块；其中传感器模块包括光纤惯导 IMU和GNSS接收机，数据采集模块为FPGA芯片，数据处理解算模块为DSP芯片；传感器模块 与数据处理解算模块通过数据采集模块的高速总线连接，光纤惯导IMU的ARM板通过FPGA 外扩RS_422串口 T2连接至DSP的UART_1，光纤惯导IMU的ARM板RS_422串口 C0M_1通 过连接器引出IMU数据调试接口；GNSS接收机的板卡C0M_A通过FPGA外扩RS_232串口 T9 连接至DSP的UART_2，GNSS接收机的板卡C0M_B接T10并引出至连接器用于接收机模式设 置；外围通讯接口模块包括RS_232串口和RS_422串口；光纤惯导IMU和GNSS接收机的量 测数据通过FPGA的总线传输至DSP进行数据处理解算，将得到的实时速度、位置及姿态信 息发送至外围通讯接口模块，外围接口模块将导航信息发送至外围设备。
[0007] -种嵌入式组合导航方法，包括W下步骤：
[0008] 步骤1，组合导航系统上电后，完成系统硬件初始化工作，然后进行自检；根据传 感器模块采集数据，检查组合系统是否异常；若无异常则进入步骤2,否则将发送异常信 息，并继续进行自检；
[0009] 步骤2,进行组合导航系统初始对准，为组合导航过程提供初始速度、位置及姿态 角信息；
[0010] 步骤3,捷联解算；将数据采集模块采集得到的传感器量测信息进行误差补偿后， 经捷联解算得到载体实时的姿态、速度及位置信息；
[0011] 步骤4,建立SINS/GNSS组合导航系统数学模型；建立SINS误差方程，利用自适应 卡尔曼滤波方法对光纤惯导IMU对应的SINS子系统和GNSS接收机对应的GNSS子系统进 行信息融合，估计光纤惯导IMU的各误差量并对其进行反馈校正；
[0012] 步骤5,子系统状态判断；根据光纤惯导IMU的巧螺采样值和加速度计采样值判断 SINS子系统的工作状态，根据GNSS接收机输出量测值判断GNSS子系统的工作状态；
[0013] 步骤6,导航决策匹配：根据SINS子系统和GNSS子系统的工作状态进行导航决 策匹配处理、输出导航信息，外围通讯接口模块将导航信息发送至外围设备，循环执行步骤 3?6,即所述多级容错组合导航流程。
[0014] 本发明与现有技术相比，其显著优点是；（1)在传感器模块中，IMU采用闭环光纤 巧螺仪，寿命长、动态范围大、瞬时启动，没有闭锁问题，也不用在石英块精密加工出光路， 成本更低；(2)数据处理解算模块采用高性能浮点型数字信号DSP，芯片采用模块化上电技 术，运算及处理速度快、功耗低；基于DSP/FPGA浅并行架构，全系统采用集中式供电，传感 器模块与数据处理模块通过高速总线通讯，数据的采集及处理将更加高效、稳定；（3)采用 自适应滤波算法作为信息融合算法，同时采用了多级容错组合导航方法，使组合系统在复 杂环境下具有更好的容错性、可靠性，更优的动态性能和更高的导航精度；（4)由于GNSS接 收装置具备输出伪距、伪距率信息的功能，使得本系统具有更好的拓展性，实现紧禪合系统 更为方便。 四【专利附图】
【附图说明】
[0015] 图1是本发明嵌入式组合导航系统的硬件结构示意图。
[0016] 图2是本发明嵌入式组合导航系统的工作流程图。
[0017] 图3是本发明嵌入式组合导航方法的原理框图。
[0018] 图4是本发明嵌入式组合导航方法的组合滤波流程示意图。 五【具体实施方式】
[0019] 下面结合附图及具体实施例对本发明作进一步详细说明。
[0020] 结合图1，本发明嵌入式组合导航系统，其整体尺寸为13cmX14cmX12cm，重 量《3kg ;该系统包括传感器模块、数据采集模块、数据处理解算模块和外围通讯接口模 块；其中传感器模块包括光纤惯导IMUQnedial Measurement化it,惯性测量单元）和 GNSS (Global P^Javigation Satellite System,全球导航卫星系统）接收机，数据采集模块 为FPGA芯片，数据处理解算模块为DSP芯片；传感器模块与数据处理解算模块通过数据采 集模块的高速总线连接，光纤惯导IMU的ARM板通过FPGA外扩RS_422串口 T2连接至DSP 的UART_1，光纤惯导IMU的ARM板RS_422串口 C0M_1通过连接器引出IMU数据调试接口； GNSS接收机的板卡C0M_A通过FPGA外扩RS_232串口 T9连接至DSP的UART_2, GNSS接收机 的板卡〇)1_8接1'10并引出至连接器用于接收机模式设置；外围通讯接口模块包括RS_232 串口和RS_422串口；
[0021] 光纤惯导IMU和GNSS接收机的量测数据通过FPGA的总线传输至DSP进行数据处 理解算，将得到的实时速度、位置及姿态信息发送至外围通讯接口模块，外围接口模块将导 航信息发送至外围设备。
[002引所述的光纤惯导IMU采用NV300系列闭环光纤巧螺惯性测量单元。NV300系列 惯性测量产品为一套完整的惯性传感器处理系统，包括了 H个闭环光纤巧螺和H个加速度 计、内部数据采集、信号处理、温控（温补）及供电电源装置。产品出厂前经过了对加速度 计、巧螺各项参数的标定和校准，使其动态性能达到最优化，从而为导航系统提供精确稳定 的惯性量测信息。该产品x，y，zH轴的角速率动态测量范围分别为±500° /s、±100° / s、± 100° /s，巧螺零偏重复性《2° /h，零偏稳定性《3° /h;H轴的加速度动态测量范 围均为± 15g，加速度计零偏重复性《0. Img，零偏稳定性《0. 05mg ;光纤惯导IMU与数据 采集模块采用RS_422单工方式连接，保证了数据操作的单向隔离，同时提供额外RS_422串 口输出IMU调试信息。
[0023] 所述的GNSS接收机采用JAVAD GG100系列，即JAVAD GPS/化0NASS二合一卫星接 收机GG100,该接收机将来可升级至北斗二代；其动态平面定位精度《3m (CE巧，高度定位 精度《6m(CE巧，视幡精度《0. lm/s，lPPS砂脉冲精度达50ns，失锁重捕时间《Is,数据输 出频率1-20化可调，具备性能好、抗干扰等特点。
[0024] 所述数据处理解算模块的DSP采用TI TMS320C6747系列，即TI公司的32位高性 能浮点型DSP TMS320C6747,该芯片具备模块化上电功能，相比其它C6000系列芯片具有更 低的功耗，更小的尺寸；其运算速度高达300MHz，处理速度> 1200MFL0PS ;内核使用两级缓 存结构，片内RAM为448KB ;同时，C6747具备丰富的外围模块，如H路UART，增强型DMA控 制器，外部接口模块EMIFA、EMIFB，两路SPI，可W满足导航处理与解算的需要。
[00巧]在数据处理解算模块中，本发明采用了复合式中断方式，通过读取DSP的GPI0 bank4和banks中断状态寄存器，实现GNSS接收机1PPS砂脉冲及FPGA外扩串口中断的集 中处理，降低了底层驱动配置及编写的难度。
[0026] 所述外围通讯接口模块主要由RS_232和RS_422接口组成，可外扩为地面监控计 算机、弹载计算机等设备。通过与外部监控设备相连（如台式计算机等），可W实现数据实 时显示与存储；也可W通过该接口将实时解算的导航信息传输至其它设备，用于进一步解 算（如弹载任务计算机等）。
[0027] 结合图2?4,本发明嵌入式组合导航方法，具体步骤如下：
[0028] 步骤1，组合导航系统上电后，完成系统硬件初始化工作，然后进行自检；根据传 感器模块采集数据，检查组合系统是否异常；若无异常则进入步骤2,否则将发送异常信 息，并继续进行自检。
[0029] 步骤2,进行组合导航系统初始对准，为组合导航过程提供初始速度、位置及精确 的姿态角信息0 0、Yci、￥(|，包括光纤惯导IMU子系统初始化、GNSS接收机子系统初始化， 然后进入图3所示的容错组合导航过程，其中初始四元数Q。可由0。、Y。、￥。确定，公式如 下：
[0030]
【权利要求】
1. 一种嵌入式组合导航系统，其特征在于，包括传感器模块、数据采集模块、数据处理 解算模块和外围通讯接口模块；其中传感器模块包括光纤惯导IMU和GNSS接收机，数据采 集模块为FPGA芯片，数据处理解算模块为DSP芯片；传感器模块与数据处理解算模块通过 数据采集模块的高速总线连接，光纤惯导IMU的ARM板通过FPGA外扩RS_422串口 T2连接 至DSP的UART_1，光纤惯导IMU的ARM板RS_422串口 C0M_1通过连接器引出IMU数据调试 接口；GNSS接收机的板卡COM_A通过FPGA外扩RS_232串口 T9连接至DSP的UART_2, GNSS 接收机的板卡COM_B接T10并引出至连接器用于接收机模式设置；外围通讯接口模块包括 RS_232 串口和 RS_422 串口； 光纤惯导IMU和GNSS接收机的量测数据通过FPGA的总线传输至DSP进行数据处理解 算，将得到的实时速度、位置及姿态信息发送至外围通讯接口模块，外围接口模块将导航信 息发送至外围设备。
2. 根据权利要求1所述的嵌入式组合导航系统，其特征在于，所述的光纤惯导IMU采用 NV300系列闭环光纤巧螺惯性测量单元。
3. 根据权利要求1所述的嵌入式组合导航系统，其特征在于，所述的GNSS接收机采用 JAVAD GPS/化ONASS 二合一卫星接收机 GG100。
4. 根据权利要求1所述的嵌入式组合导航系统，其特征在于，所述数据处理解算模块 的DSP采用TI公司的32位浮点型DSP TMS320C6747。
5. -种嵌入式组合导航方法，其特征在于，包括W下步骤： 步骤1，组合导航系统上电后，完成系统硬件初始化工作，然后进行自检；根据传感器 模块采集数据，检查组合系统是否异常；若无异常则进入步骤2,否则将发送异常信息，并 继续进行自检； 步骤2,进行组合导航系统初始对准，为组合导航过程提供初始速度、位置及姿态角信 息； 步骤3,捷联解算；将数据采集模块采集得到的传感器量测信息进行误差补偿后，经捷 联解算得到载体实时的姿态、速度及位置信息； 步骤4,建立SINS/GNSS组合导航系统数学模型；建立SINS误差方程，利用自适应卡尔 曼滤波方法对光纤惯导IMU对应的SINS子系统和GNSS接收机对应的GNSS子系统进行信 息融合，估计光纤惯导IMU的各误差量并对其进行反馈校正； 步骤5,子系统状态判断；根据光纤惯导IMU的巧螺采样值和加速度计采样值判断SINS 子系统的工作状态，根据GNSS接收机输出量测值判断GNSS子系统的工作状态； 步骤6,导航决策匹配：根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配 处理、输出导航信息，外围通讯接口模块将导航信息发送至外围设备，循环执行步骤3?6， 即所述多级容错组合导航流程。
6. 根据权利要求5所述的嵌入式组合导航方法，其特征在于，步骤3所述的捷联解算的 具体步骤如下： (3. 1)姿态更新算法采用传统四元数算法，其方法如下： 设载体坐标系相对于导航坐标系的转动四元数为： Q = q〇+Qii+屯 j+Qsk 则相应的四元数微分方程为： 0 = -〇.0 ~ 2 ~ 其中：
式中鸣《瓷、0在分别表示载体坐标系相对导航坐标系的角速率在载体坐标系x、y、 Z轴的分量； 由巧螺量测到的角速率《4计算得到载体系相对导航系的角速率在载体下的投影角速 率诚6,公式如下：
其中，为地球角速率，冷。为导航坐标系相对地球坐标系的角速率在导航坐标系下 的投影，G为导航坐标系到载体坐标系的姿态转换矩阵，巧：：为导航坐标系相对地球惯性 坐标系的角速率在载体坐标系的投影，城为导航坐标系相对地球惯性坐标系的角速率在 导航坐标系的投影； 设6也、巧I、0兔为巧螺直接测量到的角速率值在载体坐标系X、y、z轴的分量，则存 在W下关系：
其中心Vw、Vu分别为载体东北天方向的速度；L、A、h分别为载体的缔度、经度、高度； 而、咕分别为地球子午面和卯酉面主曲率半径，《 为地球自转角速度； 四元数微分方程的求解采用四阶龙格-库培法，其中T为解算周期，t+T时刻的四元数 q(t+T)如下： q(l + T)二 qU) + -少'' + 2K, + 2K'、+ K*) 白 其中：
然后，对四元数进行周期性的规范化处理，公式如下所示：
根据姿态矩阵和Q之间的关系，得到载体坐标系到导航坐标系的姿态转换矩阵;
导航所需的姿态角信息即俯仰角0、横滚角Y、航线角V从姿态矩阵得到，如下式所 示：
其中C。为的第i行第j列元素，i，j = 1，2,3,最终通过姿态角象限判断准则，得到 当前时刻更新后的姿态角； (3. 2)载体速度计算，其方法如下： 由比力方程：
式中："是在导航坐标系下载体速度矢量的变化率，fn为加速度计的测量值fb转换到 导航坐标系下的加速度值，2诚X r*为哥氏加速度，加速度城X F"是载体保持在地球表面 运动引起的对地向也加速度；gD为重力加速度； 上式表示为矩阵形式得：
式中媒、喊.、破分别为地球角速率在导航坐标系X、y、z轴的分量，fE、fw、fu是 加速度计测量到的比力通过坐标转换后得到的导航系下东、北、天H个方向上的比力分量， 即：
其中，把、记、於分别为加速度计的测量值在载体系x、y、z轴的分量； 将比力方程离散化得速度递推求解方程：
得到载体在导航坐标系下的H维速度信息； (3. 3)载体位置计算，其方法如下： 丄 T _ 、N j K sec f. h - V 由式L-而，文二而 ， 得：
其中，T为解算周期，在计算得到最新的缔度L、经度A数值之后，根据缔度更新KT时 刻的子午圈曲率半径Rm化)、卯酉圈的曲率半径咕化）。
【文档编号】G01C21/20GK104422948SQ201310413181
【公开日】2015年3月18日 申请日期:2013年9月11日 优先权日:2013年9月11日
【发明者】雷浩然, 陈帅, 车欢, 常耀伟, 王磊杰, 钟润伍, 金磊, 丁翠玲, 刘亚玲 申请人:南京理工大学