基于MIMU/GPS组合导航的箭载姿态测量系统

王林林 ， 刘 波

（1.中国科学院空间科学与应用研究中心 北京 100190；2.中国科学院大学 北京 100190）

随着火箭探空事业的发展，探空火箭所搭载的科学试验有效载荷种类越来越多，并且每次任务有效载荷的类型变化大、任务需求灵活多样，这对探空火箭箭载综合服务系统提出了越来越高的要求[1]。传统的基于分系统设计的方式已经越来越不能满足探空火箭发展的需要，无法充分发挥探空火箭任务响应快速、机动灵活、适应性强的特点，因此国际上相关机构都在开展新型探空火箭箭载综合服务系统的研制工作[2]。为了促进我国探空火箭型谱化、系列化的发展，作者在“国家863计划”的资助下开展探空火箭箭载综合服务系统模块化、小型化、通用化的研究。箭载姿态测量系统作为箭载综合服务系统的重要组成部分，为了能够满足小型化要求，并作为一个模块集成到箭载综合服务系统里，引入了微机电系统（MEMS）惯性仪表作为微型惯性测量单元（MIMU）。

MEMS是伴随着微机械制造技术、纳米技术和集成光学芯片技术等技术的发展而迅速发展起来的一个领域，是指集微型结构、微型传感器、微型执行器以及信号处理电路、接口、通讯和电源于一体的微型器件或系统[3]。MEMS器件具有集成度高、功能强、重量轻、功耗小、热常数低、可抗振动、抗冲击和耐辐射的优势。因此，在汽车工业、生物医学工程、航天航空、精密仪器、移动通信、国防科技等方面表现出良好的发展前景，目前国内外均积极开展MEMS器件在航天领域的应用研究[4-5]。MIMU是作为MEMS的一个分支发展起来的，它由微型陀螺仪、微型加速度计、专用集成电路（ASIC）等组成，能够完成和惯性测量单元（IMU）同样的功能。它具有隐蔽性好、数据更新率高、短期精度和稳定性好、体积小、重量轻、功耗低等特点，因此在军事和民用导航领域均得到越来越高的重视[6]。本文采用MIMU，并基于MIMU/GPS组合导航技术设计了箭载姿态测量系统。

1 总体方案设计

惯性导航系统以牛顿力学定律为基础，不需要来自外部的任何信息，也不会向外部辐射任何信息，可以在任何环境条件下实现自主导航，在给定载体的初始位置和初始速度的情况下，能够实时输出载体的位置、速度、方位、姿态和航向等信息，具有完全自主、不易受干扰、输出数据平稳、实时性强、短期稳定性好等优点，从而在航空、航天、航海等军用和许多民用领域得到广泛的应用。但是由于其工作原理是基于积分计算，所以它又具有导航误差随时间累积，导航精度随时间发散，无法保证长时间的高精度稳定工作的缺点。全球卫星定位系统（GPS）能够在全球范围内全天候的向用户提供高精度、连续、实时的测速、定位和授时等服务，具有观测时间短、操作简单、精度高、误差不随时间积累等优点。但是GPS系统也具有如下的缺点：自主性能差，容易受干扰，数据更新率低、难以满足实时导航的要求，且天线信号容易受障碍物遮挡造成信号失锁。惯性导航系统和GPS组合导航系统可以发挥各自的优点，克服对方的缺点，有效减小系统误差，提高系统精度，实现在高动态和强电子干扰的环境下的实时、高精度导航和定位，因此，惯性导航系统和GPS组合导航在航空、航天、航海导航和武器精确制导等方面得到越来越广泛的应用[7]。

MIMU/GPS组合导航系统以惯性导航系统作为组合导航系统的关键子系统，而GPS作为辅助子系统。通过组合，使用GPS输出的位置、速度等信息来修正陀螺漂移、加速度计偏差和初始失准角等引起的MIMU位置、速度和姿态的误差，从而获得高精度的导航信息。如图1所示为MIMU/GPS组合导航总体方案示意图，MIMU和GPS之间采用松散耦合方式进行组合，两个子系统独立工作，各自输出导航参数。这种组合方式采用位置和速度组合，将MIMU的误差方程作为组合导航系统的状态方程，将GPS和MIMU各自输出的位置、速度之差作为观测量，采用卡尔曼滤波器对MIMU的位置误差、速度误差、姿态误差及惯性仪表误差进行最优估计，然后对MIMU进行输出反馈校正。

图1 MIMU/GPS组合导航总体方案示意图Fig.1 Schematic diagram of MIMU/GPS combined navigation general scheme

这种组合方式的优点是可估计出组合导航系统的速度误差和位置误差，并可适当抑制姿态发散，通过补偿能够大幅度提高系统的定位精度，使MIMU具有动基座和空中对准能力。此外，由于此方式原理简单，容易工程实现，子系统可独立工作，并具有一定的系统冗余度，所以在实际组合导航系统中采用较多。

2 系统硬件设计

箭载姿态测量系统的硬件电路由MEMS惯性测量电路，GPS信号接收电路和导航计算机构成。其中，MEMS惯性测量电路，使用的是InvenSense公司生产的MPU6050，包括三轴陀螺仪和三轴加速度计，通过I2C接口输出3轴旋转角速率和3轴加速度；导航计算机采用的是恩智浦公司生产的ARM9系列芯片LPC3250，此芯片自带矢量浮点协处理器（VFP），在计算能力上可以媲美DSP；GPS信号接收电路采用TI公司生产的3V增强型CMOS四路差分线路接收器DS26LV32AT来接收箭载GPS系统以RS422格式发来的GPS信号。如图2所示，给出了系统硬件电路的结构框图。

图2 系统硬件电路结构框图Fig.2 Structure diagram of the system's hardware circuit

MPU6050是采用MEMS技术制造的姿态测量单元，是世界上首款并且是唯一一款6轴运动跟踪器件，具有低功耗、低成本和高性能等特点。内部包括3轴陀螺仪、3轴加速度计和一个数字运动处理器，6个16 bit的ADC用于同时采集陀螺仪和加速度计的信号，采用尺寸为4×4×0.9 mm的24脚QFN封装，通过I2C总线和导航计算机进行通信，最大通信速率400 kbps，具有1024字节的FIFO用于缓存数据。具有如下一些特性：

*数字化输出3个轴的角速率，并且角速率范围是用户可编程的，可选的范围包括±250、±500、±1 000 和±2 000°/sec；

*数字化输出3个轴的加速度，用户可编程范围为±2g，±4g，±8g 和±16g；

*宽范围数据采样率，根据用户要求采样率可以在4~8 000 Hz（陀螺）或 4~1 000 Hz（加速度计）之间；

*低功耗，最高功耗为12.87 mW（3.3 V×2.9 mA）；

LPC3250是恩智浦公司生产的基于ARM926EJ-S CPU的ARM产品，非常适合用于既要求高性能又要求低功耗的嵌入式领域，主要用来完成GPS信号的接收、MIMU加速度和角速率采集、姿态解算、卡尔曼滤波等。此款ARM具有如下主要特点：

*CPU运行时钟频率可高达266 MHz；

*具有32 kbyte指令高速缓存和32 kbyte数据高速缓存；

*支持ARMv5TEJ指令集、Jazelle指令集、DSP扩展指令；

*带有矢量浮点协处理器（VFP），这是一个数据运算功能非常强大的处理器，该处理器完全支持单精度和双精度的加、减、乘、除和乘加操作。它遵循IEEE754标准，可用于二进制浮点运算；采用流水线设计，包含有MAC操作、除法或平方根操作、Load/Store操作3种独立的流水线，这些流水线可以平行工作，也可以乱序工作；此外还提供浮点数和整数之间的转换功能；

*高达256 kbyte的内部SRAM，可用于存储代码或/和数据，可作为8位、16位或32位访问；

*带有 1个 NAND Flash接口、1个支持 SDR/DDR SDRAM和静态设备的外部接口；

*采用TFBGA296封装，封装大小只有15 mm×15 mm×0.7 mm。

3 系统软件设计

为了满足系统的实时性和可靠性要求，在LPC3250上移植了嵌入式实时多任务操作系统uC/OS-II，此系统在诸多领域内得到广泛应用，包括手机、飞行器、仪器仪表、医疗设备和工业控制等。本设计所移植的V2.52版本的操作系统在2000年得到了美国航空航天管理局（FAA）对用于商用飞机的、符合RTCA DO-178B标准的认证，表明uC/OS-II具有足够的安全性和稳定性，可以用于航空航天设备。

图3 软件设计的流程图Fig.3 Flow chart of the software design

如图3所示为系统软件设计的基本流程图。根据系统所要实现的功能把系统软件分成了5个任务：初始化任务、监控任务、初始对准任务、纯惯性导航任务和组合导航任务，分别用来完成如下功能：

1）初始化任务主要是用来完成操作系统初始化、LPC3250初始化和MPU6050初始化；

2）监控任务主要用来完成MPU6050的上电自检并给出自检结果，GPS状态的判定，读入GPS数据，系统导航的初始参数装订，给初始对准任务提供初始参数，系统内部的通信及时序管理，以及和系统外的通信；

3）初始对准任务是在导航系统工作前，根据监控任务所提供的初始姿态信息和位置信息，进行解析式粗对准和和采用卡尔曼滤波的精对准2个过程，计算出系统的初始捷联矩阵，然后导航系统以此捷联矩阵为基础开始工作；

4）纯惯性导航任务是在GPS信号无效的情况下，根据所得到的初始捷联矩阵，利用陀螺所提供的角速率和加速度计所提供的加速度，采用四阶龙格-库塔法解四元数微分方程进行姿态解算，实时解算出探空火箭的位置、姿态角和飞行速度；

5）在GPS信号有效的情况下启动组合导航任务，利用陀螺仪所提供的角速率和加速度计所提供的加速度解算出火箭的位置、姿态角和飞行速度，然后利用GPS所提供的火箭位置和速度信息作为外部参考信息，进行卡尔曼滤波，估计出位置误差、速度误差、姿态误差及惯性仪表误差，然后根据卡尔曼滤波的结果对即时捷联矩阵和位置矩阵进行修正，从而达到对探空火箭的位置、姿态和速度数据的修正。此外还利用GPS输出的火箭的速度、位置的垂直分量对惯导高度通道的速度、位置两个环节进行阻尼，从而得到最优高度解。具体的组合导航算法过程如图4所示。

图4 组合导航算法流程图Fig.4 Flow chart of the combined navigation algorithm

进入组合导航任务后，首先要确定的是初始值，根据初始条件不同初始值也不同，若为刚开始执行导航任务，则初始值为初始对准阶段所获得初始捷联矩阵和火箭初始位置，否则初始值为上次修正后的捷联矩阵T、位置矩阵C等。然后以100 Hz的采样频率采集MEMS加速度计的3轴加速度和MEMS陀螺仪的3轴角速率，并和上次卡尔曼滤波的结果一起对捷联矩阵T进行即时修正。由于加速度计输出的加速度是在载体坐标系内，所以需要在得到捷联矩阵T之后转换到导航坐标系内。在得到导航坐标系内的加速度后，就可以通过解地速的微分方程得到地速的即时修正值。接下来根据上次卡尔曼滤波的结果和上次解算得到的位置矩阵C，对位置矩阵C进行即时修正，并据此计算出曲率半径RM、RN及地球速率。通过对GPS给出的火箭垂直方向上的位置和速度进行阻尼，并结合修正之后的位置矩阵C，可得到火箭的高度h，并据此计算出加速度g的值。最后根据修正的捷联矩阵T计算火箭的姿态角和速度，根据修正的位置矩阵C计算火箭位置，并把火箭的位置和速度送入卡尔曼滤波器进行卡尔曼滤波，并把卡尔曼滤波的结果作为下次捷联矩阵T和位置矩阵C进行即时修正的输入。

4 系统测试结果

完成系统的软硬件设计之后，专门设计了上位机软件通过串口接收组合导航系统发过来的加速度、角速率、姿态角、位置和速度数据，并实时在上位机上显示，由于篇幅原因上位机软件不作过多介绍。对组合导航系统进行半物理仿真和系统测试，测试结果如下：1）初始对准时间小于5分钟；2）组合导航姿态角误差≤1°；3） 滚动角速度测量范围±2 000°/s；4）加速度测量范围±16g；5）数据输出频率20 Hz，串口波特率19 200 bps；6） 工作温度－40～+85 ℃；7） 动态功耗 980 mW（196 mA@+5 V）；8）总重量 230 g。

为了考察本系统在探空火箭飞行环境下的可靠性，对此系统进行了包括考察力学和温度特性的环模试验。试验具体内容包括：首先对整个系统进行了-10℃到+60℃的高低温循环试验，共做了6个循环；然后沿X、Y、Z 3个轴向进行了总均方根加速度值为16.9 grms的随机振动试验和总冲击加速度值为35 g的冲击试验。在试验过程中本系统均正常工作，表明此系统能够经受住探空火箭苛刻的飞行环境考验，能够应用于探空火箭任务。

5 结 论

本文针对探空火箭的应用需求，采用MEMS陀螺仪和MEMS加速度计，设计了基于MIMU/GPS组合导航的箭载姿态测量系统，并给出了组合导航系统的硬件设计和软件设计。组合导航算法采用卡尔曼滤波进行误差控制，采用四元数法进行捷联矩阵和位置矩阵的即时修正，采用四阶龙格-库塔法求解微分方程。首先对所设计的系统进行了半物理仿真和系统测试考察系统的性能，然后对系统进行了环境模拟试验考察系统的可靠性。测试和试验结果表明本系统具有初始对准速度快、动态范围广、系统误差小、功耗低、重量轻、安装方便、可靠性高、抗振动和抗冲击等特点，能够满足探空火箭在性能指标上的要求，适合应用于探空火箭任务，计划在863探空火箭项目中进行搭载试验。

[1]姜秀杰，刘波，于世强，等.探空火箭的发展现状及趋势[J].科技导报，2009，27（23）:101-110.

JIANG Xiu-jie，LIU Bo，YU Shi-qiang，et al.Development status and trend of sounding rocket[J].Science and Technology Review，2009，27（23）:101-110.

[2]Horst Pfeuffer，Josef Ettl，Frank Haenpflug. NEW TECHNIQUES AND INSTRUMENTATION TEXUS SERVICE MODULE （TSM）[C]//Proceedings of the 19th ESA Symposium on European Rocket and Balloon Programmesand Related Research， Bad Reichenhall，Germany， 7-11 June 2009.ESA Publications，2009，671:315-320.

[3]王亚珍，朱文坚.微机电系统 （MEMS）技术及发展趋势[J].机械设计与研究，2004，20（1）:10-12.

WANG Ya-zhen，ZHU Wen-jian.MEMS technology and developing trend[J].Machine Design and Research，2004，20（1）:10-12.

[4]Shea H R.MEMS for pico-to micro-satellites[C]//Society of Photo-Optical Instrumentation Engineers（SPIE）Conference Series，2009，7208:21-28.

[5]吴向东.MEMS器件在航天领域的应用及发展[J].微纳电子技术，2012，49（8）:542-547.

WU Xiang-dong.Application and prospectsofMEMS devices for space[J].Micronanoelectronic Technology，2012，49（8）:542-547.

[6]李荣冰，刘建业，曾庆化，等.基于 MEMS技术的微型惯性导航系统的发展现状[J].中国惯性技术学报，2004，12（6）:88-94.

LI Rong-bing，LIU Jian-ye，ZENG Qing-hua，et al.Evolution of MEMS based micro inertial navigation systems[J].Journal of Chinese Inertial Technology，2004，12（6）:88-94.

[7]李鹏奎，钱山，蔡洪.MIMU/GPS组合导航技术研究[J].系统仿真学报，2009（15）:4797-4800.

LI Peng-kui，QIAN Shan，CAI Hong.Research on Integrated MIMU/GPS System[J].Journal of System Simulation，2009（15）:4797-4800.