１０８ 传感器与微系统（Ｔｒａｎｓｄｕｃｅｒ ａｎｄ Ｍｉｃｒｏｓｙｓｔｅｍ Ｔｅｃｈｎｏｌｏｇｉｅｓ） ２０１３年第３２卷第８期 基于ＳＴＭ３２的无人机姿态测量系统设计 刘辉邦，褚金奎，支炜，李晓雨 （大连理工大学精密特种７３￣－ｒ教育部重点实验室，辽宁大连１１６０２３） 摘要：设计一种基于ＳＴＭ３２的无人机姿态测量系统。该系统以新型的ＡＲＭ Ｃｏｄｅｘ—Ｍ３内核微处理器 ｓＴＭ３２Ｆ１Ｏ３ｚＥＴ６作为处理单元，结合ＭＥＭＳ数字陀螺仪和ＭＥＭＳ数字加速度计设计而成，采用一种简单 有效的互补滤波算法进行姿态解算，有效地提高了姿态测量的精度。实验测试表明：该姿态测量系统简单 实用，性能稳定，精度高，能够满足一般无人机姿态测量的要求。 关键词：ＳＴＭ３２；姿态测量系统；姿态解算；互补滤波 中图分类号：Ｖ２４９ 文献标识码：Ａ 文章编号：１０００－－９７８７（２０１３）０８－０１０８－０３ Ｄｅｓｉｇｎ ｏｆ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ ｆｏｒ ＵＡＶ ｂａｓｅｄ ｏｎ ＳＴＭ３２ ＬＩＵ Ｈｕｉ－ｂａｎｇ，ＣＨＵ Ｊｉｎ—ｋｕｉ，ＺＨＩ Ｗｅｉ，ＬＩ Ｘｉａｏ—ｙｕ （Ｋｅｙ Ｌａｂｏｒａｔｏｒｙ ｏｆ Ｔｅｃｈｎｏｌｏｇｙ Ｐｒｅｃｉｓｉｏｎ＆Ｎｏｎ－ｔｒａｄｉｔｉｏｎａｌ Ｍａｃｈｉｎｉｎｇ，Ｍｉｎｉｓｔｒｙ ｏｆ Ｅｄｕｃａｔｉｏｎ， Ｄａｌｉａｎ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｔｅｃｈｎｏｌｏｇｙ，Ｄａｌｉａｎ １１６０２３，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ａｎ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ ｆｏｒ ＵＡＶ ｂａｓｅｄ ｏｎ ＳＴＭ３２ ｉｓ ｐｒｅｓｅｎｔｅｄ．Ｔｈｅ ｓｙｓｔｅｍ ｕｓｅｓ ｎｏｖｅｌ ＡＲＭ Ｃｏｒｔｅｘ－Ｍ３ ｍｉｃｒｏｐｒｏｃｅｓｓｏｒ ＳＴＭ３２Ｆ１０３ＺＥＴ６ ａｓ ｐｒｏｃｅｓｓｉｎｇ ｕｎｉｔ，ｉｔ ｉｓ ｄｅｓｉｇｎｅｄ ｃｏｍｂｉｎｉｎｇ ＭＥＭＳ ｄｉｇｉｔａｌ ｇｙｒｏｓｃｏｐｅ ｗｉｔｈ ａｃｃｅｌｅｒｏｍｅｔｅｒ，ａ ｓｉｍｐｌｅ ａｎｄ ｅｆｆｅｃｔｉｖｅ ｃｏｍｐｌｅｍｅｎｔａｒｙ ｆｉｈｅｒｉｎｇ ａｌｇｏｒｉｔｈｍ ｉｓ ａｐｐｌｉｅｄ ｔｏ ｃａｌｃｕｌａｔｅ ａｔｔｉｔｕｄｅ，ｗｈｉｃｈ ｉｍｐｒｏｖｅｓ ｔｈｅ ｐｒｅｃｉｓｉｏｎ ｏｆ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｅｆｆｅｃｔｉｖｅｌｙ．Ｔｈｅ ｅｘｐｅｒｉｍｅｎｔａｌ ｔｅｓｔｓ ｓｈｏｗ ｔｈａｔ ｔｈｅ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ ｉｓ ｓｉｍｐｌｅ，ｐｒａｃｔｉｃａｌ，ｉｔ ｈａｓ ｓｔａｂｌｅ ｐｒｏｐｅｒｔｙ ａｎｄ ｈｉｇｈ ｐｒｅｃｉｓｉｏｎ，ａｎｄ ｉｓ ａｂｌｅ ｔｏ ｍｅｅｔ ｔｈｅ ｒｅｑｕｉｒｅｍｅｎｔｓ ｆｏｒ ｇｅｎｅｒａｌ ＵＡＶ ａｔｔｉｔｕｄｅ Ｋｅｙ ｗｏｒｄｓ：ＳＴＭ３２；ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ；ａｔｔｉｔｕｄｅ ｃａｌｃｕｌａｔｉｏｎ；ｃｏｍｐｌｅｍｅｎｔａｒｙ ｆｉｌｔｅｒｉｎｇ ０引 言 姿态测量的需求。 １硬件设计 姿态测量系统是无人机飞行控制系统中的一个重要组 成部分，为飞行控制系统提供机体当前的横滚角和俯仰角， 以引导控制系统对飞机姿态进行精确的控制。使用模拟调 本文所设计的无人机姿态测量系统是无人机飞行控制 系统的重要组成部分之一。无人机飞行控制系统硬件平台 平技术的传统框架式姿态测量系统质量重、体积大、可靠性 低、维修成本高，已经不能满足新的应用需求 。随着微电 子技术、集成电路技术和加工工艺的发展，采用微机电系统 如图ｌ，主要由姿态传感器、气压传感器、ＧＰＳ、处理器、执行 机构、遥控接收机、电台等部分组成。姿态测量系统如 图２，其中，姿态传感器由三轴陀螺仪和三轴加速度计构 成，处理器通过Ｉ Ｃ（ｉｎｔｅｒ—ｉｎｔｅｇｒａｔｅｄ ｃｉｒｃｕｉｔ）总线，读取陀螺 仪和加速度计数据，采用互补滤波算法进行姿态解算，得到 准确的姿态角，为飞行控制系统的姿态控制提供依据。 （ＭＥＭＳ）技术的惯性传感器由于体积小、质量轻、成本低、抗 冲击能力强、产品可靠性高等优异特点，已大量应用于汽车、 航空、航天、武器制导等军民领域 。综上，设计一种以新型 的ＡＲＭ Ｃｏｒｔｅｘ－Ｍ３内核微处理器ＳＴＭ３２Ｆ１０３ＺＥＴ６作为处理 单元，ＭＥＭＳ数字陀螺仪和加速度计作为姿态传感器的姿 态测量系统，并采用了一种简单有效的互补滤波算法对飞 机姿态进行解算，该算法较传统卡尔曼滤波算法具有响应 速度快，计算量少，并且不需要精确已知系统模型和噪声方 差等优点，有效地提高了姿态测量的精度，满足一般无人机 收稿日期：２０１３－０１—１６ 图１飞控系统结构框图 Ｆｉｇ １ Ｓｔｒｕｃｔｕｒｅ ｂｌｏｃｋ ｄｉａｇｒａｍ ｏｆ ｌｉｆｇｈｔ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ￥基金项目：国家重点基础研究发展规划项目（２０１１ＣＢ３０２１０１，２０１１ＣＢ３０２１０５）；国家自然科学基金资助项目（５１１７５０５６） 第８期 刘辉邦，等：基于ＳＴＭ３２的无人机姿态测量系统设计 ｌ０９ 图２姿态测量系统硬件框图 Ｆｉｇ ２ Ｈａｒｄｗａｒｅ ｂｌｏｃｋ ｄｉａｇｒａｍ ｏｆ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ １．１处理器模块 处理器模块是整个控制系统的核心，主要任务是采集 各个传感器的数据，对数据进行处理并解算出飞机的姿态， 从而实现对飞机姿态和航向的精确控制。本系统采用意法 半导体公司的ＳＴＭ３２Ｆ１０３ＺＥＴ６型微处理器作为主控模块， 该微型处理器以新型ＡＲＭ Ｃｏｄｅｘ—Ｍ３为内核，在保证高性 能运算的同时，保持了较低的功耗，并具有多个定时器，可 以同时产生多路ＰＷＭ信号以驱动各种执行机构，提供了丰 富的外设资源，方便系统的开发和扩展。 １．２姿态传感器模块 姿态传感器模块实时监测无人机姿态，并将测得的数 据发送到主控模块进行解算。姿态传感器模块由三轴 ＭＥＭＳ数字陀螺仪和三轴ＭＥＭＳ数字加速计构成。 陀螺仪采用意法半导体的一种三轴数字陀螺仪 Ｌ３Ｇ４２００，在￣２５０ ｄｐｓ量程下精度可达８．７５ ｍｄｐｓ／ｄｉｇｉｔ，具 有Ｉ Ｃ／ＳＰＩ数字输出接口，片上集成了ＦＩＦＯ存储器模块，最 多可存储９６个量值，主处理器不必连续读取传感器的角速 率数据，可实现智能化程度更高的电源管理。 加速度传感器采用ＡＤＩ公司的ＡＤＸＩ２，４５，最大可感知 ＋１６ ｇ 的加速度，感应精度可达４ ｍｇ ／ＬＳＢ，倾角测量典型 误差小于１。，提供ＳＰＩ和Ｉ Ｃ数字接口。片内集成ＦＩＦＯ缓 冲器，将主处理器的负荷降到最低。 姿态传感器模块采用的数字陀螺仪和数字加速度计均 具有标准Ｉ。Ｃ接口，采用Ｉ Ｃ总线读取传感器模块各个传感 器的数据，如图３。具有接口线少、控制方式简单、通信速率 高等优点，节约硬件资源，提高整个系统的开发速度。 ＶＣＣ 串行数据（ＳＤｋ）串行时钟（ＮＣＬ） 图３ Ｉ Ｃ总线连接图 Ｆｉｇ ３ Ｃｏｎｎｅｃｔｉｏｎ ｄｉａｇｒａｍ ｏｆ １２ Ｃ ｂｕｓ ２姿态解算 目前，姿态角解算常用的方法仍以经典卡尔曼滤波或 扩展卡尔曼滤波为主，但是卡尔曼滤波要求精确已知系统 模型和噪声方差，同时由于其算法较为复杂，对处理器要求 高，需要处理多维Ｊａｃｏｂｉ矩阵，处理不当易造成严重的数据 延迟，对实时控制极为不利。 姿态解算主要是利用陀螺仪和加速度计测量的信息， 陀螺仪测量载体相对于惯性坐标系的旋转角速度，加速度 ｃ付 ｇ・ ，，， ．．．．．一／ ＾’ｅＩ ＾ ＼、●●●●／ ，， ，．．．．．．．．．．．．．．．．．．．．．．．．ｌ ｌ一／ ａ 阳 差言 肌 （３） 同样，假设陀螺仪本身不存在漂移，则可直接使用角速 率积分得到姿态角 丽 ． ㈩ Ｊ＿ ｄ 其中， 为陀螺仪测得 轴角速度；ｔＯ 为陀螺仪测得Ｙ 轴角速度。 在实际应用中，以运动加速度为主的有害噪声会影响 加速度计姿态测量的精度，而陀螺仪本身存在漂移。为了 获得准确的姿态角，采用一阶低通滤波器和一阶高通滤波 器。以０为例，如图４所示。 图４互补滤波原理 Ｆｉｇ ４ Ｐｒｉｎｃｉｐｌｅ ｏｆ ｃｏｍｐｌｅｍｅｎｔａｒｙ ｆｉｌｔｅｒｉｎｇ 其中，ｄｔ为采样时间间隔；ｆ为滤波器时间常数，１／ｒ即 为滤波器的截止频率。对于低通滤波器而言，频率低于１／ｒ 的信号能够顺利通过，频率高于１／ｒ的信号则被抑制；高通 滤波器与此相反。因而，— 具有低通特性，— 具有高 十ｄ 十ｄ 通特性。互补滤波器通过对加速度计的低通作用，可以很 好地限制运动加速度对姿态测量的影响，对陀螺仪的高通 作用可以有效抑制漂移对姿态测量的影响。根据图４可以 得到 １１０ 传感器与微系统 ４５ 第３２卷 ＝ ＋ ． （５） ３０ １５ 其中，０为最优估计姿态角； 为陀螺仪估计姿态角； 。 为加速度计估计姿态角。而 一—０ １５ ３Ｏ ４５ －Ｏｇ（ｎ）＝ （ｎ－－１）＋ｆ ｄｔ． 因此，有 （６） 图７有明显运动加速度实验姿态角测量结果 占（ｎ）＝ （ （ｎ＿１）＋』ｔ。 ）＋ ）．（７） Ｆｉｇ ７ Ｍｅａｓｕｒｅｍｅｎｔａｌ ｒｅｓｕｌｔ ｏｆ ａｔｔｉｔｕｄｅ ａｎｇｌｅ ｗｉｍ 在大于截止频率的高频段，陀螺仪对姿态解算结果起 主要作用；在小于截止频率的低频段，加速度计对姿态解算 结果起主要作用。通过调整滤波器时间常数ｔｒ，改变滤波器 的截止频率，实现对陀螺仪和加速度计权重的调整。 ３实验 在搭建的硬件平台上实现上述滤波算法，并进行了静 止实验、无明显加速度实验、有明显加速度实验和实际飞行 实验。实验中，飞控同时采用加速度计、陀螺仪和两者结合 的互补滤波算法估算姿态角，时间间隔为２０ｍｓ，滤波器时 间常数 为９８０ ｍｓ。以俯仰角测量为例，图５为静止实验 时姿态角的测量结果，图６为无明显运动加速度实验时姿 态角的测量结果，图７为有明显运动加速度实验时姿态角 的测量结果，图８为实际飞行实验时姿态角的测量结果。 图５静止实验姿态角测量结果 Ｆｉｇ ５ Ｍｅａｓｕｒｅｍｅｎｔａｌ ｒｅｓｕｌｔ ｏｆ ａｔｔｉｔｕｄｅ ａｎｇｌｅ ｗｈｉｌｅ ｉｎ ｓｔａｔｉｏｎａｒｙ ｅｘｐｅｒｉｍｅｎｔ ６０ ４０ ２０ ０ ２０ —４０ ６０ 图６ 无明显运动加速度实验姿态角测量结果 Ｆｉｇ ６ Ｍｅａｓｕｒｅｍｅｎｔａｌ ｒｅｓｕｌｔ ｏｆ ａｔｔｉｔｕｄｅ ａｎｇｌｅ ｗｉｔｈｏｕｔ ａｐｐａｒｅｎｔ ａｃｃｅｌｅｒａｔｉｏｎ ｅｘｐｅｒｉｍｅｎｔ 由以上４组实验姿态角测量结果可以看出：加速度计 在无运动加速度干扰的情况下测量结果比较准确，但运动 加速度会对其测量产生很大的干扰。陀螺仪动态特性较 好，但自身存在随机漂移误差，并且在积分过程中，误差会 随时间不断累加。采用互补滤波算法得到的姿态角，由于 利用了陀螺仪和加速度计在测量频率上互补的特性，有效 ａｐｐａｒｅｎｔ ａｃｃｅｌｅｒａｔｉｏｎ ｅｘｐｅｒｉｍｅｎｔ ３Ｏ ２０ １０ ０ １０ —２０ —３０ 图８实际飞行实验姿态角测量结果 Ｆｉｇ ８ Ｍｅａｓｕｒｅｍｅｎｔａｉ ｒｅｓｕｌｔ ｏｆ ａｔｔｉｔｕｄｅ ａｎｇｌｅ ｉｎ ａｃｔｕａｌ ｆｌｉｇｈｔ ｅｘｐｅｒｉｍｅｎｔ 地削弱运动加速度和陀螺仪漂移对姿态测量的影响，测量 结果比较稳定，能够满足一般无人机姿态测量的要求。 ４结论 本文设计的基于ＳＴＭ３２的无人机姿态测量系统，结构 简单实用，性能稳定，精度高。采用的互补滤波算法，利用 陀螺仪和加速度计在测量频率上的互补特性，提高姿态测 量的精度和动态响应。实验测试表明：该系统能够精确地 测量姿态信息，满足一般无人机的姿态测量要求，为无人机 飞行控制系统的搭建奠定了基础。 参考文献： ［１］李泽民，段凤阳，李赞平．基于ＭＥＭＳ传感器的数字式航姿系 统设计［Ｊ］．传感器与微系统，２０１２，３１（６）：９４－－１００． ［２］ 李荣冰，刘建业，曾庆化，等．基于ＭＥＭＳ技术的微型惯性导 航系统的发展现状［Ｊ］．中国惯性技术学报，２００４，１２（６）： ８８－９４． ［３］徐 玉．微小型无人直升机飞控平台与姿态融合算法研 究［Ｄ］．杭州：浙江大学，２００８． ［４］Ｌａｉ Ｙｉｎｇ Ｃｈｉｈ，Ｊａｎ Ｓｈａｕ Ｓｈｉｕｎ，Ｈｓｉａｏ Ｆｅｉ Ｂｉｎ．Ｄｅｖｅｌｏｐｍｅｎｔ ｏｆ ａ ｌｏｗ・－ｃｏｓｔ ａｔｔｉｔｕｄｅ ａｎｄ ｈｅａｄｉｎｇ ｒｅｆｅｒｅｎｃｅ ｓｙｓｔｅｍ ｕｓｉｎｇ ａ ｔｈｒｅｅ－－ａｘｉｓ ｒｏｔａｔｉｎｇ ｐｌａｔｆｏｒｍ［Ｊ］．Ｓｅｎｓｏｒｓ，２０１０，１０：２４７２—２４９１． ［５］郭晓鸿，杨忠，陈士±，等．ＥＫＦ和互补滤波器在飞行姿态 确定中的应用［Ｊ］．传感器与微系统，２０１１，３０（１１）：１４９— １５２． ［６］Ｗａｎｇ Ｍｅｉ，Ｙａｎｇ Ｙｕｎｃｈｕｎ，Ｈａｔｃｈ Ｒｏｎａｌｄ Ｒ，ｅｔ ａ１．Ａｄａｐｔｉｖｅ ｉｆｌｔｅｒ ｆｏｒ ａ ｍｉｎｉａｔｕｒｅ ＭＥＭＳ ｂａｓｅｄ ａｔｔｉｔｕｄｅ ａｎｄ ｈｅａｄｉｎｇ ｒｅｆｅｒｅｎｃｅ ｓｙｓ— ．ｔｅｍ［Ｃ］∥ＩＥＥＥ Ｐｏｓｉｔｉｏｎ Ｌｏｃａｔｉｏｎ ａｎｄ Ｎａｖｉｇａｔｉｏｎ Ｓｙｍｐｏｓｉｕｍ， Ｍｏｎｔｅｒｅｙ，ＣＡ，ＵＳＡ，２００４：１９３－－２００． 作者简介： 刘辉邦（１９８７一），男，辽宁大连人，硕士研究生，研究方向为无 人机飞行控制系统。