基于stm32的无人机控制系统设计

基于stm32的无人机控制系统设计

• **==整篇文章有两万字左右，字数太多了，实在是懒得全部放在这上面来，太废时间了。需要完整论文可主页联系==**
• 第一章 前言
• • 1.1项目背景和意义
  • 1.2国内外发展现状
  • 1.3本文研究的主要内容
• 第二章 设计方案论证与选择
• • 2.1 方案论证与选择
  • • 2.1.1 主控制器的选择与论证
    • 2.1.2 姿态传感器的选择与论证
    • 2.1.3 显示模块的选择与论证
    • 2.1.4 电源模块的选择与论证
    • 2.1.4 数据存储模块的选择与论证
    • 2.1.5 通讯模块的选择与论证
    • 2.1.6无人机机械结构外围器件的选择
  • 2.2 系统基本架构
• 第三章 硬件系统设计
• • 3.1 主控电路的设计
  • 3.2 姿态传感器电路的设计
  • 3.3 状态显示电路的设计
  • 3.4 电源电路的设计
  • 3.5 存储电路的设计
  • 3.6 无人机控制系统扩展电路的设计
• 第四章系统软件设计
• • 4.1 系统整体程序设计
  • 4.2 姿态与位置传感器数据获取程序设计
  • • 4.2.1 IIC通信
    • 4.2.2 MPU6050数据获取
    • 4.2.3 AK8975数据获取
    • 4.2.4 SPL06数据获取
  • 4.3 数据滤波算法程序设计
  • • 4.3.1 巴特沃斯滤波算法原理
    • 4.3.2 一维卡尔曼滤波算法原理
  • 4.4 姿态解算程序设计
  • • 4.4.1 姿态解算原理
    • 4.4.2 姿态融合
  • 4.5 飞行器高度位置控制程序设计
  • 4.6 飞行器姿态控制程序设计
  • • 4.6.1 四轴无人机飞行原理
    • 4.6.2 四轴无人机飞行控制原理
    • 4.6.3 四轴无人机飞行控制程序设计
  • 4.7 其他控制程序设计
• 第五章 系统调试与测试
• • 5.1 传感器数据滤波测试
  • • 5.1.1 传感器加速度计滤波测试
    • 5.1.2 传感器角速度计滤波测试
    • 5.1.3 传感器磁力计滤波测试
  • 5.2 姿态解算与气压计高度测试
  • 5.3 PID参数调节
  • 5.4 整体飞行测试
  • • 5.4.1姿态角摇杆测试
    • 5.4.2整体实际飞行测试及分析
• 第六章 总结与展望
• • 6.1 工作总结
  • 6.2 展望
• 参考文献
• 附录

整篇文章有两万字左右，字数太多了，实在是懒得全部放在这上面来，太废时间了。需要完整论文可主页联系

第一章 前言

1.1项目背景和意义

    在我国航空电子技术和通信技术的高速发展下，无人机应运而生[1]。对于人类来说，无人机无疑是一个伟大发明,它通过人工智能、信号处理以及自主驾驶等先进技术手段,实现了灵活的起降,低空循迹的自由飞翔等功能,同时具备了体积小、无人驾驶以及航程远等优点,帮助人们完成高难度和高风险任务，在民用和军用两个方面都发挥着重要作用，在民用领域，如农林业的调查与监管、玩具航模、航拍、气象探测、卫生公共安全等；在军事领域方面如维护我国领土安全、军事侦察等，各个领域都涉及到无人机的应用[2]。随着无人机技术的成熟和应用的需求，无人机的性能不断提高，新一类的无人机产品——多旋翼无人机成功面世，它在固定翼及单翼无人机的基础上提高了稳定性，可操作性和机动性都进一步得到加强，并且成本较低[3]，可以实现垂直起降及自主悬停,低空飞行时可以灵活迅速地改变机身方向，能够轻松完成复杂空中飞行任务和负载任务。多旋翼无人机涉及到的飞行原理技术非常广泛，需要多种技术共同结合才可使其组成完整的飞行控制系统，从简单的外部机架结构设计到内部稳定性控制的系统设计，都有较强的原理性，同时需要强大的理论知识支撑。虽然它的飞行原理复杂程度不高，但涉及到的各个层面都需要非常高的技术指标和要求[4]。从面世到当前，无人机的发展非常迅速，在各个领域都运用到无人机，应用前景非常广泛。
    如今，研究无人机成为当下的热点，在当今的社会种不断涌现出大量的无人机研究成果，慢慢的渗入人们的日常生活当中，它所带来的便利正在改变人们的生活方式，产生了极其重要的影响，在社会生活方面都可起到一定的作用，它可作为一种新的交通方式，给人们提供了一个穿越糟糕道路的途径；它在农业作业方面采用一种新型高效率的农作产量方式，以较低的生产成本来提高农业耕作效率[5]；它可为城市的规划与建设提供一种测量地理位置的途径，根据无人机监测所返回的信息数据可以提高地理信息的获取率，更好的促进城市建设[6]。但这些民用无人机不同于军用及商用无人机，其飞行姿态容易受到气流的干扰，如何实现姿态控制响应快是多旋翼无人机系统设计的关键，相信随着技术的不断成熟，无人机所存在的关键性问题都能得以解决，将为以后无人机的普及提供强有力的帮助，由此可见，多旋翼无人机在如今的社会生活方面占据重要的地位，发展潜力巨大，因此研究无人机姿态控制系统具有极大的实际意义。

1.2国内外发展现状

    相比于国内，国外更早开始研究无人机，在1907年，Breguet兄弟是最先开展四旋翼式飞机试验的研究者，由此实验产生了国内外的第一架多旋翼飞行器，无人机的概念由此而来[7]。但由于当时技术水平的限制，惯性导航系统重量较大，对飞行器的载荷要求较高，大大限制了多旋翼飞行器的发展，导致多旋翼飞行器体积庞大、笨重，且电子自动控制系统不成熟，无法满足人们的期望，在之后的很长一段时间里都无法解决当时的关键技术问题，改变现状。直到微机电系统技术[8]的出现，由该技术所制造的传感器成本较低，可使用时间较长，功率损耗很低，因其体积相对较小多被运用于小型多旋翼无人[9]，增强了多旋翼无人机的实用性，为后来的无人机发展打下基础。但是其传感器噪声很大，于是人们又花了很长时间来研究设计降噪和姿态控制算法[10]。要进行姿态控制就先要做到姿态解算，它是控制无人机状态的基础条件，姿态分析数据越准确，无人机所获取到的姿态数据就越靠近于无人机的真实控制状态，对于促进其他算法的改进提供了帮助[11]。有研究者提出采用模糊PID控制来对小型无人机进行姿态解算，利用传感器信息融合技术，将多种传感器所采集到的数据进行整合优化，从而能获得与被测目标相接近的信息分析或描述，能加强无人机在高距离中飞行的精度和稳定性，从而提供可靠的飞行条件[12]。也有研究者提出了采用互补滤波算法来解决无人机姿态解算的问题[13],互补滤波算法虽然可以通过改变滤波增益，从而使传感器误差对姿态解算产生的影响得到进一步的降低，但是却无法进行计算陀螺零偏漂移的误差，为此研究者进一步提出了采用卡尔曼滤波方法估算载体的姿态[14]，利用陀螺仪、加速度计和磁阻传感器进行组合，产生各种各类的线性或者非线性滤波算法估算陀螺零偏误差，以及使用神经网络的方法进行姿态融合[15]。直到2005年，通过不断的进行算法改进，稳定性较高的多旋翼飞行控制器产生了，即使这个多旋巽飞行器所用到的传感器设备并不是当下所用的高科技设备，也不能实现在空中自由飞行，但这是在飞行器领域当中取得历史性的一个技术突破，从此打开了多旋翼飞行器领域的大门。
    相比于国外开展多旋翼无人飞行器的研究，国内对多旋翼无人机的研究起步比较迟，早期带头研究的机构较少，最早开始相关工作研究的是高校与研究院，其中作为代表的哈尔滨工业大学设计了系统鲁棒控制器、天津大学研究多旋翼故障重构技术、南京航空航天大学设计一个微型机载视觉系统，使用视觉同步和映射算法来实现飞行轨迹跟踪能力等，尽管所有的研究成果都取得了不错的成绩，但研究仍处于理论算法与仿真验证阶段，能实现遥控操纵使飞行器在空中悬停的高校或研究院非常少，还无法实现自主飞行[16]。直到2012年，大疆研发出多旋翼航拍无人机，自此航拍逐渐出现在日常生活中，因能较稳定的飞行、操作简单，体积较小同时可装载摄像头用于画面传输而深受各个领域的广泛使用。同时不断出现研发无人机的国内企业，使得研究多旋翼无人机的相关机构将目光从国外转移到了国内。近年来，尽管我国在无人机领域发展较为迅速，但因起步较晚，同时缺乏相关的核心技术，与其他发达国家存在的差距还是较大的，但随着无人技术不断成熟进步，相信在以后将会变得更加智能化，实用化，在未来的现代化城市中能发挥非常重要的作用[17]，在各个领域能帮助解决某些关键性问题，实现全方位运用，也能普及到日常生活中，提高人们的生活水平质量。

1.3本文研究的主要内容

    本设计的主要研究内容是设计一款基于STM32单片机的多旋翼无人机系统，通过编写相关模块程序，完成稳定飞行测试，在此该系统上，作一些姿态检测算法及系统仿真，本文设计要求如下：
   （1）搭建四轴飞行器机械结构。完成四轴飞行硬件结构设计。
   （2）建立四轴飞行器系统模型。对飞行器控制原理进行分析及介绍。
   （3）硬件模块设计。对飞行器控制系统进行PCB硬件模块设计。
   （4）姿态检测系统设计，通过姿态检测融合算法，对传感器的数据进行采集，并对姿态融合算法下的姿态数据进行分析。
   （5）控制算法的设计，运用并设计PID算法对飞行器进行控制，对飞行器多次加入干扰，从稳定性，快速性，准确性上进行分析及比较。
   （6）四轴飞行器程序设计。通过keil5软件进行程序编程，完成飞行器的程序设计。
   （7）飞行效果测试及调试。将编写完成的程序下载到四轴飞行器中进行测试及调试，并对实验结果进行分析。

第二章 设计方案论证与选择

2.1 方案论证与选择

    本设计主要以STM32单片机为主控芯片，通过接收遥控器的指令从而控制无人机的资态，主控制板包括姿态传感器、无线发射接收模块、电子调速模块以及降压模块等；采用成品遥控器及接收机相互通信的方式进行无线遥控。结合本系统的研究内容及方案设计，通过对无人机所需电路设计的进行选择论证，确定合适的方案完成电路设计。

2.1.1 主控制器的选择与论证

    控制器是无人机控制器的核心部分，对四轴无人机 的控制实现起着至关重要的作用，在四轴无人机控制系统中，传感器的数据采集及融合，以及传感器数据的运算都通过微控制器处理芯片实现。在飞行控制器进行姿态解算算法与控制算法运算时，会涉及到大量的数学计算,所以在控制器芯片选择方案上应考虑计算特性强、计算迅速的控制器芯片。同样,无人机控制器中的各类软件模块必须有多样化的接口技术与外设实现采样,如姿态传感器的采集必须使用IIC总线协议或者SPI总线协议实现数据通信,所以选择控制器芯片应具备多样化的外设以及简单的电路设计,进一步提高硬件系统工作稳定性。在综合考虑成本,技术要求及功耗的前提下,且符合飞行控制系统性能要求,控制器芯片的选用应考虑效率高、低功耗、外设多样化的新型高性能嵌入式单片机[18]。本设计经过对半导体公司意法半导体所研发的STM32系列的单片机设计进行分析对比,给出以下几种方案进行选择。
    方案一：选择STM32F1系列的单片机，STM32F1系列的单片机是基于M3内核框架设计,最高主频可达72Mhz,内部SRAM最大64K字节，拥有5个串口，21个ADC采集通道，8个16位定时器。
    方案二：选择STM32F4系列的单片机，STM32F4系列的单片机是基于M4内核框架设计,最高主频可达168Mhz,内部SRAM最大192K字节，支持单精度浮点运算，拥有6个串口，24个ADC采集通道，12个16位定时器和2个32位定时器。
    综上所述对比，本设计选用STM32F4系列的单片机作为核心控制系统，综合外设需求与程序容量，最终选择STM32F4系列下的STM32F407VET6单片机，该单片机型号为100脚LQFP封装，片上内存FLASH为512K字节,SRAM为192K字节，且支持硬件FPU。

2.1.2 姿态传感器的选择与论证

    无人机控制器上通过姿态传感器进行无人机姿态与位置的测量。其中，无人机的姿态测量通过采集陀螺仪三轴角速度，加速度计三轴重力加速度以及磁力计三轴磁力强度的数据进行算法融合得到，高度测量通过采集气压计的数据经过数字滤波融合得出无人机飞行高度。选择适合无人机工作环境的传感器对于无人机控制设计是尤为重要的，精度高，性能好的传感器能够准确无误的感知出无人机的姿态与位置。同时考虑成本、功耗以及电路设计，本文经过部分惯导传感器进行分析对比，给出以下几种方案进行选择。
1、 三轴重力加速度计，三轴角速度计方案选择对比：
    方案一：使用ICM20602六轴传感器来获取三轴重力加速度以及三轴角速度的数据。ICM20602是一个六轴运动跟踪装置,内部结合了一个三轴角速度计以及一个三轴重力加速度计。同时内部包含一个1KB的数据缓冲器,能够减小串行总线接口数据通信量,可透过允许系统处理设备突然读写传感器数据信息,并且加入低功耗运行模型来减低耗电量。其角速度计的可编程满量程及重力加速度计可编程满量程有四种模式，其中角速度计的敏度误差为±1%，重力加速度计噪声为100μg/Hz。
    方案二：使用MPU6050六轴传感器来获取三轴重力加速度以及三轴角速度的数据。MPU6050是一款九轴姿态处理感应器。它里面整合了一套三轴旋转陀螺仪和一套三轴加速度计,还有一套可扩充的数字运动信息处理器,利用IIC总线端口连通一套第三方的数字感应器,例如磁力计。扩展之后就能够使用其IIC端口提供一个九轴的信号。陀螺仪可测范围及加速度计可测范围有四种模式。内部包含一个1KB的数据缓冲器，可有助于降低系统功耗。
    通过对比ICM20602和MPU6050,ICM20602虽然性能上较为优秀，但其价格也相对较高，参考资料也较少，考虑到硬件成本问题以及资料参考方面，本文选择方案二，以MPU6050作为三轴加速度，三轴角速度的数据采集。
2、三轴磁力计方案选择对比：
    方案一：使用HMC5883L作为三轴磁力计的数据输出。HMC5883L 是一款高集成磁力计模块，内部带有数字接口弱磁传感器芯片，可应用于多种磁场检测领域。使用IIC数字接口，内置自检功能，12位ADC，能在强磁场环境中输出高精度的航行角度，误差可达1度左右，带有三种驱动器，用于消磁、自测和偏移补偿，分别为置位、复位和偏置驱动器。最大输出频率可达160hz。
    方案二：使用AK8975作为三轴磁力计的数据输出。AK8975适用于罗盘应用的三轴磁力计装置，内置A/D转换器，用于磁力计的数据输出。输出分辨率为13Bit（0.3uT/LSB）,测量范围为±1200µT，采用IIC总线接口，同时还支持四线SPI总线接口。带有内部自检功能，拥有四种不同的工作模式，分别为断电模式、单测量模式、自测试模式和熔断器接入模式。
    通过对比HMC5883L和AK8975,AK8975输出的分辨率更高，因此本文选择AK8975测量三轴磁力计的数据。
3、气压传感器方案选择对比：
    方案一：选择MS5611测量无人机飞行高度。MS5611是一款高分辨率气压高度传感器，具有SPI和IIC总线接口，其高度分辨率可达10CM,该传感器能够输出24位的气压值和温度值，功耗低，仅1uA，其工作电压可选择1.8V或3.6V。
    方案二：选择SPL06测量无人机飞行高度。SPL06是一款具有高精度，功耗低等特点的小型数字气压传感器，支持IIC总线接口和SPI总线接口，高精度模式下可达到±5cm的误差，带有FIFO,可存储最新的32个温度测量值或者气压测量值，工作电压为1.2V-3.6V。内部信号处理器可将气压和温度传感器的输出转换为24位结果。
    通过对比MS5611和SPL06，SPL06精度更高，且价格相对比MS5611而言，价格更低，因此，本文选择SPL06来测量无人机的飞行高度。
    综上选择，本文选择六轴传感器MPU6050，三轴磁力计传感器AK8975以及气压传感器SPL06来获取无人机的姿态与位置数据。

2.1.3 显示模块的选择与论证

    为了更好的展示无人机的飞行轨迹及状态，通常在无人机控制系统上增加一些显示模块，本文对几种显示模块进行分析对比，给出以下几种方案进行选择：
    方案一：通过0.96寸OLED屏模块来显示无人机的飞行数据及飞行状态。0.96寸OLED屏模块采用SSD1306作为驱动芯片，支持IIC总线接口以及SPI总线接口，具有多个指令，可以通过指令修改OLED的工作模式，具有多种功能，可以显示汉字、字符及图案等数据信息。
    方案二：通过全彩RGB灯珠混搭出不同颜色来对应出无人机不同的飞行状态。全彩RGB灯珠是大功率LED灯源的一种，由三原色灯芯片构成，相比较同等亮度的灯珠，耗电量更少，使用寿命更长，其响应速度快，可实现色彩的动态变化及数字化控制。通过对三原色灯芯片的控制，可达到全彩渐变的变色效果。运用空间大，可用于多种场面和环境中。
    综上选择，本文选择全彩RGB灯珠来对无人机的状态进行显示。在夜间或者光线不明的环境中能够更好的显示出无人机当前的状态，相对于OLED屏幕模块，在处理一些复杂的任务过程中可能会造成OLED屏模块的损坏及掉落，则全彩RGB灯珠不会造成损坏及脱落，反而能更好的显示无人机当前的状态，方便地面人员的操作。

2.1.4 电源模块的选择与论证

    四轴无人机通常采用大容量，大电流的3S航模锂电池进行供电，而无人机控制器中的各种元件均有特定的工作电压范围，为了能让无人机主控制板各个元器件在合适的电压下稳定工作，需要将3S航模锂电池电压降压到5V,由于姿态与位置传感器的工作电压均在3.3V，所以需要将5V电压降到3.3V供给传感器。本文针对几种降压电源模块进行分析对下，给出以下几种方案进行选择：
1、5V降压电源模块方案选择：
    方案一：采用开关电源芯片LM2596配合一些外围电子元器件构成稳压电路。LM2596系列是一款能提供3A电流保护的降压开关电源型集成化稳定晶片,里面包含多种防护集成电路,分别电流控制电路、电压防护电路以及热关断集成电路等,设计简单方便,可通过少部分外围元件构成稳定工作降压电路。
    方案二：采用稳压集成电路芯片LM7805搭建直流稳压电源。LM7805是一款常用的三端稳压性元件，输出电压为5V,内部带有多种电路，输出电流最大可达1.5A。
    考虑到四轴无人机要接入的外设传感器过多，所需的电流过大，本文采用开关电源芯片LM2596配合一些外围电子元器件构成5V直流稳压电路。
2、3.3V降压电源模块方案选择：
    方案一：选用RT9193稳压器为传感器进行供电。RT9193是专门为无线和射频电路等苛刻性能及空间要求设计的一款便携式设备芯片，内部具有多种保护功能，如温度、过流等。拥有超快速响应及宽工作电压范围（2.5V-5.5V）等特性。
    方案二：选用AMS-1117稳压器为传感器进行供电。AMS-1117是一款正向低压降线性稳压器，片内具有过载、过热保护电路，输出电流为1A,适用于小型电源管理设备电池供电的仪器。
    根据芯片手册，本文选择AMS-1117芯片为传感器进行供电，一是价格相比于RT9193价格低，二是容易焊接，三是所需外围元器件少。

2.1.4 数据存储模块的选择与论证

    由于四轴无人机的惯导传感器在出厂时会有误差，为了减去四轴无人机开机校准传感器误差步骤，会将误差值数据存储在一些存储芯片中，在四轴无人机开机时即可直接读取传感器误差值数据。本文给出以下几种数据存储方案进行选择：
    方案一：选用W25QXX系列的存储芯片。W25QXX是一款采用SPI总线接口串行Flash存储器，带有多种写保护，有多种系列存储器可供选择，比普通的串行存储器性能高，数据保存时间长打20年。
    方案二：选用AT24C02存储器。AT24C02是一款串行存储器，内部存储量2KB,采用IIC总线接口通信，内部带有一个8字节的页写缓冲器以及专门的写保护功能，数据保存可长达100年之久，掉电数据不会丢失。
    综上，本文选择AT24C02存储器作为传感器误差数据的存储，一是价格比W25QXX低，二是对于本文设计的四轴无人机数据存储空间所需不大。

2.1.5 通讯模块的选择与论证

    为了能够更好的远程监测无人机的飞行状态，需要使用无线通讯模块在无人机上以及地面操控台上建立无线连接，常用的无线通讯方式有两大类，第一类是以433MHZ频段或2.4G频段的方式建立无线数据传输，第二类是通过通讯网络来建立连接，虽然通讯网络能够在有网的条件下能够真正实现无线数据通讯，但是它的使用相对来说较为复杂，通信有较大的延迟，为了更好的检测无人机的飞行状态，采用433MHZ频段或2.4G频段的方式建立无线数据通讯，本文通过介绍两种无线数据传输模块来确定通讯模块的选择。
    方案一：采用APM的3DR无线数传模块。3DR无线数传模块是一款支持MAVLink协议的无线数传模块，原用于APM开源飞控，其发射接收频率为433MHZ，功率为100MW,发射接收距离远，距离可达500米左右。
    方案二：采用匿名科创旗下研发的无线数传模块。该数传集成PA+LNA电路，以2.4G频段的方式进行发射接收，支持双向通信，自动重发以及参数可配置等功能，距离远，信号强，数据传输高速、稳定。
    经过对比分析，本文采用匿名科创旗下研发的无线数传模块进行地面控制站与无人机之间的数据通讯。

2.1.6无人机机械结构外围器件的选择

    为了更好的使无人机更好飞行，机架、遥控、电机、电调以及电池的全部选择购买现成的产品。
    飞行器机架选用的是F450四轴飞行器机架，该机架采用玻纤电路板以及高强度塑料尼龙材质机臂组成，机架总重量约280g。
    无刷电机选用的是伟力XK2212无刷电机，KV值为950KV,自带散热结构。
    电子调速器采用的是HOBBYWING公司生产的无刷电机电子调速器。该电子调速器使用超低内阻的MOSFET专用芯片，其稳定输出电流为20A,可支持的油门信号刷新率最高为621HZ，兼容各种飞控。
    电池采用航模3S锂电池，容量为4500mah，放电倍率为35C。
    遥控及接收机选用的是富斯I6X航模遥控以及十通道接收机，该遥控最大输出通道为十通道，遥控频率为2.4G,采用LCD白色背光显示，支持PWM、PPM、IBUS、SBUS协议。

2.2 系统基本架构

    该系统采用STM32F407VET6芯片作为飞控核心处理器，通过处理姿态传感器与位置传感器采集到的数据，通过一系列的姿态解算算法求解得出无人机的姿态角，求解得出的姿态角经过一系列的控制算法及控制指令得出电机的输出量，从而控制无人机的姿态及位置。根据上述需求分析，该系统由主控制器芯片、姿态传感器、电机驱动模块、通信模块、显示模块等组成。系统硬件结构图如图2-1。

                                   ;图2-1系统硬件结构图

第三章 硬件系统设计

3.1 主控电路的设计

    本设计使用的是STM32F407VET6单片机作为核心处理器芯片，该处理器芯片框架基于M4内核，具有较高的性能，和较快的运行速度并且功耗低。内部FLASH高达512KB,SRAM为192KB，拥有100个引脚，14个定时器，6个串口通道，3个IIC通道，3个SPI通道，工作频率为168MHZ，符合本设计要求，其最小系统由电源、时钟、复位以及启动电路组成。
   （1）电源电路
    电源电路如图3-1所示，由于核心处理器的工作电压为3.3V，所以选取AMS1117-3.3电压稳压器将5V电源电压降压至3.3V。AMS-1117是一款正向低压降线性稳压器，片内具有过载、过热以及限流保护电路，输出电流高达1A以上。图中F是自恢复保险丝，具有保护电路的作用，当电源电压过大或者稳压电源电路出现短路，自恢复保险丝会因载流值过大而熔断，当再次上电时会自动恢复，图中D2元件是肖特基二极管,在电路中起稳压整流的作用，当电压大于它的导通压降时，二极管开始工作。C27、C28、C29、C30为滤波电容，其中C27、C29的容值为10UF，在电路中滤除低频信号的干扰，C28、C30的容值为0.1UF，在电路中滤除高频信号的干扰。

   (2）时钟电路
    从STM32F4的芯片手册可以看出，在STM32F4系列中，有多个重要的时钟源，可分为高速和低速，其中PLL、HSE、HSI为高速时钟， LSE、LSI为低速时钟。高速外部时钟HSE，可接陶瓷、石英振荡器或者频率范围为4MHZ-26MHZ的外部时钟，该最小系统所接的是8MHZ的晶振，如图3-2所示。

图3-2 时钟电路图
   (3）复位电路
    在单片机控制集成电路中,有上电恢复复位,自动恢复复位或者看门狗恢复复位三个恢复复位方法。图3-3使用的是上电恢复复位方法。由于STM32系列单片机恢复复位功能使用的是低电平复位方式,根据图3-3,当该系统板在上电的瞬间,复位电路电容两端是处在没电0V状况,此时恢复导路处在低电平状况,并且电容C15处在充满中,当电容器充到一定值时,恢复导路就变成高电平状况。


   （4）启动电路
    STM32单片机的启动电路如图3-4，通过将BOOT0和BOOT1全部拉低，以用户闪存存储器启动。表3-1介绍了BOOT0和BOOT1的启动模式。
略
表3-1 启动模式

3.2 姿态传感器电路的设计

    姿态传感器通过采集无人机在空中的原始数据以IIC总线接口的方式传输给主控制器，主控制器将得到的数据经过滤波融合得出无人机的三个姿态角。图3-5为姿态数据采集系统框图，在图中，六轴传感器MPU6050，三轴磁力计AK8975和气压计共同连接在同一条IIC总线接口上，为了使IIC总线通信稳定，在IIC总线接口上上拉4.7K的电阻。

图3-5 姿态数据采集系统框图
    MPU6050电路图如图3-6所示,9150_INT为MPU6050的中断接口,当MPU6050采集到的数据结果进行转换,该接口会有一个低电平脉冲。I2C_SCL时钟接口和I2C_SDA数据传输接口与STM32F4单片机IIC的时钟信号和数据端口相连。AD0引脚是MPU6050的地址选择引脚，当该引脚接地或者浮空时，设备的读取地址为0X68，当接入高电平时，MPU6050设备地址为0X69。而MPU6050的AUX_DA和AUX_CL引脚可连接一个第三方的数字传感器，如磁力计传感器，本文中未使用该引脚，而是直接接入单片机端口。

图3-6六轴传感器 MPU6050电路图
    磁力计AK8975和气压计SPL06设计电路为图3-7、图3-8所示，其外围元件较少，图中的两个电容的作用均为滤除电路中低频信号的干扰。

图3-7 磁力计AK8975电路图

图3-8 气压计SPL06电路图

3.3 状态显示电路的设计

    为了能够更好的显示出无人机当前的状态，本文选择全彩RGB灯珠来对无人机的状态进行显示，通过控制单片机端口电平信号，即可达到不同的灯光效果。图3-9为状态显示电路图。

图3-9 状态显示电路图

3.4 电源电路的设计

    为了能让无人机主控制板各个元器件在合适的电压下稳定工作，通过LM2596线性稳压器将3S航模锂电池电压降压到5V，图3-10为LM2596降压电路图。其中LM2596的1号引脚VIN最高可输入直流电压为40V，最低为4.5V。2号引脚VOUT最高可输出37V直流电压，。3号引脚GND是公共端。4号引脚FB通过采集输出电压与内部基准稳压值相比较，使输出电压同比例增大或减小。5号引脚ON/OFF是稳压器的使能控制端，当该引脚接高于1.23V电压时，内部开关管被关断，电压输出，反之，输出额定电压。图中电感L1和肖基特二极管D2的作用是为了能够输出大电流，让输出电流更稳定。电容C8、C9的作用是为了滤除电路中上的低频信号的干扰，C31的作用是为了滤除电路中上的高频信号的干扰。电阻R26和LED灯PWR2的作用是为了显示该电路是否正常工作。
图3-10 LM2596降压电路图

3.5 存储电路的设计

    为了能够存储姿态传感器的误差数据，本文增加了AT24C02 EEPROM存储电路,该电路与姿态传感器共用一个IIC总线接口，通过读写设备地址，从而读写数据，图3-11为AT24C02 EEPROM存储电路图，图中电容的作用为滤除电路中的低频信号干扰，使电路稳定。

图3-11 AT24C02 EEPROM存储电路图

3.6 无人机控制系统扩展电路的设计

    除上述所述的硬件系统外，无人机控制系统还包括电机驱动电路、遥控信号采集电路、程序下载电路以及部分扩展功能接口。
遥控信号采集电路和电机驱动信号电路如图3-12所示。其中遥控信号的采集是通过单片机的输入捕获功能进行采集，电机的驱动信号则是通过单片机输出PWM进行驱动。在该电路中，为了确保STM32单片机在运行过程中，不会因信号输入输出电流过大而烧坏单片机，因此在信号输入输出端口接入一定阻值的电阻限制电流，其中遥控信号的输入端接入10K的电阻，电机驱动输出端接入1K的电阻。

图3-12遥控信号采集及电机驱动信号电路图
    程序下载电路以及部分扩展功能接口电路图如图3-13所示。通过MX1.25端子底座连接到单片机，以供后面工程所需，其中USART2供给无线数传模块使用。

图3-13 程序下载电路以及部分扩展功能接口电路图

第四章系统软件设计

    四轴无人机的软件设计涉及到多方面的知识，主要包括无人机姿态的解算、传感器数据的处理以及无人机姿态控制等多个方面。本章通过介绍四轴无人机的控制原理、姿态解算原理以及数据滤波原理，利用C语言编写其程序代码，使用单片机软件开发系统进行系统软件程序的编写、编译及调试。

4.1 系统整体程序设计

图4-1 系统整体程序流程图
    图4-1为系统程序整体流程图，该设计分为主程序和定时器中断程序两部分，其中主程序主要完成PID参数、RGB显示、串口通信、定时器输入捕获、电机PWM输出、IIC总线、姿态传感器以及定时器等一系列初始化过程，最终在主循环程序里通过串口发送数据给上位机。定时器中断采用5ms中断计时，用来完成传感器的数据采集及滤波处理、无人机的姿态解算、PID姿态控制以及遥控器数据分析等一系列功能，其中使用定时器进行处理，是为了防止程序卡死在某功能，从而使程序高效率工作。

4.2 姿态与位置传感器数据获取程序设计

     本设计的无人机控制系统为了获取无人机的姿态及位置信息由一个三轴角速度计、一个三轴加速度计、一个三轴磁力计以及一个气压传感器组成。通过对上述对传感器的数据进行采集，经过算法处理，从而获得无人机姿态与位置信息。在硬件设计上，将这些传感器连接到STM32单片机IIC接口，连接图见图3-5。

4.2.1 IIC通信

    IIC总线是通过一条时钟线IIC_SCL和一条数据线IIC_SDA两根线路组成，可以用来发送和接收数据。只有在两条信号线上传输的值等同于设备的地址值，设备才会响应。以下是IIC总线的通讯协议，其中SCL是信号的时钟线，SDA是信号的数据线。
    （1）起始信号：当时钟线,处于上升沿，数据线电平被拉低。起始信号时序如图4-2。

图4-2 起始信号时序图
    （2）停止信号：当时钟线处于上升沿，数据线电平被拉高。停止信号时序如图4-3。

图4-3 停止信号时序图
    （3）应答信号：当设备数据从高位到低位进行传输后，设备会将数据线IIC_SDA拉低，给主控制器发送一个应答位，此时该数据传输结束，反之，当设备无法接收数据，主控制器会接收到一个非应答位。图4-4为应答时序图。

图4-4 应答时序图

4.2.2 MPU6050数据获取

    MPU6050通过与主控制器之间采用IIC总线通信的方式进行读取三轴加速度和三轴角速度的数据，该过程主要包括对MPU6050进行初始化配置，然后进行MPU6050数据的读取。图4-5为MPU6050程序流程图。其中MPU6050初始化包括的MPU6050工作模式与休眠模式的配置、时钟源的配置、陀螺仪和加速度计量程的配置、采样频率的配以及低通滤波频率的配置。

图4-5 MPU6050程序流程图
    （1）MPU6050工作模式与休眠模式的配置
图4-6为MPU6050工作模式与休眠模式配置寄存器映射表，其中该寄存器地址为0x6B，通过配置寄存器第6位为0，即可进入工作模式。

图4-6 MPU6050工作模式与休眠模式配置寄存器映射表
    （2）MPU6050时钟源配置
MPU6050时钟源配置见图4-6，通过配置该寄存器第0至2位，即可配置MPU6050时钟源，本设计配置的时钟源为内部时钟源，十六进制代码为0X00。
    （3）MPU6050采样频率配置
MPU6050采样频率是通过配置寄存器SMPRT_DIV，如图4-7，该寄存器地址为0X19，本设计配置采样率为1KHZ，十六进制代码为0X00，采样率计算公式为式4-1：
（公式4-1）

图4-7 MPU6050采样频率配置寄存器映射表
    （4）MPU6050低通滤波频率配置
根据图4-8，通过配置CONFIG寄存器0X1A的第0至2位即可配置该传感器低通滤波频率，滤波值如图4-9所示。

图4-8 MPU6050低通滤波频率配置寄存器映射表

图4-9 MPU6050低通滤波频率值情况表
    （5）MPU6050角速度计和加速度计量程的配置
MPU6050需要对角速度测试仪和加速度测试仪的量程进行选择分配,图4-10MPU6050为角速度测试仪和加速度测试仪寄存器映射表,根据分配寄存器的第3位和第4位,即可选择角速度测试仪和加速度测试仪的量程,同时角速度测试仪的四个量程依次为±250º/s、±500º/s、±1000º/s、±2000º/s。加速度测试仪的量程依次为±2g、±4 g、±8 g、±16 g。

图4-10 MPU6050角速度测试仪和加速度测试仪寄存器映射表

4.2.3 AK8975数据获取

通过IIC总线与传感器AK8975进行通信，并配置磁力计AK8975的工作模式即可采集磁力计的数据。程序流程图见4-11。

图4-11 AK8975程序流程图

4.2.4 SPL06数据获取

SPL06以IIC通信的方式进行模式的配置，通读取压力寄存器的数据以及温度寄存器的数据并进行数据校准及处理。程序流程图见4-12。

图4-12 SPL06程序流程图

4.3 数据滤波算法程序设计

由于传感器在工作运行时，会有噪声的干扰，读取到的传感器数据会有一定偏差，为保证数据的准确性，因此需要将传感器采集到的数据进行处理，常用的处理方式是经过滤波器进行数据处理。本设计采用巴特沃斯滤波算法以及一维卡尔曼滤波算法对传感器读取的数据进行处理。以下是滤波算法的原理及程序设计。

4.3.1 巴特沃斯滤波算法原理

巴特沃斯滤波器是滤波器的一种，它的特点是在通带内的幅值特性能实现最大限度平坦，其幅度平方函数为式4-2：
（公式4-2）
式中N为滤波器的阶数，ΩC为3dB截止频率,Ω为频率。根据其函数求出滤波器的系数。本设计通过MATALB软件里fdatool工具通过输入参数即可求出所需滤波器的系数。根据系数带入差分方程，其差分方程为式4-3：
（公式4-3）
公式简化为式4-4：

（公式4-4）
C语言程序设计见附录1-1。

4.3.2 一维卡尔曼滤波算法原理

卡尔曼滤波是一种通过系统输入输出观测数据，对系统状态进行最优估计的算法。该算法分为预测和勘正两个过程，共有五个基本公式。
1、预测阶段：
（1）先验估计，公式如式4-5：
（公式4-5）
其中X(k|k-1)是上一时刻k的系统先验预测值，A和B是系统系数，X(k-1|k-1)是K-1时刻的系统后验状预测值，U(k)是现在时刻的系统控制量。
（2）误差协方差计算，公式如式4-6：
（公式4-6）
其中P(k|k-1)是X(k|k-1)的系统协方差，P(k-1|k-1)是X(k-1|k-1)的系统协方差，Q是系统预测过程的误差协方差。
2、勘正阶段：
（1）计算增益，公式如式4-7：
（公式4-7）
其中H为系数矩阵，R为测量的噪声协方差。
（2）更新估计，公式如式4-8：
（公式4-8）
其中Z(k)是测量的输入值。
（3）更新误差，公式如式4-9：
（公式4-9）
其中I为1的矩阵。
根据卡尔曼的五个基本公式，其C语言程序代码见附录1-1.
4.3.3 数据滤波处理
根据上述介绍的滤波算法原理，编写C语言程序代码，其中巴特沃斯滤波算法用于加速度计和陀螺仪数据处理，卡尔曼滤波用于磁力计输出数据处理，程序流程图如图4-13。

图4-13 数据滤波程序流程图

4.4 姿态解算程序设计

为了获取四轴无人机姿态角，通常采用四元数法、欧拉角法以及方向余弦矩阵法。其中四元数由于操作计算小，被广泛使用，本设计采用四元数法计算四轴无人机的姿态角。

4.4.1 姿态解算原理

1、建立坐标系
地理坐标系与机体坐标系的转换图如图4-14，图中XYZ坐标系称为n系坐标系，代表地理坐标系，XYZ坐标系称为b系坐标系，代表机体坐标系。其中绕X轴旋转的角度称为横滚角，用Φ表示，绕Y轴旋转的角度称为俯仰角，用θ表示、绕Z轴旋转的角度称为偏航角，用φ表示。

图4-14 地理坐标系与机体坐标系转换图
2、建立姿态角旋转矩阵
根据图4-12，绕Z轴的偏航角旋转矩阵可表示为式4-10: (公式4-10) 绕Y轴的俯仰角旋转矩阵可表示为式4-11:
（公式4-11）
绕X`轴的横滚角旋转矩阵可表示为式4-12:
（公式4-12）
将各个轴的角度旋转矩阵相乘，得到机体坐标系旋转矩阵，记为式4-13：
（公式4-13）
将公式4-13进行转置运算就可以得出空间坐标系旋转矩阵，记为式4-14：
（公式4-14）
将空间坐标系旋转矩阵表示为方向余弦矩阵，可表示为式4-15：

（公式4-15）

4.4.2 姿态融合

4.5 飞行器高度位置控制程序设计

4.6 飞行器姿态控制程序设计

4.6.1 四轴无人机飞行原理

4.6.2 四轴无人机飞行控制原理

4.6.3 四轴无人机飞行控制程序设计

4.7 其他控制程序设计

当姿态解算及无人机控制程序设计完，需要设计无人机姿态显示程序、上位机通信程序、电子调速器程序以及遥控器信号接收程序。
1、无人机姿态显示程序设计
无人机姿态显示程序通过配置RGB灯引脚，控制RGB灯亮灭及闪烁状态，从而达到七种不同颜色灯光效果，程序流程如图4-21：

图4-21 RGB程序流程图
2、上位机通信程序设计
本设计采用匿名科创设计的匿名上位机进行数据通信，该上位机界面如图4-22，通过上位机可以显示无人机当前姿态以及传感器数据，同时可以通过该上位机给无人机进行参数的调节，其通信协议是由四部分组成，分别为数据帧头、功能码、数据以及校验位程序，设计流程图如4-23所示。

图4-22 上位机界面图

图4-23 上位机通信程序流程图
3、电子调速器程序设计
电子调速器用来驱动无人机电机，首先配置PWM，其次设置电子调速器量程，最后将无人机姿态控制输出的PWM增量，传输给电子调速器，带动电机旋转。程序配置流程图如图4-24所示。

图4-24 电子调速器程序设计
4、遥控器信号接收程序设计
遥控器信号接收是以PWM的信号方式进行传输，因此需要配置STM32定时器输入捕获模式用来测量信号频率，程序配置流程图如图4-25所示。

图4-25 定时器输入捕获程序配置流程图

第五章 系统调试与测试

5.1 传感器数据滤波测试

5.1.1 传感器加速度计滤波测试

为了验证传感器数据滤波算法的效果，本文采用巴特沃斯滤波器对陀螺仪及加速度进行滤波处理，通过匿名上位机进行原始数据及滤波后的数据显示，图5-1是传感器加速度计滤波测试图。其中黄色曲线代表的是加速度经过巴特沃斯滤波器处理得到的数据，绿色曲线是传感器加速度原始数据，可以看出，经过巴特沃斯滤波器处理的数据具有噪声小，平稳性好，实时性好的特点。

图5-1 传感器加速度计滤波测试图

5.1.2 传感器角速度计滤波测试

5.1.3 传感器磁力计滤波测试

5.2 姿态解算与气压计高度测试

5.3 PID参数调节

5.4 整体飞行测试

5.4.1姿态角摇杆测试

5.4.2整体实际飞行测试及分析

第六章 总结与展望

6.1 工作总结

在本设计中，运用到了高等数学、物理力学、数字电路、信号处理、电工技术、数据结构以及嵌入式系统及应用等一系列专业知识，让本人明白学习的重要性，对于电子信息工程专业，不仅需要扎实的基础，更多是动手实操的经验积累。
本设计是基于STM32单片机的多旋翼无人机系统，该无人机系统包括了串级PID控制算法、数据滤波算法、姿态解算算法以及上位机通信等一系列的软件设计。该无人机机械结构包括F450玻纤合成的四轴机架，飞控减震板、电子调速器、电机以及9寸螺旋桨。其中将设计的飞行控制板固定在减震板上，可以有效减小飞行时带来的震动。
通过完成一系列飞行测试及调试，本设计的无人机能够实现稳定飞行，在飞行测试过程中，可以通过增加一些设备，实现定高定点飞行等功能，虽然在小的动态情况下该无人机能平稳飞行。但在高动态中，本设计并不能使飞行器平稳飞行，甚至可能导致坠机。另外，该四轴无人机不具备定点悬停的功能。如果能在后续的设计中，加入定点悬停的功能，通过挂载摄像头，可以使该四轴无人机进行高空拍摄。

6.2 展望

随着科技的进步，技术人才的不断增多，在未来的无人机技术中，飞控技术将会保持核心地位，更优的算法将不断提升无人机的飞行性能。无人机也会变得更加智能，操作更加简单，安全性会更加安全。同时随着能源技术的不断发展，无人机的续航能力将不会受限，无人机将会给生活带来更多的便利。
对于本设计，虽然能完成稳定飞行的要求，但有些地方需要值得改善。有待完善的主要有以下方面。
1、姿态解算以及数据滤波器设计上选择更优的算法，使该无人机数据输出更加稳定，实时性更好。
2、控制算法采用ADRC算法进行控制，使无人机在不受外界因素上，飞行控制更加稳定。
3、机械设计上采用炭纤维板进行机身结构的设计，可以使无人机机身更轻，续航更久。
4、增加定点悬停，GPS导航等功能，使本设计功能更完善。

参考文献

附录

附录1-1：巴特沃斯滤波器程序及卡尔曼滤波程序设计

//定义巴特沃斯滤波器结构体
typedef struct
{
 float Input_Butter[3];
 float Output_Butter[3];
}Butter_BufferData;
typedef struct
{
 const float a[3];
 const float b[3];
}Butter_Parameter;
//-----Butterworth系数变量-----//
Butter_Parameter Butter_20HZ_Parameter_Acce={//200hz---20hz
    1,    -1.14298050254,   0.4128015980962,
  0.06745527388907,   0.1349105477781,  0.06745527388907
};
float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter) //低通滤波器
{/* 获取最新x(n) */
	        static int LPB_Cnt=0;
        Buffer->Input_Butter[2]=curr_input;
        if(LPB_Cnt>=100)
        {	/* Butterworth滤波 */
        Buffer->Output_Butter[2]=
         Parameter->b[0] * Buffer->Input_Butter[2]
        +Parameter->b[1] * Buffer->Input_Butter[1]
		+Parameter->b[2] * Buffer->Input_Butter[0]
        -Parameter->a[1] * Buffer->Output_Butter[1]
        -Parameter->a[2] * Buffer->Output_Butter[0];
        }
        else{
          Buffer->Output_Butter[2]=Buffer->Input_Butter[2];
          LPB_Cnt++;
        }
	/* x(n) 序列保存 */
        Buffer->Input_Butter[0]=Buffer->Input_Butter[1];
        Buffer->Input_Butter[1]=Buffer->Input_Butter[2];
	/* y(n) 序列保存 */
        Buffer->Output_Butter[0]=Buffer->Output_Butter[1];
        Buffer->Output_Butter[1]=Buffer->Output_Butter[2];
        return Buffer->Output_Butter[2];
}
//定义卡尔曼结构体变量
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
}KFP；//Kalman Filter parameter
//2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height={0.02,0,0,0,0.001,0.543};
 float kalmanFilter(KFP *kfp,float input)
 {//预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
     kfp->Kg = kfp->Now_P / (kfp->NOw_P + kfp->R);
     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 为下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out；
 }

附录1-2：姿态解算程序设计

#include "IMU.h"
#include "math.h"
#include "mpu6050.h"
#include "delay.h"

_st_IMU IMU;

static float invSqrt(float x) //快速计算 1/Sqrt(x) 
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f375a86 - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

#define Kp 0.5f //比例增益控制加速度计，磁力计的收敛速率
#define Ki 0.001f //积分增益控制陀螺偏差的收敛速度

float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;     // quaternion elements representing the estimated orientation
float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;    // scaled integral error

float yaw_control;

void IMU_Update(float ax,float ay,float az,float gx,float gy,float gz)
{
	float vx, vy, vz;
	float ex, ey, ez;
	float norm;
	
	float q0q0 = q0*q0;
	float q0q1 = q0*q1;
	float q0q2 = q0*q2;
	float q0q3 = q0*q3;
	float q1q1 = q1*q1;
	float q1q2 = q1*q2;
	float q1q3 = q1*q3;
	float q2q2 = q2*q2;
	float q2q3 = q2*q3;
	float q3q3 = q3*q3;
	
	float CNTLCYCLE;
	{
		static	float last_time;
		float now_time;
		now_time = micros();
		CNTLCYCLE = now_time - last_time;
		CNTLCYCLE /=1000;
		last_time = now_time;
	}	
	
	/* 转换为弧度制，用于姿态更新*/
	gx = gx * Gyro_Gr;
	gy = gy * Gyro_Gr;
	gz = gz * Gyro_Gr;

	if(ax*ay*az==0) 	return;

	//加速度计测量的重力向量(机体坐标系) 
	norm = invSqrt(ax*ax + ay*ay + az*az); 
	ax = ax * norm;
	ay = ay * norm;
	az = az * norm;

	//陀螺仪积分估计重力向量(机体坐标系) 
	vx = 2*(q1q3 - q0q2);												
	vy = 2*(q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3 ;

	//测量的重力向量与估算的重力向量差积求出向量间的误差 
	ex = (ay*vz - az*vy);                   
	ey = (az*vx - ax*vz);
	ez = (ax*vy - ay*vx);

	if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
	{
		//用上面求出误差进行积分
		exInt = exInt + ex * Ki;								 
		eyInt = eyInt + ey * Ki;
		ezInt = ezInt + ez * Ki;

		//将误差PI后补偿到陀螺仪
		gx = gx + Kp*ex + exInt;					   		  	
		gy = gy + Kp*ey + eyInt;
		gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
	}
///* 补偿由四元数微分方程引入的姿态误差 */
//	/* 将四元数姿态导数积分,得到当前四元数姿态 */
//	/* 二阶毕卡求解微分方程 */
	float delta;
	delta = (CNTLCYCLE * gx) * (CNTLCYCLE * gx) + (CNTLCYCLE * gy) * (CNTLCYCLE * gy) + (CNTLCYCLE * gz) * (CNTLCYCLE * gz);
	q0 = (1.0f - delta / 8.0f) * q0 + (-q1*gx - q2*gy - q3*gz)*CNTLCYCLE;;
	q1 = (1.0f - delta / 8.0f) * q1 + (q0*gx + q2*gz - q3*gy)*CNTLCYCLE;
	q2 = (1.0f - delta / 8.0f) * q2 + (q0*gy - q1*gz + q3*gx)*CNTLCYCLE;
	q3 = (1.0f - delta / 8.0f) * q3 + (q0*gz + q1*gy - q2*gx)*CNTLCYCLE;

	//单位化四元数 
	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 * norm;
	q1 = q1 * norm;
	q2 = q2 * norm;  
	q3 = q3 * norm;

	IMU.pitch= asin(2 * q2q3 + 2 * q0q1)* RtA;								// Pitch
	IMU.roll= atan2(-2 * q1q3 + 2 * q0q2, -2 * q1q1 - 2 * q2q2 + 1) * RtA;	// Roll
//	IMU.yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * RtA;		//Yaw
	
	/*偏航角一阶互补*/       
	float yaw_G = gz * Gyro_G;
	if((yaw_G > 2.0f) || (yaw_G < -2.0f)) //数据太小可以认为是干扰，不是偏航动作
	{
		IMU.yaw +=gz * Gyro_G*CNTLCYCLE;
	}
	if((IMU.yaw_mag>90 && IMU.yaw<-90)|| (IMU.yaw_mag<-90 && IMU.yaw>90))
		IMU.yaw = -IMU.yaw * 0.98f + IMU.yaw_mag * 0.02f;
	else IMU.yaw = IMU.yaw * 0.98f + IMU.yaw_mag * 0.02f;				

	if(IMU.yaw<0)   yaw_control = IMU.yaw + 360; //如果小于0，则+360		
	else yaw_control = IMU.yaw;
	
	//倾斜角度的三角函数值
	IMU.Cos_Pitch = cos(IMU.pitch*AtR);
	IMU.Sin_Pitch = sin(IMU.pitch*AtR);
	IMU.Cos_Roll = cos(IMU.roll*AtR);
	IMU.Sin_Roll = sin(IMU.roll*AtR);
	IMU.Cos_Yaw = cos(IMU.yaw*AtR);
	IMU.Sin_Yaw = sin(IMU.yaw*AtR);	
}

附录1-3：PID程序设计
略