西 南 师 范 大 学 学 报 (自然科学版) 第36卷 第4期 Vol.36 No.4 JournalofSouthwestChinaNormalUniversity(NaturalScienceEdition) 文章编号:1000 5471(2011)04 0137 05 2011年8月 Aug. 2011 基于陀螺仪及加速度计信号融合的姿态角度测量① 冯智勇, 曾 瀚, 张 力, 赵亦欣, 黄 伟 西南大学 计算机与信息科学学院,重庆400715 摘要:针对在四旋翼飞行器姿态控制中传感器数据存在噪声干 扰 和 测 量 误 差,以 致 单 独 使 用 陀 螺 仪 与 加 速 度 计 不 能得到最优姿态角度的问题,建立陀螺仪和加速度计误差的数学模型,采 用 卡 尔 曼 滤 波 方 法,实 现 数 据 融 合,有 效地提高了姿态检测系统的检测精度.该方法被成功应用于四 旋 翼 飞 行 器 的 飞 行 姿 态 角 度 控 制 中,验 证 了 其 良 好 的噪声抑制能力,提高了系统对环境变化的适应性. 关 键 词:传感器融合;卡尔曼滤波;动态角度测量;飞行姿态控制 中图分类号:TP274.2 文献标志码:A 在四旋翼飞行器的飞行控制过程中,准确而实时地获得飞行器在空中的姿态角度,是决定控制精度和 系统稳定性的关键.尽管单一 MEMS传感器就可以单独进行姿态角度测量,但是其准确性主 要 取 决 于 惯 性器件的精度,单从改善硬件结构设计和工艺方面很难有大幅度的提高,并且系统误差会随时间积累,不 适用于长时间载体姿态的确定.所以,使用单一传感器难以得到相对真实的姿态角度.本文出于对姿态角 度测量准确性的考虑,采用对多传感器信号进行融合处理的方法,来获得最优姿态角度[1-2]. 基于多传感器的信息融合是 一 个 非 常 重 要 的 研 究 内 容,只 有 采 用 适 宜 的 融 合 方 法 才 能 达 到 最 好 的 效 果.文献[3]采用的加权平均法是一种简单、直观的融合方法,它将传感器信息进行加权平均的结果作为融 合值,适用于动态环境,但运算精度不高.文献[4]采用的神经网络方法,具有良好的非线性和有效的自学 习能力,但涉及到参数优化和结构模型的选择问题,其结构过于复杂或简单都会对融合精度产生影响. 本文针对由加速度计和陀螺仪建立的姿态角度测量系统,建立其特征模型,采用卡尔曼滤波方法,对 来自加速度计和陀螺仪的信号进行融合,解决噪声干扰与姿态最优估计问题,并将此方法应用于四旋翼飞 行器的角度测量系统进行验证性试验. 1 系统组成 本文所采用的姿态角度测控系统主要由加速度计、陀螺仪、微控制器、滤波电路、电机调速器、无刷电 机等部分组成.姿态检测系统的硬件平台如图1,由微处理器对陀螺仪、滤波电路和加速度计构成的传感 器组进行高速A/D 采样后,通过卡尔曼滤波器对传感器数据进行补偿和信息融合,得到准确的姿态角度信 号,此角度信号再通过PID 控制器运算,输出给电子调速器转换成PWM 信号,进而对电机进行控制. ① 收稿日期:2010 06 13 基金项目:国家大学生创新性实验计划资助项目(091063533);中央高校基本科研业务费专项资金重点资助项目(100030 2120131002). 作者简介:冯智勇(1987 ),男,山西人,本科,主要从事自动化专业方面的研究. 通信作者:黄 伟. 

831 西南师范大学学报(自然科学版) http://xbbjb.swu.cn 第36卷 加速度计 用 于 测 量 物 体 的 线 性 加 速 度,加 速 度计的输出 值 与 倾 角 呈 非 线 性 关 系,随 着 倾 角 的 增加而表现 为 正 弦 函 数 变 化.因 此 对 加 速 度 计 的 输出 进 行 反 正 弦 函 数 处 理, 才 能 得 到 其 倾 角 值[5-6].测量数据噪声与带宽的平方根成正比,即 噪声会随带宽的增加而增加. VNoise=350ug Bw ×1.5 图1 系统结构框图 式中:Bw 为传感器带宽(单位为 Hz).因此在设计卡尔曼滤波器时,首先要确定被测加速度的频率范围, 然后再设计滤波器的参数,尽量使滤波器的带宽略高于被测频率,这样不仅有助于滤除高频干扰,而且也 有利于降低系统噪声干扰.但是如果要得到精确的倾角值,带宽就需要设置得比较小,而这时加速度计动 态响应慢,不适合跟踪动态角度运动,如果期望快速的响应,又会引入较大的噪声.再加上其测量范围的 限制,使得单独应用加速度计检测飞行器倾角并不合适,需要与其他传感器共同使用. 陀螺仪的作用是用来测量角速度信号,通过对角速度积分,便能得到角度值.陀螺仪本身极易受噪声 干扰,微机械陀螺不能承受较大的震动,同时由于温度变化、不稳定力矩等因素,陀螺仪会产生漂移误差, 并会随着时间的推移而累加变大,通过积分会使得误差变得很大.因此,也不能单独使用陀螺仪作为本系 统的倾角传感器[7]. 2 卡尔曼滤波融合过程 首先建立系统的状态方程和测量方程.由于倾角和倾角角速度存在导数关系,系统倾斜真实角度φ可 以用来做一个状态向量.在该系统中,采用加速度计估计出陀螺仪常值偏差b,以此偏差作为状态向量得 到相应的状态方程和观测方程: φé ù ê ú ê úb ë û + é ê ê ë ù ú û ú1 0 ωgyro+ ù ú û wgé ê ú0 ê ë = ̇φ é ù ê ú ê úb ̇ ë û ì ï ï ï í ï ï φacce= [ ï î ê0 -1ù é ê ú ú0 0 û ë ]10 φé ù ú ê úb ê ë û +w a 式中,ωgyro为包含固定偏差的陀螺仪输出角速度,φacce为加速度计经处理后得到的角度值,wg 为陀螺仪测 量噪声,wa 为加速度计测量噪声,b为陀螺仪漂移误差,wg 与wa 相互独立,此处假设二者为满足正态分 布的白色噪声,令Ts 为系统采样周期,得到离散系统的状态方程和测量方程: -Tsù ú û ê1 ì é ï ú1 X(k-1)+ X(k)= ê ï 0 ë í ï ï ]10 X(k)+wa(k Vi(k)= [ ) î Tsé ù ê ú ê ë û ú0 ωgyro(k-1)+ wg(k)Ts ù é ê ú ê ú 0 ë û 同时,要估算k时刻的实际角度,就必须根据k-1时刻的角度值,再根据预测得到的k时刻的角度值 得到k时刻的高斯噪声的方差,在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值.在此, 须知道系统过程噪声协方差阵Q以及测量误差的协方差矩阵R,对卡尔曼滤波器进行校正.Q与R矩阵的 形式如下: Q= q_acceé 0 ê ê0 q_gy ë ù ú û úro R= r_[ ]acce 式中,q_acce和q_gyro分别是加速度计和陀螺仪测量的协方差,其数值代表卡尔曼滤波器对其传感器数据 的信任程度,值越小,表明信任程度越高.在该系统中陀螺仪的值更为接近准确值,因此取q_gyro的值小 

第4期 冯智勇,等:基于陀螺仪及加速度计信号融合的姿态角度测量 于q_acce的值.当前状态: 931 (1) X(k|k-1)=AX(k-1|k-1)+BU(k) ú0 ,X(k|k-1)是利用k预测的结果,X(k-1|k-1)是k-1时刻的最优 Tsé ù ê ú ê ë û ê1 -Tsù é ê ú ú1 0 ë û ,B= 式中,A= 结果. 则有对应于X(k|k-1)的协方差为: P(k|k-1)=AP(k-1|k-1)AT+Q (2) 式中,P(k-1|k-1)是X(k-1|k-1)对应的协方差,AT 表示A 的转置矩阵,Q是系统过程的协 方差.式子(1)、(2)即对系统的状态更新. 则状态k的最优化估算值X(k|k): K(k)=P(k|k-1)HT/(HP(k|k-1)HT+R) X(k|k)=X(k|k-1)+K(k)(Z(k)-HX(k|k-1)) (3) 其中 H=[1 0],K为卡尔曼增益(KalmanGain): (4) 此时,我们已经得到了k状态下最优的估算值X(k|k).但是为了使卡尔曼滤波器不断的运行下去直 到找到最优的角度值,我们还要更新k状态下X(k|k)的协方差: (5) ú1 其中,I为单位 阵,对 于 本 系 统 则 有,I= 1 .当 系 统 进 入k+1 状 态 时,P(k|k)就 是 式 子(2)的 P(k-1|k-1).(3)、(4)、(5)式为卡尔曼滤波器状态更新方程.计算完时间更新方程 和 测 量 更 新 方 程 后,再次重复上一次计算得到的后验估计,作为下一次计算的先验估计,这样,周而复始、循环反复地运算 下去直至找到最优的结果[8-10]. 3 实验验证 P(k|k)=(I-Kg(k)H)P(k|k-1) 为了验证卡尔曼滤波信号融合方法的有效性,构建验证系统如下: é ê ê ë ù ú û 表1 系统实验参数 微控制器 stm32f103zet6 陀螺仪 enc03 加速度计 mma7260 实验温度 室温27℃ 参数名 采样周期 参数值 0.02s A 1 -0.02 é ù êê úú 0 1 ë û 表2 滤波器初始化参数 B 10.02é ù úú0 êê û ë 0.01 0 ù é úú êê 0 0.005 û ë Q R [0.056] X0 úú0 ù é êê 0 ë û P0 0.005 0.005 é ù êê úú 0.005 0.005 ë û 如图2为加入外界干扰,即人为改变测量系统的初始角度时加速度计与陀螺仪输出波形,在外加干扰 的情况下陀螺仪能够测量出角速度值,但受噪声影响所测量值不准确且由于波动较大,积分之后误差信号 将被放大使得角度值不准确. 图3为引入卡尔曼滤波器后的波形曲线,加速度计成功地跟踪出陀螺仪的偏差,滤波输出曲线平滑滤 波效果明显,倾角估计的精度得到很大提高. 

041 西南师范大学学报(自然科学版) http://xbbjb.swu.cn 第36卷 图2 加入外界干扰时加速度计与陀螺仪输出波形 图3 引入卡尔曼滤波器后的波形曲线 4 结 论 本文研究了基于卡尔曼滤波的加速度计与陀螺仪的信号融合方法,在动态数据采集试验的基础上,通 过卡尔曼滤波有效地补偿传感器漂移与测量噪声等因素对加速度计与陀螺仪的影响,减小了姿态角度测量 误差,提高了运算精度,并将该方法成功应用于四旋翼飞行器的姿态测量系统.实验结果表明了该方法的 有效性,该方法适用于微小型机器人及飞行器中的姿态角度测量,并有很好的鲁棒性,系统更快速的收敛 性将是下一步研究的重点. 参考文献: [1] 高 嵩,潘 泉,肖秦琨.多传感器自适应滤波融合算法 [J].电子与信息学报,2008,30(8):1901-1904. [2] 金光明,张国良,陈林鹏.MEMS陀螺仪静态漂移模型与滤波方法研究 [J].传感器与微系统,2007,26(11):48-50. [3] 李媛媛,张立峰,多传感器自适应加权融合算法及其应用研究 [J].自动化与仪器仪表,2008(2):10-13. [4] 艾海舟,郝 放,刘文举.基于人工神经元网络的移动机器人导航研究 [J].机器人,1995,17(1):32-35. [5] 朱 弋,王振洲,杨舒波.MMA 系列加速度传感器的原理及其应用 [J].仪器原理,2008(6):97-98. [6] FREESCALESEMICONDUCTOR.±1.5~6gThreeAxisLow-gMicromachinedAccelerometer[EB/OL].(2008-03- 

第4期 冯智勇,等:基于陀螺仪及加速度计信号融合的姿态角度测量 141 05)[2009-09-10]http://cache.freescale.com/files/sensors/doc/data_sheet/MMA7260QT.pdf?fsrch=1&sr=1. [7] 吉训生,王寿荣,许宜申.自适应 Kalman滤波在 MEMS 陀 螺 仪 信 号 处 理 中 的 应 用 [J].传 感 器 与 微 系 统,2006(9): [8] KALMANRE.ANewApproachtoLinearFilteringandPredictionProblems[J].TransactionoftheASME-Journalof [9] 周道兵,骆 鹏,肖国强,等.利用Kalman滤波的视频运动目标跟踪 [J].西南师范大学学报:自然科学版,2009(6): BasicEngineering,1960,82(3):35-45. 79-85. 113-118. [10](美)凯.统计信号处理基础:估计与检测理论 [M].北京:电子工业出版社,2006. AngleMeasurementBasedonGyroscopeand AccelerometerSignalFusion FENGZhi-yong, ZENG Han, ZHANG Li, ZHAOYi-xin, HUANG Wei CollegeofComputerandInformationScience,SouthwestUniversity,Chongqing400715,China Abstract:Fortheproblemthatthequadrocoptercontrolusinggyroscopesandaccelerometersonlyexist noiseinterferenceandmeasurementerror,andcannotgettheoptimalattitudeangle.Tosolvethisprob- lem,mathematicalmodelofthegyroandaccelerometererrorisbuiltandthecollecteddataisfusioned usingthekalmanfiltermethod.Thiscaneffectivelyenhancetheprecisionoftheattitudeestimationsys- tem.Themethodisappliedinmicroprocessor,inthiswaytheauthorstestedtheattitudeangleofthe quadrocopter.Resultsshowthatsystemnoiseiswellreducedusingthismethodandithasgoodenviron- mentaladaptability,whichprovestheapplicabilityofthemethod. Keywords:signalfusion;Kalmanfilter;dynamicanglemeasurement;flightattitudecontrol 责任编辑 汤振金