目 录
1前言 1
11背景意义 1
12国外研究现状 1
13文工作 2
2总体方案设计 3
21方案较 3
22方案证选择 3
3飞行器原理结构 5
31飞行器原理 5
32飞行器结构 6
4单元模块设计 8
41单元模块功介绍电路设计 8
411电源 8
412 STM32F407系统 9
413 载电路 11
414 飞控姿态模块 11
415 刷电机连接电路 12
416 串口接口电路 12
42特殊器件介绍 12
421 线数传模块 12
422 飞控姿态模块 13
5软件设计 16
51软件设计原理设计工具 16
52软件设计流程框图说明 17
521串口中断流程图 17
522外部中断流程图 18
523程序流程图 18
6系统调试 20
61 通信系统 20
62 姿态传感器调试 21
621 传感器数分析处理 21
622 姿态解算 23
623 数中断 28
63 PID调试 30
631 PID姿态控制 30
632 飞控系统PID调试 33
7系统功指标参数 36
71系统实现功 36
72系统指标参数 36
8结 38
81 回顾 38
82 展 38
9总结体会 39
10谢辞 40
11参考文献 41
附录： 42
1硬件电路图 42
2PCB图 43
3部分程序 44
4外文翻译 46
1前言
11背景意义
年益现代控制理电子控制技术发展四轴飞行器广泛关注民工业领域具广泛应前景甚机战争中广泛应机发展火热中四旋翼飞行器发展突出四旋翼飞行器具特点：
(1)体积巧工作恶劣危害类健康生命环境中限度减少员伤亡飞行器全天工作需休息工作效率高
(2)支持配备高端电子产品种外设相连接相机机械臂等实现娱乐功
例高空电力线巡检中机工作员操控进行工作代工巡检象实施接检测减少工劳动强度携带传感仪器摄机等巡检象进行数收集分析存储进步提高巡检工作效率巡检精度军事局部规模战时候普通侦察机受敌方击造成必机体员伤亡机起代作利四旋翼飞行器作侦察机具振动噪声性高成低反侦察力强销毁等优势机军事价值估量四旋翼飞行器着更广阔前景等着开发通飞行器添加更加智算法实现机互动飞行器帮取物件等
目前四旋翼飞行器已许领域运总体言旧处初步发展阶段
12国外研究现状
国际较知名飞行器公司中国疆创新美国3D Robotics作全球机领域领头羊两家公司机技术发展速度迅猛形容疆公司新精灵4机实物图图11示
图11 精灵4实物图
精灵4具功：
1 感知前方障碍物动绕行
2 通视觉识动拍移动物体
3 点击相机画面指点方飞行
4 智返航感知障碍物动提升飞行高度
5 飞行时间28分钟控距离约5公里
6 高速度提升20ms (72kmh)
7 体化云台设计提升飞行影稳定性
8 利全新视觉定位技术实现室外精准定位
：3D Robotics公司新出品PIXHAWK飞控拥出色飞行稳定性搭载双传感器系统冗余电源输入扩展两组GPS系统确保飞行失误降低两家公司占领全球部分机市场技术高门槛垄断机公司已远远甩面部分机公司技术仅仅停留稳定飞行简单航拍户外GPS定位阶段
13文工作
机作电子产业里冉冉升起新星具广阔市场发展前景作代学生仅应时代潮流更作时代弄潮信心勇气四旋翼机涉知识面广泛中核心知识专业契合度相高颗功强劲微机芯片造功完备飞行控制系统恰恰微机原理单片机运流机控制系统离开动控制原理目前机电力行业应力度综毕业设计选择基STM32 ARM单片机四旋翼飞控系统
受限知识水文解决问题：
第章节：四旋翼飞行器研究背景意义国外发展现状
第二章节：四旋翼飞行器方案讨选择选择基STM32 ARM单片机四旋翼飞行控制系统
第三章节：简单叙述飞行器飞行原理机械结构
第四章节：飞行控制系统硬件设计中包括单片机系统类模块接口等
第五章节：程序流程图程序流程进行简单说明
第六章节：飞行器控制系统设计包括传感器数进行分析处理DMP姿态解算方式配置中断实时更新前飞行姿态数四旋翼飞行器PID调试策略
第七章节：位机飞行器间通信系统PID调试程
2总体方案设计
21方案较
方案：基意利开源硬件Arduino Nano作数处理姿态结算飞行控制系统飞行控制系统结构图21示
图21 Arduino飞控系统结构图
方案二：采意法半导体STM32F407VET6作飞行器控芯片飞行控制系统结构图22示
图22 基STM32飞控系统结构图
22方案证选择
方案：Arduino Nano基Atmega328P AVR单片机开源硬件具两外部中断口输出六路PWM波兼备IICUARTSPI通信功总说具丰富片资源优秀性编程Arduino C种C语言类似标准C针Arduino系统做量简化工作提供许函数库文件通性高Arduino然已开源飞行控制系统命令线遥控器发出会额外增加遥控器费外Arduino通信协议未知利增添代码调试工作
方案二：意法半导体STM32F407VET6CORTEXM4核属32位ARM微控制器常编程软件KEILIAR编程语言汇编标准C语言C++等灵活方便友编译方式提高代码通性移植性STM32F407VET6具卓越性输AVRAtmega328P里采位机传输命令飞行控制系统简化系统调试方式
考虑资金性价难易程度选择方案二基STM32F407VET6 单片机飞行控制系统
3飞行器原理结构
31飞行器原理
四轴飞行器具两种飞行模式： X 型十字型X型飞行方式四旋翼飞行器姿态改变方机身成45度角十字型飞行方式四旋翼飞行器姿态改变方飞行器机身相采X型飞行方式飞行器具更控制灵敏度稳定性选择X型飞行方式两种工作模式图31示
Y
X
Y
X
图31 X 型（左）十字型（右）飞行方式图
四旋翼飞行器四螺旋桨电机直连结构通改变电机转速获旋转机身力调整身姿态飞行器飞行程中螺旋桨会产生两力升力螺旋桨转相反反扭矩反扭矩会飞行器着螺旋桨旋转方旋果抵消反扭矩会飞行器直转会影响飞行器飞行四旋翼飞行器通分配四螺旋桨转抵消螺旋桨产生反扭矩X 型飞行方式例时针方电机编号右角电机1号电机次编号123412号螺旋桨轴方定义X轴方机头 34号螺旋桨轴方定义Y轴方抵消螺旋桨反扭矩13号螺旋桨需时针转动24号螺旋桨需逆时针转动角线螺旋角旋转方相抵消相互间反扭矩四旋翼飞行器飞行方速度飞行器倾角决定飞行器倾斜角度越飞行速度越快通调节电机转速达控制飞行器姿态速度甚飞行路径效果中四旋翼飞行器飞行姿态高度俯仰角（Pitch）横滚角（Roll）偏航角（Yaw）继续细分：升降前倾倾左倾右倾左旋右旋
(1) 高度：四螺旋桨转速相时加速时螺旋桨升力变升力飞行器重力时飞行器拥运动加速度飞行器升四螺旋桨时减速时螺旋桨产生升力变升力飞行器重力时飞行器拥运动加速度飞行器降
(2) Pitch：绕着Y轴方旋转进行控制12号电机转述等减34号电机转述等增飞行器前倾反会倾
(3)Roll：俯仰控制相似横滚绕着X轴方旋转14号电机转述等减23机转述等增加产生右倾反会左倾
(4)Yaw：理飞行器绕着Z轴旋转13号电机转述等减反扭矩升力减24号电机转述等增加反扭矩升力增加反扭矩出现衡会飞行器右转反会飞行器左转
32飞行器结构
采机架型号 F360轴距360mm螺旋桨型号1047型电机采朗宇X2212980KV刷电机加1V电压电机分钟980转电机转速约10878转＼分通常四旋翼飞行器配2200mah电池电调盈天行者30A电子调速器整飞行系统锂电池供电电机实物图图32示电子调速器实物图图33示
图32 朗宇电机实物图
图33 电子调速器实物图
飞行器组装完成图34示
图34 飞行器实物图
4单元模块设计
41单元模块功介绍电路设计
飞行控制系统硬件结构遵方案二结构框图图22重单元模块做介绍
411电源
LM1117低压差电压调节器压差输出12V时负载电流800mA国家半导体工业标准器件LM317相脚排列LM1117调电压版通2外部电阻实现125～138V输出电压范围外5固定电压输出（18V25V285V33V5V）型号设计飞行器控芯片33V选取固定输出33VLM1117LM1117提供电流限制热保护输出电压精度±1LM1117系列具LLPTO263SOT223TO220TO252DPAK等种封装飞行器提高续航力量减轻身重量里选取SOT223封装电路设计输出端需联少10uF钽电容改善瞬态响应稳定性LM1117电气特性表41示
表41 电气特性表LM1117
符号
参数
测试条件
值
典型值
值
单位
VREF
基准
电压
LM1117ADJ
IOUT10mA VINVOUT2V TJ25˚C
10mA≤IOUT≤800mA
14V≤VINVOUT≤10V
1238
1225
1250
1250
1262
1270
V
V
VOUT
输
出
电
压
LM111718
IOUT10mA VIN38V TJ25˚C
0≤IOUT≤800mA 32V≤VIN≤10V
1782
1746
1800
1800
1818
1854
V
V
LM111725
IOUT10mA VIN45V TJ25˚C
0≤IOUT≤800mA 39V≤VIN≤10V
2475
2450
2500
2500
2525
2550
V
V
LM1117285
IOUT10mA VIN485V TJ25˚C
0≤IOUT≤800mA 425V≤VIN≤10V
0≤IOUT≤500mA VIN410V
2820
2790
2790
2850
2850
2850
2880
2910
2910
V
V
V
LM111733
IOUT10mA VIN5V TJ25˚C
0≤IOUT≤800mA 475V≤VIN≤10V
3267
3235
3300
3300
3333
3365
V
V
∆VOUT
压差
LM111733
IOUT0mA 475V≤VIN≤15V
1
6
mV
电源模块原理图图41示图里5V电源源电子调速器BEC降压系统
图41 电源模块电路图
412 STM32F407系统
STM32F407VET6意法半导体基CORTEXM4核芯片STM32F407拥资源包括：集成FPUDSP指令具192KBSRAM1024KB FLASH1216位定时器232位定时器2DMA控制器（16通道）3SPI2全双工I2S3IIC6串口2USB（支持 HOST SLAVE）2CAN3 12位ADC212位DAC1RTC（带日历功）1SDIO 接口1FSMC接口110100M太网MAC控制器1摄头接口1硬件机数生成器112通IO口等该芯片配置十分强悍具卓越性相STM32F1说许功进行重改进STM32系统电路图图42示
图42 STM32系统电路图
STM32电复位默认部晶振（精度8MHz左右）飞行控制板稳定工作里选择外接8MHz晶振程序里切换外部8MHz晶振通芯片带锁相环PLL倍频168MHz需修改系统时钟配置System_stm32f4xxc文件PLL第级分频系数M修改8样时钟频率达168MHz
STM32F40xx里通BOOT[10]引脚选择三种启动模式般选闪存存储器启动启动模式表42示
表42 启动模式STM32
启动模式选择引脚
启动模式
说明
BOOT1
BOOT0
X
0
闪存存储器
闪存存储器选启动区域
0
1
系统存储器
系统存储器选启动区域
1
1
置SRAM
置SRAM选启动区域
根选定启动模式闪存存储器系统存储器SRAM方式访问：
● 闪存存储器启动：闪存存储器映射启动空间(0x0000 0000)然够原址(0x0800 0000)访问闪存存储器容两址区域访问 0x000000000x0800 0000
● 系统存储器启动：系统存储器映射启动空间(0x0000 0000)然够原址(互联型产品原址0x1FFF B000产品原址0x1FFF F000)访问
● 置SRAM启动：0x2000 0000开始址区访问SRAM
注意：置SRAM启动应程序初始化代码中必须NVIC异常表偏移寄存器新映射量表SRAM中
413 载电路
果采串口烧录程序必须先配置BOOT01BOOT10然复位键通程序载代码载完BOOT0置GND复位运行代码里采jlink（SWD）载方式需接：jlink第1脚（VDD）第7脚（TMSSWDIO应STM32PA13）第9脚（TCKSWCLK应STM32PA14）第468101214161820中意脚（脚）第15脚（RESET应STM32NRST）SW模式载电路图43示
图43 SW模式载电路图
414 飞控姿态模块
性良飞控传感器飞行器稳定飞行重保证GY86模块集成陀螺仪加速度计磁力计气压计适合飞行器GY86模块飞行控制系统连接电路图图44示
图44 GY86模块连接电路图
415 刷电机连接电路
刷电机PWM波驱动PWM波通电子调速器输送刷电机电子调速器3接口VCCGNDMx电路板需出连接口图45示
图45 刷电机接口电路图
416 串口接口电路
图46示单片机串口引出作连接线数传模块收发位机命令余串口接口外扩模块超声波模块GPS
图46 串口接口电路图
42特殊器件介绍
421 线数传模块
CC1101线数传模块体积巧尺寸仅145*28CM操作简单工作频段433MHZ穿透力强直接51AVRSTM8STM32MSP430PIC等单片机串口相连接实物图图47示
图47 线数传模块图
CC1101线数传模块STM32单片机串口连接串口实现远距离线收发数样处减少调试难度果线数传模块话购NRF24L01通信模块会增添代码量加调试难度
422 飞控姿态模块
选择GY86模块原集成度高实物图图48示
图48 GY86模块实物图
模块MPU6050InvenSense公司推出全球首款整合性6轴运动处理组件相较组件方案免组合陀螺仪加速器时轴间差问题减少安装空间
MPU6050部整合3轴陀螺仪3轴加速度传感器含第二IIC接口连接外部传感器利数字运动处理器（DMP Digital Motion Processor）硬件加速引擎通IIC接口应端输出完整9轴融合演算数DMPInvenSense公司提供运动处理资料库实现姿态解算降低运动处理运算操作系统负荷时降低开发难度MPU6050芯片实物图图49示
图49 MPU6050芯片实物图
MPU6050特点包括：
① 数字形式输出6轴9轴（需外接磁传感器）旋转矩阵四元数(quaternion)欧拉角格式(Euler Angle forma)融合演算数（需DMP支持）
② 具131 LSBs°sec敏感度全格感测范围±250±500±1000 ±2000°sec3轴角速度感测器(陀螺仪)
③ 集成程序控制范围±2g±4g±8g ±16g 3轴加速度传感器
④ 移加速器陀螺仪轴间敏感度降低设定予影响感测器飘移
⑤ 带数字运动处理(DMP Digital Motion Processing)引擎减少MCU复杂融合演算数感测器步化姿势感应等负荷
⑥ 建运作时间偏差磁力感测器校正演算技术免客户须外进行校正需求
⑦ 带数字温度传感器
⑧ 带数字输入步引脚(Sync pin)支持视频电子影相稳定技术GPS
⑨ 程序控制中断(interrupt)支持姿势识摇摄画面放缩滚动快速降中断highG中断零动作感应触击感应摇动感应功
⑩ VDD供电电压25V±530V±533V±5VLOGIC低18V±5
⑪ 陀螺仪工作电流：5mA陀螺仪机电流：5uA加速器工作电流：500uA加速器省电模式电流：40uA@10Hz
⑫ 带1024字节FIFO助降低系统功耗
⑬ 高达400KhzIIC通信接口
⑭ 超封装尺寸：4x4x09mm（QFN）
MPU6050芯片工作电路图图410示
图410 MPU6050芯片工作电路图
MPU6050采集飞行器飞行时XYZ三轴加速度偏转角速度具体讲陀螺仪测量物理量偏转倾斜时转动角速度坐标轴线加速度
MPU6050部通MEMS工艺做出参考坐标系芯片着飞行器动作时芯片坐标相参考坐标发生旋转时芯片会读出差异芯片部处理会输出绕坐标轴旋转角速度MPU6050芯片坐标轴分布图411示
图411 MPU6050芯片坐标轴分布图
关加速度测量实物体加速程中作物体力里需注意球物体会受球重力芯片例外XYZ坐标轴测加速度包含重力三坐标轴重力加速度分量
5软件设计
51软件设计原理设计工具
位机采Lab windows制作Lab windows美国国家仪器公司推出交互式C语言开发台编写检测系统数采集系统等应软件数采集分析显示软件界面图51示
图51 Lab Windows界面图
STM32F407芯片C语言软件开发系统KEIL开发KEIL提供包括C编译器功强仿真调试等完整开发方案操作界面图52示
图52 KEIL操作界面图
52软件设计流程框图说明
521串口中断流程图
线数传模块连接串口串口中断发生时说明接收位机命令需处理串口数串口中断流程图图53示
进入串口中断
接收中断数
处理数
串口标志位置1
结束
图53 串口中断流程图
522外部中断流程图
GY86飞控姿态模块数绪中断连接单片机外部中断脚STM32F407接收外部中断时说明组飞行器姿态数已绪需进行姿态解算外部中断流程图图54示
进入外部中断
读取模块数
姿态解算
DMP标志位置1
结束
图54 外部中断流程图
523程序流程图
程序里进行系列初始化进入while循环循环里做通信数处理防止飞行器飞行角度导致飞行器侧翻问题次调整PWM波占空调节刷电机转速程序流程图图55示
开始
初始化飞控板硬件设置
通信初始化
中断初始化
驱动初始化
···
飞控系统初始化
初始化系统参数
初始化传感器
通信数处理
串级PID控制
调节电机PWM波输出
图55 程序流程图
根程序流程图底层驱动部分：
(1) USART1：外接扩展传感器模块
(2) USART2：调试程序发送接收位机命令通信
(3) IIC：读取 GY86 模块值处GPIO模拟IIC
(4) GPIO：控制LED通闪烁亮灭提示飞行器运行状态
(5) SYSTICK：滴答定时器系统精确定时延时
(6) TIM定时器PWM通道：TIM3TIM4PWM功输出4路占空PWM波电子调速器控制刷电机转动
(7) 外部中断：利单片机外部中断时获取前飞行姿态
(8) 独立门狗：溢出时间2s防止系统死机停止工作
6系统调试
61 通信系统
飞行器般控制系统带四螺旋桨电机转速高距离调试意味着伴着安全隐患需构建通信系统飞行器进行远距离调试
位机飞行器实时控制位机发送飞控系统数想加减油门飞行器飞调试时候需时更改PID参数位机发送命令飞行控制系统需发送数位机前四电机转速前飞行器姿态正PID参数保证通信准确性时性数传输编写通信协议必位机图61示
图61 位机界面图
PID参数数协议传递PID参数先放1000倍进行传输保证种类型数正确误数末尾增加校验SUM飞控系统接收RCDATA会单片机里求结果位机发送SUM做两者相等时数会真正保存飞控系统接收重数时飞控系统会返回模样数位机表示已正确收重数增加飞控系统安全性
62 姿态传感器调试
621 传感器数分析处理
四旋翼飞行器保持稳状态关键正确姿态传感器数进行处理首先通操作MPU6050寄存器获飞行时线加速度转动角速度原始值加速度计测量值存储寄存器表61示
表61 加速度计测量值存储寄存器
Register(Hex)
Register(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
3B
59
ACCEL_XOUT[158]
3C
60
ACCEL_XOUT[70]
3D
61
ACCEL_YOUT[158]
3E
62
ACCEL_YOUT[70]
3F
63
ACCEL_ZOUT[158]
40
64
ACCEL_ZOUT[70]
16位加速度计测量值满量程定义MPU6050CCEL_FS寄存器（寄存器28） 满量程设置加速度计测量值灵敏度低分辨率（LSB）表62示：
表62 加速度计灵敏度低分辨率
AFS_SEL
Full Scale Range
LSB Sensitivity
0
±2g
16384 LSBg
1
±4g
8192 LSBg
2
±8g
4096 LSBg
3
±16g
2048 LSBg
陀螺仪测量值存储寄存器表63示
表63 陀螺仪测量值存储寄存器
Register(Hex)
Register(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
43
67
GYRO_XOUT[158]
44
68
GYRO_XOUT[70]
45
69
GYRO_YOUT[158]
46
70
GYRO_YOUT[70]
47
71
GYRO_ZOUT[158]
48
72
GYRO_ZOUT[70]
16位陀螺仪测量值满量程定义MPU6050FS_SEL寄存器（寄存器 27）满量程设置陀螺仪测量值灵敏度低分辨率（LSB）表64示
表64 陀螺仪灵敏度低分辨率
FS_SEL
Full Scale Range
LSB Sensitivity
0
±250°s
131 LSB°s
1
±500°s
655 LSB°s
2
±1000°s
328 LSB°s
3
±2000°s
164 LSB°s
MPU6050数int16型数范围32768+32767正数032767应16进制07FFF负数32768 1应16进制8000FFFF
MPU6050言数FFFF原码80011数FF00原码8100256
先单片机接收MPU6050原始数数应分辨率数加速度值单位转换g角速度单位转换°sec采集数取100次均值ACCEL_OUT放100倍全部数发送串口助手6次样便观察分析具体数值表6566示
表65 静止时MPU6050原始数
状态
GYRO_X
GYRO_Y
GYRO_Z
ACCEL_X
ACCEL_Y
ACCEL_Z
MPU6050静止Z轴垂直
1
0
0
1
0
100
1
0
0
1
0
100
1
0
0
1
0
100
1
1
0
1
0
99
1
0
0
1
0
99
1
0
0
1
0
100
MPU6050静止X轴垂直
1
0
0
100
0
3
1
1
0
100
0
4
1
0
0
100
0
4
1
1
0
100
0
4
1
0
0
100
0
3
1
0
0
100
0
4
MPU6050静止Y轴垂直
1
0
0
2
99
3
1
0
0
2
100
3
1
0
0
2
100
4
1
0
0
1
100
4
1
0
0
2
99
4
1
0
0
1
99
4
分析表格知静止时：XYZ三轴角速度静止0°sec加速度重力原竖直轴100（放100倍方便观察）1g竖直坐标轴1g水坐标轴加速度0g
表66 运动时MPU6050原始数
状态
GYRO_X
GYRO_Y
GYRO_Z
ACCEL_X
ACCEL_Y
ACCEL_Z
绕Y轴
时针
转动
1
35
0
37
0
90
1
28
0
42
0
88
1
26
0
47
0
85
1
21
0
50
0
83
绕Y轴
逆时针
转动
1
21
0
22
0
96
1
20
0
25
0
95
1
20
0
29
0
94
1
20
0
33
0
92
绕X轴
时针
转动
9
0
0
0
22
96
8
0
0
0
24
95
6
0
0
0
26
94
5
0
0
0
27
95
绕X轴
逆时针
转动
15
0
1
1
15
96
14
0
1
0
18
97
13
0
1
1
20
96
13
0
1
1
22
95
绕Z轴
时针
转动
1
1
17
0
1
98
1
1
16
0
0
97
1
1
13
0
1
99
1
1
14
1
2
98
绕Z轴
逆时针
转动
4
3
32
0
1
99
4
2
29
0
0
98
4
3
27
0
0
100
2
0
27
0
0
96
分析表格知传感器绕Y轴转动时：XZ轴角速度静止0°secY轴瞬时针转动角速度正逆时针转动角速度负加速度转动原Y轴加速度绝值减X轴加速度绝值增水Y坐标轴加速度0g传感器绕X轴转动时情况绕Y轴转动类似
传感器绕Z轴转动时 XY轴角速度静止0°secZ轴瞬时针转动角速度正逆时针转动角速度负XY轴水方转动Z轴直竖直XYZ轴加速度变XY轴加速度0
622 姿态解算
3飞行器原理里分析四旋翼飞行器飞行方速度飞行器倾角决定飞行器倾斜角度越飞行速度越快期姿态数欧拉角：航角（yaw）横滚角（roll）俯仰角（pitch）三角前四轴姿态想结果XYZ三轴加速度者角速度
否加速度计算姿态角呢？加速度坐标模型图62示
图62 加速度坐标模型图
知道飞行器受重力场时x^2+y^2+z^2 1g 量R加速度计测量（引力场惯性力）力矢量RxRyRz分RXYZ轴矢量投影面关系式：R^2 RX^2 + RY^2 + RZ^2
感兴趣量R XYZ轴间角度定义 AxrAyrAzr模型图图63示R Rx 组成直角三角形
图63 重力分量坐标模型图
里容易推导出面公式重力量轴间夹角姿态角
COS（Axr） RX R 类推：
COS（Ayr） RY R
COS（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矢量模（长度）时候惯性矢量方真正矢量进行规范化简化程序运算意义
水放置MPU6050Z轴感受重力量输出1g应ADC值16384 (2g量程)时R重力量Rx0 Ry0 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区分真正需加速度）干扰叠加应该测量重力加速度信号输出数法准确反映飞行器倾角图64示
图64 单轴加速度测量值曲线图
图中实线表示MPU6050单轴测加速度变化量虚线表示重力加速度该轴分量出干扰叠加重力加速度分量信号波动噪声通数滑滤波滤滑滤波方面会信号法实时反映飞行器倾角变化进减缓程序飞行器控制方面会飞行器角速度变化信息滤掉述两方面滤波效果飞行器保持衡难度加
角速度言会受飞行器运动影响该数中噪声转动角速度进行积分姿态角样进步滑信号角度信号更加稳定存问题陀螺仪角速度获角度信息需积分运算果角速度信号存微偏差漂移积分运算变化形成积累误差误差会着时间延长逐步增加终导致法形成正确角度信号图65示
图65 角速度积分漂移现象曲线图
图中实线表示角速度积分角度值虚线表示真实角度值结单单角速度角度法精确反应飞行器姿态消累积误差呢？ 种方法通面加速度获角度信息进行校正通积分角度重力加速度角度间偏差改变陀螺仪输出积分角度逐步踪加速度传感器角度
种方法利MPU6050数字运动处理器DMP首先需MPU6050原始数（线加速度转动角速度）然调数字运动处理器DMP数进行简单处理导入DMPDMP部姿态算法处理会输出组四元数四元数实旋转种表示方式里做详细介绍DMP输出四元数q30格式浮点数放2 30次方倍必须先转换浮点数230次方计算公式：
q0 quat[0] q30 q3010737418240f
q1 quat[1] q30
q2 quat[2] q30
q3 quat[3] q30
四元数需根四元数欧拉角转换公式计算出欧拉角yawrollpitch公式：
pitch asin(2 * q1 * q3 + 2 * q0* q2)* 573
roll atan2(2 * q2 * q3 + 2 * q0 * q1 2 * q1 * q1 2 * q2* q2 + 1)* 573
yaw atan2(2*(q1*q2 + q0*q3)q0*q0+q1*q1q2*q2q3*q3) * 573
样杜邦线GY86模块（模块载MPU6050芯片）接入单片机先单片机接收MPU6050原始数数做系列处理送入DMP根两公式DMP数做进步处理计算出欧拉角yawrollpitch显示TFT屏幕方便观察分析模块连接图图66示
图66 模块连接图
GY86模块倾角摆放具体表67示
表67 DMP测试
说明
GY86模块状态
TFT显示
X坐标轴roll绕动
Y坐标轴pitch绕动
Z坐标轴yaw绕动
GY86三角板30角X轴行斜边绕Y轴逆时针转动pitch30度
X坐标轴roll绕动
Y坐标轴pitch绕动
Z坐标轴yaw绕动
GY86三角板60角Y轴行斜边绕X轴时针转动roll60度
X坐标轴roll绕动
Y坐标轴pitch绕动
Z坐标轴yaw绕动
GY86三角板30角Z轴垂直斜边绕Z轴时针转动yaw30度
表67 DMP测试里出利DMP进行姿态解算准确出飞行器欧拉角
623 数中断
然通DMP模式获取正确欧拉角应该什时候读取DMP四元数呢？DMP模式数存储MPU6050里面FIFOFIFO里面存包括GYROACC四元数等参数DMP里面数FIFO读取 DMP库里做法：FIFO里数半满时会发出洗FIFO指令FIFO数足时会延时等果读取早获取次数DMP正处理需延时等果读取晚直观影响飞行器姿态调节缓慢法正常飞行意读取数者读取时机选择时（太快太慢）会出现数连续
解决问题MPU6050芯片数绪中断模式利单片机外部中断MPU6050数绪时立刻获飞行器姿态
MPU6050数绪中断模式需配置寄存器表68610示
表68 寄存器58中断标志寄存器
Register
(Hex)
Register
(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
3A
58
MOT_INT
FIFO
_OFLOW
_INT
I2C
_MST
_INT
DATA
_RDY
_INT
寄存器展示中断源中断标志位读取清寄存器需注意DATA_RDY_INT位数绪中断（Data Ready interrupt）产生该位动置 1读取寄存器该位清 0
表69 寄存器55中断配置寄存器
Register
(Hex)
Register
(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
37
55
INT_
LEVEL
INT_
OPEN
LATCH
_INT
_EN
INT_RD
_CLEAR
FSYNC
_INT
_LEVEL
FSYNC
_INT
_EN
I2C
_BYPASS
_EN
寄存器配置 INT引脚中断信号产生
INT_LEVEL该位等0 INT引脚逻辑电高电该位等1INT引脚逻辑电低电
LATCH_INT_EN该位等0INT引脚产生50us脉该位等1INT引脚保持效电直中断清
INT_RD_CLEAR该位等0读取 INT_STATUS (寄存器58) 会导致中断状态位清
INT_RD_CLEAR该位等1读取动作会导致中断状态位清
分析程序里样配置：
INT_LEVEL等1INT引脚逻辑电低电效
INT_OPEN该位等0the INTpin is configure das pushpull
LATCH_INT_EN等1INT引脚保持效电直中断清
INT_RD_CLEAR该位等1读取动作会导致中断状态位清
表610 寄存器56中断寄存器
Register
(Hex)
Register
(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
38
56
MOT_EN
FIFO
_OFLOW
_EN
I2C_MST
_INT_EN
DATA
_RDY_EN
该寄存器中断源中断产生
MOT_EN 该位置1该位运动检测（Motion detection）产生中断
FIFO_OFLOW_EN 该位置1该位FIFO缓区溢出产生中断
I2C_MST_INT_EN 该位置1该位 I2C机中断源产生中断
DATA_RDY_EN该位置1该位数绪中断（Data Ready interrupt）传感器寄存器写操作完成时会产生
寄存器需配置DATA_RDY_EN位
MPU6050数绪中断配置完成需配置单片机外部中断降触发
配置完成中断引脚连接示波器观察发现数绪中断周期10ms说明10ms数会更新次
63 PID调试
631 PID姿态控制
偏差例积分微分进行控制1（简称PID调节器称PID控制器）连续系统中技术成熟应广泛种调节器结构简单参数易调整长期应中已积累丰富验特工业程中控制象精确数学模型难建立系统参数常发生变化运控制理分析综合耗费代价预期效果采PID调节器根验进行线整定便满意控制效果PID框图图67示
积分
例
微分
r(t)
e(t)
u(t)
执行机构
象
电机
供电
c(t)
图67 PID框图
飞行控制系统数字PID控制算法P例环节I积分环节D微分环节控制处理器采样控制根采样时刻偏差值计算控制量进行离散式控制形式离散化PID程中令T采样周期k采样序号连续时间t离散时间表示求形式代连续时间积分形式增量形式代连续时间微分形式
(1)
(2)
(3)
（3）代入模拟PID计算表达式
(4)
中Kp例系数T1积分常数TD微分常数
离散PID表达式
(5)
式（5）中定义积分系数微分系数
3飞行器原理612姿态解算里分析控制四旋翼飞行器控制角度姿态角利PID算法控制四旋翼飞行器简单时容易想种控制策略角度单环PID控制器系统框图图67示：
控制信号
（期角度）
前姿态角（DMP）
PID角度单环控制器
刷
电机
电机
供电
传感器模块
图68 角度单环PID控制策略
根图68式（5）整理出单环PID控制程序思路：
前角度误差 期角度 前角度
单环PID_P项 Kp * 前角度误差
单环PID_I项 Ki * 前角度误差 + 次单环PID_I项
单环PID_D项 Kd * ( 前角度误差 次角度误差 )
单环PID输出 单环PID_P项 + 单环PID_I项 + 单环PID_D项
单环PID_输出转换单片机PWM波输出量调节前刷电机转速然实际飞行测试中发现角度单环PID控制策略效果理想需进步改进控制策略
述角度单环PID控制算法仅仅考虑飞行器角度信息需增加控制量线加速度转动角速度作MPU6050原始数两容易获取量相较线加速度角速度更够时体现出前飞行器运动情况增加飞行器稳定性(增加阻尼)增强系统快速响应选择控制飞行器角速度角度角速度串级PID控制算法应运生简言串级PID两PID控制算法套起两控制器控制飞行器会单控制器控制更变量飞行器适应力更强增强系统抗干扰性(增强稳定性)串级PID原理框图图69示：
控制信号
（期角度）
前姿态角（DMP）
PI
角度外环控制器
刷
电机
电机
供电
PID
角速度环控制器
传感器模块（角速度数）
图69 角度角速度串级PID控制策略
采控制品质更加优秀串级PID控制策略根图69式（5）整理出串级PID控制程序思路：
外环（角度环）：
前角度误差 期角度 前角度
外环PID_P项 Kp * 前角度误差
外环PID_I项 Ki * 前角度误差 + 次外环PID_I项
外环PID输出 外环PID_P项 + 外环PID_I项
环（角速度环）：
前角速度误差 外环PID输出 前角速度
环PID_P_rate项 Kp_rate *前角速度误差
环PID_I_rate项 Ki_rate *前角速度误差 + 次环PID_I_rate项
环PID_D_rate项 Kd_rate * ( 前角速度误差 次角速度误差 )
环PID_rate输出 环PID_P_rate项 + 环PID_I_rate项 + 环PID_D_rate项
里环PID_rate输出飞行器需调节身量前姿态修正期姿态控制姿态需通调节电机PWM波实现PWM波环PID_rate输出联系起构建公式
moto1throttlePID_PIT_RATEOUTPID_ROL_RATEOUTPID_YAW_RATEOUT
moto2throttlePID_PIT_RATEOUT+PID_ROL_RATEOUT+PID_YAW_RATEOUT
moto3throttle+PID_PIT_RATEOUT+PID_ROL_RATEOUTPID_YAW_RATEOUT
moto4throttle+PID_PIT_RATEOUTPID_ROL_RATEOUT+PID_YAW_RATEOUT
moto1MOTO_MxMi(moto117001105)
moto2MOTO_MxMi(moto217001105)
moto3MOTO_MxMi(moto317001105)
moto4MOTO_MxMi(moto417001105)
throttle电机值PID_PIT_RATEOUTPID_ROL_RATEOUTPID_YAW_RATEOUT环PID_rate输出moto1例moto1MOTO_MxMi(moto117001105)电机需限幅值1700值1105样防止电机转速moto值输送PWM波占空控制程序进控制电机
632 飞控系统PID调试
飞控系统调试进行PID调试组良PID参数飞行器稳起飞降落
调整PID参数时需飞行器起飞油门基础进行PID参数调整否飞行器油门调试时候稳定飞行程中会晃荡方便飞行器实物调试观察现象助工具万轮万轮飞行器拥高度具体图610示
图610 飞行器万轮调试
调整PID时遵循口诀：
先整定环整定外环
参数整定找佳序查
先例积分微分加
曲线振荡频繁例度盘放
曲线漂浮绕湾例度盘扳
曲线偏离回复慢积分时间降
曲线波动周期长积分时间加长
曲线振荡频率快先微分降
动差波动慢微分时间应加长
理想曲线两波前高低41
调整PID参数程：
串级PID调试中环参数关键理想环参数够舵（角速度控制模式舵）控制量具体情况概衡位置附（正负30度左右）舵量突加飞机快速响应舵量回中飞机立刻停止运动（没回弹震荡）
调节角速度环首先改变程序角度外环掉舵量作角速度环期值（角速度控制模式）
1先调节角速度环P参数果P参数太表现修正角速度误差感觉飞行器软倾斜难修正舵响应差果P参数太衡位置容易抖动舵回中干扰（手突加干扰）时会震荡合适P较舵进行响应太会震荡舵量回中会回弹停止（没D）
2然加角速度环D参数D参数加快舵响应作抑制舵量回中震荡谓立竿见影D会横滚俯仰混控制时表现出具体表现四轴抓手里推油门会抽搐
3加角速度环I参数立马感觉飞行器调节程变柔I参数表现：舵飞机偏离衡位置舵量回中飞机立刻停止转动没I太飞机会重力继续转动
4第四步增添角度外环参数P（里需修改程序改角度作控制信号）外环P加舵会应期角度理果P参数太舵灵敏P参数太舵回中易震荡合适舵反应速度良P参数
5加角度外环参数I 里参数I作修正静态误差飞行器稳定飞行时机身保持衡位置
万轮模拟调试基完成飞行器试飞程中需根飞行器表现微调PID参数直飞行器够稳起飞降落正常飞行
7系统功指标参数
71系统实现功
长达周系统PID参数调试飞行器实现稳起飞正常飞行稳降落功位机发送命令飞行器接收命令飞行器灵敏响应飞行情况图71示
图71 飞行器飞行情况
72系统指标参数
进行试飞调试前飞行器万轮模拟调试组PID参数表71示
表71 PID万轮模拟参数
角度PITCHP＼ROLLP35
角度PITCHI＼ROLLI5
角度PITCHD＼ROLLD0
速度PITCHP＼ROLLP180
速度PITCHI＼ROLLI6
速度PITCHD＼ROLLD8
速度YAWP80
速度YAWI80
速度YAWD0
油门值1350
参数表中出PitchRollPID三组参数分相等三方面原：
1 PitchRoll物理模型具相似性
2 表中PID参数实放1000倍程序里1000值
3 万轮模拟飞行调试相等方便观察调试现象
试飞期间导入万轮模拟PID参数然试飞程中通眼观察飞行器空中否保持稳幅度摆动根实际情况微调PID参数值断修正整定出组合理飞行PID参数表72示
表72 PID飞行参数
角度PITCHP50
角度PITCHI4
角度PITCHD0
角度 ROLLP48
角度 ROLLI4
角度 ROLLD0
速度PITCHP150
速度PITCHI6
速度PITCHD10
速度 ROLLP151
速度 ROLLI6
速度 ROLLD9
速度YAWP80
速度YAWI78
速度YAWD0
油门值1250
8结
81 回顾
受限知识水文做：
1简单分析四旋翼飞行器机械结构飞行器硬件软件底层做设计
2分析两种姿态解算方式采全新DMP解算方式相解算方法DMP种解算方式数样精确稳定配置中断时更新飞行姿态数种解算方法减轻单片机处理姿态数时间单片机处理务
3分阐述普通单环PID串级PID两种控制策略串级 PID 超调量响应快参数整定难度较低选取串级PID串级PID策略两控制器控制飞行器会单控制器控制更变量飞行器适应力更强增强系统抗干扰性够更加灵敏快速调节飞行姿态实验表明串级 PID 够满足四旋翼飞行器控制需求
4更监测控制四旋翼飞行器C语言编写四旋翼飞行器虚拟控制位机软件
文着重四旋翼飞行器姿态估计控制系统设计进行阐述四旋翼飞行器相关技术中然许足传感器误差导致长期姿态估计产生积分漂移误差强烈外界干扰控制系统震荡等稳定素姿态估计算法精度长期稳定性考证控制算法精度稳定程度进步提高
82 展
鉴时间精力限制文尚许完善未完成部分希时间深入研究飞行器室定位技术：户外通GPS气压计完成定位操作室定位技术尚成熟室GPS法接收卫星信号采取视觉超声波构成定位系统助超声波模块精确采集飞行器距离面高度数通摄头采集图摄头采成较低OV7670带FIFO彩色摄头助图处理相关算法够前飞行器运动轨迹综合两者信息利PID控制算法飞行器稳定室固定位置适应摄头云台：云台搭载摄头样飞行器飞行程中采集量航拍数勘探形巡线等云台两舵机机架摄头组成通姿态估计姿态角信息计算出云台角度补偿值云台飞行器飞行时够动面保持定夹角通GPS规划航线飞行器预定轨迹飞行
9总结体会
毕业设计程中拓展生阅历专业电气工程动化更深认识获诸感触
万事开头难毕业设计刚开始时毫头绪杨帆指导老师提醒先传感器模块入手查阅传感器资料针相关芯片全英文资料逐字逐句翻译编写程序步步尝试终灵活正确传感器认识学知识脚踏实步脚急功利走成功期间检测传感器许测量工具万表示波器等减轻设计负担时掌握许前陌生工具方法
接进行四旋翼飞行器困难姿态结算块数学知识提出较高求样惯性导航机体坐标系球坐标系解读等陌生知识难度较度想放弃老师帮助学鼓励通软件单步仿真技术分析公式数正误终正确姿态数里学仅仅新知识认识知识广阔性感觉掌握知识太微足道现学专业知识算皮毛外面世界知识永远处发展中必须树立永止境学观念次 更历练面未知勇气信心想信走工作岗位面机遇挑战断更新知识够容应
进入飞行器总体调试阶段量时间花费串级PID调试面前未PID调试验老师帮助明白正确调试系统开始见飞行器万轮阵乱动完全没头绪老师引导仔细分析飞行器动作修改某PID参数前飞行器动作进行历连续周分析PID调试飞行器稳实现正常飞行里老师验提高毕业设计调试进程遇困难更应该端正态度虚心老师前辈请教少走弯路
10谢辞
次毕业设计完成情况基符合初设想然许功受时间精力资金力限制法短时间里完成设计结果较满意毕业设计完成际想许致诚挚感谢：
首先想感谢指导老师杨帆老师困惑时候进行指导精神予鼓励毕业设计利进行
次想感谢周围学鼓励步步完成毕业设计
第三想感谢父母没二十年辛勤养育没现
太感谢三言两语间难表达献诚挚祝福希学生活中够进步帆风感谢学校学院领导老师安排毕业设计学业生涯写隆重笔
11参考文献
[1] 高钟毓．惯性导航系统技术[M]．北京：清华学出版社2012
[2] 刘火良 STM32库开发实战指南[M] 北京 机械工业出版社 2013
[3] 韩建达玉庆赵新刚．移动机器系统[M]．北京：科学出版社2011
[4] 曹承志．微型计算机控制技术[M]．北京：北京工业出版社2008
[5] 雷丹．运动控制系统[M]．北京：民邮电出版社2013
[6] 徐科军．传感器检测技术（第3版）[M]．北京：电子工业出版社2014
[7] 张毅刚彭喜元彭宇．单片机原理应[M]．北京：高等教育出版社2013
[8] 刘军张洋严汉宇．精通STM32F4库函数版[M]．北京：北京航空航天学出版社2015
[9] 王贵山刘亚茹．四旋翼四轴飞行器设计[J]．河南科技2013
[10] 米培良．四旋翼飞行器控制实现[J]．连理工学2015
附录：
1硬件电路图
2PCB图
3部分程序
程序：
Copter_Board
#include boardh
Copter_System
#include Copter_Coreh
int main(void)
{
Quadcopter_Board_Init() 初始化飞控板设置
Copter_Core_Init() 初始化参数
Sensor_Init() 传感器初始化
启独立门狗
IWDG_Init(41000) 分频数64重载值1000溢出时间2s
while(1)
{
if((USART_RX_STA&0x80)0x80) 接收数完成
{
Data_Receive(USART_RX_BUF&Num) 处理位机数
USART_RX_STA0 数组清0接收完成标志清0
}
switch (IRQ_Sign)
{
case 1 MPU6050DMP中断
if(IS_LOCK0) 解锁开始衡姿态
{
Balance_Atitude(pitrolyawpit_Trol_Tyaw_TGyro)
Balance_Height(HeightHeight_T)
}
IRQ_Sign 0
break
}
if((throttle1000)||(IS_LOCK1)) 积分清0
{
PID_ROL_ANGLEioutPID_PIT_ANGLEioutPID_YAW_ANGLEiout0
PID_ROL_RATEiout PID_PIT_RATEiout PID_YAW_RATEiout 0
}
if((abs(rol)>50)||(abs(pit)>50)||(abs(yaw)>70))倾角保护
{
moto1moto2moto3moto41000
IS_LOCK1 锁指示灯开启 角度 警报
throttle1000
}
Motor_Out()
IWDG_Feed() 喂狗
}
}
中断：
void EXTI0_IRQHandler(void) 外部中断0服务程序
{
if(EXTI_GetITStatus(EXTI_Line0)RESET) 判断某线中断否发生
{
mpu_dmp_get_data(Gyro&pit&rol&yaw) 获取DMP数
EXTI_ClearITPendingBit(EXTI_Line0) 清LINE0中断标志位
IRQ_Sign 1
}
}
void USART1_IRQHandler(void) 串口1中断服务程序
{ u8 Res
if(USART_GetITStatus(USART1 USART_IT_RXNE) RESET)
{
ResUSART_ReceiveData(USART1) 读取接收数
接收协议start
if((USART_RX_STA&0x80)0)接收数未完成
{
USART_RX_BUF[USART_RX_STA&0X7F]Res
USART_RX_STA++
if(USART_RX_STA0x02)
{
if(((USART_RX_BUF[0]0xAA)&&(USART_RX_BUF[1]0x55)))
USART_RX_STA0 帧头错误重新开始
}
if(USART_RX_STA0x04) NumUSART_RX_BUF[3]获取数数
if(USART_RX_STA>(USART_REC_LEN1))
USART_RX_STA0 接收数容量重新开始接收
容量帧头正确继续接收
if(((USART_RX_STA&0X7F)Num)&&(Num>4))
USART_RX_STA|0x80 置标志位0x80表示接收数完成
}
接收协议end
}
}
4外文翻译
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档
1前言 1
11背景意义 1
12国外研究现状 1
13文工作 2
2总体方案设计 3
21方案较 3
22方案证选择 3
3飞行器原理结构 5
31飞行器原理 5
32飞行器结构 6
4单元模块设计 8
41单元模块功介绍电路设计 8
411电源 8
412 STM32F407系统 9
413 载电路 11
414 飞控姿态模块 11
415 刷电机连接电路 12
416 串口接口电路 12
42特殊器件介绍 12
421 线数传模块 12
422 飞控姿态模块 13
5软件设计 16
51软件设计原理设计工具 16
52软件设计流程框图说明 17
521串口中断流程图 17
522外部中断流程图 18
523程序流程图 18
6系统调试 20
61 通信系统 20
62 姿态传感器调试 21
621 传感器数分析处理 21
622 姿态解算 23
623 数中断 28
63 PID调试 30
631 PID姿态控制 30
632 飞控系统PID调试 33
7系统功指标参数 36
71系统实现功 36
72系统指标参数 36
8结 38
81 回顾 38
82 展 38
9总结体会 39
10谢辞 40
11参考文献 41
附录： 42
1硬件电路图 42
2PCB图 43
3部分程序 44
4外文翻译 46
1前言
11背景意义
年益现代控制理电子控制技术发展四轴飞行器广泛关注民工业领域具广泛应前景甚机战争中广泛应机发展火热中四旋翼飞行器发展突出四旋翼飞行器具特点：
(1)体积巧工作恶劣危害类健康生命环境中限度减少员伤亡飞行器全天工作需休息工作效率高
(2)支持配备高端电子产品种外设相连接相机机械臂等实现娱乐功
例高空电力线巡检中机工作员操控进行工作代工巡检象实施接检测减少工劳动强度携带传感仪器摄机等巡检象进行数收集分析存储进步提高巡检工作效率巡检精度军事局部规模战时候普通侦察机受敌方击造成必机体员伤亡机起代作利四旋翼飞行器作侦察机具振动噪声性高成低反侦察力强销毁等优势机军事价值估量四旋翼飞行器着更广阔前景等着开发通飞行器添加更加智算法实现机互动飞行器帮取物件等
目前四旋翼飞行器已许领域运总体言旧处初步发展阶段
12国外研究现状
国际较知名飞行器公司中国疆创新美国3D Robotics作全球机领域领头羊两家公司机技术发展速度迅猛形容疆公司新精灵4机实物图图11示
图11 精灵4实物图
精灵4具功：
1 感知前方障碍物动绕行
2 通视觉识动拍移动物体
3 点击相机画面指点方飞行
4 智返航感知障碍物动提升飞行高度
5 飞行时间28分钟控距离约5公里
6 高速度提升20ms (72kmh)
7 体化云台设计提升飞行影稳定性
8 利全新视觉定位技术实现室外精准定位
：3D Robotics公司新出品PIXHAWK飞控拥出色飞行稳定性搭载双传感器系统冗余电源输入扩展两组GPS系统确保飞行失误降低两家公司占领全球部分机市场技术高门槛垄断机公司已远远甩面部分机公司技术仅仅停留稳定飞行简单航拍户外GPS定位阶段
13文工作
机作电子产业里冉冉升起新星具广阔市场发展前景作代学生仅应时代潮流更作时代弄潮信心勇气四旋翼机涉知识面广泛中核心知识专业契合度相高颗功强劲微机芯片造功完备飞行控制系统恰恰微机原理单片机运流机控制系统离开动控制原理目前机电力行业应力度综毕业设计选择基STM32 ARM单片机四旋翼飞控系统
受限知识水文解决问题：
第章节：四旋翼飞行器研究背景意义国外发展现状
第二章节：四旋翼飞行器方案讨选择选择基STM32 ARM单片机四旋翼飞行控制系统
第三章节：简单叙述飞行器飞行原理机械结构
第四章节：飞行控制系统硬件设计中包括单片机系统类模块接口等
第五章节：程序流程图程序流程进行简单说明
第六章节：飞行器控制系统设计包括传感器数进行分析处理DMP姿态解算方式配置中断实时更新前飞行姿态数四旋翼飞行器PID调试策略
第七章节：位机飞行器间通信系统PID调试程
2总体方案设计
21方案较
方案：基意利开源硬件Arduino Nano作数处理姿态结算飞行控制系统飞行控制系统结构图21示
图21 Arduino飞控系统结构图
方案二：采意法半导体STM32F407VET6作飞行器控芯片飞行控制系统结构图22示
图22 基STM32飞控系统结构图
22方案证选择
方案：Arduino Nano基Atmega328P AVR单片机开源硬件具两外部中断口输出六路PWM波兼备IICUARTSPI通信功总说具丰富片资源优秀性编程Arduino C种C语言类似标准C针Arduino系统做量简化工作提供许函数库文件通性高Arduino然已开源飞行控制系统命令线遥控器发出会额外增加遥控器费外Arduino通信协议未知利增添代码调试工作
方案二：意法半导体STM32F407VET6CORTEXM4核属32位ARM微控制器常编程软件KEILIAR编程语言汇编标准C语言C++等灵活方便友编译方式提高代码通性移植性STM32F407VET6具卓越性输AVRAtmega328P里采位机传输命令飞行控制系统简化系统调试方式
考虑资金性价难易程度选择方案二基STM32F407VET6 单片机飞行控制系统
3飞行器原理结构
31飞行器原理
四轴飞行器具两种飞行模式： X 型十字型X型飞行方式四旋翼飞行器姿态改变方机身成45度角十字型飞行方式四旋翼飞行器姿态改变方飞行器机身相采X型飞行方式飞行器具更控制灵敏度稳定性选择X型飞行方式两种工作模式图31示
Y
X
Y
X
图31 X 型（左）十字型（右）飞行方式图
四旋翼飞行器四螺旋桨电机直连结构通改变电机转速获旋转机身力调整身姿态飞行器飞行程中螺旋桨会产生两力升力螺旋桨转相反反扭矩反扭矩会飞行器着螺旋桨旋转方旋果抵消反扭矩会飞行器直转会影响飞行器飞行四旋翼飞行器通分配四螺旋桨转抵消螺旋桨产生反扭矩X 型飞行方式例时针方电机编号右角电机1号电机次编号123412号螺旋桨轴方定义X轴方机头 34号螺旋桨轴方定义Y轴方抵消螺旋桨反扭矩13号螺旋桨需时针转动24号螺旋桨需逆时针转动角线螺旋角旋转方相抵消相互间反扭矩四旋翼飞行器飞行方速度飞行器倾角决定飞行器倾斜角度越飞行速度越快通调节电机转速达控制飞行器姿态速度甚飞行路径效果中四旋翼飞行器飞行姿态高度俯仰角（Pitch）横滚角（Roll）偏航角（Yaw）继续细分：升降前倾倾左倾右倾左旋右旋
(1) 高度：四螺旋桨转速相时加速时螺旋桨升力变升力飞行器重力时飞行器拥运动加速度飞行器升四螺旋桨时减速时螺旋桨产生升力变升力飞行器重力时飞行器拥运动加速度飞行器降
(2) Pitch：绕着Y轴方旋转进行控制12号电机转述等减34号电机转述等增飞行器前倾反会倾
(3)Roll：俯仰控制相似横滚绕着X轴方旋转14号电机转述等减23机转述等增加产生右倾反会左倾
(4)Yaw：理飞行器绕着Z轴旋转13号电机转述等减反扭矩升力减24号电机转述等增加反扭矩升力增加反扭矩出现衡会飞行器右转反会飞行器左转
32飞行器结构
采机架型号 F360轴距360mm螺旋桨型号1047型电机采朗宇X2212980KV刷电机加1V电压电机分钟980转电机转速约10878转＼分通常四旋翼飞行器配2200mah电池电调盈天行者30A电子调速器整飞行系统锂电池供电电机实物图图32示电子调速器实物图图33示
图32 朗宇电机实物图
图33 电子调速器实物图
飞行器组装完成图34示
图34 飞行器实物图
4单元模块设计
41单元模块功介绍电路设计
飞行控制系统硬件结构遵方案二结构框图图22重单元模块做介绍
411电源
LM1117低压差电压调节器压差输出12V时负载电流800mA国家半导体工业标准器件LM317相脚排列LM1117调电压版通2外部电阻实现125～138V输出电压范围外5固定电压输出（18V25V285V33V5V）型号设计飞行器控芯片33V选取固定输出33VLM1117LM1117提供电流限制热保护输出电压精度±1LM1117系列具LLPTO263SOT223TO220TO252DPAK等种封装飞行器提高续航力量减轻身重量里选取SOT223封装电路设计输出端需联少10uF钽电容改善瞬态响应稳定性LM1117电气特性表41示
表41 电气特性表LM1117
符号
参数
测试条件
值
典型值
值
单位
VREF
基准
电压
LM1117ADJ
IOUT10mA VINVOUT2V TJ25˚C
10mA≤IOUT≤800mA
14V≤VINVOUT≤10V
1238
1225
1250
1250
1262
1270
V
V
VOUT
输
出
电
压
LM111718
IOUT10mA VIN38V TJ25˚C
0≤IOUT≤800mA 32V≤VIN≤10V
1782
1746
1800
1800
1818
1854
V
V
LM111725
IOUT10mA VIN45V TJ25˚C
0≤IOUT≤800mA 39V≤VIN≤10V
2475
2450
2500
2500
2525
2550
V
V
LM1117285
IOUT10mA VIN485V TJ25˚C
0≤IOUT≤800mA 425V≤VIN≤10V
0≤IOUT≤500mA VIN410V
2820
2790
2790
2850
2850
2850
2880
2910
2910
V
V
V
LM111733
IOUT10mA VIN5V TJ25˚C
0≤IOUT≤800mA 475V≤VIN≤10V
3267
3235
3300
3300
3333
3365
V
V
∆VOUT
压差
LM111733
IOUT0mA 475V≤VIN≤15V
1
6
mV
电源模块原理图图41示图里5V电源源电子调速器BEC降压系统
图41 电源模块电路图
412 STM32F407系统
STM32F407VET6意法半导体基CORTEXM4核芯片STM32F407拥资源包括：集成FPUDSP指令具192KBSRAM1024KB FLASH1216位定时器232位定时器2DMA控制器（16通道）3SPI2全双工I2S3IIC6串口2USB（支持 HOST SLAVE）2CAN3 12位ADC212位DAC1RTC（带日历功）1SDIO 接口1FSMC接口110100M太网MAC控制器1摄头接口1硬件机数生成器112通IO口等该芯片配置十分强悍具卓越性相STM32F1说许功进行重改进STM32系统电路图图42示
图42 STM32系统电路图
STM32电复位默认部晶振（精度8MHz左右）飞行控制板稳定工作里选择外接8MHz晶振程序里切换外部8MHz晶振通芯片带锁相环PLL倍频168MHz需修改系统时钟配置System_stm32f4xxc文件PLL第级分频系数M修改8样时钟频率达168MHz
STM32F40xx里通BOOT[10]引脚选择三种启动模式般选闪存存储器启动启动模式表42示
表42 启动模式STM32
启动模式选择引脚
启动模式
说明
BOOT1
BOOT0
X
0
闪存存储器
闪存存储器选启动区域
0
1
系统存储器
系统存储器选启动区域
1
1
置SRAM
置SRAM选启动区域
根选定启动模式闪存存储器系统存储器SRAM方式访问：
● 闪存存储器启动：闪存存储器映射启动空间(0x0000 0000)然够原址(0x0800 0000)访问闪存存储器容两址区域访问 0x000000000x0800 0000
● 系统存储器启动：系统存储器映射启动空间(0x0000 0000)然够原址(互联型产品原址0x1FFF B000产品原址0x1FFF F000)访问
● 置SRAM启动：0x2000 0000开始址区访问SRAM
注意：置SRAM启动应程序初始化代码中必须NVIC异常表偏移寄存器新映射量表SRAM中
413 载电路
果采串口烧录程序必须先配置BOOT01BOOT10然复位键通程序载代码载完BOOT0置GND复位运行代码里采jlink（SWD）载方式需接：jlink第1脚（VDD）第7脚（TMSSWDIO应STM32PA13）第9脚（TCKSWCLK应STM32PA14）第468101214161820中意脚（脚）第15脚（RESET应STM32NRST）SW模式载电路图43示
图43 SW模式载电路图
414 飞控姿态模块
性良飞控传感器飞行器稳定飞行重保证GY86模块集成陀螺仪加速度计磁力计气压计适合飞行器GY86模块飞行控制系统连接电路图图44示
图44 GY86模块连接电路图
415 刷电机连接电路
刷电机PWM波驱动PWM波通电子调速器输送刷电机电子调速器3接口VCCGNDMx电路板需出连接口图45示
图45 刷电机接口电路图
416 串口接口电路
图46示单片机串口引出作连接线数传模块收发位机命令余串口接口外扩模块超声波模块GPS
图46 串口接口电路图
42特殊器件介绍
421 线数传模块
CC1101线数传模块体积巧尺寸仅145*28CM操作简单工作频段433MHZ穿透力强直接51AVRSTM8STM32MSP430PIC等单片机串口相连接实物图图47示
图47 线数传模块图
CC1101线数传模块STM32单片机串口连接串口实现远距离线收发数样处减少调试难度果线数传模块话购NRF24L01通信模块会增添代码量加调试难度
422 飞控姿态模块
选择GY86模块原集成度高实物图图48示
图48 GY86模块实物图
模块MPU6050InvenSense公司推出全球首款整合性6轴运动处理组件相较组件方案免组合陀螺仪加速器时轴间差问题减少安装空间
MPU6050部整合3轴陀螺仪3轴加速度传感器含第二IIC接口连接外部传感器利数字运动处理器（DMP Digital Motion Processor）硬件加速引擎通IIC接口应端输出完整9轴融合演算数DMPInvenSense公司提供运动处理资料库实现姿态解算降低运动处理运算操作系统负荷时降低开发难度MPU6050芯片实物图图49示
图49 MPU6050芯片实物图
MPU6050特点包括：
① 数字形式输出6轴9轴（需外接磁传感器）旋转矩阵四元数(quaternion)欧拉角格式(Euler Angle forma)融合演算数（需DMP支持）
② 具131 LSBs°sec敏感度全格感测范围±250±500±1000 ±2000°sec3轴角速度感测器(陀螺仪)
③ 集成程序控制范围±2g±4g±8g ±16g 3轴加速度传感器
④ 移加速器陀螺仪轴间敏感度降低设定予影响感测器飘移
⑤ 带数字运动处理(DMP Digital Motion Processing)引擎减少MCU复杂融合演算数感测器步化姿势感应等负荷
⑥ 建运作时间偏差磁力感测器校正演算技术免客户须外进行校正需求
⑦ 带数字温度传感器
⑧ 带数字输入步引脚(Sync pin)支持视频电子影相稳定技术GPS
⑨ 程序控制中断(interrupt)支持姿势识摇摄画面放缩滚动快速降中断highG中断零动作感应触击感应摇动感应功
⑩ VDD供电电压25V±530V±533V±5VLOGIC低18V±5
⑪ 陀螺仪工作电流：5mA陀螺仪机电流：5uA加速器工作电流：500uA加速器省电模式电流：40uA@10Hz
⑫ 带1024字节FIFO助降低系统功耗
⑬ 高达400KhzIIC通信接口
⑭ 超封装尺寸：4x4x09mm（QFN）
MPU6050芯片工作电路图图410示
图410 MPU6050芯片工作电路图
MPU6050采集飞行器飞行时XYZ三轴加速度偏转角速度具体讲陀螺仪测量物理量偏转倾斜时转动角速度坐标轴线加速度
MPU6050部通MEMS工艺做出参考坐标系芯片着飞行器动作时芯片坐标相参考坐标发生旋转时芯片会读出差异芯片部处理会输出绕坐标轴旋转角速度MPU6050芯片坐标轴分布图411示
图411 MPU6050芯片坐标轴分布图
关加速度测量实物体加速程中作物体力里需注意球物体会受球重力芯片例外XYZ坐标轴测加速度包含重力三坐标轴重力加速度分量
5软件设计
51软件设计原理设计工具
位机采Lab windows制作Lab windows美国国家仪器公司推出交互式C语言开发台编写检测系统数采集系统等应软件数采集分析显示软件界面图51示
图51 Lab Windows界面图
STM32F407芯片C语言软件开发系统KEIL开发KEIL提供包括C编译器功强仿真调试等完整开发方案操作界面图52示
图52 KEIL操作界面图
52软件设计流程框图说明
521串口中断流程图
线数传模块连接串口串口中断发生时说明接收位机命令需处理串口数串口中断流程图图53示
进入串口中断
接收中断数
处理数
串口标志位置1
结束
图53 串口中断流程图
522外部中断流程图
GY86飞控姿态模块数绪中断连接单片机外部中断脚STM32F407接收外部中断时说明组飞行器姿态数已绪需进行姿态解算外部中断流程图图54示
进入外部中断
读取模块数
姿态解算
DMP标志位置1
结束
图54 外部中断流程图
523程序流程图
程序里进行系列初始化进入while循环循环里做通信数处理防止飞行器飞行角度导致飞行器侧翻问题次调整PWM波占空调节刷电机转速程序流程图图55示
开始
初始化飞控板硬件设置
通信初始化
中断初始化
驱动初始化
···
飞控系统初始化
初始化系统参数
初始化传感器
通信数处理
串级PID控制
调节电机PWM波输出
图55 程序流程图
根程序流程图底层驱动部分：
(1) USART1：外接扩展传感器模块
(2) USART2：调试程序发送接收位机命令通信
(3) IIC：读取 GY86 模块值处GPIO模拟IIC
(4) GPIO：控制LED通闪烁亮灭提示飞行器运行状态
(5) SYSTICK：滴答定时器系统精确定时延时
(6) TIM定时器PWM通道：TIM3TIM4PWM功输出4路占空PWM波电子调速器控制刷电机转动
(7) 外部中断：利单片机外部中断时获取前飞行姿态
(8) 独立门狗：溢出时间2s防止系统死机停止工作
6系统调试
61 通信系统
飞行器般控制系统带四螺旋桨电机转速高距离调试意味着伴着安全隐患需构建通信系统飞行器进行远距离调试
位机飞行器实时控制位机发送飞控系统数想加减油门飞行器飞调试时候需时更改PID参数位机发送命令飞行控制系统需发送数位机前四电机转速前飞行器姿态正PID参数保证通信准确性时性数传输编写通信协议必位机图61示
图61 位机界面图
PID参数数协议传递PID参数先放1000倍进行传输保证种类型数正确误数末尾增加校验SUM飞控系统接收RCDATA会单片机里求结果位机发送SUM做两者相等时数会真正保存飞控系统接收重数时飞控系统会返回模样数位机表示已正确收重数增加飞控系统安全性
62 姿态传感器调试
621 传感器数分析处理
四旋翼飞行器保持稳状态关键正确姿态传感器数进行处理首先通操作MPU6050寄存器获飞行时线加速度转动角速度原始值加速度计测量值存储寄存器表61示
表61 加速度计测量值存储寄存器
Register(Hex)
Register(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
3B
59
ACCEL_XOUT[158]
3C
60
ACCEL_XOUT[70]
3D
61
ACCEL_YOUT[158]
3E
62
ACCEL_YOUT[70]
3F
63
ACCEL_ZOUT[158]
40
64
ACCEL_ZOUT[70]
16位加速度计测量值满量程定义MPU6050CCEL_FS寄存器（寄存器28） 满量程设置加速度计测量值灵敏度低分辨率（LSB）表62示：
表62 加速度计灵敏度低分辨率
AFS_SEL
Full Scale Range
LSB Sensitivity
0
±2g
16384 LSBg
1
±4g
8192 LSBg
2
±8g
4096 LSBg
3
±16g
2048 LSBg
陀螺仪测量值存储寄存器表63示
表63 陀螺仪测量值存储寄存器
Register(Hex)
Register(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
43
67
GYRO_XOUT[158]
44
68
GYRO_XOUT[70]
45
69
GYRO_YOUT[158]
46
70
GYRO_YOUT[70]
47
71
GYRO_ZOUT[158]
48
72
GYRO_ZOUT[70]
16位陀螺仪测量值满量程定义MPU6050FS_SEL寄存器（寄存器 27）满量程设置陀螺仪测量值灵敏度低分辨率（LSB）表64示
表64 陀螺仪灵敏度低分辨率
FS_SEL
Full Scale Range
LSB Sensitivity
0
±250°s
131 LSB°s
1
±500°s
655 LSB°s
2
±1000°s
328 LSB°s
3
±2000°s
164 LSB°s
MPU6050数int16型数范围32768+32767正数032767应16进制07FFF负数32768 1应16进制8000FFFF
MPU6050言数FFFF原码80011数FF00原码8100256
先单片机接收MPU6050原始数数应分辨率数加速度值单位转换g角速度单位转换°sec采集数取100次均值ACCEL_OUT放100倍全部数发送串口助手6次样便观察分析具体数值表6566示
表65 静止时MPU6050原始数
状态
GYRO_X
GYRO_Y
GYRO_Z
ACCEL_X
ACCEL_Y
ACCEL_Z
MPU6050静止Z轴垂直
1
0
0
1
0
100
1
0
0
1
0
100
1
0
0
1
0
100
1
1
0
1
0
99
1
0
0
1
0
99
1
0
0
1
0
100
MPU6050静止X轴垂直
1
0
0
100
0
3
1
1
0
100
0
4
1
0
0
100
0
4
1
1
0
100
0
4
1
0
0
100
0
3
1
0
0
100
0
4
MPU6050静止Y轴垂直
1
0
0
2
99
3
1
0
0
2
100
3
1
0
0
2
100
4
1
0
0
1
100
4
1
0
0
2
99
4
1
0
0
1
99
4
分析表格知静止时：XYZ三轴角速度静止0°sec加速度重力原竖直轴100（放100倍方便观察）1g竖直坐标轴1g水坐标轴加速度0g
表66 运动时MPU6050原始数
状态
GYRO_X
GYRO_Y
GYRO_Z
ACCEL_X
ACCEL_Y
ACCEL_Z
绕Y轴
时针
转动
1
35
0
37
0
90
1
28
0
42
0
88
1
26
0
47
0
85
1
21
0
50
0
83
绕Y轴
逆时针
转动
1
21
0
22
0
96
1
20
0
25
0
95
1
20
0
29
0
94
1
20
0
33
0
92
绕X轴
时针
转动
9
0
0
0
22
96
8
0
0
0
24
95
6
0
0
0
26
94
5
0
0
0
27
95
绕X轴
逆时针
转动
15
0
1
1
15
96
14
0
1
0
18
97
13
0
1
1
20
96
13
0
1
1
22
95
绕Z轴
时针
转动
1
1
17
0
1
98
1
1
16
0
0
97
1
1
13
0
1
99
1
1
14
1
2
98
绕Z轴
逆时针
转动
4
3
32
0
1
99
4
2
29
0
0
98
4
3
27
0
0
100
2
0
27
0
0
96
分析表格知传感器绕Y轴转动时：XZ轴角速度静止0°secY轴瞬时针转动角速度正逆时针转动角速度负加速度转动原Y轴加速度绝值减X轴加速度绝值增水Y坐标轴加速度0g传感器绕X轴转动时情况绕Y轴转动类似
传感器绕Z轴转动时 XY轴角速度静止0°secZ轴瞬时针转动角速度正逆时针转动角速度负XY轴水方转动Z轴直竖直XYZ轴加速度变XY轴加速度0
622 姿态解算
3飞行器原理里分析四旋翼飞行器飞行方速度飞行器倾角决定飞行器倾斜角度越飞行速度越快期姿态数欧拉角：航角（yaw）横滚角（roll）俯仰角（pitch）三角前四轴姿态想结果XYZ三轴加速度者角速度
否加速度计算姿态角呢？加速度坐标模型图62示
图62 加速度坐标模型图
知道飞行器受重力场时x^2+y^2+z^2 1g 量R加速度计测量（引力场惯性力）力矢量RxRyRz分RXYZ轴矢量投影面关系式：R^2 RX^2 + RY^2 + RZ^2
感兴趣量R XYZ轴间角度定义 AxrAyrAzr模型图图63示R Rx 组成直角三角形
图63 重力分量坐标模型图
里容易推导出面公式重力量轴间夹角姿态角
COS（Axr） RX R 类推：
COS（Ayr） RY R
COS（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矢量模（长度）时候惯性矢量方真正矢量进行规范化简化程序运算意义
水放置MPU6050Z轴感受重力量输出1g应ADC值16384 (2g量程)时R重力量Rx0 Ry0 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区分真正需加速度）干扰叠加应该测量重力加速度信号输出数法准确反映飞行器倾角图64示
图64 单轴加速度测量值曲线图
图中实线表示MPU6050单轴测加速度变化量虚线表示重力加速度该轴分量出干扰叠加重力加速度分量信号波动噪声通数滑滤波滤滑滤波方面会信号法实时反映飞行器倾角变化进减缓程序飞行器控制方面会飞行器角速度变化信息滤掉述两方面滤波效果飞行器保持衡难度加
角速度言会受飞行器运动影响该数中噪声转动角速度进行积分姿态角样进步滑信号角度信号更加稳定存问题陀螺仪角速度获角度信息需积分运算果角速度信号存微偏差漂移积分运算变化形成积累误差误差会着时间延长逐步增加终导致法形成正确角度信号图65示
图65 角速度积分漂移现象曲线图
图中实线表示角速度积分角度值虚线表示真实角度值结单单角速度角度法精确反应飞行器姿态消累积误差呢？ 种方法通面加速度获角度信息进行校正通积分角度重力加速度角度间偏差改变陀螺仪输出积分角度逐步踪加速度传感器角度
种方法利MPU6050数字运动处理器DMP首先需MPU6050原始数（线加速度转动角速度）然调数字运动处理器DMP数进行简单处理导入DMPDMP部姿态算法处理会输出组四元数四元数实旋转种表示方式里做详细介绍DMP输出四元数q30格式浮点数放2 30次方倍必须先转换浮点数230次方计算公式：
q0 quat[0] q30 q3010737418240f
q1 quat[1] q30
q2 quat[2] q30
q3 quat[3] q30
四元数需根四元数欧拉角转换公式计算出欧拉角yawrollpitch公式：
pitch asin(2 * q1 * q3 + 2 * q0* q2)* 573
roll atan2(2 * q2 * q3 + 2 * q0 * q1 2 * q1 * q1 2 * q2* q2 + 1)* 573
yaw atan2(2*(q1*q2 + q0*q3)q0*q0+q1*q1q2*q2q3*q3) * 573
样杜邦线GY86模块（模块载MPU6050芯片）接入单片机先单片机接收MPU6050原始数数做系列处理送入DMP根两公式DMP数做进步处理计算出欧拉角yawrollpitch显示TFT屏幕方便观察分析模块连接图图66示
图66 模块连接图
GY86模块倾角摆放具体表67示
表67 DMP测试
说明
GY86模块状态
TFT显示
X坐标轴roll绕动
Y坐标轴pitch绕动
Z坐标轴yaw绕动
GY86三角板30角X轴行斜边绕Y轴逆时针转动pitch30度
X坐标轴roll绕动
Y坐标轴pitch绕动
Z坐标轴yaw绕动
GY86三角板60角Y轴行斜边绕X轴时针转动roll60度
X坐标轴roll绕动
Y坐标轴pitch绕动
Z坐标轴yaw绕动
GY86三角板30角Z轴垂直斜边绕Z轴时针转动yaw30度
表67 DMP测试里出利DMP进行姿态解算准确出飞行器欧拉角
623 数中断
然通DMP模式获取正确欧拉角应该什时候读取DMP四元数呢？DMP模式数存储MPU6050里面FIFOFIFO里面存包括GYROACC四元数等参数DMP里面数FIFO读取 DMP库里做法：FIFO里数半满时会发出洗FIFO指令FIFO数足时会延时等果读取早获取次数DMP正处理需延时等果读取晚直观影响飞行器姿态调节缓慢法正常飞行意读取数者读取时机选择时（太快太慢）会出现数连续
解决问题MPU6050芯片数绪中断模式利单片机外部中断MPU6050数绪时立刻获飞行器姿态
MPU6050数绪中断模式需配置寄存器表68610示
表68 寄存器58中断标志寄存器
Register
(Hex)
Register
(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
3A
58
MOT_INT
FIFO
_OFLOW
_INT
I2C
_MST
_INT
DATA
_RDY
_INT
寄存器展示中断源中断标志位读取清寄存器需注意DATA_RDY_INT位数绪中断（Data Ready interrupt）产生该位动置 1读取寄存器该位清 0
表69 寄存器55中断配置寄存器
Register
(Hex)
Register
(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
37
55
INT_
LEVEL
INT_
OPEN
LATCH
_INT
_EN
INT_RD
_CLEAR
FSYNC
_INT
_LEVEL
FSYNC
_INT
_EN
I2C
_BYPASS
_EN
寄存器配置 INT引脚中断信号产生
INT_LEVEL该位等0 INT引脚逻辑电高电该位等1INT引脚逻辑电低电
LATCH_INT_EN该位等0INT引脚产生50us脉该位等1INT引脚保持效电直中断清
INT_RD_CLEAR该位等0读取 INT_STATUS (寄存器58) 会导致中断状态位清
INT_RD_CLEAR该位等1读取动作会导致中断状态位清
分析程序里样配置：
INT_LEVEL等1INT引脚逻辑电低电效
INT_OPEN该位等0the INTpin is configure das pushpull
LATCH_INT_EN等1INT引脚保持效电直中断清
INT_RD_CLEAR该位等1读取动作会导致中断状态位清
表610 寄存器56中断寄存器
Register
(Hex)
Register
(Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
38
56
MOT_EN
FIFO
_OFLOW
_EN
I2C_MST
_INT_EN
DATA
_RDY_EN
该寄存器中断源中断产生
MOT_EN 该位置1该位运动检测（Motion detection）产生中断
FIFO_OFLOW_EN 该位置1该位FIFO缓区溢出产生中断
I2C_MST_INT_EN 该位置1该位 I2C机中断源产生中断
DATA_RDY_EN该位置1该位数绪中断（Data Ready interrupt）传感器寄存器写操作完成时会产生
寄存器需配置DATA_RDY_EN位
MPU6050数绪中断配置完成需配置单片机外部中断降触发
配置完成中断引脚连接示波器观察发现数绪中断周期10ms说明10ms数会更新次
63 PID调试
631 PID姿态控制
偏差例积分微分进行控制1（简称PID调节器称PID控制器）连续系统中技术成熟应广泛种调节器结构简单参数易调整长期应中已积累丰富验特工业程中控制象精确数学模型难建立系统参数常发生变化运控制理分析综合耗费代价预期效果采PID调节器根验进行线整定便满意控制效果PID框图图67示
积分
例
微分
r(t)
e(t)
u(t)
执行机构
象
电机
供电
c(t)
图67 PID框图
飞行控制系统数字PID控制算法P例环节I积分环节D微分环节控制处理器采样控制根采样时刻偏差值计算控制量进行离散式控制形式离散化PID程中令T采样周期k采样序号连续时间t离散时间表示求形式代连续时间积分形式增量形式代连续时间微分形式
(1)
(2)
(3)
（3）代入模拟PID计算表达式
(4)
中Kp例系数T1积分常数TD微分常数
离散PID表达式
(5)
式（5）中定义积分系数微分系数
3飞行器原理612姿态解算里分析控制四旋翼飞行器控制角度姿态角利PID算法控制四旋翼飞行器简单时容易想种控制策略角度单环PID控制器系统框图图67示：
控制信号
（期角度）
前姿态角（DMP）
PID角度单环控制器
刷
电机
电机
供电
传感器模块
图68 角度单环PID控制策略
根图68式（5）整理出单环PID控制程序思路：
前角度误差 期角度 前角度
单环PID_P项 Kp * 前角度误差
单环PID_I项 Ki * 前角度误差 + 次单环PID_I项
单环PID_D项 Kd * ( 前角度误差 次角度误差 )
单环PID输出 单环PID_P项 + 单环PID_I项 + 单环PID_D项
单环PID_输出转换单片机PWM波输出量调节前刷电机转速然实际飞行测试中发现角度单环PID控制策略效果理想需进步改进控制策略
述角度单环PID控制算法仅仅考虑飞行器角度信息需增加控制量线加速度转动角速度作MPU6050原始数两容易获取量相较线加速度角速度更够时体现出前飞行器运动情况增加飞行器稳定性(增加阻尼)增强系统快速响应选择控制飞行器角速度角度角速度串级PID控制算法应运生简言串级PID两PID控制算法套起两控制器控制飞行器会单控制器控制更变量飞行器适应力更强增强系统抗干扰性(增强稳定性)串级PID原理框图图69示：
控制信号
（期角度）
前姿态角（DMP）
PI
角度外环控制器
刷
电机
电机
供电
PID
角速度环控制器
传感器模块（角速度数）
图69 角度角速度串级PID控制策略
采控制品质更加优秀串级PID控制策略根图69式（5）整理出串级PID控制程序思路：
外环（角度环）：
前角度误差 期角度 前角度
外环PID_P项 Kp * 前角度误差
外环PID_I项 Ki * 前角度误差 + 次外环PID_I项
外环PID输出 外环PID_P项 + 外环PID_I项
环（角速度环）：
前角速度误差 外环PID输出 前角速度
环PID_P_rate项 Kp_rate *前角速度误差
环PID_I_rate项 Ki_rate *前角速度误差 + 次环PID_I_rate项
环PID_D_rate项 Kd_rate * ( 前角速度误差 次角速度误差 )
环PID_rate输出 环PID_P_rate项 + 环PID_I_rate项 + 环PID_D_rate项
里环PID_rate输出飞行器需调节身量前姿态修正期姿态控制姿态需通调节电机PWM波实现PWM波环PID_rate输出联系起构建公式
moto1throttlePID_PIT_RATEOUTPID_ROL_RATEOUTPID_YAW_RATEOUT
moto2throttlePID_PIT_RATEOUT+PID_ROL_RATEOUT+PID_YAW_RATEOUT
moto3throttle+PID_PIT_RATEOUT+PID_ROL_RATEOUTPID_YAW_RATEOUT
moto4throttle+PID_PIT_RATEOUTPID_ROL_RATEOUT+PID_YAW_RATEOUT
moto1MOTO_MxMi(moto117001105)
moto2MOTO_MxMi(moto217001105)
moto3MOTO_MxMi(moto317001105)
moto4MOTO_MxMi(moto417001105)
throttle电机值PID_PIT_RATEOUTPID_ROL_RATEOUTPID_YAW_RATEOUT环PID_rate输出moto1例moto1MOTO_MxMi(moto117001105)电机需限幅值1700值1105样防止电机转速moto值输送PWM波占空控制程序进控制电机
632 飞控系统PID调试
飞控系统调试进行PID调试组良PID参数飞行器稳起飞降落
调整PID参数时需飞行器起飞油门基础进行PID参数调整否飞行器油门调试时候稳定飞行程中会晃荡方便飞行器实物调试观察现象助工具万轮万轮飞行器拥高度具体图610示
图610 飞行器万轮调试
调整PID时遵循口诀：
先整定环整定外环
参数整定找佳序查
先例积分微分加
曲线振荡频繁例度盘放
曲线漂浮绕湾例度盘扳
曲线偏离回复慢积分时间降
曲线波动周期长积分时间加长
曲线振荡频率快先微分降
动差波动慢微分时间应加长
理想曲线两波前高低41
调整PID参数程：
串级PID调试中环参数关键理想环参数够舵（角速度控制模式舵）控制量具体情况概衡位置附（正负30度左右）舵量突加飞机快速响应舵量回中飞机立刻停止运动（没回弹震荡）
调节角速度环首先改变程序角度外环掉舵量作角速度环期值（角速度控制模式）
1先调节角速度环P参数果P参数太表现修正角速度误差感觉飞行器软倾斜难修正舵响应差果P参数太衡位置容易抖动舵回中干扰（手突加干扰）时会震荡合适P较舵进行响应太会震荡舵量回中会回弹停止（没D）
2然加角速度环D参数D参数加快舵响应作抑制舵量回中震荡谓立竿见影D会横滚俯仰混控制时表现出具体表现四轴抓手里推油门会抽搐
3加角速度环I参数立马感觉飞行器调节程变柔I参数表现：舵飞机偏离衡位置舵量回中飞机立刻停止转动没I太飞机会重力继续转动
4第四步增添角度外环参数P（里需修改程序改角度作控制信号）外环P加舵会应期角度理果P参数太舵灵敏P参数太舵回中易震荡合适舵反应速度良P参数
5加角度外环参数I 里参数I作修正静态误差飞行器稳定飞行时机身保持衡位置
万轮模拟调试基完成飞行器试飞程中需根飞行器表现微调PID参数直飞行器够稳起飞降落正常飞行
7系统功指标参数
71系统实现功
长达周系统PID参数调试飞行器实现稳起飞正常飞行稳降落功位机发送命令飞行器接收命令飞行器灵敏响应飞行情况图71示
图71 飞行器飞行情况
72系统指标参数
进行试飞调试前飞行器万轮模拟调试组PID参数表71示
表71 PID万轮模拟参数
角度PITCHP＼ROLLP35
角度PITCHI＼ROLLI5
角度PITCHD＼ROLLD0
速度PITCHP＼ROLLP180
速度PITCHI＼ROLLI6
速度PITCHD＼ROLLD8
速度YAWP80
速度YAWI80
速度YAWD0
油门值1350
参数表中出PitchRollPID三组参数分相等三方面原：
1 PitchRoll物理模型具相似性
2 表中PID参数实放1000倍程序里1000值
3 万轮模拟飞行调试相等方便观察调试现象
试飞期间导入万轮模拟PID参数然试飞程中通眼观察飞行器空中否保持稳幅度摆动根实际情况微调PID参数值断修正整定出组合理飞行PID参数表72示
表72 PID飞行参数
角度PITCHP50
角度PITCHI4
角度PITCHD0
角度 ROLLP48
角度 ROLLI4
角度 ROLLD0
速度PITCHP150
速度PITCHI6
速度PITCHD10
速度 ROLLP151
速度 ROLLI6
速度 ROLLD9
速度YAWP80
速度YAWI78
速度YAWD0
油门值1250
8结
81 回顾
受限知识水文做：
1简单分析四旋翼飞行器机械结构飞行器硬件软件底层做设计
2分析两种姿态解算方式采全新DMP解算方式相解算方法DMP种解算方式数样精确稳定配置中断时更新飞行姿态数种解算方法减轻单片机处理姿态数时间单片机处理务
3分阐述普通单环PID串级PID两种控制策略串级 PID 超调量响应快参数整定难度较低选取串级PID串级PID策略两控制器控制飞行器会单控制器控制更变量飞行器适应力更强增强系统抗干扰性够更加灵敏快速调节飞行姿态实验表明串级 PID 够满足四旋翼飞行器控制需求
4更监测控制四旋翼飞行器C语言编写四旋翼飞行器虚拟控制位机软件
文着重四旋翼飞行器姿态估计控制系统设计进行阐述四旋翼飞行器相关技术中然许足传感器误差导致长期姿态估计产生积分漂移误差强烈外界干扰控制系统震荡等稳定素姿态估计算法精度长期稳定性考证控制算法精度稳定程度进步提高
82 展
鉴时间精力限制文尚许完善未完成部分希时间深入研究飞行器室定位技术：户外通GPS气压计完成定位操作室定位技术尚成熟室GPS法接收卫星信号采取视觉超声波构成定位系统助超声波模块精确采集飞行器距离面高度数通摄头采集图摄头采成较低OV7670带FIFO彩色摄头助图处理相关算法够前飞行器运动轨迹综合两者信息利PID控制算法飞行器稳定室固定位置适应摄头云台：云台搭载摄头样飞行器飞行程中采集量航拍数勘探形巡线等云台两舵机机架摄头组成通姿态估计姿态角信息计算出云台角度补偿值云台飞行器飞行时够动面保持定夹角通GPS规划航线飞行器预定轨迹飞行
9总结体会
毕业设计程中拓展生阅历专业电气工程动化更深认识获诸感触
万事开头难毕业设计刚开始时毫头绪杨帆指导老师提醒先传感器模块入手查阅传感器资料针相关芯片全英文资料逐字逐句翻译编写程序步步尝试终灵活正确传感器认识学知识脚踏实步脚急功利走成功期间检测传感器许测量工具万表示波器等减轻设计负担时掌握许前陌生工具方法
接进行四旋翼飞行器困难姿态结算块数学知识提出较高求样惯性导航机体坐标系球坐标系解读等陌生知识难度较度想放弃老师帮助学鼓励通软件单步仿真技术分析公式数正误终正确姿态数里学仅仅新知识认识知识广阔性感觉掌握知识太微足道现学专业知识算皮毛外面世界知识永远处发展中必须树立永止境学观念次 更历练面未知勇气信心想信走工作岗位面机遇挑战断更新知识够容应
进入飞行器总体调试阶段量时间花费串级PID调试面前未PID调试验老师帮助明白正确调试系统开始见飞行器万轮阵乱动完全没头绪老师引导仔细分析飞行器动作修改某PID参数前飞行器动作进行历连续周分析PID调试飞行器稳实现正常飞行里老师验提高毕业设计调试进程遇困难更应该端正态度虚心老师前辈请教少走弯路
10谢辞
次毕业设计完成情况基符合初设想然许功受时间精力资金力限制法短时间里完成设计结果较满意毕业设计完成际想许致诚挚感谢：
首先想感谢指导老师杨帆老师困惑时候进行指导精神予鼓励毕业设计利进行
次想感谢周围学鼓励步步完成毕业设计
第三想感谢父母没二十年辛勤养育没现
太感谢三言两语间难表达献诚挚祝福希学生活中够进步帆风感谢学校学院领导老师安排毕业设计学业生涯写隆重笔
11参考文献
[1] 高钟毓．惯性导航系统技术[M]．北京：清华学出版社2012
[2] 刘火良 STM32库开发实战指南[M] 北京 机械工业出版社 2013
[3] 韩建达玉庆赵新刚．移动机器系统[M]．北京：科学出版社2011
[4] 曹承志．微型计算机控制技术[M]．北京：北京工业出版社2008
[5] 雷丹．运动控制系统[M]．北京：民邮电出版社2013
[6] 徐科军．传感器检测技术（第3版）[M]．北京：电子工业出版社2014
[7] 张毅刚彭喜元彭宇．单片机原理应[M]．北京：高等教育出版社2013
[8] 刘军张洋严汉宇．精通STM32F4库函数版[M]．北京：北京航空航天学出版社2015
[9] 王贵山刘亚茹．四旋翼四轴飞行器设计[J]．河南科技2013
[10] 米培良．四旋翼飞行器控制实现[J]．连理工学2015
附录：
1硬件电路图
2PCB图
3部分程序
程序：
Copter_Board
#include boardh
Copter_System
#include Copter_Coreh
int main(void)
{
Quadcopter_Board_Init() 初始化飞控板设置
Copter_Core_Init() 初始化参数
Sensor_Init() 传感器初始化
启独立门狗
IWDG_Init(41000) 分频数64重载值1000溢出时间2s
while(1)
{
if((USART_RX_STA&0x80)0x80) 接收数完成
{
Data_Receive(USART_RX_BUF&Num) 处理位机数
USART_RX_STA0 数组清0接收完成标志清0
}
switch (IRQ_Sign)
{
case 1 MPU6050DMP中断
if(IS_LOCK0) 解锁开始衡姿态
{
Balance_Atitude(pitrolyawpit_Trol_Tyaw_TGyro)
Balance_Height(HeightHeight_T)
}
IRQ_Sign 0
break
}
if((throttle1000)||(IS_LOCK1)) 积分清0
{
PID_ROL_ANGLEioutPID_PIT_ANGLEioutPID_YAW_ANGLEiout0
PID_ROL_RATEiout PID_PIT_RATEiout PID_YAW_RATEiout 0
}
if((abs(rol)>50)||(abs(pit)>50)||(abs(yaw)>70))倾角保护
{
moto1moto2moto3moto41000
IS_LOCK1 锁指示灯开启 角度 警报
throttle1000
}
Motor_Out()
IWDG_Feed() 喂狗
}
}
中断：
void EXTI0_IRQHandler(void) 外部中断0服务程序
{
if(EXTI_GetITStatus(EXTI_Line0)RESET) 判断某线中断否发生
{
mpu_dmp_get_data(Gyro&pit&rol&yaw) 获取DMP数
EXTI_ClearITPendingBit(EXTI_Line0) 清LINE0中断标志位
IRQ_Sign 1
}
}
void USART1_IRQHandler(void) 串口1中断服务程序
{ u8 Res
if(USART_GetITStatus(USART1 USART_IT_RXNE) RESET)
{
ResUSART_ReceiveData(USART1) 读取接收数
接收协议start
if((USART_RX_STA&0x80)0)接收数未完成
{
USART_RX_BUF[USART_RX_STA&0X7F]Res
USART_RX_STA++
if(USART_RX_STA0x02)
{
if(((USART_RX_BUF[0]0xAA)&&(USART_RX_BUF[1]0x55)))
USART_RX_STA0 帧头错误重新开始
}
if(USART_RX_STA0x04) NumUSART_RX_BUF[3]获取数数
if(USART_RX_STA>(USART_REC_LEN1))
USART_RX_STA0 接收数容量重新开始接收
容量帧头正确继续接收
if(((USART_RX_STA&0X7F)Num)&&(Num>4))
USART_RX_STA|0x80 置标志位0x80表示接收数完成
}
接收协议end
}
}
4外文翻译
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档
