四旋翼无人飞行器设计

目录

第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的I/O口初始化实现

4.2.2IIC的I/O口初始化实现

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飞控板连接电路

参考文献

第1章简介
1.1四旋翼飞行器发展历史无人机是一种通过无线电遥控设备或自备程序算法自行控制的不载人飞行器。早在20世纪初，无人机就在多次局部战争中施展其无与伦比的优越性，并且为美国、以色列等国在战争中取得胜利起了重要作用。近三十年的时间里，随着微电子技术、通信技术、材料及推进系统等技术的迅猛发展，无人飞行器不仅外形上更加独特新颖，而且性能和功能上更加先进。从一开始通过地面摇杆控制飞行器，到用人眼控制飞行器，近些年又出现了用人脑来控制飞行器。由于无人机在作战中的优异表现，并且具有先天零伤亡的优势，各国在军事研究上都将其置于优先发展的地位，国际上也掀起了研究的热潮，其中以美国和以色列位居榜首。在无人机迅猛发展的同时，能够垂直起降的无人机备受关注，其中发展快、研究多的是四旋翼飞行器。它具有其他固定翼无人机无可匹敌的优越性，不仅能够执行各种非杀伤性任务，同时又能执行如侦察、目标截获、监视、通信中断等各种具有软硬杀伤性的任务。其结构与普通的单旋翼无人机设计不同，飞行器只能通过改变两对螺旋桨的转速来实现各种姿态的变化，通过四个螺旋桨的升力来实现各种飞行轨迹的控制。与传统的无人机比较，飞行器具有如下几点明显的优势：  体积小、重量轻，并且可以灵活地垂直起降；  易于控制，具有很好的机动性，并且能够快速地进行姿态控制；  结构简单、造价低廉，并且适用于一些比较危险的场合。四旋翼飞行器具有相互交叉的两对旋翼，通过控制旋翼的转向和转速，可抵消每对螺旋桨产生的反桨扭矩。此外还可以通过调节两对旋翼的转速改变其扭矩大小和升力，实现对飞行器姿态的控制，而不需要调节繁杂的机械装置，从而减轻了飞行器重量、减少了飞行器的体积、提高了其负载能力。20世纪初就有学者对四旋翼飞行器进行了研究。直到近几年来，随着传感器、处理器以及能源供给等相关技术的发展，同时又出现了新型的飞行控制理论，为解决四旋翼飞行器的各种疑难问题提供了极大的帮助。早期的飞行器如图11和图12所示。

图11Louis Breguet(1907)

图12George de Bothezat (1922.12)

CurtissWright VZ7是垂直起降飞机，是CurtissWright公司专为美国陆军设计的，如图13所示，VZ7通过改变4个螺旋桨的推力实现控制。

图13CurtissWright VZ7（1958)

1.2四旋翼飞行器的研究现状在过去的几十年里，小型无人机已经应用于许多领域。四旋翼飞行器研究的主要热点是如何提高机动性和悬停能力。四轴飞行器的四转子设计使得飞行器设计相对简单、可靠和易操作。目前四轴飞行器的研究热点在多机通信、环境探索及可操作性，如果把这些特性融合在一起，它可以完成目前其他车辆所不可能完成的很多任务。目前正在研发的有：  贝尔波音四倾转旋翼机的概念，提出对C130大型军用运输机采用固定直升机概念和倾斜转子概念进一步结合；  Aermatica Spa Anteos是个旋转翼遥控飞机，获得官方许可的民用空域飞行，由意大利民航局设计制造，并首次工作在非隔离空域；  Aeroquad和Arducopter基于Arduino的四旋翼飞行器DIY（Do It Yourself,自己动手做）建设开源硬件和软件项目；  Parrot AR.Drone是一款带有摄像机无线控制的四旋翼飞行器，可以通过智能电话和平板设备进行操控，如图14所示；  Nixie是一款带小相机配备的飞行器，可以作为一种穿戴设备，如图15所示。

图14Parrot AR.Drone飞行器

图15Nixie飞行器

1.3四旋翼飞行器的主要应用因为四轴飞行器体积小,重量轻，携带方便，能轻易进入人不易进入的各种恶劣环境。该飞行器常用来制作模型，也用来执行航拍电影取景、实时监控、地形勘探等飞行任务。目前主要应用在以下几个方面。 研发平台： 四旋翼飞行器是大学研究人员测试和评估在多个不同领域新思路的一个有用工具，包括飞行控制、导航、实时系统和机器人。近年来，许多高校已经让四旋翼飞行器从事越来越复杂的空中演习。成群的四轴飞行器可以悬停在空中，自主执行复杂的飞行程序，如翻转。 军事和执法： 四旋翼无人机用于军事侦察和执法机构侦察，以及在城市环境中的搜索和救援任务。由加拿大公司Aeryon Labs研发的小型无人机Scout，可以静静地在原地徘徊，并用相机观察地面上的人和物。用户在超过视线范围内3km操作Scout，在距地面100~150m的飞行速度达50km/h，续航25min。它允许飞行在恶劣天气条件下，所有通信都采用数字加密，从而降低了劫持和视频截取的风险，如图16所示。

图16Scout飞行器

图17大疆无人机

 商业应用： 商业应用成功的是大疆公司，它约占世界民用无人机市场份额的70%，以及全球小型无人机市场份额的50%。随着大疆公司把目光从无人机本身的平台，逐步移向建立在飞行器平台上的增值应用，未来的市场空间对于这家新兴公司来说，仍然是个未知数。在美国使用四轴飞行器的领域是航空影像。四旋翼无人机为航拍节约了大量成本。大疆无人机如图17所示。

第2章四旋翼飞行器的控制原理
2.1四旋翼飞行器的结构如图21所示，四旋翼飞行器一般是由4个可以独立控制转速的外转子直流无刷电机驱动的螺旋桨提供全部动力的飞行运动装置，4个固定迎角的螺旋桨分别安装在两个十字相交的刚性碳素杆两端。

图21四旋翼飞行器结构示意图

对于绝大多数四旋翼飞行器来讲，飞行器的结构关于两根碳素杆的交点对称，并且两个相邻的螺旋桨旋转方向相反。正是由于这种独特结构，使四旋翼飞行器抵消了飞机的陀螺效应，更方便建模。与传统的单旋翼飞行器，特别是直升机相比，四旋翼飞行器没有尾桨，这使之拥有更高的能量利用率。另外，四旋翼飞行器四个旋翼的转速比直升机的螺旋桨转速明显低很多。因此，它可以近距离地靠近目标物体，适合室内飞行和近地面飞行。
2.2四旋翼飞行器的运动控制方法四旋翼飞行器系统共有4个输入，分别为一个上升力和三个方向的转矩。但是飞行器在空间中却有6个自由度的输出坐标，可以进行三个坐标轴方向的平动运动和围绕三个坐标轴方向的转动运动。如果沿着任意给定方向的独立运动，飞行器没有给予足够多的运动驱动，那么该飞行器就是欠驱动的。可见，四旋翼飞行器是欠驱动和动力不稳定的系统。因此，针对该系统实现全部的运动控制目标，必然存在旋转力矩与平移系统的耦合。传统的纵列式直升机为了平衡反扭矩，需借助尾桨来实现。四旋翼飞行器采用了4个旋翼的机械结构，4个电机作为飞行的直接动力源，通过改变4个螺旋桨的转速，进而改变螺旋桨产生的升力来控制飞行器姿态和运动。这种设计理念使飞行器结构和动力学特性得到了很大简化。四旋翼的前桨1和后桨3逆时针旋转，左右2、4两桨顺时针旋转，这种反向对称结构代替了传统直升机尾旋翼。在飞行过程中，如图22所示，改变4个旋翼螺旋桨的转速，可使四旋翼产生各种飞行姿态，也可使四旋翼飞行器向预定方向运动，完成任务。

图22四旋翼飞行器飞行运动原理

根据四旋翼飞行器的运动方式的特点将其飞行控制划分为4种基本的飞行控制方式：  垂直飞行控制；  横滚控制；  俯仰控制；  偏航控制。下面分别对以上4种飞行控制方式进行阐述。垂直飞行控制主要是控制飞机的爬升、下降和悬停。如图23所示，弧线箭头方向表示螺旋桨旋转的方向。当四旋翼处于水平位置时，在垂直方向上，惯性坐标系同机体坐标系重合。同时增加或减小4个旋翼的螺旋桨转速，4个旋翼产生的升力使得机体上升或下降，从而实现爬升和下降。悬停时，保持4个旋翼的螺旋桨转速相等，并且保证产生的合推力与重力相平衡，使四旋翼在某一高度处于相对静止状态，各姿态角为零。垂直飞行控制的关键是要稳定4个旋翼的螺旋桨转速，使其变化一致。

横滚控制，如图24所示。通过增加左边旋翼螺旋桨转速，使拉力增大，相应减小右边旋翼螺旋桨转速，使拉力减小，同时保持其他两个旋翼螺旋桨转速不变。这样由于存在拉力差，机身会产生侧向倾斜，从而使旋翼拉力产生水平分量，使机体向右运动。当Δ2=Δ4时可控制四旋翼飞行器作侧向平飞运动。

图23垂直飞行控制示意图

图24横滚运动控制示意图

俯仰控制，如图25所示，与横滚控制相似，在保持左右两个旋翼螺旋桨转速不变的情况下，减少前面旋翼螺旋桨的转速，并相应增加后面旋翼螺旋桨的转速，使得前后两个旋翼存在拉力差，从而引起机身的前后倾斜，使旋翼拉力产生与横滚控制中水平方向正交的水平分量，使机体向前运动。类似地，当Δ1=Δ3时可控制四旋翼飞行器作纵向平飞运动。

偏航控制，如图26所示。四旋翼飞行器为了克服反扭矩影响，4个旋翼螺旋桨中的两个顺时针转，两个逆时针转，且对角线上的两个旋翼螺旋桨转动方向相同。

图25俯仰运动控制示意图

图26偏航控制示意图

反扭矩的大小与旋翼螺旋桨转速有关，当4个旋翼螺旋桨转速不完全相同时，不平衡的反扭矩会引起机体转动。根据上面的原理，可以设计四旋翼飞行器的偏航控制，即同时提升一对同方向旋转的旋翼螺旋桨转速并降低另一对相反方向旋转的旋翼螺旋桨转速，保证转速增加的旋翼螺旋桨转动方向与四旋翼飞行器机身的转动方向相反。

2.3四旋翼飞行器各部分的工作原理2.3.1飞行姿态与升力关系

为便于进行四轴飞行器运动分析，建立刚体三轴坐标系，将四轴飞行器置于刚体坐标系其中如图27所示，飞行器运动过程中飞行姿态与各螺旋桨所产生升力之间的关系借助此坐标轴进行分析。
1. 飞行器绕y轴旋转α角度与升力之间的关系如图28所示，飞行器与y轴之间夹角α。主要通过左右螺旋桨产生升力差控制，其控制关系为

∑M=Ixα¨（21）

式中： M——力矩； Ix——转动惯量；α¨——飞行器与y轴夹角二阶导数，即角加速度。
lxF右-F左=Ixα¨（22）
式中： F右——右侧螺旋桨旋转产生升力； F左——左侧螺旋桨旋转产生升力;lx——螺旋桨与飞行器中心轴距。
α¨=lx（F右-F左）Ix（23）

图27飞行器坐标轴建立

图28飞行器绕y轴的角度α与F左、F右的关系

2. 飞行器绕x轴旋转β角度与升力之间的关系 如图29所示，飞行器与x轴夹角β主要通过前后两个螺旋桨所产生升力差值进行控制，其控制关系为

∑M=Iyβ¨(24)
ly(F前-F后)=Iyβ¨(25)
β¨=ly（F前-F后）Iy(26)

图29飞行器绕x轴的角度β与F前、F后的关系

3. 飞行器绕z轴旋转γ角度与升力之间的关系 如图210所示，飞行器绕z轴旋转γ角度，使螺旋桨产生扭矩及升力与旋转角度之间的关系为
∑M=Izγ¨(27)
M右 M左-M前-M后=Izγ¨(28)

图210飞行器绕z轴旋转角度γ与M前、M后、M左、M右之间的关系

γ¨=M右 M左-M前-M后Iz(29)
由于螺旋桨所产生的升力和力矩之间存在关系M=cF，所以上式可以表示为γ与升力之间的关系： 
γ¨=c右F右 c左F左-c前F前-c后F后Iz(210)
假定各个螺旋桨性能参数一致，则可以认为c前=c后=c左=c右=c，上式可以简化为
γ¨=c(F左 F右-F前-F后)Iz(211)

4. 飞行器飞行速度与螺旋桨升力之间的关系 根据牛顿第二定律： 
∑F=z¨ml(212)
F前 F后 F左 F右-mtg=z¨ml(213)
z¨=F前 F后 F左 F右-mtgml(214)
2.3.2飞行姿态的测量飞行姿态是一个真实飞行物体与参考坐标系之间的角度关系。如2.3.1节中分析使用到的α、β、γ角，这三个角度也称为欧拉角，对应Pitch、Yaw、Roll。常用姿态测量传感器有加速度传感器、角速度传感器、磁力传感器、气压传感器、超声波及GPS（Global Positioning System,全球定位系统）等。若需要获取比较精确姿态定位数据，则需要融合计算上述多个传感器测量数据。对于嵌入式平台应用，多种传感器数据融合计算对微处理器的运算能力要求较高。选择与实际开发平台相符合的姿态传感器尤为重要，本设计采用加速度与角速度测量飞行器姿态，两者测量数据互补融合计算姿态角，可以满足飞行姿态稳定性要求。2.3.3加速度传感器工作原理及角度测量加速度传感器是测量由物体重力加速度引起的加速度量。物理静止或运动过程中，受重力作用，会产生物体相对于三个坐标轴方向上的重力分量，通过对重力分量进行量化，运用三角函数可计算出物体相对于三个坐标轴的倾角。图211所示为加速度传感器测量时重力惯性矢量的三轴分量Rx、Ry、Rz。利用三角函数即可求出重力加速度与三个坐标轴夹角α、β、γ。

α=arccosRyR(215)
β=arccosRxR(216)
γ=arccosRzR(217)
R=R2x R2y R2z(218)
式中： α——重力矢量与y轴夹角； β——重力矢量与x轴夹角； γ——重力矢量与z轴夹角； Rx——加速度计测量重力加速度x轴分量； Ry——加速度计测量重力加速度y轴分量； Rz——加速度计测量重力加速度z轴分量； R——重力矢量。

图211加速度测量

2.3.4陀螺仪传感器工作原理及角度测量角速度传感器（陀螺仪）用来测量一段时间内角度变化速率。对两次测量时间差值进行积分可得到角度增量值。增量值可正可负，正值表示向角度增大方向旋转，负值表示向原角度减小反方向旋转，积分后与测量前初始角度求和可计算出当前角度。
θ=θ0 ∫t0ωdt(219)
式中： θ——旋转角度值； θ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 hbyhbx(220)
单独地使用磁力计测量航向角时，由于磁力计非水平放置，导致磁力计产生倾斜角误差，因此磁力计常与加速度计一起组成电子罗盘。当磁力计与惯性传感器组合使用进行姿态测试时，磁力计用来估算运动目标的航向角，用以校正陀螺仪漂移误差。当外界磁场发生突变时，磁力计对载体的航向角估算值失真。2.4姿态解算方法2.4.1互补滤波算法

互补滤波器作为一种频域特性滤波器，常用于融合来自不同传感器测量得到的数据。一般地，互补滤波器包含至少两种频率特性互补的输入信号。例如，对于陀螺仪和加速度计解算姿态这一双输入系统，两个输入量都能分别对姿态角进行解算，其中加速度计输入量包含高频噪声，应通过低通滤波器来滤除； 陀螺仪则包含低频噪声（积分漂移），应采用高频滤波器滤除。两者的频率特性互补，可用互补滤波思想进行姿态解算，终输出较准确信号，其工作原理如图212所示。

图212互补滤波姿态融合原理 

设运用加速度计和陀螺仪分别解算出的飞行器姿态角x的值为
x1=x u1x2=x u2(221)
其中，u1、u2分别为加速度计高频噪声和陀螺仪低频噪声。 取互补滤波器由低通滤波器FL(s)=1fs 1和高通滤波器FH(s)=fsfs 1两部分构成， f为滤波器常数，有
FL(s) FH(s)=1(222)
则姿态角x的估计值X^可表示为 
X^s=FLsX1s FHsX2s=Xs FLU1s FHU2s(223)
式(223)对应差分方程表达式为 
X^k=1-fX^k-1 X2k-X2k-1 fX1k(224)
式中，X2k为k时刻陀螺仪角速度积分值； X1k为加速度计解算的角度值。相对于单传感器方案，互补滤波可以避免加速度计精度和动态性能不足的问题，也能避免陀螺仪的漂移误差。由式(224)可见互补滤波器的结构简单、计算量小，其编程思想可以描述为
angle=1-A×angle_last gyro×dt A×acc(225) 
式中： angle当前融合而成的姿态角； angle_last为上一次姿态角融合结果值； gyro为当前陀螺仪测量的角速度； dt为积分时间； acc为加速度计解算出的姿态角。滤波器系数为A，一般取A小于0.1，表示每次解算结果中陀螺仪积分角度占比重较大，加速度计解算角度占比重较小，这也就相当于对陀螺仪信号高通滤波而对加速度计信号低通滤波。由此可以看出，虽然加速度计信号在每次姿态解算中所占权值较小，但随着运行时间的增加，加速度计信号实际上是在不断对陀螺仪积分进行缓慢的矫正，从而减小因陀螺仪积分时间增加而引起的漂移误差。图213是取互补滤波器系数A为0.02时对四轴飞行器俯仰角进行姿态解算的结果。

图213俯仰角互补滤波融合结果

从图213(a)中可以看出，只用加速度计进行姿态结算虽然没有静态误差，但其波形是含有一定量的噪声，尤其是在图中15~20s时，由于飞行器姿态改变，其外力加速度将使解算角度波动较大。图213(b)中的陀螺仪直接积分解算结果波形虽然平滑，即使在飞行器姿态改变时，其积分角度也没有波动，但随着时间的增加，将出现积分角度的漂移。图中仅进行了25s姿态解算，陀螺仪漂移误差就达到了将近2°。因此，进行姿态融合是十分必要的。在图213(c)中，互补滤波取得了较好的效果，不仅波形平滑，而且陀螺仪漂移误差也得到有效矫正。互补滤波姿态融合虽然计算简单，运算量小，也能够取得较理想的融合效果，但对于参数A选取却没有比较好的解决办法，一般是通过不断调试和反复修改进行确定。若A过大，则角度收敛较慢，动态性能降低； 若A过小，则角度波动较大，滤波效果降低。因此一般还需要运用模糊算法或其他自适应算法对参数值进行实时调整，以满足系统对静态和动态时的性能要求。2.4.2卡尔曼滤波算法卡尔曼滤波算法是由匈牙利数学家鲁道夫·卡尔曼于1960年提出的一种递归更新滤波算法。该算法为了描述整个计算更新的过程，提供了一组有效的递归推算方程组来估计过程的状态量，其间使估计均方误差小化。由于卡尔曼滤波器的概率原型解释及推导过程等更完整的讨论相对烦琐，不是本文研究重点，下面仅简单介绍离散卡尔曼滤波算法。设离散时间控制系统状态变量X∈Rn，可由以下离散随机差分方程描述： 
Xk=AX(k-1) BUk W(k)(226)
观测反馈量即系统输出方程可描述为
Zk=HX(k) V(k)(227)
其中，W(k)和V(k)均为随机信号，分别表示过程白噪声和观测白噪声且相互独立，服从正态分布： 
pw~N（0，Q）pv~N（0，R）(228)
式中，噪声协方差系数R与Q的取值，关系到终滤波的效果和响应速度。但两者相互制约，R取值越小，滤波响应和收敛越迅速； Q取值越小，抑制、滤除噪声的能力越强。卡尔曼滤波的基本公式为(1) 状态预测方程： 由系统状态变量k-1时刻的值X(k-1|k-1)和系统输入U(k)可以求出k时刻系统预测值X(k|k-1)为
X(k|k-1)=AXk-1|k-1 BU(k)(229)
(2) 协方差预测方程： 根据k-1时刻系统协方差矩阵Pk-1|k-1预测k时刻系统协方差矩阵Pk|k-1为
Pk|k-1=APk-1|k-1AT Q(230)
(3) 卡尔曼增益计算方程： 根据状态变量预测值和协方差矩阵预测值计算卡尔曼增益 Kg(k)为
Kg(k)=Pk|k-1HT/(HPk|k-1HT R)(231)
(4) 值更新方程： 由状态变量预测值和系统测量值计算k时刻状态变量值X(k|k)为
X(k|k)=Xk|k-1 Kg(k)(Zk-HXk|k-1)(232)
(5) 协方差更新方程： 更新k时刻协方差矩阵P(k|k)为
P(k|k)=(1-Kg(k)H)P(k|k-1)(233)
值和协方差更新方程都计算结束后，整个滤波过程进入下一轮循环，重复(1)~(5)步。递推算法

图214卡尔曼滤波器工作流程

的本质体现在估计过程，每次只需根据以前的测量值递归计算，就能得到当前时刻的状态估计。卡尔曼滤波器的工作流程如图214所示。以四轴飞行器俯仰角状态变量为例，进行卡尔曼融合姿态解算。令状态变量X为俯仰角度θ，系统状态方程为和测量方程分别为
Xk=θk=1×Xk-1 Ts×Wgyro Ts×wg(k)(234)
Zk=1×Xacc(k) wa(k)(235)
式中，Ts为系统采样周期，Wgyro为陀螺仪测量角速度，Xacc为加速度计计算的俯仰角度，wg、wa分别为陀螺仪和加速度计测量的噪声。取系统采样周期Ts=0.005s，系统噪声协方差矩阵Q=0.001，测量误差协方差矩阵R=0.5，滤波器初始条件X(0)=0、P(0)=1，根据卡尔曼滤波基本公式（式(229)~式(233)）设计俯仰角姿态融合算法，得到姿态解算结果如图215所示。

图215俯仰角卡尔曼滤波融合结果

在图215中使用的传感器数据与之前互补滤波使用的数据相同，可以看出，采用卡尔曼滤波进行姿态融合也可以得到较好的融合效果，其波形平滑度和静态漂移误差比互补滤波稍好。在滤波融合算法设计过程中，主要对协方差Q和R的取值进行设计，R取值越小，滤波响应和收敛越迅速； Q取值越小，抑制、滤除噪声的能力越强。因此，其具体取值也需要反复实际调试进行权衡确定。
2.4.3DMP姿态数据获取运用互补滤波与卡尔曼滤波思想进行姿态融合的过程归根结底都是利用加速度计解算出的姿态角去修正陀螺仪积分的漂移误差。这两种方法在姿态融合过程中姿态角的表示形式都是欧拉角表示。用欧拉角进行姿态解算在大角度计算时会出现万向节锁（角度为90°时加速度计进行姿态解算的反三角函数无解），为了避免该问题，可采用四元数来解算姿态。四元数姿态解算流程如下。(1) 初始化四元数： 设当前的坐标系为机体坐标系，则四元数列向量： 
q=［q0q1q2q3］T=［1000］T(236)
(2） 获取角速度、加速度： 读取MPU6050三轴加速度计和三轴陀螺仪的信号并经过IIR低通滤波器滤波滤除振动噪声，得到重力加速度分量accx、accy、accz和角速度分量ωx、ωy、ωz。(3) 将加速度计测量值accx、accy、accz转化为三维的单位向量(归一化)： 
ax=accxacc2x acc2y acc2zay=accyacc2x acc2y acc2zaz=acczacc2x acc2y acc2z(237)
(4) 用四元数表示三轴的重力分量Vx、Vy、Vz： 
Vx=2（q1q3-q0q2）Vy=2（q1q0-q3q2）Vz=q20-q21-q22 q23(238)
式中Vx、Vy、Vz： 即重力单位向量在机体坐标系中的分量。 (5) 求四元数所求重力分量与加速度计测量值的误差值： 
ex=ay×Vz-az×Vyey=az×Vx-ax×Vzez=ax×Vy-ay×Vx(239)
在机体坐标系中，加速度计测量的重力加速度分量为accx、accy、accz； 陀螺仪积分后推算得到的重力向量是Vx、Vy、Vz。两者之间的误差，即陀螺仪积分误差。此处，向量间的误差ex、ey、ez是用向量积(外积、叉乘)表示，该误差向量仍位于机体坐标系中。
(6) 利用所得的误差修正陀螺仪的测量值： 
exint=e^xint ki×exω·x=ωx kp×ex exint(240)
eyint=e^yint ki×eyω·y=ωy kp×ey eyint(241)
ezint=e^zint ki×ezω·z=ωz kp×ez ezint(242)
式中，参数ki、kp用以控制加速度计修正陀螺仪误差的速度。(7) 利用修正后的陀螺仪值ω·x 、ω·y、ω·z更新四元数： 
q0=q^0 dt2(-q1ω·x-q2ω·y-q3ω·z)q1=q^1 dt2(q0ω·x q2ω·z-q3ω·y)q2=q^2 dt2(q0ω·y-q1ω·z q3ω·x)q3=q^3 dt2(q0ω·z q1ω·y-q2ω·x)(243)
(8) 将得到更新后的四元数规范化： 
q0=q^0q20 q21 q22 q23q1=q^1q20 q21 q22 q23q2=q^2q20 q21 q22 q23q3=q^3q20 q21 q22 q23(244)
公式（240）~（244）中e^xint,e^yint,e^zint,q^0~q^4表示更新后的值。(9) 得到新四元数后即完成了一次四元数法姿态融合的运算。将新四元数作为下一次四元数运算的初始四元数，再从步骤(1)开始下一次的四元数运算。为了直观表示飞行器的姿态，可将新四元数转化成为三个欧拉角： 
=arctan2(q2q3 q0q1)q20-q21-q22 q23(245)
θ=arcsin(-2(q1q3-q0q2))(246)
ψ=arctan2(q2q1 q0q3)q20 q21 q22 q23(247)
总结以上9个步骤，四元数算法流程如图216所示。
通过四元数姿态解算的流程可以看出，其主要思想还是利用加速度计对陀螺仪进行修正，其修正的快慢程度由参数kp和ki进行控制。由于使用该方法步骤较为烦琐，涉及中间变量转换较多，且计算量较大，占用内存也较大，使用起来很不方便。为解决该问题，运动控制传感器MPU6050提供了DMP内部四元数解算功能，可以直接输出四元数数据，从而省略了烦琐的计算步骤，给设计带来了极大便利。运动处理传感器MPU6050除了提供三轴陀螺仪和三轴加速度计传感器的16位ADC（AnalogtoDigital Converter,模/数转换器）信号采集功能之外，还集成了数字低通滤波器和数字运动处理器DMP（Digital Motion Processor），可以直接输出经低通滤波处理和四元数姿态解算后的四元数数据。将该四元数转换为欧拉角，可以得到准确的俯仰角和横滚角。配置MPU6050内部DMP功能的流程如图217所示。

图216四元数法姿态解算流程

图217MPU6050内部DMP功能配置流程

按如图217所示的流程对DMP功能进行配置后，即可调用库函数直接读取四元数姿态角数据。由于没有融合磁力计数据，因此偏航角的计算仍然是由陀螺仪积分得到。对偏航角数据的融合，也可采用之前介绍的互补滤波、卡尔曼滤波及四元数法，其具体实现过程与用加速度计修正陀螺仪误差类似，在此不再赘述。对于同一组飞行姿态数据，图218所示展示了互补滤波姿态融合、卡尔曼滤波姿态融合以及MPU6050内部DMP姿态解算俯仰角波形对比结果。

图218不同解算方法姿态解算结果对比图

可以看出，三种姿态解算方法都能取得较好的解算效果，其中DMP内部解算在飞行器静止时稳态噪声小，在姿态角动态改变时，互补滤波和卡尔曼滤波姿态解算受加速度计影响更大，而DMP解算则收敛相对较慢。总的来说，DMP解算姿态数据更平稳，动态性能也比较理想。从设计难度上来说，互补滤波和卡尔曼滤波姿态解算都需要根据实际调试反复修改滤波器参数才能达到较好的解算效果，而DMP解算直接输出四元数数据，只需要将其转换为欧拉角即可用于姿态控制，大大简化了四轴飞行器姿态解算难度。综合考虑，本书设计的四轴飞行器姿态解算方法采用MPU6050内部姿态解算。2.5PID控制算法2.5.1PID概述
PID即比例、积分、微分控制器,作为早实用化的控制器已有近百年历史，也是现在应用为广泛的工业控制器,其基本结构如图219所示。

图219PID控制器基本结构

如图219所示，PID控制器由比例单元、积分单元和微分单元组成，其输入是期望值r(t)和输出测量值y(t)之间的误差e(t)。早期的PID控制器通过硬件实现称为模拟PID，其基本公式为 
u(t)=Kpet 1Ti∫t0etdt Tde(t)dt(248)
式中，Kp为控制器比例系数，Ti为积分系数，Td为微分系数。对应传递函数为