基于MEMS/GPS的航向姿态测量系统设计*

刘　强，王昌刚，刘玉宝，刘晓川

（中国船舶重工集团第716研究所，江苏连云港222006）

基于MEMS/GPS的航向姿态测量系统设计*

刘强*，王昌刚，刘玉宝，刘晓川

（中国船舶重工集团第716研究所，江苏连云港222006）

姿态信息是飞行控制中最关键的参数之一。针对飞行控制系统首要解决的姿态测量问题。本文利用多微机电系统（MEMS）传感器与GPS组合，研制了一种微型航向姿态测量系统。考虑在以往的姿态测量系统中，动加速度的影响限制其应用，通过设计利用GPS进行辅助修正的姿态解算算法，有效规避了运动加速度对测量精度的影响，使系统可同时满足静态情况和动态情况的使用。将试验结果与商用姿态测量产品MTI-G-700的姿态结果进行了比较，各项指标均达到系统设计的性能要求，验证了航向姿测量系统设计的有效性。

航向姿态测量系统；微机电系统；GPS；飞行控制；

姿态测量是无人机实现姿态控制的前提，是导航系统不可分割的一个重要组成部分［1-4］。伴随着导航系统的发展，姿态航向测量技术也越加的成熟起来。采用陀螺进行姿态测量是最常用的方法，但陀螺测得的姿态角是发散的，不能长期使用。GPS是载体姿态测量的另一重要工具，通常有多天线测姿和基于“伪姿态”的单天线测姿两种测量方法，但其也有诸多的限制条件。随着组合导航技术的产生，组合姿态航向测量技术逐渐取代了单一陀螺测姿的方法，使得姿态测量的精度越来越高。荷兰的Xsens公司的MTI-G航姿器是一款六自由度的微型航姿系统［5］，集成了MEMS、IMU、GPS、磁强计、静态气压计、温度传感器，经数据融合后能提供精确的地磁数据、姿态数据、位置数据和速度数据；法国SBG Systems公司的IG-500A/E/N微型姿态航向参考系统（AHRS）［6］拥有嵌入式的扩展卡尔曼滤波器，在高动态条件下能够提供高精度的姿态和方位信息，该航姿系统还带有GPS辅助定位功能。此类姿态测量系统虽然精度较高，但是因其价格昂贵，不适用于微型飞行器或小型无人机。

本文以微型飞行器为应用背景开展了姿态测量研究，针对MEMS传感器存在精度低，漂移大，且当飞行器运动时，加速度计信号将不能正确反映机体的姿态信息等缺点，利用三轴MEMS陀螺、三轴MEMS加速度计、三轴MEMS磁强计和微型GPS等多个传感器设计合理可行的软硬件解决方案。研制了一种应用于微小型飞行器或小型无人机的姿态测量系统［7-8］。

1　系统总体方案

针对微小型无人机体积小、重量轻、功耗低等特点，系统选用基于MEMS的微小型惯性测量元件和GPS作为传感器，为姿态解算提供原始的数据。采用扩展卡尔曼滤波器设计了一种基于MEMS陀螺/加速度计/磁强计/GPS组合的姿态测量方法，实现了微小型无人机的精确姿态测量功能。系统总体结构框图如1所示。

图1　系统总体结构图

图1中虚线框为扩展卡尔曼滤波算法，实线框为基于GPS测量信息的辅助修正算法，利用GPS经纬高和速度，解算出实际的线性加速度补偿加速度计输出，用补偿后所得的沿机体三轴的重力加速度分量作为观测量，采用基于四元数的扩展卡尔曼滤波算法实现对无人机姿态四元数的滤波估计，并利用地磁偏角对解算出的航向角进行修正。有效解决了机体运动加速度和地磁偏角对航向姿态测量精度的影响，提高了航向姿态测量系统的测量精度。

2　硬件设计

该系统硬件平台采用“DSP+MIMU+GPS”的架构形式，其结构如图2所示。图2中，虚线框内为DSP及其外围电路组成的核心处理单元，用于系统的实时解算和通信。惯性测量元件和GPS用于提供系统姿态解算的原始信息［9］。

图2　硬件结构图

①处理器选型

考虑姿态解算过程中需完成状态预测、状态估值、估计均方误差等诸多运算，涉及到大量的高阶矩阵运算过程；系统中包括矩阵元素在内的绝大部分变量，均为64位单精度浮点类型，大大提高了其对运算精度的要求；姿态解算频率要求达到100 Hz，对运算速度要求高。基于上述诸多要求，选用TI（德州仪器）公司的的32位TMS320F28335型DSP控制器作为该嵌入式平台的处理器。

③惯导模块选择

考虑到微小型无人机机舱空间、载荷有限及指标要求，选用ADI公司生产的中等精度的MEMS 磁/惯性传感器 ADIS16407作为惯性测量单元。ADIS16407惯性测量模块具有集成度高、体积小的特点，内部集成了三轴角速率陀螺、三轴线加速度计和三轴磁强计等部件，内部集成FIR滤波算法，能够为无人机姿态解算提供所需的原始数据。

②GPS模块选择

GPS模块采用瑞士 u-blox公司的 UBLOX NEO-6M芯片，该模块尺寸小、功耗低，具有丰富的通信接口，硬件电路设计简单，设计中通过UART接口与DSP处理器通信。

基于系统结构图和核心部件的选型，完成了组合航向姿态测量系统的研制，如图3所示。

图3　航向姿态测量系统实物图

3　算法设计

3.1姿态算法推导

首先假设陀螺仪的偏差漂移只受到高斯白噪声影响［10］，即

接着根据四元数和角速度之间的关系，有下式

其中

ω1，ω2、ω3是机体坐标相对于磁场NED坐标的角速度。陀螺仪测量到的角速度大小是机体坐标系相对于惯性坐标系，需要将角速度转换成机体坐标相对于磁场NED坐标的角速度。惯性坐标系相对于磁场NED坐标的角速度可以从下式得到

其中 ωearth是地球角速度，包含了地球的公转和自转，ωtranslation是无人机运动而造成的角速率。

接着将正确的角速度带入Ω

合并（2）（4）式，并且令系统状态

对其进行微分

接着使用加速度计和磁力计当做系统的量测值，加速度计的测量值包含重力场和无人机的线性加速度，其中重力场的方向就是NED坐标的Z轴方向，但是加速度计没有办法单独测量重力场，因此需要将无人机的线性加速度扣除才是正确的量测值［11］，即

而磁力计测量的是地球的磁场方向，地球的磁场方向位于磁场NED坐标的XZ平面上，不能直接当做磁场NED坐标任一轴的量测值，但可以利用重力场方向和地球磁场方向的外积，因为这两个方向的外积正好是磁场NED坐标的Y轴。假设地球磁场方向为E、大小为M，重力场方向为g、大小为G，如此可以得到Gg=gxb1+gyb2+gzb3

因此

3.2机动加速度计算

姿态测量系统一般利用重力场的三轴分量来计算俯仰角和滚转角。假设飞机某一瞬间的俯仰角为θ，滚转角为φ，且沿飞机X、Y、Z三轴的重力场分量为gx，gy，gz。则

若已知gx，gy，gz就可以经由下式求得俯仰角及滚转角：

但在飞行过程中无法直接测量gx，gy，gz。如果飞行瞬间的三轴速度分别为U、V、W，三轴速度分别为p、q、r，而加速度计量测得到的加速度分别为ax、ay、az，则加速度与这些参数之间的关系为：

通过机械三轴加速度计测量可以得知ax、ay、az，由机械三轴陀螺仪可以测得p、q、r，而飞行瞬间的三轴加速度U˙、V˙、W˙则可以通过GPS接收器所测的速度的一次差分而取得，详细计算过程如下

其中，下标n为时间标志；V˙N、V˙E、V˙D分别表示飞行器向北、向东、向下的速度；V˙N、V˙E、V˙D分别表示飞行器向北、向东、向下的加速度；Δt为系统的采样时间。最后将算出的VN、VE、VD、V˙N、V˙E、V˙D透过旋转矩阵将器转成飞行器机体坐标系上的U˙、V˙、W˙。

旋转矩阵表示如下：

因此

取得以上的信息后，便可以通过之前的公式算出三轴重力场分量gx，gy，gz。此方法的优点是没有积分运算，所以量测误差不会累积甚至发散，长时间而言具有较高的准确性。但其缺点是需要量测或计算三轴加速度分量，其结果通常都含有较高的噪声。

3.3磁偏角计算

GPS可以提供用大地坐标系表示的信息，通过文献［12］可知GPS参考大地坐标系与地磁坐标系之间会随着地理经纬度的变化而变化，因此只要能够找到GPS坐标系与地磁坐标系之间的转换关系，即可利用GPS经纬度实时计算地磁偏角［13］。

其中α，β分别表示经度和纬度。

其中αN，βN分别表示磁北极的经度和纬度。GCM表示地磁坐标系到大地坐标系的转换矩阵

其中

推导出所有坐标关系后，可以得到

地磁坐标系和地理坐标之间的转换旋转角（磁偏角）为

4　试验验证

为了验证算法的可行性，本文在翼展2 m的飞行器上分别进行了地面试验，空中飞行不带GPS修正和带GPS修正动态试验，主要验证内容如下：①姿态解算算法的性能；②高机动情况下，带GPS修正算法的机动性补偿效果。并将三种种试验条件下得到的俯仰角、滚转角、航向角输出结果与国外最新的航姿测量产品MTI-G-700的输出结果进行了比较，地面试验结果如图4所示。空中飞行不带GPS修正动态试验结果如图5所示，空中飞行带GPS修正动态试验结果如图6所示。

图4　地面动态试验试验结果

图5　空中飞行不带GPS修正动态试验结果

图6　空中飞行带GPS修正动态试验结果

由图4可知在地面小机动情况下本文所设计的航姿测量系统与MTI姿态测量产品输出的俯仰角、滚转角和航向角结果非常吻合。图5可知在动态无GPS修正的情况下，姿态测量结果较差，测量曲线波动剧烈，这与基于MEMS微惯性器件的姿态测量设备容易受机动加速度，而传统的姿态测量方法无法消除运动加速度对姿态测量精度的影响，导致试验数据波动剧烈，测量精度较差。由图6可知在GPS修正的情况下，通过GPS的测量信息融合到姿态测量算法之中，有效消除了运动加速度对姿态测量精度的影响，从而实现姿态输出与真值测量设备趋势一致，且跟踪误差满足动态精度要求，俯仰角和滚转角精度小于2°（RMS），航向角精度小于3°（RMS），达到了系统的设计指标。动态试验表明，本文所设计的姿态测量方法在无人机高机动飞行状态下仍能够有效抑制陀螺漂移导致的姿态发散，有效消除了MEMS微惯性器件姿态测量受机动加速度的影响，且能够实时高精度的跟踪无人机的真实姿态。

5　结论

本文提出一种基于三轴MEMS陀螺，三轴MEMS加速度计，MEMS磁力计和微型GPS研制微型姿态测量系统的方法，通过地面和空中飞行动态试验，将本文结果与最新的商用姿态测量产品MTI-G-700输出的姿态结果进行了对比，验证了本文方法的可行性和优点。高机动飞行试验表明，本文算法远好于未作机动性补偿的MEMS微惯性器件的姿态测量算法，且系统设计简单，尺寸较小（60×50×30），造价低，便于在小型无人机上应用，总体性能已接近国外同类产品水平，目前该型产品已经在某小型无人机的飞行控制及导航系统上获得成功应用。

［1］王昌刚，刘强，韩瑜.姿态航向测量与误差补偿方法研究［J］.传感器与微系统，2014，33（11）：33-40.

［2］叶小岭，杜浩，王伟.应用于多旋翼MAVs的姿态测量系统［J］.传感技术学报，2012，25（3）：344-348.

［3］Jeffrey D Barton.Fundamentals of Small Unmanned Aircraft Flight ［J］.Johns Hopkins Apl Technical Digest，2012，31（2）：132-149.

［4］吴峰，王向军，汤其剑.基于数字调节方法的MEMS陀螺零位补偿技术研究［J］.传感技术学报，2012，25（12）：25-28.

［5］MTI-G User Manual and Technical Documentation［Z］.Xsens Technologies，2007.

［6］IG-500NGPS Aided AHRSUserManual［Z］.SBG Systems，2009.

［7］Chul Woo Kang，Chan Gook Park.Improvement of Attitude Estimation Using Hidden Markov Model Classification［J］.American Institute of Aeronautics and Astronautics，2010，25（11）：15-18.

［8］Karlgaard and H.Schaub.Nonsingular Attitude Filtering Using Modified Rodrigues Parameters［J］.Journal of the Astronautical Sciences，2010，57（4）：777-791.

［9］Or D Dantsker，Renato Mancuso，Michael S Selig.High Frequency Sensor Data Acquisition System for Flight Control and Aerodynamic Data Collection［J］.American Institute of Aeronautics and Astronautics，2014，14（8）：42-47.

［10］Zhi Yu Peng.Attitude Determination Based on Rodrigues Parameters［D］.The Thesis of Tamkang Universty，2012.

［11］Liu qiang，Gao Fu yin，Wang Chang Gang，et al.Improvement of Attitude Estimation Using Modified Rodrigues Parameters and In-telligent Controller［J］.Applied Mechanics and Materials，2014，32（1）：635-639.

［12］Rotenberg D，Luinge H，Baten C，et al.Veltink.Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation［J］.IEEE Trans Neural Syst Rehabil，2005，13：395-401.

［13］Daniel Roetenberg，Per J Slycke，Peter H Veltink.Ambulatory Position and Orientation Tracking Fusing Magnetic and Inertial Sensing［J］.IEEE Transacti-Ons On Biomedical Engineering，2007，54 （5）：161-167.

刘强（1985-），男，汉族，中船重工第七一六所工程师，硕士研究生学历，主要研究方向为无人机导航与飞行控制，工业机器人运动控制技术，基于MEMS传感器的误差建模与补偿技术，SAR无人机侦察路径规划等，nuaalq@qq.com。

Design of Attitude and Heading Reference System Base on MEMS/GPS*

LIU Qiang*，WANG Changgang，LIU Yubao，LIU Xiaochuan
（716 Institute of China Shipbuilding Industry Company，Lianyungang Jiangsu 222006，China）

Attitude information is one of the most important parameters in flight control.Focus on the most important problem of attitude measurement in flight control system.This paper，a kind of micro attitude attitude and heading reference（AHRS）is developed based on the combination of multiple MEMS sensors and GPS.In the past，the attitude measurement system，kinetic acceleration effects limit its application，through design of auxiliary correction algorithm based on GPS velocity，effectively avoid the influence of kinetic acceleration on the measurement accuracy. The system can meet the use of static and dynamic situation.The test results are compared with commercial attitude measurement product MTI-G-700 show that，the design of attitude and heading reference system is validity.

AHRS；MEMS；GPS；Flight Control

V249

A

1004-1699（2016）08-1290-07

EEACC：723010.3969/j.issn.1004-1699.2016.08.028

项目来源：江苏省自然科学基金青年基金项目（BK2012236）

2016-01-21修改日期：2016-04-11