) o l .36 N o . 4 J o u r n a l o f S o u t h w e s t C h i n aN o r m a lU n i v e r s i t N a t u r a l S c i e n c eE d i t i o n y (
() 文章编号:[***********]05
第36卷 第4期
西南师范大学学报(自然科学版)
2011年8月
A u . 2011g
基于陀螺仪及加速度计信号融合的姿态角度测量
冯智勇, 曾 瀚, 张 力, 赵亦欣, 黄 伟
西南大学计算机与信息科学学院, 重庆400715
①
摘要:针对在四旋翼飞行器姿态控制中传感器数据存在噪声干扰和测量误差, 以致单独使用陀螺仪与加速度计不能得到最优姿态角度的问题, 建立陀螺仪和加速度计误差的数学模型, 采用卡尔曼滤波方法, 实现数据融合, 有效地提高了姿态检测系统的检测精度. 该方法被成功应用于四旋翼飞行器的飞行姿态角度控制中, 验证了其良好的噪声抑制能力, 提高了系统对环境变化的适应性. 中图分类号:T P 274. 2
关 键 词:传感器融合; 卡尔曼滤波; 动态角度测量; 飞行姿态控制
文献标志码:A
在四旋翼飞行器的飞行控制过程中, 准确而实时地获得飞行器在空中的姿态角度, 是决定控制精度和系统稳定性的关键. 尽管单一M E M S 传感器就可以单独进行姿态角度测量, 但是其准确性主要取决于惯性器件的精度, 单从改善硬件结构设计和工艺方面很难有大幅度的提高, 并且系统误差会随时间积累, 不
]1-2
度测量准确性的考虑, 采用对多传感器信号进行融合处理的方法, 来获得最优姿态角度[.
适用于长时间载体姿态的确定. 所以, 使用单一传感器难以得到相对真实的姿态角度. 本文出于对姿态角
基于多传感器的信息融合是一个非常重要的研究内容, 只有采用适宜的融合方法才能达到最好的效
]果. 文献[采用的加权平均法是一种简单㊁ 直观的融合方法, 它将传感器信息进行加权平均的结果作为融3]合值, 适用于动态环境, 但运算精度不高. 文献[采用的神经网络方法, 具有良好的非线性和有效的自学4习能力, 但涉及到参数优化和结构模型的选择问题, 其结构过于复杂或简单都会对融合精度产生影响.
本文针对由加速度计和陀螺仪建立的姿态角度测量系统, 建立其特征模型, 采用卡尔曼滤波方法, 对
来自加速度计和陀螺仪的信号进行融合, 解决噪声干扰与姿态最优估计问题, 并将此方法应用于四旋翼飞行器的角度测量系统进行验证性试验.
1 系统组成
本文所采用的姿态角度测控系统主要由加速度计㊁ 陀螺仪㊁ 微控制器㊁ 滤波电路㊁ 电机调速器㊁ 无刷电机等部分组成. 姿态检测系统的硬件平台如图1, 由微处理器对陀螺仪㊁ 滤波电路和加速度计构成的传感/器组进行高速A D 采样后, 通过卡尔曼滤波器对传感器数据进行补偿和信息融合, 得到准确的姿态角度信号, 此角度信号再通过P I D 控制器运算, 输出给电子调速器转换成P WM 信号, 进而对电机进行控制.
①
收稿日期:20100613
) ; 中央高校基本科研业务费专项资金重点资助项目() 基金项目:国家大学生创新性实验计划资助项目([***********]0131002.
, 男, 山西人, 本科, 主要从事自动化专业方面的研究. 作者简介:冯智勇(1987) 通信作者:黄 伟.
138
测量倾角
://西南师范大学学报(自然科学版) h t t x b b b . s w u . c n 第36卷p j
于量的性度, 加速与角非线关着角的正函变化对速计的输出行正函处, 才得其角
]5-6值. 测量数据噪声与带宽的平方根成正比, 即
噪声会随带宽的增加而增加.
) . 因此在设计卡尔曼滤波器时, 首先要确定被测加速度的频率范围, 式中:单位为H B w 为传感器带宽(z 然后再设计滤波器的参数, 尽量使滤波器的带宽略高于被测频率, 这样不仅有助于滤除高频干扰, 而且也有利于降低系统噪声干扰. 但是如果要得到精确的倾角值, 带宽就需要设置得比较小, 而这时加速度计动态响应慢, 不适合跟踪动态角度运动, 如果期望快速的响应, 又会引入较大的噪声. 再加上其测量范围的限制, 使得单独应用加速度计检测飞行器倾角并不合适, 需要与其他传感器共同使用.
测量角速度
V N o 350u g w i s e =
图1 系统结构框图
干扰, 微机械陀螺不能承受较大的震动, 同时由于温度变化㊁ 不稳定力矩等因素, 陀螺仪会产生漂移误差, 并会随着时间的推移而累加变大, 通过积分会使得误差变得很大. 因此, 也不能单独使用陀螺仪作为本系
7]统的倾角传感器[.
陀螺仪的作用是用来测量角速度信号, 通过对角速度积分, 便能得到角度值. 陀螺仪本身极易受噪声
2 卡尔曼滤波融合过程
, 以此偏差作为状态向量得以用来做一个状态向量. 在该系统中, 采用加速度计估计出陀螺仪常值偏差b 到相应的状态方程和观测方程:
首先建立系统的状态方程和测量方程. 由于倾角和倾角角速度存在导数关系, 系统倾斜真实角度可
陀螺仪
加速度计
̇ìφùïéêú=
ïê̇b úûïëíïïa
c e =φc ïî
01é
ê-ùúê00úëû
éφùêú+
êb úëû
éw g ù1éêúêùúωr o +g y
êúêú0ûëë0û
éφùú+w a
[10]êêb úëû
式中, ωw g 为陀螺仪测r o 为包含固定偏差的陀螺仪输出角速度, a c c e 为加速度计经处理后得到的角度值, g y φ量噪声, w a 为加速度计测量噪声, b 为陀螺仪漂移误差, w g 与w a 相互独立, 此处假设二者为满足正态分布的白色噪声, 令T s 为系统采样周期, 得到离散系统的状态方程和测量方程:
ì1T s úï() éê-ù) X k X (k -1=+êúï01ëûí
ï
ï() []() îV i k =10X k +w a (k )
w g (k ) T s ùéT s úéêúêù() ωk 1-+r o g y
êúêú0ûëëû0
同时, 要估算k 时刻的实际角度, 就必须根据k -1时刻的角度值, 再根据预测得到的k 时刻的角度值须知道系统过程噪声协方差阵以及测量误差的协方差矩阵对卡尔曼滤波器进行校正. Q 与R 矩阵的形式如下:
得到k 时刻的高斯噪声的方差, 在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值. 在此,
式中, a c c e 和q _r o 分别是加速度计和陀螺仪测量的协方差, 其数值代表卡尔曼滤波器对其传感器数据q _g y 的信任程度, 值越小, 表明信任程度越高. 在该系统中陀螺仪的值更为接近准确值, 因此取q _g r o 的值小y
a c c e 0éùq _êú R =[r _a c c e ]Q =êú_0r o ûëq g y
第4期 冯智勇, 等:基于陀螺仪及加速度计信号融合的姿态角度测量于q _a c c e 的值. 当前状态:
) ) X (k |k -1X (k -1|k -1U (k ) =A +B
139() 1
1-T s ùT s ùééú, ê, (k -1) ) 式中, 是利用k 预测的结果, 是k -1时刻的最优A =êB X (k -1k -1=êú|êúúX k |
010ûëûë结果.
) 的协方差为:则有对应于X (k |k -1
) ) 是X (对应的协方差, 式中, P (k -1k -1k -1k -1A T 表示A 的转置矩阵, ||Q 是系统过程的协) ㊁ () 即对系统的状态更新. 方差. 式子(12
:则状态k 的最优化估算值X (k |k )
) ) P (k |k -1P (k -1|k -1A T +Q =A
() 2
], ) :其中H =[1 0K 为卡尔曼增益(K a l m a nG a i n
) () ) X (k |k ) k |k -1k ) Z (k ) k |k -1=X (+K (-H X (
H 为系统输出矩阵
() 3() 4
此时, 我们已经得到了k 状态下最优的估算值X (k |k ) . 但是为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值, 我们还要更新k 状态下X (k |k ) 的协方差:
) P (k |k ) I -K k ) H ) P (k |k -1=(g (
() K (k ) k |k -1T /H P (k |k -1H T R ) =P (
) ) ㊁ () ㊁ () 式为卡尔曼滤波器状态更新方程. 计算完时间更新方程和测量更新方程P (k -1|k -1. (345后, 再次重复上一次计算得到的后验估计, 作为下一次计算的先验估计, 这样, 周而复始㊁ 循环反复地运算
810]-下去直至找到最优的结果[.
1ùéú. 当系统进入k +1状态时, P () 的其中, I 为单位阵, 对于本系统则有, I =êk |k ) 就是式子(2ê1úëû
() 5
3 实验验证
为了验证卡尔曼滤波信号融合方法的有效性, 构建验证系统如下:
表1 系统实验参数
微控制器s t m 32f 103z e t 6
陀螺仪e n c 03
加速度计mm a 7260
实验温度室温27ħ
表2 滤波器初始化参数
参数名
采样周期
A 1-0. 02ùéêúêú
ë01û
B 10. 02ùéêúêú
ë0û
Q 0. 010ùéêúêúë00. 005û
R []0. 056
X 00ùéêêúú
ë0û
P 0
0. 0050. 005ùéêúêúë0. 0050. 005û
参数值
0. 02s
如图2为加入外界干扰, 即人为改变测量系统的初始角度时加速度计与陀螺仪输出波形, 在外加干扰的情况下陀螺仪能够测量出角速度值, 但受噪声影响所测量值不准确且由于波动较大, 积分之后误差信号将被放大使得角度值不准确.
图3为引入卡尔曼滤波器后的波形曲线, 加速度计成功地跟踪出陀螺仪的偏差, 滤波输出曲线平滑滤
波效果明显, 倾角估计的精度得到很大提高.
140://西南师范大学学报(自然科学版) h t t x b b b . s w u . c n 第36卷p j
图2 加入外界干扰时加速度计与陀螺仪输出波形
图3 引入卡尔曼滤波器后的波形曲线
4 结 论
本文研究了基于卡尔曼滤波的加速度计与陀螺仪的信号融合方法, 在动态数据采集试验的基础上, 通
过卡尔曼滤波有效地补偿传感器漂移与测量噪声等因素对加速度计与陀螺仪的影响, 减小了姿态角度测量误差, 提高了运算精度, 并将该方法成功应用于四旋翼飞行器的姿态测量系统. 实验结果表明了该方法的有效性, 该方法适用于微小型机器人及飞行器中的姿态角度测量, 并有很好的鲁棒性, 系统更快速的收敛性将是下一步研究的重点. 参考文献:
[]高 嵩, 潘 泉, 肖秦琨. 多传感器自适应滤波融合算法[]() :1J . 电子与信息学报, 2008, 3081901-1904. []]() :3 李媛媛, 张立峰, 多传感器自适应加权融合算法及其应用研究[J . 自动化与仪器仪表, 2008210-13. []]() :5 朱 弋, 王振洲, 杨舒波.MMA 系列加速度传感器的原理及其应用[J . 仪器原理, 2008697-98.
[]]() :2 金光明, 张国良, 陈林鹏. M E M S 陀螺仪静态漂移模型与滤波方法研究[J . 传感器与微系统, 2007, 261148-50. []]() :机器人, 4 艾海舟, 郝 放, 刘文举. 基于人工神经元网络的移动机器人导航研究[J . 1995, 17132-35.
[]/6 FR E E S C A L ES E M I C O N D U C T O R.ʃ 1. 5~6g T h r e eA x i sL o w i c r o m a c h i n e dA c c e l e r o m e t e r [E B O L ]. (2008-03-g M -
第4期 冯智勇, 等:基于陀螺仪及加速度计信号融合的姿态角度测量
) []://////_/MM A ? f 052009-09-10h t t c a c h e . f r e e s c a l e . c o m f i l e s s e n s o r s d o c d a t a s h e e t 7260Q T . d f s r c h =1&s r =1. p p 79-85.
141
[]]:7 吉训生, 王寿荣, 许宜申. 自适应K a l m a n 滤波在M E M S 陀螺仪信号处理中的应用[J . 传感器与微系统, 2006(9) []]. 8 KA L MA NRE . A N e w A r o a c h t oL i n e a r F i l t e r i n n dP r e d i c t i o nP r o b l e m s [J T r a n s a c t i o n o f t h eA S M E -J o u r n a l o f p p g a []]() :9 周道兵, 骆 鹏, 肖国强, 等. 利用K a l m a n 滤波的视频运动目标跟踪[J . 西南师范大学学报:自然科学版, 20096[](美) 凯. 统计信号处理基础:估计与检测理论[10M ]. 北京:电子工业出版社, 2006.
113-118.
, () :B a s i cE n i n e e r i n 1960, 82335-45. g g
b s t r a c t :F o r h e o m i s e , i m n a t t e h r f e e m r e a n t t i c e s i n g t h ek a l m a c n u m. a d r o T c h o em p t e r e t . h R o m e n t a l a d a p t a b i l i e e y w
o r d s :s i g n a A n g l eM e A a c s c u e l r e e r m o e m n e t t e B r a s S e i d o nG g
n a l F u y s r i o o s n c o p
e a n d F E N GZ h Z i -HA y o n O g , Y i -
x Z i E n N , G H HUa A n N , G W Z HA e i N G Li , o l l e g eo f C o m p u t e r a n d I n f o r m a t i o nS c i e n c e , S o u t h w e s t U n i v e r s i t y , C h o n g q i n g 4
00715, C h i n a p n r dm o b l e mt h a t t h e q u a d r o c o p t e rc o n t r o lu s i n gg y r o s c o p e sa n da c c e l e r o o d e e l a s o f u r t e h m e e g n y
t r e oa r r o n r da , a c n c d e l c e a r nn o m o e t t e g re e t r t r h o e r o i sb p t i m u i a l l ta a t n t i dt t u d h e ec a n o g l l l e e c . t e T i l t e rm e t h o d . T h i s c dd u i sa l t s p p l i e d i n m i c r o p r a o ne c e s f f s e o c r t i , v i e nt l y h e i n sw h a n a c y e t t h h ea e p u r e t c h i o s r i s o no t e s f t e t dt h e h a t ea t i t t u t i d , w h s i h c o h wt p r o h v a t e s s y t s h t e e a mn p p l o i c i s a e b i i l sw i t y e o
l f l t r h e em d u c e e du t h o s d i n . g t h i sm e t h o da n d i t h u s i o n ; K a l m a n f i l t e r ; d y n a m i c a n g l em e a s u r e m e n t ; f l i g h t a t t i t u d e c o 责任编辑 t e r so n l y e
x i s t a o t l a v e i s t h f i u s s i p o r n o e b d -d e s ea t i m a t i o ns y s g o o n de g l eo n v f -
i r t o h n e -o l 汤振金
C l a lm n e a o s u f e A e m e t d t u t y s a s q K l f n t r