卡尔曼滤波在四旋翼飞行器姿态测量中的应用

2011-01 兵工自动化 30(1) Ordnance Industry Automation ·73· doi: 10.3969/j.issn.1006-1576.2011.01.022 卡尔曼滤波在四旋翼飞行器姿态测量中的应用 王帅，魏国 （哈尔滨工业大学 电气学院，黑龙江 哈尔滨 150001） 摘要：针对简单均值滤波很难满足要求的不足，介绍一种四旋翼飞行器系统飞行姿态测量的新算法。在使用传 统的加速度传感器获取数据的基础上，使用卡尔曼滤波的方法，将加速度传感器数据和陀螺仪数据进行融合，以单 片机为核心的惯性测量单元进行试验。结果表明，该算法能避免动态噪声对加速度传感器数据的影响，提高四旋翼 飞行器姿态测量的精度。 关键词：卡尔曼滤波；四旋翼飞行器；姿态测量 中图分类号：TP206 文献标志码：A Application of Kalman Filter in Attitude Measurement for Four-Rotor Aircraft Flight Wang Shuai, Wei Guo (School of Electrical, Harbin Institute of Technology, Harbin 150001, China) Abstract: Introduce a new algorithm used in a four-rotor aircraft flight attitude measurement system, according to the shortage of simple average filter. On the basis of traditional method of accessing data use acceleration sensors, the fusion with acceleration sensor data and gyroscopes data by Kalman filter, testing in inertial measurement unit with MCU. The results show that the method can avoid dynamic noise of acceleration sensor, and improve the accuracy of attitude measurement of quadrotor helicopter. Keywords: Kalman filter; quadrotor helicopter; attitude measurement 0 引言 四旋翼飞行器是一种垂直起降机，是固联在刚 性十字交叉结构上，由 4 个独立电机驱动的螺旋桨 组成的 6 自由度系统[1-3]。尽管有 4 个驱动，但因为 四旋翼飞行器具有 6 个坐标输出，所以仍然是欠驱 动和动力不稳定的系统。为了控制飞行器在飞行过 程中的稳定性，需要实时的了解飞行器的当前姿态， 并调整驱动电机的转速。飞行器姿态角（俯仰角和 横滚角）在静态时可以通过加速度传感器计算，但 飞行时，加速度传感器的数据会受到非重力加速度 的影响，使计算不准确[4]。 卡尔曼滤波是一个最优化自回归数据处理算 法。近年来被广泛应用于计算机图像处理方面 [5]。 在四旋翼飞行系统中，需要对陀螺仪和加速度传感 器所测得的数据进行综合和校正。如果使用简单的 均值滤波，很难满足精度和实时性上的要求。故通 过卡尔曼滤波将加速度传感器和陀螺仪的数据进行 融合，在计算四旋翼飞行器的实时姿态时很好地抑 制了噪声的干扰作用，以提高测量精度，为自主飞 行控制创造了条件。 1 建立飞行器姿态模型 1.1 线性系统模型描述 线性系统的模型可以描述为： 状态方程： 1k k k kx Ax Bu w+ = + + (1) 输出方程： k k ky Cx z= + (2) 在上述方程中，A、B和 C均为矩阵，k 是时间 系数，x 称为系统状态，u 是系统的已知输入，y 是 所测量的输出，w 和 z 表示噪声。其中，变量 w 称 为进程噪声，z 称为测量噪声，它们都是向量，因 此包含多个元素。x 中包含系统当前状态的所有资 讯，但不能被直接测量。因此，要测量向量 y，y 是受到噪声 z 干扰的系统状态 x 的函数[6]。 1.2 姿态模型 使用陀螺仪数据建立四轴飞行器姿态的线性模 型如下： ( )1k k k ku dtα α β+ = + − (3) 其中，α 是飞行姿态角（俯仰角或横滚角），β 收稿日期：2010-06-12；修回日期：2010-09-09 项目基金：国家大学生科技创新性实验 计划 项目进度计划表范例计划下载计划下载计划下载课程教学计划下载 项目“用于危险区域物品清理的四旋翼飞行抓捕手”（091021326） 作者简介：王帅（1989-），男，黑龙江人，本科，从事测控技术与仪器研究。 兵工自动化 ·74· 第 30 卷 是陀螺仪的误差， ku 是陀螺仪输出的角速度数据， dt 是采样时间间隔。 假设陀螺误差为常量，即 1k kβ β+ = (4) 结合式 (3)、式 (4) 得到状态矩阵方程： 1 1 0 1 0 kk k dt dt u α α β β+ −⎛ ⎞ ⎛ ⎞⎛ ⎞ ⎛ ⎞= +⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠⎝ ⎠ ⎝ ⎠ (5) 取 x α β ⎛ ⎞= ⎜ ⎟⎝ ⎠， 1 0 1 dt A −⎛ ⎞= ⎜ ⎟⎝ ⎠， 0 dt B ⎛ ⎞= ⎜ ⎟⎝ ⎠ 得到系统状态方程 1k k kx Ax Bu+ = + (6) 2 卡尔曼滤波算法 算法具体的实现步骤为： 1) 读取当前陀螺仪数据 u； 2) 由陀螺仪数据更新系统的状态预测 est stax Ax Bu= + (7) 3) 读取由加速度传感器计算的转角数据 y； 4) 计算测量过程的革新 estInn y Cx= − (8) 其中 ( )1 0C = 5) 计算协方差 ' zs CPC S= + (9) 其中， zS 为测量噪声协方差 ( )Tz k kS E z z= (10) 6) 计算卡尔曼增益 ' 1K APC s−= (11) 7) 更新系统的状态估计 sta estx x K Inn= + • (12) 8) 计算预测误差的协方差 ' ' wP APA K CPA S= − • + (13) 其中， wS 为进程噪声协方差矩阵。 ( )TwS E x x= (14) 即 [ ] [ ]( )'wS E α β α β= • (15) 9) 等待采样间隔 dt ，返回步骤 1)。 以往在使用卡尔曼滤波的过程中，一般认为它 是一个固定值，但事实上测量噪声协方差是随着当 前飞行器的状态在不断变化的。因此，笔者在算法 的 设计 领导形象设计圆作业设计ao工艺污水处理厂设计附属工程施工组织设计清扫机器人结构设计 上根据每一段时间内加速度传感器测得的数 据相应的改变测量噪声协方差，再将这些值分别带 入到卡尔曼滤波算法中，以进一步减小误差。 3 试验与结果 实验使用三轴加速度传感器 MMA7260，单轴 陀螺仪 EWTS82 和 AVR32 单片机 UC3B0256 组成 姿态测量单元。通过单片机采集传感器数据，并运 行卡尔曼滤波算法进行数据融合。 通过单片机定时器设定采样间隔为 dt =0.01 s， 卡尔曼滤波的初始参数为： 1 0.01 0 1 A −⎛ ⎞= ⎜ ⎟⎝ ⎠ ， 0.01 0 B ⎛ ⎞= ⎜ ⎟⎝ ⎠ ， ( )1 0C = ， 0.005 0.005 0.005 0.005w S ⎛ ⎞= ⎜ ⎟⎝ ⎠ ， 0 0sta x ⎛ ⎞= ⎜ ⎟⎝ ⎠ ， 0.005 0.005 0.005 0.005 P ⎛ ⎞= ⎜ ⎟⎝ ⎠ 。 分别进行静态和动态的测量。搭建一个实验台， 使模型以一定的角度倾斜放置在实验台上。在进行 静态测量时，把测量单元在横滚方向倾斜 9°，并保 持静止。对比由加速度数据计算的倾角和经卡尔曼 滤波的倾角如图 1、图 2。图中，纵坐标为测试所得 倾角，横坐标为间隔 0.01 s 的采样点数。可以看出， 静态时 2 条线基本吻合，卡尔曼滤波的噪声较小。 图 1 静态时由加速度计算的倾角 图 2 静态时由卡尔曼滤波计算的倾角 在使整个系统处于悬空状态并且保持电机转动 的情况下，使系统与水平面保持一定的倾角，可以 在此状态下完成动态测量。在动态测量中，维持横 滚角为 9°，并在其垂直方向施加一低频振动，对比 由加速度数据计算的倾角和经卡尔曼滤波的倾角如 图 3、图 4。可以看出，动态时加速度计算的倾角受 非重力加速度的影响很大，而卡尔曼滤波的效果明 显，对噪声起到了有效的抑制作用。 （下转第 80 页） 兵工自动化 ·80· 第 30 卷 图 6 报警数据详细 资料 新概念英语资料下载李居明饿命改运学pdf成本会计期末资料社会工作导论资料工程结算所需资料清单 界面 图 7 故障逻辑判决示意图 通常，如果满足第 1 条规则，测量值略超出门限， 而其他 7 条规则未满足，可认定为 检测 工程第三方检测合同工程防雷检测合同植筋拉拔检测方案传感器技术课后答案检测机构通用要求培训 系统虚警。如 果不满足第 1 条规则，偶尔满足第 2～8 条规则中的 某 1 条，可判定被测设备正常。如果不满足第 1 条规 则，但同时满足其它多条规则或某条规则持续满足， 即可判定被测设备存在潜在故障。如果满足第 1 条规 则，也满足其它某条规则，则可判定发动机设备已发 生功能故障，需要停机检查，如图 7。 4 结束语 该系统可大幅降低报警系统的虚警率和误警 率，还可及时发现导弹发动机的功能故障潜在故障， 有效避免或降低故障发生所带来的损失。 参考文献： [1] 张公绪 , 何国伟 , 郑慧英 . 新编质量管理学[M]. 北京 : 高等教育出版社, 2003: 71-79. [2] 王飞 , 李保华 , 冯伟 , 等 . 统计过程控制在无人机故障 预报系统中的应用 [J]. 中国测试技术 , 2007, 33(1): 23-25. [3] 童国强 . 基于数据融合技术的多模型状态监测与故障 预报[D]. 南京: 南京航空航天大学, 2005. [4] 崔吉俊 . 火箭导弹测试技术 [M]. 北京 : 国防工业出版 社, 1999. [5] 高明皓 , 王林斗 , 匡俊华 , 等 . 家庭报警信号处理系统 [J]. 电子测量技术. 2006, 29(2): 143-144. [6] 杨世杰 . 动态测试数据中坏点处理的一种新方法-绝对 均值法及应用研究 [J]. 中国测试技术 , 2006, 32(1): 47-49. ********************************************************************************************************* （上接第 74 页） 图 3 动态时由加速度计算的倾角 图 4 动态时由卡尔曼滤波计算的倾角 4 结论 该算法通过使用卡尔曼滤波算法对加速度传感 器数据和陀螺仪数据进行融合，实时地改变测量噪 声协方差的值，避免了动态噪声对加速度传感器的 影响，提高了飞行器姿态测量的精度，为飞行器的 自主飞行控制提供了可靠的姿态反馈，也为四旋翼 飞行器系统姿态的进一步调整创造了条件，具有一 定的应用价值。 参考文献： [1] Bora Erginer, Erdinc Altug. Modeling and PD Control of a Quadrotor VTOL Vehicle[C]. Proceeding of 2007 IEEE Intelligent Vehicles Symposium Istanbul, Turkey, 2007, 894-899. [2] 杨明志 , 王敏 . 四旋翼微型飞行器控制系统设计[J]. 计 算机测量与控制, 2008, 16(4): 485-487. [3] 聂博文 , 马宏绪 , 王剑 , 等 . 微小型四旋翼飞行器的研 究 现 状 与 关 键 技 术 [J]. 电 光 与 控 制 , 2007, 14(6): 113-117. [4] 陈福深 . 伺服加速度计用于倾角测量的抗振动试验研 究[C]. 中国惯性技术学会光电技术专业委员会第四次 学术交流会暨重庆惯性技术学会第八次学术交流会论 文集, 2000. [5] 李艳芳 , 张佐理 , 郑可飙 . 动态系统运动状态最优化估 计研究[J]. 计算机工程与设计, 2009, 30(2): 453-464, [6] 周瑞雄 , 陈梧桐 , 陈春明 , 等 . Kalman Predictor and Mutitarget Tracking Algorithm[J]. 中华技术学院学报 , 2003, 5(26): 22-36.