基于SOPC的MEMS组合导航仪设计与实现

梁海波 司文杰 刘志华 肖昌怡

1.北京航天自动控制研究所，北京 1008542.中国西南电子设备研究所，成都 610036

基于SOPC的MEMS组合导航仪设计与实现

梁海波1司文杰1刘志华1肖昌怡2

1.北京航天自动控制研究所，北京 100854
2.中国西南电子设备研究所，成都 610036

为了提高以微机电系统(Micro Electro Mechanical System，MEMS)惯性器件为核心的惯性导航系统精度，提出了由MEMS惯性测量单元、GPS接收机、倾斜仪和电子罗盘构成组合导航系统的方案。该系统以FPGA作为中央处理器，采用SOPC(System on Programmable Chip)技术构建导航计算机，并开发多传感器数据融合算法，完成组合导航功能。通过静态导航试验，表明该系统能够实现在1000s时间内，位置误差均值小于2.1m，标准差不大于3.1m。

组合导航;数据融合;微机电系统;片上可编程系统

目前，市场上基于微机电系统(Micro Electro Mechanical System，MEMS)惯性器件的惯性导航产品日益增多，但受制造工艺所限，MEMS惯性器件的精度相比传统惯性器件还比较低，加之惯性导航误差随时间累积，使得MEMS惯性导航系统还不具备独立的长时间导航定位能力［1-2］。在实际应用中，为减小导航误差，提高系统精度，引入辅助传感器和组合导航技术是行之有效的方案。近年来，基于MEMS惯性器件的多传感器组合导航方案和算法，国内外学者开展了较深入的研究。例如，文献［3］对基于MEMS惯性测量单元与GPS接收机组合导航技术的发展状况进行了总结与分析;文献［4］给出了一种采用MEMS惯性器件和GPS接收机构建组合导航系统的方案;文献［5］将视觉信息引入到组合导航系统中，进一步丰富了导航辅助信息的种类，导航算法的设计也随之复杂化。这里，提出一种新的以SOPC(System on Programmable Chip)技术为载体，采用MEMS惯性器件、GPS接收机、倾斜仪和电子罗盘构建多传感器组合导航系统的方案，并研制了原理样机，设计了相应的多传感器数据融合算法(即组合导航算法)，以达到改善导航精度的目的。

1 硬件设计

如图1所示，组合导航仪由电源、液晶显示、传感器和导航计算机4大功能模块构成，各功能模块的具体功能如下:

1)电源模块将外界输入的24V直流电源转换为5V二次电源，供导航仪内部各子功能模块使用;

2)液晶显示模块作为人机交互的接口，为用户实时显示导航结果;

图1 系统结构框图

3)传感器模块包括MEMS惯性测量单元、GPS接收机、倾斜仪和电子罗盘，各传感器的性能指标如表1所示。MEMS惯性测量单元结合捷联惯性导航算法可以实现短时间、高动态条件下的姿态、速度和位置解算，而GPS接收机可以实时提供载体的速度和位置信息，倾斜仪能实现低动态条件下的水平姿态测量，电子罗盘能提供地磁北向基准。通过对多传感器数据融合算法的设计，可取各传感器之所长，提供较长时间、较高精度的导航结果;

4)导航计算机作为组合导航仪的核心模块之一，负责各传感器数据的采集，并作为组合导航算法的载体，进行组合导航解算。

2 软件设计

2.1 基于SOPC的导航计算机定制

导航计算机由处理器和外围功能电路组成，处理器选用Altera公司的FPGA芯片EP1C12Q240I7。该芯片内部资源丰富，具有12060个逻辑单元、239616位RAM、2个锁相环，用户可用的最大I/O数为249个，足以满足组合导航仪的硬件需求。

SOPC(System On Programmable Chip)技术是一种特殊的嵌入式处理器技术，它将CPU和逻辑电路以IP核的形式集成到FPGA芯片上，仅由单个芯片即可完成整个系统的主要逻辑功能，具有设计方式灵活、功能可裁减、可扩充、可升级的优点，并具备软硬件“在系统可编程”的功能［6-7］。

这里，根据组合导航仪的功能需求，对SOPC进行了定制，具体包括如下IP核:CPU，定时器，PIO，SPI，UART，EPCS控制器，SDRAM 控制器等。在Quartus 8.0集成开发环境下，通过SOPC Builder工具对各IP核进行如下配置:

1)CPU选用快速型Nios II带硬件乘法器内核，以保证高速的导航解算性能;

2)定制2个定时器，其中1个用于传感器的定时采样，另1个供μC/OS-II操作系统使用;

3)定制SPI内核1个，用于采集MEMS惯性测量单元的数据。这里，为了与MEMS惯性测量单元的通讯协议保持一致，将SPI通信频率设为2MHz，奇偶极性和相位极性都设置为1;

4)定制 UART内核6个，分别用于外部RS422，GPS接收机、倾斜仪、电子罗盘、显示模块，以及底层监控程序的通信，并对各自串行通信所需的波特率、奇偶校验等参数进行设置;

5)定制EPCS控制器1个，该控制器一方面能够保证组合导航仪软件和试验数据能保存到EPCS存储器中，另一方面在系统上电后，能保证处理器对EPCS存储器进行主动串行配置;

6)定制SDRAM控制器1个，作为Nios II内核与外部SDRAM芯片的通信媒介。

2.2 软件设计步骤

组合导航仪对软件的实时性和可靠性要求较高，而嵌入式操作系统μC/OS-II是一个完整的、可移植、固化、剪裁的占先式实时多任务系统［9］，恰能满足组合导航仪的设计需求。因此，选用μC/OS-II作为组合导航仪的软件运行平台。在μC/OS-II环境下，按照如下步骤进行软件设计:

1)根据组合导航仪的功能需求，将系统任务划分为:惯性测量单元数据采集任务(SPI接口)、GPS接收机数据采集任务(UART接口)、倾斜仪数据采集任务(UART接口)、电子罗盘数据采集任务(UART接口)、组合导航解算任务、底层监控任务(UART接口)、向液晶显示模块发送数据任务和系统运行状态指示任务等子任务;

2)为各任务定义任务堆栈。μC/OS-II中有1个专用的OS_STK数据类型用于定义任务堆栈，只需按照μC/OS-II的使用说明，创建长度为TASK_STK_SIZE(根据需要定义大小)、类型为OS_STK的数组，即完成了相应任务堆栈空间的创建;

3)定义任务优先级。注意每个任务必须有唯一的优先级，且不能高于指定的最大优先级;

4)定义信号量、消息队列、消息邮箱和全局变量，编写相关软件函数。除系统函数外，还需编写本节第1)条中定义的各子任务函数;

5)在主函数中创建初始任务。在初始任务中创建消息队列和邮箱，以及本节第1)条中定义的各系统子任务;

6)启动操作系统，按照操作系统的调度机制来执行各任务。

2.3 组合导航算法设计

在设计组合导航算法时，将组合导航仪内部的传感器分为2类:①惯性测量单元，用于进行捷联惯性导航解算，得到载体实时的姿态、速度和位置信息;②GPS接收机、倾斜仪和电子罗盘，作为惯性导航解算的辅助数据。

如图2所示，组合导航算法的基本思路是:在捷联惯性导航系统误差方程的基础上，结合辅助数据，建立组合导航系统的状态方程和量测方程;然后采用Kalman滤波对惯性导航解算的误差进行估计;最后利用估计值对惯性导航解算结果进行校正，从而得到稳定的、较高精度的姿态、速度和位置信息，实现组合导航功能［10］。

图2 组合导航算法示意图

(1)系统状态方程

惯导误差方程反映了惯性导航系统的误差传播特性，是组合导航理论的基础之一。这里，以东北天地理坐标系作为导航坐标系，建立如下位移(以开机时刻所在位置作为位置零点)、速度和姿态误差方程［11-12］:

式中，δr·n，δVn，δφ·n分别为导航系下的位移误差、速度误差和姿态误差;fn为加速度计在导航系下测量的比力为由载体系到导航系的坐标转换矩阵(即捷联矩阵);▽b和εb分别为载体系下的加速度计零偏和陀螺常值漂移，这里认为二者是常值［13］。=［0 ΩcosLΩsinL］T(Ω和L分别为地球自转角速度和纬度)(RM和RN分别为地球子午曲率半径和卯酉曲率半径，h为当前位置的高度);=

将系统噪声考虑在内，得到组合导航系统状态方程为

(2)系统量测方程

系统量测方程为

以GPS接收机提供的位置和速度、以倾斜仪提供的水平姿态、以电子罗盘提供的航向作为辅助数据，将捷联惯性解算的位置、速度和姿态与上述数据相减，得到两类数据的差值。这里，取东向和北向位移差值、东向和北向速度差值、3个姿态差值作为组合导航系统的量测值，即

通过对各辅助传感器的数据进行统计学分析，可以得到组合导航系统量测噪声的统计特性。至此，便建立了组合导航系统的状态方程和量测方程，可以利用Kalman滤波进行组合导航解算。需要指出的是，前面所建立的组合导航系统方程针对的是时间连续系统，但在实际操作中需将其离散化，然后在导航计算机中按照离散系统方程进行计算。时间连续系统离散化的实质是根据连续系统的系统矩阵Ft计算出离散系统的转移矩阵，以及根据连续系统的噪声方差阵Qt计算出离散系统噪声方差阵，限于文章篇幅，具体的离散化方法可以参见文献［14］，这里不再赘述。

3 试验与结果分析

静态组合导航试验具有操作简便、现象直观的特点，可以用于验证组合导航算法的有效性。将组合导航仪装配于三轴速率转台上，使导航仪坐标系与转台坐标系重合，将GPS接收机的天线置于户外，控制转台给定某一恒定姿态。开启组合导航仪，待GPS信息有效后，将转台的姿态、GPS接收机提供的位置作为组合导航仪的初始姿态和初始位置，并将初始速度置0，开始组合导航解算。

试验过程中，对组合导航仪输出的姿态、速度和位置等导航数据进行记录，并存储在组合导航仪内部的EPCS存储器中。试验共进行了3次，每次持续时间1000s。通过与试验真值进行对比，得到了组合导航误差。通过分析，3次试验结果的重复性较好，图3给出了第2次试验的误差曲线。

由图3可以看出，组合导航算法克服了纯惯性导航解算误差随时间积累的缺点，姿态、速度和位置误差不再发散，而是在某一范围内波动并且有收敛趋势，导航定位精度有了明显改善。由于惯性导航误差按照姿态、速度、位置的顺序传播，位置误差的大小可在一定程度上反映导航仪性能的优劣，因此对位置误差进行了如表2所示的统计分析。从表中数据可以看出，在1000s时间内，位置误差均值被控制在2.1m以内，且误差标准差不大于3.1m。

表2 组合导航位置误差

图3 静态试验结果

4 结论与展望

实践证明，组合导航技术是提高MEMS惯性导航精度的有效手段，SOPC系统开发的灵活性特点在组合导航仪的设计中得到了充分的体现。在后续的研究中，将根据导航仪的实际应用需求，从GPS接收机信号有/无、导航仪高/低动态等方面对组合导航算法进行更深入的研究。

通过进一步改进，该组合导航仪将有望在摄像稳定平台、船舶/车辆的稳定控制和制导弹药等领域发挥重要的作用。

［1］张英，戚红向，李德强，等.MEMS航天惯导产品及技术发展简介［J］.航天标准化，2010，(3):40-43.(Zhang Ying，Qi Hongxiang，Li Deqiang，et al.Development of MEMS Inertial Navigation System in Aerospace Field ［J］.Aerospace Standardization，2010，(3):40-43.)

［2］王永海，刘佳琪，张力，杜润乐.基于小型一体化MEMS惯性组合单元的应用研究［J］.航天标准化，2010，(3):36-40.(Wang Yonghai，Liu Jiaqi，Zhang Li，Du Runle.Application of Small Integrated MEMS-based Inertial Unit［J］.Aerospace Standardization，2010，(3):36-40.)

［3］康岳林，高杨，高立.基于MEMS的GPS/SINS组合导航技术发展现状［J］.船电技术，2011，31(5):26-29.(Kang Yuelin，Gao Yang，Gao Li.Trend of MEMS-based GPS/SINS Integrated Navigation System Based on MEMS［J］.Marine Electric ＆ Electronic Engineering，2011，31(5):26-29.)

［4］El-Sheimy N，Niu X.An Expert Knowledge GPS/INS System for Mobile Mapping and GIS Applications［C］∥Proceedings of the 2000 National Technical Meeting of the Satellite Division of the Institute of Navigation，2005，1:26-28.

［5］邵玮，祝小平，赵刚.基于视觉/GPS/MEMS-INS数据融合的MUAV导航方案［J］.系统仿真技术，2011，7(1):77-81.(Shao Wei，Zhu Xiaoping，Zhao Gang.Integrated Navigation Scheme for MUAV Based on Vision/GPS/MEMS-INS ［J］.System Simulation Technology，2011，7(1):77-81.)

［6］Altera Corp.Nios ll Processor Reference Handbook［EB/OL］.2008.

［7］Altera Corp.Nios ll Software Developer's Handbook［EB/OL］.2008.

［8］周立功.SOPC嵌入式系统基础教程［M］.北京:北京航空航天大学出版社，2006.(Zhou Ligong.SOPC Embedded System Tutorial［M］.Beijing:Beijing University of Aeronautics and Astronautics Press，2006.

［9］任哲.嵌入式实时操作系统 μC/OS-II原理及应用(第二版)［M］.北京:北京航空航天大学出版社，2009.(Ren Zhe.Embedded Real-time Operating System μC/OS-II Principles and Applications［M］.2nd ed.Beijing:Beijing University of Aeronautics and Astronautics Press，2006.)

［10］Robert M.Rogers.Applied Mathematics in Integrated Navigation Systems［M］.3rd ed.American Institute of Aeronautics and Astronautics，2007.

［11］秦永元.惯性导航［M］.北京:科学出版社，2006:355-360.(Qin Yongyuan.Inertial Navigation［M］.Beijing:Science Press，2006:355-360.)

［12］王惠南.GPS导航原理与应用［M］.北京:科学出版社，2003:224-229.(Wang Huinan.Principles and Applications of GPS Navigation［M］.Beijing:Science Press，2003:224-229.)

［13］孙畅，富立，刘文丽.微陀螺零偏在线补偿方法及仿真［J］.弹箭与制导学报，2008，28(3):52-54.(Sun Chang，Fu Li，Liu Wenli.On-line Compensation for Zero Bias of MEMS Gyro［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2008，28(3):52-54.)

［14］张国良，曾静.组合导航原理与技术［M］.西安:西安交通大学出版社，2008:124-125.(Zhang Guoliang，Zeng Jing.Navigation Principles and Techniques［M］.Xi’an:Xi’an Jiaotong University Press，2008:124-125.)

Design and Realization of MEMS Integrated Navigator Based on SOPC

LIANG Haibo1SI Wenjie1LIU Zhihua1XIAO Changyi2
1.Beijing Aerospace Automatic Control Institute，Beijing 100854，China
2.Southwest China Research Institute of Electronic Equipment，Chengdu 610036，China

In order to enhance the accuracy of navigation system based onMEMS(Micro Electro Mechanical System)inertial devices，an integrated navigation system，including inertial measurement unit，magnetometer，inclinometer andGPS，is presented．In this system，theFPGAchip is adopted as the navigation computer，supported by theSOPC(System on Programmable Chip)technique．Furthermore，the software of multiple sensors fusion is developed．The static navigation test indicates that the integrated navigator can improve the precision of navigation system effectively．

Integrated navigation;Data fusion;MEMS;SOPC

V241.6

A

1006-3242(2014)02-0003-06

2012-10-18

梁海波(1984-)，男，天津人，博士，工程师，主要研究方向为MEMS惯性导航与组合导航技术;司文杰(1982-)，男，浙江人，博士，工程师，主要研究方向为控制系统电气设计技术;刘志华(1978-)，女，湖南人，硕士，高级工程师，主要研究方向为控制系统电气设计技术;肖昌怡(1986-)，男，湖南人，硕士，工程师，主要研究方向为组合导航技术。