基stm32四旋翼机设计——飞行控制系统软件设计
基stm32四旋翼机设计——飞行控制系统软件设计
摘
四旋翼机种稳定性强结构简单机次设计STM32F103C8T6核心MPU6050作飞行姿态感应模块采卡拉曼算法四元数姿态解算PID控制算法软硬结合方式研究制作四旋翼机文通量文献查阅基础讲述四旋翼机发展历程国外发展概况简述机结构基原理硬件部分设计选择详细述软件部分设计算法实现量调试终制作出稳定飞行机实际操作表明该设计具性稳定易操控等优点达设计求
关键词：四旋翼机四元数姿态解算PID控制算法卡拉曼算法
Design of Fourrotor UAV Based on stm32
Abstract
The fourrotor UAV is a kind of UAV with strong stability and simple structure This design takes the STM32F103C8T6 as the core takes the MPU6050 as the flight attitude sensing module uses the Kalman Filter Algorithm the quaternion attitude computation the PID control algorithm soft and hard union way to study and manufacture the fourrotor UAV On the basis of a large number of references this paper describes the development course of the fourrotor UAV and the development situation at home and abroad and briefly describes the structure and basic principle of the UAV the design and selection of the hardware partThe design of software and the realization of algorithm are discussed in detail After a lot of debugging the UAV can be made to fly stably Practical operation shows that the design has stable performance and is easy to control and other advantages to meet the design requirements
Key words：The fourrotor UAVquaternion attitude computationPID control algorithmKalman Filter Algorithm
目 录
1 前言 1
11 四旋翼机设计目意义应达技术求 1
111 四旋翼机设计目意义 1
112 四旋翼机技术求 1
12 四旋翼机发展历程概况 1
13 四旋翼机简介前景 3
14 四旋翼机设计应解决问题 4
2 四旋翼机设计总体思路 5
21 软件设计思路 5
22 方案选择 5
23 研究方法 6
3 四旋翼机结构运动姿态 7
31 机结构 7
32 四旋翼机运动姿态 7
4 飞行控制系统硬件设计 9
41 控制器 9
42 传感器模块 9
43 电机驱动 10
44 电源电路 10
5 软件设计总体思路 12
51 MPU6050六轴数获取 12
511 IIC 12
512 读取MPU6050 12
513 具体程序解析 12
52 卡尔曼滤波 13
521 滤波作 13
522 具体程序解析 14
53 机姿态描述 15
54 四元数姿态解算 17
55 姿态控制 19
551 PID简单介绍 19
552 PID参数整定 19
553 四旋翼机姿态控制 20
554 PID调节序具体程序解析 20
56 PWM控制输出 21
561 PWM简介 21
562 PWM电机控制简述 22
6 系统调试飞行测试 23
7 总结 24
参考文献 25
谢 辞 26
附 录 27
1 前言
四旋翼机称四旋翼直升机指没员机体遥控种机体四旋翼机具四独立电机结构运现代控制技术实现操控应领域十分广泛单旋翼直升机相具低成体积高性构造独特等特点着计算机通讯迅猛发展机械电子优化升级机成研究热点四旋翼机具低成体积高性等特点广泛应领域
文基STM32四旋翼机进行全面简述历史进行概括述通讲述机特点控现未趋势简述机体飞行原理硬件设计着重述程序控制算法演算通惯性导航引出四元数算法浅入深解释卡尔曼原理通制作实物验证设计合理性
11 四旋翼机设计目意义应达技术求
111 四旋翼机设计目意义
设计做出架四旋翼机旨通设计动手组装调试程提高嵌入式深层理解提高机电体设计力解惯性导航高科技领域体会算法优美处通协作方式提高团队项目建设力团队合作研发力通课题研究程做理实践相结合仅提高理水提高身实践水
时架机制作深入探讨机姿态控制软件部分笔者学验算法进行述推导希读者定启发做点补充作四旋翼机热点途十分广泛通次设计领域进行初步探索
112 四旋翼机技术求
四旋翼机分成硬件系统设计软件系统设计机体结构设计机体信号处理基求机实现稳定飞行位机控制做左右飞行油门水旋转等功笔者负责软件系统设计实现飞行控制前提冗余提高软件代码效率时考虑通PID参数整定提高机安全性性
12 四旋翼机发展历程概况
1907年第架四旋翼机制造出稳定性问题飞行效果次尝试现代四旋翼机发展起作发明法国兄弟
图11法国第架四旋翼直升机
世纪20年代初美国发明世界第台型四旋翼机美国专门成立飞行器组进行三年深入研究Oemichen四旋翼直升机诞生成果垂直飞行高度达1000米时飞行高度说举世震惊段时间做许尝试遗憾稳定性提高成桎梏军工领域机做相稳定达相应速度高度求四旋翼机研究直没取什重进展
图12 Oemichen四旋翼直升机
着微机电系统断研究发展惯性导航系统着十分轻巧等特点1990年初快研究者开发机动控制奠定基础时旋翼机玩具形式进行发展90年代初美国工程师MDammar长时间设计磨终通努力开发四旋翼Roswell Flyer款机器特点便电池发电选择加Dragonflyer公司合作产品销售出MEMS惯导系统然广泛应传感器噪声长时间研究降噪飞行姿态控制算法终德国2005年制作出MD4200实现悬停做真正稳定时谓枝独秀旋翼机商飞行2004年刚刚开始时候疆陪跑者许国家制作出搭载高清摄机键起飞动悬停等功机
图13 四旋翼Roswell Flyer
传感器进行检测数值常常够稳定精确伴着MEMS惯性传感器加速计陀螺仪磁传感器等兴起制作者更选择轴惯性传感器仅成次降低检测更准确外WiFi等通信芯片控制传输图信息完全满足传输需求GPS重量仅克时电池进步升级机续航时间达15分钟甚更高零部件升级旋翼机拥更广阔天发展
2012年初疆创新推出世界第款手飞精灵Phantom体机款机出厂前完成测试操作门槛降低时价格众接受彻底改变旋翼机市场疆战成名中国国研究开始进入国家眼中2014年11月疆颇纪念意义年月公司发布Inspire 1款产品第次做远距离高清数字图传应机时款机带独立云台控制搭载高清摄头款产品取巨成功尤影视行业四旋翼机开始继续着智化前进
13 四旋翼机简介前景
四旋翼机特点体积较较灵活成直升机相较低隐藏较强熟悉便控制性价较高四螺旋桨电机直接带动加齿轮结构传感器相互测出值进行卡尔曼滤波者单独传感器检测通算法处理器处理做飞行姿态解算机飞行轨迹姿态通电机转速调整时四旋翼机事发生承担成低飞行器优点基具备军事领域机减少战士伤亡增加增强探测手段科技领域帮助科学家进行采集探测等工作民领域般作众玩具者航拍工具安装探测设备火灾等较危险方应十分广泛研究热点然四旋翼机存诸问题续航时间短抗干扰力弱等
stn32控制四旋翼机受愈愈消费者喜爱学消费者DIY进行心设计航模消费者直接购买组装磨炼航拍技术尤控制算法日益精进情况优势愈发明显四旋翼机述众领域外探测等方面放光彩年四旋翼机零部件性越越价格越越便宜加代代探索优势愈加明显商业化进程中占极高优势未机发展趋势见斑
14 四旋翼机设计应解决问题
四旋翼机飞行中避免会受微风等外部影响传感器精确等部影响4控制输出数量较运算量软件编写较复杂
笔者负责利软件解决实际问题实际问题抽象出卡拉曼滤波四元数姿态解算PID参数整定PWM控制输出等姿态控制问题通解决问题期做姿态控制稳定时笔者发现类控制代码中代码较冗长优化代码次解决问题机够稳定笔者PID参数整定功夫合理范围做调试工作整系统更加稳定
2 四旋翼机设计总体思路
确定机基需求包括机功性成功耗尺寸重量等确定针需求进行硬件模块选择软件初步构思
机进行系统设计实际问题转变工程问题确定总体系统结构分确定硬件结构软件结构画出流程框架中重点软件硬件间划分划分结果开发进度着较影响
机进行详细设计设计选择符合机系统具体构件包括硬件模块cup6050包括软件模块直接调函数阶段更需特定设计具体结构优化
进行系统集成测试通机系统反复测试修正构件中错误软件程序进行固化
21 软件设计思路
四旋翼机硬件开发解决飞行问题什器件控制问题检测前机姿态问题飞行起似咫尺实际电机特性相参数会完全致转动周围环境微风等扰动会失衡达控
解决问题笔者姿态控制选择PID算法进行控制机相动调节脑想PID需实际飞行姿态参数程序需MPU6050采集数进行处理四元数姿态解算欧拉角采集数传感器客观原常常够准确需卡拉曼算法进行滤波通3算法配合基飞行姿态进行控制时程序编写模块形式方便检测更改程序错误进行调试
22 方案选择
keil MDK 514 集成开发环境次软件编译工作台算法采单函数方式便函数调采IIC总线进行通信控制硬件选STM32F103C8T6作控制器选陀螺仪MPU6050作传感器检测机飞行姿态遥控控STM32F103C8T6机架选优迪U818A电机选8520
221 分析问题
四旋翼机利飞行整体离开硬件电路稳压稳流离开传感器正确传输电机正常工作离开种软件间配合文会详细阐述解决问题
然细节性问题四旋翼机悬停问题机空中稳运动握电机转速简单线性控制法满足机灵敏系统需PID控制算法时刻调整电机PID参数整定项工作需综合考虑方面达较理想结果需定测试关四旋翼机稳定性安全性确保软件硬件相结合增加软件灵活性提高机安全保障文述范畴
23 研究方法
次研究方法文献研究法查阅相关资料整设计整体控细节明确时学东西复学许新知识储备续研发设计坚实基础实验研究法利实物制作检验理正确性运实验法通种方法进行PID参数整定
3 四旋翼机结构运动姿态
31 机结构
般言机机体架构Ｘ型十字型取决机头方定义区坐标系位置致Ｘ型机机头方两机翼中间十字型机中机翼正方两种机体架构优劣总体言Ｘ型机架机性更通调查发现选X型作机架机常常表现更稳定易操作次设计选X型机身构造
Ｘ型四旋翼机种４独立电机驱动X型架构机身飞行器具体结构图31示4电机机提供动力独立结构四电机处水面电机中心距离相等时针转动电机2电机4称正桨逆时针旋转电机1电机3称反桨图出4旋翼分２组角线组组电机旋转方致相邻两电机反两旋翼转速完全相情况两旋翼产生反扭力矩方相反相便抵消力矩达达衡
图31Ｘ型四旋翼机具体结构
32 四旋翼机运动姿态
通改变４电机转速改变４旋翼升力进步改变四旋翼机动作做垂直升降左右横滚移动俯仰移动中果两电机加减速造成力衡常常机做出俯仰横滚等动作般言四旋翼机刚起飞时会四电机相输出功率总拉力加总拉力机重力没扰动时机垂直升反时减输出功率机会垂直降力加环境干扰升力重力加环境干扰压力相等时机会会保持悬停状态
面俯仰滚转运动进行分析两运动通改变电机速度造成力衡飞行姿态发生改变具体：电机12转速时变()电机34转速时变（）机实现滚转运动电机23转速变快应螺旋桨23呈升方电机14转速变时应螺旋桨14呈降方实现仰运动反电机转速变化方相反实现俯运动偏航运动通旋翼产生反扭矩改变偏航角偏航动作程中电机 13 24转速增加时外两电机转速保持变机翼13机翼24转动方产生反扭力矩增加四旋翼机旋转方衡时水方产生扭力改变偏航角实现偏移反然
4 飞行控制系统硬件设计
图41示四旋翼机硬件电路STM32系统电源传感器LED接口六部分组成控制器STM32F103C8T6负责解算传感器模块采集信息利采集信息提供飞行务指令时结合PID控制算法PWM波信号输出电机转速发生规律变化实现飞行器控飞行
图41四旋翼机硬件电路
41 控制器
控单元整机心脏控制器更整机控心脏通测量转换融合传感器数控制器计算出机姿态信息进机进行控制控制器选择应基满足高性低功耗低成够快速解算四旋翼机姿态求设计决定ARM公司推出STM32F103系列时考虑程序容量需引脚数终选择STM32F103C8T6款芯片 该芯片48脚LQFP封装采ARM CortexM3 32位核工作频率达72MHz存储器0等周期访问时达125DMipsMHz系统设计需求完全满足次设计需求
42 传感器模块
传感器模块整机缺部分通陀螺仪加速度传感器传感器测量出机加速度角速度然通算法出飞行姿态测量出数传送控制器处理传感器模块应选择功符合述求传感器时兼具低功耗低成高性传感器设计采MPU60X0系列传感器考虑引脚数选择MPU60509轴运动处理传感器包括陀螺仪加速度计扩展位（加磁力计）四旋翼机６度３坐标轴方线运动角运动姿态信息MPU6050采集该器件户十分友测量范围户控制通身FIFO设计降低系统耗
43 电机驱动
图42四旋翼机电机驱动电路
电机6050空心杯电机优点简单易时空心杯电机质量轻够提高电机响应速度进快速控制旋翼转速启动转矩低机体刚飞行容易稳定噪音较周围环境影响连接电机电路图示中100Ω电阻起限流作MOS开启瞬间时需电流果电流会导致瞬间开启缩短MOS寿命
10K电阻作IC复位状态IO口处浮空输入状态加10K拉电阻避免单片机没工作时MOS导通电机疯狂旋转伤电容作吸收电机产生高频杂讯防堵转部分防堵转信号异常复杂极易出现误检测没电路板焊接
44 电源电路
选37V 标准1S锂电池作整飞行系统供电系统该锂电池提供68分钟时间选该电池方面较便宜方面较轻便电池整流电路图图43示
图43四旋翼机电源电路
机飞行程中电量断降降定程度会造成电源电压低33V样稳压会法正常工作导致输出截断硬件电路中配备升压中肖特基二极里起续流作
5 软件设计总体思路
软件 keil MDK 514 集成开发环境飞行控制软件包括方面 mpu6050传感器初始数传送STM32 单片机采I2C通信方式飞行器实时姿态数进行滤波四元数姿态解算实际姿态 PID控制姿态机法达理值时旧处稳定状态 通占空改变PWM波信号控制电机具体程序框图图51示
图51四旋翼机软件程序设计框图
51 MPU6050六轴数获取
511 IIC
IICARM处理芯片互联通信组件集成电路互联种总线标准两根信号线根时钟线根数线完成数传输操作MPU6050作控器件接IIC实现STM32进行数传输需注意根求设置相应IIC关功寄存器
512 读取MPU6050
根MPU6050数手册确定寄存器址采IIC通信方式读取螺仪重力加速度分量次设置飞行控制器传感器采样率2000Hz外陀螺仪量程越取值越准确量程应太
513 具体程序解析
#define SMPLRT_DIV 0x19 陀螺仪采样率200HZ
#define CONFIGL 0x1A 低通滤波频率
#define GYRO_CONFIG 0x1B 陀螺仪检测量范围
#define ACCEL_CONFIG 0x1C 加速计检测量范围高通滤波频率
#define PWR_MGMT_1 0x6B 电源理 0x00表示正常启
#define WHO_AM_I 0x75 IIC址寄存器(默认数值0x68状态读)
段程序定义MPU6050寄存器分配MPU6050操作方式数输出符合预期目
int8_t MpuInit(void)
{
uint8_t date SUCCESS
do
{
date IIC_Write_One_Byte(MPU6050_ADDRESS PWR_MGMT_1 0x80) 复位
delay_ms(30)
date + IIC_Write_One_Byte(MPU6050_ADDRESS SMPLRT_DIV 0x02) 陀螺仪采样率
date + IIC_Write_One_Byte(MPU6050_ADDRESS PWR_MGMT_1 0x03) 设置设备时钟源陀螺仪Z轴
date + IIC_Write_One_Byte(MPU6050_ADDRESS CONFIGL 0x03) 低通滤波频率
date + IIC_Write_One_Byte(MPU6050_ADDRESS GYRO_CONFIG 0x18)+2000degs
date + IIC_Write_One_Byte(MPU6050_ADDRESS ACCEL_CONFIG 0x09)+4G
}
while(date SUCCESS)
date IIC_Read_One_Byte(MPU6050_ADDRESS 0x75)
if(date MPU6050_PRODUCT_ID)
return FAILED
else
MpuGetOffset()
return SUCCESS
}
段程序MPU6050初始化数采集做准备
52 卡尔曼滤波
定义：卡尔曼滤波（Kalman filtering）种利线性系统状态方程通系统输入输出观测数系统状态进行优估计算法观测数中包括系统中噪声干扰影响优估计作滤波程
521 滤波作
简单讲前观测数估计传感器观测时目标位置速度角速度等测量值常常出现噪声（观测误差）实际值偏差卡尔曼滤波利目标动态信息噪声减弱前测量值估计
程具体情况动态信息秒检测计算值通算法秒做出预测（说成计算模型）时秒测量值预测进行优化估计新估计时卡尔曼滤波测量更新完成
四旋翼机卡尔曼算法非常理想占存少处理速度快适合确定环境中找较正确值四旋翼机计算模型运协方差表示变量相关度前佳估计出刻预测出高斯分布基高斯分布找佳估计更新协方差然微风扰动需添加新确定性排外界干扰产生协方差均值相新高斯分布
522 具体程序解析
void MpuGetData(void) 读取陀螺仪数加滤波
{
uint8_t i
uint8_t buffer[12]
Gyro_Read()
Acc_Read()
for(i0i<6i++)
{
pMpu[i] (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])MpuOffset[i]
if(i < 3)
{
{
Static struct _1_ekf_filter ekf[3] {{00200000010543}{00200000010543}{00200000010543}}
kalman_1(&ekf[i](float)pMpu[i]) 维卡尔曼
pMpu[i] (int16_t)ekf[i]out
}
}
if(i > 2)
{
uint8_t ki3
const float factor 015f 滤波素
static float tBuff[3]
pMpu[i] tBuff[k] tBuff[k] * (1 factor) + pMpu[i] * factor
}
}
}
53 机姿态描述
描述机姿态里pitch（俯仰角）roll（横滚角）yaw（航角）加描述想描述角度需建立空间坐标系机飞行高度较低空间需定义两坐标系参考坐标系机体坐标系里两坐标系原点致
机体说运动成两坐标系变换程机体两坐标系原点致矩阵方式表示两坐标系关系先改变变量进行分析假设机体面旋转（里垂直天方z轴）两坐标系相改变言需研究x轴y轴变化里假设矢量进行投影容易图52示意图中参考坐标系分矢量分x1y1机体坐标系分矢量分x2y2α表示旋转角
图52坐标系旋转映射
图容易式子
（式51）
述式子写成矩阵形式
（式52）
记
（式53）
显然出该式描述矢量两坐标系投影关系中公式记号表示坐标系1坐标系2变换矩阵实两坐标原点相情况两坐标什样关系复杂变换复杂角度通述方式轴动进行旋转轴转3次变换两坐标关系定通变换矩阵连表示出连基序姿态变化先序右左序排列
机体空间姿态图53示z轴航轴旋转角航角x轴俯仰轴旋转角俯仰角y轴横滚轴旋转角横滚角
图53机体空间角位置确定
绕z轴绕x轴绕y轴公式
（式54）
姿态矩阵
（式55）
54 四元数姿态解算
姿态解算根陀螺仪数实时更新姿态矩阵求出机姿态角pitch（俯仰角）roll（横滚角）yaw（航角）进准确描述机姿态信息程四旋翼旋转角表示姿态解算核心般常方余弦法欧拉角法四元数法中选择方余弦法利三角函数坐标表示机姿态信息计算量较效率低欧拉角利角表示旋转较直观存缺点角没办法表示出法四元数法利四元数表示出旋转轴旋转角数值计算较简单时考虑姿态解算断进行选择易操作实现四元数法 综述四元数法设置中间变量四元数读入陀螺仪值转化姿态控制算法需四元数值姿态控制算法输入参数欧拉角需四元数转化欧拉角形式面公式知姿态矩阵3行3列矩阵确定旋转物体中某点位置惯性导航出式子进行代换出
四元数定义：Qq0＋q1i＋q2j＋q3k 中q0q1q2q3实数ijk相互正交单位量 令 q0cosθ2 q1lsinθ2 q2msinθ2 q3nsinθ2 uli+mj+nk Qcosθ2+usinθ2 Qq0＋q1i＋q2j＋q3kcosθ2+usinθ2
q0q1q2q3赋值刚体坐标系参考坐标系间变换矩阵值通样设置四元数表示出
代换化简出式子
（式56）
四元数q0~q3转成方余弦矩阵
（式57）
中注意式子T应关系条式子准
（式58）
方余弦矩阵转换欧拉角：
θarcsinT32 俯仰角pitch
θarctan (T31 T33 ) 横滚角roll
θarctan(T12 T22) 航角yaw
实姿态解算目非传感器测量出姿态信息解算出机实时姿态角量作姿态控制系统反馈量
541 具体程序解析
四元数转欧拉角
void Quaternionto_euler(float *roll float *pitch float *yaw)
{
if (roll)
{
*roll degrees(atan2f(20f*(q1*q2 + q3*q4)1 20f*(q2*q2 + q3*q3)))
}
if (pitch)
{
safe_asin()处理pitch接9090时奇点
*pitch degrees(safe_asin(20f*(q1*q3 q2*q4)))
}
if (yaw)
{
*yaw degrees(atan2f(20f*(q2*q3 q1*q4) 20f*(q1*q1 + q2*q2) 1))
}
}
四元数更新姿态
#define Kp 20f 加速度权重越加速度测量值收敛越快
#define Ki 0001f 误差积分增益
void ANO_IMUQuaternion_CF(Vector3f gyroVector3f acc float deltaT)
{
Vector3f V_gravity V_error V_error_I
1重力加速度化
accnormalize()
2提取四元数等效余弦矩阵中重力分量
Qvector_gravity(V_gravity)
3量叉积出姿态误差
V_error acc V_gravity
4误差进行积分
V_error_I + V_error * Ki
5互补滤波姿态误差补偿角速度修正角速度积分漂移
Gyro + V_error * Kp + V_error_I
6阶龙格库塔法更新四元数
QRunge_Kutta_1st(Gyro deltaT)
7四元数化
Qnormalize()
8四元数转欧拉角
Qto_euler(&anglex &angley &anglez)
}
55 姿态控制
551 PID简单介绍
简单说PID动调节种方式输入偏差（输入偏差指调量设定值差值）进行例积分微分运算P例认杠杆输入发生改变通例作改变输出达整体稳定I积分运算时输入输出呈线性相关例达想目标需进行积分微调D微分运算较敏感称预动作检测者
552 PID参数整定
参数整定般分理计算法验法理计算法需较强数学功底需根计算结果进步修改数笔者通查阅资料知通观察曲线参数整定较效方法属验法种需收集曲线设定值调量波动PID输出然根曲线波动进行分析
整定参数原第需灵活性原性控轻易改动控制策略第二系统成整体孤立整体PID操作参数伺服放器执行器等环节需解孤立问题需复杂问题简单化明确思路全面孤立辩证统关系间相辅相成全面整体问题孤立细化问题问题简单化
553 四旋翼机姿态控制
姿态控制四旋翼机关键理讲四旋翼转速完全致情况电机24正转13反转两旋翼产生反扭力矩方相反相等飞机稳飞行实际中四旋翼识法拥相转速机常常会失衡需姿态控制算法时刻调整飞行姿态PID控制算法操作较简单资料详实控制效果终决定采方法机进行实时控制
四旋翼机串级PID控制算法具体程首先目标期角度时刻机实际测出输出值偏差进行例积分微分线性组合构成控制量然输入四旋翼机中修正机时刻状态信息次目标期值较然进行循环运算
554 PID调节序具体程序解析
PID调节序：先调PID算法处理外环调节环参数里外环指电流环环速度环选择环电流环原速度环调节命令较命令果环话会导致产生非常电流烧坏电机安全角度应该样设置外环Ｐ调节环角速度调节核心时PID算法做PIＤ三者进行结合调节直接PID串级算法效果致相
控制算法：
pidRateXmeasured MPU6050gyroX * Gyro_G 环测量值角度秒
pidRateYmeasured MPU6050gyroY * Gyro_G
pidRateZmeasured MPU6050gyroZ * Gyro_G
pidPitchmeasured Anglepitch 外环测量值
pidRollmeasured Angleroll 横滚角roll
pidYawmeasured Angleyaw
pidUpdate(&pidRolldt) 调PID处理函数处理外环 横滚角PID
pidRateXdesired pidRollout 外环PID输出作环PID期值串级PID
pidUpdate(&pidRateXdt) 调环
pidUpdate(&pidPitchdt) 调PID处理函数处理外环 俯仰角PID
pidRateYdesired pidPitchout
pidUpdate(&pidRateYdt) 调环
CascadePID(&pidRateZ&pidYawdt) 直接调串级PID函数处理
PID算法程序表示：
void pidUpdate(PidObject* pidconst float dt)
{
float error
float deriv
error pid>desired pid>measured 前角度实际角度误差
pid>integ + error * dt 误差积分累加值
deriv (error pid>prevError)dt 前两次误差做微分
pid>out pid>kp * error + pid>ki * pid>integ + pid>kd * derivPID输出
pid>prevError error 更新次误差
}
56 PWM控制输出
561 PWM简介
PWM名称脉宽度调制器定时计数器件定时计数器特点特定输入时钟通分频接计数器进行加减运算达设定值时会引发中断设定标志位PWM定时达会输出特定波形PWM输出缓寄存器控制周期较寄存器控制占空
占空指周期高电占例形象描述图示周期10s波形出周期电高低相第部分占40第二部分占60占空概念尝试周期断变灯例开始灯亮灭亮灭形式呈现达时间间隔足够时候呈现光强度变弱占空越低光强度越弱种表现认周期均电压降低总结电源电压稳定时候输出电压电压（占空100）占空电机样通调节占空调节电机转速
图54脉信号波形图
562 PWM电机控制简述
PWM通改变输出方波占空类等效电压改变改变电机转速进达机控制目PWM控制信号取决PID姿态控制信号需注意电机驱动需预留定高电低占空电机法运转
6 系统调试飞行测试
然程序较严格证段程序进行检查实际环境常常变信号传输干扰空气流动造成扰动等操作手法造成飞行稳定制作出实物需根实际情况进行微调
进行飞行测试时笔者通团队讨选择宽阔室试飞考量室墙壁反射信号机容易失控次测试阶段选择扰动量方发现问题便分析便整体修改
测试容包括机飞行移动悬停图四旋翼机飞行时图片动态性良飞行测试中四旋翼机够遥控控制方进行前进倒退左移右移倾角误差±4°飞行姿态较稳控制命令发送四旋翼机响应时间2s左右系统响应实时性较足处降落时需缓慢降否电源瞬间切断直接掉落造成危险总说测试结果达预期设计求
7 总结
次毕业设计回顾许学期间学知识高数线代控制理嵌入式单片机等课程回顾期间方面感受深深足方面学寸步难行面未学知识时候老师教授学技巧书皮文献目录找真正需东西原理解释理解数学解释续学起事半功倍效果整体言次毕业设计受益良现次毕业设计总结
现次毕业设计进行总结文四旋翼机发展入手机飞行原理飞行姿态控制进行简单描述整硬件模块进行简单介绍两铺垫软件容出现会显突兀读者快速简单解整四旋翼机进软件部分致思路实际握
文软件部分进行详细述卡尔曼滤波四元数姿态解算PID三算法进行浅薄理解详细述参考量程序进写出应程序整体言文算法述占较篇幅较通俗易懂话写出相应初次接触该领域读者点参考
参考文献
［1］程 煦 郭珊珊 陈华宾．基STM32单片机四旋翼飞行器设计［J］．技术交流201703（05）：121124
［2］张福炎马维华戴志涛等 嵌入式系统开发技术［M］ 高等教育出版社2018
［3］颜王丽丹李梦柯等．基STM32四旋翼飞行器设计实现［J］．电子设程201624（02）：187193
［4］潘春荣许化．基STM32Ｘ型四旋翼机设计［J］工程设计学报201609（16）：197201
［5］金薇．基STM32四旋翼飞行器研究设计［J］山西电子技术201601（12）：9293
［6］秦永元 惯性导航［M］ 北京 科学出版社2006
［7］周权 四旋翼飞行台飞行控制惯性导航研究［D］ 南京：南京航空航天学2008
［8］川李智王勇军．基STM32四旋翼飞行器姿态优估计［J］．电子技术应201541（12）：6164
［9］李文鹏唐海洋．基STM32四旋翼飞行器姿态解算研究［J］单片机嵌入式系统应201541（12）：1316
［10］饶文 四旋翼飞行器控制系统设计控制算法研究［D］ 合肥工业学 2017
［11］汤洋章磊张瑶．四旋翼飞行器设计制作［J］湖北理工学院学报201603（02）：58
附 录
四旋翼机整体实物图：
四旋翼机机翼实物图：
四旋翼机电池实物图：
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档
基stm32四旋翼机设计——飞行控制系统软件设计
基stm32四旋翼机设计——飞行控制系统软件设计
摘
四旋翼机种稳定性强结构简单机次设计STM32F103C8T6核心MPU6050作飞行姿态感应模块采卡拉曼算法四元数姿态解算PID控制算法软硬结合方式研究制作四旋翼机文通量文献查阅基础讲述四旋翼机发展历程国外发展概况简述机结构基原理硬件部分设计选择详细述软件部分设计算法实现量调试终制作出稳定飞行机实际操作表明该设计具性稳定易操控等优点达设计求
关键词：四旋翼机四元数姿态解算PID控制算法卡拉曼算法
Design of Fourrotor UAV Based on stm32
Abstract
The fourrotor UAV is a kind of UAV with strong stability and simple structure This design takes the STM32F103C8T6 as the core takes the MPU6050 as the flight attitude sensing module uses the Kalman Filter Algorithm the quaternion attitude computation the PID control algorithm soft and hard union way to study and manufacture the fourrotor UAV On the basis of a large number of references this paper describes the development course of the fourrotor UAV and the development situation at home and abroad and briefly describes the structure and basic principle of the UAV the design and selection of the hardware partThe design of software and the realization of algorithm are discussed in detail After a lot of debugging the UAV can be made to fly stably Practical operation shows that the design has stable performance and is easy to control and other advantages to meet the design requirements
Key words：The fourrotor UAVquaternion attitude computationPID control algorithmKalman Filter Algorithm
目 录
1 前言 1
11 四旋翼机设计目意义应达技术求 1
111 四旋翼机设计目意义 1
112 四旋翼机技术求 1
12 四旋翼机发展历程概况 1
13 四旋翼机简介前景 3
14 四旋翼机设计应解决问题 4
2 四旋翼机设计总体思路 5
21 软件设计思路 5
22 方案选择 5
23 研究方法 6
3 四旋翼机结构运动姿态 7
31 机结构 7
32 四旋翼机运动姿态 7
4 飞行控制系统硬件设计 9
41 控制器 9
42 传感器模块 9
43 电机驱动 10
44 电源电路 10
5 软件设计总体思路 12
51 MPU6050六轴数获取 12
511 IIC 12
512 读取MPU6050 12
513 具体程序解析 12
52 卡尔曼滤波 13
521 滤波作 13
522 具体程序解析 14
53 机姿态描述 15
54 四元数姿态解算 17
55 姿态控制 19
551 PID简单介绍 19
552 PID参数整定 19
553 四旋翼机姿态控制 20
554 PID调节序具体程序解析 20
56 PWM控制输出 21
561 PWM简介 21
562 PWM电机控制简述 22
6 系统调试飞行测试 23
7 总结 24
参考文献 25
谢 辞 26
附 录 27
1 前言
四旋翼机称四旋翼直升机指没员机体遥控种机体四旋翼机具四独立电机结构运现代控制技术实现操控应领域十分广泛单旋翼直升机相具低成体积高性构造独特等特点着计算机通讯迅猛发展机械电子优化升级机成研究热点四旋翼机具低成体积高性等特点广泛应领域
文基STM32四旋翼机进行全面简述历史进行概括述通讲述机特点控现未趋势简述机体飞行原理硬件设计着重述程序控制算法演算通惯性导航引出四元数算法浅入深解释卡尔曼原理通制作实物验证设计合理性
11 四旋翼机设计目意义应达技术求
111 四旋翼机设计目意义
设计做出架四旋翼机旨通设计动手组装调试程提高嵌入式深层理解提高机电体设计力解惯性导航高科技领域体会算法优美处通协作方式提高团队项目建设力团队合作研发力通课题研究程做理实践相结合仅提高理水提高身实践水
时架机制作深入探讨机姿态控制软件部分笔者学验算法进行述推导希读者定启发做点补充作四旋翼机热点途十分广泛通次设计领域进行初步探索
112 四旋翼机技术求
四旋翼机分成硬件系统设计软件系统设计机体结构设计机体信号处理基求机实现稳定飞行位机控制做左右飞行油门水旋转等功笔者负责软件系统设计实现飞行控制前提冗余提高软件代码效率时考虑通PID参数整定提高机安全性性
12 四旋翼机发展历程概况
1907年第架四旋翼机制造出稳定性问题飞行效果次尝试现代四旋翼机发展起作发明法国兄弟
图11法国第架四旋翼直升机
世纪20年代初美国发明世界第台型四旋翼机美国专门成立飞行器组进行三年深入研究Oemichen四旋翼直升机诞生成果垂直飞行高度达1000米时飞行高度说举世震惊段时间做许尝试遗憾稳定性提高成桎梏军工领域机做相稳定达相应速度高度求四旋翼机研究直没取什重进展
图12 Oemichen四旋翼直升机
着微机电系统断研究发展惯性导航系统着十分轻巧等特点1990年初快研究者开发机动控制奠定基础时旋翼机玩具形式进行发展90年代初美国工程师MDammar长时间设计磨终通努力开发四旋翼Roswell Flyer款机器特点便电池发电选择加Dragonflyer公司合作产品销售出MEMS惯导系统然广泛应传感器噪声长时间研究降噪飞行姿态控制算法终德国2005年制作出MD4200实现悬停做真正稳定时谓枝独秀旋翼机商飞行2004年刚刚开始时候疆陪跑者许国家制作出搭载高清摄机键起飞动悬停等功机
图13 四旋翼Roswell Flyer
传感器进行检测数值常常够稳定精确伴着MEMS惯性传感器加速计陀螺仪磁传感器等兴起制作者更选择轴惯性传感器仅成次降低检测更准确外WiFi等通信芯片控制传输图信息完全满足传输需求GPS重量仅克时电池进步升级机续航时间达15分钟甚更高零部件升级旋翼机拥更广阔天发展
2012年初疆创新推出世界第款手飞精灵Phantom体机款机出厂前完成测试操作门槛降低时价格众接受彻底改变旋翼机市场疆战成名中国国研究开始进入国家眼中2014年11月疆颇纪念意义年月公司发布Inspire 1款产品第次做远距离高清数字图传应机时款机带独立云台控制搭载高清摄头款产品取巨成功尤影视行业四旋翼机开始继续着智化前进
13 四旋翼机简介前景
四旋翼机特点体积较较灵活成直升机相较低隐藏较强熟悉便控制性价较高四螺旋桨电机直接带动加齿轮结构传感器相互测出值进行卡尔曼滤波者单独传感器检测通算法处理器处理做飞行姿态解算机飞行轨迹姿态通电机转速调整时四旋翼机事发生承担成低飞行器优点基具备军事领域机减少战士伤亡增加增强探测手段科技领域帮助科学家进行采集探测等工作民领域般作众玩具者航拍工具安装探测设备火灾等较危险方应十分广泛研究热点然四旋翼机存诸问题续航时间短抗干扰力弱等
stn32控制四旋翼机受愈愈消费者喜爱学消费者DIY进行心设计航模消费者直接购买组装磨炼航拍技术尤控制算法日益精进情况优势愈发明显四旋翼机述众领域外探测等方面放光彩年四旋翼机零部件性越越价格越越便宜加代代探索优势愈加明显商业化进程中占极高优势未机发展趋势见斑
14 四旋翼机设计应解决问题
四旋翼机飞行中避免会受微风等外部影响传感器精确等部影响4控制输出数量较运算量软件编写较复杂
笔者负责利软件解决实际问题实际问题抽象出卡拉曼滤波四元数姿态解算PID参数整定PWM控制输出等姿态控制问题通解决问题期做姿态控制稳定时笔者发现类控制代码中代码较冗长优化代码次解决问题机够稳定笔者PID参数整定功夫合理范围做调试工作整系统更加稳定
2 四旋翼机设计总体思路
确定机基需求包括机功性成功耗尺寸重量等确定针需求进行硬件模块选择软件初步构思
机进行系统设计实际问题转变工程问题确定总体系统结构分确定硬件结构软件结构画出流程框架中重点软件硬件间划分划分结果开发进度着较影响
机进行详细设计设计选择符合机系统具体构件包括硬件模块cup6050包括软件模块直接调函数阶段更需特定设计具体结构优化
进行系统集成测试通机系统反复测试修正构件中错误软件程序进行固化
21 软件设计思路
四旋翼机硬件开发解决飞行问题什器件控制问题检测前机姿态问题飞行起似咫尺实际电机特性相参数会完全致转动周围环境微风等扰动会失衡达控
解决问题笔者姿态控制选择PID算法进行控制机相动调节脑想PID需实际飞行姿态参数程序需MPU6050采集数进行处理四元数姿态解算欧拉角采集数传感器客观原常常够准确需卡拉曼算法进行滤波通3算法配合基飞行姿态进行控制时程序编写模块形式方便检测更改程序错误进行调试
22 方案选择
keil MDK 514 集成开发环境次软件编译工作台算法采单函数方式便函数调采IIC总线进行通信控制硬件选STM32F103C8T6作控制器选陀螺仪MPU6050作传感器检测机飞行姿态遥控控STM32F103C8T6机架选优迪U818A电机选8520
221 分析问题
四旋翼机利飞行整体离开硬件电路稳压稳流离开传感器正确传输电机正常工作离开种软件间配合文会详细阐述解决问题
然细节性问题四旋翼机悬停问题机空中稳运动握电机转速简单线性控制法满足机灵敏系统需PID控制算法时刻调整电机PID参数整定项工作需综合考虑方面达较理想结果需定测试关四旋翼机稳定性安全性确保软件硬件相结合增加软件灵活性提高机安全保障文述范畴
23 研究方法
次研究方法文献研究法查阅相关资料整设计整体控细节明确时学东西复学许新知识储备续研发设计坚实基础实验研究法利实物制作检验理正确性运实验法通种方法进行PID参数整定
3 四旋翼机结构运动姿态
31 机结构
般言机机体架构Ｘ型十字型取决机头方定义区坐标系位置致Ｘ型机机头方两机翼中间十字型机中机翼正方两种机体架构优劣总体言Ｘ型机架机性更通调查发现选X型作机架机常常表现更稳定易操作次设计选X型机身构造
Ｘ型四旋翼机种４独立电机驱动X型架构机身飞行器具体结构图31示4电机机提供动力独立结构四电机处水面电机中心距离相等时针转动电机2电机4称正桨逆时针旋转电机1电机3称反桨图出4旋翼分２组角线组组电机旋转方致相邻两电机反两旋翼转速完全相情况两旋翼产生反扭力矩方相反相便抵消力矩达达衡
图31Ｘ型四旋翼机具体结构
32 四旋翼机运动姿态
通改变４电机转速改变４旋翼升力进步改变四旋翼机动作做垂直升降左右横滚移动俯仰移动中果两电机加减速造成力衡常常机做出俯仰横滚等动作般言四旋翼机刚起飞时会四电机相输出功率总拉力加总拉力机重力没扰动时机垂直升反时减输出功率机会垂直降力加环境干扰升力重力加环境干扰压力相等时机会会保持悬停状态
面俯仰滚转运动进行分析两运动通改变电机速度造成力衡飞行姿态发生改变具体：电机12转速时变()电机34转速时变（）机实现滚转运动电机23转速变快应螺旋桨23呈升方电机14转速变时应螺旋桨14呈降方实现仰运动反电机转速变化方相反实现俯运动偏航运动通旋翼产生反扭矩改变偏航角偏航动作程中电机 13 24转速增加时外两电机转速保持变机翼13机翼24转动方产生反扭力矩增加四旋翼机旋转方衡时水方产生扭力改变偏航角实现偏移反然
4 飞行控制系统硬件设计
图41示四旋翼机硬件电路STM32系统电源传感器LED接口六部分组成控制器STM32F103C8T6负责解算传感器模块采集信息利采集信息提供飞行务指令时结合PID控制算法PWM波信号输出电机转速发生规律变化实现飞行器控飞行
图41四旋翼机硬件电路
41 控制器
控单元整机心脏控制器更整机控心脏通测量转换融合传感器数控制器计算出机姿态信息进机进行控制控制器选择应基满足高性低功耗低成够快速解算四旋翼机姿态求设计决定ARM公司推出STM32F103系列时考虑程序容量需引脚数终选择STM32F103C8T6款芯片 该芯片48脚LQFP封装采ARM CortexM3 32位核工作频率达72MHz存储器0等周期访问时达125DMipsMHz系统设计需求完全满足次设计需求
42 传感器模块
传感器模块整机缺部分通陀螺仪加速度传感器传感器测量出机加速度角速度然通算法出飞行姿态测量出数传送控制器处理传感器模块应选择功符合述求传感器时兼具低功耗低成高性传感器设计采MPU60X0系列传感器考虑引脚数选择MPU60509轴运动处理传感器包括陀螺仪加速度计扩展位（加磁力计）四旋翼机６度３坐标轴方线运动角运动姿态信息MPU6050采集该器件户十分友测量范围户控制通身FIFO设计降低系统耗
43 电机驱动
图42四旋翼机电机驱动电路
电机6050空心杯电机优点简单易时空心杯电机质量轻够提高电机响应速度进快速控制旋翼转速启动转矩低机体刚飞行容易稳定噪音较周围环境影响连接电机电路图示中100Ω电阻起限流作MOS开启瞬间时需电流果电流会导致瞬间开启缩短MOS寿命
10K电阻作IC复位状态IO口处浮空输入状态加10K拉电阻避免单片机没工作时MOS导通电机疯狂旋转伤电容作吸收电机产生高频杂讯防堵转部分防堵转信号异常复杂极易出现误检测没电路板焊接
44 电源电路
选37V 标准1S锂电池作整飞行系统供电系统该锂电池提供68分钟时间选该电池方面较便宜方面较轻便电池整流电路图图43示
图43四旋翼机电源电路
机飞行程中电量断降降定程度会造成电源电压低33V样稳压会法正常工作导致输出截断硬件电路中配备升压中肖特基二极里起续流作
5 软件设计总体思路
软件 keil MDK 514 集成开发环境飞行控制软件包括方面 mpu6050传感器初始数传送STM32 单片机采I2C通信方式飞行器实时姿态数进行滤波四元数姿态解算实际姿态 PID控制姿态机法达理值时旧处稳定状态 通占空改变PWM波信号控制电机具体程序框图图51示
图51四旋翼机软件程序设计框图
51 MPU6050六轴数获取
511 IIC
IICARM处理芯片互联通信组件集成电路互联种总线标准两根信号线根时钟线根数线完成数传输操作MPU6050作控器件接IIC实现STM32进行数传输需注意根求设置相应IIC关功寄存器
512 读取MPU6050
根MPU6050数手册确定寄存器址采IIC通信方式读取螺仪重力加速度分量次设置飞行控制器传感器采样率2000Hz外陀螺仪量程越取值越准确量程应太
513 具体程序解析
#define SMPLRT_DIV 0x19 陀螺仪采样率200HZ
#define CONFIGL 0x1A 低通滤波频率
#define GYRO_CONFIG 0x1B 陀螺仪检测量范围
#define ACCEL_CONFIG 0x1C 加速计检测量范围高通滤波频率
#define PWR_MGMT_1 0x6B 电源理 0x00表示正常启
#define WHO_AM_I 0x75 IIC址寄存器(默认数值0x68状态读)
段程序定义MPU6050寄存器分配MPU6050操作方式数输出符合预期目
int8_t MpuInit(void)
{
uint8_t date SUCCESS
do
{
date IIC_Write_One_Byte(MPU6050_ADDRESS PWR_MGMT_1 0x80) 复位
delay_ms(30)
date + IIC_Write_One_Byte(MPU6050_ADDRESS SMPLRT_DIV 0x02) 陀螺仪采样率
date + IIC_Write_One_Byte(MPU6050_ADDRESS PWR_MGMT_1 0x03) 设置设备时钟源陀螺仪Z轴
date + IIC_Write_One_Byte(MPU6050_ADDRESS CONFIGL 0x03) 低通滤波频率
date + IIC_Write_One_Byte(MPU6050_ADDRESS GYRO_CONFIG 0x18)+2000degs
date + IIC_Write_One_Byte(MPU6050_ADDRESS ACCEL_CONFIG 0x09)+4G
}
while(date SUCCESS)
date IIC_Read_One_Byte(MPU6050_ADDRESS 0x75)
if(date MPU6050_PRODUCT_ID)
return FAILED
else
MpuGetOffset()
return SUCCESS
}
段程序MPU6050初始化数采集做准备
52 卡尔曼滤波
定义：卡尔曼滤波（Kalman filtering）种利线性系统状态方程通系统输入输出观测数系统状态进行优估计算法观测数中包括系统中噪声干扰影响优估计作滤波程
521 滤波作
简单讲前观测数估计传感器观测时目标位置速度角速度等测量值常常出现噪声（观测误差）实际值偏差卡尔曼滤波利目标动态信息噪声减弱前测量值估计
程具体情况动态信息秒检测计算值通算法秒做出预测（说成计算模型）时秒测量值预测进行优化估计新估计时卡尔曼滤波测量更新完成
四旋翼机卡尔曼算法非常理想占存少处理速度快适合确定环境中找较正确值四旋翼机计算模型运协方差表示变量相关度前佳估计出刻预测出高斯分布基高斯分布找佳估计更新协方差然微风扰动需添加新确定性排外界干扰产生协方差均值相新高斯分布
522 具体程序解析
void MpuGetData(void) 读取陀螺仪数加滤波
{
uint8_t i
uint8_t buffer[12]
Gyro_Read()
Acc_Read()
for(i0i<6i++)
{
pMpu[i] (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])MpuOffset[i]
if(i < 3)
{
{
Static struct _1_ekf_filter ekf[3] {{00200000010543}{00200000010543}{00200000010543}}
kalman_1(&ekf[i](float)pMpu[i]) 维卡尔曼
pMpu[i] (int16_t)ekf[i]out
}
}
if(i > 2)
{
uint8_t ki3
const float factor 015f 滤波素
static float tBuff[3]
pMpu[i] tBuff[k] tBuff[k] * (1 factor) + pMpu[i] * factor
}
}
}
53 机姿态描述
描述机姿态里pitch（俯仰角）roll（横滚角）yaw（航角）加描述想描述角度需建立空间坐标系机飞行高度较低空间需定义两坐标系参考坐标系机体坐标系里两坐标系原点致
机体说运动成两坐标系变换程机体两坐标系原点致矩阵方式表示两坐标系关系先改变变量进行分析假设机体面旋转（里垂直天方z轴）两坐标系相改变言需研究x轴y轴变化里假设矢量进行投影容易图52示意图中参考坐标系分矢量分x1y1机体坐标系分矢量分x2y2α表示旋转角
图52坐标系旋转映射
图容易式子
（式51）
述式子写成矩阵形式
（式52）
记
（式53）
显然出该式描述矢量两坐标系投影关系中公式记号表示坐标系1坐标系2变换矩阵实两坐标原点相情况两坐标什样关系复杂变换复杂角度通述方式轴动进行旋转轴转3次变换两坐标关系定通变换矩阵连表示出连基序姿态变化先序右左序排列
机体空间姿态图53示z轴航轴旋转角航角x轴俯仰轴旋转角俯仰角y轴横滚轴旋转角横滚角
图53机体空间角位置确定
绕z轴绕x轴绕y轴公式
（式54）
姿态矩阵
（式55）
54 四元数姿态解算
姿态解算根陀螺仪数实时更新姿态矩阵求出机姿态角pitch（俯仰角）roll（横滚角）yaw（航角）进准确描述机姿态信息程四旋翼旋转角表示姿态解算核心般常方余弦法欧拉角法四元数法中选择方余弦法利三角函数坐标表示机姿态信息计算量较效率低欧拉角利角表示旋转较直观存缺点角没办法表示出法四元数法利四元数表示出旋转轴旋转角数值计算较简单时考虑姿态解算断进行选择易操作实现四元数法 综述四元数法设置中间变量四元数读入陀螺仪值转化姿态控制算法需四元数值姿态控制算法输入参数欧拉角需四元数转化欧拉角形式面公式知姿态矩阵3行3列矩阵确定旋转物体中某点位置惯性导航出式子进行代换出
四元数定义：Qq0＋q1i＋q2j＋q3k 中q0q1q2q3实数ijk相互正交单位量 令 q0cosθ2 q1lsinθ2 q2msinθ2 q3nsinθ2 uli+mj+nk Qcosθ2+usinθ2 Qq0＋q1i＋q2j＋q3kcosθ2+usinθ2
q0q1q2q3赋值刚体坐标系参考坐标系间变换矩阵值通样设置四元数表示出
代换化简出式子
（式56）
四元数q0~q3转成方余弦矩阵
（式57）
中注意式子T应关系条式子准
（式58）
方余弦矩阵转换欧拉角：
θarcsinT32 俯仰角pitch
θarctan (T31 T33 ) 横滚角roll
θarctan(T12 T22) 航角yaw
实姿态解算目非传感器测量出姿态信息解算出机实时姿态角量作姿态控制系统反馈量
541 具体程序解析
四元数转欧拉角
void Quaternionto_euler(float *roll float *pitch float *yaw)
{
if (roll)
{
*roll degrees(atan2f(20f*(q1*q2 + q3*q4)1 20f*(q2*q2 + q3*q3)))
}
if (pitch)
{
safe_asin()处理pitch接9090时奇点
*pitch degrees(safe_asin(20f*(q1*q3 q2*q4)))
}
if (yaw)
{
*yaw degrees(atan2f(20f*(q2*q3 q1*q4) 20f*(q1*q1 + q2*q2) 1))
}
}
四元数更新姿态
#define Kp 20f 加速度权重越加速度测量值收敛越快
#define Ki 0001f 误差积分增益
void ANO_IMUQuaternion_CF(Vector3f gyroVector3f acc float deltaT)
{
Vector3f V_gravity V_error V_error_I
1重力加速度化
accnormalize()
2提取四元数等效余弦矩阵中重力分量
Qvector_gravity(V_gravity)
3量叉积出姿态误差
V_error acc V_gravity
4误差进行积分
V_error_I + V_error * Ki
5互补滤波姿态误差补偿角速度修正角速度积分漂移
Gyro + V_error * Kp + V_error_I
6阶龙格库塔法更新四元数
QRunge_Kutta_1st(Gyro deltaT)
7四元数化
Qnormalize()
8四元数转欧拉角
Qto_euler(&anglex &angley &anglez)
}
55 姿态控制
551 PID简单介绍
简单说PID动调节种方式输入偏差（输入偏差指调量设定值差值）进行例积分微分运算P例认杠杆输入发生改变通例作改变输出达整体稳定I积分运算时输入输出呈线性相关例达想目标需进行积分微调D微分运算较敏感称预动作检测者
552 PID参数整定
参数整定般分理计算法验法理计算法需较强数学功底需根计算结果进步修改数笔者通查阅资料知通观察曲线参数整定较效方法属验法种需收集曲线设定值调量波动PID输出然根曲线波动进行分析
整定参数原第需灵活性原性控轻易改动控制策略第二系统成整体孤立整体PID操作参数伺服放器执行器等环节需解孤立问题需复杂问题简单化明确思路全面孤立辩证统关系间相辅相成全面整体问题孤立细化问题问题简单化
553 四旋翼机姿态控制
姿态控制四旋翼机关键理讲四旋翼转速完全致情况电机24正转13反转两旋翼产生反扭力矩方相反相等飞机稳飞行实际中四旋翼识法拥相转速机常常会失衡需姿态控制算法时刻调整飞行姿态PID控制算法操作较简单资料详实控制效果终决定采方法机进行实时控制
四旋翼机串级PID控制算法具体程首先目标期角度时刻机实际测出输出值偏差进行例积分微分线性组合构成控制量然输入四旋翼机中修正机时刻状态信息次目标期值较然进行循环运算
554 PID调节序具体程序解析
PID调节序：先调PID算法处理外环调节环参数里外环指电流环环速度环选择环电流环原速度环调节命令较命令果环话会导致产生非常电流烧坏电机安全角度应该样设置外环Ｐ调节环角速度调节核心时PID算法做PIＤ三者进行结合调节直接PID串级算法效果致相
控制算法：
pidRateXmeasured MPU6050gyroX * Gyro_G 环测量值角度秒
pidRateYmeasured MPU6050gyroY * Gyro_G
pidRateZmeasured MPU6050gyroZ * Gyro_G
pidPitchmeasured Anglepitch 外环测量值
pidRollmeasured Angleroll 横滚角roll
pidYawmeasured Angleyaw
pidUpdate(&pidRolldt) 调PID处理函数处理外环 横滚角PID
pidRateXdesired pidRollout 外环PID输出作环PID期值串级PID
pidUpdate(&pidRateXdt) 调环
pidUpdate(&pidPitchdt) 调PID处理函数处理外环 俯仰角PID
pidRateYdesired pidPitchout
pidUpdate(&pidRateYdt) 调环
CascadePID(&pidRateZ&pidYawdt) 直接调串级PID函数处理
PID算法程序表示：
void pidUpdate(PidObject* pidconst float dt)
{
float error
float deriv
error pid>desired pid>measured 前角度实际角度误差
pid>integ + error * dt 误差积分累加值
deriv (error pid>prevError)dt 前两次误差做微分
pid>out pid>kp * error + pid>ki * pid>integ + pid>kd * derivPID输出
pid>prevError error 更新次误差
}
56 PWM控制输出
561 PWM简介
PWM名称脉宽度调制器定时计数器件定时计数器特点特定输入时钟通分频接计数器进行加减运算达设定值时会引发中断设定标志位PWM定时达会输出特定波形PWM输出缓寄存器控制周期较寄存器控制占空
占空指周期高电占例形象描述图示周期10s波形出周期电高低相第部分占40第二部分占60占空概念尝试周期断变灯例开始灯亮灭亮灭形式呈现达时间间隔足够时候呈现光强度变弱占空越低光强度越弱种表现认周期均电压降低总结电源电压稳定时候输出电压电压（占空100）占空电机样通调节占空调节电机转速
图54脉信号波形图
562 PWM电机控制简述
PWM通改变输出方波占空类等效电压改变改变电机转速进达机控制目PWM控制信号取决PID姿态控制信号需注意电机驱动需预留定高电低占空电机法运转
6 系统调试飞行测试
然程序较严格证段程序进行检查实际环境常常变信号传输干扰空气流动造成扰动等操作手法造成飞行稳定制作出实物需根实际情况进行微调
进行飞行测试时笔者通团队讨选择宽阔室试飞考量室墙壁反射信号机容易失控次测试阶段选择扰动量方发现问题便分析便整体修改
测试容包括机飞行移动悬停图四旋翼机飞行时图片动态性良飞行测试中四旋翼机够遥控控制方进行前进倒退左移右移倾角误差±4°飞行姿态较稳控制命令发送四旋翼机响应时间2s左右系统响应实时性较足处降落时需缓慢降否电源瞬间切断直接掉落造成危险总说测试结果达预期设计求
7 总结
次毕业设计回顾许学期间学知识高数线代控制理嵌入式单片机等课程回顾期间方面感受深深足方面学寸步难行面未学知识时候老师教授学技巧书皮文献目录找真正需东西原理解释理解数学解释续学起事半功倍效果整体言次毕业设计受益良现次毕业设计总结
现次毕业设计进行总结文四旋翼机发展入手机飞行原理飞行姿态控制进行简单描述整硬件模块进行简单介绍两铺垫软件容出现会显突兀读者快速简单解整四旋翼机进软件部分致思路实际握
文软件部分进行详细述卡尔曼滤波四元数姿态解算PID三算法进行浅薄理解详细述参考量程序进写出应程序整体言文算法述占较篇幅较通俗易懂话写出相应初次接触该领域读者点参考
参考文献
［1］程 煦 郭珊珊 陈华宾．基STM32单片机四旋翼飞行器设计［J］．技术交流201703（05）：121124
［2］张福炎马维华戴志涛等 嵌入式系统开发技术［M］ 高等教育出版社2018
［3］颜王丽丹李梦柯等．基STM32四旋翼飞行器设计实现［J］．电子设程201624（02）：187193
［4］潘春荣许化．基STM32Ｘ型四旋翼机设计［J］工程设计学报201609（16）：197201
［5］金薇．基STM32四旋翼飞行器研究设计［J］山西电子技术201601（12）：9293
［6］秦永元 惯性导航［M］ 北京 科学出版社2006
［7］周权 四旋翼飞行台飞行控制惯性导航研究［D］ 南京：南京航空航天学2008
［8］川李智王勇军．基STM32四旋翼飞行器姿态优估计［J］．电子技术应201541（12）：6164
［9］李文鹏唐海洋．基STM32四旋翼飞行器姿态解算研究［J］单片机嵌入式系统应201541（12）：1316
［10］饶文 四旋翼飞行器控制系统设计控制算法研究［D］ 合肥工业学 2017
［11］汤洋章磊张瑶．四旋翼飞行器设计制作［J］湖北理工学院学报201603（02）：58
附 录
四旋翼机整体实物图：
四旋翼机机翼实物图：
四旋翼机电池实物图：
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档
