叶小岭，杜 浩，王 伟，2

(1.南京信息工程大学信息与控制学院，南京 210044；2.南京信息工程大学江苏省气象传感网技术工程中心，南京 210044)

旋翼(rotary wing)微型飞行器(Micro Aerial Vehicles，MAVS)［1］具有负载和功耗的限制，不能使用重量传统传感器。因此设计一种使用廉价轻量惯性传感器设计的姿态测量系统，也可称为惯性测量单元(简称IMU)［2］，用于输出载体的姿态和航向等参数。进行姿态估算的方法很多:文献［3］设计了基于方向旋转矩阵的UKF滤波器。文献［4］介绍基于廉价传感器的姿态检测系统设计，提出基于欧拉角和四元数的融合算法［5］。虽然考虑了动加速度和陀螺偏差，但是其数据更新频率都较低，不适用于旋翼MAVs，而且该姿态检测系统没有搭载到实际MAVs中飞行实验。文献［6］设计基于低成本MEMS器件的航姿系统，采用二子样等效旋转矢量更新四元数，并应用了降阶的卡尔曼滤波器。文献［7］研究了模型之间的转换问题，对三种状态空间模型进行了随机漂移和角速率估计试验。文献［8］中提出了一种可以在载体运动情况下补偿陀螺漂移的姿态确定方法。该方法利用三轴MEMS加速度计的输出判断载体是否处于加速运动状态。如果载体处于运动状态，则根据MEMS陀螺的测量，采用等效矢量法计算载体姿态；如处于静止状态，则利用加速度计和磁强计计算载体姿态误差。而如果载体一直处于运动状态，则陀螺仪的输出随着时间的推移，不断发散。本设计与以上文献方法不同，应用微机械(MEMS)惯性传感器，设计廉价轻量姿态测量系统，采用四元数法进行姿态解算，并应用扩展卡尔曼滤波器。将机体在平移运动的情况下产生的动加速度做为噪音项来减少估算误差。

1 基于扩展卡尔曼的姿态角推测算法

飞行器的姿态和航向是机体坐标系相对于参考坐标系的方位关系。载体相对于参考坐标系的姿态可以用不同的数学表达式来表示。常用的有三种姿态表达式:方向余弦、欧拉角和四元数。四元数由于可以避免欧拉角的奇异问题而应用得更加广泛，采用四元数来表示机体的姿态，四元数在文献［2］中有详细介绍。

本文用到参考坐标系和机体坐标系，参考坐标系是以地上的任意点为原点，磁场的北极方向为Xr轴，重力方向为Zr轴，XrZr平面的垂直方向取为Yr轴。而机体坐标系是以机体的重心为原点，机体的前方为Xb轴，机体的右方向为Yb轴，机体的下方取为Zb轴。另外，机体坐标系相对于参考坐标系的姿态定义为机体姿态。

参考坐标系上的重力矢量设为gr=［0 0g］T，地磁矢量为 mr=［mn0md］T。g为重力加速度，mn是地磁场在参考坐标系中的水平分量，md是地磁场在参考坐标系中的垂直分量。表示现在的姿态的四元数的推定值设为的话，机体坐标系上的重力矢量及地磁计矢量的推测值可表示为下式。

机体上搭载的加速度传感器与地磁传感器的输出分别定义为 ameasure=［axayaz］，mmeasure=［mxmymz］，与之前讲述的机体坐标系上的重力矢量和地磁矢量的推测值的偏差的矢量可表示为下式。

构成扩展卡尔曼滤波器首先需要建立系统的过程模型。下面推导状态方程式。四元数微分方程如下:

式(4)右边的ωb可以从角速度陀螺来得到，但角速度陀螺与加速度传感器这些惯性传感器因为各种因素而存在误差。为了实现更高精度的推测，期望能够把这些误差也推测出来，所以把角速度陀螺的误差作为状态量导入到系统中。把从角速度陀螺获得的角速度定义为ωmeasure=［ωxωyωz］T，角速度陀螺的误差为 Δωb=［σωxσωyσωz］T，这些量和ωb的关系可以表达为下式

把式(5)带入式(4)得

一般情况下，导致角速度陀螺误差的原因可以考虑为偏移误差ωbias、未对准误差及噪音等，本文设定Δωb≅ωbias，只对偏移误差进行推测，根据参考文献［9］，从实际经验可以知道角速度陀螺的偏移误差具有以下的动力学结构。

其中 ω=［ωxωyωz］T为白色噪声，βx，βy，βz为参数，这是通过实测长时间静止放置的角速度陀螺的数据得到的。通过之前的推导，可以得到状态量x=［()T，(ωbias)T］T的状态方程为:

其中的各个行列式为

ω=［ωxωyωz］T为白色噪声。

式(8)经过离散化后可得离散时间的状态方程表示如式(12)

其次是考虑到包括加到机体上的动加速度的加速度误差，把动加速度作为姿态传感器系统的噪音项。以下为观测方程式。

Δa=［δaxδayδaz］T及地磁计误差 Δm=［δmxδmyδmz］T，加速度传感器及地磁计传感器的输出可以写为下式:

其中

经过推导，得到式(12)与式(15)的过程模型。但这两个公式都是非线性方程式，不能直接运用线性卡尔曼滤波器算法。这种情况下，本文使用了扩展卡尔曼滤波器。

设计扩展卡尔曼滤波器，把tk时刻的被估计状态xk的滤波估计值设为，预测估计值为时，行列式Fk及Hk可以定义为下式:

使用这些行列，扩展卡尔曼滤波器可以通过下面这些公式来得到［10］。

其中，Kk为卡尔曼增益，Pk为估计误差方差矩阵，Pk，k－1为一步预测误差方差矩阵，Qk是系统过程噪声的非负定方差矩阵，Rk为观测噪声的正定方差矩阵，Hk为观测矩阵。Gk，k－1为噪声输入矩阵。式(18)到式(22)的算法由使用得到的观测值来对推测值进行滤波的部分及对下一步的推测值进行预测两个部分构成，式(18)到式(20)为前一部分，式(21)及(22)为后一部分。前面的部分也可称为观测更新，后面的部分可称为时间更新。使用上面给出的这几个公式按顺序进行计算，能够推测出最接近于tk时刻的xk的真实值的滤波估计值。

2 姿态测量系统设计

姿态测量系统硬件部分主要包括三个单轴陀螺仪、三轴加速度计、地磁计、12位模数转换器、微控制器等。本文利用这些微机械(MEMS)传感器组合研制了一种应用于微小型飞行器的姿态测量系统，该系统的硬件组成框图如图1所示。

图1 姿态测量系统硬件组成

加速度是MAV姿态测量以及分析MAV飞行性能的重要状态量，选择的ADXL335能以±3 gn的满量程范围测量加速度。通过调节输出引脚上的电容来选择合适的带宽。采用的加速度计的X轴和Y轴的带宽范围为0.5 Hz至1 600 Hz，Z轴的带宽范围为0.5 Hz～550 Hz。为了减少噪音，将加速度传感器带宽设定为50 HZ，即使用0.1 μF滤波电容［11］。文中所用的陀螺是ADI公司的一款功能完备、成本低廉的角速率传感器(陀螺仪)，用于测量角速率，该陀螺仪测量范围达到±300°/s，抗干扰能力强，具有温度校准功能，漂移误差小，能够满足多旋翼飞行器的飞行控制性能需求。利用外接电容和片上电阻构成一个低通滤波器用于限制ADXRS610速率响应带宽［12］，带宽设为 361 Hz。模数转换器使用了12位分辨率的MCP3204，带SPI串行接口，当供电电压为5 V时；最大采样速率能够达到100 ksps。为了增强微型MAVS的抗风能力，需要提高ADC的采样率，设计的姿态测量系统将以100 Hz的频率输出数据。信号以模拟和数字(RS－232)两种格式输出。模拟信号包括三轴加速度计电压，三轴磁强计电压，加速度计电压。数字量输出包括三轴角速率；滚转、俯仰、偏航角。留有GPS输入接口，ADC输入引脚留有扩展接口，方便根据实际需要增加相应的传感器。设计的姿态测量系统由两块双层电路板构成，上层包括三个单轴角速率陀螺、12位模数转换器、微处理器、PWM信号输出接口；下层包括DC/DC电路、一个三轴加速度，一个三轴地磁计、一个ADC转换器，ADC留有扩展接口方便根据飞行需要添加相关传感器。

3 实验及实验结果

为了证明上边介绍的姿态估计算法及硬件电路的可行性，将数据采集及姿态估计算法程序，下载到设计的姿态测量电路板中进行验证。下面结合实际研制的姿态测量系统对实验过程和结果进行讲解分析。

实验共分为三个部分。实验一为将自行研制的姿态测量系统和商用高精度传感器同时装载到飞行器上进行比较，来验证测量系统的性能。实验二为不考虑动加速度的传统姿态角推算法和本文提出的将动加速度考虑为噪音项的算法的比较，来验证该算法的有效性。实验三为使用本文的姿态测量系统来实现飞行器的姿态控制，目的在于验证该系统的实用性。

本研制姿态测量系统与商用高精度传感器比较结果如图2所示。商用高精度传感器采用Crossbow公司的MNAV100CA，MNAV100CA是一种已校准的数字传感器与伺服控制系统，图中实线x是使用本文研制的传感器得到的估算姿态角，虚线y为MNAV100CA传感器输出姿态角。实验数据是将两个传感器同时搭载在多旋翼MAV上，在飞行中实时记录得到。从图中横轴时间轴看，0～15 s处于加速飞行状态，20 s～60 s基本处于盘旋状态，二者对比可以看出，在各种运动状态下，上文介绍的系统基本能得到商用姿态测量系统相同的测量结果。

图2 与高精度传感器比较结果

将传感器水平放置在桌面上，在保持倾角不变的基础上做水平加速度运动时输出的姿态角，结果如图3所示。图中横坐标为时间轴，纵坐标为姿态角输出轴，初始水平线为传感器水平静止时状态，载体在做线加速度运动时，理论值应该为一条水平直线。实线x为没有考虑动加速度的情况下，机体在做线性加速度运动时，姿态角在－15°～+15°范围内变化。虚线y为使用上文研究的算法后的推算角，在－5°～+5°范围内波动，从对比的结果可以看出，考虑了动加速度的算法能够削减由动加速度引起的估算误差，能够更加精确的推算出飞行器的姿态角。

图3 线加速度运动姿态角输出结果

使用本文设计姿态测量系统，实现姿态控制结果如图4所示。图4中，横坐标为时间轴，纵坐标为滚转角和俯仰角，实线为参考输入，虚线跟踪结果。从图4可以看出，使用本设计的姿态测量系统能够达到很好的姿态控制效果。

图4 姿态控制实验结果

4 结束语

应用MEMS惯性传感器，设计应用于多旋翼MAVs的轻量廉价姿态测量系统，采用四元数法进行姿态矩阵计算，并应用了扩展卡尔曼滤波器。飞行器在飞行过程中，加速度计除了检测到重力加速度在机体坐标系上坐标变换的成分外，还能检测到由飞行器的运动而产生的动加速度，采用的算法将动加速度作为姿态传感器系统的噪音项，然后利用卡尔曼滤波器来构成姿态推测算法来降低误差。通过实际飞行中对比商用高精度传感器，证明所提的姿态测量系统能够很好的反映机体姿态，能够应用于多旋翼MAVs的飞行控制。

通过对噪音项加权矩阵的调节，可以降低动加速度导致的姿态推测误差。但是在实际设计过程中，如果过多的考虑动加速度噪音的影响，将导致姿态推算收敛变慢，在飞行中无法正确的推测出飞行器的姿态。因此，通过大量的仿真来获得合适的加权函数是本姿态传感系统的一个关键。另外，本系统算法没有涉及到飞行器的运动原理，如果能在估算法中结合飞行器的姿态数学模型的话，将会更加精确的推测出动加速度，实现更高精度的姿态角估算与姿态控制。

［1］Kenzo Nonami，Farid Kendoul，Satoshi Suzuki，Wei Wang，et al.Autonomous Flying Robots:Unmanned Aerial Vehicles and Micro Aerial Vehicles［M］.Berlin:Springer，2010.

［2］黄旭，王常虹，伊国兴，等.利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器［J］.中国惯性技术学报，2005，13(2):27－30，34.

［3］赵鹤，王喆垚.基于UKF的MEMS传感器姿态测量系统［J］.传感技术学报，2011，24(5):642－646.

［4］Gebre－Egziabher D，Hayward R C，Powell J D.Design of Multi－SensorAttitude Determination Systems［J］.Aerospace ＆Electronic Systems Society，2004，40(2):627－649.

［5］ZHAI Yu－tao，WEI Qiang，WANG Xiao－hao，et al.Micro Attitude Determination System Based on MEMS Inertial Sensors［J］.Nanotechnology and Precision Engineering，2009，7(4):375－379

［6］杜继永，黄国荣，张凤鸣，等.基于低成本MEMS器件的捷联航姿系统设计［J］.传感技术学报，2010，23(11):1662－1666.

［7］袁赣南，梁海波，何昆鹏，等.MEMS陀螺随机漂移的状态空间模型分析及应用［J］.传感技术学报，2011，24(6):853－858.

［8］高宗余，李德胜.多MEMS传感器姿态测量系统的研究［J］.电光与控制，2010，17(3):68－70，97.

［9］Robert M Rogers.Applied Mathematics in Integrated Navigation System［M］.Second Edition.AIAA，2003.

［10］付梦印，邓志宏，闫莉萍.Kalman滤波理论及其在导航系统中的应用［M］.第二版.北京:科学出版社，2010.

［11］Analog Devices.Inc.ADXL335 DataSheet［EB/OL］.http://www.analog.com/en/index.html

［12］Analog Devices.Inc.ADXRS610 DataSheet［EB/OL］.http://www.analog.com/en/index.html