四軸飛行器定速巡航系統(tǒng)設(shè)計(jì)畢設(shè)論文

┊┊┊┊┊┊┊┊┊┊┊┊┊裝┊┊┊┊┊訂┊┊┊┊┊線┊┊┊┊┊┊┊┊┊┊┊┊┊長(zhǎng)春大學(xué)畢業(yè)設(shè)計(jì)（論文）共18頁(yè)第18頁(yè)四軸飛行器定速巡航系統(tǒng)設(shè)計(jì)[摘要]四軸飛行器作為低空低成本的遙感平臺(tái)，在各個(gè)領(lǐng)域應(yīng)用廣泛。相比其他類(lèi)型的飛行器，四軸飛行器硬件結(jié)構(gòu)簡(jiǎn)單緊湊，而軟件復(fù)雜。本文介紹四軸飛行器的一個(gè)實(shí)現(xiàn)方案，重點(diǎn)講軟件算法，包括加速度計(jì)校正、姿態(tài)計(jì)算和姿態(tài)控制三部分。校正加速度計(jì)采用最小二乘法。計(jì)算姿態(tài)采用姿態(tài)插值法、梯度下降法或互補(bǔ)濾波法，需要對(duì)比這三種方法然后選出一種來(lái)應(yīng)用?？刂谱藨B(tài)采用歐拉角控制或四元數(shù)控制。最后比較各種方法的效果，并附上C語(yǔ)言的算法實(shí)現(xiàn)代碼。[關(guān)鍵詞]四軸飛行器；姿態(tài)；控制Fouraircraftcruisecontrolsystemdesign[Abstract]Quadcopterisalow-costlowaltituderemotesensingplatforms,Whichwidelyusedinvariousfields.Comparedtoothertypesofaircraft,thequadcopterhassimplerhardware,resultinmorecomplexsoftware.Thispaperdescribesanimplementationofthequadcopter,includingthecalibrationofaccelerometer,estimationofattitude,andcontrolofattitude.ItusedtheLeastSquaresMethodtocalibrationtheaccelerometer.ItusesAttitudeInterpolationMethod,GradientDescentMethodorComplementaryFilterMethodtoestimationattitude.ItusesEuleranglesorquaterniontocontroltheattitude.Finally,therearecomparisonsofthemethods.TheC-languageimplementationofthemethodsisappended.[Keywords]Quadcopter；Attitude；Control目錄TOC\o"1-3"\h\u26801.引言 4201932.飛行器的構(gòu)成 532072.1硬件構(gòu)成 523422.1.1機(jī)械構(gòu)成 5122932.1.2電氣構(gòu)成 6278712.2軟件構(gòu)成 7191692.2.1.上位機(jī) 7144292.2.2.下位機(jī) 7189013.飛行器原理 7302963.1.坐標(biāo)系統(tǒng) 8168913.2.姿態(tài)的表示和運(yùn)算 8204173.3.動(dòng)力學(xué)原理 9265814.姿態(tài)測(cè)量 10248564.1.傳感器校正 10245134.1.1.陀螺儀 10224274.1.2.加速度計(jì)和電子羅盤(pán) 11150994.2.數(shù)據(jù)融合 11319444.2.1.概述 11198484.2.2.姿態(tài)插值法 11192034.2.3.梯度下降法 12114714.2.4.互補(bǔ)濾波法 13154605.姿態(tài)控制 14255135.1歐拉角控制 14197035.2.四元數(shù)控制 1541386.算法效果 15132966.1加速度計(jì)校正 151116.2姿態(tài)計(jì)算 15132277.結(jié)論及存在的問(wèn)題 1620285參考文獻(xiàn)： 17引言四軸飛行器最開(kāi)始是由軍方研發(fā)的一種新式飛行器。隨著MEMS傳感器、單片機(jī)、電機(jī)和電池技術(shù)的發(fā)展和普及，四軸飛行器成為航模界的新銳力量。到今天，四軸飛行器已經(jīng)應(yīng)用到各個(gè)領(lǐng)域，如軍事打擊、公安追捕、災(zāi)害搜救、農(nóng)林業(yè)調(diào)查、輸電線巡查、廣告宣傳航拍、航模玩具等，已經(jīng)成為重要的遙感平臺(tái)。以農(nóng)業(yè)調(diào)查為例，傳統(tǒng)的調(diào)查方式為到現(xiàn)場(chǎng)抽樣調(diào)查或用航空航遙感。抽樣的方式工作量大，而且準(zhǔn)確性受主觀因素影響；而遙感的方式可以大范圍同時(shí)調(diào)查，時(shí)效性和準(zhǔn)確性都有保證，但只能得到大型作物的宏觀的指標(biāo)，而且成本很高。不連續(xù)的地塊、小種作物等很難用上遙感調(diào)查。因此，低空低成本遙感技術(shù)顯得相當(dāng)重要，而四軸飛行器正符合低空低成本遙感平臺(tái)的要求。目前應(yīng)用廣泛的飛行器有：固定翼飛行器和單軸的直升機(jī)。與固定翼飛行器相比，四軸飛行器機(jī)動(dòng)性好，動(dòng)作靈活，可以垂直起飛降落和懸停，缺點(diǎn)是續(xù)航時(shí)間短得多、飛行速度不快；而與單軸直升機(jī)比，四軸飛行器的機(jī)械簡(jiǎn)單，無(wú)需尾槳抵消反力矩，成本低。本文就小型電動(dòng)四軸飛行器，介紹四軸飛行器的一種實(shí)現(xiàn)方案，重點(diǎn)講解四軸飛行器的原理和用到的算法，并對(duì)幾種姿態(tài)算法進(jìn)行比較。飛行器的構(gòu)成四軸飛行器的實(shí)現(xiàn)可以分為硬件和軟件兩部分。比起其他類(lèi)型的飛行器，四軸飛行器的硬件比較簡(jiǎn)單，而把系統(tǒng)的復(fù)雜性轉(zhuǎn)移到軟件上，所以本文的主要內(nèi)容是軟件的實(shí)現(xiàn)，特別是算法、公式的推導(dǎo)。2.1硬件構(gòu)成飛行器由機(jī)架、電機(jī)、螺旋槳和控制電路構(gòu)成。2.1.1機(jī)械構(gòu)成機(jī)架呈十字狀，是固定其他部件的平臺(tái)，本項(xiàng)目采用的是尼龍材料的機(jī)架。電機(jī)采用無(wú)刷直流電機(jī)，固定在機(jī)架的四個(gè)端點(diǎn)上，而螺旋槳固定在電機(jī)轉(zhuǎn)子上，迎風(fēng)面垂直向下。螺旋槳按旋轉(zhuǎn)方向分正槳和反槳，從迎風(fēng)面看逆時(shí)針轉(zhuǎn)的為正槳，四個(gè)槳的中心連成的正方形，正槳反槳交錯(cuò)安裝。整體如圖2-1。圖2-1四軸飛行器整機(jī)2.1.2電氣構(gòu)成電氣部分包括：控制電路板、電子調(diào)速器、電池，和一些外接的通訊、傳感器模塊。控制電路板是電氣部分的核心，上面包含MCU、陀螺儀、加速度計(jì)、電子羅盤(pán)、氣壓計(jì)等芯片，負(fù)責(zé)計(jì)算姿態(tài)、處理通信命令和輸出控制信號(hào)到電子調(diào)速器。電子調(diào)速器簡(jiǎn)稱(chēng)電調(diào)，用于控制無(wú)刷直流電機(jī)。電氣連接如圖2-2所示。圖2-2四周飛行器電氣連接圖硬件清單如表2-1表2-1四軸飛行器硬件清單2.2軟件構(gòu)成2.2.1.上位機(jī)上位機(jī)是針對(duì)飛行器的需要，在QtSDK上寫(xiě)的一個(gè)桌面程序，可以通過(guò)串口與飛行器相連，具備傳感器校正、顯示姿態(tài)、測(cè)試電機(jī)、查看電量、設(shè)置參數(shù)等功能。2.2.2.下位機(jī)下位機(jī)為飛行器上MCU里的程序，主要有三個(gè)任務(wù)：計(jì)算姿態(tài)、接受命令和輸出控制。下位機(jī)直接控制電機(jī)功率，飛行器的安全性、穩(wěn)定性、可操縱性都取決于它。下位機(jī)的三個(gè)任務(wù)實(shí)時(shí)性都要求很高，所以計(jì)算姿態(tài)的頻率設(shè)為200Hz，輸出控制的頻率為100Hz，而接收到命令后，立即處理。因?yàn)殡娮诱{(diào)速器接受的信號(hào)為PWM信號(hào)，高電平時(shí)間在1ms~2ms之3.1.坐標(biāo)系統(tǒng)間，所以控制信號(hào)輸出頻率也不能太高。飛行器原理3.1.坐標(biāo)系統(tǒng)飛行器涉及兩個(gè)空間直角坐標(biāo)系統(tǒng)：地理坐標(biāo)系和機(jī)體坐標(biāo)系。地理坐標(biāo)系是固連在地面的坐標(biāo)系，機(jī)體坐標(biāo)系是固連在飛行器上的坐標(biāo)系。四軸飛行器運(yùn)動(dòng)范圍小，可以不考慮地面曲率，且假設(shè)地面為慣性系。地理坐標(biāo)系采用“東北天坐標(biāo)系”，X軸指向東，為方便羅盤(pán)的使用，Y軸指向地磁北，Z軸指向天頂。機(jī)體坐標(biāo)系原點(diǎn)在飛行器中心，xy平面為電機(jī)所在平面，電機(jī)分布在{|x|=|y|,z=0}的直線上，第一象限的電機(jī)帶正槳，z軸指向飛行器上方。如圖3-1所示。圖3-1地理坐標(biāo)系和機(jī)體坐標(biāo)系圖示（坐標(biāo)系方向重合時(shí)）3.2.姿態(tài)的表示和運(yùn)算飛行器的姿態(tài)，是指飛行器的指向，一般用三個(gè)姿態(tài)角表示，包括偏航角(yaw)、俯仰角(pitch)和滾轉(zhuǎn)角(roll)。更深一層，姿態(tài)其實(shí)是一個(gè)旋轉(zhuǎn)變換，表示機(jī)體坐標(biāo)系與地理坐標(biāo)系的旋轉(zhuǎn)關(guān)系，這里定義姿態(tài)為機(jī)體坐標(biāo)系向地理坐標(biāo)系的轉(zhuǎn)換。旋轉(zhuǎn)變換有多種表示方式，包括變換矩陣、姿態(tài)角、轉(zhuǎn)軸轉(zhuǎn)角、四元數(shù)等。在本文中，矩陣用加粗大寫(xiě)字母表示，如EAR，左上標(biāo)和左下標(biāo)表示從機(jī)體坐標(biāo)系（Aircraft）變換到地理坐標(biāo)系（Earth）；四元數(shù)用加粗小寫(xiě)字母表示，如EAq，上下標(biāo)意義與變換矩陣一樣；向量用帶箭頭加粗小寫(xiě)字母表示，如Av，左上標(biāo)A表示向量的值是在機(jī)體坐標(biāo)系的坐標(biāo)值。因?yàn)樽藨B(tài)實(shí)質(zhì)是一個(gè)旋轉(zhuǎn)變換，根據(jù)剛體有限轉(zhuǎn)動(dòng)的歐拉定理，旋轉(zhuǎn)變換是可以串聯(lián)的，所以一個(gè)姿態(tài)可以經(jīng)過(guò)一個(gè)旋轉(zhuǎn)變換，變成另一個(gè)姿態(tài)。類(lèi)比點(diǎn)和向量的概念，姿態(tài)相當(dāng)于點(diǎn)，旋轉(zhuǎn)相當(dāng)于向量，點(diǎn)可以通過(guò)加向量，變成另一個(gè)點(diǎn)。如果用矩陣表示旋轉(zhuǎn)，旋轉(zhuǎn)的串聯(lián)由矩陣乘法來(lái)實(shí)現(xiàn)。如果用四元數(shù)表示旋轉(zhuǎn)，則由四元數(shù)的乘法來(lái)實(shí)現(xiàn)旋轉(zhuǎn)串聯(lián)。用四元數(shù)來(lái)表示旋轉(zhuǎn)，組合旋轉(zhuǎn)時(shí)比用其他方法運(yùn)算量更少，所以無(wú)論在計(jì)算機(jī)圖形學(xué)、飛行器控制等涉及剛體旋轉(zhuǎn)的領(lǐng)域，四元數(shù)都有舉足輕重的地位[9]。飛行器的姿態(tài)計(jì)算是圍繞姿態(tài)四元數(shù)進(jìn)行的，下面簡(jiǎn)要介紹一下四元數(shù)的運(yùn)算。一個(gè)四元數(shù)由4個(gè)實(shí)數(shù)組成。q=[wqxqyqzq]T(3-1)規(guī)范化的四元數(shù)可以表示旋轉(zhuǎn)，見(jiàn)(3-2)式,單位向[xwywzw]T為旋轉(zhuǎn)的轉(zhuǎn)軸。wq=cos(@/2)xq=xw*sin(@/2)yq=yw*sin(@/2)(3-2)zq=zw*sin(@/2)記四元數(shù)乘法的符號(hào)為。四元數(shù)乘法跟矩陣一樣，有結(jié)合律，沒(méi)有交換律。運(yùn)算過(guò)程見(jiàn)(3-3)式。wr=wpwq-xpxq-ypyq-zpzqxr=wpxq+xpwq+ypzq-zpyqyr=wpyq-xpzq+ypzq+zpxq(3-3)zr=wpzq+xpyq-ypxq+zpwq3.3.動(dòng)力學(xué)原理螺旋槳旋轉(zhuǎn)時(shí)，把空氣對(duì)螺旋槳的壓力在軸向和側(cè)向兩個(gè)方向分解，得到兩種力學(xué)效應(yīng)：推力和轉(zhuǎn)矩。當(dāng)四軸飛行器懸停時(shí)，合外力為0，螺旋槳的推力用于抵消重力，轉(zhuǎn)矩則由成對(duì)的正槳反槳抵消。當(dāng)飛行器運(yùn)動(dòng)時(shí)，因?yàn)橥屏χ荒苎剌S向，所以只能通過(guò)傾斜姿態(tài)來(lái)提供水平的動(dòng)力，控制運(yùn)動(dòng)由控制姿態(tài)來(lái)間接實(shí)現(xiàn)。假設(shè)四軸為剛體，根據(jù)質(zhì)點(diǎn)系動(dòng)量矩定理，角速度和角加速度由外力矩決定[10]，通過(guò)控制四個(gè)螺旋槳，可以產(chǎn)生需要的力矩。首先對(duì)螺旋槳編號(hào)：第一象限的為0號(hào)，然后逆時(shí)針依次遞增，如圖(3-1)。同步增加0號(hào)和1號(hào)、減小2號(hào)和3號(hào)槳的功率，可以在不改變推力的情況下，提供x軸的力矩；同步增加1號(hào)和2號(hào)、減小0號(hào)和3號(hào)槳的功率，可以在不改變推力的情況下，提供y軸的力矩；同步增加1號(hào)和3號(hào)、減小0號(hào)和2號(hào)槳的功率，可以在不改變推力的情況下，提供z軸的力矩。以上“增加”和“減小”只是表明變化的方向，可以增加負(fù)數(shù)和減小負(fù)數(shù)，提供的力矩就沿對(duì)應(yīng)軸的負(fù)方向了。把三個(gè)軸的力矩疊加起來(lái)，就得到各螺旋槳功率變化與提供的力矩的對(duì)應(yīng)關(guān)系，可以用一個(gè)矩陣等式表示，見(jiàn)(3-5)式。T是螺旋槳的功率變化量，為4×1矩陣，每行分別對(duì)應(yīng)0到3號(hào)螺旋槳；向量m是力矩，為3×1矩陣。mx、my和mz是各軸的力矩系數(shù)，用于把力矩轉(zhuǎn)換成功率變化量，具體數(shù)值與電機(jī)力矩特性、電機(jī)安裝位置等有關(guān)。姿態(tài)測(cè)量獲取當(dāng)前姿態(tài)是控制飛行器平穩(wěn)飛行的基礎(chǔ)，姿態(tài)的測(cè)量要求低噪聲、高輸出頻率，當(dāng)采用陀螺儀等需要積分的傳感器時(shí)，還需要考慮積分發(fā)散等問(wèn)題。近年來(lái)MEMS傳感器越來(lái)越成熟、應(yīng)用廣泛，成為低成本姿態(tài)測(cè)量的首選器件[11]，因此該項(xiàng)目使用的傳感器全部都是MEMS傳感器，在使用傳感器的值進(jìn)行姿態(tài)計(jì)算之前，有必要校正傳感器。4.1.傳感器校正由于實(shí)驗(yàn)條件限制，傳感器的校正只有兩項(xiàng)，分別對(duì)應(yīng)兩種類(lèi)型的傳感器：陀螺儀——靜止時(shí)0輸出的傳感器、加速度計(jì)與羅盤(pán)——測(cè)量某向量場(chǎng)強(qiáng)度的傳感器。4.1.1.陀螺儀對(duì)于陀螺儀等靜止時(shí)0輸出的傳感器，可以很方便地校正零偏。把傳感器固定好，這時(shí)對(duì)輸出值Xf求平均，得到的A即為零偏，實(shí)際使用時(shí)，把測(cè)得的值減去零偏，得到的值就是校正值。實(shí)際應(yīng)用的公式如(4-2)，A為零偏值，3×1矩陣，單位：LSB；Yi為校正好的值，3×1矩陣，單位：rad/s；Xi為測(cè)量原始值，單位：LSB；gain為轉(zhuǎn)換系數(shù)，單位：(rad/s)/LSB，由傳感器的數(shù)據(jù)手冊(cè)給出。4.1.2.加速度計(jì)和電子羅盤(pán)加速度計(jì)和羅盤(pán)都是測(cè)量所在點(diǎn)的某個(gè)向量場(chǎng)的值的傳感器，靜態(tài)時(shí)加速度計(jì)測(cè)的是等效重力加速度場(chǎng)，電子羅盤(pán)測(cè)的是地磁場(chǎng)。下面僅介紹加速度計(jì)的校正，羅盤(pán)的校正同理。加速度計(jì)測(cè)量的對(duì)象是比力，也就是等效重力加速度和運(yùn)動(dòng)加速度的和，當(dāng)靜止時(shí)，運(yùn)動(dòng)加速度為0，加速度計(jì)的測(cè)量值為等效重力加速度，可以利用這一點(diǎn)校正加速度計(jì)。加速度計(jì)的校正的思路為：對(duì)測(cè)量值平移和縮放，把測(cè)量值擬合到重力加速度。因此校正的任務(wù)為：尋找最佳的平移和縮放參數(shù)，使總體測(cè)量數(shù)據(jù)的更靠近重力加速度。記測(cè)量值為[xmymzm]T，校正后的值為[xcyczc]T，平移參數(shù)為[oxoyoz]T，縮放參數(shù)為[gxgygz]T。xc=(xm+ox)*gxyc=(ym+oy)*gy(4-3)Zc=(zm+oz)*gz4.2.數(shù)據(jù)融合4.2.1.概述有了傳感器的數(shù)據(jù)，就可以用來(lái)計(jì)算姿態(tài)了。計(jì)算姿態(tài)主要用到3個(gè)傳感器：陀螺儀、加速度計(jì)、電子羅盤(pán)。加速度計(jì)測(cè)量對(duì)象為比力，受運(yùn)動(dòng)加速度影響大，特別是受飛行器機(jī)架的振動(dòng)的影響，振動(dòng)振幅高達(dá)2G。電子羅盤(pán)測(cè)磁場(chǎng)強(qiáng)度，受環(huán)境影響也很大，特別是受電子調(diào)速器、電機(jī)等大電流器件影響。而陀螺儀則受外部影響弱，穩(wěn)定性好，但輸出量為角速度，需要積分才能得到姿態(tài)，無(wú)法避免誤差的累積。為了得到穩(wěn)定的、近實(shí)時(shí)的姿態(tài)，對(duì)各傳感器的數(shù)據(jù)取長(zhǎng)補(bǔ)短，需要研究各種數(shù)據(jù)融合方法。4.2.2.姿態(tài)插值法首先介紹自己構(gòu)思的“姿態(tài)插值法”，過(guò)程分成兩部分：一、如果已知初始姿態(tài)，可以利用陀螺積分，不斷推算下一個(gè)姿態(tài)，這部分動(dòng)態(tài)性能好，但誤差會(huì)累積；二、利用加速度計(jì)和羅盤(pán)，可以直接算出一個(gè)姿態(tài)，這個(gè)姿態(tài)的期望是正確的，但正如上文所述，這個(gè)姿態(tài)噪聲大，不穩(wěn)定。為了把兩個(gè)系統(tǒng)的優(yōu)點(diǎn)結(jié)合起來(lái)，對(duì)兩個(gè)系統(tǒng)的結(jié)果進(jìn)行插值，得到的值作為當(dāng)前的姿態(tài)。下面講“姿態(tài)插值法”的計(jì)算過(guò)程。首先是第一個(gè)部分——陀螺積分算姿態(tài)。陀螺的輸出為時(shí)間離散的角速度，要對(duì)時(shí)間積分才得到角度。記陀螺輸出的角速度為向量g=[xgygzg]T，單位為rad/s，假定采樣間隔足夠短，在一個(gè)采樣時(shí)間間隔里，角速度不變，且轉(zhuǎn)角足夠小，忽略三角函數(shù)高階項(xiàng)，各軸間相互的影響忽略不計(jì)，該間隔里的旋轉(zhuǎn)可以用四元數(shù)表示成。接著講第二個(gè)部分。姿態(tài)的定義為兩個(gè)坐標(biāo)系統(tǒng)的旋轉(zhuǎn)變換，因此只要知道兩對(duì)在兩個(gè)坐標(biāo)系對(duì)應(yīng)的向量，就可以把姿態(tài)求出來(lái)。兩對(duì)變量為加速度和磁場(chǎng)強(qiáng)度，測(cè)量出的加速度和磁場(chǎng)強(qiáng)度是在機(jī)體坐標(biāo)系的，而地理坐標(biāo)系的加速度和磁場(chǎng)是常量，存在一個(gè)旋轉(zhuǎn)，可以把機(jī)體坐標(biāo)系的加速度和磁場(chǎng)強(qiáng)度轉(zhuǎn)換到與地理坐標(biāo)系的對(duì)應(yīng)的常量重合，這個(gè)旋轉(zhuǎn)就是所求的姿態(tài)。由于傳感器系統(tǒng)誤差、噪聲等影響，測(cè)量出的加速度和磁場(chǎng)的夾角不是恒定值，不可能精確旋轉(zhuǎn)到與常量一樣，只能求最接近的旋轉(zhuǎn)。最接近的原則可定為：一、旋轉(zhuǎn)后四個(gè)向量共面；二、加速度和磁場(chǎng)成一定角度，旋轉(zhuǎn)后的角平分線與常量的角平分線重合。圖4-1姿態(tài)插值法數(shù)據(jù)流程圖4.2.3.梯度下降法從實(shí)際的使用效果來(lái)說(shuō)，姿態(tài)插值法已經(jīng)滿足要求，但用了三角函數(shù)，即使經(jīng)過(guò)優(yōu)化，運(yùn)算量還是很大，不方便應(yīng)用到低性能MCU或提高運(yùn)算頻率。SebastianO.H.Madgwick提出了更加有效的方法——用梯度下降法計(jì)算姿態(tài)[12]。其思路為：陀螺儀不斷積分姿態(tài)；同時(shí)把加速度和磁場(chǎng)強(qiáng)度的誤差表示成姿態(tài)的函數(shù)，令誤差最小的點(diǎn)為極值點(diǎn)，然后用梯度下降法逼近誤差最小的點(diǎn)，即糾正了陀螺的積分誤差。這個(gè)方法，陀螺計(jì)算的姿態(tài)占主要，加速度計(jì)和羅盤(pán)只是輔助糾正。用陀螺積分姿態(tài)的方法跟“姿態(tài)插值法”的第一部分一樣，關(guān)鍵是如何用加速度和磁場(chǎng)強(qiáng)度糾正誤差。首先要定義誤差，最直觀就是定義為測(cè)量值與常量值之差，這樣加速度和磁場(chǎng)強(qiáng)度就分別有一個(gè)誤差了。因?yàn)橹豢紤]方向，相減前要規(guī)范化。為了減少運(yùn)算量、利用常量值里的0和1分量，相減之前統(tǒng)一到機(jī)體坐標(biāo)系，而不是地理坐標(biāo)系。旋轉(zhuǎn)矩陣的逆等于其轉(zhuǎn)置矩陣。圖4-2梯度下降法數(shù)據(jù)流程圖4.2.4.互補(bǔ)濾波法MarkEuston提出了運(yùn)算量比梯度下降法更小的互補(bǔ)濾波法[13]。相比梯度下降法用加速度和磁場(chǎng)強(qiáng)度計(jì)算姿態(tài)的梯度，互補(bǔ)濾波法是把加速度和磁場(chǎng)強(qiáng)度的誤差構(gòu)造成糾正旋轉(zhuǎn)，疊加到陀螺測(cè)出的角增量上，實(shí)現(xiàn)高效的數(shù)據(jù)融合。統(tǒng)一到同一坐標(biāo)系后，規(guī)范化的加速度和磁場(chǎng)強(qiáng)度的測(cè)量值和常量值可以作一個(gè)叉積，叉積的模為角度誤差的正弦，小角情況下認(rèn)為正比于角度，方向根據(jù)右手法則，可以作為糾正的旋轉(zhuǎn)軸，乘上一個(gè)系數(shù)后，可以與陀螺算出的角增量疊加。圖4-3互補(bǔ)濾波法數(shù)據(jù)流程圖5.姿態(tài)控制姿態(tài)計(jì)算出來(lái)后，就可以輸出控制了。根據(jù)被控姿態(tài)的表示方式，分為歐拉角控制和四元數(shù)控制。控制的思路為：設(shè)定一個(gè)目標(biāo)姿態(tài)，調(diào)整螺旋槳，使測(cè)量出的姿態(tài)變?yōu)槟繕?biāo)姿態(tài)。為了避免復(fù)雜的精確動(dòng)力學(xué)建模，選用PID控制器。5.1歐拉角控制由于歐拉角對(duì)應(yīng)3個(gè)軸的旋轉(zhuǎn)，當(dāng)前姿態(tài)和目標(biāo)姿態(tài)的差值可以作為控制輸入量，角度的誤差直接可以對(duì)應(yīng)力矩的輸出。如果當(dāng)前姿態(tài)和目標(biāo)姿態(tài)相差不大，可以忽略旋轉(zhuǎn)順序的影響。首先把姿態(tài)四元數(shù)轉(zhuǎn)成歐拉角。歐拉角有多種定義方式，這里定義x軸正方向?yàn)楹较颍@z軸轉(zhuǎn)的角度為偏航角(yaw)，繞y軸轉(zhuǎn)的角度為俯仰角(pitch)，繞x軸轉(zhuǎn)的角度為滾轉(zhuǎn)角(roll)，旋轉(zhuǎn)順序?yàn)閆YX。具體轉(zhuǎn)換公式見(jiàn)(5-1)。yaw:ya=arctan(2wqzq+2xqyq,1-2y2q-2z2q)Pitch:pa=arcsin(2wqyq-2zqxq)(5-1)Roll:ra=arctan2(2wqxq+yqzq,1-2x2q-2y2q)5.2.四元數(shù)控制用歐拉角來(lái)控制姿態(tài)，每次控制都要算3次三角函數(shù)，運(yùn)算量很大。為了避免三角函數(shù)，可以直接用姿態(tài)四元數(shù)來(lái)控制。思路跟歐拉角控制一樣，先求姿態(tài)差，再把姿態(tài)差輸入到PID控制器，來(lái)輸出油門(mén)變化量。算法效果6.1加速度計(jì)校正注：數(shù)據(jù)在三個(gè)軸正負(fù)半軸附近采集，各采16組、共96組數(shù)據(jù)。6.2姿態(tài)計(jì)算為了比較幾種姿態(tài)計(jì)算算法的效果，先在下位機(jī)采集數(shù)據(jù)，然后在電腦上離線處理，這樣可以用相同的數(shù)據(jù)進(jìn)行計(jì)算和比較。數(shù)據(jù)分兩組，分別對(duì)應(yīng)靜態(tài)和動(dòng)態(tài)的情況，測(cè)量時(shí)電機(jī)都是開(kāi)的，因此把電機(jī)振動(dòng)也考慮進(jìn)來(lái)了。先比較靜態(tài)的情況。因?yàn)閹追N姿態(tài)融合方法的思路都是：陀螺為主、加速度計(jì)和羅盤(pán)用于糾正陀螺誤差，因此動(dòng)態(tài)性能取決于陀螺，靜態(tài)性能取決于加速度計(jì)和羅盤(pán)，所以靜態(tài)的情況最能反映姿態(tài)融合算法的優(yōu)劣，三種方法算出的滾轉(zhuǎn)角，取了其中連續(xù)的1000個(gè)樣點(diǎn)，即連續(xù)5秒時(shí)間的數(shù)據(jù)。為了公平比較，先把參數(shù)調(diào)整到臨界值，即剛好能糾正陀螺漂移的值。由圖可以看出，姿態(tài)插值法和互補(bǔ)濾波法效果差不多，梯度下降法噪聲振幅比前兩者都大。結(jié)論及存在的問(wèn)題雖然通過(guò)使用上面介紹的各種方法，已經(jīng)成功讓飛行器飛起來(lái)了，但為了把飛行器應(yīng)用到遙感，至少還需要三個(gè)改進(jìn)：一、改進(jìn)控制算法，使機(jī)體更平穩(wěn)；二、增加衛(wèi)星導(dǎo)航定位，實(shí)現(xiàn)預(yù)定航線飛行；三、要用增穩(wěn)云臺(tái)，遙感傳感器要裝在云臺(tái)上。參考文獻(xiàn)：[1]彭軍橋.非共軸式碟形飛行器研究[D].上海大學(xué)2001級(jí)碩士研究生學(xué)位畢業(yè)論文:1-68.[2]吳東國(guó).基于四旋翼飛行器平臺(tái)的低空遙感技術(shù)在公路環(huán)境調(diào)查中的應(yīng)用[J].公路交通技術(shù).2012.第6期:137-138.[3]姬江濤，扈菲菲，賀智濤，杜新武，劉劍君.四旋翼無(wú)人機(jī)在農(nóng)田信息獲取中的應(yīng)用[J].農(nóng)機(jī)化研究.2013.第2期:1-4.[4]趙晨，杜勇.四旋翼無(wú)人機(jī)在輸電線路巡視中的應(yīng)用[J].湖北電力.2012.第36卷第6期:35-36.[5]張鵬，程飛，曹宇強(qiáng)，孫來(lái)，王琪.一種新型四軸搜救飛行器設(shè)計(jì)[J].科技廣場(chǎng).2010年09期:145-147.[6]王帥，周洋.用于危險(xiǎn)區(qū)域物品清理的四旋翼飛行抓捕手[J].兵工自動(dòng)化.2011-03.第30卷第3期:78-80.[7]劉慶元，徐柳華，沈彩蓮，王小平.基于無(wú)人飛行器遙感影像的數(shù)字?jǐn)z影測(cè)量關(guān)鍵技術(shù)研究[J].測(cè)繪科學(xué).2010.第35卷第1期:28-30.[8]岳基隆，張慶杰，朱華勇.微小型四旋翼無(wú)人機(jī)研究進(jìn)展及關(guān)鍵技術(shù)淺析[J].電光與控制.2010-10.第17卷第10期:46-52.[9]PhilipJ.Schneider,DavidH.Eberly.計(jì)算機(jī)圖形學(xué)幾何工具算法詳解[M].第一次印刷.北京.電子工業(yè)出版社.2005:633-641.[10]李俊峰，張雄.理論力學(xué)[M].第二版.北京.清華大學(xué)出版社.2010:213-216.[11]蔣慶仙.關(guān)于MEMS慣性傳感器的發(fā)展及在組合導(dǎo)航中的應(yīng)用前景[J].測(cè)繪通報(bào).2006.09:5-8.[12]SebastianO.H.Madgwick,AndrewJ.L.Harrison,RaviVaidyanathan.EstimationofIMUandMARGorientationusingagradien