四旋翼惯性导航姿态解算算法的改进与仿真

李汉博，秦文华

(曲阜师范大学 物理工程学院，山东 曲阜 273165)

四旋翼惯性导航姿态解算算法的改进与仿真

李汉博，秦文华

(曲阜师范大学 物理工程学院，山东 曲阜 273165)

为了实现四旋翼飞行器的高精度导航，提出了互补滤波法和四元数算法对传感器获得的数据进行修正，最大限度的抑制干扰误差并提高姿态角解算的准确度;首先简单推导了捷联式惯性导航系统的算法基本原理并利用互补滤波算法进行改进，然后给出了惯性导航系统的力学编排模型分析旋翼飞行器的运动姿态;最后仿真验证数据选用惯性仪表MPU6050和HMC5883所得到实测数据采集并进行仿真分析，平台处理器选用STM32来仿真惯性仪表的测量速度，最终得到实验结果证明算法可行性。

四旋翼飞行器；互补滤波法；四元数算法；传感器；姿态角

0 引言

近些年来，随着智能化产业迅速的发展，无人飞行器开始逐渐的走进人们视线。四旋翼飞行器(quadrotor)拥有4个旋转螺旋桨,前后的两个螺旋桨按照顺时针方向旋转,左右的两个螺旋桨按照逆时针方向旋转[1]。同时两个轴向电机马达反向的旋转方式相互抵消彼此的转矩,使得四旋翼飞行器能够在半空中保持悬停。前后的螺旋桨所产生的推力差别可以使其做俯仰动作,而翻滚动作则是由于左右的螺旋桨推力的差别所产生的;其本身通过改变2个轴向转速来实现飞行器的自旋。四旋翼飞行器结构简单并且可靠，具有定点悬停、垂直起降、在小空间内飞行还有稳定低速飞行等特点， 受到了广泛关注，并且在很多个领域获得到了广泛应用。

但是由于四旋翼无人飞行器所使用的 MEMS 传感器的精度较低，尤其受 MEMS 的陀螺仪逐次的启动参数误差以及零偏不稳定等因素的影响，系统所得到姿态参数发散较快，难以满足对导航精度和速度的要求[2]，所以本设计基于以上情况，创新性的提出了使用互补滤波算法[3]对传感器的采集数据进行修正。给出了设计中惯性导航系统的力学编排模型和分析，系统解算过程的影响因素，对算法进行了改进来消除对系统的影响。最后通过Matlab软件对本设计中使用的算法进行了仿真验证。

1 导航解算算法的原理与分析

1.1 导航解算算法基础

导航算法包含获取姿态的算法、得到速度算法、获得位置的算法等。我们采用获取姿态的算法进行导航解算来获取飞行器的位置，获取姿态的过程又称为姿态更新，姿态更新又包括以下几种算法：旋转矢量法、欧拉角法、方向余弦法和四元数法，这些算法各有利弊。其中四元数法计算量相对来说比较小，简单易于实现且精度相对比较高[4]。

要使用四元数法对四旋翼进行导航解算，通常要选择合适的坐标进行运算处理，即惯性测量仪表测得的载体相关数据必需通过一定的转换关系转换到地理坐标系才能应用于导航[5-6]。常用的坐标系有地心坐标系(i系，惯性参考坐标系[7])，地球坐标系(e系)，地理坐标系(n系，导航坐标系)，地平坐标系(t系)，载体坐标系(b系)。除此之外，在本设计还定义了姿态解算过程中的3个姿态角，用θ表示姿态角中的俯仰角(Pitch)，用γ表示姿态角中的横滚角(Roll)，用φ表示姿态角中的航向角(Yaw)。

在导航的惯性系统中，需要进行坐标系的转换来获取姿态信息，姿态矩阵也就是地理坐标系n与载体坐标系b之间的转换关系[8]。地理坐标系0xnynzn坐标绕zn轴方向旋转，航向角φ得到坐标0x'y'z'，0x'y'z'绕y'轴旋转俯仰角θ得到坐标0xnynzn，0x"y"z"再绕x"轴旋转横滚角γ则得到载体坐标系0xbybzb，根据整个运算旋转过程的值，可以得到载体坐标系与地理坐标系之间的旋转角度的运算关系式，我们可以用方向余弦阵表示为式(1)， 由于姿态矩阵是3个姿态角的函数，即可有姿态矩阵确定姿态角主值的四元数表达式，由式(2)表示，在得到姿态角的主值之后就可以根据姿态角的定义域来确定姿态角的真值，(3)式为姿态角的真值式

(1)

(2)

(3)

1.2 导航解算算法中的互补滤波原理与应用

在实际环境中，我们需要惯性仪表如陀螺仪和加速度计输出的数据来进行飞行器的导航姿态解算，然而它们存在一定的误差。而在解算的过程中，这两个器件的输出数据是否准确关系十分重大。所以对两个器件的输出必须经过一定的处理以提高其精度[9-10]，就是本设计所使用的关键利用互补滤波算法来去噪滤波处理提升输出数据的精度。

由于陀螺仪漂移的存在，会导致其测得的数据越来越不准。互补滤波就是在短时间内较多参考陀螺仪的数据，而随着时间的增加，对陀螺仪测得的数据的信任度降低的时候，采用加速度计测得的数据对其数据进行补偿校正。从原理上说来，是由于陀螺仪和加速度计其各自的特性决定的。如加速度计由于其动态响应慢，在高频段存在误差，可以通过低通滤波器来抑制其高频误差；而陀螺仪由于常值零漂低频段存在较大误差，所以需滤波器来抑制其低频误差。而互补滤波器就是根据其在频域上的互补特性来设计，以抑制干扰误差。这样就能够将两个惯性器件的优点结合起来，得到更加精确的数据。通过加速计测得矢量a(三轴测量值)和参考矢量v(重力加速度在载体坐标系的比例的分量)做叉乘可得到加速度计的测量误差，同时对导航中另一传感器电子罗盘测量矢量(三轴测量值)和参考矢量(磁场矢量在载体坐标系的比例分量)做叉乘得到电子罗盘测量误差，以上两者相加可以得到修正陀螺仪的误差矢量

(4)

设陀螺仪的采样起始时刻为tk、更新周期为T，那么测量修正值可用下式表示

(5)

其中：Kp为误差比例系数常量，Ki为误差积分系数常量。一般的，Kp为Ki的10—100倍[11-12]。

1.3 导航解算算法中的四元数与姿态矩阵

上面已经提到过利用四元数对导航姿态矩阵进行解算，四元数理论是数学的一个分支，是一个实数单位为1和3个虚数单位组成的含有4个元的数，可用下式表达Q=q0+q1i+q2j+q3k。一个坐标系相对于另外一个坐标系的转动，可以用四元数唯一的表示出来。那么载体坐标系相对于导航坐标系运动时可如下表示

(6)

即可以得到利用四元数算法表示的姿态矩阵

(7)

也就是说明姿态矩阵中九个元素都可以使用四元数一一对应表示出来进行进一步的解算。

2 导航解算的力学分析与算法改进

旋翼飞行器具有十字交叉的数个螺旋桨，它可以通过改变4个螺旋桨升力实现不同的运动，其主要方法是改变螺旋桨的转速。旋翼有且仅有4个输入力，却需要产生6个自由度方向的运动，属于典型的欠驱动系统。而且四旋翼飞行器具有高度的耦合动特性，一个螺旋桨速度发生变化，将会同时改变6个状态量。四旋翼的姿态控制特性是发散的，升力的偏斜会使得姿态偏离中立位置越来越远，直到升力不足以支撑重力导致飞行器坠落。

2.1 建立理想状态的动力学模型

使用Simulink建立如图1所示的旋翼飞行器的惯导系统的开环响应模型。开环响应模型中的姿态解算部分如图2所示，采用了上文中所设计的算法。

图1 惯性导航系统的开环响应模型

图2 姿态解算的理论模型

2.2 惯性导航算法的改进

将控制信号输入仿真模型，产生的三轴姿态角如图3所示。

图3 三轴姿态角输出

姿态角3个方向间耦合度很小，姿态响应较控制信号的输入慢，姿态变化导致升力方向变化进而引起旋翼机运动。在旋翼飞行器进行倾斜运动时，需要及时进行垂直速度补偿，否则将会减速而下降。若消除机体姿态角解算自然发散的趋势，就需要对输入数据和输出数据进行特殊耦合，即使用前文所使用的互补滤波算法进行处理互补滤波脉冲做为欧拉角的输入。输入和输出分别如图4和5所示，可以发现经过输入信号加入互补滤波

后姿态角的输出值高度耦合，达到算法改进的目的。

图4 互补滤波控制信号输出

图5 改进后姿态角输出

3 导航算法的仿真和验证

算法的仿真验证选择使用直接由惯性仪表(陀螺仪、加速度计和电子罗盘)得到的实测数据进行仿真分析。实测数据能真实反映随机误差和量化误差，如果能得到误差相对比较小的算法，那么在实际中执行时遇到的困难和问题比较少。

加速度计和陀螺仪选用了InvenSence公司的6轴运动处理组件MPU6050，该芯片集成了3轴MEMS陀螺仪和3轴MEMS加速度计，免除了组合陀螺仪与加速器时之轴间差的问题。

电子罗盘选用Honeywell公司的HMC5883L芯片。该芯片采用各向异性磁阻技术，具有在轴向高灵敏度和线性高精度的特点，测量范围从1毫高斯到8高斯。

飞行器的采集数据平台处理器选用STM32芯片，将已经改进加入互补滤波的算法加入芯片，进行数据的采集和解算分析。

每次采集数据共有十二个值，分别为加速度计三轴测量值、电子罗盘三轴测量值、陀螺仪三轴测量值和通过处理器解算得到的3个姿态角，此处以俯仰角为例，波形图如图6第一行波形所示。

同时为了验证互补滤波算法对系统误差的影响，也使用Matlab对无互补滤波补偿修正算法进行仿真验证，如图6第二行波形所示，第三行为前两行波形的误差波形

图6 俯仰角实测值与无互补滤波修正的解算值比较

图7与图6不同的是第二行波形是在Matlab仿真过程中已经加入了互补滤波算法，可以得到已经加入算法的实测值与仿真值误差基本为零，证明该算法在实际操作中的可行性。

图7 俯仰角实测值与互补滤波修正的解算值比较

4 结语

四旋翼飞行器导航系统首先需要保证导航姿态角的精度和可靠性，才能够有效地融合其他传感器的数据来估计出位置和速度。本文创新之处在于实现了一种基于互补滤波的算法，有效的解决了传感器噪声所造成的误差问题，提高了姿态角的准确度。通过对导航控制系统的力学模型分析，以及解算过程的影响因素，对算法进行了改进消除了噪声对飞行导航解算的影响。通过算法实测与，Matlab仿真对本设计中的算法进行了验证，证明了该算法能够有效的消除噪声，为后续进一步实现飞行控制系统硬线电路设计提供了基础。

[1] 马远超.四旋翼飞行器导航及控制技术研究 [D]. 哈尔滨:哈尔滨工程大学，2013.

[2] Brockhaus R.Flugregelung[M].NewYork:Springer-Verlag, 1994: 5-14.

[3] Robert M，Tarek H，Jean M P．Complimentary filter design on the special orthogonal group SO(3)[A]. Proceedings of the 44thIEEE Conference on Decision and Control and European Control Conference 2005[C]. Seville, Spain, Dec 12-15, 2005: 1477-1484.

[4] Madgwick S, Harrison A, Vaidyananthan R. Estimation of IMU and MARG orientation using a gradient descent algorithm[A].IEEE International Conference on Rehabilitation Robotics[C]. Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29-July 1, 2011.

[5] 贾胜文. 基于ARM7CPU和FPGA的低成本小型捷联惯导系统的设计[D].西安: 西北工业大学, 2006.

[6] 栗瑞江. 低成本SINS/GPS组合导航系统研究[D]. 西安: 西北工业大学, 2001.

[7] 邓正隆. 惯性技术[M]. 哈尔滨: 哈尔滨工业大学出版社, 2006.

[8] 黄溪流. 一种四旋翼无人直升机飞行控制器的设计[D]. 南京: 南京理工大学, 2010.

[9] Mellinger D, Michael N, Kumar V. Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors[J]. The International Journal of Robotics Research,2012,31(5): 664-674.

[10] Euston M,Coote P, Mahony R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[A]. 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems[C]. Nice, France, Sept 22-26, 2008: 340-345．

[11] 黄德鸣, 程 禄. 惯性导航系统[M]. 北京: 国防工业出版社, 1986.

[12] 雷 芳, 王 华, 焦国太, 等. 弹道修正弹药的姿态测量技术研究[J]. 弹箭与制导学报, 2009, 29(4): 123-125.

Improvement and Simulation of Four Rotor Aircraft Inertial Navigation Attitude Algorithm

Li Hanbo, Qin Wenhua

(Physics Engineering College, Qufu Normal University, Qufu 273165,China)

In order to implement high precision four rotor aircraft navigation that puts forward the complementary filter method and the quaternion algorithm for sensor data amended, it can restrain the interference of the maximum error and improve the accuracy of attitude Angle calculating. First of all, a simple algorithm of strapdown inertial navigation system was deduced basic principle and use the complementary filter algorithm was improved, the inertial navigation system is given and then the mechanical layout model analysis of rotor aircraft motion. Finally simulation data selection of inertial instrument MPU6050 and HMC5883 simulation analysis was made on the measured data, platform selection STM32 processor to simulate inertial instrument measurement speed, eventually get the experimental results show feasibility of the algorithm.

four-rotor aircraft ;the complementary filter ; quaternion-algorithm; sensor; attitude-angle

2015-12-14；

2016-02-15。

山东省科技发展计划项目(2014GGX101027)；山东省高校计划项目(J15LN08)。

李汉博(1991-)，女，山东济宁人，硕士研究生，主要从事智能信息处理方向的研究。

秦文华(1968-)，男，山东聊城人，硕士，副教授，主要从事无线通信技术、信号的采集与处理方向的研究。

1671-4598(2016)07-0187-03

10.16526/j.cnki.11-4762/tp.2016.07.051

TP391 文献标识码：A