新書推薦：
《
英国商业500年（见证大国崛起与企业兴衰，启迪未来商业智慧。）
》
售價：HK$
82.8
《
万千心理·儿童心理治疗中的心智化：临床实践指导
》
售價：HK$
89.7
《
自我囚禁的人：完美主义的心理成因与自我松绑（破除你对完美主义的迷思，尝试打破自我评价过低与焦虑的恶性循环）
》
售價：HK$
67.9
《
周易
》
售價：HK$
45.8
《
东南亚的传统与发展
》
售價：HK$
69.0
《
乾隆制造
》
售價：HK$
87.4
《
资治通鉴臣光曰辑存 资治通鉴目录（司马光全集）（全二册）
》
售價：HK$
296.7
《
明代社会变迁时期生活质量研究
》
售價：HK$
308.2
|
| 編輯推薦： |
|
本书系统论述了四旋翼飞行器的基本原理，基本控制方法，本书从四旋翼飞行器的遥控器设计、四旋翼飞行器核心板设计以及陀螺仪、气压计等相关电路的设计和原理讲解，从零开始一点一点实现四旋翼飞行器的设计，同时还给出了上位机的软件代码，项目都提供了全部源代码，可直接拿来二次开发，用于课程设计或科研项目。本书配套提供了全部设计源代码、设计图纸及演示视频，方便读者学习使用，提供了学习交流QQ群（185156135），欢迎读者参与进来，和广大的STM32爱好者共同学习，及时解决设计中遇到的问题。
|
| 內容簡介： |
|
四轴飞行器是一种无人飞行器，也是一种智能机器人，四轴指飞行器的动力由4个旋翼式的飞行引擎提供。人们对于四轴飞行器的研究从军用到民用、商用领域都有涉及。近几十年来，随着现代控制理论与电子控制技术的发展，运用现代控制技术，使用电机代替油动力引擎进行四轴飞行器控制研究。本书利用主流控制器STM32系列微处理器平台，从设计的方案论证、器件选型、代码调试的全过程对四轴飞行器设计透彻细致地讲解，读者可以根据书中给出的电路和代码自行设计。本书可作为电子、通信及控制等相关专业的参考书，也可以作为相关技术人员的技术参考书。
|
| 關於作者： |
|
冯新宇：从事嵌入式系统项目开发和相关教学工作。曾主持或参与嵌入式相关课题与项目开发20余项；近10年来一直作为指导教师参与全国电子设计大赛、飞思卡尔电子设计大赛、黑龙江省电子设计大赛等工作，所指导的学生多次获得各种赛事重大奖励。个人代表性著作有《ADS2009射频电路设计与仿真》《ARM 9嵌入式开发基础与实例进阶》与《ARM Cortex-M3体系结构与编程》）。
|
| 目錄：
|
目录
第1章简介
1.1四旋翼飞行器发展历史
1.2四旋翼飞行器的研究现状
1.3四旋翼飞行器的主要应用
第2章四旋翼飞行器的控制原理
2.1四旋翼飞行器的结构
2.2四旋翼飞行器的运动控制方法
2.3四旋翼飞行器各部分的工作原理
2.3.1飞行姿态与升力关系
2.3.2飞行姿态的测量
2.3.3加速度传感器工作原理及角度测量
2.3.4陀螺仪传感器工作原理及角度测量
2.3.5磁力计传感器工作原理及测量方法
2.4姿态解算方法
2.4.1互补滤波算法
2.4.2卡尔曼滤波算法
2.4.3DMP姿态数据获取
2.5PID控制算法
2.5.1PID概述
2.5.2四轴飞行器PID控制器设计
第3章硬件设计
3.1协议预备知识
3.1.1SPI总线
3.1.2I2C总线
3.1.3USART总线
3.2总体设计
3.2.1遥控器电路基本框架
3.2.2飞行器主控电路基本框架
3.3飞行器主控电路最小系统设计
3.3.1基本原理
3.3.2硬件电路设计
3.4姿态传感器模块
3.4.1基本原理
3.4.2硬件电路设计
3.5无线通信模块
3.5.1基本原理
3.5.2硬件电路设计
3.6定高模块
3.6.1超声波定高模块
3.6.2气压计定高模块
3.7电机及驱动模块
3.7.1基本原理
3.7.2硬件电路设计
3.8遥控器模块设计
3.8.1基本原理
3.8.2硬件电路设计
3.9电源模块选择
3.10四轴飞行器的组装
3.10.1电机、浆、电池、机型的相互关系
3.10.2机架的组装
第4章软件设计
4.1软件预备知识
4.1.1刚体的空间角位置描述
4.1.2用欧拉角描述定点转动刚体的角位置
4.1.3四元数
4.1.4控制与滤波算法
4.2主控程序初始化设置及说明
4.2.1SPI的IO口初始化实现
4.2.2IIC的IO口初始化实现
4.2.3定时器初始化实现
4.2.4电子调速器初始化实现
4.3姿态传感器软件设计
4.3.1软件设计基本思路
4.3.2DMP
4.3.3代码实现及解析
4.4气压计软件设计
4.4.1软件设计基本思路
4.4.2代码实现及解析
4.4.3自主高度控制的实现
4.5遥控器软件设计
4.5.1软件设计基本思路
4.5.2无线模块代码实现及解析
4.5.3摇杆代码实现及解析
4.6摄像头软件设计
4.6.1软件设计基本思路
4.6.2摄像头的数据读取
4.6.3摄像头的数据处理
4.7上位机设计
4.7.1帧头检测模块
4.7.23D模型路径模块
4.7.33D模型属性设置模块
4.7.4陀螺仪3D数据显示模块
第5章调试、问题解析及改进方向随想
附录ASTM32F4最小系统电路图
附录B遥控器电路
附录C飞控板连接电路
参考文献
|
| 內容試閱：
|
前言
无人直升机，即旋转翼无人机。从20世纪50年代出现至今，它的发展较为缓慢，但是，随着与其相关技术的发展和应用，无人机飞行器领域迎来了革命性的巨变，其中无人直升机的研究和发展也迎来了一个全新的时代。无人直升机的功能越来越全面，体积越来越小，在军事领域和民用领域共同发展。本书的写作初衷是2015年的全国电子设计大赛和学生选择了飞控类的题目，把我们参加比赛制作的过程做了一个记录。比赛后，经过增加一些功能，PCB的全部设计制作，四轴飞行器飞控相对地稳定。结合实验室的一些项目，把上位机等相关的内容添加进来，斗胆把自己的一些心得写成一本书和大家分享。本书是按照无人机制作顺序来写的，更像是一本内容多一些的论文。个人觉得本书与其他众多书籍最大的不同是，上位机软件设计和操作手柄设计没有使用现有的成品。这么做是为了达到实践和锻炼的目的，特别是针对电子类相关专业的本科生。如果学生能照着做一个，调试出来，就能学到很多知识。例如，利用LabVIEW学习上位机软件设计，对于非计算机专业的学生还是挺实用的。本书中核心设计的内容应该是姿态数据的获取和处理，采用了DMP实现解算。稍后笔者编写的代码会发布在QQ群内和广大的程序爱好者一起研究和完善。互联网有很多优秀开源的代码，读者只要认真地熟读一个完整代码，其他的问题就会迎刃而解。入门的过程较难，特别是对于大学二年级与三年级的学生，需要自学的东西较多。本书完成得匆忙，很多东西加工得不是很细致，留个QQ群号，方便读者交流学习，QQ群： 185156135。本书的很多素材、资料都是来源于一些知名网络论坛和优秀的参考书，这里一并感谢！感谢黑龙江科技学院电子技术实验室支持我的同学和同事。由于时间仓促，书中难免存在不足之处，欢迎读者批评指正。
编者2017年4月
第1章简介
1.1四旋翼飞行器发展历史无人机是一种通过无线电遥控设备或自备程序算法自行控制的不载人飞行器。早在20世纪初，无人机就在多次局部战争中施展其无与伦比的优越性，并且为美国、以色列等国在战争中取得胜利起了重要作用。近三十年的时间里，随着微电子技术、通信技术、材料及推进系统等技术的迅猛发展，无人飞行器不仅外形上更加独特新颖，而且性能和功能上更加先进。从一开始通过地面摇杆控制飞行器，到用人眼控制飞行器，近些年又出现了用人脑来控制飞行器。由于无人机在作战中的优异表现，并且具有先天零伤亡的优势，各国在军事研究上都将其置于优先发展的地位，国际上也掀起了研究的热潮，其中以美国和以色列位居榜首。在无人机迅猛发展的同时，能够垂直起降的无人机备受关注，其中发展最快、研究最多的是四旋翼飞行器。它具有其他固定翼无人机无可匹敌的优越性，不仅能够执行各种非杀伤性任务，同时又能执行如侦察、目标截获、监视、通信中断等各种具有软硬杀伤性的任务。其结构与普通的单旋翼无人机设计不同，飞行器只能通过改变两对螺旋桨的转速来实现各种姿态的变化，通过四个螺旋桨的升力来实现各种飞行轨迹的控制。与传统的无人机比较，飞行器具有如下几点明显的优势： 体积小、重量轻，并且可以灵活地垂直起降； 易于控制，具有很好的机动性，并且能够快速地进行姿态控制； 结构简单、造价低廉，并且适用于一些比较危险的场合。四旋翼飞行器具有相互交叉的两对旋翼，通过控制旋翼的转向和转速，可抵消每对螺旋桨产生的反桨扭矩。此外还可以通过调节两对旋翼的转速改变其扭矩大小和升力，实现对飞行器姿态的控制，而不需要调节繁杂的机械装置，从而减轻了飞行器重量、减少了飞行器的体积、提高了其负载能力。20世纪初就有学者对四旋翼飞行器进行了研究。直到近几年来，随着传感器、处理器以及能源供给等相关技术的发展，同时又出现了新型的飞行控制理论，为解决四旋翼飞行器的各种疑难问题提供了极大的帮助。早期的飞行器如图11和图12所示。
图11Louis Breguet1907
图12George de Bothezat 1922.12
CurtissWright VZ7是垂直起降飞机，是CurtissWright公司专为美国陆军设计的，如图13所示，VZ7通过改变4个螺旋桨的推力实现控制。
图13CurtissWright VZ7（1958
1.2四旋翼飞行器的研究现状在过去的几十年里，小型无人机已经应用于许多领域。四旋翼飞行器研究的主要热点是如何提高机动性和悬停能力。四轴飞行器的四转子设计使得飞行器设计相对简单、可靠和易操作。目前四轴飞行器的研究热点在多机通信、环境探索及可操作性，如果把这些特性融合在一起，它可以完成目前其他车辆所不可能完成的很多任务。目前正在研发的有： 贝尔波音四倾转旋翼机的概念，提出对C130大型军用运输机采用固定直升机概念和倾斜转子概念进一步结合； Aermatica Spa Anteos是第一个旋转翼遥控飞机，获得官方许可的民用空域飞行，由意大利民航局设计制造，并首次工作在非隔离空域； Aeroquad和Arducopter基于Arduino的四旋翼飞行器DIY（Do It Yourself,自己动手做）建设开源硬件和软件项目； Parrot AR.Drone是一款带有摄像机无线控制的四旋翼飞行器，可以通过智能电话和平板设备进行操控，如图14所示； Nixie是一款带小相机配备的飞行器，可以作为一种穿戴设备，如图15所示。
图14Parrot AR.Drone飞行器
图15Nixie飞行器
1.3四旋翼飞行器的主要应用因为四轴飞行器体积小,重量轻，携带方便，能轻易进入人不易进入的各种恶劣环境。该飞行器常用来制作模型，也用来执行航拍电影取景、实时监控、地形勘探等飞行任务。目前主要应用在以下几个方面。 研发平台： 四旋翼飞行器是大学研究人员测试和评估在多个不同领域新思路的一个有用工具，包括飞行控制、导航、实时系统和机器人。近年来，许多高校已经让四旋翼飞行器从事越来越复杂的空中演习。成群的四轴飞行器可以悬停在空中，自主执行复杂的飞行程序，如翻转。 军事和执法： 四旋翼无人机用于军事侦察和执法机构侦察，以及在城市环境中的搜索和救援任务。由加拿大公司Aeryon Labs研发的小型无人机Scout，可以静静地在原地徘徊，并用相机观察地面上的人和物。用户在超过视线范围内3km操作Scout，在距地面100~150m的飞行速度达50kmh，续航25min。它允许飞行在恶劣天气条件下，所有通信都采用数字加密，从而降低了劫持和视频截取的风险，如图16所示。
图16Scout飞行器
图17大疆无人机
商业应用： 商业应用最成功的是大疆公司，它约占世界民用无人机市场份额的70%，以及全球小型无人机市场份额的50%。随着大疆公司把目光从无人机本身的平台，逐步移向建立在飞行器平台上的增值应用，未来的市场空间对于这家新兴公司来说，仍然是个未知数。在美国使用四轴飞行器最大的领域是航空影像。四旋翼无人机为航拍节约了大量成本。大疆无人机如图17所示。
第2章四旋翼飞行器的控制原理
2.1四旋翼飞行器的结构如图21所示，四旋翼飞行器一般是由4个可以独立控制转速的外转子直流无刷电机驱动的螺旋桨提供全部动力的飞行运动装置，4个固定迎角的螺旋桨分别安装在两个十字相交的刚性碳素杆两端。
图21四旋翼飞行器结构示意图
对于绝大多数四旋翼飞行器来讲，飞行器的结构关于两根碳素杆的交点对称，并且两个相邻的螺旋桨旋转方向相反。正是由于这种独特结构，使四旋翼飞行器抵消了飞机的陀螺效应，更方便建模。与传统的单旋翼飞行器，特别是直升机相比，四旋翼飞行器没有尾桨，这使之拥有更高的能量利用率。另外，四旋翼飞行器四个旋翼的转速比直升机的螺旋桨转速明显低很多。因此，它可以近距离地靠近目标物体，适合室内飞行和近地面飞行。
2.2四旋翼飞行器的运动控制方法四旋翼飞行器系统共有4个输入，分别为一个上升力和三个方向的转矩。但是飞行器在空间中却有6个自由度的输出坐标，可以进行三个坐标轴方向的平动运动和围绕三个坐标轴方向的转动运动。如果沿着任意给定方向的独立运动，飞行器没有给予足够多的运动驱动，那么该飞行器就是欠驱动的。可见，四旋翼飞行器是欠驱动和动力不稳定的系统。因此，针对该系统实现全部的运动控制目标，必然存在旋转力矩与平移系统的耦合。传统的纵列式直升机为了平衡反扭矩，需借助尾桨来实现。四旋翼飞行器采用了4个旋翼的机械结构，4个电机作为飞行的直接动力源，通过改变4个螺旋桨的转速，进而改变螺旋桨产生的升力来控制飞行器姿态和运动。这种设计理念使飞行器结构和动力学特性得到了很大简化。四旋翼的前桨1和后桨3逆时针旋转，左右2、4两桨顺时针旋转，这种反向对称结构代替了传统直升机尾旋翼。在飞行过程中，如图22所示，改变4个旋翼螺旋桨的转速，可使四旋翼产生各种飞行姿态，也可使四旋翼飞行器向预定方向运动，完成任务。
图22四旋翼飞行器飞行运动原理
根据四旋翼飞行器的运动方式的特点将其飞行控制划分为4种基本的飞行控制方式： 垂直飞行控制； 横滚控制； 俯仰控制； 偏航控制。下面分别对以上4种飞行控制方式进行阐述。垂直飞行控制主要是控制飞机的爬升、下降和悬停。如图23所示，弧线箭头方向表示螺旋桨旋转的方向。当四旋翼处于水平位置时，在垂直方向上，惯性坐标系同机体坐标系重合。同时增加或减小4个旋翼的螺旋桨转速，4个旋翼产生的升力使得机体上升或下降，从而实现爬升和下降。悬停时，保持4个旋翼的螺旋桨转速相等，并且保证产生的合推力与重力相平衡，使四旋翼在某一高度处于相对静止状态，各姿态角为零。垂直飞行控制的关键是要稳定4个旋翼的螺旋桨转速，使其变化一致。
横滚控制，如图24所示。通过增加左边旋翼螺旋桨转速，使拉力增大，相应减小右边旋翼螺旋桨转速，使拉力减小，同时保持其他两个旋翼螺旋桨转速不变。这样由于存在拉力差，机身会产生侧向倾斜，从而使旋翼拉力产生水平分量，使机体向右运动。当2=4时可控制四旋翼飞行器作侧向平飞运动。
图23垂直飞行控制示意图
图24横滚运动控制示意图
俯仰控制，如图25所示，与横滚控制相似，在保持左右两个旋翼螺旋桨转速不变的情况下，减少前面旋翼螺旋桨的转速，并相应增加后面旋翼螺旋桨的转速，使得前后两个旋翼存在拉力差，从而引起机身的前后倾斜，使旋翼拉力产生与横滚控制中水平方向正交的水平分量，使机体向前运动。类似地，当1=3时可控制四旋翼飞行器作纵向平飞运动。
偏航控制，如图26所示。四旋翼飞行器为了克服反扭矩影响，4个旋翼螺旋桨中的两个顺时针转，两个逆时针转，且对角线上的两个旋翼螺旋桨转动方向相同。
图25俯仰运动控制示意图
图26偏航控制示意图
反扭矩的大小与旋翼螺旋桨转速有关，当4个旋翼螺旋桨转速不完全相同时，不平衡的反扭矩会引起机体转动。根据上面的原理，可以设计四旋翼飞行器的偏航控制，即同时提升一对同方向旋转的旋翼螺旋桨转速并降低另一对相反方向旋转的旋翼螺旋桨转速，保证转速增加的旋翼螺旋桨转动方向与四旋翼飞行器机身的转动方向相反。
2.3四旋翼飞行器各部分的工作原理2.3.1飞行姿态与升力关系
为便于进行四轴飞行器运动分析，建立刚体三轴坐标系，将四轴飞行器置于刚体坐标系其中如图27所示，飞行器运动过程中飞行姿态与各螺旋桨所产生升力之间的关系借助此坐标轴进行分析。
1. 飞行器绕y轴旋转角度与升力之间的关系如图28所示，飞行器与y轴之间夹角。主要通过左右螺旋桨产生升力差控制，其控制关系为
M=Ix（21）
式中： M力矩；Ix转动惯量；飞行器与y轴夹角二阶导数，即角加速度。
lxF右-F左=Ix（22）
式中： F右右侧螺旋桨旋转产生升力；F左左侧螺旋桨旋转产生升力;lx螺旋桨与飞行器中心轴距。
=lx（F右-F左）Ix（23）
图27飞行器坐标轴建立
图28飞行器绕y轴的角度与F左、F右的关系
2. 飞行器绕x轴旋转角度与升力之间的关系如图29所示，飞行器与x轴夹角主要通过前后两个螺旋桨所产生升力差值进行控制，其控制关系为
M=Iy24
lyF前-F后=Iy25
=ly（F前-F后）Iy26
图29飞行器绕x轴的角度与F前、F后的关系
3. 飞行器绕z轴旋转角度与升力之间的关系如图210所示，飞行器绕z轴旋转角度，使螺旋桨产生扭矩及升力与旋转角度之间的关系为
M=Iz27
M右 M左-M前-M后=Iz28
图210飞行器绕z轴旋转角度与M前、M后、M左、M右之间的关系
=M右 M左-M前-M后Iz29
由于螺旋桨所产生的升力和力矩之间存在关系M=cF，所以上式可以表示为与升力之间的关系：
=c右F右 c左F左-c前F前-c后F后Iz210
假定各个螺旋桨性能参数一致，则可以认为c前=c后=c左=c右=c，上式可以简化为
=cF左 F右-F前-F后Iz211
4. 飞行器飞行速度与螺旋桨升力之间的关系根据牛顿第二定律：
F=zml212
F前 F后 F左 F右-mtg=zml213
z=F前 F后 F左 F右-mtgml214
2.3.2飞行姿态的测量飞行姿态是一个真实飞行物体与参考坐标系之间的角度关系。如2.3.1节中分析使用到的、、角，这三个角度也称为欧拉角，对应Pitch、Yaw、Roll。常用姿态测量传感器有加速度传感器、角速度传感器、磁力传感器、气压传感器、超声波及GPS（Global Positioning System,全球定位系统）等。若需要获取比较精确姿态定位数据，则需要融合计算上述多个传感器测量数据。对于嵌入式平台应用，多种传感器数据融合计算对微处理器的运算能力要求较高。选择与实际开发平台相符合的姿态传感器尤为重要，本设计采用加速度与角速度测量飞行器姿态，两者测量数据互补融合计算姿态角，可以满足飞行姿态稳定性要求。2.3.3加速度传感器工作原理及角度测量加速度传感器是测量由物体重力加速度引起的加速度量。物理静止或运动过程中，受重力作用，会产生物体相对于三个坐标轴方向上的重力分量，通过对重力分量进行量化，运用三角函数可计算出物体相对于三个坐标轴的倾角。图211所示为加速度传感器测量时重力惯性矢量的三轴分量Rx、Ry、Rz。利用三角函数即可求出重力加速度与三个坐标轴夹角、、。
=arccosRyR215
=arccosRxR216
=arccosRzR217
R=R2x R2y R2z218
式中：重力矢量与y轴夹角；重力矢量与x轴夹角；重力矢量与z轴夹角；Rx加速度计测量重力加速度x轴分量；Ry加速度计测量重力加速度y轴分量；Rz加速度计测量重力加速度z轴分量；R重力矢量。
图211加速度测量
2.3.4陀螺仪传感器工作原理及角度测量角速度传感器（陀螺仪）用来测量一段时间内角度变化速率。对两次测量时间差值进行积分可得到角度增量值。增量值可正可负，正值表示向角度增大方向旋转，负值表示向原角度减小反方向旋转，积分后与测量前初始角度求和可计算出当前角度。
=0 t0dt219
式中：旋转角度值；0上一次旋转角度值；角速度测量值；t测量间隔时间。实际使用中，需要得到更加精确的角速度值，可以使角速度测量值取前一次测量值与后一次测量值进行求平均，且两次测量时间应尽量短。但是角速度测量值在多次积分之后会引入很大的误差，误差一部分为积分时间间隔误差; 另一部分即陀螺仪本身存在一定误差（漂移）。为尽量减小误差可采取两个措施，减小测量时间间隔和一段时间间隔内重新校准陀螺仪。
2.3.5磁力计传感器工作原理及测量方法地球的磁场就像一个偶极子，地球的南北极为这个偶极子的两极。在地球极地处，地球磁场的磁场强度为0.6高斯，赤道处的磁场强度为0.3高斯。但是，偶极子只是对地球磁场的简单比喻。对于地球磁场来说，国际参考磁场是一个更加准确的模型。此模型中包含一系列球谐条款，根据球谐条款对应一个系数，利用这个系数可以计算出当地的磁场强度，由于地球磁场随着时间发生漂移，所以这些系数每5年被国际地磁与高空物理学协会更新一次。一些情况下地球磁场会发生变化，日常由于太阳辐射产生的电离层会导致地球磁场发生 0.0001~0.001高斯的变化。每个月发生的几次太阳耀斑磁暴可产生高达0.01高斯强度的磁场变化。这些因素在一定程度上使地球磁场的强度和方向发生变化。目前用来测量地球磁场的磁力计主要有三种：磁通门式： 磁通门式磁力计在1928年问世，一直沿用至今。磁通门式磁力计基于磁饱和法，利用被测磁场中磁芯在交变磁场饱和励磁下其磁感应强度与磁场强度的非线性关系来测量弱磁场的一种方法。这些设备往往是笨重的，而且不耐用，作为较小的集成传感器，响应时间慢。霍尔效应式： 霍尔效应磁力计的工作原理为通过感测附近的交变磁场而产生输出电压的传感器。这种磁力计的设计简单，价格低廉，适用于对强磁的测量，但由于灵敏度低，噪声大而不适用于测量地磁场。磁阻式： 利用磁阻效应的传感器利用电阻组成惠斯登电桥测量磁场。磁阻式传感器的灵敏度高、体积小、响应时间快。磁力计作为测试磁场密度的传感器，广泛应用于科研和工程等领域。在导航领域中，磁力计用于求取载体姿态中航向角的估计，航向角为磁力计用测量信息在水平方向上的分量求得。三轴磁力计在地球磁场坐标系下的测量值为
hb=［hbxhbyhbz］T
其中，上标b表示了向量h为地球磁场坐标系下的向量，下标x，y，z，表示三轴磁力计各轴的分量。当磁力计水平放置水平面上时，可以利用向量h在水平方向上的两个分量航向hx和hy求出磁力计坐标系x与地球磁场北极的夹角，即航向角推导公式为：
=arctan hbyhbx220
单独地使用磁力计测量航向角时，由于磁力计非水平放置，导致磁力计产生倾斜角误差，因此磁力计常与加速度计一起组成电子罗盘。当磁力计与惯性传感器组合使用进行姿态测试时，磁力计用来估算运动目标的航向角，用以校正陀螺仪漂移误差。当外界磁场发生突变时，磁力计对载体的航向角估算值失真。2.4姿态解算方法2.4.1互补滤波算法
互补滤波器作为一种频域特性滤波器，常用于融合来自不同传感器测量得到的数据。一般地，互补滤波器包含至少两种频率特性互补的输入信号。例如，对于陀螺仪和加速度计解算姿态这一双输入系统，两个输入量都能分别对姿态角进行解算，其中加速度计输入量包含高频噪声，应通过低通滤波器来滤除； 陀螺仪则包含低频噪声（积分漂移），应采用高频滤波器滤除。两者的频率特性互补，可用互补滤波思想进行姿态解算，最终输出较准确信号，其工作原理如图212所示。
图212互补滤波姿态融合原理
设运用加速度计和陀螺仪分别解算出的飞行器姿态角x的值为
x1=x u1x2=x u2221
其中，u1、u2分别为加速度计高频噪声和陀螺仪低频噪声。取互补滤波器由低通滤波器FLs=1fs 1和高通滤波器FHs=fsfs 1两部分构成，f为滤波器常数，有
FLs FHs=1222
则姿态角x的估计值X^可表示为
X^s=FLsX1s FHsX2s=Xs FLU1s FHU2s223
式223对应差分方程表达式为
X^k=1-fX^k-1 X2k-X2k-1 fX1k224
式中，X2k为k时刻陀螺仪角速度积分值； X1k为加速度计解算的角度值。相对于单传感器方案，互补滤波可以避免加速度计精度和动态性能不足的问题，也能避免陀螺仪的漂移误差。由式224可见互补滤波器的结构简单、计算量小，其编程思想可以描述为
angle=1-Aangle_last gyrodt Aacc225
式中：angle当前融合而成的姿态角；angle_last为上一次姿态角融合结果值；gyro为当前陀螺仪测量的角速度；dt为积分时间；acc为加速度计解算出的姿态角。滤波器系数为A，一般取A小于0.1，表示每次解算结果中陀螺仪积分角度占比重较大，加速度计解算角度占比重较小，这也就相当于对陀螺仪信号高通滤波而对加速度计信号低通滤波。由此可以看出，虽然加速度计信号在每次姿态解算中所占权值较小，但随着运行时间的增加，加速度计信号实际上是在不断对陀螺仪积分进行缓慢的矫正，从而减小因陀螺仪积分时间增加而引起的漂移误差。图213是取互补滤波器系数A为0.02时对四轴飞行器俯仰角进行姿态解算的结果。
图213俯仰角互补滤波融合结果
从图213a中可以看出，只用加速度计进行姿态结算虽然没有静态误差，但其波形是含有一定量的噪声，尤其是在图中15~20s时，由于飞行器姿态改变，其外力加速度将使解算角度波动较大。图213b中的陀螺仪直接积分解算结果波形虽然平滑，即使在飞行器姿态改变时，其积分角度也没有波动，但随着时间的增加，将出现积分角度的漂移。图中仅进行了25s姿态解算，陀螺仪漂移误差就达到了将近2。因此，进行姿态融合是十分必要的。在图213c中，互补滤波取得了较好的效果，不仅波形平滑，而且陀螺仪漂移误差也得到有效矫正。互补滤波姿态融合虽然计算简单，运算量小，也能够取得较理想的融合效果，但对于参数A选取却没有比较好的解决办法，一般是通过不断调试和反复修改进行确定。若A过大，则角度收敛较慢，动态性能降低； 若A过小，则角度波动较大，滤波效果降低。因此一般还需要运用模糊算法或其他自适应算法对参数值进行实时调整，以满足系统对静态和动态时的性能要求。2.4.2卡尔曼滤波算法卡尔曼滤波算法是由匈牙利数学家鲁道夫卡尔曼于1960年提出的一种递归更新滤波算法。该算法为了描述整个计算更新的过程，提供了一组有效的递归推算方程组来估计过程的状态量，其间使估计均方误差最小化。由于卡尔曼滤波器的概率原型解释及推导过程等更完整的讨论相对烦琐，不是本文研究重点，下面仅简单介绍离散卡尔曼滤波算法。设离散时间控制系统状态变量XRn，可由以下离散随机差分方程描述：
Xk=AXk-1 BUk Wk226
观测反馈量即系统输出方程可描述为
Zk=HXk Vk227
其中，Wk和Vk均为随机信号，分别表示过程白噪声和观测白噪声且相互独立，服从正态分布：
pw~N（0，Q）pv~N（0，R）228
式中，噪声协方差系数R与Q的取值，关系到最终滤波的效果和响应速度。但两者相互制约，R取值越小，滤波响应和收敛越迅速； Q取值越小，抑制、滤除噪声的能力越强。卡尔曼滤波的基本公式为1 状态预测方程： 由系统状态变量k-1时刻的最优值Xk-1|k-1和系统输入Uk可以求出k时刻系统预测值Xk|k-1为
Xk|k-1=AXk-1|k-1 BUk229
2 协方差预测方程： 根据k-1时刻系统协方差矩阵Pk-1|k-1预测k时刻系统协方差矩阵Pk|k-1为
Pk|k-1=APk-1|k-1AT Q230
3 卡尔曼增益计算方程： 根据状态变量预测值和协方差矩阵预测值计算卡尔曼增益 Kgk为
Kgk=Pk|k-1HTHPk|k-1HT R231
4 最优值更新方程： 由状态变量预测值和系统测量值计算k时刻状态变量最优值Xk|k为
Xk|k=Xk|k-1 KgkZk-HXk|k-1232
5 协方差更新方程： 更新k时刻协方差矩阵Pk|k为
Pk|k=1-KgkHPk|k-1233
最优值和协方差更新方程都计算结束后，整个滤波过程进入下一轮循环，重复1~5步。递推算法
图214卡尔曼滤波器工作流程
的本质体现在估计过程，每次只需根据以前的测量值递归计算，就能得到当前时刻的状态估计。卡尔曼滤波器的工作流程如图214所示。以四轴飞行器俯仰角状态变量为例，进行卡尔曼融合姿态解算。令状态变量X为俯仰角度，系统状态方程为和测量方程分别为
Xk=k=1Xk-1 TsWgyro Tswgk234
Zk=1Xacck wak235
式中，Ts为系统采样周期，Wgyro为陀螺仪测量角速度，Xacc为加速度计计算的俯仰角度，wg、wa分别为陀螺仪和加速度计测量的噪声。取系统采样周期Ts=0.005s，系统噪声协方差矩阵Q=0.001，测量误差协方差矩阵R=0.5，滤波器初始条件X0=0、P0=1，根据卡尔曼滤波基本公式（式229~式233）设计俯仰角姿态融合算法，得到姿态解算结果如图215所示。
图215俯仰角卡尔曼滤波融合结果
在图215中使用的传感器数据与之前互补滤波使用的数据相同，可以看出，采用卡尔曼滤波进行姿态融合也可以得到较好的融合效果，其波形平滑度和静态漂移误差比互补滤波稍好。在滤波融合算法设计过程中，主要对协方差Q和R的取值进行设计，R取值越小，滤波响应和收敛越迅速； Q取值越小，抑制、滤除噪声的能力越强。因此，其具体取值也需要反复实际调试进行权衡确定。
2.4.3DMP姿态数据获取运用互补滤波与卡尔曼滤波思想进行姿态融合的过程归根结底都是利用加速度计解算出的姿态角去修正陀螺仪积分的漂移误差。这两种方法在姿态融合过程中姿态角的表示形式都是欧拉角表示。用欧拉角进行姿态解算在大角度计算时会出现万向节锁（角度为90时加速度计进行姿态解算的反三角函数无解），为了避免该问题，可采用四元数来解算姿态。四元数姿态解算流程如下。1 初始化四元数：设当前的坐标系为机体坐标系，则四元数列向量：
q=［q0q1q2q3］T=［1000］T236
2） 获取角速度、加速度： 读取MPU6050三轴加速度计和三轴陀螺仪的信号并经过IIR低通滤波器滤波滤除振动噪声，得到重力加速度分量accx、accy、accz和角速度分量x、y、z。3 将加速度计测量值accx、accy、accz转化为三维的单位向量归一化：
ax=accxacc2x acc2y acc2zay=accyacc2x acc2y acc2zaz=acczacc2x acc2y acc2z237
4 用四元数表示三轴的重力分量Vx、Vy、Vz：
Vx=2（q1q3-q0q2）Vy=2（q1q0-q3q2）Vz=q20-q21-q22 q23238
式中Vx、Vy、Vz： 即重力单位向量在机体坐标系中的分量。5 求四元数所求重力分量与加速度计测量值的误差值：
ex=ayVz-azVyey=azVx-axVzez=axVy-ayVx239
在机体坐标系中，加速度计测量的重力加速度分量为accx、accy、accz； 陀螺仪积分后推算得到的重力向量是Vx、Vy、Vz。两者之间的误差，即陀螺仪积分误差。此处，向量间的误差ex、ey、ez是用向量积外积、叉乘表示，该误差向量仍位于机体坐标系中。
6 利用所得的误差修正陀螺仪的测量值：
exint=e^xint kiexx=x kpex exint240
eyint=e^yint kieyy=y kpey eyint241
ezint=e^zint kiezz=z kpez ezint242
式中，参数ki、kp用以控制加速度计修正陀螺仪误差的速度。7 利用修正后的陀螺仪值x 、y、z更新四元数：
q0=q^0 dt2-q1x-q2y-q3zq1=q^1 dt2q0x q2z-q3yq2=q^2 dt2q0y-q1z q3xq3=q^3 dt2q0z q1y-q2x243
8 将得到更新后的四元数规范化：
q0=q^0q20 q21 q22 q23q1=q^1q20 q21 q22 q23q2=q^2q20 q21 q22 q23q3=q^3q20 q21 q22 q23244
公式（240）~（244）中e^xint,e^yint,e^zint,q^0~q^4表示更新后的值。9 得到新四元数后即完成了一次四元数法姿态融合的运算。将新四元数作为下一次四元数运算的初始四元数，再从步骤1开始下一次的四元数运算。为了直观表示飞行器的姿态，可将新四元数转化成为三个欧拉角：
=arctan2q2q3 q0q1q20-q21-q22 q23245
=arcsin-2q1q3-q0q2246
=arctan2q2q1 q0q3q20 q21 q22 q23247
总结以上9个步骤，四元数算法流程如图216所示。
通过四元数姿态解算的流程可以看出，其主要思想还是利用加速度计对陀螺仪进行修正，其修正的快慢程度由参数kp和ki进行控制。由于使用该方法步骤较为烦琐，涉及中间变量转换较多，且计算量较大，占用内存也较大，使用起来很不方便。为解决该问题，运动控制传感器MPU6050提供了DMP内部四元数解算功能，可以直接输出四元数数据，从而省略了烦琐的计算步骤，给设计带来了极大便利。运动处理传感器MPU6050除了提供三轴陀螺仪和三轴加速度计传感器的16位ADC（AnalogtoDigital Converter,模数转换器）信号采集功能之外，还集成了数字低通滤波器和数字运动处理器DMP（Digital Motion Processor），可以直接输出经低通滤波处理和四元数姿态解算后的四元数数据。将该四元数转换为欧拉角，可以得到准确的俯仰角和横滚角。配置MPU6050内部DMP功能的流程如图217所示。
图216四元数法姿态解算流程
图217MPU6050内部DMP功能配置流程
按如图217所示的流程对DMP功能进行配置后，即可调用库函数直接读取四元数姿态角数据。由于没有融合磁力计数据，因此偏航角的计算仍然是由陀螺仪积分得到。对偏航角数据的融合，也可采用之前介绍的互补滤波、卡尔曼滤波及四元数法，其具体实现过程与用加速度计修正陀螺仪误差类似，在此不再赘述。对于同一组飞行姿态数据，图218所示展示了互补滤波姿态融合、卡尔曼滤波姿态融合以及MPU6050内部DMP姿态解算俯仰角波形对比结果。
图218不同解算方法姿态解算结果对比图
可以看出，三种姿态解算方法都能取得较好的解算效果，其中DMP内部解算在飞行器静止时稳态噪声最小，在姿态角动态改变时，互补滤波和卡尔曼滤波姿态解算受加速度计影响更大，而DMP解算则收敛相对较慢。总的来说，DMP解算姿态数据更平稳，动态性能也比较理想。从设计难度上来说，互补滤波和卡尔曼滤波姿态解算都需要根据实际调试反复修改滤波器参数才能达到较好的解算效果，而DMP解算直接输出四元数数据，只需要将其转换为欧拉角即可用于姿态控制，大大简化了四轴飞行器姿态解算难度。综合考虑，本书设计的四轴飞行器姿态解算方法采用MPU6050内部姿态解算。2.5PID控制算法2.5.1PID概述
PID即比例、积分、微分控制器,作为最早实用化的控制器已有近百年历史，也是现在应用最为广泛的工业控制器,其基本结构如图219所示。
图219PID控制器基本结构
如图219所示，PID控制器由比例单元、积分单元和微分单元组成，其输入是期望值rt和输出测量值yt之间的误差et。早期的PID控制器通过硬件实现称为模拟PID，其基本公式为
ut=Kpet 1Tit0etdt Tdetdt248
式中，Kp为控制器比例系数，Ti为积分系数，Td为微分系数。对应传递函数为
|
|
購物車/收銀台(0) | 在線留言板
| 付款方式
| 運費計算
| 聯絡我們
| 幫助中心 |
加入書簽
HOME
新書上架
