四轴飞行器设计与实现
姓 名 李亚东
专 业 软件工程
学 号 1250210907
学 籍 校 天津工业大学
学校指导教师 张艳丽讲师
企业指导教师 王西鹏工程师
2014年 6 月
摘 要
四轴飞行器以其平稳的飞行姿态、硬件结构相对简单等优势成为近几年来用于搜救、航拍等领域的佼佼者。现如今关于四轴飞行器的研究已经如火如荼，不管是航模爱好者还是研究机构都对四轴飞行器情有独钟。本文主要的研究方向是四轴飞行器的硬件工作原理、软件算法等方面的专业知识。硬件方面主要包括基于STM32的核心飞控板，飞控板上面集成了一块儿MPU6050用于检测角加速度以及重力加速度、用于控制电流大小的电子调速器、驱动螺旋桨给四轴飞行器提供升力的直流无刷电机、基于A7105射频芯片向STM32发射控制信号的遥控器等。软件方面主要介绍算法的应用、飞行姿态的校准等。
关键词：四轴飞行器；STM32；MPU6050；A7105
ABSTRACT
Four aircraft flight attitude with its smooth, relatively simple hardware structure in recent years, which has become the leader for search and rescue, aerial photography and other fields. Nowadays studies on Four aircraft have been in full swing, whether or research institutes model aircraft enthusiasts are soft spot for Four aircraft. The main research direction of this paper is the working principle of the hardware expertise, software, algorithms and other aspects of the axis of the aircraft. Hardware, including core-based STM32 flight control panels, integrated flight control board above together MPU6050 for detecting angular acceleration and gravity, is used to control the current size of the electronic governor, propeller-driven aircraft to provide lift axis brushless DC motor, A7105 RF chip based STM32 transmit control signals to the remote control. Introduces software applications, such as flight attitude calibration algorithm.
Keywords：STM32；MPU6050；A7105
目 录
前言…………………………………………………………………………………………………………………………….. 1
第一章 概述…………………………………………………………………………………………………………….. 2
1.1 四轴飞行器的研究背景………………………………………………………………………………. 2
1.2 四轴飞行器的发展历程………………………………………………………………………………. 2
1.3 国内外研究现状…………………………………………………………………………………………. 3
第二章 四轴飞行器的总体设计…………………………………………………………………………… 5
2.1 四轴飞行器的零件清单………………………………………………………………………………. 5
2.2 四轴飞行器的功能特征………………………………………………………………………………. 5
2.3 四轴飞行器的总体方案………………………………………………………………………………. 6
第三章 四轴飞行器的硬件设计…………………………………………………………………………… 8
3.1 飞行器的硬件选择……………………………………………………………………………………… 8
3.2 飞行器的硬件详解……………………………………………………………………………………… 8
3.2.1 基于STM32的核心飞控板………………………………………………………………… 8
3.2.2 电子调速器……………………………………………………………………………………….. 9
3.2.3 传感器…………………………………………………………………………………………….. 10
3.2.4 直流无刷电机………………………………………………………………………………….. 12
3.2.5 螺旋桨…………………………………………………………………………………………….. 13
3.3 飞行原理………………………………………………………………………………………………….. 14
第四章 四轴飞行器的软件设计与调试……………………………………………………………. 16
4.1 软件实现………………………………………………………………………………………………….. 16
4.2 调试…………………………………………………………………………………………………………. 19
4.3 试飞及结论………………………………………………………………………………………………. 22
结语…………………………………………………………………………………………………………………………… 24
参考文献…………………………………………………………………………………………………………………… 25
附录一……………………………………………………………………………………………………………………….. 26
附录二……………………………………………………………………………………………………………………….. 28
附录三……………………………………………………………………………………………………………………….. 37
Ⅰ英文原文…………………………………………………………………………………………………….. 37
Ⅱ中文译文…………………………………………………………………………………………………….. 40
致 谢……………………………………………………………………………………………………………………….. 42
前言
四轴飞行器最开始是由航模爱好者研发的一种新式飞行器。随着四轴飞行器的相关配件，如MEMS传感器以及单片机、锂电池、之流无刷电机的技术发展和普及，四轴飞行器成为航模界的新锐力量。到今天，四轴飞行器已经在诸如军事打击、公安追捕、灾害搜救、农林业调查、输电线巡查、广告宣传航拍、航模玩具等一些特殊领域内得到了广泛的应用，四轴飞行器俨然已经成为非常重要的遥感应用平台。
仅以农林业的调查为例子，传统的调查方式是实地到农林抽样调查或者采用用航空级遥感平台。如果采用抽样的话，工作量会非常的大，并且调查的结果会受到人的主观因素的影响；如果采用航空级遥感平台的话就可以在很大的范围内同时调查，这样的调查结果不但及时，而且准确，不足点就是只能得到大型作物的宏观的指标，而且成本很高。对于山区的小地块以及其他复杂环境的地方就很难用上航空遥感。而四轴飞行器作为低空遥感平台在此刻就显得尤为重要。
就目前来说应用比较广泛的飞行器有儋州直升机和固定翼飞行器。四轴飞行器比起固定翼飞行器的优点有：机动性好，动作灵活，可以垂直起飞降落和悬停，缺点是续航时间短得多、飞行速度不快；四轴飞行器的机械结构相对于单轴直升机来说机械结构更加简单，抵消反力矩的螺旋桨能够提供向上的升力，成本也比较低。
本文介绍的是依靠锂电池驱动的直流无刷电机的四轴飞行器，介绍其依靠STM32的实现方案，着重介绍四轴飞行器的原理。
第一章 概述
1.1 四轴飞行器的研究背景
无人飞行器指的是能够依靠自身的程序设定自主飞行，不需要依靠人在其上操作的飞行器；它利用螺旋桨或者其他动力装置所产生的空气动力来抵消自身的重力，能够实现自主智能飞行或者是在人操作遥控器的控制下遥控飞行；随着近些年来无人机方面科研技术步伐的加快，无人飞行器得到了其发展世上最快速的进步，由于无人飞行器具有重要的军事意义，所以军方非常重视，民间来说，航模爱好者也对它非常青睐，各方面的应用如：地形勘察，监视，测绘等等，如果依靠无人飞行器来完成这些任务的话，不仅操作灵活，降低成本，还可以避免人员消耗。
飞行器依据机翼类型可分为固定翼飞行器和旋翼飞行器。[1]在过去的几十年里，固定翼无人飞行器已经有较成熟的技术，曾经为美国、以色列等国家的军队在战争中取得胜利作出重大贡献，也在军事领域中显示了它独特的性能。在“伊拉克行动”中也有应用，美军大量使用了无人侦察机，用来对目标进行识别和锁定。
相对固定翼无人飞行器，旋翼飞行器的发展却较为缓慢，这是因为旋翼无人飞行器的控制较固定翼复杂，早期的技术水平无法实现飞行器的自主飞行控制；然而，旋翼飞行器却拥有其自身独特的优点：1）机械结构较为简单，只需要协调旋翼电机的转速即可实现控制，飞行较为灵活；2）不受较大机翼的限制，能够应用于各种环境中；3）具备自主起飞和降落的功能，系统高度智能，可以实现较多的飞行姿态，例如：垂直悬停、俯仰升降、偏航转向等等，且飞行器姿态保持能力较高。这些优势也就决定了旋翼飞行器在未来将有着更为广阔的应用前景。
随着社会上各种科研机构对旋翼飞行器越来越重视，从20世纪50年代到现在，各个科研机构的不同类型的旋翼飞行器竞相出现，由于人们对所谓的外星人以及飞碟非常感兴趣，所以碟形飞行器的出现引起的极大的关注，各个科研机构都做出了自己的品牌的碟形飞行器如美国的“Cypher ”、加拿大的CL-327“哨兵”等；四轴飞行器就是这种“蝶形”的飞行器，其以硬件布局结构新颖，飞行方式独特的特点，逐渐成为当前飞行控制领域研究的热点。
四轴飞行器跟其他飞行器相比较来说，其优点在于构造简单、飞行灵活，较传统的直升机更容易接近任务目标；更重要的是，四轴飞行器的研究涉及到众多领域的高、精、尖技术，如：空气动力学、自动控制、自主导航、传感技术等等；目前随着传感器技术和计算机技术的不断发展，四轴飞行器也为这些领域提供了一个综合的研究平台。因此，无论是科学研究，还是综合应用四轴飞行器的研究都有着极高的研究价值。
1.2 四轴飞行器的发展历程
关于四轴飞行器的发展历程，最早的可以追溯到100年以前。
早在1907年的时候，法国的Breguet兄弟就制造出来了世界上第一架四轴飞行器。这架飞行器不是现在能够见到的小飞行器。当时兄弟两个造出来的飞行器可以载着两个人飞起来。由于当时没有采用控制类的技术，所以飞行的稳定性能很差劲。
Breguet兄弟的四轴飞行器的面世引起了极大的轰动，尤其是军方。1921年，美国空军就建造了一架大型的四轴飞行器，由于技术还不太成熟，所以进行了多次试飞还是没有办法很好的控制四轴飞行器的飞行，其稳定性远远没有达到美国空军部的要求。
随着四轴飞行器相关技术的发展，在1924年的时候已经出现了能够达到一千米飞行高度的四轴飞行器。当然，这架四轴飞行器还是载人的，并且实现的是垂直飞行。这架四轴飞行器叫做Oemichen。
对现代四轴飞行器影响最大的算是1956年Convertawing的研究成果了。这架四轴飞行器用两个发动机给四个超过19英寸的螺旋桨提供力量，其最大的成果在于改变螺旋桨的推力来控制四轴飞行器。
从1956年之后的很长一段时间里，四轴飞行器的相关技术没有没有什么重大的进展。或许就是技术的限制，很多先进的想法没有办法实现。最近的这些年，微处理器、微控制系统、传感器以及机械控制理论等四轴飞行器相关技术的大步发展，使得四轴飞行器的综合技术得以巨大的进步。现在的研究方向主要是集中在实现四轴飞行器更小的结构、更灵活的控制以及混合能源等方面。
1.3 国内外研究现状
一、Draganflyer 四轴
Draganflyer是RC电子玩具公司研制生产的一个用于商业销售的一系列产品，与大多数四轴飞行器一样它由无线遥控器和控制系统处理器共同实现对飞行器的运行控制；其中控制系统包括：一个与遥控器对应的无线接收装置，三个角速度传感器分别测量三个反向的角速度，以及必要的微控制器来处理这些数据。[3]目前最新的Draganflyer 为Draganflyer X6。这款飞行器不是传统的四轴飞行器，而是采用了不太稳定的三轴设计方案，在电机部分安装了上下两个电机，以此来弥补因为缺少一个轴而产生的扭矩差。这种设计还能够实现更灵活的飞行需求。除了基本的配件之外还安装了三个红外热传感模块，主要是用来协助此飞行器在室外飞行时的平衡保持。此飞行器拥有500克的有效荷载能力，GPS设备使其拥有位置保持功能，其框架是由碳纤维杆组装而成的，并且这款飞行器能够折叠起来。[4]
二、Quattrocopter 四轴
Quattrocopter是一个德国的科研机构研究的一种新型概念的四轴飞行器，是 EADS 公司设计在高楼林立的程适中作战时实用性很强的垂直飞行荷载量0.5KG的作战四轴飞行器。此款作战四轴飞行器的接收设备是RC，RC可以确保四轴飞行器接收到远程的控制信号，以使四轴飞行器按照指定的命令执行任务。与此同时，此款作战四轴飞行器还采用了最新的MEMS 惯性测量传感器，惯性测量传感器使得四轴飞行器的飞行姿态有更大的可能性，回传的数据也就更多样化。作为作战四轴飞行器，它还配备了气压传感器和GPS卫星定位装置，能够实现卫星定位，定点飞行，定高飞行等等特殊任务；这种小型四轴飞行器的电源都是一个大问题，如果电源过大，那就意味着要失去更多地灵活性，机动性，以及最重要的是电源多出来多少，负载量就要减少多少。而此款飞行器采用专门设计的电源，起重量很小，却能够保证此款作战四轴飞行器正常飞行二十五分钟。此作战四轴飞行器的长度为65厘米，重量被科研人员控制的恰到其分，只有500克。所以来说，电源能量的一般功率都可以用来负载，这个研究方向正是很多科研机构正在努力的。
对于国内来说，四轴飞行器的科研项目主要集中在哈尔滨工业大学，西安交通大学等等高等学校的科研小组。其次就是各种各样的电子大赛开始关注四轴飞行器，民间的航模爱好者也是四轴飞行器技术进步不可忽略的一部分。
第二章 四轴飞行器的总体设计
2.1 四轴飞行器的零件清单
下表2-1列出了四轴飞行器所用零件清单。
表2-1 四轴飞行器硬件清单
零部件 型号 参数
机架 无（DIY） 红木材料，重量210g，桨距（相邻15，相对23）
电机 新西达A2212 1000KV
螺旋桨 1045 桨叶角45°，直径25cm
电子调速器 中特威30A 可提供30A的电流
电池 狮子2500mah 11.1V，2500mAH，25C
主控芯片 STM32F103 主频168MHZ
传感器 MPU6050 16位分辨率
遥控器 T7A 2.4G，7通道
正如表中所列出来的，组装一架完整的四轴飞行器所需要的基本零部件就是这些，还有其他的扩展可以加上。扩展方面，GPS卫星定位装置可以实现四轴飞行器的定点飞行，定高飞行。气压传感器可以实时向控制端返回四轴飞行器所处的高度。还有电压报警装置，对于四轴飞行器来说，电池都是采用航模电池，这种电池如果过度放电，电池会很快报废。
机架部分采用的木制结构，木板不是按照某种标准制造的，仅仅是在建材市场捡来的边角料。关于机架部分需要多说一点的是，木板稍许有些扭曲，这对后期上电机之后产生了重要的影响。因为木板是扭曲的，所以螺旋桨扇出来的风自然就不是垂直向下的，这就大大影响到了四轴飞行器的飞行稳定性。后期不得不对木板做了很多加工，使其尽量保证电机的平衡。
2.2 四轴飞行器的功能特征
四轴飞行器属于统称的微型飞行器中的一员，广义来说也算是智能机器人的一种。四轴飞行器并不是诞生在高级的实验室里面的，四轴飞行器是诞生在航模爱好者手中的，具体的始祖已经无法考究。四轴飞行器诞生之后受到了社会极大的关注，不仅仅是其他航模爱好者的竞相模仿，而且还引起了美国军方的关注。自然，不少的自动化厂商发现了四轴飞行器的用途可以延伸很多，也投入大笔的资金开始加紧研制四轴飞行器由四个螺旋桨提供拉力，负载量是普通微型飞行器无法与之比较的。四个螺旋桨控制起来可以实现更加灵活的动作，现在已经有航模爱好者做出来了可以弹钢琴的四轴飞行器。由于其载重量大，重量轻，体积小，飞行动作灵活，所以现在四轴飞行器越来越多的在救灾等场合出现。
四轴飞行器是具有四个螺旋桨的微型飞行器，四个桨叶呈十字形交叉。四个螺旋桨分为两对，其中相对着的两个螺旋桨是向相同方向旋转的。四轴飞行器的螺旋桨分为正反桨，顺时针旋转，往下扇风（即提供向上的拉力）的螺旋桨称为正桨；逆时针旋转，往下扇风（提供向上的拉力）的螺旋桨称为反桨。之所以使用正反桨，是因为每个桨叶旋转都会产生四个螺旋桨共同提供的向上拉力，这样产生的拉力自然要比其他微型飞行器所产生的拉力大得多。四轴飞行器的动作改变就是通过改变相应螺旋桨的旋转速度所产生的，但是这种改变方式却能够产生很多种灵活的飞行动作。目前，有些研究机构已经研制出了通过改变螺旋桨的间距来改变四轴飞行器的飞行动作的微型四轴飞行器。
四轴飞行器以其稳定性吸引了很多航拍爱好者。四轴飞行器因为是由四个螺旋桨提供升力，所以其稳定性是不容置疑的。对于航拍来说越稳定的飞行器拍出来的景就越好，搭载自动云台之后的四轴飞行器俨然就是一个航拍神器。目前很多公司都生产出来了商品化的航拍四轴飞行器。
不少的大学生也对四轴飞行器产生了浓厚的兴趣，有些大学生将一些复杂的算法用到了四轴飞行器上面。这样的四轴飞行器就会更加的智能化，不但能够自主巡航，而且还能实现更加复杂的飞行表演、飞行任务等。
2.3 四轴飞行器的总体方案
四轴飞行器总体控制方案为：遥控器发出信号，接收机接收信号并将信号传送给主控制芯片STM32，主控制芯片将信号处理并发送给四个电子调速器，电子调速器按照信号调整电流大小进而控制电机。电源方面，电池给四个电调供电，电调通过BEC接口给主控电路提供电源[4]。总体的控制过程框图如图2-1所示。
图2-1 总体控制框图
第三章 四轴飞行器的硬件设计
3.1 飞行器的硬件选择
四轴飞行器之所以进入到智能机器人的行列，就是因为其所具备的高度智能化的控制系统。对于控制系统的选择，我首先就定位到了嵌入式系统。不仅仅是因为所学，更重要的事嵌入式系统正好符合四轴飞行器的要求，比如说快速灵敏的反应、安全稳定的性能等等。嵌入式系统近些年来发展速度非常快，仅就主控芯片来说，基于ARM构架的芯片收到了越来越多人的信赖。智能手机很大一部分市场份额都使用的ARM构架的芯片。嵌入式系统的英文名字为Embedded system，嵌入式系统是将系统程序完全嵌入到主控芯片内部，为了实现特定的更能的专用的计算机系统。嵌入式系统是众多先进技术的融合，比如计算机技术、半导体技术、电子技术等等。多种技术的高度融合就必然将嵌入式系统定位到了技术密集、资金密集的地位[5]。
嵌入式系统对于四轴飞行器来说是最好的选择，对于其他的智能系统来说，嵌入式系统也能够很出色的完成任务。
就现在的技术成熟度以及个人知识层面来考虑，主控制器选择STM32F103芯片。
3.2 飞行器的硬件详解
3.2.1 基于STM32的核心飞控板
四轴飞行器的核心就是主控电路板，主控芯片的主频决定着系统的运行速度。也就是主控芯片主频越高，其处理信号的速度也就越快，信号处理速度快了自然就对操作信号更加的灵敏。
设计之初，方案是决定自己画一块飞控板的。但是由于个人技术原因，板子印刷出来之后总是有信号影响传感器的正常工作。最后出于资金以及精力考虑还是决定采用商品化的继承飞控板。
基于STM32的核心飞控板用专业术语来说就是MMC飞控。本设计所采用的飞控板是香港科比特公司所生产的飞控板MMC-10。这款飞控板采用的是STM32F103作为主控芯片的，STM32F103的频率是168MHZ，完全能够胜任四轴飞行器的任务要求。再者，STM32F103是一款ARM构架的处理器，嵌入式系统的发展与ARM芯片的发展是分不开的。
ARM公司将STM32系列的芯片定位为高性能、低成本、低功耗的位置就是为了更好的服务嵌入式系统。STM32系列采用的是ARM Cortex-M3内核，按照性能又分成了两个系列，分别为STM32F103增强型系列的主控芯片，和STM32F101基本型系列。顾名思义，STM32F103的性能要比STM32F101强。增强型的STM32F103的时钟频率达到72MHz，就这个时钟频率而言，这款芯片是同类产品中性能最高的。当STM32F103满负荷工作，时钟频率72MHz的时候从闪存执行内部代码，STM32F103的功耗仅仅只有36mA，就这个功耗级别来说，其是32位芯片市场上功耗最低的产品，相当于是0.5mA/MHz。这也就是为什么越来越多的平板电脑厂商以及智能手机厂商开始采用ARM芯片作为主控芯片的原因。就耗电量而言，对于四轴飞行器是很好的。虽说四轴飞行器的耗电量集中在电机的驱动上面，但是主控电路的耗电量也是不能够忽视的。所以说选择一块运算速度快，功耗低，外围接口明确的处理器是非常有必要的[6] [7] [8] [9]。
最后选择作为本设计使用的飞控板为MMC-10，这款飞控板最多支持8通道的PWM波输入，6通道的PWM波输出。支持普通的商业PWM电调以及I2C电调。板子上面集成了三轴数字陀螺仪跟三轴加速度传感器。
3.2.2 电子调速器
四轴飞行器为什么需要电调？调条在四轴飞行器中的作用就是将飞控板传过来的信号转变为将电源电流调整之后的电流传送给直流无刷电机，以电流的大小来控制电机的转速。直流无刷电机所需要的驱动电流是很大的，通常每个直流无刷电机正常工作时所需要的电流大小大约是3A，直接采用电源通过飞控板给电机供电的话，将会有12A的电流从飞控板上面通过。而现在的PCB技术根本没有办法使得飞控板承受住12A的大电流，此外，飞控板也没有办法直接驱动直流无刷电机（空心杯电机除外）。四轴飞行器电源是提供的11.1V的直流电压，飞控板的承压范围远远低于这个电压值。所以飞控板就得通过BEC来从电调上面获取电源，于是电子调速器又充当了电压变化器的作用，将11.1V的电压变为5V电压来为飞控板供电。
电调的选择，电调的型号一般都是xxA的，比如30A，40A等等，这个数字代表的就是这款电调所能够提供的最大电流。在实际应用中，大电流的电子调速器能够用到需要小电流的电子设备中，但是小电流的电子调速器就不能够用到大电流的电子设备中。在四轴飞行器中，电调电流的选择还要综合电机以及螺旋桨来考虑。2212电机如果配上1045螺旋桨的话每个电机的最大电流能够达到5A，这个时候的最小配置就是20A的电子调速器，当然用30A或者40A的电子调速器也是可以的。与此同时要考虑的是如此大的电流从电源到电子调速器，再到电机，这中间的导线要选择大电流的导线。不然的话四轴飞行器一上天就会因为导线过热而燃烧，由此可见导线的选择也很重要。
因为四轴飞行器的特殊要求，所以电调的响应速度必须很快。一般的电子调速器都能够硬件编程的，可以通过硬件编程来调整电子调速器的响应时间。所谓的硬件编程就是给电子调速器相应的电信号来改变电子调速器内相应的配置，以达到自己所需要的效果。本设计所采用的电子调速器的相关配置如表3-1所示。
表 3-1 电子调速器的相关配置
长度 43mm
宽度 25mm
高度 8mm
重量 24g
持续电流 20A
瞬间电流 30A
电池 2-4Lipo/5-12NC
BEC输出 5V/2A
本设计四轴飞行器所使用电子调速器如图3-1所示。
图3-1 电子调速器
3.2.3 传感器
角速度传感器：说到角速度传感器，不得不提的是科里奥利力。所谓的科里奥利力就是物体运动时所具有的惯性，在一个旋转的环境里面，运动物体保持直线运动，一段时间之后如果此时以旋转环境的视角观察的话会发现运动物体产生了一定程度的偏移。当一个物体由于惯性做直线运动时，其运动轨迹相对于旋转体来说是一条曲线。从旋转体系来说，可以认为有一个力量驱使着物体的直线轨迹形成了曲线，这个力量就是科里奥利力[10]。
角速度传感器应用到四轴飞行器中，即我们可以对X、Y、Z三个方向轴的运动方向进行检测，以此来知道四轴飞行器的运动方向。这个三轴角速度传感器就是我们通常所说的三轴陀螺仪。
角速度传感器利用旋转物体的旋转轴在没有受到外力影响的时候所指的方向不变的性质，以此来测量任何外力对四轴飞行器的影响。旋转物体的旋转轴的方向是不确定的，因而角速度传感器只能用来测量四轴飞行器的位置的改变。
重力传感器：重力传感器，顾名思义就是测量重力加速度的，也就是我们通常所说的加速度传感器。用在四轴飞行器中德作用是测量四轴飞行器相对于水平面的倾斜角度。通过将重力加速度进行分解之后，重力传感器可以将X，Y轴方向上的力转化为电压信号。通过读取这些电压值可以得到四轴飞行器相对于水平面的倾斜角度。但是在实际运用中却不然，是因为运动中的四轴飞行器会有不同方向的加速度重合在一起。这就造成了四轴飞行器各个方向上的力并不能非常正确的显示出由于倾斜所产生的重力。
地磁传感器：也就是数字罗盘，它可以通过地磁场来确定方向。四轴飞行器上所用的是三轴地磁传感器，它将地磁场的力映射到四轴飞行器的X、Y、Z三轴上，以此来确定四轴飞行器的飞行姿态。比如仰俯、翻转等等。但是在实际运用中值得注意的是，现在可以产生磁场的物体有很多，这都会影响到四轴飞行器的正常飞行。并且四轴飞行器自身也会产生一定强度的磁场，这些磁场都会影响到三轴地磁传感器向MCU传输正确的信号。
气压传感器：在初中物理中就已经学过，气压传感器的作用就是通过测量地球上空气的重力所产生的压力值来计算被测量物体所在的平面高度。在四轴飞行器上，飞行器所处平面越高，空气密度越小气压值越低；飞行器所处的平面越低，大气密度越大，气压值越高。通过测量气压值就能知道四轴飞行器所处的高度。
面所说的各种传感器只是可能会用到的，基于实际情况考虑，有些误差不能够避免。不少的影响实际效果的外界因素也不能彻底屏蔽。所以就要有所取舍的选择传感器。否则非但不能够让四轴飞行器飞行的更加稳定灵活，反而会影响到四轴飞行器的正常飞行。综合所有的现实状况，最后还是选择了购买模块化的封装完好的集成模块[11]。
MPU-6050叫做角速度传感器模块。其实角度传感器模块是一个系列的，称为MPU-60X0。这个MPU-60X0是一款模块化的9轴传感器。此模块集成了3轴陀螺仪传感器，3轴加速度传感器。MPU-6050原理图如图3-2所示。
图3-2 MPU-6050原理图
3.2.4 直流无刷电机
在电机的选择上，可供选择做为四轴飞行器动力的电机有直流无刷电机、直流有刷电机、空心杯电机。这其中空心杯电机结构最简单，也不需要驱动电路，不需要电子调速器。空心杯电机在做微型四轴飞行器的时候是不二的选择，一来可以减轻电机的重量，二来省去了电子调速器，减少了四轴飞行器的整体体积。但是本设计所要做的四轴飞行器的直径在50厘米左右，所以排除空心杯电机。直流无刷电机是通过电子转向，而直流有刷电机是通过电刷才能换向。所以说通过电刷换向的直流有刷电机的噪音比较大，寿命方面要低一点，有刷电机的寿命一般来说只有600小时，但是直流无刷电机的寿命则长达5000小时。有刷直流电机旋转时需要电刷频繁的切换，这回产生一定程度的电磁干扰，电磁干扰对于四轴飞行器的影响是非常大的。轻则致使四轴飞行器控制不稳定，重则直接影响遥控器对四轴飞行器的控制，致使四轴飞行器失控[12]。这一点就可以排除直流有刷电机作为四轴飞行器的动力电机的方案，再加上直流有刷电机的转速等问题就只有选择高性能的直流无刷电机了。直流无刷电机不但可以通过电压的改变来调整速度，而且还能通过信号线传输的PWM波莱改变转速。直流无刷电机内部的霍尔元件对其至关重要，霍尔元件可以把转子的位置反馈给控制电路，这样的话控制电路就能够获得电机相位转换的精确时间。很多直流无刷电机内部都带有三个霍尔传感器。
无刷直流电机的型号有诸如2212、2018之类的，不管任何厂商生产的直流无刷电机又要有这四个数字，前面两位是直流无刷电机内部转子的直径，而后面的两位数字指的是直流无刷电机内部转子的高度。意思就是前面两个数字越大，直流无刷电机看起来也就越粗，后面两位数字越大，直流无刷电机的高度看起来也就越高。直流无刷电机的功率越大，其就越高越粗。对于四轴飞行器来说，2212号直流无刷电机是最好的搭配了。除了这四个数字，直流无刷电机还有另外一个单位，那就是KV值。任何一个直流无刷电机都会有标注KV值得，这个KV值就是在外界加1V电压时，直流无刷电机每分钟的转动圈数。比如说1000KV的直流无刷电机，在加1V电压时，电机在空转的情况下每分钟可以旋转1000圈。每增加1V电压，直流无刷电机的空转就增加每分钟1000转。根据配得桨的型号，最终选择了2212电机，1000KV值的2212直流无刷电机作为四轴飞行器的动力装置。所使用直流无刷电机如图3-3所示。
图3-3 直流无刷电机
3.2.5 螺旋桨
直流无刷电机的型号已经确定为2212，1000KV的，所以螺旋桨就要选择相应的螺旋桨。如果螺旋桨过大，则会使直流无刷电机负载过大，如果选择螺旋桨过小又会致使四轴飞行器没有足够的动力来源，飞起来会很吃力。螺旋桨的型号也是四个数字比如1145，这前两位11至的事螺旋桨的长度，即旋转起来的直径，单位是英寸。后面两位45则是代表螺旋桨的倾斜角是45°。四轴飞行器需要四个螺旋桨，但是这四个螺旋桨是不一样的。原因是每个螺旋桨在旋转的时候都会产生一个自旋扭力，如果四个螺旋桨全部都是朝一个方向旋转的话，就会导致四轴飞行器不停地自旋。为了抵消这个扭力，就将螺旋桨的正反桨应用到四轴飞行器当中。所谓的正反桨，当螺旋桨顺时针旋转的时候产生向上的升力，那就说明这个螺旋桨式正桨，反之，当螺旋桨逆时针旋转的时候产生向上的升力，那就说明这个螺旋桨是反桨[13]。在四轴飞行器上面，螺旋桨正反桨间隔安装就会抵消每个螺旋桨所产生的自旋。当然，在控制里面也能够里用自旋，适时控制两个正桨或者两个反桨下面的电机转速就能够使四轴飞行器原地改变方向。
3.3 飞行原理
四轴飞行器在表面上看来都是十字交叉型的，实际上是有十字型与X型之分的。十字型比较适合刚刚接触四轴飞行器的，十字型操作起来比较稳定，而X型比较灵活。区别十字型与X型的方法就是看飞控板上传感器部分的指向，如果指向与某个轴是同向的，那就是十字型，如果只想正好在两个轴的分割线上，那就说明这是X型的。本设计采用的是X型的结构[14]。如图3-4所示。
图3-4 四轴飞行器X型结构
当四轴飞行器的四个电机以相同的转速运转时，四轴飞行器所产生的力量是直上直下的，此时可以实现垂直飞行。当四个螺旋桨所产生的推力正好等于四轴飞行器的自身重量时，四轴飞行器就会悬停在空中。
当四轴飞行器左边的两个电机转速低于右边两个电机转速的时候，右边两个螺旋桨所产生向上的力量大于左边两个电机所产生的向上的推力。这时右边将会微微向上抬起，由于重力的作用，四轴飞行器将会向左边移动。同理可以实现四轴飞行器的向右侧移动。当突然减小某一侧的电机转速的时候，四轴飞行器就会做仰俯的动作。
前面也提到过关于四轴飞行器的原地旋转运动。当同时减小通一个旋转方向的直流无刷电机的转速时，另外一组所产生的扭力就要大于速度较小的这一组所产生的扭力，当扭力不能抵消的时候，四轴飞行器就会原地旋转。一次可以实现四轴飞行器的偏航运动。
第四章 四轴飞行器的软件设计与调试
4.1 软件实现
主控芯片的飞控控制程序是采用的C语言编写的，意法半导体公司对STM32系列的芯片提供了很强大的函数支持，所以飞控系统的设计就集中在稳定性，以及灵活性这两方面了。飞控系统要实现的主要功能是主控板伺服，接收机接收到遥控发出的信号以后解码并传送给主控芯片，主控芯片获取传感器的数据，之后将信号传送给电子调速器，电子调速器调整电流大小并控制直流无刷电机。飞行器系统的输入输出如图 4-1 所示：
图4-1 控制系统输入输出图
系统输入输出框图4-1 中，输入信号主要有：来自遥控器的四路遥控器指令，分别来控制飞行器的升降、前后飞行、左右飞行、自旋；来自角速度传感器和加速度传感器的6 路数据用来感知飞行器当前姿态，并与目标姿态进行比较，实现闭环控制；飞控板计算出与目标姿态的差值，通过PID算法计算出四个旋翼的控制量，最终以PWM占空比形式输出控制直流无刷电机。飞行器控制系统整体流程图如图4-2所示。
控制系统的整体流程为，系统首先初始化MCU时钟以及要使用的内部资源，然后定时读取接收机数据，将所获的遥控信号和传感器信号做比较，计算出偏差，最后通过PID控制得到电机控制量；系统运行时需要一个准确的时基信号，来确保一些对时间要求较高的任务的安全运行；另外根据STM32系列处理器特性，采用ST公司提供的功能函数库，还需要打开各类端口的时钟源信号。
图4-2 飞行器控制系统整体流程图
关于系统的基准时钟，设计时采用一个 1ms 的时钟计数，通过访问内核计数器SysTick中的计数值来确定微秒级定时，具体设计如下：
STM32f103工作于72MHz，SysTick时钟周期为 72M/1000：
SysTick_Config(SystemCoreClock / 1000);
即实现毫秒中断：
void SysTick_Handler(void)
{
sysTickUptime++; //时间计数
}
则当前时间可以用如下函数得到：
uint32_t micros(void)
{
register uint32_t ms, cycle_cnt;
do {
ms = sysTickUptime;
cycle_cnt = SysTick->VAL;
} while (ms != sysTickUptime); //1ms 中断
return (ms * 1000) + (72000 – cycle_cnt) / 72; // 返回当前时间
}
STM32f103具备丰富的内部资源，针对飞行器的需求，使用到的资源配置描述如下：
(a) IIC配置
设计中使用STM32f103 的硬件IIC 资源，其初始化的代码流程如下：
void i2cInit(I2C_TypeDef * I2C)
{
GPIO_Init(); // 配置IO引脚
I2C_ITConfig(); //配置EVT 和ERR 中断
I2C_Cmd(); //使能中断
NVIC_Init(); //配置中断服务程序
}
(b) PWM波
PWM的输出设置和PPM 信号的输入计数功能都需要使用STM32f103 的内部定时器功能来完成。STM32f103 共拥有11 个定时器，其中包括2 个高级控制定时器TIM1和TIM8，4 个普通定时器TIM2-TIM5 ；TIM1和TIM8能够产生3 对PWM 互补输出用于驱动三相电机，而TIM2-TIM5 则能实现输入捕获计时，PWM 输出及内部计时的功能。
控制系统共需要4 路PWM 和4 路PPM 信号，采用TIM2-TIM5 既可以实现，设计中使用TIM2实现4 个通道的输入捕获，TIM4则实现4 路PWM 信号输出；两者配置类似，伪代码如下：
PPM_PWMConfig()
{
pTimeBase(); //配置使用的定时器
pGPIOConfig(); //配置IO引脚
pICConfig(); //配置TIM 通道
TIM_Cmd(); //使能端口
NVICConfig(); //配置中断服务程序
TIM_ITConfig(); //使能中断源
}
(c) UART设置
控制系统中的UART 接口用作调试输出及控制参数设定等功能，具体实现的配置如下：
uartInit()
{
GPIO_Init(); //配置端口
NVIC_Init(); //设置中断
USART_Init(); //配置串口参数
DMA_Init(); //使用DMA传输
USART_DMACmd (); //使能DMA
USART_Cmd(); //使能串口
}
传感器的设置，主要是对MPU-6050的初始化，其初始化的内容包括模块的量程、工作模式等信息，具体的配置设定如下：
static void mpu6050Init(void)
{
i2cWrite(MPU_ ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); // 复位芯片
delay(5); //等待复位完成
i2cWrite(MPU_ ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); // 角速度采样率设置
i2cWrite(MPU_ ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // 时钟源设置
i2cWrite(MPU_ ADDRESS, MPU_RA_CONFIG,MPU_DLPF_CFG); //低通滤波设置
i2cWrite(MPU_ ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); //角速度量程设置
i2cWrite(MPU_ ADDRESS, MPU_RA_ACCEL_CONFIG, 1 << 3); // 加速度量程及滤波设置
}[15]
4.2 调试
软件环境：windows2008；
硬件环境：华硕笔记本；
开发工具：Keil uVision4（MDK4.12）；
程序的主体部分是通过开源代码移植过来的，所以调试的时候没有费太多的时间。主要的还是上位机对四轴飞行器的校准部分。
程序调试使用的是Keil uVision4（MDK4.12），keil开发软件提供了很多有利于程序编写以及查错的内部工具，给程序的移植以及调试提供了极大地帮助[16]。经过移植，以及参考国外优秀代码，程序最终实现了零错误，零警告。程序调试截图如图4-3所示。
图 4-3 程序调试截图
程序下载到飞控板时需要5p杜邦线将下载器与飞控板连接起来，安装所需驱动程序之后即可将程序下载到飞控板。下载器及连线如图所4-4示：
图4-4 下载器
程序下载之后就要对四轴飞行器进行调试，上位机调试部分主要校准四轴飞行器的水平形态，利用上位机软件很容易就能够进行调试。等待串口连接之后，就可以进行油门、水平位置等的校准[17]。调试界面如图 4-5所示。
图4-5 上位机调试
调试完成之后整机如图4-6所示：
图4-6 整机图
4.3 试飞及结论
试飞结果完全满足预期要求，能实现垂直起飞，前后左右四个方向的转动等等一些常规动作。稳定性方面，如果风比较小的花还能够较为稳定的起飞，大风对四轴飞行器的影响还是比较大的。电源方面，电池充满之后能够飞行12分钟左右。控制方面，7通道遥控器以及接收机能够完美的传输信号，最远控制距离尝试过的达到了150m左右。
当然，试飞过程中也发现了一些不足之处。比如前面所说的电源与电调之间的导线电流过大的问题，后来及时更换了大电流的导线解决了此问题。还有一点影响飞行稳定性的就是前面提到过的机架的问题，因为机架是自己采用角料制作的，在平整度上面很难实现完美，所以安装完电机之后我对电机底座做了一些调整，使其尽量保持垂直状态。因为点击如果不是垂直状态的话，螺旋桨所产生的动力对于四轴飞行器平面就不是垂直的，这造成的结果就是四轴飞行器稳定性不够。调整结果最后的差距距离垂直大约在2-4mm左右，由于个人水平以及手头工具的限制也只能调整到这个程度了。虽然如此，但良好的操控能力还是能够抵消部分不稳定的晃动的。
根据试飞结果，本设计实现了课题的所有要求。
结语
四轴飞行器在飞行器领域算是一种结构比较特殊的飞行器，不管是在民用、商用、还是军用范围内都有着很好的应用前景，四轴飞行器属于旋翼类飞行器的一种，但四轴飞行器与直升机还有固定翼飞行器仍旧有着很大的区别；四轴飞行器不但能够和其他的飞行器一样实现多样的飞行动作，值得注意的是四轴飞行器的控制方式却与一般旋翼类飞行器有一些不同之处，四轴飞行器不是通过控制螺旋桨的桨距来实现不同方式的运动，而是通过调整不同螺旋桨之间的转动速度来完成不同的飞行动作。
文中依据四轴飞行器较为不同的构造，对他的半自主化的飞行控制做了初步的探讨以及设计工作，并且通过亲手制作的样机对所设计的控制系统的功能和姿态算法等进行了较为全面的验证，设计及实现的结果对其应用于无线传感器组成网络具有重要的理论以及实际意义；论文主要进行了如下几个方面的工作：
(1)综述了四轴飞行器的研究现状及其应用价值，简单介绍了四轴飞行器现在的相关技术。
(2)针对四轴飞行器的特殊架构，借鉴常规旋翼类飞行器运动原理，分析了四轴飞行器的结构特点、飞行运动方式及其控制原理。
(5)课题最后对设计的结果进行了基本的测试，测试结果表明飞行器能按照预定实现基本飞行功能；由于本人水平有限，对飞行器相关动力学方面没有作较为深入的研究，设计软件控制系统时参考了国外开源的优秀四轴飞行器代码，成果初步完成了一些必要的飞行动作，但本人对于控制系统以及动力学的研究必然有不足之处，许多方面还有待进一步研究和改进。针对现在的实现成果，本人研究的四轴飞行器还存在一些问题需要解决：
(1) 电源供应方面亟待解决：目前使用的所谓高聚合度锂电池只能实现短时间的应用飞行，如果要实现长时间的飞行任务，需要性能更高的能源供应；
(2) 四轴飞行器稳定性有待提高：本课题设计的四轴飞行器只实现了简单的飞行功能，为了实现更多更灵活的运动，设计时飞行器的稳定性要作为重中之重来考虑；
(3) 对不同环境的适应性：四轴飞行器将来的应用任务必定非常广泛，所以设计时需要使其具备自动适应各种不同环境的功能。
参考文献
[1] 陈天华,郭培源.小型无人机自主飞行控制系统的实现[J].航天控制.2006,24(5):86~90.
[2] 古月徐,杨忠,龚华军.基于DSP的飞行控制器的设计,[J].自动化技术与应用.2005,24(2):28~32.
[3] 韩京清.自抗扰控制器及其应用[J].控制与决策,1998,1,3(1):19~23.
[4] 肖永力,张琛.微型飞行器的研究现状与关键技术[J].宇航学报.2001,22~32.
[5] 严隽,高金源,屠巴宁.小型无人机水平导航研究[J].飞行力学,2000,18,24～27.
[6] 朱加强,郭锁凤,朱纪洪,胡春华.战斗机非线性控制技术的研究与发展.[J].航空学报,2005,26(6):720～725.
[7] 梁秋憧,程维明,潘志浩超小型飞行器自主导航系统地面站设计,机电一体化,2002年06期23~26.
[8] 张新国.从自动飞行到自主飞行—飞行控制与导航技术发展的转折和面临的挑战[J]飞机设计,2003,03,55~59.
[9] 王亮,李正,宁婷婷.VGA汉字显示的FPGA设计与实现[J].计算机工程与设计,2009,30(2):275~277,281.
[10]谢昭莉,蒋涛,刘亮.基于ARM嵌入式系统的VGA接口的研究与设计.[J].液晶与显示,2007,22(6):761~764.
[11]陈亚萍,陈明.基于DSP和CPLD的液晶显示控制器的设计.[J].计算机测量与控制,2007,15(4):482~484.
[12]黄杰勇,邓春健.基于CPLD的VGA图像显示控制系统[J].现代计算机,2008,(12):167~169.
[13]张飙,徐和飞,张玉.LED光带型交通诱导屏的设计.[J].液晶与显示,2009,24(1):103~109.
[14]周润景,图雅,张丽敏.基于Quartus II的FPGA/CPLD数字系统设计实例,[M].北京:电子工业出版社,2007.
[15]吴成富,段晓军.基于Matlab和VxWorks的无人机飞控系统半物理仿真平台研究[J].西北工业大学学报,2005,23(3):337~340.
[16] P.Tsiotras and J.Luo.Controlof underactuated spacecraft with bounded inputs.Automatica,36(8):1153~1169,2000.
[17] EFE M O.Robust low altitude behavior control of a quadrotor ro-torcraft through slidingmodes[C]//Proceeding of the 15th Mediter-ranean Conference on Control and Automation.Athens,Greece:IEEE,2006:783~794.
附录一
四轴飞行器部分原理图
四轴飞行器pcb
附录二
// 定义四元数类型。
typedef struct
{
float w;
float x;
float y;
float z;
} quaternion;
// 规范化四元数。
void quaternion_normalize( quaternion * q)
{
float norm_r = 1.0f / sqrtf (q-> w*q- >w + q -> x*q- >x + q- >y *q-> y + q- >z *q-> z);
q- >w *= norm_r;
q- >x *= norm_r;
q- >y *= norm_r;
q- >z *= norm_r;
}
// 四元数相乘
// left : 被乘数，输入。
// right : 乘数，输入。
// result : 积，输出。
void quaternion_mult (quaternion * result, const quaternion * left,const quaternion * right)
{
result-> w = left-> w * right -> w – left-> x * right- >x – left-> y * right- >y – left-> z * right- >z ;
result-> x = left-> x * right -> w + left-> w * right- >x + left-> y * right- >z – left-> z * right- >y ;
result-> y = left-> y * right -> w + left-> w * right- >y + left-> z * right- >x – left-> x * right- >
result-> z = left-> z * right -> w + left-> w * right- >z + left-> x * right- >y – left-> y * right- >
}
// 用四元数来旋转向量。
void quaternion_rotateVector ( const quaternion * rotation, const float from[ 3],float to[3 ])
{
float x2 = rotation-> x * 2 ;
float y2 = rotation -> y * 2 ;
float z2 = rotation-> z * 2 ;
float wx2 = rotation-> w * x2;
float wy2 = rotation-> w * y2;
float wz2 = rotation-> w * z2;
float xx2 = rotation-> x * x2;
float yy2 = rotation-> y * y2;
float zz2 = rotation-> z * z2;
float xy2 = rotation-> x * y2;
float yz2 = rotation-> y * z2;
float xz2 = rotation-> z * x2;
//
to[0 ] = from[0 ]*(1 – yy2 – zz2) + from[ 1]*(xy2 – wz2) + from[2 ]*(xz2 + wy2);
to[1 ] = from[0 ]*(xy2 + wz2) + from[ 1]*( 1 – xx2 – zz2) + from[2 ]*(yz2 – wx2);
to[2 ] = from[0 ]*(xz2 – wy2) + from[ 1]*(yz2 + wx2) + from[2 ]*(1 – xx2 – yy2);
}
//
// 两向量旋转→四元数旋转。
// 输入：from、to两向量，长度都必须大于0 。
// 输出：从from方向转到to方向的旋转。
void quaternion_fromTwoVectorRotation(quaternion * result, const float from[3 ],const float to[ 3])
{
float from_norm = fabsf(from[0]*from[ 0] + from[ 1]*from[ 1 ] + from[2 ]*from[2 ]);
float to_norm = fabsf(to[0]*to[ 0] + to[ 1]*to[ 1] + to[ 2 ]*to[2 ]);
//
from_norm = sqrtf(from_norm);
to_norm = sqrtf(to_norm);
float cos_theta = (from[0 ]*to[0 ] + from[1 ]*to[1 ] + from[ 2]*to[ 2]) / (from_norm*to_norm);
result-> w = sqrtf((1.0f + cos_theta) / 2); // cos(theta/2)
float sin_half_theta = sqrtf ((1 – cos_theta) / 2);
float cross_x = from[1 ]*to[ 2] – from[ 2]*to[ 1];
float cross_y = from[2 ]*to[ 0] – from[ 0]*to[ 2];
float cross_z = from[0 ]*to[ 1] – from[ 1]*to[ 0];
//
float sin_half_theta_div_cross_norm = sin_half_theta /
sqrtf(cross_x*cross_x + cross_y*cross_y + cross_z*cross_z);
result-> x = cross_x * sin_half_theta_div_cross_norm;
result-> y = cross_y * sin_half_theta_div_cross_norm;
result-> z = cross_z * sin_half_theta_div_cross_norm;
}
// 姿态插值法。
void mix_AttitudeInterpolation ( quaternion * attitude, const float gyr[3 ],
const float acc[3],const float mag[3],float interval)
{
//
// 第一部分，陀螺积分。
// 构造增量旋转。
// 由于角增量是小角，w 会很接近1 ，为了减小运算量，令其为1 。
float delta_x = gyr[0] * interval / 2 ;
float delta_y = gyr[1] * interval / 2 ;
float delta_z = gyr[2] * interval / 2 ;
//
// 融合，四元数乘法。
float q_w = attitude-> w;
float q_x = attitude-> x;
float q_y = attitude-> y;
float q_z = attitude-> z;
attitude-> w = q_w – q_x*delta_x – q_y*delta_y – q_z*delta_z;
attitude-> x = q_w*delta_x + q_x + q_y*delta_z – q_z*delta_y;
attitude-> y = q_w*delta_y – q_x*delta_z + q_y + q_z*delta_x;
attitude-> z = q_w*delta_z + q_x*delta_y – q_y*delta_x + q_z;
//
// 第二部分，加速度计和罗盘纠正积分漂移。
// 先规范化测量的加速度和磁场强度。
float acc_n[3],mag_n[3 ];
float a_rsqrt = 1.0f / sqrtf (acc[ 0]*acc[0 ]+acc[1]*acc[1]+acc[2 ]*acc[2]);
float h_rsqrt = 1.0f / sqrtf (mag[ 0]*mag[0 ]+mag[1]*mag[1]+mag[2 ]*mag[2]);
for( int i=0;i< 3;i++)
{
acc_n[i] = acc[i] * a_rsqrt;
mag_n[i] = mag[i] * h_rsqrt;
}
//
// 计算对角线和平面法线。
const static float MAG_VECTOR[] = { 0, 0.743144f/*cos(42)*/ , -0.669130f /*sin(-42)*/ };
const static float ACC_VECTOR[] = { 0, 0, 1};
float mid_from[3 ],mid_to[ 3],cross_from[ 3],cross_to[ 3 ];
math_vector_cross(cross_from,acc_n,mag_n );
math_vector_cross(cross_to,ACC_VECTOR,MAG_VECTOR);
for( int i=0;i< 3;i++)
{
mid_from[i] = acc_n[i] + mag_n[i];
mid_to[i] = MIX_ACC_VECTOR[i] + MIX_MAG_VECTOR[i];
}
//
// 先把 mid 转到重合。
quaternion rotation_1;
quaternion_fromTwoVectorRotation (&rotation_1,mid_from,mid_to);
//
// 然后把 cross 转到重合。
quaternion rotation_2;
float cross_from_1[3 ];
quaternion_rotateVector(&rotation_1,cross_from,cross_from_1);
quaternion_fromTwoVectorRotation(&rotation_2,cross_from_1,cross_to);
//
// 再组合两次旋转。
quaternion target;
quaternion_mult(&target,&rotation_2,&rotation_1);
quaternion_normalize(&target);
//
// 把两部分的姿态进行插值。
static const float ALPHA = 0.003; // 插值系数。
attitude-> w = (1 -A LPHA)*attitude- >w + ALPHA*target. w ;
attitude-> x = (1 -ALPHA)*attitude – >x + ALPHA*target. x ;
attitude-> y = (1 -ALPHA)*attitude – >y + ALPHA*target. y ;
attitude-> z = (1 -ALPHA)*attitude – >z + ALPHA*target. z ;
//
// 最后规范化一下姿态，防止发散。
quaternion_normalize(attitude);
}
// 梯度下降法。
void mix_SteepestDescent (quaternion * attitude, const float gyr[3 ],
const float acc[3],const float mag[3],float interval)
{
//
// 先处理陀螺积分。
// 构造增量旋转。
// 由于角增量是小角，w 会很接近1 ，为了减小运算量，令其为1 。
float delta_x = gyr[0] * interval / 2 ;
float delta_y = gyr[1] * interval / 2 ;
float delta_z = gyr[2] * interval / 2 ;
//
// 融合，四元数乘法。
float q_w = attitude-> w;
float q_x = attitude-> x;
float q_y = attitude-> y;
float q_z = attitude-> z;
attitude-> w = q_w – q_x*delta_x – q_y*delta_y – q_z*delta_z;
attitude-> x = q_w*delta_x + q_x + q_y*delta_z – q_z*delta_y;
attitude-> y = q_w*delta_y – q_x*delta_z + q_y + q_z*delta_x;
attitude-> z = q_w*delta_z + q_x*delta _y – q_y*delta_x + q_z;
//
// 然后用梯度下降法纠正陀螺漂移。
// 规范化测量的加速度和磁场强度。
float a_rsqrt = 1.0f / sqrtf (acc[ 0]*acc[0 ]+acc[1]*acc[1]+acc[2 ]*acc[2]);
float x_a = acc[0] * a_rsqrt;
float y_a = acc[1] * a_rsqrt;
float z_a = acc[2] * a_rsqrt;
float h_rsqrt = 1.0f / sqrtf (mag[ 0]*mag[0 ]+mag[1]*mag[1]+mag[2 ]*mag[2]);
float x_h = mag[0] * h_rsqrt;
float y_h = mag[1] * h_rsqrt;
float z_h = mag[2] * h_rsqrt;
//
// 预先计算一些重复使用的值，减少运算浪费。
float w_q = attitude-> w;
float x_q = attitude-> x;
float y_q = attitude-> y;
float z_q = attitude-> z;
float x_q_2 = x_q * 2;
float y_q_2 = y_q * 2;
float z_q_2 = z_q * 2;
//
// 计算加速度和磁场强度的误差。
const static float MAG_Y = 0.743144f ; // cos(- 42°)，实测当地磁倾角为- 42°。
const static float MAG_Z = -0.669130f ; // sin(- 42°)
float x_da = x_q*z_q_2 – w_q*y_q_2 – x_a;
float y_da = y_q*z_q_2 + w_q*x_q_2 – y_a;
float z_da = 1 – x_q*x_q_2 – y_q*y_q_2 – z_a;
float x_dh = MAG_Y*(x_q*y_q_2 + w_q*z_q_2) + MAG_Z*(x_q*z_q_2 – w_q*y_q_2) – x_h;
float y_dh = MAG_Y*( 1 – x_q*x_q_2 – z_q*z_q_2) + MAG_Z*(y_q*z_q_2 + w_q*x_q_2) – y_h;
float z_dh = MAG_Y*(y_q*z_q_2 – w_q*x_q_2) + MAG_Z*( 1 – x_q*x_q_2 – y_q*y_q_2) – z_h;
//
// 计算误差对姿态的梯度的1 /4。
float w_pf = – x_da*y_q + y_da*x_q + x_dh*(MAG_Y*z_q – MAG_Z*y_q) \
+ y_dh*MAG_Z*x_q – z_dh*MAG_Y*x_q;
float x_pf = x_da*z_q + y_da*w_q – z_da*x_q + x_dh*(MAG_Y*y_q + MAG_Z*z_q) \
+ y_dh*(MAG_Z*w_q – MAG_Y*x _q) – z_dh*(MAG_Y*w_q + MAG_Z*x_q);
float y_pf = – x_da*w_q + y_da*z_q – z_da*y_q + x_dh*(MAG_Y*x_q – MAG_Z*w_q) \
+ y_dh*MAG_Z*z_q + z_dh*(MAG_Y*z_q – MAG_Z*y_q);
float z_pf = x_da*x_q + y_da*y_q + x_dh*(MAG_Y*w_q + MAG_Z*x_q) \
+ y_dh *(MAG_Z*y_q – MAG_Y*z_q) + z_dh*MAG_Y*y_q;
//
// 用梯度更新姿态。
const static float BETA = 0.002 *4 ;
attitude-> w -= w_pf * BETA;
attitude-> x -= x_pf * BETA;
attitude-> y -= y_pf * BETA;
attitude-> z -= z_pf * BET A;
//
// 最后要规范化姿态，防止发散。
quaternion_normalize(attitude);
}
// 互补滤波法。
void mix_ComplementaryFilter ( quaternion * attitude, const float gyr[3 ],
const float acc[3],const float mag[3],float interval)
{
//
// 规范化测量的加速度和磁场强度。
float a_rsqrt = 1.0f / sqrtf (acc[ 0]*acc[0 ]+acc[1]*acc[1]+acc[2 ]*acc[2]);
float x_a = acc[0] * a_rsqrt;
float y_a = acc[1] * a_rsqrt;
float z_a = acc[2] * a_rsqrt;
float h_rsqrt = 1.0f / sqrtf (mag[ 0]*mag[0 ]+mag[1]*mag[1]+mag[2 ]*mag[2]);
float x_h = mag[0] * h_rsqrt;
float y_h = mag[1] * h_rsqrt;
float z_h = mag[2] * h_rsqrt;
//
// 预先计算一些重复使用的值，减少运算浪费。
float w_q = attitude-> w;
float x_q = attitude-> x;
float y_q = attitude-> y;
float z_q = attitude-> z;
float x_q_2 = x_q * 2;
float y_q_2 = y_q * 2;
float z_q_2 = z_q * 2;
//
// 计算常量在机体坐标下的表示。
const static float MAG_Y = 0.743144f ; // cos(- 42°)，实测当地磁倾角为- 42°。
const static float MAG_Z = -0.669130f ; // sin(- 42°)
float x_ae = x_q*z_q_2 – w_q*y_q_2;
float y_ae = y_q*z_q_2 + w_q*x_q_2;
float z_ae = 1 – x_q*x_q_2 – y_q*y_q_2;
float x_he = MAG_Y*(x_q*y_q_2 + w_q*z_q_2) + MAG_Z*(x_q*z_q_2 – w_q*y_q_2);
float y_he = MAG_Y*( 1 – x_q*x_q_2 – z_q*z_q_2) + MAG_Z*(y_q*z_q_2 + w_q*x_q_2);
float z_he = MAG_Y*(y_q*z_q_2 – w_q*x_q_2) + MAG_Z*( 1 – x_q*x_q_2 – y_q*y_q_2);
//
// 测量值与常量的叉积。
float x_ac = y_a * z_ae – z_a * y_ae;
float y_ac = z_a * x_ae – x_a * z_ae;
float z_ac = x_a * y_ae – y_a * x_ae;
float x_hc = y_h * z_he – z_h * y_he;
float y_hc = z_h * x_he – x_h * z_he;
float z_hc = x_h * y_he – y_h * x_he;
//
// 构造增量旋转。
const static float GAMMA = 0.003;
const static float LAMDA = 0.003;
float delta_x = gyr[0] * interval / 2 + x_ac * GAMMA + x_hc * LAMDA;
float delta_y = gyr[1] * interval / 2 + y_ac * GAMMA + y_hc * LAMDA;
float delta_z = gyr[2] * interval / 2 + z_ac * GAMMA + z_hc * LAMDA;
//
// 融合，四元数乘法。
attitude-> w = w_q – x_q*delta_x – y_q*delta_y – z_q*delta_z;
attitude-> x = w_q*delta_x + x_q + y_q*delta_z – z_q*delta_y;
attitude-> y = w_q*delta_y – x_q*delta_z + y_q + z_q*delta_x;
attitude-> z = w_q*delta_z + x_q*delta_y – y_q*delta_x + z_q;
//
// 最后要规范化姿态，防止发散。
quaternion_normalize(attitude);
}
附录三
Ⅰ英文原文
This paper considers the question of using a non- linear complementary filter for attitude estimation of fixed-wing unmanned aerial vehicle (UAV) given only measurements from a low-cost inertial measurement unit. A nonlinear complementary filter is proposed that combines accelerometer output for low frequency attitude estimation with integrated gyrometer output for high frequency estimation. The raw accelerometer output includes a component for the airframe acceleration, that occurs primarily as the aircraft turns, as well as the gravitational acceleration that is required for the filter. The airframe acceleration is estimated using a simple centripetal force model (based on additional airspeed measurements), augmented by a first order dynamic model for angle-of-attack, and used to obtain estimates of the gravitational direction independent of the airplane manoeuvres. Experimental results are provided on a real-world data set and the performance of the filter is evaluated against the output from a full GPS/INS that was available for the data set.
Attitude determination is an essential task for an Unmanned Aerial Vehicle (UAV). With the growing range of applications in UAV’s, and the push to make vehicles cheaper and more reliable, it is of interest to develop robust and simple algorithms for attitude estimation. There is a large literature on attitude filtering techniques, see for example the recent review article by Crassidis et al. Most of the advanced filter techniques (particle filtering, etc.) are computationally demanding and unsuitable for the small scale embedded processors in UAV avionic systems. The two methods that are commonly used are extended Kalman filtering (EKF) or some form of constant gain state observer, often termed a complimentary filter due to its frequency filtering properties for linear systems . Extended Kalman Filtering has been studied for a range of aerospace applications . Such filters, however, have proved difficult to apply robustly. In practice, many applications use simple linear single-input single-output complementary filters . In recent work, a number of authors have developed nonlinear analogous of single-input single-output (SISO) filters for attitude estimation . To implement these schemes on a UAV using inertial measurement unit (IMU) data the accelerometer output is used to estimate the gravitational direction. The recent work by the authors allows the full estimation of vehicle attitude (up to a constant heading error) as well as gyro biases based just on the accelerometer and gyrometer data. The filter fails, however, when the vehicle dynamics are sufficiently large that accelerometer output no longer provides a good estimate of the gravitational direction. This is particularly the case for a fixed wing UAV manoeuvering in a limited space and making repeated rapid turns.In this paper, we develop a nonlinear complementary filter, augmented by a simple first order model of vehicle dynamics that provides excellent attitude estimates for a fixed wing UAV. The key contribution is to develop a model of the non-inertial acceleration of the airframe that can be used to compensate the accelerometer output to obtain a zero bias estimate of the gravitational direction. The model is based on a simple centripetal force model derived from the airspeed and the rate of turn of the vehicle. However, the angle-of-attack of the airplane is significantly higher during a sharp turn, and this must be modelled to correctly align the compensation terms for the accelerometer output. We address this problem by incorporating a simple first order model of the angle-of-attack dynamics of the airframe driven by the pitch rate measurement obtained from the gyrometer output. The combined system is simple to implement and achieves excellent performance, given the minimal data that is available. The algorithm is verified on experimental data from a fixed wing aerial robotic vehicle. The performance of the algorithm is confirmed by comparison with an attitude estimate obtained from a full INS/GPS stochastic filter that has been run on the experimental data.
A complementary filter for attitude estimation performs low-pass filtering on a low-frequency attitude estimate, obtained from accelerometer data, and high-pass filtering on a biased high-frequency attitude estimate, obtained by direct integration of gyrometer output, and fuses these estimates together to obtain an all-pass estimate of attitude. When the pitch and roll of a airplane are modelled as decoupled processes, a SISO filter can designed for each signal that uses the the angle between the accelerometer output and the body-fixed-frame as attitude reference and the separate gyrometer axis output as velocity reference in a classical linear complementary filter. When a low pass estimate of the full attitude can be reconstructed from the IMU measurements, for example if magnetometer data is also available and the full coupled rotation matrix for attitude can be computed as an algebraic function of the gravitational and magnetic fields measured in the body-fixed-frame, then nonlinear extensions of the complementary filters have been available for fifteen years. Magnetometers are rarely useful on small scale UAVs due to the perturbation of the magnetic field resulting from electric propulsion systems and other disturbances. The recent work by the authors provided a formulation of explicit complementary filtering (ECF), posed directly on the set of rotation matrices, that is driven by a single inertial direction measurement, such as provided by the accelerometer output, along with the gyrometer output.
Four aircraft will need to complete a variety of tasks manually or radio remote control navigation of autonomous navigation . Artificial remote navigation flight only within the field of view , and if Four aircraft to perform tasks outside the field of view , it must be autonomous navigation .
Conventional aircraft inertial navigation equipment for general use or Doppler ground speed equipment , but because of the huge volume of factors , such as high prices , difficult to apply lightweight and inexpensive Four aircraft . And today’s global positioning system GPS, has all-weather , global , continuous three-dimensional precision navigation and positioning capabilities , and the weight and volume are also very suitable for unmanned aircraft axis , while the price is quite cheap, a GPS module generally also finished 100 yuan or so, so in the aviation GPS navigation, marine navigation and land navigation and other aspects of the application is very wide .
Autonomous aircraft loaded GPS navigation system greatly expanded after its pan with the civilian and military .
Type of autonomous cruise for civilian aircraft perform tasks such as disaster relief survey flood, fire , earthquake , etc. ; concentration of chemical plants and other hazardous gas monitoring dangerous places ; spraying farmland , forest pesticide inspections pipeline , transmission line ; continuous monitoring important facilities ; regional air – ground , air – sea communications relay ; when specific areas of daily environmental monitoring when using this aircraft to perform is also very convenient and efficient , independent inspections after destination and can automatically return automatically record data under storage , greatly reducing labor costs. It was reported that since September 2010 , in order to improve the level of transmission line inspection , Jiangxi Power Company adopted the drone aircraft patrol transmission lines , transmission lines body defects hidden channels for fast detection in a variety of terrain, adverse weather adverse conditions , accurate, timely and efficient manner to obtain field data in the first time .
Ⅱ中文译文
本文论述了使用非线性互补滤波器对给定从一个低成本的惯性测量单元只测量固定翼无人机（ UAV）的姿态估计问题。非线性互补滤波器建议加速度计输出的低频姿态估计相结合，集成陀螺测试仪输出，用于高频率估计。的原始加速度计的输出包括一个组件，用于机身加速度，发生主要是由于飞机转弯，以及所需要的滤波器的重力加速度。估计机身加速度使用简单的向心力模型（基于附加空速测量） ，通过一阶动态模型角的攻加力，并用于获得独立于飞机的操纵的重力方向的估计。所提供的真实世界数据集的实验结果，过滤器的性能对从完整的GPS / INS是利用用于数据集的输出求值。
姿态确定是一个无人机（ UAV）的一项重要任务。随着越来越多范围的无人机的应用程序，并推使车辆更便宜和更可靠，它感兴趣的是开发强大和简单的算法姿态估计。有一个大的文学态度上的滤波技术，例如参见由Crassidis等人最近的评论文章。大多数先进的过滤技术（粒子滤波等）是计算量大，不适合在无人机航空电子系统的小型嵌入式处理器。这两种方法是常用的是扩展卡尔曼滤波（ EKF）或某种形式的固定增益状态观测器，通常被称为一个免费的过滤器，由于其为线性系统的频率滤波特性。扩展卡尔曼滤波已经研究了一系列的航空航天应用。这样的过滤器，但是，已经证明难以鲁棒适用。在实践中，许多应用程序使用简单的线性单输入单输出互补的过滤器。在最近的工作中，许多作者已经开发出单输入单输出（SISO ）过滤器姿态估计的非线性相似。在使用惯性测量单元的加速度计的输出来估计重力方向（IMU）的数据的UAV实施这些方案。
最近的工作由作者允许的车辆的姿势（最多为恒定方位误差）以及陀螺仪偏差的完整估计仅基于加速度计和陀螺测试仪的数据。然而过滤器出现故障时，当车辆动力是足够大，加速度计的输出不再提供重力方向的良好估计。这是特别为在有限的空间中的固定翼无人机布阵的情况下，使反复快速turns.In本文中，我们开发了一个非线性互补滤波器，通过车辆动态特性的简单的一阶模型，它提供了卓越的态度估计一个固定的增强翼无人机。的主要贡献是开发可用于补偿加速度计的输出，以获得重力方向的零偏置估算机体的非惯性加速度模型。该模型是基于空速和转弯车辆的速度得出一个简单的向心力模型。然而，飞机的的攻击角度是在一个急转弯显著较高，而这必须仿照正确对齐补偿条款的加速度计输出。我们通过引入迎角的机身由来自陀螺测试仪的输出中获得的俯仰速率的测量驱动的动态一个简单的一阶模型解决这个问题。将合并的系统易于实现，并达到优良的性能，给定的最小的数据是可用的。该算法是在从固定翼空中机器人车辆试验数据进行了验证。该算法的性能是通过使用从已经运行在实验数据的完整GPS / INS随机滤波器而得到的姿态估计的比较证实。
一个互补的过滤器进行姿态估计进行低通滤波的低频姿态估计，从加速度传感器获得的数据，并在偏高频姿态估计，通过直接集成陀螺测试仪的输出获得高通滤波，并融合这些估计在一起，以获得姿态的一个全通的估计。当飞机的俯仰和侧滚建模为解耦过程中， SISO过滤器可以为一个使用加速度计的输出与上述被检体内的固定帧作为参考姿态和速度参考的单独的轴陀螺测试仪输出之间的角度的每个信号设计在一个经典的线性互补滤波器。时的全姿态低通估算可以来自IMU测量，例如进行重构，如果磁力计的数据也可和态度充分耦合的旋转矩阵可以被计算为在体内测定的引力和磁场的代数函数固定框架，则互补滤波器的非线性扩展已经面世了十五年。磁力是对小规模无人机由于磁场从电力推进系统和其它干扰造成的扰动很少有用。最近的工作由作者提供的明确的互补滤波（ECF）的制剂，直接构成对设定的旋转矩阵，即由单一的惯性测量方向驱动，如由加速度计的输出提供，随着陀螺测试仪的输出。
四轴飞行器要完成各种任务就需要人工无线电遥控导航或者自主导航。人工遥控导航飞行只能在视野范围内进行，如果四轴飞行器要执行视野范围外的任务，就必须自主导航。
常规飞行器一般用惯性导航设备或多普勒测地速设备，但由于庞大的体积、昂贵的价格等因素，难以应用于轻巧而廉价的四轴飞行器。而现今的全球定位系统GPS，拥有全天候、全球性、连续的精密三维导航与定位能力，并且重量和体积也非常适合无人四轴飞行器，同时价格也相当便宜，一块成品的GPS模块一般也就100人民币左右，因而GPS在航空导航、航海导航以及地面导航等方面应用非常广。
装载了GPS自主巡航系统后的飞行器大大拓展了其在民用与军用上的泛用性。用于民用型的自主巡航飞行器可执行灾情调查救援任务如水灾、火灾、地震等；喷洒农田、林区农药；监测化工厂等危险场所的危险气体的浓度；巡查输油管线、输电线路；连续监控重要的设施；区域性空－地、空－海通讯中继；当对特定地区进行日常环境监测的时候，用这种飞行器来执行也很方便和高效，自主巡查完后可以自动返回目的地并自动记录下存储的数据，大大减少了人力成本。据报道自2010年9月起，为了提高输电线路的巡检水平，江西省电力公司采用了无人机航巡输电线路，对输电线路本体缺陷、通道隐患进行快速探测，在各种地形复杂、气候恶劣的不利条件下，在第一时间里准确、及时、高效地取得现场资料。
致 谢
在完成学位论文之际，首先要感谢我的指导老师张艳丽，她在我做毕设期间给予我了莫大的帮助，我的进步和成长离不开导师的谆谆教诲和指导。在论文写作期间，张老师严格要求论文质量，认真仔细地审阅论文，给出很多宝贵意见。她渊博的知识、严谨的治学态度、求实的工作作风都深深地影响着我，激励着我。在此，再次对张老师表示诚挚的感谢和崇高的敬意！
同时，更要深深感谢企业导师王西鹏对我在课题研究方面的热情帮助，他不但在学术上给予我很多指导，还提供了许多珍贵的科研资料，帮我解决了很多科研中遇到的问题，从他身上我学到了踏实严谨的科研态度，刻苦努力的钻研精神。
最后，还要感谢我的父母家人和朋友，感谢他们对我的支持和鼓励，让我顺利完成学业。
厉害了我的哥