毕业设计（论文）-基于STM32的四旋翼飞行控制系统毕业设计.doc

西华大学毕业设计说明书 目 录1前言11.1背景与意义11.2国内外研究现状11.3论文主要工作22总体方案设计32.1方案比较32.2方案论证与选择33飞行器原理与结构53.1飞行器原理53.2飞行器结构64单元模块设计84.1各单元模块功能介绍及电路设计84.1.1电源84.1.2 STM32F407最小系统94.1.3 下载电路114.1.4 飞控姿态模块114.1.5 无刷电机连接电路124.1.6 串口接口电路124.2特殊器件的介绍124.2.1 无线数传模块124.2.2 飞控姿态模块135软件设计165.1软件设计原理及设计所用工具165.2主要软件设计流程框图及说明175.2.1串口中断流程图175.2.2外部中断流程图185.2.3主程序流程图186系统调试206.1 通信系统206.2 姿态传感器调试216.2.1 传感器数据分析与处理216.2.2 姿态解算236.2.3 数据中断286.3 PID调试306.3.1 PID姿态控制306.3.2 飞控系统PID调试337系统功能、指标参数367.1系统能实现的功能367.2系统指标参数368结论388.1 回顾388.2 展望389总结与体会3910谢辞4011参考文献41附录：421.硬件电路图422.PCB图433.部分程序444.外文翻译46 1前言1.1背景与意义近年来得益于现代控制理论与电子控制技术的发展，四轴飞行器得到了广泛的关注，在民用与工业领域，具有广泛的应用前景。甚至无人机在战争中得到广泛的应用。当下无人机发展火热，其中以四旋翼飞行器的发展最为突出。四旋翼飞行器其具有以下特点：(1)体积小巧，可以工作在恶劣的，危害人类健康和生命的环境中，最大限度地减少人员伤亡，飞行器可以全天工作无需休息，工作效率高。(2)支持配备高端电子产品，多种外设相连接，如照相机、机械臂等，可以实现一些娱乐功能。例如在高空电力线巡检中，无人机能在工作人员的操控下进行工作，可以代替人工对巡检对象实施接近检测，减少工人的劳动强度。也可以携带传感仪器、摄像机等，对巡检对象进行数据收集、分析与存储，这进一步提高巡检的工作效率和巡检精度。在军事上，在局部小规模对战的时候，一些普通的侦察机，可能受到敌方打击而造成不必要的机体人员伤亡，无人机则可以很好地起到替代作用。利用四旋翼飞行器作为侦察机，具有振动小、噪声小、可靠性高、成本低、反侦察能力强、自我销毁等优势。因此无人机的军事价值不可估量。四旋翼飞行器还有着更为广阔的前景等待着开发。比如可以通过为飞行器的添加更加智能的算法实现人机互动，让飞行器帮人取物件等。尽管目前四旋翼飞行器已经在许多领域得到运用，但总体而言依旧处于初步发展阶段。1.2国内外研究现状国际上比较知名的飞行器公司有中国大疆创新和美国的3D Robotics。作为全球无人机领域的领头羊，这两家公司在无人机技术发展速度可以用迅猛来形容。比如大疆公司最新的精灵4无人机，实物图如图1.1所示。图1.1 精灵4实物图精灵4具有以下功能：1. 可感知前方障碍物并自动绕行。 2. 通过视觉识别自动跟拍移动物体。 3. 点击相机画面，即可向指点方向自主飞行。 4. 智能返航，感知障碍物后可自动提升飞行高度。 5. 最大飞行时间28分钟，最大可控距离约5公里。 6. 最高速度提升至20m/s (72km/h)。 7. 一体化云台设计，提升了飞行和影像的稳定性。 8. 利用全新的视觉定位技术，可实现室内外精准定位。又比如：3D Robotics公司最新出品的PIXHAWK飞控，拥有出色飞行稳定性，搭载双传感器系统和冗余电源输入并且可以扩展两组GPS系统，确保飞行失误降到最低。这两家公司占领了全球大部分无人机市场，并且由于技术的高门槛和垄断，其他无人机公司已经被远远的甩在了后面。大部分的无人机公司的技术仅仅停留在稳定飞行、简单航拍和户外GPS定位阶段。1.3论文主要工作无人机作为当今电子产业里一个冉冉升起的新星，具有广阔的市场和发展前景。作为当代大学生，不仅要顺应时代的潮流，更要有作为时代弄潮儿的信心与勇气。四旋翼无人机涉及知识面很广泛，其中核心知识与本专业契合度相当高。比如，一颗功能强劲的微机芯片就可以打造一个功能完备的飞行控制系统。而这恰恰是微机原理和单片机的运用。主流无人机的控制系统离不开自动控制原理。并且目前无人机在电力行业的应用力度很大，综上，毕业设计选择了基于STM32 ARM单片机的四旋翼飞控系统。 受限于本人知识水平，本文解决以下问题：第一章节：四旋翼飞行器研究背景与意义，国内外发展现状。第二章节：就四旋翼飞行器方案讨论与选择，选择了基于STM32 ARM单片机的四旋翼飞行控制系统。第三章节：简单叙述了飞行器飞行原理，以及机械结构。第四章节：飞行控制系统硬件设计，其中包括单片机最小系统，各类模块接口等。第五章节：程序流程图，对程序流程进行简单说明。第六章节：飞行器控制系统设计，包括传感器数据进行分析和处理，DMP姿态解算方式，配置中断实时更新当前飞行姿态数据。四旋翼飞行器的PID调试策略。第七章节：上位机与飞行器之间的通信系统，PID调试过程。 2总体方案设计 2.1方案比较方案一：基于意大利开源硬件Arduino Nano作为数据处理，姿态结算的飞行控制系统。其飞行控制系统结构如图2.1所示。图2.1 Arduino飞控系统结构图方案二：采用意法半导体的STM32F407VET6作为飞行器的主控芯片。其飞行控制系统结构如图2.2所示。图2.2 基于STM32飞控系统结构图2.2方案论证与选择方案一：Arduino Nano是基于Atmega328P AVR单片机的开源硬件，具有两个外部中断口，可以输出六路PWM波，兼备IIC,UART,SPI通信功能，总的来说，其具有丰富的片上资源和优秀的性能。对它编程使用的是Arduino C,这种C语言类似于标准C,但又针对Arduino系统做了大量的简化工作，提供了许多函数和库文件，但是通用性不高。Arduino虽然是已开源的飞行控制系统，命令是依靠无线遥控器发出的，这会额外增加遥控器的费用。另外Arduino通信协议未知，这并不利于增添代码后的调试工作。方案二：意法半导体的STM32F407VET6为CORTEX-M4内核，属于32位ARM微控制器，常用的编程软件是KEIL和IAR，编程语言可以是汇编，标准C语言，C+等，使用灵活方便。友好的编译方式大大提高了代码的通用性和可移植性。STM32F407VET6具有卓越的性能，并不输于AVR的Atmega328P。这里采用上位机传输命令给飞行控制系统，也简化了系统调试方式。最后考虑到资金、性价比和使用的难易程度选择方案二，基于STM32F407VET6 单片机的飞行控制系统。3飞行器原理与结构3.1飞行器原理四轴飞行器具有两种不同的飞行模式： X 型与十字型。X型飞行方式的四旋翼飞行器姿态改变的方向与机身成一个45度角，十字型飞行方式四旋翼飞行器姿态改变方向与飞行器机身相同。因为采用X型飞行方式的飞行器具有更好的控制灵敏度与稳定性，所以选择了X型的飞行方式。两种工作模式如图3.1所示。YXYX图3.1 X 型（左）与十字型（右）飞行方式图四旋翼飞行器的四个螺旋桨都是电机直连的结构，通过改变电机转速获得旋转机身的力，从而调整自身姿态。在飞行器飞行过程中，螺旋桨会产生两个力，一个是升力，一个是与螺旋桨转向相反的反扭矩。反扭矩会使飞行器沿着螺旋桨旋转的方向自旋，如果不抵消反扭矩会让飞行器一直自转，这会影响飞行器的飞行。四旋翼飞行器通过分配四只螺旋桨的转向来抵消各个螺旋桨产生的反扭矩。以X 型飞行方式为例，按顺时针方向为每个电机编号，右上角电机为1号电机，依次编号1、2、3、4。并将1、2号螺旋桨所在的轴向方向定义为X轴方向，即机头。 3、4号螺旋桨所在的轴向方向定义为Y轴方向。为了抵消螺旋桨的反扭矩，1、3号螺旋桨需要顺时针转动，2、4号螺旋桨需要逆时针转动，即对角线上的螺旋角旋转方向相同，以此抵消相互之间的反扭矩。四旋翼飞行器的飞行方向与速度都是由飞行器的倾角决定的，并且飞行器倾斜的角度越大，飞行速度也就越快。通过调节各个电机的转速可以达到控制飞行器姿态、速度、甚至是飞行路径的效果。其中，四旋翼飞行器飞行的姿态主要是高度、俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw），可以继续细分为：上升、下降、前倾、后倾、左倾、右倾、左旋、右旋。(1) 高度：使四个螺旋桨转速相同，当其同时加速时，螺旋桨升力变大，当升力大于飞行器重力时，飞行器拥有向上运动的加速度，飞行器上升；当四个螺旋桨同时减速时，螺旋桨产生的升力变小，当升力小于飞行器重力时，飞行器拥有向下运动的加速度，飞行器下降。 (2) Pitch：就是绕着Y轴方向旋转，所进行的控制为1，2号电机转述同等减小，3，4号电机转述同等增大，飞行器往前倾；反之，会后倾。 (3)Roll：与俯仰控制相似，横滚就是绕着X轴方向旋转，1,4号电机转述同等减小，2,3机转述同等增加，产生右倾；反之，会左倾。 (4)Yaw：同理可得，就是飞行器绕着Z轴旋转。当1、3号电机转述同等减小，其反扭矩和升力减小，并且2、4号电机转述同等增加，其反扭矩和升力增加，由于反扭矩出现不平衡，会使飞行器向右转，反之，会使飞行器向左转。3.2飞行器结构采用的机架型号为 F360，轴距360mm。螺旋桨型号是1047型。电机采用的是朗宇X2212，980KV无刷电机，即每加1V的电压，电机每分钟980转，电机转速大约是10878转分。通常四旋翼飞行器配2200mah的电池。电调为好盈天行者30A的电子调速器，整个飞行系统用锂电池供电。电机实物图如图3.2所示，电子调速器实物图如图3.3所示。图3.2 朗宇电机实物图图3.3 电子调速器实物图飞行器组装完成后如图3.4所示图3.4 飞行器实物图4单元模块设计 4.1各单元模块功能介绍及电路设计飞行控制系统的硬件结构遵从于方案二的结构框图，即图2.2。以下就重要的单元模块做介绍。4.1.1电源LM1117为低压差电压调节器。其压差输出为1.2V时，负载电流为800mA。它与国家半导体的工业标准器件LM317有相同的管脚排列。LM1117有可调电压的版本，通过2个外部电阻可实现1.2513.8V输出电压范围。另外还有5个固定电压输出（1.8V、2.5V、2.85V、3.3V和5V）的型号。因为设计的飞行器的主控芯片是3.3V的。所以选取固定输出为3.3V的LM1117。LM1117提供电流限制和热保护。输出电压的精度在1%以内。LM1117系列具有LLP、TO-263、SOT-223、TO-220和TO-252、D-PAK等多种封装。飞行器为了提高续航能力，要尽量减轻自身重量，所以这里选取的是SOT-223封装。在电路设计的输出端需要并联一个至少10uF的钽电容来改善瞬态响应和稳定性。LM1117电气特性如表4.1所示。表4.1 电气特性表-LM1117符号参数测试条件最小值典型值最大值单位VREF 基准电压LM1117-ADJIOUT=10mA, VIN-VOUT=2V, TJ=25C10mAIOUT800mA, 1.4VVIN-VOUT10V1.2381.2251.2501.2501.2621.270VVVOUT输出电压LM1117-1.8IOUT=10mA, VIN=3.8V, TJ=25C0IOUT800mA, 3.2VVIN10V1.7821.7461.8001.8001.8181.854VVLM1117-2.5IOUT=10mA, VIN=4.5V, TJ=25C0IOUT800mA, 3.9VVIN10V2.4752.4502.5002.5002.5252.550VVLM1117-2.85IOUT=10mA, VIN=4.85V, TJ=25C0IOUT800mA, 4.25VVIN10V0IOUT500mA, VIN=4.10V2.8202.7902.7902.8502.8502.8502.8802.9102.910VVVLM1117-3.3IOUT=10mA, VIN=5V TJ=25C0IOUT800mA, 4.75VVIN10V3.2673.2353.3003.3003.3333.365VVVOUT压差LM1117-3.3IOUT=0mA, 4.75VVIN15V16mV 电源模块的原理图如图4.1所示。图里的5V电源来源于电子调速器的BEC降压系统。图4.1 电源模块电路图4.1.2 STM32F407最小系统STM32F407VET6是意法半导体基于CORTEX-M4内核的芯片，STM32F407拥有的资源包括：集成FPU和DSP指令，并具有192KBSRAM、1024KB FLASH、12个16位定时器、2个32位定时器、2个DMA控制器（共16个通道）、3个SPI、2个全双工I2S、3个IIC、6个串口、2个USB（支持 HOST /SLAVE）、2个CAN、3个 12位ADC、2个12位DAC、1个RTC（带日历功能）、1个SDIO 接口、1个FSMC接口、1个10/100M以太网MAC控制器、1个摄像头接口、1个硬件随机数生成器、以及112个通用IO口等。该芯片的配置十分强悍，具有卓越的性能。相对STM32F1来说，许多功能进行了重大改进。STM32最小系统电路图如图4.2所示。图4.2 STM32最小系统电路图STM32上电复位后默认使用内部晶振（精度8MHz左右），为了让飞行控制板稳定工作，这里选择了外接8MHz的晶振，在程序里切换为使用外部8MHz晶振，并通过芯片自带的锁相环PLL倍频到168MHz。所以需要修改系统时钟配置System_stm32f4xx.c文件，把PLL第一级分频系数M修改为8，这样才能让主时钟频率达到168MHz。在STM32F40xx里，可以通过BOOT1:0引脚选择三种不同启动模式。一般我们选用从主闪存存储器启动。启动模式如表4.2所示。表4.2 启动模式-STM32启动模式选择引脚启动模式说明BOOT1BOOT0X0主闪存存储器主闪存存储器被选为启动区域01系统存储器系统存储器被选为启动区域11内置SRAM内置SRAM被选为启动区域根据选定的启动模式，主闪存存储器、系统存储器或SRAM可以按照以下方式访问： 从主闪存存储器启动：主闪存存储器被映射到启动空间(0x0000 0000)，但仍然能够在它原有的地址(0x0800 0000)访问它，即闪存存储器的内容可以在两个地址区域访问， 0x00000000或0x0800 0000。 从系统存储器启动：系统存储器被映射到启动空间(0x0000 0000)，但仍然能够在它原有的地址(互联型产品原有地址为0x1FFF B000，其它产品原有地址为0x1FFF F000)访问它。 从内置SRAM启动：只能在0x2000 0000开始的地址区访问SRAM。注意：当从内置SRAM启动，在应用程序的初始化代码中，必须使用NVIC的异常表和偏移寄存器，从新映射向量表之SRAM中。 4.1.3 下载电路如果采用用串口烧录程序，必须先配置BOOT0为1，BOOT1为0，然后按复位键，最后再通过程序下载代码，下载完以后将BOOT0置GND，复位后运行代码。这里采用了jlink（SWD）下载方式，需要接：jlink的第1脚（VDD）、第7脚（TMS/SWDIO对应STM32的PA13）、第9脚（TCK/SWCLK对应STM32的PA14）、第4.6.8.10.12.14.16.18.20中的任意一个脚（地脚）、第15脚（RESET对应STM32的NRST）。SW模式下载电路如图4.3所示。图4.3 SW模式下载电路图4.1.4 飞控姿态模块性能良好的飞控传感器是飞行器稳定飞行重要保证，GY-86模块上集成了陀螺仪、加速度计、磁力计和气压计，很适合飞行器上使用。GY-86模块与飞行控制系统连接电路图如图4.4所示。图4.4 GY-86模块连接电路图4.1.5 无刷电机连接电路无刷电机依靠PWM波来驱动，PWM波通过电子调速器输送到无刷电机，电子调速器有3个接口，VCC，GND和Mx。在电路板上需要给出连接口。如图4.5所示。图4.5 无刷电机接口电路图4.1.6 串口接口电路如图4.6所示，将单片机上的串口一一引出，主要作用是连接无线数传模块，用于收发上位机命令。其余串口接口可用来外扩模块，如超声波模块和GPS。图4.6 串口接口电路图4.2特殊器件的介绍4.2.1 无线数传模块CC1101无线数传模块，体积小巧，尺寸仅为1.45*2.8CM，操作简单，工作频段为433MHZ，穿透能力强，可直接和51、AVR、STM8、STM32、MSP430、PIC等单片机的串口相连接。实物图如图4.7所示。 图4.7 无线数传模块图CC1101无线数传模块和STM32单片机串口连接后，串口就可以实现远距离无线收发数据，这样的好处就是减少了调试难度，因为如果不使用无线数传模块的话，就只有另购NRF24L01通信模块，这会增添代码量，加大调试难度。4.2.2 飞控姿态模块选择GY-86模块的原因是其集成度很高，实物图如图4.8所示。图4.8 GY-86模块实物图模块上的MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。MPU6050内部整合了3轴陀螺仪和3轴加速度传感器，并且含有一个第二IIC接口，可用于连接外部传感器，并利用数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主IIC接口，向应用端输出完整的9轴融合演算数据。有了DMP，我们可以使用InvenSense公司提供的运动处理资料库，实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。MPU6050芯片实物图如图4.9所示。 图4.9 MPU6050芯片实物图MPU6050的特点包括： 以数字形式输出6轴或9轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据（需DMP支持）。 具有131 LSBs/sec敏感度与全格感测范围为250、500、1000 与2000/sec的3轴角速度感测器(陀螺仪)。 集成可程序控制，范围为2g、4g、8g 和16g 的3轴加速度传感器。 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移。 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少MCU复杂的融合演算数据、感测器同步化、姿势感应等的负荷。 内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求。 自带一个数字温度传感器。 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与GPS。 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G中断、零动作感应、触击感应、摇动感应功能。 VDD供电电压为2.5V5%、3.0V5%、3.3V5%；VLOGIC可低至1.8V5%。 陀螺仪工作电流：5mA，陀螺仪待机电流：5uA；加速器工作电流：500uA，加速器省电模式电流：40uA10Hz。 自带1024字节FIFO，有助于降低系统功耗。 高达400Khz的IIC通信接口。 超小封装尺寸：4x4x0.9mm（QFN）。MPU6050芯片工作电路图如图4.10所示。图4.10 MPU6050芯片工作电路图MPU6050主要用于采集飞行器飞行时X,Y,Z三个轴上的加速度和偏转角速度。具体的讲，陀螺仪的测量物理量是偏转、倾斜时的转动角速度和坐标轴上的线加速度。MPU6050内部通过MEMS工艺做出了一个参考坐标系,当芯片随着飞行器动作时，芯片的坐标相对参考坐标发生旋转时，芯片会读出这个差异。经过芯片内部处理后，会输出一个绕坐标轴旋转的角速度。MPU6050芯片坐标轴分布如图4.11所示。图4.11 MPU6050芯片坐标轴分布图关于加速度的测量其实是当物体在加速过程中作用在物体上的力，这里需要注意的是在地球上任何物体都会受到地球给的重力，芯片也不例外，所以X,Y,Z坐标轴上测得的加速度包含了重力在三个坐标轴上的重力加速度分量。5软件设计 5.1软件设计原理及设计所用工具上位机采用Lab windows制作。Lab windows是美国国家仪器公司推出的交互式C语言开发平台。可编写检测系统、数据采集系统等应用软件。用于数据采集分析和显示。软件界面如图5.1所示。图5.1 Lab Windows界面图STM32F407芯片使用C语言软件开发系统KEIL开发。KEIL提供了包括C编译器和功能强大的仿真调试等在内的完整开发方案。操作界面如图5.2所示。图5.2 KEIL操作界面图5.2主要软件设计流程框图及说明5.2.1串口中断流程图无线数传模块连接在串口上，当串口有中断发生时，说明接收到上位机命令，需要处理串口数据。串口中断流程图如图5.3所示。 进入串口中断接收中断数据处理数据串口标志位置1结束图5.3 串口中断流程图5.2.2外部中断流程图GY-86飞控姿态模块的数据就绪中断连接在单片机外部中断的管脚上，当STM32F407接收到外部中断时，说明一组飞行器姿态数据已经就绪，需要进行姿态解算。外部中断流程图如图5.4所示。进入外部中断读取模块数据姿态解算DMP标志位置1结束图5.4 外部中断流程图5.2.3主程序流程图在主程序里，进行一系列初始化后就进入while大循环，在循环里主要做了通信数据处理，这是为了防止飞行器飞行角度过大导致飞行器侧翻问题，其次就是调整PWM波占空比调节无刷电机转速。主程序流程图如图5.5所示。 开始初始化飞控板硬件设置通信初始化中断初始化驱动初始化飞控系统初始化初始化系统参数初始化传感器通信数据处理串级PID控制调节电机PWM波输出图5.5 主程序流程图根据主程序流程图底层驱动主要使用到的部分如下： (1) USART1：外接扩展的传感器模块。(2) USART2：用于调试程序、发送和接收上位机命令的通信。 (3) IIC：用于读取 GY-86 模块的值，此处使用GPIO模拟IIC。 (4) GPIO：用于控制LED，通过不同的闪烁以及亮灭提示飞行器的运行状态。(5) SYSTICK：滴答定时器，用于系统的精确定时以及延时。 (6) TIM定时器的PWM通道：使用TIM3和TIM4的PWM功能输出4路不同占空比的PWM波给电子调速器从而控制无刷电机的转动。(7) 外部中断：利用单片机的外部中断，可以及时的获取当前的飞行姿态。(8) 独立看门狗：溢出时间为2s，防止系统死机停止工作。6系统调试6.1 通信系统飞行器不同于一般的控制系统，由于带有四个螺旋桨，并且电机转速很高，近距离调试意味着伴随着安全隐患。需要构建一个通信系统对飞行器进行远距离调试。用上位机对飞行器实时控制，上位机发送到飞控系统的数据很多，比如想要加减油门，让飞行器往哪飞，在调试的时候需要随时更改PID参数，这也是上位机要发送命令。对于飞行控制系统，也需要发送数据到上位机，比如，当前四个电机的转速，当前飞行器的姿态，以及正在使用的PID参数。为保证通信的准确性，及时性，对于数据传输编写一个通信协议很有必要。上位机如图6.1所示。图6.1 上位机界面图因为使用的PID参数有小数，所以协议传递的PID参数都是先放大了1000倍在进行传输的。为了保证每种类型的数据都正确无误，在数据末尾增加了和校验SUM，比如飞控系统在接收到RCDATA后会在单片机里求和，并将结果同上位机发送来的SUM做对比，只有当两者相等时，数据才会真正保存到飞控系统。并且在接收到重要的数据时，飞控系统还会返回一模一样的数据给上位机，表示已经正确的收到了这些重要数据，大大增加了飞控系统的安全性。6.2 姿态传感器调试6.2.1 传感器数据分析与处理四旋翼飞行器保持平稳状态的关键是正确对姿态传感器的数据进行处理。首先，通过操作MPU6050的寄存器可以获得飞行时的线加速度和转动角速度原始值。加速度计测量值存储寄存器如表6.1所示。表6.1 加速度计测量值存储寄存器Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit03B59ACCEL_XOUT15:83C60ACCEL_XOUT7:03D61ACCEL_YOUT15:83E62ACCEL_YOUT7:03F63ACCEL_ZOUT15:84064ACCEL_ZOUT7:0每个16位加速度计测量值的满量程定义在MPU6050的CCEL_FS寄存器（寄存器28）。 对于每个满量程的设置，加速度计测量值的灵敏度最低分辨率（LSB）如下表6.2所示：表6.2 加速度计灵敏度最低分辨率AFS_SEL Full Scale RangeLSB Sensitivity02g16384 LSB/g14g8192 LSB/g28g4096 LSB/g316g2048 LSB/g陀螺仪测量值存储寄存器如表6.3所示。表6.3 陀螺仪测量值存储寄存器Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit04367GYRO_XOUT15:84468GYRO_XOUT7:04569GYRO_YOUT15:84670GYRO_YOUT7:04771GYRO_ZOUT15:84872GYRO_ZOUT7:0每个16位陀螺仪测量值的满量程定义在MPU6050的FS_SEL寄存器（寄存器 27）。对于每个满量程的设置，陀螺仪测量值的灵敏度最低分辨率（LSB）如下表6.4所示。表6.4 陀螺仪灵敏度最低分辨率FS_SELFull Scale RangeLSB Sensitivity0250/s131 LSB/s1500/s65.5 LSB/s21000/s32.8 LSB/s32000/s16.4 LSB/sMPU6050的数据是为int16型，数据范围是-32768到+32767。即正数是0-32767对应16进制即是0-7FFF。负数是-32768- -1对应16进制即是8000-FFFF。就MPU6050而言，比如数据为FFFF，原码是8001，即-1。数据为FF00，原码是8100，即-256先让单片机接收MPU6050原始数据，并把数据除以对应的分辨率，可将数据加速度值单位转换为g，角速度单位转换为/sec。采集数据并取100次平均值后，把ACCEL_OUT放大100倍，再将全部数据发送到串口助手6次，这样便于观察和分析。具体数值如表6.5和6.6所示。表6.5 静止时MPU6050原始数据状态GYRO_XGYRO_YGYRO_ZACCEL_XACCEL_YACCEL_ZMPU6050静止Z轴垂直向上1001010010010100100101001101099100109910010100MPU6050静止X轴垂直向下100-10003110-10004100-10004110-10004100-10003100-10004MPU6050静止Y轴垂直向下-100-2-99-3-100-2-100-3-100-2-100-4-100-1-100-4-100-2-99-4-100-1-99-4分析表格可知，当静止时：X、Y、Z三个轴上的角速度因为静止几乎为0/sec。而加速度因为重力的原因，竖直向上的轴为100（放大了100倍，方便观察），即1g，竖直向下的坐标轴为-1g，而水平的坐标轴加速度几乎为0g。表6.6 运动时MPU6050原始数据状态GYRO_XGYRO_YGYRO_ZACCEL_XACCEL_YACCEL_Z绕Y轴顺时针转动-1350-37090-1280-42088-1260-47085-1210-50083绕Y轴逆时针转动-1-21022096-1-20025095-1-20029094-1-20033092绕X轴顺时针转动90002296800024956000269450002795绕X轴逆时针转动-15011-1596-14010-1897-13011-2096-13011-2295绕Z轴顺时针转动-11170198-11160097-11130-199-11141298绕Z轴逆时针转动-43-320199-42-2900984-3-2700100-20-270096分析表格可知，当传感器绕Y轴转动时：X、Z轴上的角速度因为静止几乎为0/sec。Y轴上，瞬时针转动角速度为正，逆时针转动角速度为负。而加速度因为转动的原因，Y轴上的加速度绝对值在减小，而X轴上的加速度绝对值在增大，而水平上的Y坐标轴加速度几乎为0g。当传感器绕X轴转动时情况和绕Y轴转动类似。当传感器绕Z轴转动时, X、Y轴上的角速度因为静止几乎为0/sec。Z轴上，瞬时针转动角速度为正，逆时针转动角速度为负。因为X,Y轴都在水平方向上转动，Z轴一直竖直向上，所以X,Y,Z轴上的加速度都几乎不变，并且X,Y轴上的加速度几乎为0。6.2.2 姿态解算在3.飞行器原理里分析到，四旋翼飞行器的飞行方向与速度都是由飞行器的倾角决定的，并且飞行器倾斜的角度越大，飞行速度也就越快。所以我们期望得到的是姿态数据，也就是欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。有了这三个角，我们就可以得到当前四轴的姿态，这才是我们想要的结果。而不是X,Y,Z三个轴的加速度或者角速度。那能否使用加速度来计算姿态角呢？加速度坐标模型如图6.2所示。图6.2 加速度坐标模型图要知道当飞行器只受重力场时，x2+y2+z2 =1g, 向量R是加速度计测量（它可以是引力场或惯性力）的力矢量。Rx，Ry，Rz，分别R在X，Y，Z轴上的矢量投影。有下面的关系式：R2 = RX2 + RY2 + RZ2。我们感兴趣的是向量R 和 X、Y、Z轴之间的角度，将它们定义为 Axr、Ayr、Azr。模型图如图6.3所示。可以看到由R 和Rx 组成的直角三角形。 图6.3 重力分量坐标模型图这里我们可以很容易推导出下面的公式，得到重力向量与各个轴间的夹角，即姿态角。COS（Axr）= RX / R 依此类推：COS（Ayr）= RY / RCOS（Azr）= RZ / R 当使用arccos()反余弦 :Axr = arccos(RX / R)Ayr = arccos(RY / R)Azr = arccos(RZ / R)具体而言，就是：cosX = cos(Axr) = Rx / R cosY = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R这三个公式通常被称为方向余弦。可以很容易地证明：SQRT（cosX 2 + COSY 2 + cosZ 2）= 1这个特点可以避免记录R矢量的模（长度）。很多时候，只有惯性矢量方向才是真正有用的，对矢量进行规范化对简化程序运算很有意义。比如，当水平放置MPU6050,只有Z轴感受到重力向量，它将输出1g。对应的ADC值就是16384 (2g的量程)。此时,R就是重力向量,Rx=0. Ry=0. Rz = R =1g.满足 R 2 = RX 2 + RY 2 + RZ 2 得到重力向量与各个轴的夹角。Axr = arccos(RX / R) = 90度。Ayr = arccos(RY / R) = 90度。Azr = arccos(RZ / R) = 0 度。这里似乎只需要加速度就可以获得飞行器的姿态角，但飞行器在实际飞行的过程中，由于飞行器本身的摆动、振动或者加速运动时所产生的加速度会对重力加速度产生很大的干扰信号（MPU6050并不能区分哪个是真正需要的加速度），干扰它叠加在应该测量的重力加速度信号上使得输出的数据无法准确反映飞行器的倾角，如图6.4所示。图6.4 单轴加速度测量值曲线图图中实线表示MPU6050单轴测得加速度变化量，而虚线表示重力加速度在该轴上的分量，可以看出，是有干扰叠加在重力加速度分量信号上的。这些波动噪声可以通过数据平滑滤波将其滤除。但是平滑滤波一方面会使得信号无法实时反映飞行器的倾角变化，进而减缓了程序对于飞行器控制。另一方面也会将飞行器角速度的变化信息滤掉。上述两方面的滤波效果使得飞行器保持平衡的难度加大。对于角速度而言，它并不会受到飞行器运动的影响，所以该数据中的噪声很小，可以把转动角速度进行积分得到姿态角，这样又可以进一步平滑信号，从而使得到的角度信号更加稳定。但是这存在一个问题，由于从陀螺仪角速度获得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差和漂移，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致无法形成正确的角度信号。如图6.5所示。图6.5 角速度积分漂移现象曲线图图中实线表示由角速度积分后的角度值，虚线表示真实的角度值。可以得到结论，单单依靠角速度的到的角度无法精确反应飞行器的姿态。如何消除这个累积误差呢？ 一种方法就是通过上面的加速度获得的角度信息对此进行校正。通过对比积分所得到的角度与重力加速度所得到的角度，使用它们之间的偏差改变陀螺仪的输出，从而积分的角度逐步跟踪到加速度传感器所得到的角度。 还有一种方法，利用MPU6050的数字运动处理器，即DMP。首先我们需要得到MPU6050的原始数据（线加速度和转动角速度），然后调用数字运动处理器DMP，将数据进行简单处理后导入DMP，经过DMP内部的姿态算法处理，会输出一组四元数。四元数其实是旋转另一种的表示方式，这里不做详细介绍。DMP输出的四元数是q30格式的，也就是浮点数放大了2的 30次方倍。必须先将其转换为浮点数，再除以2的30次方，计算公式为：q0 = quat0 / q30;/ q30=1073741824.0fq1 = quat1 / q30;q2 = quat2 / q30;q3 = quat3 / q30;在得到四元数之后，还需要根据四元数到欧拉角的转换公式计算出欧拉角，即得到yaw、roll和pitch。公式如下：pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;同样的，我们用杜邦线把GY-86模块（模块上载有MPU6050芯片）接入单片机，先让单片机接收MPU6050原始数据，并把数据做一系列的处理并送入DMP，再根据以上两个公式对DMP的数据做进一步的处理，最后将计算出的欧拉角yaw、roll和pitch显示在TFT屏幕上，方便观察和分析。模块连接图如图6.6所示。图6.6 模块连接图将GY-86模块按不同倾角摆放，具体如下表6.7所示。表6.7 DMP测试说明GY-86模块状态TFT显示X坐标轴-roll绕动Y坐标轴-pitch绕动Z坐标轴-yaw绕动GY-86依靠三角板的30角，X轴平行于斜边。即绕Y轴逆时针转动，所以pitch为-30度。X坐标轴-roll绕动Y坐标轴-pitch绕动Z坐标轴-yaw绕动GY-86依靠三角板的60角，Y轴平行于斜边。即绕X轴顺时针转动，所以roll为60度。X坐标轴-roll绕动Y坐标轴-pitch绕动Z坐标轴-yaw绕动GY-86依靠三角板的30角，Z轴垂直于斜边。即绕Z轴顺时针转动，所以yaw为30度。从表6.7 DMP测试里可以看出，利用DMP进行姿态解算，可以准确地得出飞行器的欧拉角。6.2.3 数据中断虽然我们可以通过DMP模式获取正确的欧拉角，但是应该在什么时候去读取DMP的四元数呢？因为DMP模式下这要的数据都是存储在MPU6050里面的FIFO，FIFO里面存的包括GYRO，ACC，四元数等参数。并且DMP里面的数据只能从FIFO读取的， DMP库里的做法是：FIFO里数据过半满时，会发出冲洗FIFO指令。当FIFO数据不足时，会延时等待。如果我们读取早了，可能获取的是上一次的数据，也可能DMP正在处理，需要延时等待。如果读取晚了，最直观的影响就是飞行器姿态调节缓慢，无法正常飞行。所以随意的去读取数据或者读取的时机选择得不好时（太快或太慢）就会出现数据不连续。为了解决这个问题，使用了MPU6050芯片的数据就绪中断模式，再利用单片机的外部中断，我们就可以在MPU6050数据就绪时立刻获得飞行器的姿态。使用MPU6050数据就绪中断模式需要配置以下的寄存器，如表6.8-6.10所示。表6.8 寄存器58-中断标志寄存器Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit03A58-MOT_INT-FIFO_OFLOW_INTI2C_MST_INT-DATA_RDY_INT这个寄存器展示各个中断源的中断标志。每个位在读取后被清除。这个寄存器我们需要注意DATA_RDY_INT位。当数据就绪中断（Data Ready interrupt）产生，该位自动置 1。读取寄存器该位清 0。表6.9 寄存器55-中断配置寄存器Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1Bit03755INT_LEVELINT_OPENLATCH_INT_ENINT_RD_CLEARFSYNC_INT