基于四元数自补偿四旋翼飞行器姿态解算

马 敏，吴海超

MA Min,WU Hai-chao

（中国民航大学 航空自动化学院，天津 300300）

0 引言

四旋翼(Quad-Rotor)无人飞行器是由四螺旋桨驱动的、能够实现垂直起降的飞行器，由前后和左右两组旋翼组成，旋转时对应方向上电机旋转方向相反，其与一般直升机最主要的不同点是四旋翼飞行器不需要用尾桨来平衡机体。特别适合在近地面环境中执行监视和侦察等任务，具有广阔的军事和民用前景[1]。

其中，姿态解算的精度是实现飞行器稳定飞行的前提。而体积小、功耗低及易于集成的MEMS 传感器是实现低成本航姿测量的最佳选择，主要由三轴陀螺仪、三轴加速度计和三轴电子罗盘组成。MEMS 陀螺仪测量精度较低，虽然动态性能良好，但存在累计漂移误差，加速度计和电子罗盘有着较好的静态特性，没有漂移问题，但容易受机体振动和外部磁场等因素的干扰，使其动态可信度降低[2]；为有效抑制惯性器件常值偏差对惯导系统的影响，本文设计了四元数自补偿方法。由于旋转的引入，惯导系统中陀螺仪和加速度计的常值偏差被调制成正弦信号，通过积分运算可以有效地消除常值偏差对惯系统的影响。实现了长时间稳定地输出准确姿态数据。

1 导航坐标系描述

所谓姿态解算是指将运载体上惯性单元的输出，实时转换成运载体的姿态。这里的姿态通常指机体坐标系(b系)相对于导航坐标系(n系)的角位置。

导航坐标系(n系)，它的原点位于载体质心，x、y和z 轴的方向选为东北天(ENU)右手坐标系。机体坐标系（b系）的原点与载体质心重合，对于飞行器而言，x 轴沿载体横轴向右，y轴沿载体纵轴向前，z 轴沿载体竖轴向上，即“右前上”坐标系，可见，机体坐标系是与载体固连的[3]。根据机体坐标系和地理坐标系之间的相对角位置关系，可以定义载体的 3 个姿态角，分别为偏航角j、俯仰角q和横滚角g，如图1所示。

图1 机体坐标系和导航坐标系角度关系

机体系(b)和导航系(n)之间的方向余弦阵可由式(1)表示：

2 四元数自补偿算法研究

2.1 姿态解算的四元数法

四元数，是由一个实数和三个虚数组成的“超复数”，表示为：

在计算运载体姿态时，当只关心机体坐标系相对于导航坐标系的角位置时，可以认为机体坐标系(b系)是由导航坐标系(n系)经过一次性等效旋转形成的，四元素Q包含了这种等效旋转的全部信息[4]。其姿态变换公式如式（4）所示：

刚体转动理论中，依欧拉定理，式（1）的四元数表示为：

由旋转矩阵式(1)得到的欧拉角求解公式如下：

偏航角j、横滚角g真值通过表1和表2确定。

表1 偏航角真值表

表2 翻滚角真值表

由于四旋翼飞行器飞行高度低，飞行速度慢，可以忽略地球自转等因素的影响，那么四元数Q具有如下微分方程关系：

设T为采样周期，那么四元数微分方程的一阶龙格-库塔法计算式为：

每个采样周期读取陀螺仪、加速度计和电子罗盘的数据，对上式进行迭代运算，即可实现四元数随时间的更新，进而求得3个姿态角。而在实际中常值偏差是变化的，即在一次启动后随着陀螺仪、加速度计和电子罗盘运转的时间的增长，惯性测量器件的常值偏差也缓慢的变化到导航系统精度无法允许的程度。

2.2 四元数自补偿算法研究

在传统的惯性导航系统中，惯性测量元件与运载体固连，它们之间没有相对运动。四元数自动补偿方法是将安装有陀螺仪、加速度计和电子罗盘的惯导系统相对机体坐标系旋转，使惯性器件常值偏差沿着机体系敏感轴方向呈周期性变化。可有效抑制零位误差对系统的影响。

由于自补偿算法在三个轴方向具有相同的应用，本文仅讲解单轴旋转的自补偿。

设初始时刻导航坐标系(n系)与机体坐标系重合(b系)，然后机体坐标系以角速度w绕ozb轴旋转。机体坐标系和导航坐标系的夹角为wt，两个坐标系之间的关系用方向余弦表示为[6]：

陀螺仪和加速度计的输出从导航坐标系到机体坐标系的转换关系表示为：

导航坐标系中陀螺仪和加速度计的输出为：

式（17）、（18）可以看出当机体坐标系以角速度w绕ozb轴旋转时，常值陀螺漂移ex、ey和常值加速度计零位误差∇x、∇y在导航坐标系oxb和oxb轴上的分量呈周期变化[7]。所以，ozb轴方向上的陀螺常值漂移和加速度计零位误差没有得到调制，有效抑制了惯性器件的偏差对解算精度的影响。

3 实验结果

实验中使用陀螺仪 L3G4200D，加速度计ADXL345和电子罗盘 HMC5883L 构成姿态解算单元，采用 TMS320F28335做为主控制器，通过I2C接口读取3个传感器数据进行实时姿态解算，得到的姿态角通过 SCI 接口与上位机进行通讯[8]。实验平台如图2所示。

图2 搭建硬件平台

上位机将相应的数据绘制成曲线显示出来。未加入补偿算法得出曲线如图 3 所示。可以看出一些时间段内翻滚角度超出了基准范围[-90～90]，所以，直接运用四元数法进行姿态解算，动态性能良好，但存在一定的漂移。图4给出了加入自补偿算法后曲线图，其解算姿态角，既能保持在姿态角基准范围内，又具有四元数法的良好动态特性。

图3 未加入自补偿曲线图

图4 加入自补偿曲线图

4 结束语

本文分析了MEMS器件常值偏差的变化对姿态解算的影响[9]，针对MEMS器件精度低、易发散的问题提出四元数自补偿算法，并在小型四旋翼飞行器姿态解算系统中进行了实验。实验结果表明：该算法能有效抑制常值误差的变动对姿态解算精度的影响，且保证了四元数法的良好动态特性。

[1] 单海燕.四旋翼无人直升机飞行控制技术研究[D].南京航空航天大学.2008.

[2] 聂博文,马宏绪,王剑,等.微小型四旋翼飞行器的研究现状与关键技术[J].电光与控制,2007,14(6)∶113-117.

[3] 刘俊,石云波,李杰.微惯性技术[M].北京∶电子工业出版社,2005.

[4] YANG Y,MIAO L J,SHEN J.Method of improving the navigation accuracy of SINS by continuous rotation[J].Journa1 of Beijing Institute ofTechno1ogy,2005,14(1)∶45-49.

[5] HUANG W Q,ZHAO G L,TAN ZH F,et a1.Research of Gyro case rotating monitor technique[J].Ship Bui1ding of China,2002,43∶54-59.

[6] 于旭东,王宇,张鹏飞,等.单轴旋转对惯导系统误差特性的影响[J].惯性技术学报,2008,16(6)∶643-648.

[7] 王其,徐晓苏.旋转IMU在光纤捷联航姿系统中的应用[J].中国惯性技术学报,2007,15(3)∶265-368.

[8] 苏奎峰,吕强,邓志东,汤霞青.TMS320x28xxx原理与开发[M].北京∶电子工业出版社.2009.

[9] 洪森涛,金智慧,李志强.四旋翼飞行器建模及姿态稳定性分析[J].航空制造技术.2006(5).91-94.