畢業(yè)論文-旋翼飛行機器人的控制系統(tǒng)設(shè)計

+畢業(yè)設(shè)計（論文）論文題目：旋翼飛行機器人的控制系統(tǒng)設(shè)計 學生姓名 學生學號 專業(yè)班級 自動化1103班 學院名稱 電氣與信息工程學院 指導教師 教授 學院院長 2014年5月日II摘 要導傳感器精度的提高，四旋翼飛行器在近些年得到了快速的發(fā)展。如下：(1)本文針對四旋翼飛行器的機械結(jié)構(gòu)、硬件、軟件底層設(shè)計及自行設(shè)計的嵌入式遙控的硬件設(shè)計、軟件底層設(shè)計、GUI設(shè)計及其軟件實現(xiàn)流程進行了解析。(2) 本文對捷聯(lián)慣導技術(shù)進行了簡介，并對四旋翼飛行器進行了基于牛頓-歐拉公PID了卡爾曼濾波與四元數(shù)法結(jié)合進行姿態(tài)估計。在控制算法方面，嘗試了單級位置式PID與串級PID響應(yīng)快，超調(diào)量小，因此，本文最終選擇了串級PID高度傳感器的自主懸停算法。(3)為了滿足調(diào)試的需求，本文采用C#編寫四旋翼虛擬控制臺上位機，該上位機DirectX3D數(shù)據(jù)對前文的算法進行了分析對比，并最終得出結(jié)論。關(guān)鍵詞：四旋翼飛行器，姿態(tài)估計，串級PID控制IIIIDESIGNofTHECONTROLSYSTEMforROTORFLIGHTROBOTABSTRACTQuadrotor-UAVisaveryhotdirectionofUAVStudy.WiththeDevelopmentoftheIMU;Quadrotor-UAVhasgotrapiddevelopmentintheseyears.ThispaperdesignedcontrolsystemfortheQuadrotor-UAV,studyingplentyofAttitudealgorithms,controlAttitudealgorithmsandself-hoveringalgorithmsandrealizeditintheQuadrotor-UAV.Themainpurposeandresultsofthispaperasarethat:Designingthemechanicalstructure、hardware、softwarefortheQuadrotor-UAV,andalsofortheremotecontrollerandthenanalyzetherealization.ThispaperintroducestheSINSandmodeltheQuadrotor-UAVbasedonNewton-Eulerequation,andthensimplifiesthemodel.alsocomparesthedirectioncosinealgorithmandthequaternionalgorithm.Thequaternionalgorithm'scalculatedamountisquitelowandithasnosingularpoint,soIfinallychooseittocalculatetheattitude.Meanwhile,Icomparethreeattitudeerror-compensationalgorithmsandfinallychoosetheKaman-filterwithquaternionalgorithmtocalculatetheattitude.Aboutthecontrolalgorithm,ItriedclassicalPIDcontrollerandcascadePIDcontroller,inaword,thecascadePIDcontrollerresponsequicklyandovershootless.ThispaperfinallyusesthecascadePIDcontrollertocontroltheQuadrotor-UAV.Ontheotherhand,Itriednewself-hoveringalgorithmthatbasedonSINSandheightsensor.Tomeettheneedofdebug,Iusec#tomakeaQuadrotor-UAVvirtualcontrolleruppercomputersoftware,itcanrealizereal-timecommunication,controllingtheQuadrotor-UAV,anditusetheD3DtechniquetorendertheQuadrotor-UAV.Withtheexperimentinformation,IcompareddifferentkindsofalgorithmsthatImentionedbeforeandfinallygotaconclusion.Keywords:QuadrotorUAV,AttitudeCalculation,CascadePIDControllerPAGEPAGE4目 錄HYPERLINK摘要……………IHYPERLINKIIHYPERLINK第1章 緒論 1HYPERLINK1.1課題研究的背景與意義 1HYPERLINK1.2國內(nèi)外四旋翼飛行器研究現(xiàn)狀 3HYPERLINK1.3論文的主要工作 5HYPERLINK第2章 四旋翼飛行器原理與硬件及軟件設(shè)計 6HYPERLINK2.1四旋翼飛行器原理分析 6HYPERLINK2.1.1 6HYPERLINK2.1.2 7HYPERLINK2.1.3 9HYPERLINK2.2機械結(jié)構(gòu)設(shè)計及材料選擇 10HYPERLINK2.3飛控板硬件電路與底層驅(qū)動 HYPERLINK2.3.1 MCU及傳感器選擇 HYPERLINK2.3.2 12HYPERLINK2.3.3 13HYPERLINK2.4遙控硬件電路及軟件設(shè)計 13HYPERLINK2.4.1 14HYPERLINK2.4.2 3.2寸觸摸 14HYPERLINK2.4.3 14HYPERLINK2.5本章小結(jié) 17HYPERLINK第3章 四旋翼飛行器控制系統(tǒng)設(shè)計 18HYPERLINK3.1姿態(tài)估計算法 18HYPERLINK3.1.1 18HYPERLINK3.1.2 18HYPERLINK3.2姿態(tài)誤差補償算法 19HYPERLINK3.3姿態(tài)估計算法的軟件實現(xiàn) 20HYPERLINK3.4姿態(tài)控制算法 23HYPERLINK3.4.1 串級PID控制器 23HYPERLINK3.4.2 PID算法中積分飽和現(xiàn)象的幾種處理方式 24HYPERLINK3.4.3 PID算法中微分項的幾種處理方式 25HYPERLINK3.5定高控制算法 26HYPERLINK3.6基于慣導與高度傳感器的懸停算法 28HYPERLINK3.7控制算法軟件實現(xiàn) 31HYPERLINK3.8本章小結(jié) 32HYPERLINK第4章 上位機設(shè)計及實驗結(jié)果分析 33HYPERLINK4.1上位機功能簡介 33HYPERLINK4.2上位機軟件設(shè)計流程及實現(xiàn) 35HYPERLINK4.2.1 36HYPERLINK4.2.2 37HYPERLINK4.2.3 38HYPERLINK4.2.4 38HYPERLINK4.2.5 39HYPERLINK4.2.6 40HYPERLINK4.2.7 40HYPERLINK4.3實驗結(jié)果分析 41HYPERLINK4.4本章小結(jié) 46HYPERLINK第5章 總結(jié) 47HYPERLINK5.147HYPERLINK5.247HYPERLINK參考文獻 49HYPERLINK致謝…………………………..51湖南大學本科畢業(yè)設(shè)計湖南大學本科畢業(yè)設(shè)計（論文）PAGEPAGE41第1章 緒 論1.1 課題研究的背景與意義活動。無人機的發(fā)展中，以四旋翼飛行器的發(fā)展最為突出。它以其強穩(wěn)定性，低成本、高效益在戰(zhàn)爭中得到廣泛的應(yīng)用。即使如此，僅僅幾次局部戰(zhàn)爭中所運用的飛行器，也只是局限于應(yīng)用在無人偵察機，干擾微高空電子，以及照相偵查等相對比較簡單的任務(wù)。但是，目前的幾種應(yīng)用僅實現(xiàn)了很小一部分，因此，世界各國對軍事無人機技術(shù)的研究和開發(fā)都在迅猛地發(fā)展著。比如美國使用的無人偵察機“捕食者”和“全球鷹”，任務(wù)完成率高達大的幫助。在未來的信息化戰(zhàn)爭中，其地位將會得到很大的提升[1]。的作業(yè)，如航拍功能、搜索功能等等。備普及以及新的信息技術(shù)功能的實現(xiàn)使得智能化普及于現(xiàn)代社會。四旋翼飛行器就是很好的體現(xiàn)。其具有以下特點：(1) 體積較小可以工作在許多狹小的空間中，如深井、巷道、洞穴、管道等。(2)可以工作在惡劣的，危害人類健康和生命的環(huán)境中，從而最大限度地減少人員傷亡，四旋翼飛行器可以全天工作，無需休息，工作效率高。(3)多種功能。(4) 機械簡單，能夠避免復(fù)雜的空氣動力學問題。目標，如果僅僅是一些普通的偵察機，則可能受到敵方打擊而造成不必要的機體人員傷亡，因此，無人的四旋翼直升機則可以很好地起到替代作用。利用四旋翼飛行器作為偵因此，四旋翼飛行器的軍事價值不可估量[2]。外，還可以對高空空氣質(zhì)量狀況提供詳細數(shù)據(jù)?？`。實現(xiàn)多方面的利益，因此其潛力很大[3]。的研究，在大幅提高其性能的同時，也大大推動了四旋翼飛行器的發(fā)展。與傳統(tǒng)的人工作業(yè)比起，四旋翼飛行器在各個方面都有很大的現(xiàn)實效益，同時還能保證工作人員的生命財產(chǎn)安全。最大限度的利用工具創(chuàng)造更多的價值。從軍事角度來比較，無人四旋翼飛行器的運用，可以使得傳統(tǒng)的偵察機效率大大提高。成本低、隱蔽性強、安全、信息獲取能力強、保密性強。從工業(yè)作業(yè)作對比，工業(yè)的環(huán)境可以多種多樣，高溫、強輻射、高空、空間狹小、有毒等環(huán)境完全可以代替人完成其任務(wù)。甚至比同時時間將會很長，甚至會導致錯過最佳救助時間，如果使用四旋翼飛行器，在空中對災(zāi)害處進行定點搜索，然后縮小范圍搜索，可以很好地探尋到所需的目標，極大地提高了效率，并且成本較低。旋翼飛行器也具有巨大的現(xiàn)實效益。1.2 國內(nèi)外四旋翼飛行器研究現(xiàn)狀遙控航模四旋翼飛行器的典型代表是美國Dfaganflyer公司研制的DraganlyerIII和香港銀輝玩具制品有限公司研制的X.UFO。DraganflyerIII是一款世界著名的遙控航模四旋翼飛行器，主要的用途是航拍。其機體最大長度為(翼尖到翼尖)76.2cm，高18cm，重III4個電機的3個壓電晶體陀螺儀進行姿態(tài)增穩(wěn)控制。X.UFO的機體最大長度為68.5cm，高14cm；持續(xù)飛行時間約為5分鐘；其遙控距離可達100m。X.UFO的旋翼被置于發(fā)泡聚丙烯成的圓環(huán)中，比DraganflyerIII有更好的安全性。世界上對小型四旋翼飛行器的研究主要分為在3飛行控制和自主飛行器系統(tǒng)方案，其典型代表分別是：瑞士洛桑聯(lián)邦科技學院、賓夕法尼亞大學和佐治亞理工大學。航空航天大學等。如圖1-1、1-2為不同機械結(jié)構(gòu)的四旋翼飛行器。圖1-1 航拍中的四旋翼飛行器 1 (b) 2 (c) 四旋翼飛行器3 (d) 四旋翼飛行器4圖1-2 各種不同的四旋翼飛行器盡管近年來，國內(nèi)外四旋翼飛行器的研究進行得如火如荼，無論是民用還是商用的由于傳感器誤差導致的長期姿態(tài)估計產(chǎn)生的積分漂移誤差、強烈的外界干擾下的控制系統(tǒng)震蕩等不穩(wěn)定因素。因此，姿態(tài)估計算法的精度與長期穩(wěn)定性、控制算法的精度及穩(wěn)定性以及路徑規(guī)劃等問題成為了當前四旋翼飛行器研究的熱點。1.3 論文的主要工作位機以及嵌入式遙控的功能及實現(xiàn)進行詳細的闡述。本文分為五章，各章的主要內(nèi)容如下：要研究內(nèi)容。嵌入式遙控的硬件、軟件底層設(shè)計進行了介紹。計算法，對幾種四旋翼飛行器的控制算法進行了分析與實驗。第四章介紹了四旋翼飛行器虛擬控制臺上位機的設(shè)計思路及其實現(xiàn)并結(jié)合上位機論。研究方向提出一些設(shè)想。第2章 四旋翼飛行器原理與硬件及軟件設(shè)計實驗平臺的基礎(chǔ)，因此本章將著重介紹四旋翼飛行器的機械結(jié)構(gòu)與硬件電路設(shè)計。2.1 四旋翼飛行器原理分析2.1.1 捷聯(lián)式慣性導航技術(shù)尤其在現(xiàn)代軍事航空領(lǐng)域中發(fā)揮著舉足輕重的作用。早期的慣導系統(tǒng)由于采用了機械式費用昂貴，而且系統(tǒng)性能還受到機械結(jié)構(gòu)的復(fù)雜性和極限精度的制約。隨著計算機和微技術(shù)的迅猛發(fā)展，利用計算機的強大運算能力和控制功能代替?zhèn)鹘y(tǒng)機電系統(tǒng)成為可能。20世紀601969號登月飛船的應(yīng)急備份裝置，在其服務(wù)艙發(fā)生爆炸時，將飛船成功地引導到返回地球的軌道上時起到了決定性作用，成為捷聯(lián)式慣導系統(tǒng)發(fā)展中的一個里程碑[4]。或其組合等方式對慣導進行定時修正，以獲取持續(xù)準確的位置參數(shù)。慣導系統(tǒng)基本工作原理是以牛頓力學定律為基礎(chǔ)，通過測量載體在慣性參考系的加偏航角和位置信息等。在捷聯(lián)慣導系統(tǒng)中，有以下獨特優(yōu)點：(1) 去掉了復(fù)雜的平臺機械系統(tǒng)，系統(tǒng)結(jié)構(gòu)極為簡單，減小了系統(tǒng)的體積和重量，同時降低了成本，簡化了維修，提高了可靠性。(2)關(guān)的誤差。(3) 無框架鎖定系統(tǒng)，允許全方位（全姿態(tài)）工作。(4)和加速度信息。性元件及機載計算機等部件也提出了較高的要求。(1) 要求加速度表在寬動態(tài)范圍內(nèi)具有高性能、高可靠性，且能數(shù)字輸出。(2) 因為要保證大攻角下的計算精度，對計算機的速度、容量提出了較高的要求。2.1.2 四旋翼飛行器動力學分析四旋翼飛行器通常具有兩種不同的飛行方式：X型飛行方式的四旋翼飛行器的姿態(tài)改變方向與機身成45度，十字型飛行方式四旋翼飛行器姿態(tài)改變方向X型飛行方式的飛行器具有更好的控制靈敏度與穩(wěn)定性，但其物理模型較為復(fù)雜。如圖2-1為十字型與X型四旋翼飛行器動力學模型[5]。XXYX圖2-1 四旋翼飛行器動力學模型四旋翼飛行器飛行時螺旋槳高速旋轉(zhuǎn)會產(chǎn)生與螺旋槳轉(zhuǎn)向相反的反扭矩。反扭矩會造成飛行器沿著螺旋槳旋轉(zhuǎn)的方向自旋，若不能抵消反扭矩會嚴重影響飛行器的穩(wěn)定飛行，因此，四旋翼飛行器通過合理地安排四只螺旋槳的轉(zhuǎn)向來抵消各個螺旋槳產(chǎn)生的反號螺旋槳逆時針旋轉(zhuǎn)，即對角線上的螺旋角旋轉(zhuǎn)方向相同，以此抵消相互之間的反扭矩[6]。方式，因此著重對十字型飛行方式的姿態(tài)控制原理進行分析：(1)旋槳產(chǎn)生的升力變小，當升力小于飛行器重力時，飛行器擁有向下運動的加速度。(2)號螺旋槳所在的軸向方向定義為X號螺旋槳所在的軸向方向定義為Y軸方向，因此，繞著Y軸方向旋轉(zhuǎn)時，所進行的控制為俯仰控制。2號螺旋槳轉(zhuǎn)速減小，4號螺旋槳轉(zhuǎn)速增加，飛行器產(chǎn)生左傾；反之，則右傾。(3)橫滾控制：與俯仰控制相似，繞著X軸方向旋轉(zhuǎn)時，所進行的控制為橫滾控制。1號螺旋槳轉(zhuǎn)速減小，3號螺旋槳轉(zhuǎn)速增加，產(chǎn)生前傾；反之，則后傾。(4)航向控制：當1、3號螺旋槳轉(zhuǎn)速同時減小，其反扭矩和升力減??；2、4號螺旋；反之，則右旋。2.1.3 四旋翼飛行器動力學建模通過上一節(jié)四旋翼飛行器的原理分析，可以得知四旋翼飛行器時一個非線性、多變量、高度耦合、欠驅(qū)動系統(tǒng)（6個自由度，4(1) 機體坐標系原點與飛行器質(zhì)心重合。(2) 除螺旋槳旋轉(zhuǎn)產(chǎn)生的氣流外，空氣流速為零。(3) 四旋翼飛行器與螺旋槳都為剛體。(4) 機體所受空氣阻力忽略不計，不考慮地效。(5)旋槳轉(zhuǎn)速成正比。E和機體坐標系S，兩者均為右手坐標系。其中機體坐標系以1號螺旋槳反向為X軸正向，4號螺旋槳方向為Y軸正向，重力坐標系E的定義方向類似[7]。來實現(xiàn)。種運動方式建模的依據(jù)為牛頓-歐拉方程： .FmV .

H其中F為四旋翼飛行器收到的外力和，m為四旋翼飛行為四旋翼飛行器相對于地面坐標是牛頓第二定律構(gòu)建的針對四旋翼飛行器的平移運動的平移方程，MH是歐拉方程，描述剛體旋轉(zhuǎn)運動的旋轉(zhuǎn)方程。設(shè)為俯仰角，為橫滾角，為航向角，根據(jù)歐拉方程MH可以得到三軸力矩平衡方程：..

. M

/Iy

(U2Ky)/Iy.. . M/Ix(U3Kx)/Ix.. M

/I .

K

)/I4 z4 z由于四旋翼飛行器一般的飛行姿態(tài)變化較小，因此可以忽略空氣阻力的影響，由此得到簡化后的動力學模型：U2/Iy..U3/Ix..U4/Iz

是一個二階系統(tǒng)，因此本文將采用PID控制器對其進行控制。2.2 機械結(jié)構(gòu)設(shè)計及材料選擇四旋翼機架的型號為F450，重量282克，電機軸距450mm，螺旋槳采用1047型，電機則采用朗宇30A30A，短時電流極馬達2100002200mah鋰電池，該部分如圖2-2所示。 （b） 圖2-2 四旋翼飛行器機械結(jié)構(gòu)及硬件2.3 飛控板硬件電路與底層驅(qū)動如圖2-3所示為飛控板硬件電路結(jié)構(gòu)圖。陀螺儀加計陀螺儀加計MPU600磁力計HMC5883L氣壓計MS561超聲波HC-SR04藍牙4.0HM-10主控芯片S2F103圖2-3 飛控板硬件電路結(jié)構(gòu)圖2.3.1 MCU及傳感器選擇STM32F103VET6單片機作為飛控系統(tǒng)的主控芯片。STM32F系列屬于中低端的32位ARM微控制器，該系列芯片是意法半導體（ST）STM32F103VET6擁有512KFLASH以及72M主頻，芯片集成定時器，CAN，ADC，SPI，I2C，USB，UART，等多種功能，能夠滿足飛控的所有需求。傳感器采用模塊與超聲波HC-SR04，它是一個9軸模塊，其集成了三軸陀螺儀與三軸加速度傳感器MPU6050以及三軸磁力計HMC5883L，同時還配有高精度氣壓計MS5611。2.3.2通信模塊首先使用的通信模塊為2.4GSPI協(xié)議對其進行讀寫操作。在不使用功放的情況下，其具有10米左右的通信距離，該距離過短。同時，其一次最多發(fā)送32個字節(jié)，因此，若數(shù)據(jù)幀超過32個字節(jié)，則需人為進行分包處理。由于系統(tǒng)中的數(shù)據(jù)幀為72個字節(jié)，分包處理顯得較為繁瑣，于是，本文最終選擇使用藍牙4.0模塊HM-10代替NRF24L01。藍牙4.0是20123.03.03毫秒低延遲、超長有效連接距離、AES-128加密等功能。更重要的一點是藍牙4.0的通信距離可以達到602.1傳輸周期較短時會出現(xiàn)丟包現(xiàn)象，因此，本文將通信周期限制在40MS左右，這也基本滿足了控制的需求。如圖2-4所示為通信模塊原理圖。圖2-4 通信模塊原理圖2.3.3 飛控底層驅(qū)動底層驅(qū)動主要使用到的部分如下：用于調(diào)試程序以及藍牙串口的通信。ADC：用于讀取電源的電壓值，確保四旋翼飛行器具有安全的系統(tǒng)供電。I2C：用于讀取模塊的值，此處使用GPIO模擬I2C。GPIO：用于控制LED的現(xiàn)實，通過不同的閃爍以及亮滅提示四旋翼飛行器的狀態(tài)。SYSTICK：滴答定時器，用于系統(tǒng)的精確定時。(6) TIM定時器的PWM通道：使用TIM3的PWM功能輸出4路不同占空比的PWM波通過電調(diào)控制無刷電機的轉(zhuǎn)速。2.4 遙控硬件電路及軟件設(shè)計搖桿電位器搖桿電位器3.寸觸摸液晶3.V穩(wěn)壓電路藍牙40HM10主控芯片STM3F103圖2-5 遙控硬件電路結(jié)構(gòu)圖如圖2-5所示為飛控板硬件原理圖。采用的主控芯片與通信模塊與飛控板相同，因此不再贅述。2.4.1 搖桿電位器通過STM32的內(nèi)部ADC態(tài)角以及油門的遙控值。2.4.2 3.2寸觸摸液晶使用STM32的FSMC模擬ILI9341芯片的8080接口控制3.2寸觸摸液晶屏。而觸摸屏的控制器則采用了STM32的SPI接口與TSC2046進行通信讀取觸摸屏測得的觸點坐標。2.4.3 遙控軟件設(shè)計內(nèi)像素點的顏色分布，以此實現(xiàn)光源渲染效果，然而實現(xiàn)后發(fā)現(xiàn)主頻72M的STM32完全無法實現(xiàn)如此大計算量的運算和顯示，由此3D姿態(tài)顯示完全成為了雞肋，最后不得不刪除這個功能。這為之后的定高等參數(shù)顯示留出更多的空間。如圖2-6所示為嵌入式遙控功能結(jié)構(gòu)圖[8]。遙控功能姿態(tài)顯示藍牙通信波形顯示搖桿控制在線設(shè)參圖2-6 遙控功能結(jié)構(gòu)圖遙控主要功能的軟件實現(xiàn)如下：(1) 姿態(tài)顯示：位有效數(shù)字。(2) 藍牙通信：遙控與飛控同樣使用藍牙4.0，HM-10模塊，該模塊的特性不再贅述。值得一提的是，僅管藍牙4.0的數(shù)據(jù)傳輸距離比藍牙2.1遠的多，但其數(shù)據(jù)傳輸能力卻大打折扣，這使得本文不得不降低數(shù)據(jù)傳輸?shù)乃俾蕘肀ＷC通信的穩(wěn)定性。(3) 波形顯示：區(qū)溢出時，向前移動隊列，對整個波形顯示區(qū)進行刷新。(4) 搖桿控制：通過STM32自帶的ADC期望姿態(tài)角，在每個通信周期將其發(fā)送至飛控進行姿態(tài)控制。(5) 在線設(shè)參：STM32的片內(nèi)FLASH使得每次開機都能很方便的找到上一次的參數(shù)。2.5 本章小結(jié)本章對四旋翼飛行器進行了動力學建模介紹了本文所采用的四旋翼飛行器機械結(jié)構(gòu)與飛控板、遙控的硬件設(shè)計，并針對遙控的軟件實現(xiàn)部分進行了詳細的解析。第3章 四旋翼飛行器控制系統(tǒng)設(shè)計本章將著重對四旋翼飛行器的姿態(tài)估計算法及其控制算法設(shè)計進行分析對比。3.1 姿態(tài)估計算法3.1.1 四元數(shù)法i、j組成，而且它們有如下的關(guān)系：四元數(shù)的優(yōu)點是：

i2j2k2ijk

(3-1)(1) (2) (3) 單位四元數(shù)的對可以表示四維空間中的一個轉(zhuǎn)動。簡而言之，四元數(shù)包含了剛體旋轉(zhuǎn)的所有信息，而在四旋翼飛行器的姿態(tài)解算中，往往使用的是四元數(shù)微分方程對四元數(shù)進行更新[9]。3.1.2 方向余弦法余弦。通常，整個剛體的空間位形可以簡易地以以下參數(shù)設(shè)定：(1)剛體的“位置”挑選剛體內(nèi)部一點G來代表整個剛體，通常會設(shè)定物體的質(zhì)SG示位置可以應(yīng)用向量的概念。向量的起點為參考系S位置需要三個坐標，例如，采用直角坐標系，這三個坐標為x坐標、y坐標、z坐標。這用掉了三個自由度[10]。(2)等等。這些方法設(shè)定一個附體參考系B的取向（相對于空間參考系飛機的參考系則可設(shè)定為附體參考系。剛體的取向需要用到另外三個自由度。方向余弦法可以用來設(shè)定附體參考系B運算量較大。3.2姿態(tài)誤差補償算法(1) 卡爾曼濾波：卡爾曼濾波是一種具有對從包含噪聲的有限信號中預(yù)測出目標真實值的濾波算法。濾波在控制理論及控制工程中有非常廣泛的應(yīng)用[12]。(2) 互補濾波：具有短期可信，長期不穩(wěn)定的特點，因此可以使用互補濾波對其進行融合。簡而言之，對陀螺儀進行高通濾波，通過整定合適的參數(shù)來得到一個較好的濾波器特性[11]。(3) 梯度下降法：梯度下降法是一種迭代求極值的算法，通過四元數(shù)算法進行姿態(tài)解算有幾個缺點：a.沒有充分利用重力場和地磁場的特性；b.全相信”這個結(jié)果。的姿態(tài)解算中，可以把0看作是誤差函數(shù)的一個極小值[12]。3.3 姿態(tài)估計算法的軟件實現(xiàn)的誤差。如下圖3-1所示，為姿態(tài)估計算法實現(xiàn)流程圖。歐拉角歐拉角陀螺儀加計磁力計預(yù)處理四元數(shù)法卡爾曼濾波NoYES傳感器初始化圖3-1 姿態(tài)估計算法實現(xiàn)流程圖在進行姿態(tài)估計之前，需要對傳感器的原始數(shù)據(jù)進行預(yù)處理，主要分為三個部分：濾波。(1) 加速度計校準及濾波由于加速度傳感器的測量值必然存在誤差，因此，有必要在使用前對其進行校準，的理論值的平方和與加速度計三個軸得到的測量值的平方和進行做差得到重力加速度的平方的相反數(shù)，以此來構(gòu)造平方誤差，進而使用最小二乘法對誤差進行估計[13]。(2) 陀螺儀零偏計算200次陀螺儀的測量值進行均值濾波，并將其作為陀螺儀的零偏[14]。(3) 磁力計偏心矯正及傾斜補償理論上，磁力計的三個軸的測量值范圍是對稱的，即在空間中呈球體分布。然而，在原點的橢球體，因此需要對其測量值進行偏心校正[15]?；静襟E為：a.將磁力計水平旋轉(zhuǎn)360度計算其X軸與Y軸最大值與最小值，由此計算修正增益Xs與，修正偏置Xb與。這里以X軸為基準，令Xs1。sY(XXmin)s)

XX

1(X

X )X b s2 Y

1

Y )Y b s2b.通過修正增益與修正偏置計算磁力計的修正值：XoutXsXinXbYYs bc.同樣以X軸為基準，對Z軸進行矯正，最終得到三個軸的修正值。XoutXsXinXbYYY Z ZZZZout s in b

(3-4)(3-5)d.磁力計傾斜補償器并不是保持水平的，通常它和水平面都有一個夾角。這個夾角會影響航向角的精度，需要通過姿態(tài)角進行傾斜補償。傾斜補償?shù)墓饺缦拢篴rctan(mx*sin(Rol)*sin(Pit)my*cos(Rol)*sin(Roll)*cos(Pit),如圖3-2所示為磁力計校準前后效果圖。(4) 姿態(tài)估計

mx*cos(Pit)mz*sin(Pit))

(3-6)誤差輸入PI控制器后與本次姿態(tài)更新周期中陀螺儀測得的角速度相加，最終得到一個修正的角速度值，將其輸入四元數(shù)微分方程，更新四元數(shù)[16]。四元數(shù)微分方程如下：圖3-2 磁力計校準前后效果圖*gx*gy*gz)*T/21 1 0 x 2 z 3 y Q(1 1 0 x 2 z 3 y*gy*gz*gx)*T/23 3 0 z 1 y 2 xQQ3 3 0 z 1 y 2 x值得注意的是，每次四元數(shù)更新之后需要將其歸一化。

(3-7)使用PI控制器對加速度計測得的重力向量與四元數(shù)換算的重力向量的誤差進行最得到估計的角速度后，將其輸入四元數(shù)微分方程更新四元數(shù)，進行迭代[17]。元數(shù)之后，一般就可以將其轉(zhuǎn)換成歐拉角進行PID加速度計一起融入PI控制器或使用互補濾波等算法對航向角修正兩種，也就是典型的AHRS和IMU算法的區(qū)別。本文擇了IMU，因為將磁力計融合四元數(shù)對于使用無刷電機的四旋翼而言是非常危險的，無刷電機產(chǎn)生的不均勻磁場會對磁力計產(chǎn)生巨大的干三點：a.濾波器的參數(shù)不合理；b.磁力計未做校準；c.引入了航向角的計算環(huán)節(jié)。本文改用了卡爾曼濾波，并針對磁力計在180度到-180度之間的突變進行了反轉(zhuǎn)，由此解決了卡爾曼濾波出現(xiàn)的反向旋轉(zhuǎn)到正確角度的現(xiàn)象。但卡爾曼的響應(yīng)并不是特別理想，當然，對于四旋翼而言，航向角的控制主要是將其角速度控制為0即可，而對于全姿態(tài)的姿態(tài)控制而言，這恐怕是遠遠不能滿足要求的[18]。此外還有梯度下降法進行姿態(tài)解算，這其實就是把牛頓高斯算法改為梯度下降法，其原理為將0看成是誤差的一個極小值，而梯度的方向為極大值的方向，因此只需要逆STM32的運算能力之內(nèi)[19]。3.4姿態(tài)控制算法3.4.1串級PID控制器一些控制系統(tǒng)把數(shù)個PID的速度是由控制馬達位置的主控制器來管理的。對于本系統(tǒng)則采用了將角度控制與角速度控制級聯(lián)的方式組成整個串級PID控制器。如圖3-3所示為串級PID示意圖。無刷電機無刷電機姿態(tài)解算陀螺儀加速度計200HZ400HZ圖3-3 四旋翼飛行器串級PID示意圖串級PID夠直接對系統(tǒng)的角速度進行較好的閉環(huán)控制，必然會改善系統(tǒng)的動態(tài)特性及其穩(wěn)定性，角的精確控制[20]。3.4.2PID算法中積分飽和現(xiàn)象的幾種處理方式本文在PID算法中引入積分項對四旋翼飛行器進行控制的主要目的在于消除系統(tǒng)PID進入飽和區(qū)后，相對于PD控制，其需要更多的時間用于退出飽和區(qū)，這會導致系統(tǒng)的飽和現(xiàn)象。(1) 積分分離法:較大時，忽略積分項的作用，即采用PD控制，以避免系統(tǒng)由于積分飽和而產(chǎn)生不穩(wěn)定的現(xiàn)象；當輸入量與設(shè)定值的偏差接近于0PID起到消除靜差的作用，提高控制精度。設(shè)為積分項前的增益系數(shù)，則有以下表達式：ekek其中為設(shè)定的閾值，ek為輸入量與設(shè)定值的偏差。

(3-8)(2) 遇限削弱積分法(k)(k是否超出限制范圍，若超出范圍則只累加負偏差，反之，則累加正偏差，采用該算法來避免控制量長期停留在飽和區(qū)。(3) 變速積分法越大積分速度越慢，偏差越小積分速度越快。設(shè)積分項前的系數(shù)為f，則積分項表達式為kuikie(j)f(e(k))e(k)f的表達式為

j0

(3-9)f(e(k))Ae(k)B

e(k)B,e(k)AB A

e(k)AB (3-10)上三種算法對于解決積分飽和現(xiàn)象都有較為顯著的效果且均較易實現(xiàn)[21]。3.4.3 PID算法中微分項的幾種處理方式結(jié)合不同的PID(1)而從理論上講，應(yīng)當對陀螺儀的零點進行動態(tài)修正，以保證微分項的準確性。(2)一階慣性濾波，使微分項較為平滑。(3)使用微分先行PID較為準確的姿態(tài)角，因此，將其用于微分項在理論上較為合理。方法的前提為，具備響應(yīng)極快的姿態(tài)解算算法且控制頻率較高[22]。3.5 定高控制算法3.6 基于慣導與高度傳感器的懸停算法姿態(tài)角角速度環(huán)PD控制四路PWM姿態(tài)角角速度環(huán)PD控制四路PWM角度環(huán)PID控制陀螺儀加速度計加速度計速度環(huán)PD控制油門積分及濾波高度環(huán)PID控制超聲波四路電機圖3-8 四旋翼飛行器控制算法軟件實現(xiàn)流程圖四路電機如圖3-8為四旋翼飛行器控制算法軟件實現(xiàn)流程圖，控制算法主要分為兩部分：姿PID控制對姿態(tài)角進行閉環(huán)控制，高度控制通過加速度計與超聲波融合得到并通過方向余弦矩陣分解得到的豎直速度與超聲波得到的高度進行串級PID控制。3.8 本章小結(jié)本章主要討論了四旋翼飛行器的幾種自主懸停算法的可行性，傳統(tǒng)的位移法在沒有位移傳感器的介入下計算位移會產(chǎn)生積分漂移，而本文所討論的算法均從微元的角度出發(fā)，由小及大，其難點在于對加速度計的濾波處理。另一方面，本文將超聲波與加速度計融合后通過串級PID控制器進行高度控制，同時使用串級PID控制器進行姿態(tài)控制。第4章 上位機設(shè)計及實驗結(jié)果分析結(jié)論。4.1 上位機功能簡介上位機是所有在線實時系統(tǒng)監(jiān)測調(diào)試仿真必不可少的一個環(huán)節(jié)，它不僅可以實現(xiàn)在線監(jiān)測，并且可以嵌入算法以及調(diào)用諸如Matlab等仿真軟件生成的DLL進行在線仿真計算，以及三維重現(xiàn)等可視化功能，因此上位機在四旋翼飛行器調(diào)試過程中也是必不可Qt等等[24]。仿真，重構(gòu)。(1)理。這里僅使用串口通信，至于TCP/UDP等網(wǎng)絡(luò)通信方式亦是大同小異的，不同之處中經(jīng)常會出現(xiàn)意想不到的問題，如死鎖，多線程的邏輯錯誤等。(2)時性的前提下，完成了高效的通信。(3) 8攝像頭的圖像格式），設(shè)計的初衷是便于顯示航拍圖像。至于波形顯示，則更是必不可少的部分，在調(diào)試過程中，其可將如角度信息，PID的各項信息進行波形顯示，便于使用者有針對性波形數(shù)據(jù)也為理論分析提供了大量依據(jù)。(4) PID在接收到下位機的ACK信號之后進行相關(guān)的顯示。(5) 重構(gòu)。這里的重構(gòu)指的是三維姿態(tài)顯示，最初采用的是OpenGL(OpenGL是個專業(yè)的圖形程序接口，是一個功能強大，調(diào)用方便的底層圖形庫)，而在.NET環(huán)境下，主要有SharpGL和CSGL。由于微軟的extension，簡稱DX）是由微軟公司創(chuàng)建的多媒體編程接口)和OpenGL一直處于競爭關(guān)系，因此C#對于OpenGL的支持并不友好，效率低下，于是本文嘗試使用C++編寫OpenGL程序，然而導入C#程序卻十分困難，最后只能寫成兩個程序，在C#中通過調(diào)用進程的方式處理，顯然，這樣十分別扭，于是，本文不得不放棄使用OpenGL，轉(zhuǎn)而使用微軟的DirectX，初步學習DirectX后，發(fā)現(xiàn)使用C#編寫DirectX程序十分簡便，于是使用3dsMax制作了四旋翼飛行器的三維模型，簡單的渲染之后就可以用DirectX導入了[25]。如圖為上位機主界面圖。4.2 上位機軟件設(shè)計流程及實現(xiàn)如圖4-2為上位機功能結(jié)構(gòu)圖。本節(jié)將介紹上位機功能的軟件實現(xiàn)流程。上位機功能上位機功能三維姿態(tài)重構(gòu)串口通信波形顯示參數(shù)顯示及報警控制及參數(shù)設(shè)定彩色/灰度圖像顯示DirectX3D3dsMax建模SerialPor類MeasurementStudioRGB565圖像顯示8位灰度圖像顯示圖4-2 上位機功能結(jié)構(gòu)圖4.2.1 串口通信本上位機中的串口通信主要使用SerialPort類實現(xiàn)，SerialPort類中封裝了對串口底層硬件的寄存器訪問，可以很容易地通過SerialPort的類方法來直接對串口的接收緩沖區(qū)以及發(fā)送緩沖區(qū)中的數(shù)據(jù)進行操作，設(shè)置串口的波特率，校驗位，停止位以及串口通道自然也是必不可少的。然而，由于Windows操作系統(tǒng)下的硬件是對所有進程共享的，即任何在生命周期內(nèi)的進程都擁有對硬件的訪問權(quán)（少數(shù)對訪問權(quán)做了限制的硬件除串口通信另一個需要注意的地方是其執(zhí)行效率。通常，在SerialPort類的DataReceived事件中進行數(shù)據(jù)的接收處理，每當串口緩沖區(qū)中的數(shù)據(jù)達到配置的閾值，則會觸發(fā)該事件，而這個閾值通常設(shè)定為以滿足用戶的需求。在該事件中，只進行了數(shù)據(jù)幀的解碼并將相關(guān)資源釋放以及更新相ThreadPool開啟幾個工作者線程進行數(shù)據(jù)處理和顯示，這樣做的好處是避免了線程同步的問題，同時提高了程序執(zhí)行效率。由于DataReceived事件觸發(fā)時會自動分配一個輔助線程，該線程用于運行DataReceived事件，用于響應(yīng)用戶輸入的線程稱之為主UI線程，而由于本文所使用的所有控件均創(chuàng)建于主UI線程，因此在DataReceived事件中訪問控件需要使用委托，這么做的目的是為了保證線程安全。雖然在C#中可以通過配置編譯器的設(shè)置來忽略跨線程訪問控件時不使用委托的警告，但事實上這并不安全[26]。4.2.2上、下位機通信協(xié)議按照數(shù)據(jù)幀的類型對幀頭以及數(shù)據(jù)幀長度進行了分類，主要的格式為：幀頭（2個字節(jié)）+幀長度（2字節(jié)）+數(shù)據(jù)幀全時的棄幀策略。由于下位機的數(shù)據(jù)類型中含有Float類型，因此綜合考慮到程序效率與顯示精度，本文采用了將單個浮點數(shù)據(jù)在下位機拆分成4個字節(jié)進行發(fā)送，它們分別為：符號字節(jié)，整數(shù)部分高8位，整數(shù)部分低8位，以及小數(shù)部分前兩位。在接收到完整的數(shù)據(jù)幀后將對應(yīng)數(shù)據(jù)進行還原。解碼的基本思路如圖4-4所示：根據(jù)幀頭分類根據(jù)幀頭分類填充緩沖區(qū)數(shù)據(jù)處理顯示棄幀處理接收完畢數(shù)據(jù)完整NOYES等待數(shù)據(jù)圖4-4 解碼流程圖4.2.3 波形顯示及分析由于C#并沒有自帶的用于顯示波形的控件，那么就只能通過GDI+技術(shù)，DirectX技術(shù)或是第三方的SDK。GDI+繪圖效率較低，而使用DirectX制作二維控件有些大材小用，權(quán)衡之后采用了NI的MeasurementStudio。它是一個第三方的SDK，其中有許多控件可供使用，例如虛擬示波器等。并且其控件風格與Labview十分相似，唯一的缺陷在于，該SDK的控件功能與Labview版本無法相提并論，就虛擬示波器來講，其只具備顯示和標記的功能，而很多原本十分實用的功能則被刪除了，例如波形縮放。歷史數(shù)據(jù)的極值，最值，并在波形圖中對其位置進行跟蹤標示[27]。如圖4-5所示為波形顯示圖。圖4-5 4.2.4 彩色及灰度圖像顯示該功能用于顯示航拍拍攝到的圖像，主要支持的格式分為16位RGB565彩色圖以及8位灰度圖。(1) 16位RGB565彩色圖。對接收到的完整的圖像數(shù)據(jù)進行還原，并且對數(shù)據(jù)進行量化補償，具體方法如下：將數(shù)據(jù)填充至高位對于低位，用原始數(shù)據(jù)的低位進行補償。例子：16位RGB565->24位RGB888轉(zhuǎn)換16位RGB565：{R4R3R2R1R0}{G5G4G3G2G1G0}{B4B3B2B1B0}24位RGB888:{R4R3R2R1R0000}{G5G4G3G2G1G000}{B4B3B2B1B0000}像素值組合成一幅完整的圖像，使用PictureBox進行顯示。(2) 8位灰度圖原，因此只需要將數(shù)據(jù)整合成圖像并顯示即可。4.2.5 狀態(tài)參數(shù)顯示及報警圖4-6 四軸狀態(tài)參數(shù)顯示及報警如圖4-6為四軸狀態(tài)參數(shù)顯示及報警界面，該部分主要用于顯示四旋翼飛行器的狀態(tài)參數(shù)以及在線設(shè)置飛行器的PIDDirectX.DirectSound空間下的一些類方法分[27]。4.2.6 三維姿態(tài)重構(gòu)使用D3DMesh類保存模型的頂點數(shù)據(jù)以及其法線向量等信息，并使用相關(guān)類方法進行渲染。如圖4-7所示為姿態(tài)更新的流程圖。三維渲染三維渲染提取姿態(tài)信息姿態(tài)更新載入三維模型等待數(shù)據(jù)數(shù)據(jù)幀接收完畢?NOYES圖4-7 姿態(tài)更新流程圖4.2.7 飛行器遠程控制及參數(shù)設(shè)定該部分功能充當遙控的角色，主要用于調(diào)試四旋翼飛行器，通過旋鈕向四旋翼飛行器發(fā)送期望姿態(tài)角以及油門值。同時，在飛行器鎖定狀態(tài)下，可以在線設(shè)定PID參數(shù)，當下位機收到完整的PIDPID參數(shù)設(shè)定成功以及指示燈的提示，該功能配合藍牙，無線等模塊使遠距離操