如何烧录 arduino 的 bootloader_百度知道
如何烧录 arduino 的 bootloader
我有更好的答案
为了恢复Arduino的功能，必须得重新给ATMega328P单片机里烧写bootloader（其实Arduino就是在AVR单片机中预置了一个bootloader程序，这样再配合Arduino开发环境就是所谓的Arduino了，但Arduino这个词本身只留给官方使用）。当然在淘宝上也能买到已经烧写好的AVR单片机，只是自己手上已经有了AVR单片机，就没必要去花那冤枉钱了。于是马上打开万能的百度，输入关键字“Arduino bootloader”果然，一大堆乱七八糟相关的问题就出来了，经过一轮的筛选，发现许多说的也并不是很清晰，不过让我明白了个大概：一个普通的AVR单片机确实能烧写bootloader之后作为Arduino使用。那么问题来了，如何把bootloader烧录到AVR单片机中去了？这的确是个问题，反正对于不懂AVR开发的我来说是个问题。不过还好之前在淘宝上买了一个USBASP下载器，跟之前擦除bootloader一样，也要用到progisp软件（因为此时的ATMega328P只是一个普通的AVR单片机，用Arduino开发软件是烧写不进去的）。既然要烧录bootloader，那么什么是bootloader呢？正如你所想，bootloader当然是一个程序，既然要烧录到单片机中去，应该是一个.HEX之类的文件。但是，bootloader文件从哪来呢？继续百度。。。找到了，在Arduino软件安装目录中的hardware\arduino中。此文件夹下的“boards.txt”文件很重要，是很重要，其他的或许都可以在百度上找到，但因为这个文件折腾了我大半个晚上，百度上也没有明确的说明。打开文件，找到自己对应的Arduino版本，因为我的是“Arduino Uno”，于是锁定“uno.name=Arduino Uno”区域的说明，因为看上去很乱，一定要仔细去看。找到“uno.bootloader.low_fuses=0xff&指编程熔丝低位为FF，”uno.bootloader.high_fuses=0xde“指编程熔丝高位为DE，”uno.bootloader.extended_fuses=0x05“指扩展位为05。这三个值非常重要，是决定你烧录bootloader成功的关键。至于这三个值具体是什么意思，我也不是很不清楚，但AVR单片机的烧录时需要设置熔丝位，而且不能乱设置，这也是AVR跟51单片机的不同之处。用progisp软件把这三个熔丝位的值写入到AVR单片机中，然后继续浏览”boards.txt“文件往下看，找到”uno.bootloader.path=optiboot“这就是说要烧录的bootloader文件在bootloaders\option文件夹中，“uno.bootloader.file=optiboot_atmega328.hex”文件名就是optiboot_atmega328.hex，这就是接下来需要烧录到AVR单片机中的程序。用progisp软件调入此文件，然后直接写入到AVR单片机中就行了，一个支持Arduino的AVR就做好了。。。其实这个问题本身不难，但对于没接触过AVR的人来说，还是要花许多时间的，中间也会存在许许多多各种各样的问题。至于progisp的使用方法很简单，百度上的教程也很多，还有手上必须有一块可以烧录AVR单片机的下载器。我当时就是忽略了那三个熔丝位的值，所以反反复复也没有成功，百度上面也没有对”boards.txt“文件做详细的解释。没设置熔丝位程序是可以正常的烧录进去，但Arduino是不能用的，所以必须要按照官网给出的说明操作。我当时大多数时间主要花在两个问题上，一个是.HEX文件具体位置，因为Arduino有不同的版本，所以.HEX文件也不同，还有一个就是熔丝位上。
采纳率：94%
来自团队：
为您推荐：
其他类似问题
换一换
回答问题，赢新手礼包
个人、企业类
违法有害信息,请在下方选择后提交
色情、暴力
我们会通过消息、邮箱等方式尽快将举报结果通知您。Arduino中变量声明和引脚定义怎么分别_百度知道
Arduino中变量声明和引脚定义怎么分别
我有更好的答案
不连续的管脚是无法一次性操作的，除非使用函数。但连续的管脚，比如P1.0、P1.1....p1.7，则可直接使用宏来定义：#define
P0 使用的时候直接用temp=0x55; 即可给这些引脚赋值
采纳率：82%
来自团队：
为您推荐：
其他类似问题
换一换
回答问题，赢新手礼包
个人、企业类
违法有害信息,请在下方选择后提交
色情、暴力
我们会通过消息、邮箱等方式尽快将举报结果通知您。当前位置： >>
基于Arduino兼容的Stm32单片机的四旋翼飞行器设计
厦门大学学位论文原创性声明本人呈交的学位论文是本人在导师指导下，独立完成的研究成 果。本人在论文写作中参考其他个人或集体已经发表的研究成果，均 在文中以适当方式明确标明，并符合法律规范和《厦门大学研究生学 术活动规范（试行）》。 另外，该学位论文为（ 的研究成果，获得（ 资助，在（ ）课题（组） ）课题（组）经费或实验室的 ）实验室完成。（请在以上括号内填写课题或课题组负责人或实验室名称，未有此项声明内容的，可以不作 特别声明。）声明人ｃ㈣：穆怒加解岁月日厦门大学学位论文著作权使用声明本人同意厦门大学根据《中华人民共和国学位条例暂行实施办 法》等规定保留和使用此学位论文，并向主管部门或其指定机构送交 学位论文（包括纸质版和电子版），允许学位论文进入厦门大学图书 馆及其数据库被查阅、借阅。本人同意厦门大学将学位论文加入全国 博士、硕士学位论文共建单位数据库进行检索，将学位论文的标题和 摘要汇编出版，采用影印、缩印或者其它方式合理复制学位论文。 本学位论文属于：（）１．经厦门大学保密委员会审查核定的保密学位论文，年／于月日解密，解密后适用上述授权。（√）２．不保密，适用上述授权。（请在以上相应括号内打“√”或填上相应内容。保密学位论文 应是已经厦门大学保密委员会审定过的学位论文，未经厦门大学保密 委员会审定的学位论文均为公开学位论文。此声明栏不填写的，默认 为公开学位论文，均适用上述授权。）声明人ｃ㈣：勰劬似年箩月日摘要摘要四轴飞行器拥有四个旋翼，属于多旋翼直升机。四轴飞行器具有四个成对称 分布的旋翼。它通过控制四个旋翼的旋转速度而非机械结构来实现各种飞行动 作。四轴飞行器具有成本低、机体结构简单、没有机械结构、飞行稳定性好、重 量轻、有利于小型化无人化等特点。因此可以应用在人无法到达的一些复杂环境 之中。目前四旋翼飞行器等多旋翼飞行器已经在很多行业比如航空拍摄、遥感勘 测、实时监控、军事侦察、喷洒农药巾得到了广泛的应用，并已经形成了相关产 业。四旋翼飞行器具有非线性控制、控制量多、飞行姿态控制过程复杂等特性。 本课题基于实现四轴飞行器低成本小型化通用化的思路，通过研究剖析四旋翼飞 行器飞行的原理，根据其数学模型和控制系统的功能要求，在ＭＣｕ上实现了四旋翼飞行器的姿态数据的获取、飞行姿态解算以及飞行姿态控制。本课题硬件上采 用ｓ锄３２系列ＳＴＭ３２Ｆ１０３Ｃ８Ｔ６ ３２位处理器作为主控制器负责分析处理数据，根据姿态运算结果，输出电机控制信号；主要使用惯性测量单元ＭＰＵ一６０５０等传感 器模块用于姿态信息的检测；采用场效应管驱动电路来驱动空心杯电机；蓝牙模 块负责和上位机进行通信以实时采集飞行数据便于分析测试。整个软硬件系统均 基于模块化设计的思想。各传感器采集飞行器的传感器数据都使用通用数字接口 和Ｍｃｕ进行数据交换和通信。软件上，编写飞行姿态控制软件，在ｓｔｍ３２单片 机上实现了四元数法和卡尔曼滤波算法，解算出飞行器正确的姿态角，并使用 ＰＩＤ控制进行姿态角的闭环控制，稳定飞行姿态。实验结果表明，本课题设计的 四轴飞行器能够较好的自主达到稳定飞行状态，抗扰动能力强。飞行姿态控制算法完全实现了使四旋翼飞行器能在室内平稳色行的控制要求。关键词：四旋翼飞行器；姿态控制算法；ｓＴＭ３２；ＡｂｓｔｒａｃｔＡｂ ｓｔｒａｃｔＱｕａｄｒｏｔｏｒ，ａｌｓｏｌ（ｎｏｗｎａｓＦｏｕｒ―ｒｏｔｏｒ ａｉｆｃｒａｆｔ，ｉｓａｂａｓｉｃ ｆｏｒｎｌ ｏｆ ｍｕｌｔｉ－ｒｏｔｏｒｈｅｌｉｃｏｐｔｅｒ ｗｈｉｃｈ ｈａｓ ｆｏｕｒ ｓｙｍｍｅｔｒｉｃａｌｌｙ ｒｏｔｏｒｓ．Ｉｔ ｆｌｏａｔｓ ａｎｄ ｍｏＶｅｓ ｂｙ ｓｐｅｅｄ ｏｆ ｒｏｔｏｒ ｗｉｎｇｓ，ｉｎｓｔｅａｄ ｏｆ ｍｏｄｕｌａｔｉｎｇ ｉｔＳ ｍｅｃｈａｎｉｃａｌａｄｊｕｓｔｉｎｇｔｈｅｓｔｍｃｔｕｒｅ．Ｑｕａｄｒｏｔｏｒａｎｄ １ｉｇｈｔｗｅｉｇｈｔ ｂｏｄｙｅｎｊｏｙｓｌｏｔｓ ｏｆ ｓｔｒｏｎｇ ｐｏｉｎｔｓ ｓｕｃｈａｓ１０ｗｃｏｓｔ，ｓ仃ａｉｇｈｔｆｏ刑ａｒｄａｓｗｉｔｈｏｕｔ ｃｏｎｌｐｌｉｃａｔｅｄ ｍｅｃｈａｎｉｃａｌ ｓｔｌｌｌｃｔｕｒｅ，ａｓ ｗｅｌｌｅｘｃｅＵｅｎｔ ｎｉｇｈｔ ｓｔａｂｉｌｉｔｙ．Ｂｅｓｉｄｅｓ，ｃａｎｉｔ ａｌｓｏ ｈａｓ ｔｈｅ ａｄｖａｎｔａｇｅｏｕｓ ｃｈａｒａｃｔｅｒ ｏｆ ｍｉｎｉａｍｒｅ ａｎｄ ｌａｂｏｒｌｅｓｓ．ＳＯ ｉｔ ｉｎ ｓｏｍｅ ｃｏｍｐｌｅｘ ｅｎＶｉｒｏｎｍｅｎｔｓ ｓｕｃｈ ｍｉｌｉｔａｒｙ ｒｅｃｏｎｎａｉｓｓａＩｌｃｅａｓｂｅ ａｐｐｌｉｅｄ ｒｅａｌ－ｔｉｍｅａｅｒｉａｌｐｈｏｔｏｇｒａｐｈｙ， ｔｅｌｅｍｅｔⅨｔｈｕｓ ｈａｓｍｏｎｉｔｏｒｉｎｇ， ｃｏｎｓｉｄｅｒａｂｌｅａｎｄ ｐｅｓｔｉｃｉｄｅ ｓｐｒａｙｉｎｇ，ｆｂｍｅｄａｉｎｄｕｓ时ｉｎｔｅｒｒｅｌａｔｅｄ．Ｙｅｔａｓｔｈｉｓ ｋｉｎｄ ｏｆ ｆｏｕｒ―ｒｏｔｏｒ ａｉｒｃｒａＲ ｓｔｉｌｌ ｈａｓ ｃｅｒｔａｉｎｂｌｉｎｄ ｓｉｄｅ ｓｕｃｈ ｃｏｎ仃ｏｌｌｉｎｇｎｏｎｌｉｎｅａｒ ｃｏｎｔｒｏｌ，ｈｅａＶｙ ｃｏｎｔｒｏｌ Ｖｏｌｕｍｅ ａｎｄ ｃｏｍｐｌｅｘ ｐｒｏｃｅｓｓ Ｏｆ ｐｏｓｔｕｒｅｓ． Ｔｈｉｓ ｒｅｓｅａｒｃｈ ｗａｓ ｂａｓｅｄｏｎｆｌｙｉｎｇｔｈｅｉｄｅａｏｆ ｅｎｄｕｉｎｇＱｕａｄｒｏｔｏｒｓ ａｎａｌｙｚｉｎｇｗｉｔｈ ｐｒａｃｔｉｃａｌｉｔｙ ｌｉｋｅ ｌｏｗ ｃｏｓｔ，ｍｉｎｉａｔｕｒｉｚａｔｉｏｎ ａｎｄ ｇｅｎｅｒａｌ ｓｅｒｖｉｃｅ ｔｈｍｕｇｈａｓｔｈｅ ｎｙｉｎｇ ｐｒｉｎｃｉｐｌｅ ｏｆ ｔｈｉｓ ｈｎｄ ｏｆ ｆ．ｏｕｒ－ｒｏｔｏｒ ａｉｒｃｒａＲ，ａｓ ｗｅｌｌａｃｑｕｉｒｉｎｇａｎｄ ｃａｌｃｕｌａｔｉｎｇ ｄａｔａ ａｂｏｕｔ日ｙｉｎｇ ｐｏｓｔｕｒｅｓ ａｎｄｃｏｎｔｒｏｌＩｉＩ培ｔｈ锄ｏｎｔｈｅ ｂａｓｉｓ ｏｆ ＭＣＵａｃｃｏｒｄｉｎｇ ｔｏ ｉｔｓ ｍａｔｈｅｍａｔｉｃａｌ ｍｏｄｅｌ ａｎｄ ｔｈｅ ｍｎｃｔｉｏｎａｌ ｒｅｑｕｉｒｅｍｅｎｔｓ ｏｆｔｈｅ叩ｅｒａｔｉｎｇａｓｓｙｓｔｅｍ．Ｔｈｅ ＳＴＭ３２Ｆ １ ０３Ｃ８Ｔ６ ３２－ｂｉｔ ｐｒｏｃｅｓｓｏｒ ｉｎ ｓ舡１１３２ ｓｅｒｉｅｓ ｗａｓ ａｐｐｌｉｅｄ ｍａｉｎ ｃｏｎｔｒｏｌｌｅｒｔｏｔｈｅａｎａｌｙｚｅ ａｎｄ ｐｒｏｃｅｓｓ ｄａｔａ，ａＩｌｄ ｔｏ ｏｕ印ｕｔ ｔｈｅ ｓｉｇｎａｌｓ ｏｆ ｍｏｔｏｒｓ ｉｎｔｅｍｓｏｆ ｔｈｅ ｆｅｓｕｌｔｓ ｃａｌｃｕｌａｔｅｄ．Ｉｎ ａｄｄｉｔｉｏｎ．ｓｅｎｓｏｒ ｍｏｄｕｌｅｓ ｌｉｋｅ ＭＰＵ－６０５０ ｗｅｒｅｍａｉｎｌｙ ｕｓｅｄ ｆｏｒ ｄｅｔｅｃｔｉｎｇ ｇｅｓｔｕｒｅｓ ａｎｄ ｏｔｈｅｒｏｎｉｎｆｏｍａｔｉｏｎ，ＦＥＴｃｉｒｃｕｉｔ ｗａｓｃ印ｉｔａｌｉｚｅｄｄｒｉＶｉｎｇ ｔｈｅ ｃｏｒｅｌｅｓｓ ｍｏｔｏｒｓ ａｎｄ Ｂ＿Ｉｕｅｔｏｏｔｈ ｍｏｄｕｌｅ ｔｏ ｃｏｍｍｕｎｉｃａｔｅ ｗｉｍ ｔｈｅ ｈｏｓｔｃｏｍｐｕｔｅｒ ｆｏｒ ｃ０１１ｅｃｔｉｎｇ ａｎｄ ａｎａｌｙｚｉｎｇ ｔｈｅ ｒｅａｌ―ｔｉｍｅ ｎｉｇｈｔ ｄａｔａ，Ｔｈｅ ｅｎｔｉｒｅ ｓｙｓｔｅｍ ｏｆｓｏｆｈａｒｅａｎｄ ｈａｒｄｗａｒｅ ｗａｓ ｂａｓｅｄｏｎｔｈｅ ｉｄｅａ ｏｆ ｍｏｄｕｌａｒｉｚｅｄ ｄｅｓｉｇｎ．Ｓｅｎｓｏｒ ｄａｔａ ｗｉｔｈ ＭＣＵ ｔ１１ｍｕｇｈ ｔｈｅ ＩＩＣｏｎｃｏｌｌｅｃｔｅｄ ｗｅｒｅ ａｌｌ ｕｓｅｄ ｔｏｃｏｍｍ珊ｉｃａｔｅｂｕｓ．Ｑｕａｔｅｍｉｏｎａｎｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ ａｌｇｏｒｉｔｈｍ ｗａｓ ｉｍｐｌｅｍｅｎｔｅｄｓｔｍ３２ ＳＣＭ ｂｙ ｔｈｅ ｗｒｉｔｉｎｇ ｏｒｄｅｒｓｅｑｕｅｎｃｅｓ ｉｎｔ０Ｉ ｆｌｉｇｈｔ ｐｏｓｔｕｒｅｓ ｃｏｎｔｒｏｌｌｉｎｇ ｓｏｆｔｗａｒｅ，ａｎｄ ｔｈｕｓ ｔｈｅ ｃｏｎ－ｅｃｔ ａｔｔｉｎｌｄｅ ａｎｇｌｅｗａｓ ｒｅｓｏｌＶｅｄ．Ｂｅｓｉｄｅｓ，ＰＩＤ ｗａｓ ａｌｓｏ ａｄｏｐｔｅｄ ａｓ ｃｌｏｓｅｄ－ｌｏｏｐ ｃｏｎｔｒｏｌ ｆｏｒ ｃｏｎ拄ｏｎｉｎｇ ｍｅａｔｔｉｔｕｄｅ ａｎｇｌｅ ａｎｄ ｓｔａｂｉｌｉｚｉｎｇ ｔｈｅ ｎｉｇｈｔ ａｔｔｉｔｕｄｅ．Ｒｅｓｕｌｔｓ ｏｆ ｔｈｉｓ ｒｅｓｅａｒｃｈ ｓｈｏｗ ｔｈａｔ ｔｈｉｓ ｆｏｕｒ－１‘ｏｔｏｒ ａｉｒｃｒａｎ ｄｅｓｉｇｎｅｄ ｉｎ ｔｈｉｓｐｒｏｊｅｃｔｃａｎｅ师ｃｉｅｎｔｌｙ ａｃｈｉｅＶｅ ｔｈｅ ｏｂｊｅｃｔｉＶｅ ｏｆＩＩＡｂｓｔｒａｃｔｓｔａｂｉｌｉｚｉｎｇ ｎｉｇｈｔ ａｔｔｉｔＩＪｄｅ ａｕｔｏｎｏｍｏｕｓｌｙ ｗｉｔｈ ｏｕｔｓｔａｎｄｉｎｇ ａｎｔｉ－ｄｉｓｔｕｒｂａｎｃｅ ｃａｐａｂｉｌｉｔｙＷｈａｔ’ｓｍｏｒｅ，ｔｈｅ ａｌｇｏｒｉｔｈｍ ｉｎ ｃａｌｃｕｌａｔｉｎｇ ｎｉｇｈｔ ａｔｔｉｔｕｄｅ ｃｏｎｔｍｌ ｆｕｌｌｙ ｍｅｅｔｓ ｔｈｅｒｅｑｕｉｒｅｍｅｎｔｓ ｏｆ ｓｔａｂｌｅ ｎｙｉｎｇ ｆｏｒ ｗｈｉｃｈ ｔｈｅ ｆｏｕｒ－ｒｏｔｏｒ ａｉｒｃｒａＲ ｓｈｏｕｌｄ ｐｅｒｆｏｒｍ．Ｋｅｙ ｗｏｒｄｓ：Ｑｕａｄｒｏｔｏｒ；Ａｔｔｉｔｕｄｅｃｏｎｔｒｏｌ；ＳＴＭ３２１ＩＩ目录目录第一章绪论……………………………………………………………．１１．１ １．２选题背景……………．……………………………．．１ 国内外研究现状………．……………………………．．２１．２．１国内四旋翼飞行器的研究现状………．．．……．．．．．……．．．．２１．２．２国外四轴飞行器的研究现状……………………．………．３１．３四旋翼飞行器主要优点和技术难点：……………．………．．３１．３。１四旋翼飞行器主要优点…，……．………………．………３１．３．２四轴飞行器的技术难点…．………．……．．……………．．４１．４ １．５本文研究的主要内容……………．……．………………４ 本文结构安排………………………………………．．５第二章四轴飞行器飞行原理、飞行姿态及滤波算法…………６２．１ ２．２四轴飞行器的飞行原理…．…………．………．…………６ 四轴飞行器姿态的表示和运算………．．…………………１０２．２．１坐标系统的建立………．……………………………．１０２．２．２四轴飞行器飞行姿态的表示和换算………………………．１２２．３滤波算法以及修正融合………………………………．．１５２．３．１卡尔曼滤波………………………………．．……．．．．１８ ２．３．２互补滤波算法………………………………………．２６２．４本章小结…．………．．。……．…………………．…．．３０第三章四轴飞行器的软硬件系统架构…………………．．３１３．１ ３．２硬件结构框图………………………………………．３１ 硬件电路设计……………………………………．…３２３．２．１电机及其驱动电路…………………………．………．．３２３．２．２控制电路部分及传感器模块．…………………．………．．３４３。３ ３．４ ３．５系统板实物图……………………………。。………。．４２软件框图及思路………．……．．………．……………．４３ 本章小结…………．……………………………．…４４ｌＶ目录第四章飞行姿态控制算法实现………………………４５４．１ ４．２软件初始化过程……．……．…．……………………．．４５ 四元数法及其姿态解算的算法实现……………．…………４７４．２．１四元数运算及四元数转欧拉角算法流程，…，．．．．……………４８４．２．２四元数运算流程．．．…．……………………．…………５０ ４．２．３卡尔曼滤波的算法实现．……………………．…．．．．．…．５１４．３ ４．３．１ ＰＩ ＰＩＤ算法、参数整定及其控制…………．………………．．５４ Ｄ算法的原理……．．．．．……．．．．．…．．．…．．．．．．．．．．．．．．５４４．３．２四轴飞行器的ＰＩＤ调节原理…………………．…………５６ ４．３．３四轴飞行器ＰＩＤ参数的整定…………………．…………５８４．４四轴飞行器平飞实测数据及分析…………………………６０４．４，１加速度计数据波形及分析………．…………．．…………６１ ４．４．２陀螺仪数据波形及分析．……………………．…………６２ ４．４．３卡尔曼滤波融合后实测波形…．……．…．．．．．．．．………．．．６３４．５本章小结………。………………………．…………６４第五章总结与展望………………………………………………．．６５参考文献…………．……………………………………．６６致谢……………………………………………………………………６８ＶＤｉｒｅｃｔｏｒｖＤｉｒｅｃｔｏｒｙＩ ｎｔｒｏｄｕｃｔ ｉ１．１ １．２ １．２．１ Ｔｈｅｏｎ………………………………………………………。１ｂａｃｋｇｒｏｕｎｄ……．．…．．………．……．．……．．…．．．１ ｓｔａｔｕｓ…．……．．…………………………．．２ｏｎＲｅｓｅａｒｃｈＰｒｅｓｅｎｔ Ｒｅｓｅａｒｃｈ０ｕａｄｒｏｔｏｒｉｎ Ｃｈｉｎａ…………………２１．２．２ Ｆｏｒｅｉｇｎ ｒｅｓｅａｒｃｈ １。３ １。３。１ Ｔｈｅ ｍａｉｎｓｔａｔｕｓ ｏｎＱｕａｄｒｏｔｏｒ…．．．……．………．３ｑｕａｄｒｏｔｏｒ．……………，．．…。．３ ｑｕａｄｒｏｔｏｒ．…………………．．４ａｄＶａｎｔａｇｅ ｏｆｑｕａｄｒｏｔｏｒ ａｎｄ ｔｅｃｈｎｉｃａＩ ｄｉｆｆｉｃｕｌｔｉｅｓ．３Ｔｈｅ ｍ日ｉｎ ａｄｖａｎｔａｇｅ ｏｆ ｔｈｅ ｄｉｆｆｉｃｕＩｔｉｅｓ ｏｆｃｏｎｔｅｎｔ１．３．２ Ｔｅｃｈｎｉ ｃａＩ １．４ １．５Ｔｈｅ鹏ｉｎｏｆ ｔｈｉｓｐａｐｅｒ………………………．．４ｏｆ ｔｈｉｓＳｔｒｕｃｔｕｒａＩａｒｒａｎｇｅｍｅｎｔｓｐａｐｅｒ．……．…………．．５ａｔｔｉｔｕｄｅ ａｎｄ２Ｑｕａｄｒｏｔｏｒ’ｓ ｆＩ ｉｇｈｔ ｐｒｉｎｃｉｐＩｅ，ａｆＩ ｉｇｈｔｆ ｉ ｌｔｅｒ ｌ ｎｇ２．１ ２．２Ｉ ｇｏｒ ｉｔｈｍｓ…………………………………………………６ＦＬｉ ｇｈｔ ｐｒ ｉ ｎｃｉｐＩｅ ｏｆ ｔｈｅｑｕａｄｒＯｔｏｒ…．．……．．．．……．．…６ ａｒｉｔｈｍｅｔｉｃ…．…．…，．１０ＦＩ ｉｇｈｔ ａｔｔｉｔｕｄｅ ｒｅｐｒｅｓｅｎｔａｔｉｏｎ ａｎｄ２．２．１ Ｃｏｏｒｄｉ ｎａｔｅＳｙｓｔｅｍｓ．…………………………………１０ ｔｒａｎｓｌａｔｅｄ…．………．．１２ ａｍｅｎｄｍｅｎｔｓ………．．．…．．１５２．２．２ Ｑｕａｄｒｏｔｏｒ ａｔｔｉｔｕｄｅ ｅｘｐｒｅｓｓｅｄ ａｎｄ ２．３ Ｆｕｓｉｏｎ ｆｉ Ｉｔｅｒ ｉｎｇ ａＩｇｏｒ ｉｔｈｍ ａｎｄ ｆｉ２．３．１ ＫａＩｍａｎＩｔｅｒ…．．…………………………．．．……．１８ ａＩｇｏｒｉｔｈｍ．………．．．…………２６２．３．２ ＣｏｍｐＩｅｍｅｎｔａｒｙ ｆｉ Ｉｔｅｒｉ ｎｇ ２．４Ｓｕ呻协ｒｙ…．．………………………………………．３０ｈａｒｄｗａｒｅ ａｎｄ ｓｏｆｔｗａｒｅ ｓｙｓｔｅｍ ａｒｃｈ ｉ ｔｅｃｔｕｒｅ ３１３Ｑｕａｄｒｏｔｏｒ’ｓ３．１ ３．２ ３．２．１Ｈａｒｄｗａｒｅ ｂＩｏｃｋ ｄｉ ａｇｒａｍ………………．．……………．３１ Ｈａｒｄｗａｒｅ Ｄｅｓｉ Ｍｏｔｏｒ ａｎｄｇｎ……………………………………．３２ｖｅｉｔｓ ｄｒ ｉｃＩｒｃｕｉｔ………………………．…３２ ｍｏｄｕＩｅ．……………．．．．．…．３４３．２．２ Ｃｏｎｔｒｏｌ ３．３ ３．４ ３．５ｃｉ ｒｃｕｉｔ ａｎｄｓｅｎｓｏｒＳｙｓｔｅｍ ｂｏａｒｄ ｐｈｙｓｉｃａＩ Ｓｏｆｔｗａｒｅ ｄｉａｇｒａｍ ａｎｄｍａｐ……………．．．．…………．．４２ ｉｄｅａｓ…………………………．．４３Ｓｕ唧ａｒｙ…．…．……………………………．．……．．４４ＶＩＤｉｒｅｃｔｏⅣ４Ｆｌ ｉ ｇｈｔ ａｔｔ ｉ ｔｕｄｅ ｃｏｎｔｒｏ Ｉ４．１ ４．２ Ｓｏｆｔｗａｒｅ ＴｈｅｉｒａＩ ｇｏｒ ｉ ｔｈｍ…．……．．．…．４５ｉ ｎｉｔｉａＩ ｉ ｚａｔｉｏｎｐｒｏｃｅｓｓ………………．．……．４５ｏｎａｔｔｉｔｕｄｅ ｑｕａｔｅｒｎｉａｌ ｇｏｒ ｉｔｈｍｓｏｌｖｅｒ……．．．．．．．．．．４７ＥｕＩｅｒ ａｎｇｌｅｓ４．２．１ ０ｕａｔｅｒｎｉｏｎ ａｒｉｔｈｍ旧ｔｉｃ ａｎｄ ｑｕａｔｅｒｎｉｏｎ ｒｏｔａｔｉｏｎａＩ ｇｏｒ ｉｔｈｍｆＩｏｗ…………………………………………．．ｏｎ４．２．２ ０ｕａｔｅｒｎｉ ４．２．３ Ｋａ Ｉｍａｎ ４．３ａｒｉｔｈｍｅｔｌｃ ａＩｇｏｒｐｒｏｃｅｓｓｅｓ……．………．．．．．．．．．．．５０ｆｉ Ｉｔｅｒｉｔｈｍ………………………．．．．…．５１ＰＩ Ｄ ａＩｇｏｒｉｔｈｍ，ａｐａｒａｍｅｔｅｒ ｔｕｎｉ ｎｇ ａｎｄｃｏｎｔｒ０１．．．。…．……．５４４．３．１ ＰＩ ＤＩｇｏｒ ｉｔｈｍ ｐｒ ｉ ｎｃｉｐＩｅ…．．．．……．……。．．…………５４ｉｎｃｉｐｌｅｓ………．．．．．．．．．．．．５６ ａｎａＩｙｓＩｓ…………．６０ ｔｕｎｉｎｇ．．……………………５８４．３．２ ０ｕａｄｒｏｔｏｒ’ｓ ＰｌＤ ｒｅｇｕＩａｔｉｏｎ ｐｒ ４．３．３ ０ｕａｄｒｏｔｏｒ’ｓ ＰｌＤ ｐａｒａｍｅｔｅｒ ４．４ ０ｕａｄｒｏｔｏｒ’ｓ ｆＩ ｉｇｈｔｍｅａｓｕｒｅｄ ｄａｔａ ａｎｄ４．４．１ ＡｃｃｅＩｅｒｏｍｅｔｅｒ ｄａｔａ ｗａｖｅｆｏｒｍｓ ａｎｄ ａｎａＩｙｓｌ ４．４．２ Ｇｙｒｏｓｃｏｐｅ ｄａｔａ ｗａｖｅｆｏｒｍｓ ａｎｄ ４．４．３ Ｋａｌｍａｎ ４．５ ｆｉ ｌｔｅｒｉｎｇ ｆｕｓｉｏｎｓ……………．．６１ａｎａＩｙｓｌｓ…………．．……．６２ｍｅａｓｕｒｅｄｗａｖｅｆｏｒｍ……………．．６３５Ｓｕｍｍａｒｙ ａｎｄ０ｕｔｌｏｏｋ……………………………………………．６５Ｒｅｆｅｒｅｎｃｅｓ………………………………………………６６ ＡｃｋｎｏｗｌｅｄｇｅⅢｅｎｔｓ……………………………………………………．６８Ｖｌｌ第一章绪论第一章绪论１．１选题背景多旋翼飞行器最早出现于百年前的欧洲，由于其控制算法的复杂性，超过了 当时的科技水平，早期的多旋翼飞行器因性能较差和体积庞大而无法实用化，仅 仅停留于科研阶段【１１。近年来由于新材料（碳纤维等）、新型微机电（ＭＥＭＳ） 传感器、微型惯性导航控制器（Ｍ１ＭＵ）以及计算机自动控制算法的不断发展和 进步，以四旋翼飞行器为代表的多旋翼飞行器得以在控制技术上取得巨大飞跃， 开始迅速实用化产业化。并呈现小型化微型化的趋势，逐步在许多行业崭露头角，进入了人们的生活中‘２１。四轴飞行器作为一种有代表性的多旋翼飞行器［３］，具有体积小，重量轻，结构简单、性能可靠、控制灵活、飞行稳定的特点。因为四轴 飞行器比起传统直升机更为稳定和简便可靠，几乎不会出现机械结构上的问题， 它能够成为非常好的无人机平台，以执行一些目前需要小型飞行器来执行的任 务，可以进入一些不易进入的环境之中进行勘察和拍摄等任务，如火山、地震、 洪水灾区等。目前四轴飞行器的应用主要在以下几个领域：（１）航空拍摄 因为四轴飞行器的小型化和灵活稳定的飞行性能又具有一定的载重能力和续航能力，使得其可以搭载拍摄云台和摄像设备，实现一定范围内的航空拍摄任务。比起传统的出动有人操控直升机或者固定翼飞机成本更为低廉而且安全便 捷。目前几乎各种比赛赛事都有多轴飞行器执行航拍任务，是四轴飞行器应用最 为广泛的民用领域。（２）小范围实时监控 四轴飞行器可搭载监控设备进行一定范围内的侦查和监控任务，如军事侦查、火灾或犯罪现场监控等‘４１。四轴飞行器同时在军事和民事警务领域有很广阔 的前景（３）地形勘察和农业 四轴飞行器可以在一定范围内携带勘察设备进行农业、林业等勘察任务，有效节省人力资源。喷洒农药也成为目前四轴飞行器商业运用的一个重要领域［５１。１第一章绪论图１．１早期的四旋翼飞行器（１９２３年）图１．２当代的四轴飞行器１．２国内外研究现状１．２．１国内四旋翼飞行器的研究现状当前四轴飞行器的研究在国内蓬勃发展起来并形成产业。已经有许多的公司 （如Ｄｊ大疆公司）将四轴飞行器等多轴飞行器实现了商业化应用。目前主要研 究点主要集中在：２第一章绪论（１）四轴飞行器乃至多轴飞行器的姿态控制。多轴飞行器的主要技术难点在于其飞行姿态控制系统。因为其旋翼多（一般 为偶数个），因此多轴飞行器的飞行姿态控制远比传统单轴直升机复杂。目前在此领域的研究主要是飞行姿态的数学建模、控制算法以及滤波理论。目前主要的有刚体旋转理论、四元数理论、非线性数字滤波、级联惯性导航、神经元ＰｌＤ 算法等控制理论的综合研究和应用旧Ｊ。（２）适用于多轴飞行器的新的传感器技术发展及新材料的应用 国内外目前也致力于新传感器技术的发展和运用。出现了通用的整合一体的传感器模块，以及这些模块小型化微型化，精度也越来越高，使得多轴飞行器的体积大大缩小。新的材料如碳纤维也运用于机架，使得目前机体重量进一步降低， 载重和续航能力进一步提高。（３）电机、电池领域技术的发展。 近年来，电机技术不断发展。无刷电机和空心杯电机进一步普及应用于多旋翼飞行器上。大大提高了多旋翼飞行器的动力。锂电池甚至燃料电池的出现和应 用使得续航能力大大提高【７ Ｊ。 １．２．２国外四轴飞行器的研究现状 目前国外在四轴飞行器的研究也是主要集中在飞行控制系统的新的理论的 研究，比如神经元网络控制算法、模糊控制算法等。国外还致力于四轴飞行器的 脱离人遥控的自主飞行以及多机协同运作等方面的研究【８】。１．３四旋翼飞行器主要优点和技术难点１．３．１四旋翼飞行器主要优点（１） （２）拥有简单的机械结构。没有传统直升机复杂的旋翼控制机构。 稳定得多的飞行姿态。 四轴飞行器有四个旋翼，没有复杂的机械结构。同时飞行控制系统通过传感器采集飞行姿态数据，实时监测和控制飞行姿态，可以实飞行器保持平稳飞行。 相比传统单旋翼直升飞机，四轴飞行器的飞行更为稳定。（３）由于机械结构简单，容易将体积做得很小，可以实现自主飞行器的小型化微型化。１第一章绪论（４）四轴飞行器拥有四个电机，机身结构简单轻巧，所以其具有更大载重力１．３．２四轴飞行器的技术难点（１）在飞行过程中它不但受到本身软硬件、机体结构等等冈素的影响，还容易受到气流等外部条件的干扰。比如其驱动四个旋翼的电机产牛的震动和干扰， 使得加速度计的数据变化剧烈，含有大量振动噪声；电机产生的磁场会干扰电子 罗盘模块的测量数据。（２）四轴飞行器空间上具有６个自由度，有４个控制输入［９］。其控制变量多、传感器多、数据量大、算法复杂、运算量大和干扰火的特性，使得飞行控制系统 的软件比一般直升机复杂得多。（３）四轴飞行器的主要的姿态传感器陀螺仪传感器输出变化缓慢，但是时间长了有较大的累积误差，而且还有温度漂移。利用陀螺仪进行物体姿态检测需要 考虑到累计误差的消除。加速度计比较敏感、变化速度快，使得加速度计在飞行 过程中的采集到的数据带有大量的噪声（主要是由震动产生的）。因此在四轴飞 行器的飞行姿态控制系统中，必须将陀螺仪和加速度计的数据通过数据滤波算法 进行融合和滤除噪声干扰，以此来得到正确的姿态数据。１．４本文研究的主要内容本设计课题研究的主要内容和思路就是尝试使用通用的ＭＣＵ小系统板，通 用的运动姿态传感器模块、无线通信模块、简单的空心杯小电机、简单机架等， 设计并制作一个小型的四轴飞行器。研究在ＭＣＵ上应用相关的旋转理论来解算 飞行姿态、滤波和数据融合算法，实现对加速度计、陀螺仪等传感器测量的数据 进行计算，使传感器数据合成正确的飞行姿态数据。通过ＭｃＵ对姿态数据解算， 并控制电机，根据地理坐标系，调整和控制飞行姿态，从而初步实现飞行器自主 的稳定的１ｓ行控制。 本课题的主要技术难点在于：（１）传感器主要有加速度计和陀螺仪。这－者得到的数据分别是加速度（归一化到重力加速度场）和角加速度。必须对这两个数据进行运算，计算出欧拉角。 但是采用直接计算出欧拉角的的方法运算量巨大，处理速度缓慢。我们采用了四４第一章绪论元数算法来实现将加速度和角加速度合成为欧拉角。我们就是要在ＭＣＵ板上实 现四元数算法和四元数到欧拉角的转换。（２）四元数法将这二者的数据进行四元数运算然后变换成欧拉角得到，然后是角度数据的滤波和融合，消除掉陀螺仪数据的累积误差。从而得到较为准确的 飞行器姿态数据。我们主要是通过四元数算法加上卡尔曼滤波算法，三轴加速度 计和三轴陀螺仪的数据进行滤波和融合，就能将二者数据合成为三个欧拉角的１ｓ 行姿态数据。（３）由得到姿态数据对比飞行器静止水平时的姿态值，得到偏差量。然后用偏差量控制四个电机通过ＰＩＤ控制方法来使飞行器达到平稳的飞行状态。 本课题基于实现四轴飞行器小型化通用化的思路，利用小型机架、空心杯电机 等，采用加速度计、陀螺仪、电子罗盘、气压计等传感器模块，利用四元数法、 滤波融合算法等设计并制作了小型的四轴飞行器，自主达到平稳飞行的状态１．５本文结构安排本文的机构安排是这样的：第一章是绪论，介绍四轴飞行器的性能和目前的 应用现状和研究现状。第二章主要介绍四轴飞行器的原理、飞行器的坐标系、飞 行姿态的数学表达和姿态数据的换算以及滤波算法，将卡尔曼滤波算法和互补滤 波算法进行比较论证。第三章主要介绍了四轴飞行器的软硬件设计思路。硬件方 面是硬件模块选择和设计。软件方面是平稳飞行控制实现的思路，先进行四元数 姿态运算然后进行卡尔曼滤波。第四章主要介绍了四轴匕行器稳定飞行姿态控制 算法的实现。按照软件思路介绍了基于四元数的姿态算法得出欧拉角。然后进行 卡尔曼滤波，最后描述了卡尔曼滤波的实现。第四章的最后分析了获取的实飞姿 态数据。最后一章第五章是总结和展望。第二章四轴飞行器飞行原理、飞行姿态及滤波算法第二章四轴飞行器飞行原理、飞行姿态及滤波算法２．１四轴飞行器的飞行原理旋翼飞行器的一个旋翼转动，会对机身产生一个反扭矩，如若飞行器只有一个旋翼，旋翼在转动的同时，机身也会朝旋翼反方向旋转，这就是反扭矩作用的结果。四轴飞行器的四个旋翼中，两两旋转方向相反，电机之间的反扭矩平衡抵消，机身不会产生自旋。四轴飞行器的可以分别沿着机体的ｘ、ｙ、ｚ三个轴进行旋转或者平移的运动，因此在每个轴向上有两个自由度。四轴飞行器有六种基本 飞行动作【１０】。（１）升降运动――在图２―１中，升降运动实际上就是飞行器在Ｚ轴方向的上下运动。我们假设四轴飞行器处于平稳状态，此时四个旋翼转速完全一样。此时同时使四个旋翼转速增加，可以让升力克服机体重量，使得机体垂直向上运动。反之，同时减小四个旋翼的转速，可使机体垂直下降。当四个旋翼的处于某一个 转速时，升力和机体重力相等，此时飞行器处于平稳悬停的状态。图２．１四轴飞行器的垂直运动６第二章四轴飞行器飞行原理、飞行姿态及滤波算法（２）俯仰运动――假设以ｘ轴正方向为飞行器前进方向（电机０为前方），俯仰运动是飞行器以机体坐标ｙ轴为中心轴的一个转动。如图２―２，以仰动作为例， 在保持电机ｌ、３转速不变的同时，电机Ｏ的转速增加升力加大，电机２的转速 减小升力降低。飞行器会以机体Ｙ轴（电机ｌ、３所在轴）为转轴产生一个转动， 电机Ｏ上升，而电机２卜－降，这就是飞行器的仰动作。反之。保持电机ｌ、电机 ３的升力不变，电机２转速增加，电机０转速减小，将实现飞行器俯的动作。俯 仰动作需要控制飞行器前后方向上的两个电机的速率来实现。俯仰动作实质上还 会影响飞行器的前进和后退动作。图２．２四轴飞行器的俯仰运动（３）滚转运动――以ｘ轴正方向为飞行器前进方向（电机Ｏ为前方）与俯仰动作原理相同。如图２．３，在保持电机Ｏ和电机２的转速不变不变，分别改变电 机ｌ、３的转速，可使机身以机体坐标的ｘ轴为旋转轴旋转。滚转动作实质上还 会影响１ｓ行器的左移和右移动作。７第二章四轴匕行器毪行原理、匕行姿态及滤波算法图２．３四轴飞行器的滚转运动 （４）偏航运动（自旋）――偏航（转向）运动实际上是机体绕自身坐标ｚ轴的一个白旋转的过程。从单个旋翼来看，旋转中的旋翼会对机体有一个反扭矩。 如果飞行器为单旋翼，在旋翼转动时，机身会朝反方向旋转。因此，多轴飞行器 的旋翼均为偶数个，而且正反转各一半。这样在飞行中，正反转旋翼的反扭矩会 相互抵消。四轴飞行器的偏航运动实质上是通过四个旋翼转速不同而使得扭矩不 平衡的结果，就是使得机身绕机体坐标ｚ轴旋转。如图２―４，当马达Ｏ和马达２ 转速卜升，同时马达ｌ和马达３转速下降，马达Ｏ、２的反扭矩大于电机】、３ 的反扭矩，机身会以机体Ｚ轴为转轴以电机Ｏ、２旋转方向的反方向旋转。反之， 当电机１、３的转速上升，电机０、２的转速下降时，机身会朝与电机Ｏ、３转动 相反的方向旋转。８第二章四轴飞行器飞行原理、飞行姿态及滤波算法图２＿４四轴飞行器的偏航运动 （５）前后平移运动――以ｘ轴正方向为飞行器前进方向（电机０为前方）为了实现飞行器在水平面（ｘ．ｖ面）内左右、前后的动作，要在水平面内对毪行器 施加作用力，使之发生倾斜，进而产生平移。如图２．５中，使飞行器稍微有一个 俯仰运动，即电机０转速减小，电机２转速增加。此时飞行器的实质是一个小的 滚转动作。这时候电机Ｏ下降，电机２上升，机身是俯的动作，机体产生倾斜， 会朝机体ｘ轴的正方向前进。向后飞行与向前毪行相反。图２．５四轴飞行器的前后平移运动９第＿章四轴飞行器飞行原理、飞行姿态及滤波算法（６）左右平移运动――在图２．６中，由于四轴飞行器结构上是对称的，侧向飞行的工作原理与前后运动一样，所不同的是电机１、３增大或减小功率。侧向 和前后运动可以看作是平移运动。图２．６四轴飞行器的左右平移运动２．２四轴飞行器姿态的表示和运算２．２．１坐标系统的建立 四轴飞行器的关键在于其空间姿态的检测、控制和保持。要表征飞行器在空 间中的姿态，必须建立一个空间的直角坐标系来为其姿态作出参照，这个坐标系 称为地理坐标系。因为四轴活动范围比较小，因此可以将地面视作是水平的平面。 在本课题中，我们采用“东北天”地理坐标系来作为参照系…］：Ｚ轴的正方向指 向大顶，负方向指向地理地面；ｘ轴的正方向指向地理上的正东，负方向指向地 理上的正西；Ｙ轴的正方向指向北面，负方向指向南面。同时四轴飞行器本身作 为刚体也存在着一个坐标系，称为机体坐标系（如下图所示）。四轴飞行器处于 平衡状态下，飞行器本身的坐标系和地理坐标系重合，机体坐标系的ｚ轴与地 理坐标系的Ｚ轴重合。四个电机及中心控制部分都分布在ｘＹ平面上。根据刚体 的欧拉旋转定理，四轴飞行器的在空间里飞行姿态可用欧拉角来表示。欧拉角就 是地理坐标系与机体坐标系的旋转关系。坐标系统如图２―７所示：１０第二章四轴飞行器飞行原理、飞行姿态及滤波算法图２．７飞行器的坐标系统 飞行器的姿态，是指在飞行器的正方向的上，用三个姿态角即通常所说的欧拉角表示，包括偏航角（ｙａｗ）、俯仰角（ｐｉｔｃｈ）和滚转角（ｒｏｌｌ），如图２―８。飞行姿态是一个旋转变换，表示机体坐标系与地理坐标系的旋转转换关系，我们定义飞行姿态为机体坐标系向地理坐标系的转换。旋转变换有多种表达和转换的方式，包括方向余弦、欧拉角、四元数法等。 飞行器的姿态是旋转上的某种变换。由欧拉旋转定理可知，一种姿态经过相互串联的一系列旋转可以变为另一个姿态。如果我们用矩阵表示旋转，旋转的串联就由矩阵乘法来实现。第二章四轴飞行器乜行原理、飞行姿态及滤波算法Ｘｂｌ、Ｉ ｚｂ＼。甄 矿ｂ图２．８四轴飞行器的欧拉角２．２．２四轴飞行器飞行姿态的表示和换算欧拉角是飞行器的三个姿态角即俯仰角（ｐｉｔｃｈ）、滚转角（ｒ０１１）和偏航角 （ｙａｗ）。根据欧拉旋转定律，可以用三次旋转使得飞行器本身的坐标系与地理 参考坐标系重合。每一次旋转都是以机体坐标系的ｘ，ｙ，ｚ轴中的一个坐标就是欧拉角姿态矩掣１ ２１。形式如２一ｌ：『ｌｏ ｏ１『ｃｏｓ矽。一ｓｉｎ口］ｏ ｌ ｏ ｏ『ｃｏｓｙｓｉｎｙ ｃｏｓｙｏ０１ｏ １恐（咖＝｜ｏｏｃｏｓＰｓｉｎ≯ｉ眉∥（臼）＝｝ｌｔ（ｙ）＝Ｉ―ｓｉｎｙｏｌ１―ｓ址口ｃｏｓ妒ｉｓｉｎ毋ｃｏｓ目ｌｃｏｓ９ｃｏｓｙｃｏｓ臼ｓｉｎｙ ｓｉｎ妒ｓｉｎ目ｃｏｓ杪＋ｃｏｓ妒ｃｏｓｙ ｃｏｓ妒ｓｉｎ曰ｓｉｎｌｌｆ，一ｓｉｎｐｃｏｓｙ一 ．吼 ｎ甄们彳＝是（ｙ）Ｒ（ｐ）足（妒）２Ｉｓｉｎ妒ｓｉｎ臼ｃｏｓ杪一ｃｏｓ妒ｓｉｎｙ ｃｏｓ妒ｓｉｎ目ｃｏｓｙ＋ｓｉｎ缈ｓｉｎ缈瞄 Ⅸ硌 缈妒 瞄 口口 １● ● ●，ｊ（２．２）１２第二章四轴１｛‘行器１ｓ行原理、飞行姿态及滤波算法由此姿态矩阵Ａ可看出，姿态矩阵采用欧拉角方式计算，三角函数运算比较多，在Ｍｃｕ上实现计算三角函数运算，运算量较大，运算速度缓慢。（２）四元数法采用四元数法，在于归１化后，计算组合旋转的时候比起用欧拉角或方向余 弦的方法运算量大大减少。实际上是将角度变换到多维空间上进行计算，较大减 小了计算量。本课题中，四轴飞行器的初步飞行姿态，我们应用四元数法来计算 飞行姿态‘。３１。 １个四元数由４个实数组成。ｇ＝［％‘ｙ。乙］７旋转的四元数表示法，如（２―４）式，Ｏ是转过的角，是一个单位向量。，目、（２－３）哗２ ｃ０８（ｉ）．，臼、≮２％８ｍ（ｉ）ｐ＝（２．４）．，口、蚱２％８ｍ（ｉ）２ｚｐｚ“８ｍ（－）．，ｐ、四元数乘法的符号为＠，则： ｗｒ２Ｗ―ｑ―ｘｐｘｑ―ｙｐ＞ｑ―ｚｐｚｑｒ＝ｐｏｑ§一２％‘＋０Ｋ＋蚱ｚｇ―ｚｐ％ｙｚｒ（２．５）ｒ２ｗｐｙｑ―ｘｐｚｑ＋ｙ口Ｗｑ＋Ｚ ｐｘｑ２Ｗｐｚｑ七ｘ ｐ）；ｑ―ｙｐｘｑ七ｚ―ｑ四元数转换成矩阵函数Ｒ为：１―２ｙ；一２ｚ；２．～ｙｑ一２ＫｚｇＲ（ｑ）２‘ｚ，＋２％ｙ。２ｙ。ｚ。一２１％％ （２．６）２％ｙｇ＋２，％ｚ。１―２《一２ｚ；２％ｚ。一２ｗ口％２％ｚ。＋２％‘１―２《一２ｙ；由四元数姿态矩阵Ｒ，可以看出四元数姿态矩阵的运算较为简便，基本由加减乘 以及平方构成，运算量减小很多。第一二章四轴飞行器飞行原理、飞行姿态及滤波算法（３）欧拉角转换为四元数刚体绕固定点的任意有限的位移，可由通过此点的某一个轴转过一个角度来 得到。那么在单位时问△ｔ内，我们设刚体旋转的角速度为∞，此转动轴的方向 是已：苫：昌Ｈ所转过的角度西：（２．７）≯：网缸矽的四元数的表示式为：（２．８）ｇ＝ｃｇ。，ｇ－，ｇ：，ｇ，，７＝『；。］＝ 苫ｓｉｎ（要约束条件为：９０２＋９１２＋９２２＋９３２＝１ｃ。ｓ（罢）（２．９）（２一１０）超复数的形式为：ｇ＝ｃ。ｓ（詈）＋ｆｓｉｎ（芝）＋，ｓｉｎ（芝）＋尼ｓｉｎ（罢）约束条件是：ｆ２＝，２＝七２＝一ｌ（２一１１）Ｖ一，仁庀ｊｋ＝一畸＝ｔ 航＝一派＝／（２一１２）代入换角公式：ｃｏｓ西：２ｃ。ｓｚ鱼一１，２ｓｉｎ西：２ ｓｉｎ至ｃ。ｓ至就可以将四元数化为姿态矩掣１６】：９０２＋ｑ１２一９２２一９３２２（９１９２＋ｇ０９３）９０２一９１２＋９２２一９３２２（ｇ１９３一ｇ０９２） ２（９２９３＋ｇ０９１）９０２一纺２一９２２＋９３２爿＝ｌ２（ｇ１９２―９０９３） ２（９１９３＋ｇ０９２）（２－１３）２（９２９３一ｇ０９１）１４第二章四轴飞行器飞行原理、飞行姿态及滤波算法这样就是将欧拉角转换为四兀数矩１５军，以万使计算要态矩阵。（４）四元数转换为欧拉角由三个欧拉角的公式，有：一ｃｔ文矧］臼＝ａｒｃｓｉｎ（４（１，３））协川（２－１５）将四元数代入得到：一ｃｔ可糕］ ｙ＝ａｒ咖（糕］臼＝ａｒｃｓｉｎ（一２（吼９３一吼９２））一ｃｔ吖锱］僖㈣协川（２－１８）泣㈣由此就可以通过四元数得到三个欧拉角（ｒ０１１、ｐｉｔｃｈ、ｙａｗ），从而得到飞行 器的姿态。可以看出，只要计算出四元数ｑ０、ｑｌ、ｑ２、ｑ３，就能够转化成为三个 欧｝奇角，计笪量大大减轩。２．３滤波算法以及修正融合如何获取飞行器当前姿态的数据是飞控系统控制四轴飞行器达到平衡浮空的 基本前提。当采用陀螺仪等需要积分的传感器时，还需要考虑积分的发散性等问 题。近年来陀螺仪、加速度计等微机电传感器越来越成熟、应用广泛，成为低成 本姿态测量的首选器件，因此本课题中测量飞行器姿态使用的传感器是陀螺仪以 及加速度计。由于存在累积误差、温度漂移和干扰，在使用传感器的值进行姿态 的计算之前，要校正相应的传感器 在本课题中，我们主要用到加速度计和陀螺仪两种微机电（ＭＥＭＳ）传感器。 加速度计测量的对象是加速度，通过比照重力加速度，可以用加速度计测量值来 计算出加速度与重力加速度的角度。但是加速度计测量的加速度数据对外部干扰１５第二章四轴飞行器飞行原理、飞行姿态及滤波算法如振动非常敏感，实际运用在飞行器上时，经过实测数据分析，加速度计测得的 数据值存在非常多的振动引起噪声。这些振动噪声主要由电机和螺旋桨的高速转 动所引起的。相比加速度计，陀螺仪测量的数据是角速度。由于其测量数据变化 较为缓慢，陀螺仪本身对外部影响不敏感。但是角速度要通过积分才能计算出角 度，除了前面提及的零度漂移，因此会有累积的误差，运行时间越长，累积误差 越大，会引起姿态数据错误。我们将四个空心杯电机上电后，ＣＰＵ用ＩＩＣ总线读取 集成传感器模块ＭＰＵ一６０５０中的加速度计三个轴的测量值换算后，未经由滤波算法 处理，直接通过蓝牙模块回传、由上位机程序通过蓝牙模块接收存储，转换为波 形。结果如图２―９、２―１０、２―１１所示｝㈣ ≮ 饿 ＩＩｌｌ 渊ｆｒ』１ ｜｜－’＿ １ｆ１ ” ｆｌ蝶？｜ｆ 铽 燃 ～ｐ ｙ””『．叭ｉｊ盯【｛∞”一。１…图２－９加速度计ｘ轴数据波形｝扛 ｆ卜燃 ‘缈 Ⅶ 帆 ‰∥｝誓璺渊辫¨｜删 ‰ｌＩ批【ｌ《？图２．１０加速度计Ｙ轴数据波形１６第二章四轴飞行器飞行原理、飞行姿态及滤波算法图２．１１加速度计ｚ轴数据波形 通过数据波形图，我们可以看到，由于四个电机的启动，６０５０内部加速度计 ｘ、Ｙ、Ｚ个轴的数据带有大量的由高频震动引起的噪声干扰。相比加速度计的数据，ＭＰｕ一６０５０内部三轴陀螺仪的数据在同一时刻读取，经由同样的方式回传记录， 波形图如图２―１２、２―１３、２―１４所示：图２．１２陀螺仪ｘ轴的波形图２．１３陀螺仪Ｙ轴的波形１７第二章四轴飞行器飞行原理、飞行姿态及滤波算法卜＿＿、，…，一．。，ｖ？，ｆ、＾，、…。＾，、 ．№～…１，，＼一―＾；｛ ｐ叠ｌｌｌ图２．１４陀螺仪ｚ轴的波形通过分析陀螺仪测得的数据波形，可以发现ＭＰＵ一６０５０三轴陀螺仪的数据中，同样存在噪声。但是其噪声的幅度小于加速度计波形上的噪声，并不会影响即使的飞行姿态测量，只需要进行校正即可 综合上述数据，从截取的数据波形图中，我们可以得出这样的结论：加速度计测得的值是加速度ｇ，由于灵敏度高和自身特性的原因，在四轴飞行器电机工作时，加速度计会有大量的由振动引起的噪声信号，造成加速度计的波形极为严 重的失真。相较之下，因为陀螺仪传感器本身的特性，其测量值是角速度，对于 机体振动并不敏感。我们可以确定在短时间内由陀螺仪计算出来的姿态角度是正 确的。因为角速度数据通过积分累加计算出角度，所以系统运行一段时间之后有 累积的误差出现。此时计算出来的四元数是包含了误差及噪声的。所以，我们须 在系统中加入有效的滤波算法，滤除噪声以还原Ｘ、Ｙ、Ｚ三个轴的加速度，同时将角速度和加速度进行融合，校正陀螺仪的累积误差，从而得到四轴飞行器的姿态，即三轴欧拉角。２．３．１卡尔曼滤波由于加速度计本身的特性，以及飞行器四个马达和机体的振动，安装在飞行 器上的三轴加速度计采集的三个轴的角速度ｗ数据包含大量的噪声，而陀螺仪本身受噪声影响较小，所以必须对加速度计的数据进行滤波，去除噪声，再和陀螺 仪的数据进行融合，以消除陀螺仪的累积误差，从而得到飞行器正确的姿态。１、卡尔曼滤波算法的原理简介１９６０年，数学家鲁道夫?卡尔曼提出了卡尔曼滤波的算法。卡尔曼滤波的思１８第二章四轴飞行器飞行原理、飞行姿态及滤波算法路并不是得出完全正确无误的当前状态，而是采用一种递归白更新的方法来估计 出当前的状态，且保证原状态与估计出来的状态的均方误差收敛至最小‘２５１。卡 尔曼滤波通过递归更新的算法，可以简单且高效地应用在许多的领域［２ ６１。 我们一般测得的信号都是受到噪声干扰的，如本课题中的加速度计所测的信 号。怎样有效地抑制或消除掉噪声，将所需信号完整提取出来，是信号处理的目 的，滤除噪声的系统我们称为滤波器。我们先只考虑加性噪声的影响，观测数据 ｘ（，ｚ）是信号ｓ（ｎ）与噪声ｖ（玎）之和（如图２．１５），即ｘ（ｎ）＝ｓ（以）＋Ｖ（以）（２－２０）图２一１５输入信号与噪声一般的信号处理模型如图２．１６所示：图２．１６信号处理过程模型 假设信号和噪声没有相关性（即互相关为０）则有：啡）＝渊＝捣１９⑦２Ｄ我们可见到，没有噪声时，信号得到还原；没有输入信号时，噪声完全被滤 除。维纳滤波能够随着信号的强弱自适应地将噪声抑制掉。 维纳滤波的依据是全部的过去测量值和当前状态Ｋ时刻下的测量值进行比 对，以这二者的误差的均方收敛到某个最小值为目标。维纳滤波的结果是一个确 定的传递函数Ｈ。卡尔曼滤波所需的样本依据只是当前状态Ｋ时刻的估计值和 前一个状态Ｋ一１时刻的测量值，通过不断递归更新的方法，并彳ｉ需要追溯过去所第二章四辅飞行器飞行原理、飞行姿态及滤波算法有的观测值，但是目标同样是误差的均方值收敛到最小值。卡尔曼滤波的结果是 预估的状态值。 卡尔曼滤波具有以下的特点：ａ）采用递归的算法。递推的算法就是将Ｋ时刻卡尔曼滤波计算得出的状态 值作为Ｋ＋１时刻的测量值。ｂ）用递归法计算，并不需要知道全部过去的值，用状态方程描述状态变量 的动态变化规律，因此信号可以是平稳的，也可以是非平稳的。卡尔曼滤波 可适用于平稳和非平稳过程。从这一点可以看出，采用卡尔曼滤波算法控制 姿态的飞行器，其初始状态，可以不用是稳态，即平稳起飞。ｃ）卡尔曼滤波采取的误差准则为估计误差的均方值的最小。 假设某系统ｋ时刻的状态变量为枷，状态方程和量测方程也就是输出方程表示为： 状态方程： ％＋１＝４％＋％ 输出方程： ｙ＾＝Ｇ以＋唯 七表示时问，指系统的第尼时刻，相应时刻信号的取值。 ｘｋ＋】表示ｋ＋１时刻的状态变量 ｙｋ表示Ｋ时刻的输出信号 ｕｔ输入信号白噪声 ｖｔ是输出信号白噪声 Ａｔ是前后两个时刻状态变量之间的增益矩阵 ＣＡ表示ｋ＋１时刻状态变量ｘｋ＋１与输出信号之间的增益矩阵 卡尔曼滤波信号处理模型图如图２．１７：（２－２３） （２－２２）２０第二章四轴飞行器飞行原理、飞行姿态及滤波算法图２－１７卡尔曼滤波信号处理模型将状态方程中时问变量尼用缸１代替，得到的状态方程如式４．６所示： 吒＝４％一Ｉ＋Ⅵ一ｌ 那么输出方程为：（２－２４）儿＝ｑ％＋Ｕ（２－２５）设状态变量的增益矩阵Ａ不随时间变化而变化，甜ｂⅥ均为正态白噪声（数学期望为Ｏ），方差是甄还有风，ｇ和风的初态与∞ｔ以及Ⅶ不相关，ｙ，√表示相关系数。 基于上述条件，有如下定义：ｑ：ｑｑ】＝ｏ，或＝ｇ，‰，ｑ２ｇ％（２．２６）Ｋ：研ｕ］－ｏ，《＝Ｒ，ｙ＂，＝Ｒ岛其中：产６脚２１０ｆ１七＝， 足≠√（２―２７）我们先忽略系统中测得的噪声和输入系统的信号，假设这二者为０。卡尔曼 滤波递推回归的思路是这样的：先得估计状态变量以及估计输出信号，得到这二 者的估计值；我们的目标是让状态变量的预估与实际状态变量测量值的差值即均 方差收敛以达到最小，就必须用输出信号的估计值通过某种加权来修正状态变量 的估计值。因此，卡尔曼滤波算法实现的基础是得到使均方误差达到最小的加权 矩阵的最优值。 我们现在先假设输入信号和观测噪声为０，此时的状态方程： 曼?ｔ＝爿赢一】 量测方程为：（２．２８）２１第二章四轴飞行器飞行原理、飞行姿态及滤波算法多’Ｆ Ｃ饭’ｔ＝ｃ胡斑一１Ａｋ为增益参数矩阵、Ｃｋ为校正参数矩阵。（２－２９）假设噪声为ｏ，有一个误差存在于输出信号的估计值与实测值之间，这个误差值用Ａ表示，那么就有：皿＝弦一多’ｔ（２－３０）根据卡尔曼滤波的递推思想，为了提高状态估计的准确度，用输出信号的估 计误差Ａ来校正状态变量。于是就得到如下式：．氪＝爿度ｔ―ｌ＋胁（ｙ女一歹’女）＝彳赢一１＋凰（弘一洲赢一１）其中Ｈｋ为一个加权矩阵。（２－３１）经过校『Ｆ后的状态变量的估计误差及其均方值分别用五和最来表示。把未 经校正的状态变量的估计误差的均方值用只来表示。校正后的状态变量 五＝吒一毫 即状态变量％减去不考虑噪时的状态变量五。那么均方值：（２?３２）只＝Ｅ［（＿一袁）（以一毫）Ｔ］未经过校正的均方值：（２―３３）卑＝目（％一≈）（五一是）’］（２―３４）按照卡尔曼滤波算法，状态变量的估计值与实际值的误差的均方值只要达到最小，首先要推导出最和风之间的关系式，再调整日。使圮达到最小。那么要得到卡尔曼滤波的递推公式，先推导状态变量的估计值袁和状态变量 的估计误差文。然后再计算袁的均方值圪，然后再通过化简Ｒ，得到ｋａｌｍａｎ滤波算法四个递推公式：第二章四轴飞行器ｌ｛Ｉ行原理、匕行姿态及滤波算法毫＝４彝一。＋日。（儿一Ｇ４文一。）峨＝只口（Ｇ只四十Ｒ）。１ 爿＝４只一，《＋Ｑ一。 只＝（』一风Ｇ）只２、卡尔曼滤波算法的递推流程如下（２．３５）ａ）从前一个时刻的状态变量的估计值袁一。和它的均方值只一．导出未更新的均方值：砭＝４只一。群＋ｇ－１． ｂ）得到的《代入日Ｋ＝《《（ｃＫ爿ｃ：＋Ｒ）。中，得到加权参数矩阵也； ｃ）将Ｈｋ代入毫＝４毫一。＋巩（ｙ。一Ｇ４五一．）得到毫 ｄ）将Ｈｋ代入只＝（，一４ｑ）最，得到最至此，就完成了卡尔曼滤波的一次完整的递推过程。卡尔曼滤波是不断递归的运算过程，因此在完成前一次的递推过程后，毫就变为下一次递推运算的毫一，。由此可以看出，若起始状态是非稳态。则卡尔曼滤波需要一定的时问进行若干次 递归运算来达到稳态的收敛的输出值。３、卡尔曼滤波算法存在的问题ａ）卡尔曼滤波需要一个过渡过程来进入稳态。 维纳滤波是要求平稳的随机过程，卡尔曼滤波不要求平稳过程。卡尔曼允许 初始时问不是负无穷大，这在很多情况下是有实际意义的，但是其需要一定的时 间，使其递归先验误差收敛，来进入稳态，而且这段时间可能相对其余滤波融合算法来说要比较长。此外，维纳滤波要求过程的自相关函数和互关函数的简单（先验１知识，而卡尔曼滤波则要求时域中状态变量及信号产生过程的详细知识。 ｂ）模型误差和数值的发散 卡尔曼滤波实现的关键是找到合适的系统状态迭代模型即加权参数矩阵。但 实际的信号处理系统有时很难得到精确的描述，只能用近似的系统模型来替代。 即使能够获得精确的模型，也常会因精确的系统状态模型太过复杂，维数过高而 与实时处理必须减少计算量及尽量简化模型的要求相矛盾。我们建立的近似的系 统状态模型与实际系统必然存在一定的误差，这种误差必然会给滤波带来影响，２３第二章四轴飞行器飞行原理、飞行姿态及滤波算法严重时偏差时（系统状态模型估计错误）还会造成滤波结果（均方误差）不收敛。 若无法收敛，飞行器无法达到稳定状态会造成倾覆。因此在调试时，需要不少的 时间去找寻或者调整增益参数矩阵Ａｋ、校正参数矩阵Ｃｋ，来尽量消除系统模型上 误差和数值发散。ｃ）卡尔曼滤波的运算量大，因而实时性较差卡尔曼滤波算法实时性的决定因素主要是系统状态维数ｎ以及卡尔曼增益矩 阵运算。矩阵运算计算量大。那么一般我们在程序中应采取某些方法或者措施， 例如在精度允许的情况下尽量减少维数等到措施，从而减少计算量以满足实时滤 波的要求。４、卡尔曼滤波算法效果的仿真测试：为了测试验证卡尔曼滤波算法的效果，我们通过蓝牙模块采集四轴飞行器上 ＭＰＵ６０５０加速度计和陀螺仪的实时数据。将其导入ｍａｔｌａｂ中，利用在ｍａｔｌａｂ上编 制卡尔曼滤波代码进行仿真验证，以确定卡尔曼滤波算法的有效性。 采集的部分数据表如表２．１：由于数据太长，只取其一小部分来示意表２．１部分采集的数据加速度计ｘ一３１ ２ １４ １０７ ２４０ ―２５３ ２８０ ―２６１ ―３１９ ―２４１加速度计Ｙ―５８ １４ ８ １６０ ２８１ ―４２５ ―４２９ ―４７９ ―４６１ ―５１９陀螺仪ｘ―３１７８ ６７３ １３１２ 一３１５２ ―３４３２ ―１２６８ ３８９ ―８６２ ―４３７ ５９７陀螺仪ｙ―１０４ｌ ２５９６ ―２５５０ ―３６６２ ―２１２２ ―２０８７ １３５０ ２３６７ １８１３ ３２７０此组数据各轴的完整的数据波形如图２―１８、２―１９、２．２０、２．２１所示：２４第二章四轴飞行器飞行原理、飞行姿态及滤波算法图２．１８ ＭＰｕ６０５０加速度计ｘ轴波形图图２―１９ ＭＰｕ６０５０加速度计Ｙ轴波形图图２．２０ ＭＰｕ６０５０陀螺仪ｘ轴波形图第二章四轴飞行器飞行原理、飞行姿态及滤波算法图２―２ｌ ＭＰｕ６０５０陀螺仪Ｙ轴波形图卡尔曼滤波算法的仿真效果如图２．２２所示：图２．２２卡尔曼滤波算法的仿真效果图从仿真效果图可以看出，卡尔曼滤波算法实现了角速度和角加速度融合成为角度数据输出，达到了滤波融合修正累积误差的效果，对于陀螺仪的实测曲线具 有良好的跟踪性【２９１。２．３．２互补滤波算法互补滤波算法的思路：加速度计由于其自身灵敏度较高。在震动较大的场合， 加速度计的测量值容易混入振动等噪声。也就是说加速度计在瞬时精度上容易受 到外部干扰而有很大误差，直接用加速度计瞬时值通过ｉ角函数计算得出的角度 误差很大。陀螺仪的测量值是角速度。短时间内将角速度按时间积分得到的角度２６第二章四轴飞行器飞行原理、飞行姿态及滤波算法是比较准确的。但是此时得到的角度带有累积误差。我们可以利用这两种传感器的特性来互补得到正确的角度。实现互补滤波的思路就是在将加速度计测量值做一个均值，通过三角函数计算出角度，利用这个值来视时间长短加权校正陀螺仪 测得并换算的角度值，达到去除陀螺仪角度累积误差的效果。简单来说，就是短 时间内陀螺仪的测量值比较准确，以它为主来计算得到姿态；长时间上看用加速度 计滤波后的测量值比较准确，这时候加大加速度计纠正偏差的加权比重。本课题讨论的互补滤波算法就是把加速度计算出的角度取，构造成角度上的校正量，叠加到陀螺仪测出的角度上，实现数据融合。实现过程是这样的：将加 速度计的数据，进行均值运算。均值后的加速度计值为： 首先算出加速度计的均值，有如下式２．３６口ｃｃ＝去∑如，● ｎ（２．３６）ａｃｃ为均值后的加速度计值。设ｇ为重力加速度，我们先用ａｃｃ换算出角度Ａｃｃ： 彳ＣＣ：ａｒｃｃｏｓ（旦竺）ｇ（２―３７）陀螺仪测得的角速度经过积分可以计算出角度ｐ：１／ｎ∑包＝』∞班互补滤波融合的公式如２．３７： Ｇ术（臼＋包）＋４木爿ＣＣ 更新姿态角，则融合后的角度如下：（２－３８）（２－３９）肚伊（¨ＰＭ锄ｃｃｏｓ（争问上是趋近于加速度计的加权平均值。Ｑ．４０）最终修正值日即为融合后的角度。Ｇ和Ａ是根据时间常数算出来的参数，且 Ｇ＋Ａ＝１。在应用中主要是调节这两个值得大小。在短时间内以陀螺仪为主，长时２７第二章四轴飞行器飞行原理、飞行姿态及滤波算法互补滤波器的流程图如图２―２３：图２．２３互补滤波器流程图互补滤波对同一组数据的仿真效果图如图２．２４：图２．２４互补滤波效果图通过图２．２４我们可以看出，采用互补滤波算法同样可以得融合出较为比较 平稳的姿态角度，且跟踪性较好。但是互补滤波比较接近于加速度计的曲线。在 某些情况下并未完全滤除干扰。在四轴飞行器的姿态数据中，加速度的数据变化 剧烈，虽然没有累计误差的问题，但是在干扰强烈的时候，特别是在飞行时有风 （如刚起飞时）的情况下，容易进入不稳定状态而坠机。 对于同一组数据的处理结果的对比，我们会发现在同样的时问下，相比互补 滤波算法，卡尔曼滤波算法更为平稳，可是可以看出，实时性较为不够理想，但 足这可以通过调整其参数矩阵和调节ＰＩＤ参数来弥补。 利用卡尔曼滤波，在设定和调整好参数矩阵的前提下，我们可以做到机体平 稳飞行而不需要其起始状态为平稳的。互补滤波则无法做到起始状态非平稳。卡２８第二章四轴飞行器飞行原理、飞行姿态及滤波算法尔曼滤波的优势在于其适用于稳态和非稳态。因此，在滤波方案中，我们采用｝ 尔曼滤波来实现。 我们又再次采集了一组机体传感器数据，利用Ｍａｔｌａｂ仿真软件对此组数据分 别进行了卡尔曼滤波、互补滤波以及四元数法之后的仿真并绘图‘３１１。 数据的曲线图如图２．２５所示：图２．２５仿真效果波形对比图通过对于同一组机体采集到的数据，在Ｍａｔｌａｂ中的仿真对比，我们可以看出 滤波法中的两个方案，卡尔曼滤波更为平滑，其曲线与陀螺仪相关度较高，有效 地去除了加速度计的噪声，融合出了有效的角度‘３０１。而四元数计算后如果不经 过滤波融合算法，同互补滤波一样更接近于加速度计，其调节速度更快，但是在 长时间的工作过程中，若在稳态中受到严重干扰，容易失去稳定的状态，而且其 起始状态必须为稳定的初态，而且陀螺仪的累积误差始终存在。采用卡尔曼滤波 算法，１ｓ行器控制实时性会较差，也需要时间从稳态进入非稳态，但可以有效地 解决飞行姿态的稳定性问题，抗干扰能力也比较强，在外力干扰较大的时候，稳 定性较强。因此在本课题中，我们选择卡尔曼滤波算法来实现数据的滤波融合， 以实现四轴飞行器的姿态运算。２９第二章四轴飞行器飞行原理、飞行姿态及滤波算法２．４本章小结本章先介绍了四轴飞行器的主要飞行原理和６种基本的飞行动作。四轴飞行 器基本的飞行动作都通过ＣＰＵ控制和调节四个旋翼电机的转速来实现的。然后 本章着重阐述了地理的东北天坐标系统与飞行器机体坐标系之间的换算关系。地 理坐标系是飞行器在空间上的参考坐标系。飞行器本身可以视为刚体，自身也有 一个Ｘ―Ｙ＿Ｚ坐标系来表达其姿态。两个坐标系之间的偏角即为欧拉角。本章介 绍了四轴飞行器飞行姿态的数学表示即欧拉角和四元数，以及欧拉角和四元数之 间的换算。本章还着重论述了卡尔曼和互补两种滤波算法，并使用同一组采集到的机体姿态数据分别采用这两种算法进行了ｍａｔｌａｂ仿真，并根据对仿真的结果，分析决定在本设计中采用卡尔曼滤波算法。３０第三章四轴飞行器的软硬件系统架构第三章四轴飞行器的软硬件系统架构３．１硬件结构框图硬件框图如图３．１所示：图３―１硬件系统框图 如外形图３―２所示，本课题的采用十字形塑料机架。机架是四轴飞行器的主 要承载平台。四个空心杯电机，固定在十字形机架的四个端点上。螺旋桨分为『Ｆ 反桨。正反桨各两个，通过减速齿轮组安装在四个电机上。因为空心杯电机拉力 小但转速快，可通过减速齿轮来转换力矩，以提升拉力。安装在同一轴上的两个 螺旋桨旋转方向相反。整体外观如图３．２：３ｌ第三章四辅飞行器的软硬件系统架构图３－２外观图３．２硬件电路设计四轴飞行器的硬件部分包括：控制电路部分、电机、驱动电路、电池、通讯 模块、传感器模块。控制电路板是四轴飞行器硬件部分的控制核心，主要是Ｍｃｕ小系统板。传感器模块用来检测飞行器的姿态数据。系统板负责从传感器模块采集传感器的姿态数据，融合数据、计算姿态、接收并处理信令和输出控制信号到 电机驱动电路。驱动电路用于驱动四个空心杯电机（０～３号电机）。蓝牙模块用来实现飞行器与上位机之问的通信四轴飞行器的电路设计，主要分为两大部分：３．２．１电机及其驱动电路图３－３无刷电机图３１４空心杯电机３２第三章四轴飞行器的软硬件系统架构如图３．３、３．４所示：四轴飞行器目前采用的电机主要有两类。一种是无刷电机，一种是小型空心杯电机。无刷电机动力强，效率高，负载能力强，耗电量大，需要较大较结实的机架。 目前主流的巾、大型多轴飞行器均采用无刷电机作为动力电机‘均】。无刷电机需 要通过相对应的电子调速器电路对其进行驱动和控制，控制起来较为复杂，成本 也比较高。而日．由于动力大，需要一个更为专业和更多防护的丌发调试和测试环 境，否则存在一定安全性问题，调试过程中容易有安全隐患，比如容易被螺旋桨 打伤。 空心杯电机具有体积小、质量小、转速高、节能、驱动简便、控制简单、精 度高等优点［１８】，非常适合用于微型小型的四轴飞行器。但是其拉力较弱，负载 能力也比较弱。但是可以通过减速齿轮组来转换力矩，使之具有一定的起飞拉力。 因为是本课题是以低成本通用化小型四轴飞行器的为目标的，所以本次设计 采用的是空心杯电机，通过减速齿轮组来提升拉力。本课题使用的空心杯电机额 定电压３．３Ｖ，最大电流为１．２Ａ，最大功率达到２．６ｗ，额定转速３４０００，单电机最 大拉力可达到５．１０９。 电机驱动电路如图３．５，函图３．５空心杯电机驱动电路图四个空心杯电机采用ＮＭＯＳ场效应管ｉｒｌ咖６４叭驱动。场效应管ｉｒｌｍｌ６４０１其单管峰值电流能够达到４．３Ａ，功率可以达到１．３ｗ，导通电压为≥Ｏ．５５Ｖ，完 全满足设计要求。 经过实际测试，单个电机达到最大拉力时，电流达到了１．１２Ａ。单个拉力达３３第三章四轴飞行器的软硬件系统架构到５．１０９，四个电机最大总拉力达到了２０．３６９。３．２．２控制电路部分及传感器模块（１）ＭｃＵ模块的选型及其参数ＭＣＵ控制器是四轴飞行器的大脑。选择一个适合于四轴飞行器非常重要。目前有很多适合于多轴飞行器控制的小型的ＭＣｕ。如意法半导体公司 （ｓＴＭｉｃｒｏｅｌｅｃｔｒ０１１ｉｃｓ）的ｓｔｍ３２，美国ＴＩ公司的ＭｓＰ４３０，ａｔｌｌｌｅｌ公司的ＡＶＲ３２８， Ａｖｌ也５６０等。 四轴飞行器飞行控制系统的通用化是本课题的探索的思路之一，在采用不同 的机架平台上能够自由地移植是我们的目标，因此面对众多的Ｍｃｕ，可以选择 夸ｃＰｕ架构的通用的ａｒｄｕｉｎｏ标准的开源软硬件平台。 ａｒｄｕｉｎｏ是近年来使用了ＡＶＲ系列、ｓｔＩＩｌ３２系列等单片机的一个开放的软、 硬件开发平台，并拥有开源的开发环境和ＩＤＥ。ａｒｄｕｉｎｏ可以看做是一种通用输 入输出单片机系统板。它可以通过数字Ｉ／ｏ口、ＡＤＣ等方式很方便地读取和处 理外部系统（开关、传感器等元器件或模块）的数字或模拟信号，也町以通过输 出控制信号，并通过串口实现与ＰＣ机的通信，结合计算机上的ｎａＳｈ、ｐｒｏｃｅｓｓｉＩｌｇ等软件，可以实现卜下位机的通信和互动等丰富的功能㈣。除了ａｔｍｅｌ公司的ＡｖＲ系列单片机以外，许多ＭＣＵ厂家都针对自产的单片机系列产品，推出支持ａｒｄｕｉｎｏ标准的ＭＣＵ小系统板。如使用ｓｔｌｌｌ３２作为ＣＰＵ的１ｅａ劬ａｐｌｅ ｍｉｎｉ板。英特尔公司也推出自己了基于使用Ｏｕａ呔系列的３２位芯片支持ａｒｄｕｉｎ０软硬件标准的ｇａｌｉｌｅｏ系统板。利用ａｒｄｕｉｎｏ平台，可以实现飞行控制系统的快速开发，而且町以实现飞行控制系统的通用化， 多轴飞行器的快速而且方便移植。 考虑到机体的重量和空心杯电机的拉力，为实现四轴飞行器小型化通用化的 思路，可以选择有兼容ａｒｄｕｉｎｏ标准的ａｒｄｕｉｎｏ控制核心为ＡＶＲ ｍａｇａ）最小系统 板。如：ａｒｄｕｉｎｏ ｎａｎｏ型小系统板（控制核心是ＡｔＩｌｌｅｌ公司的ａｖｒ ｍａｇａ３２８Ｐ），外 形如图３―６： 即可实现小型四轴飞行器飞控系统到大型、３４第三章四轴飞行器的软硬件系统架构图３－６ ａｒｄｕｉｎｏ系列ｎａｎｏ板【３７】还有ｍｉｃｍｄｕｉｎｏ型小系统板（控制核心是ＡｎＴｌｅｌ公司的ａｗ ｍｅｇａ ３２８Ｐ／１６８Ｐ）， 如图３．７：图３－７ ｍｉｃｍｄｕｉＩＩｏ系统板【３５】以及ｌｅａｎａｂｓ团队推出的基于ｓｔＩｌｌ３２的Ｍａｐｌｅ ｍｉｎｉ型小系统板体积小巧的ＭＣＵ 模块【１ ５１，如图３．８：３５第三章四轴飞行器的软硬件系统架构图３．８ｌｅａｎａｂｓ ｓｔｍ３２ ｍａｐｌｅｍｉｌｌｉ模块［１５】性能对比：１、ａｒｄｕｉｎｏｎａＪｌｏｖ３系统板的技术参数如下：（１）、采用８位Ａｔｎｌｅｌ Ａｔｍｅｇａ３２８Ｐ―Ａｕ单片机，主频２０ＭＨｚ （２）、８个模拟输入端口； （３）、１个串口； （４）、６个ＰＷＭ端口； （５）、１２个ｄｉ百ｔａｌ Ｉ／Ｏ端口； （６）、带有一个ＩＩｃ总线接口以及一个ｓＰＩ接口； ２、ｍｉｃｒｏｄｕｉｎｏ系统板的的技术参数如下： （１）、采用８位Ａｎｌｌｅｌ ｍｅｇａｌ２８４单片机，主频２０ＭＨｚ （２）、１４个数字输入、输出端口。（３）、８个１０位精度的ＡｎａｌｏｇＩ／Ｏ模拟输入口（４）、六路ＰＷＭ输出端口 （５）、一个ＩＩＣ总线和一个ＳＰＩ总线接口３、ｓｔＩＩｌ３２ ｍａｐｌｅｍｉｎｉ系统板的的技术参数如下：（１）、采用Ｃｏｒｔｅｘ架构Ｍ３内核的３２位Ｍ３２Ｆ１０３ＲＢＴ６嵌入式单片机 （２）、主频：７２ＭＨｚ （３）、Ｄｉｇｉｔａｌ Ｉ／Ｏ（ＧＰＩＯ）３４个。 （４）、９个１２位精度的Ａｎａｌｏｇ Ｉ／Ｏ模拟输入口（ＡＤＣ） （５）、１２个１６位精度ＰＷＭ端口３６第二章四轴飞行器的软硬件系统架构（６）、２路ＩＩＣ总线和２路ｓＰＩ总线接口 （７）、３个ＵｓＡＲＴ（串口）通道 （８）、７通道直接内存存取（ＤＭＡ） 这三种系统板均带有ＩＩｃ总线接口以及ｓＰＩ接口，可以方便地实现与传感器 模块之间的通信连接，通信速率满足要求。但是基于四轴飞行器飞行控制系统中的传感器数据的采集、数据的运算处理算法的实现。凶此必须考虑到，传感器的数据量较大，传输速度快，卡尔曼滤波互补滤波等算法浮点运算大。所以，基于本课题的实现，我们应该选择主频高的Ｍｃｕ模块。基于３２位嵌入式处理器的ｓｔｍ３２核心的ＬｅａｆＭａｐｌｅ ｍｉｎｉ主频达到７２ＭＨｚ。Ａｔｍｅｌ的ｍｅｇａ系列单片机的主 频为２０Ｍ，而且ｍｅｇａ系列单片机为８位单片机，运算能力丌ｉ如ｓｔｍ３２系列。所 以我们选择ＬｅａｆＭ印ｌｅ的ｓｔｍ３２ ｍ印１ｅ ｍ“系统板来作为主控系统板，它的性能 完全可满足可以满足本课题的要求。 ＳＴＭ３２ｍａｐｌｅｍｉｎｉ是１ｅａｎａｂｓ组织开发的、基于ａｒｄｕｉｎｏ软硬件标准的、采用３２位ｓＴＭ３２单片机为控制核心的开源硬件平台，就是一个兼容ａｒｄｕｉｎｏ的小 系统板。ｓｔｍ３２比ａｖｒｍｅｇａ系列８位单片机拥有有更快的速度和更好的精度，硬 件性能更强，接口资源也更为丰富。ｓｔＩｌｌ３２ ｍｉｎｉ系统板引脚设计及结构符合 ａｒｄｕｉｎｏ标准。ｓｔｌｌｌ３２ ｍａｐｌｅｍｊｎｉ系统板结合了ｓｔｌｌｌ３２嵌入式单片机的强大性能以 及ａｒｄｕｉｎｏ易用、普及、丌放的软硬件架构，可以实现快速、灵活兼顾强大性能 的工程丌发。（２）传感器模块的论证、选择及其参数四轴飞行器所需的传感器主要有：加速度计、陀螺仪、电子罗盘以及气压计。加速度计主要用来测量加速度、陀螺仪主要用来测量角加速度，电子罗盘用来测量方向，以纠正自旋。气压计用来测量高度，便于飞行时定高。（１）加速度计和陀螺仪 ｌ、加速度计目前主流的型号有ＡＤｘＬ３４５、ＢＭＡｌ８０和ＭＰｕ６０５０等。ＡＤｘＬ３４５是由ＡＮＡＬＯＧ ＤＥＶＩＣＥ公司推出的低功耗集成数字式３轴加速 度计，固定测量精度为１０位，但可以根据测量的加速度大小动态改变，测量加 速度值达到最大的±１６９时，此时最高精度可以达到１３位，ｘ、ｙ、ｚ轴分辨率分别为：２３０ＬＳＢ协２５６ ＬＳＢ／ｇ、２８２ ＬＳＢ儋。工作电压为２．５Ｖ～３．６Ｖ。拥有一个ＩＩＣ３７第■章四轴飞行器的软硬件系统架构总线接口和一个ｓＰＩ总线接口来进行通信。 ＢＭＡｌ８０是博世（ＢＯＳＣＨ）公司推出的超高精度三轴加速度传感器，测量精 度可以达到１４位，最大精度可达到８１９２ ＬＳＢ儋，工作电压为１．６２Ｖ～３．６Ｖ，也同 样拥有ＩＩＣ总线和ｓＰＩ总线接ｕ。（２）陀螺仪的主要型号有ｌＴＧ３２００和ＭＰｕ６０５０等。ＩＴＧ３２００是高精度陀螺仪芯片，其测量的角速度精度是１６位，分辨率在１４．３７５ＬｓＢ。／ｓ，最大测量范围在２０００ ｏ／ｓ，工作电压２．６Ｖ，使用ＩＩＣ总线接口进行数据传输。 ＭＰＵ．６０５０是结合了陀螺仪与加速度计，集成一体的姿态测量芯片。它内部 集成了３轴加速度计、３轴陀螺仪。ＭＰｕ６０５０的陀螺仪与加速度计均具备１６位 精度的ＡＤｃ模数转换器。内部陀螺仪的量程分别为±２５０。／秒，±５００。／秒， ±１０００。／秒，±２０００。／秒，加速度计的量程分别为±２９，±４９，±８９，±１６９。 加速度计和陀螺仪的量程可在程序通过寄存器中动态地改变和设定。ＭＰｕ６０５０ 使用ＩＩｃ总线进行连接，同时也可以利用ＩＩｃ总线扩展连接其他传感器模块。 基于我们实现小型化通用化四轴飞行器的思路，因为目前大部分传感器模块 都很方便地使用ＩＩＣ总线连接和通信，通信速度上ＩＩＣ总线可以达到４００ＫＨｚ， 速度上完全可以满足我们的设计要求。因此可以选用ＡＤｘＬ３４５和ＩＴＧ３２００等 ＭＥＭＳ传感器作为测量飞行器姿态的传感器。但是，相比前二者，同样使用ＩＩＣ 总线通信，ＭＰＵ６０５０具有更高１６位精度，而且它同时集成了加速度计模块和陀螺仪模士央【】７］。从重量上看，一块ＭＰｕ６０５０比两块独立的加速度计模块还有陀螺仪模块，总量上和体积上更轻巧。小型化的四轴飞行器，在机体重量上必须要通 盘考虑。因此，从课题的思路出发，综合各方面性能考虑ＭＰＵ６０５０更具有优势。 所以本课题使用ＭＰｕ６０５０集成姿态测量模块作为飞行器的姿态检测传感器。３８第三章四轴飞行器的软硬件系统架构ＭＰｕ．６０５０模块外观如图３．９：图３．９ ＭＰｕ６０５０模块【３４】（３）修正ＭＰｕ６０５０内部陀螺仪的零飘当陀螺仪静止时，理论上其输出应该为０，但实际上静止时陀螺仪会有一个 零度飘移。即输出不为０值。零飘会影响陀螺仪的测量值，导致飞行姿态测量错 误。我们采用取均值的方式来纠正零飘。在系统初始化中，我们读取若干个陀螺仪零飘值，取平均值，就得到了本次运行的平均零飘值。在后面的程序中，只需将读取的陀螺仪值，减去此均值即可。如３―１和３―２：如Ｘ，是陀螺仪读取值。Ａ为 零飘值的平均值。Ｙ，是校正后的陀螺仪测量值彳＝｛∑■ ７。，ｚ（３―１）ｌ＝（墨一彳）?彩加（３―２）因为ＭＰＵ６０５０的陀螺仪存在零值飘移的情况，即在机体并没有动的情况下， 陀螺仪会有一个输出。且此零飘值并不确定，会随着环境变化而变化。因此要在 初始化的时候，先将陀螺仪的零飘校正回来。在实际编程中，采取的方法是在静 止的读取１００个零飘数值做零飘的均值。在工作时读取的数据均减去此均值３９第三章四轴飞行器的软硬件系统架构（４）电子罗盘模块电子罗盘模块用来测量磁场强度，通过换算可以得到方向偏角。本课题中电子罗盘模块选用ＨＭｃ５８８３。如图３．１０：图３．１０ ＨＭｃ５８船模块【３６］（５）气压计 小型四轴飞行器的载重量较小，超声波模块重量较大，因此我们选用气压计 来实现高度的测量便于飞行器的定高。气压计我们选择ＢＭＰ０５气压测量模块。 如图３．１１：图３．１１ ＢＭＰ０５气压测量模块第三章四轴飞行器的软硬件系统架构（６）蓝牙模块选用ＢＣ一０４如图３―１２：１．ｒＸＤ ２．ＲｘＤ２８．酗ＡｓＴＥＲ，ｓＬＡｖＥ ２７．ｓ１鼙，Ｈｗ２６．ｃ【ＥＡＲ２５．Ｍｃｕ．１Ｎｒ２４，ＬＥＤ １２．３．３ｖ ｉ３．ＧＮＤ图３．１２ Ｂｃ－４蓝牙模块【３８】总体硬件电路连接图３．１３：图３．１３硬件原理图４１第三章四轴飞行器的软硬件系统架构３．３系统板实物图从硬件外观图，可以看出，我们把硬件模块全部都集中在机架中央，这样有 助于稳定重心，保持四轴飞行器的重心在机架中央，使得平衡调整较为简单，基 本无需做过多调整。转接底板的作用是将各个传感器模块通过ＩＩＣ总线和ＳＴＭ３２Ｍａｐｌｅ系统板连接在一起。转接底板搭载电机驱动电路并和系统板连接，同时还 承载电池和各个模块的连线。４２第ｊ章四轴飞行器的软硬件系统架构３．４软件框图及思路采集三轴加速 度计测量数据上采集三轴陀螺 仪测量数据上四元数法飞行器 姿态解算上四元数转换成欧 拉角上卡尔曼滤波及融 合以消除误差上欧拉角转换为偏 移量上读取蓝牙遥控信号‘ＰＩＤ控制ＪＰｗＭ输出到电机上Ｉ刚ｌ图３．１５总体软件框图根据本课题的设计思路还有四轴飞行器硬件系统结构，总程序框图如图３．１５。总体软件思路：程序在ｓｔｍ３２和ＭＰＵ６０５０等各传感器模块上电之后，首先４３第■章四轴飞行器的软硬件系统架构进行系统的初始化，在初始化中要配置Ｍｃｕ自身以及配置传感器，设定传感器 模块的参数。初始化之后是获耿传感器数据。传感器数据经过公式换算，ｊ轴加 速度计会得到ｉ轴的加速度值，三轴陀螺仪会得到三轴角加速度。我们将加速度 和角速度采用四元数算法来计算出飞行姿态。运算的结果可以转化为表征飞行器 姿态的三个欧拉角：滚转角ｍ１１、俯仰角ｐｉｔｃｈ、偏航角ｙａｗ。但是【大Ｊ为这样得到 的欧拉角数据是没有经过滤波算法而得来的。陀螺仪的测量值吲为积分运算会有 累积的误差。这个累积误差与时间的长短成正比。飞行一段时间后就会出现姿态 错误的现象。因此我们必须运用滤波算法对得出的欧拉角、以及原先的角加速度 数据进行滤波和长期融合，从而得到正确的欧拉角。３．５本章小结本章主要介绍了四轴飞行器的软硬件设计。我们先对比电机的方案进行选 择，从设计思路出发，我们选择了使用空心杯电机。根据设计思路，我们对ＭＣＵ 小系统模块进行对比和选型，确定选择ＳＴＭ３２ｍａｐｌｅｍｉｎｉ作为主控模块。传感器模块方面，通过对比论证，我们选择集成了陀螺仪及加速度计ＭＰｕ６０５０模块 作为姿态传感器。软件设计方面，主要阐述了软件设计的思路和根据思路给出了 软件结构框图。本文实现四轴飞行器平稳飞行的思路是ｃＰｕ从姿态传感器中采 集姿态数据通过四元数算法进行姿态运算，得到飞行器的一个初步的姿态（三个 欧拉角），然后针对这三个欧拉角进行卡尔曼滤波，消除累积误差和干扰，最终 得到三个准确的欧拉角。利用这三个欧拉角，通过ＰＩＤ控制方法，来稳定１５Ｉ行 器的飞行姿态。第四章飞行姿态控制算法实现第四章飞行姿态控制算法实现４．１软件初始化过程１、ｃＰｕ的初始化ＣＰＵ的初始化主要进行ＭＣＵ本身的设置和操作，设定ＭＣｕ的一些功能处 于正确的状态或工作模式，主要完成如下几点：（１）关闭全局中断在ＳＴＭ３２ ｍａｐｌｅ ｍｉｎｉ上电后，因为默认全局中断是开启的，可能会影响后 续初始化，所以在程序的最开头先要先将全局中断关闭，必要时再打开。（２）波特率设为１１５２００，启动串口１因为使用了蓝牙串口模块，蓝牙模块Ｂｃ０４的串口Ｒｘ、ＴＸ端分别和系统板 的串口Ｔｘ、ＲＸ（Ｄ２６ Ｄ２５）端连接。所以，将串口通信波特率设置在１１５２００， 以方便和蓝牙模块进行通信，实现数据回传上位机以及遥控功能。（３）初始化ＩＩｃ总线。 ＩＩｃ总线是沟通Ｍｃｕ和各个传感器模块的主要数据通道。根据ｓＴＭ３２ ｍａｐｌｅｍｉＪｌｊ的数据手册，该系统板｝：２对ＩＩＣ总线接口ＳＣＬ、ＳＤＡ两线分别为１脚和０ 脚，还有１５脚和１６脚。我们选择１５脚和１６脚作为ＩＩＣ总线的接口。（４）初始化ＰｗＭ输出状态我们利用Ｍ印１ｅ系统板输出ＰｗＭ波，用来控制四个电机的转速。在ａｒｄｕｉｎｏ 标准中，ＰｗＭ频率固定在４９０Ｈｚ，但在Ｍ叩１ｅ中，ＰｗＭ频率是可调的，默认 频率为５５０Ｈｚ。如连接图所示，我们设置Ｄ３、Ｄ４、Ｄ５、Ｄ８为输出ＰｗＭ信号 的四个引脚分别输出到四路ＰＷＭ控制信号信号电机的四个Ｍｏｓ驱动管。（５）初始化定时器时间设置５Ｍｓ设定主程序循环运行一次的时间为５ｍｓ，也即是程序从开始采集数据到运算 完成输出角度这样一个循环的时间长度。４５第四章飞行姿态控制算法实现２、传感器的初始化表４―１名称 设备地址ＭＰｕ６０５０主要寄存器地址和功能寄存器地址０ｘ７５ ０ｘ６ｂ ０ｘ１９ ０ｘ１Ｂ ０ｘｌＣ说明 设备的ＩＩＣ地址，默认值０ｘ６８ 设置电源模式（休眠等） 设置采样率 陀螺仪配置寄存器 加速度讣配置寄存器复位 采样率分频器陀螺仪配置 加速度计配置初始化ＭＰＵ６０５０，首先就是先将ＭＰＵ６０５０复位一下，即向ｏｘ６ｂ送数１ ｘｘｘ ｘｘｘｘ先回到卜电初始状态，免除随机状态的麻烦。接下来是将休眠状态解除，即 向ｏｘ６ｂ地址送数００Ｈ。然后进行ＭＰｕ６０５０的相关寄存器设置，以设置ＭＰｕ６０５０ 的工作状态。主要有：（１）设置陀螺仪的采样率分频表４．２Ｒｅｇｉｓｔｅｒ （ｈｅｘ） Ｒｅｇｉｓｔｅｒ （Ｄｅｃｉｍａｌ）ＭＰｕ６０５０的采样率设置寄存器（ｏｘｌ９）Ｂｉｔ７ Ｂｉｔ６ Ｂｉｔ５ Ｂｉｔ４ Ｂｉｔ３ Ｂｉｔ２ Ｂｉｔｌ ＢｉｔＯ１９２５ＳＭＰＬＲＴ＿ＤⅣ７根据ＭＰｕ６０５０的资料表４．２，Ｏｘｌ９是陀螺仪采样率分频器设置寄存器。采样频率＝陀螺仪输出频率／（１＋设定值）。陀螺仪输出频率由ＤＬＰＦ ｃＦＧ（ｏｘｌＡ的 后三位）的值所决定。若ＤＬＰＦ为无效（ＤＬＰＦＣＦＧ＝０００ｏｒ或者１１１），陀螺仪输出频率为８Ｋ；若ＤＬＰＦ为有效（ＤＬＰＦ ＣＦＧ＝００１～１１０），陀螺仪输出频率为 １Ｋ。通过向Ｏｘｌ９写入分频数据０００００１１１，将６０５０的陀螺仪采样率定为８ｋ。（２）三轴陀螺仪的激活和量程设置表４．３ＲｅｇｉｓｔｅｒＭＰｕ６０５０陀螺仪设置寄存器（ｏｘｌＢ）Ｂｉｔ６ Ｂｉｔ５ Ｂｉｔ４ Ｂｉｔ３ Ｂｉｔ２ Ｂｉｔｌ Ｂｉｔ０Ｒｅｇｉｓｔｅ‘ （Ｄｅｃｉｍ Ｂｉｔ７（ｈｅｘ）１Ｂａ１）２７ＸＧＳＴＹＧＳＴＺＧＳＴＦＳ―ＳＥＬ［１：０］根据ＭＰｕ６０５０芯片资料表４―３陀螺仪的配置寄存器地址为０ｘ１Ｂ，陀螺仪的 ３个轴通过设置这个寄存器的ｘＧ―ＳＴ，，ＹＧ―ＳＴ’和ｚＧ二ｓＴ位来激活。送数０为 激活。Ｆｓ ｓＥＬ是两位二进制数，来配置和改变陀螺仪的四种量程，如表４．４：第四章飞行姿态控制算法实现表４―４陀螺仪量程设置对应表ＦＳ―ＳＥＬ【１：０】０ １ ２ ３ Ｆｕ¨Ｓｃａｌｅ Ｒａｎｇｅ±２５００ ±５０００ ±１００００ ±２００００在这里我们对０ｘ１Ｂ写入００００１０００（０８Ｈ），来启动三轴陀螺仪，并设置其量程为±５０００／ｓ（３）三轴加速度计的激活和量程设置 表４―５加速度计设置寄存器（ｏｘｌｃ）Ｒｅ百ｓｔｅｒＲｅｇｉｓｔｅｒＢｉｔ７ （Ｄｅｃｉ ｍａｌ）Ｂｉｔ６Ｂｉｔ５Ｂｉ“Ｂｉｔ３Ｂｉｔ２ＢｉｔｌＢｉｔＯ（ｈｅｘ）ｌＣ２８ＸＡ ＳＴＹＡＳＴＺＡＳＴＡＦＳ―ＳＥＬ【１：０】如表４．５加速度计配置的寄存器地址为０ｘｌｃ，每个加速度计的轴通过控制这个寄存器的ｘＡ―ＳＴ，，ＹＡ―ＳＴ，和ＺＡ。ＳＴ位来激活。送数０为激活。ＡＦｓ―ＳＥＬ 用来设置加速度计的量程，如表４．６：表４．６加速度计量程设置对应表ＡＦＳ―ＳＥＬ【１：Ｏｌ０ １ ２３ＦｕⅡＳｃａｌｅ Ｒａｎｇｅ±２９ ±４９ ±８９＋１６９在这里我们对０ｘｌｃ写入００００１０００（０８Ｈ），激活三轴陀螺仪，并设置其量程为±４９。４．２四元数法及其姿态解算的算法实现在主程序中，通过多次调用四元数子程序，将三轴加速度计和三轴陀螺仪计 算得出初步的角度数据。这些角度数据为平面角度，现在我们需要将角度数据转 化成为欧拉角，以实现姿态的结算。在这里，我们通过四元数法，先对角度数据进行四元数计算，并将四元数转换计算得到欧拉角。４７第四章飞行姿态摔制算法实现４．２．１四元数运算及四元数转欧拉角算法流程初始化四元数（１）假设当前的坐标系为地理坐标系，则四元数列向量［２１】ｇ＝［９０（２）ｇ。ｇ：ｇ，】７＝［１ｏｏｏ］７（４―１）获取角速度、加速度。 从ＭＰＵ６０５０读取三轴加速度计的测量值即加速度ａｃｃ，、口ｃｃ。、以ｃｃｚ，陀螺仪的测量值即角速度ｑ，∞。，Ｑ（３）将三个轴加速度计得出来三个轴的加速度值日ｃｃ，、以ｃｃ，、以ｃｔ转化为三维的单位向量（规范化）【２０１。得到：ｄＪ２面ｉ袁口ＣＣ― 口ｃｃ．ｑ２面ｉ毒面口ＣＣ一件２’口．＝―１２＝＝＝＝＝＝＝＝三－＿＝＝＝＝＝＝一２√ｎｃ《＋口ｃ弓＋口ｃ《（４）从四元数里获得三轴的重力量融，哆，圪：【圪＝ｑ；一ｇ？一ｇ；＋ｇ；（５）得到四元数重力量之后将其与加速度计误差值，作为陀螺仪修证量［２２】： ｑ＝口，｛哆一呸木＿｛Ｐ，＝口：｝■一口，木哆 【巳＝ａ，；０一哆＋圪（４―４）』巳ｊｎｔ ２舫ｆ巳砒＋航ｆ，？出Ｉｑ柚２哆＋印４巳＋肮．｜：巳巩（４―５ａ）第四章飞行姿态控制算法实现ｊｅｍｔ＝肪ｆ勺以＋肪￡。勺砒 【哆谳＝哆＋印木勺＋岛ｆ嘭出（４．５ｂ）』吃叫２航ｆ乞比＋舷￡。吃出【哆。。ｔ ２哆＋印木Ｐ：＋兢Ｊ：乞出Ｏ，ｋｐ可以以０初始，０．０１为步进调节。（７）（４．５ｃ）其中：ｋｉ和ｋｐ是一个调整参数，需在实际调试中来确定。其中，ｋｉ