第２３卷第１１期　２０１０年１１月　传感技术学报　

ＣＨＩＮＥＳＥ　ＪＯＵＲＮＡＬ　ＯＦ　ＳＥＮＳＯＲＳ　ＡＮＤ　ＡＣＴＵＡＴＯＲＳ　Ｖｏ１．２３　Ｎｏ．１１　ＮＯＶ．２０１０　

Ｄｅｓｉｇｎ　０ｆ　ＡＨＲＳ　Ｂａｓｅｄ　ｏｎ　Ｌｏｗ．Ｃｏｓｔ　ＭＥＭＳ　

ＤＵ　Ｊｉｙｏｎｇ，ＨＵＡＮＧ　Ｇｕｏｒｏｎｇ　，Ｚｈａｎｇ　Ｆｅｎｇｍｉｎｇ，ＬＩＵ　Ｈｕａｗｅｉ　

（Ｅｎｇｉｎｅｅｒｉｎｇ　Ｃｏｌｌｅｇｅ，Ａｉｒ　Ｆｏｒｃｅ　Ｅｎｇｉｎｅｅｒｉｎｇ　Ｕｎｉｖｅｒｓｉｔｙ，Ｘｉ’ａｎ　７１００３８，Ｃｈｉｎａ）　

Ａｂｓｔｒａｃｔ：Ｔｏ　ｓａｔｉｓｆｙ　ｔｈｅ　ｅａｇｅｒ　ｒｅｑｕｉｒｅｍｅｎｔｓ　ｏｆ　ｌｏｗ－ｃｏｓｔ　ａｎｄ　ｍｉｎｉａｔｕｒｉｚａｔｉｏｎ　ａｔｔｉｔｕｄｅ　ｈｅａｄｉｎｇ　ｒｅｆｅｒｅｎｃｅ　ｓｙｓｔｅｍ　（ＡＨＲＳ）ｉｎ　ｔｈｅ　ｍｉｌｉｔａｒｙ　ａｎｄ　ｔｅｃｈｎｉｃａｌ　ｆｉｅｌｄｓ，ａ　ｓｅｌｆ－ｄｅｖｅｌｏｐｅｄ　ｓｐｅｃｉａｌ　ＭＥＭＳ—ｂａｓｅｄ　ｉｓ　ｉｎｔｒｏｄｕｃｅｄ，ｔｈｅ　ｋｅｙ　ｔｅｃｈｎｏｌ—　ｏｇｉｅｓ　ｏｆ　ｗｈｉｃｈ　ａｒｅ　ｐｒｏｐｏｓｅｄ．Ｍａｇｎｅｔｉｃ　ｓｅｎｓｏｒ　ａｉｄｅｄ　ａｔｔｉｔｕｄｅ　ｅｒｒｏｒ　ｃｏｒｒｅｃｔｉｏｎ　ｉｓ　ｐｒｏｐｏｓｅｄ，ｒｏｔａｔｉｏｎ　ｖｅｃｔｏｒ　ａｌｇｏｒｉｔｈｍ　ｉｓ　

ａｄｏｐｔｅｄ，ａｎｄ　ｌｏｗ—ｏｒｄｅｒ　Ｋａｌｍａｎ　ｆｉｌｔｅｒ　ｉｓ　ｄｅｓｉｇｎｅｄ，ａｌｌ　ｏｆ　ａｂｏｖｅ　ａｒｅ　ｂａｓｅｄ　ｏｎ　ｔｈｅ　ＭＩＭＵ　ｐｅｒｆｏｒｍａｎｃｅ　ａｎｄ　ｔｈｅ　ｃｏｒｅ　ｐｒｏｃｅｓｓｏｒ　ｃｈａｒａｃｔｅｒｉｓｔｉｃ．Ａｎ　ｅｍｂｅｄｄｅｄ　ＲＴＯＳ—ｂａｓｅｄ　ｓｏｆｔｗａｒｅ　ｓｙｓｔｅｍ　ｉｓ　ｔｒａｎｓｐｌａｎｔｅｄ．Ｅｘｐｅｒｉｍｅｎｔａｌ　ｒｅｓｕｌｔｓ　ｓｈｏｗ　ｔｈａｔ　ｔｈｅ　ＡＨＲＳ　ｗｏｒｋｓ　ｉｎ　ｒｅａｌ　ｔｉｍｅ　ａｎｄ　ｓｔｅａｄｉｌｙ，ｗｉｔｈ　ｈｉｇｈｅｒ　ｐｒｅｃｉｓｉｏｎ，ｗｈｉｃｈ　ｈａｓ　ｖａｓｔ　ｏｆ　ａｐｐｌｉｅｄ　ｆｏｒｅｇｒｏｕｎｄ．　Ｋｅｙ　ｗｏｒｄｓ：ａｔｔｉｔｕｄｅ　ｈｅａｄｉｎｇ　ｒｅｆｅｒｅｎｃｅ　ｓｙｓｔｅｍ；ｌｏｗ　ｃｏｓｔ；ｒｏｔａｔｉｏｎ　ｖｅｃｔｏｒ　ａｌｇｏｒｉｔｈｍ；ｌｏｗ—ｏｒｄｅｒ　Ｋａｌｍａｎ　ｆｉｌｔｅｒ；ｔｘＣ／ＯＳ一１Ｉ　

ＥＥＡＣＣ：７６３０　ｄｏｉ：１０．３９６９／ｊ．ｉｓｓｎ．１００４—１６９９．２０１０．１１．０２９　

基于低成本ＭＥＭＳ器件的捷联航姿系统设计术　

杜继永，黄国荣　，张凤鸣，刘华伟　

（空军工程大学工程学院，西安７１００３８）　

摘　要：针对军事和工程领域对低成本、微小型航姿系统的迫切需求，介绍了自行设计的基于低成本ＭＥＭＳ器件的航姿系　统，深入研究了构建该系统的关键技术。针对ＭＩＭＵ的性能特点及核心处理器的运算速度，给出了磁传感器的补偿算法，采　用了基于等效旋转矢量的姿态解算方法，并应用了降阶卡尔曼滤波器。通过移植￣Ｃ／ＯＳ一Ⅱ实时操作系统，增强了系统的实　时性。实验结果表明，所设计的航姿系统实时性好，精度可达要求，具有广阔的应用前景。　关键词：航姿系统；低成本；等效旋转矢量法；低阶卡尔曼滤波器；￣Ｃ／ＯＳ一Ⅱ　中图分类号：Ｕ６６６．１；Ｖ２４９　文献标识码：Ａ　文章编号：１００４—１６９９（２０１０）１１—１６６２—０５　

当前，应用微小型航姿系统的场合日益增多，尤　

其在微小型运载体的导航、制导与姿态控制等领域，　

倍受亲睐。受国外所销售ＭＥＭＳ惯性测量器件的　精度限制，在提高微机械惯性测量（Ｍｉｃｒｏ．Ｅｌｅｃｔｒｏｎｉｃ　Ｓｙｓｔｅｍ　Ｉｎｅｒｔｉａｌ　Ｍｅａｓｕｒｅｍｅｎｔ　Ｕｎｉｔ，ＭＥＭＳ—ＩＭＵ）方面　我国进行了一些研究ｕ　。　

本文立足市场现有低成本ＭＥＭＳ惯性测量器　

件（ＡＤ公司的ＡＤＩＳ１６４０５型ＭＩＭＵ），设计了以　ＡＲＭ系列微处理器作为导航计算机的微小型航姿　系统。针对其陀螺的性能特点，应用了低阶卡尔曼　

滤波算法以保证姿态测量精度；为满足系统实时性　要求，采用基于等效旋转矢量算法进行姿态解算，以　

降低导航计算机运算负担，并移植了ＩｘＣ／ＯＳ一Ⅱ操　作系统协调各导航任务。动、静态实验结果表明，该　

系统实时性好，姿态误差可控制在１。范围内。　

１系统硬件组成　

微型捷联航姿系统硬件主要包括：微惯性测量　

项目来源：国家自然科学基金项目资助（６０３０４００４）　收稿日期：２０１０—０５—０８　修改日期：２０１０—０６—２３　单元、ＧＰＳ～ＯＥＭ板、ＡＲＭ导航计算机、程序升级接　

口及参数显示模块。需要指出的是，采用ＧＰＳ—　ＯＥＭ板主要作为速度传感器使用，这样设计既降低　

了硬件开发的难度，又便于借助该航姿系统的硬件　平台开展组合导航系统的研究，节约了二次开发成　

本。系统硬件结构如图１所示。　

图１　系统硬件结构图　１．１微惯性测量组合　ＡＤＩＳ１６４０５惯性测量单元集成了三轴陀螺仪、　三轴加速度计、三轴磁力计和温度传感器，

具有抗干　第１１期　杜继永，黄国荣等：基于低成本ＭＥＭＳ器件的捷联航姿系统设计　１６６３　

扰能力强、可靠性高、体积小巧等优点。该惯性测量　单元通过ＳＰＩ接口进行系统集成。　

１．２核心处理器　处理器作为捷联航姿系统的核心，主要完成　ＭＩＭＵ数据的接收、航姿解算以及结果的输出等。　

本文选择基于ＡＲＭ７内核的ＬＰＣ２１１４芯片作为核　心处理器，主要基于以下方面考虑：首先，可以满足　

系统软件和应用程序运行时的资源要求，包括处理　器的速度、存储空间和运行空间等；其次，可以提供　

丰富的通讯接口，包括程序下载的接口、导航计算机　与外部传感器以及与上位机的通讯接口等。　１．３通信接口　

系统的接口标准及实现功能如下：　（１）与微机械惯性测量组件进行通讯，通讯协　

议为同步串行接口ＳＰＩ；　

（２）与磁传感器进行通讯，通讯协议为ＳＰＩ。系　统的磁传感器内嵌在ＩＭＵ中；　（３）与ＰＣ机进行通讯，实现航姿的实时显示及　解算结果的保存，通讯协议为ＲＳ２３２；　（４）与ＧＰＳ接收机进行通讯，通讯协议　

为ＲＳ２３２；　（５）与上位机进行通信，用于导航程序的改进　与升级，采用ＪＴＡＧ接口。　

２系统软件设计　

２．１　ｔ￣￣／ｏｓ—ＩＩ实时操作系统的移植　

为协调各任务问的同步与通信，增强系统的实　时性，移植了ＩｘＣ／ＯＳ—Ｉ１实时操作系统。ＬＰＣ２１１４　

具有ＡＲＭ和Ｔｈｕｍｂ两种指令集、７种处理器模式，　只有系统模式作为一种特权模式与异常无关。因　

此，ＩｘＣ／ＯＳ—ＩＩ的可用模式有两个，用户模式和系统　模式。ＩｘＣ／ＯＳ—ＩＩ任务的默认模式为用户模式，可　选模式为系统模式。为了实现任务工作模式的切　

换，在移植时还要添加两个用于任务切换的系统函　数：ＣｈａｎｇｅＴｏＳＹＳＭｏｄｅ（）和ＣｈａｎｇｅＴＯＵＳＲＭｏｄｅ（）。　为了使Ｉ．ｚＣ／ＯＳ—ＩＩ的底层接口函数不受访问权限的　

限制，可以将该底层函数放在软中断服务程序中。　具体的移植过程和代码可以参见文献［９］。　

利用￣Ｃ／ＯＳ—ＩＩ的多任务机制实现航姿解算。　应用程序中除操作系统本身外，建立了四个用户任　

务，分别是姿态解算、组合滤波、初始对准、结果显示　任务；两个中断服务，如图２所示。　

外部中断ＥＩＮＴ１负责ＭＥＭＳ传感器的采样，串　口ＵＡＲＴ１负责ＧＰＳ数据的接收。任务与任务和任　

务与中断服务函数之间的通信采用消息队列，共建　消息队列１；　；消息队列２；　ｉ消息队列３；　

图２　系统软件结构　立了四个队列分别负责外部中断１与初始对准和姿　

态解算的通信、姿态解算与组合滤波的通信、ＵＡＲＴ１　与组合滤波的通信以及姿态解算与监控计算机显示　

任务的通信。　２．２航姿显示软件　为直观监测载体姿态的变化，在Ｌａｂｗｉｎｄｏｗｓ／　ＣＶＩ环境下设计了基于上位机的航姿显示软件，其　

绘制工作主要由５个画布控件、２个ＮＵＭＲＩＣ控件　并借助画布绘图函数完成。各画布的位置及大小通　

过ＳｅｔＣｔｒｌＡｔｔｒｉｂｕｔｅ（）函数确定，各控件问的叠放层　

次根据显示效果进行调整。图３为航姿显示软件的　结构示意图。　

图３航姿显示软件设计　

３　系统算法　

３．１磁传感器误差补偿　磁传感器误差可分为制造误差、安装误差以及　

因周围磁场影响而产生的罗差　。在理想情况下，　当磁传感器在水平面内作圆形旋转且无磁干扰时，　和ｙ轴磁传感器的输出在坐标系中的映射应为　

圆，圆心在（０，０）处。而硬磁干扰会使该圆圆心偏　

移，引起零位误差；软磁干扰会使该圆畸变为椭圆，　

引起灵敏度误差。采用以下方法对磁传感器误差进　ｌ６６４　传感技术学报　第２３卷　

藩；一㈩　

：（　一　…）．Ｋｘ　

箍：Ｂ　㈩　Ｈ　＝Ｋ　・矗　＋　

图４　圆周测试曲线　

３．２基于等效旋转矢量算法的姿态四元数计算　

在捷联航姿系统中，飞行器的姿态和航向即　

为载体坐标系相对地理坐标系的方位关系。确定　两个坐标系之间的方位关系问题是力学中的刚体　

定点转动理论。通过计算等效旋转矢量补偿了因　

转动而引起的不可交换性误差，并采用更新四元　数的方式来更新姿态，达到了四阶龙格库塔法的　精度¨　，计算量却较之很低，降低了对导航计算机　

的运算性能的要求。　基于等效旋转矢量算法的姿态更新如下：　

ｇ　＋１）＝ｇ　ｏｑ　高速更新　（４）　

ｇ　ｎ（ｋ＋＋ｌ　＝ｑ　＋Ｊ］低速更新　（５）　

『。ｏｓ０・５　］　其中：ｑ　”＝ｌ　ｓｉｎ０．５　Ｉ，　＝　－ｎ　ＡＴ，为导航系在　ｌ一—　

时间△　内相对惯性系的等效旋转矢量，　为　的模；　ｌ　ｃｏｓＯ・５　ｍ　Ｉ　ｑｂ（　ｍ））＝　ｓｉｎ０．５　ｌ，　＝　－ｂ６　Ａｔ，为载体系在Ａｔ时　ｌＴ　Ｊ　

间内相对陨性系的等效转动矢量，　为　的模。　理论上讲，如果陀螺没有量化误差，而且系统采　

样周期趋于零，即姿态更新周期内采样次数尽可能　多，就可以减少或消除可交换性误差，但计算量也会　随之增大。在实际应用中要综合考虑算法实时性和　

计算负担，选择合适的子样算法。本文中，采用二子　样算法：　

，）　．　一　咖＝０１＋０２＋亍（０ｌ×０２）　（６）　

３．３低阶卡尔曼滤波器设计　

为增强系统的实时性，降低由于卡尔曼滤波器　状态维数增加而带来的滤波计算负担　ｊ，一般情况　

下，常常忽略那些影响较小的不可观测、或者观测度　

较小的误差状态，尽管这将导致次优滤波器，但却可　以大大减少运算量。另外，系统采用的ＩＭＵ本身精　

度不高，对器件的精确建模意义不大，因此，本文采　用低阶卡尔曼滤波器。　建立系统的状态方程如下：　

Ｘ（ｔ）＝ＡＸ（ｔ）＋Ｗ（ｔ）　（７）　状态变量选为：航向角误差　，东向速度误差６　，　北向速度６　，平台失准角　，　，　，陀螺漂移　，　

Ⅳ，　ｕ，即　（ｔ）＝［６ｌｆｒ，６　，６　，　Ｅ，　Ⅳ，　ｕ，　Ｅ，　Ⅳ，　］Ｔ　ｕ　Ｊ　。　将ＳＩＮＳ水平速度误差　、６　和磁传感器的　

航向角误差　作为量测量，可得量测方程为：　ｚ（　）＝ＨＸ（ｔ）＋Ｖ（ｔ）　（８）　

必须指出的是：由于陀螺仪漂移和加速度计零　

偏被简化为白噪声，磁传感器的航向角误差近似用　

一阶马尔科夫过程进行描述，在模型实际应用时，必　须对滤波器各误差参数进行反复测试。试验表明，　该低阶滤波器具有较高的精度。　

４试验结果　

为验证捷联航姿系统的性能和测量精度，在实　验室进行了大量的静态测试实验。图６～图８为一　组３０　ｍｉｎ的静态试验结果。从试验结果中可以看　

出，水平姿态角的变化在０．４。范围内，达到了系统　设计的要求。　

为验证系统的动态航姿测量精度，依托实验室　

的手动位置转台进行了多次转台试验。图９为磁航　向角圆周试验的结果。经磁传感器误差补偿后，航　

向角在起止位置的重复性较好，并且转台停止转动