【正文】 X=Aircraft_Angle_X*Aircraft_P_X。 //微分 OutData[2]=nD_X。 n_Speed_Y=nP_Y+nD_Y。 nP_Y=Aircraft_Angle_Y*Aircraft_P_Y。 OutData[1]=nI_X 。//。 Aircraft_Angle_X+=(Air_GyroVal_X+Value_Register)*。 endif void Angle_calculate(void) { float Value_Register。 define Filter_Method 1 // 濾波方式選擇， 0：卡爾曼濾波 其他：互補(bǔ)濾波 if Filter_Method == 0 //卡爾曼濾波 else //互補(bǔ)濾波 float Aircraft_P_X=。 float Speed_Out_X_L_Old,Speed_Out_X_R_Old,Speed_Out_Y_L_Old,Speed_Out_Y_R_Old。 } } SPI_NRF_WriteReg(NRF_WRITE_REG+STATUS,state)。 HOVER=0。 BACK=0。 FORWARD=0。 DOWN=0。 UP=0。 STOP=0。 } if(i==5) Uart1Purchar(0x11)。 // } void NRF_Check(void) { u8 buf[5]={0xC2,0xC2,0xC2,0xC2,0xC2}。 //設(shè)置陀螺儀的量程為177。 Init_MPU6050()。 GPIO_Configuration()。 當(dāng)然還有 王 **院長(zhǎng)及所有給我上過(guò)課的老師們，是你們將我的自動(dòng)化知識(shí)體系一點(diǎn)一點(diǎn)筑起來(lái)的，這些知識(shí)，足以讓我在社會(huì)立足，找到作為自動(dòng)化人的自信！謝謝！ 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 26 參考文獻(xiàn) [1]劉杰 .四軸飛行器研究與設(shè)計(jì) [D].南京：南京郵電大學(xué) . [2] Unmanned Aerial Vehicles Dynamic Model[R]. [3]梁延德，程敏，何福本，李航 .互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算 [J].大連：大連理工 大學(xué) .2020:3011 [4]聶博文，馬宏緒，王劍， 王建文 .微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù) [J].長(zhǎng)沙 國(guó)防科學(xué)技術(shù)大學(xué) .:146 [5]楊慶華，宋召青，時(shí)磊 .四旋翼飛行器建模、控制與仿真平 [J].煙臺(tái)：海軍航空工程學(xué) 學(xué)報(bào) .:524 [6]楊素行 .模擬電子技術(shù)基礎(chǔ)簡(jiǎn)明教程（第三部版） [M].北京：高等教育出版社 . [7]張樹粹，孟佳娜，劉迎軍 .C/C++程序設(shè)計(jì) [M].北京：清華大學(xué)出版社 . [8]劉勝，張紅梅 .最優(yōu)估計(jì)理論 [M].北京：科學(xué)出版社 .2101 [9]秦永元，張洪鉞，汪叔華 .卡爾曼濾波與組合導(dǎo)航原理 [M].西安：西北工業(yè)大學(xué)出版 社 . 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 27 附錄 A 部分代碼 int main(void) { CCR0_Val= ADC_Configuration()。先調(diào)試 X 軸的參數(shù)，是飛行器能在該軸上趨于平衡，在調(diào)試 Y 軸參數(shù)。電機(jī)輸出方向必須正確 ，否則飛行器無(wú)法正常飛行，測(cè)試任何一個(gè)量，在不確定符號(hào)的情況下，需要將別的輸出量屏蔽掉，找對(duì)方向了再加回原來(lái)的量，這樣不易出錯(cuò)。 提出假設(shè)并實(shí)驗(yàn)驗(yàn)證，排查結(jié)果如下： （ 1） 最小系統(tǒng)板設(shè)計(jì)電磁兼容性差，高頻信號(hào)影響了通信： NRF24L01 通信頻段為，最小系統(tǒng)板上沒有能達(dá)到該頻段的器件，該假設(shè)不成立。讀取 MPU6050 需要先對(duì)芯片控制寄存器寫入控制字，設(shè)定工作模式后，才能從存儲(chǔ)相應(yīng)軸數(shù)據(jù)的寄存器讀取測(cè)量值。 多維度角度融合算法軟件實(shí)現(xiàn)如圖 所示。本系統(tǒng)應(yīng)用的是 PID 模型是根據(jù)硬件特性類比 PID 算法得出的控制算法，在編程上有一定的共同之處。在自動(dòng)控制理論課中證明過(guò) PID 控制的諸多優(yōu)點(diǎn)，筆者選擇 PID 除了能從書本上的證明論證可行性 之外，還從飛行器自身的特性來(lái)考慮。 由此將兩者特性結(jié)合起來(lái)應(yīng)用，可以滿足濾除四旋翼飛行器傾角信號(hào)噪聲的缺點(diǎn)，還原飛行器真實(shí)的角度。 互補(bǔ)濾波器可行性分析 IIC 采集回來(lái)的傾角數(shù)據(jù)是有噪聲的，如圖 所示。價(jià)格也圖 系統(tǒng)鎖相環(huán)時(shí)鐘配置驅(qū)動(dòng)程序 I I C 配 置 寫 控 制 字 讀 取 數(shù) 據(jù)圖 MPU6050 配置流程 圖 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 14 不高。 RCC 配置如圖 所示。同時(shí)，通過(guò)配置功能寄存器，還可以得到芯片內(nèi)部自行融合的三軸角度值，除此之外，MPU6050 還集成了基于地球磁場(chǎng)感應(yīng)的三軸數(shù)據(jù)功能，加上該傳感質(zhì) 量輕，輸出數(shù)據(jù)平滑，是航模制作中常用的傾角傳感器，其原理圖如圖 所示。 圖 Altium Designer 登陸界面 圖 虛擬示波器界面 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 10 4 四旋翼飛行器 詳細(xì) 方案 設(shè)計(jì) 本章節(jié)將從硬件及相應(yīng)的驅(qū)動(dòng)軟件進(jìn)行講解四旋翼飛行器的詳細(xì)方案設(shè)計(jì)。本設(shè)計(jì)采用的是本公司的試用版的 Altium Designer 進(jìn)行雙層 PCB 電路板的繪制。 3 四旋翼 飛行器總體設(shè)計(jì) 功能模塊劃分 系統(tǒng)可以劃分成硬件和軟件兩大部分。 當(dāng)四旋翼飛行 器沿 Z 軸旋轉(zhuǎn)時(shí)，其轉(zhuǎn)過(guò)的角度稱之為偏航角，該姿態(tài)依靠對(duì)角兩組電機(jī)的反作用旋轉(zhuǎn)扭矩失衡來(lái)實(shí)現(xiàn)偏航運(yùn)動(dòng)。 四旋翼飛行器可以實(shí)現(xiàn)多維度的運(yùn)動(dòng)姿態(tài)控制，最基本的姿態(tài)是自平衡懸停。完成畢業(yè)設(shè)計(jì)后，對(duì)今后的工作、研究提出指導(dǎo)性展望。 第三部分：四旋翼飛行器總體方案設(shè)計(jì) 對(duì)四旋翼的功能模塊進(jìn)行劃分并給出系統(tǒng)模塊設(shè)計(jì)圖及系統(tǒng)工作流程圖，最后簡(jiǎn)單介紹本系統(tǒng)開發(fā)調(diào)試的平臺(tái)工具。 Stanford 的 STAMAC 平臺(tái)如圖 所示： (3) 賓夕法尼亞大學(xué)大學(xué)的四軸飛行器 傳遍全世界的 TED 視頻，來(lái)自賓夕法尼亞大學(xué)的四軸飛行器依靠室內(nèi)攝像頭識(shí)別飛行器上標(biāo)記球，完成各種飛行動(dòng)作。從地上跑的自平衡機(jī)器人到天上飛的無(wú)人飛行器都離不開這些微電子技術(shù)及慣性傳感器。硬件上，采用 STM32F103 作為微控制器，以 MPU6050 作為四旋翼飛行器姿態(tài)傳感器件，通過(guò) AO3402MOS 管驅(qū)動(dòng)四個(gè)空心杯電機(jī)改變飛行器姿態(tài)，設(shè)計(jì)結(jié)果是能準(zhǔn)確測(cè)量飛行器姿態(tài)并將測(cè)量角度輸出給相應(yīng)坐標(biāo)的電機(jī)，進(jìn)行姿態(tài)調(diào) 整。自平衡 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） Abstract: This paper is a research about algorithm of Quadrotor Microaircraft Selfbalancing. It will detailed analysis the idea about using Complementary filter deal with the digital signals and how to set the paramerers. Using the data after filtering for fusing of Quadrotor attitude Angle and calculating the Quadrotor Microaircraft Pitch angle, Roll angle and Yaw angle. On the basis of flyingAngle using PID algorithm controlling Quadrotor Microaircraft achieves the selfbalancing hovering and relating motion control. Hardware uses STM32F103 as micro controller, with MPU6050 as attitude sensor of Quadrotor Microaircraft, through AO3402MOS tube driving four hollow cup motor to change the spacecraft attitude. The design result can accurately measure spacecraft attitude and output the measuring Angle to the corresponding coordinates of the motor and realize the attitude adjustment. This article will show the whole production process of the Quadrotor Microaircraft in detail from the hardware, software, the initialization, control algorithm, debug and so on. Key words: Micro four rotor aircraft。國(guó)防軍工事業(yè)離不開無(wú)人飛行器，四旋翼飛行器的優(yōu)異性能使它在偵查、監(jiān)視、跟蹤等任務(wù)中獨(dú)領(lǐng)風(fēng)騷；現(xiàn)代農(nóng)業(yè)，各式各樣的四旋翼農(nóng)藥噴灑飛行器呼嘯于各大農(nóng)場(chǎng)的上空；四旋翼飛行器的研究，不僅具有學(xué)術(shù)研究意義，還有可觀的經(jīng)濟(jì)現(xiàn)實(shí)意義，是學(xué)以致用的重要實(shí)踐過(guò)程。 四旋翼在未來(lái)，將變得更加的智能化，隨著各種傳感器的不斷發(fā)展，四旋翼飛行器穩(wěn)定性必將得到極大的提升，負(fù)重能力及續(xù)航能力都將大幅度提高。 第五部分：四旋翼飛行器控制算法實(shí)現(xiàn) 本部分提出應(yīng)用互補(bǔ)濾波器來(lái)處理角度、角速度數(shù)據(jù)的想法并分析互補(bǔ)濾波器在本模型中的可行性，最后附上互補(bǔ)濾波器的軟件實(shí)現(xiàn)思路；運(yùn)算出傾角后，提出應(yīng)用 PID 來(lái)控制飛行器姿態(tài)的觀點(diǎn)并分析可行性。也不像直升飛機(jī)那樣，需要設(shè)計(jì)一個(gè)消除反作用 旋轉(zhuǎn)扭矩的槳葉。 當(dāng)四旋翼飛行器的四個(gè)電機(jī)輸出的合升力大于其本身所受重力時(shí)，在無(wú)外力作用下，四旋翼能一直保持上升飛行，反之則下降。在此基礎(chǔ)上，僅需要對(duì)系統(tǒng)模型的控制模型進(jìn)行分析即可得知系統(tǒng)設(shè)計(jì)是否可行。 筆者在本次設(shè)計(jì)中重點(diǎn)做控制算法這一塊，故會(huì)用比較大的篇幅來(lái)論述控制算法的實(shí)現(xiàn)及實(shí)際調(diào)試的一些總結(jié)、體會(huì)。 AD 公司的軟件圖標(biāo)如圖 8 所示。 本次設(shè)計(jì)用到了 GPIO、 IIC、 SPI、定時(shí)器、 ADC 這幾個(gè)功能。詳細(xì)設(shè)計(jì)見原理圖 。 NRF24L01 初始化 NRF24L01 模塊采用的是 SPI 通信協(xié)議， SPI 和 IIC 都是基于串行數(shù)據(jù)總線通信的協(xié)議，配置起來(lái)有許多相似的地方。詳細(xì)的 PWM 配置見圖 ?；パa(bǔ)濾波的原理框圖如圖 所示。在程序中已經(jīng)做了詳細(xì)的注釋，此處不再重復(fù)。 然后，角度值與機(jī)體的角度看似呈線性變化的，聯(lián)系 PID 中的比例分量，兩者用著共* ( a ng l e gy r o* dt) ( 1 ) * a c can gle K p K p? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（ 2 ）圖 互補(bǔ)濾波器算法軟件實(shí)現(xiàn) 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 18 同的物理意義，由此可以將 角度值乘以一個(gè) Kp 得到比例分量。其編程語(yǔ)句如圖 所示。 在所有的角度控制子程序中，所有的變量都是 float 型在，其它的子程序用到的變量聲明可在附錄 A 部分代碼中查閱，此處不再重復(fù)說(shuō)明。查閱了相關(guān)資料，發(fā)現(xiàn)在STM32 系列芯片中，數(shù)據(jù)轉(zhuǎn)換不需加強(qiáng)制轉(zhuǎn)換符且字符型轉(zhuǎn)換成 short 型后，可以向更高一級(jí)的數(shù)據(jù)類型轉(zhuǎn)換運(yùn)算。 由此得出的結(jié)論是：無(wú)法正常通信的原因是電源電源拉低后 NRF 無(wú)法正常工作，總結(jié)的經(jīng)驗(yàn)是選購(gòu)電池是應(yīng)該選容量大的電池或者將 NRF 直接獨(dú)立供電。 姿態(tài)角度數(shù)據(jù)融合功能 互補(bǔ)濾波器的一共有三個(gè)參數(shù)要整定，如圖 互補(bǔ)濾波器子程序所示，數(shù)字歸一本來(lái)的意思是回歸數(shù)據(jù)的原本物理意義，在此筆者乘以 并沒有回歸到角度和角加速度的物理數(shù)值上來(lái)，這樣做只是為了將數(shù)據(jù)縮小 10 倍放在數(shù)據(jù)溢出。 這期間的收獲有很多，最主要的是磨練了我的毅力。0。 NVIC_Configuration()。 while(1) { debug0()。 //設(shè)置加速 度計(jì)的量程為177。 /*寫入 5 個(gè)字節(jié)的地址 . */ SPI_NRF_WriteBuf(NRF_WRITE_REG+TX_ADDR,buf,5)。 /*