多旋翼自主飛行器（電賽四軸設(shè)計(jì)論文）MPU6050陀螺儀+R5F100LEA單片機(jī)

隨著傳感器技術(shù)和控制理論的不斷發(fā)展,尤其是微電子和微機(jī)械技術(shù)的逐步成熟使四軸飛行器的自主飛行控制變得易實(shí)現(xiàn)并成為國(guó)際上的熱點(diǎn)研究對(duì)象。以此為背景，2015年第十二屆全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽本科組C題要求設(shè)計(jì)一個(gè)四旋翼自主飛行器，可以自動(dòng)識(shí)別指示線、降落區(qū)，實(shí)現(xiàn)自主循線飛行、起降吸合貼片等動(dòng)作。本論文針對(duì)該問題，采用新型磷酸鐵鋰電池組為動(dòng)力，MPU6050整合性6軸運(yùn)動(dòng)處理組件作為飛行器姿態(tài)檢測(cè)傳感器，通過卡爾曼、窗口濾波對(duì)MPU6050測(cè)量數(shù)據(jù)進(jìn)行整定，并以R5F100LEA單片機(jī)作為主控芯片，利用其內(nèi)部資源，進(jìn)行運(yùn)算完成四元數(shù)矩陣轉(zhuǎn)換及姿態(tài)解算、融和、矯正，飛行導(dǎo)航用攝像頭作為檢測(cè)模塊，設(shè)計(jì)實(shí)現(xiàn)了四旋翼自主飛行器工作系統(tǒng)。 下面對(duì)各個(gè)關(guān)鍵模塊進(jìn)行說明。

方案一：利用串口讀取模式MPU6050（JY-61），模塊內(nèi)部集成了姿態(tài)解算器，配合動(dòng)態(tài)卡爾曼濾波算法，能夠在動(dòng)態(tài)環(huán)境下準(zhǔn)確輸出模塊的當(dāng)前姿態(tài)，姿態(tài)測(cè)量精度0.01度，穩(wěn)定性極高。采用高精度的陀螺加速度計(jì)MPU6050，通過處理器讀取MPU6050的測(cè)量數(shù)據(jù)然后通過串口輸出，免去了用戶自己去開發(fā)MPU6050復(fù)雜的I2C協(xié)議。模塊內(nèi)部自帶電壓穩(wěn)定電路，可以兼容3.3V/5V的嵌入式系統(tǒng)，連接方便。采用先進(jìn)的數(shù)字濾波技術(shù)，能有效降低測(cè)量噪聲，提高測(cè)量精度。

方案二：利用I2C讀取模式6軸飛控傳感器模塊GY-521（MPC6050），以數(shù)字輸出6軸或9軸的旋轉(zhuǎn)矩陣、四元數(shù)、歐拉角格式的融合演算數(shù)據(jù)。具有131 LSBs/°c 敏感度與全格感測(cè)范圍為±250、±500、±1000與±2000°c 的3軸角速度感測(cè)器(陀螺儀)�？沙淌娇刂�，且程式控制范圍為±2g、±4g、±8g和±16g的3軸加速器。數(shù)字運(yùn)動(dòng)處理（DMP）引擎可減少?gòu)?fù)雜的融合演算數(shù)據(jù)、感測(cè)器同步化、姿勢(shì)感應(yīng)等的負(fù)荷。高達(dá)400kHz快速模式的I2C。

方案三：利用I2C讀取模式10軸飛控傳感器模塊GY-86，模塊I2C總線上掛載MPU6050+HMC5883L+MS5611，可以測(cè)量出三軸加速度、三軸角速度、三軸磁場(chǎng)和氣壓。

綜合比較，方案三包含方案二的所有優(yōu)點(diǎn)，且采樣周期較方案一短，方案三中數(shù)據(jù)采樣更多，在運(yùn)動(dòng)過程中，GY-86更適合于四旋翼飛行器的姿態(tài)控制，故選擇方案三。

方案一：利用鉛蓄電池，電極主要由鉛制成，電解液是硫酸溶液的一種蓄電池。其通過氧化還原反應(yīng)對(duì)用電設(shè)備進(jìn)行供電。

方案二：利用鋰聚合物電池，鋰聚合物電池是采用銼合金做正極，采用高分子導(dǎo)電材料、聚乙炔、聚苯胺或聚對(duì)苯酚等做負(fù)極，有機(jī)溶劑作為電解質(zhì)。該電池具有安全性能好、更輕薄、容量大、內(nèi)阻小、放電性能佳等優(yōu)點(diǎn)。

綜合比較，通過對(duì)飛行器的載重能力進(jìn)行分析，故選擇方案二。

方案一：OV7620

OV7620是一款CMOS攝像頭器件，是一款彩色CMOS型圖像采集集成芯片，提供高性能的單一小體積封裝，該器件分辨率可以達(dá)到640X480，傳輸速率可以達(dá)到30幀。內(nèi)置10位雙通道A/D轉(zhuǎn)換器,輸出8位圖像數(shù)據(jù);具有自動(dòng)增益和自動(dòng)白平衡控制,能進(jìn)行亮度、對(duì)比度、飽和度、γ校正等多種調(diào)節(jié)功能;其視頻時(shí)序產(chǎn)生電路可產(chǎn)生行同步、場(chǎng)同步、混合視頻同步等多種同步信號(hào)和像素時(shí)鐘等多種時(shí)序信號(hào);5V電源供電,工作時(shí)功耗<120mW,待機(jī)時(shí)功耗<10μW。針對(duì)于本次電子設(shè)計(jì)大賽，OV7620能夠采集相應(yīng)場(chǎng)地情況，在姿態(tài)較為平穩(wěn)的條件，實(shí)現(xiàn)比賽場(chǎng)地?cái)?shù)據(jù)讀取，可姿態(tài)飛行控制，實(shí)現(xiàn)巡線起飛降落等多個(gè)功能實(shí)現(xiàn)的關(guān)鍵控制因素。其實(shí)物圖如圖2.1所示。

圖2.1  OV7620攝像頭

方案二：TSL1401CL線性CCD

TSL1401CL 的線性傳感器陣列由一個(gè) 128×1 的光電二極管陣列，相關(guān)的電荷放大器電路，和一個(gè)內(nèi)部的像素?cái)?shù)據(jù)保持功能，它提供同時(shí)集成起始和停止時(shí)間所有像素。該陣列是由 128 個(gè)像素，其中每一個(gè)具有光敏面積 3,524.3 平方微米。 像素之間的間隔是 8 微米。 操作簡(jiǎn)化內(nèi)部控制邏輯，需要只有一個(gè)串行輸入端（SI）的信號(hào)和時(shí)鐘。其具有操作較為簡(jiǎn)單，數(shù)據(jù)量少，處理難度低等優(yōu)點(diǎn)。

綜合比較， OV7620在單片機(jī)資源豐富的情況下，能夠識(shí)別更多場(chǎng)地信息，對(duì)比TSL1401CL線性CCD，其信息更加豐富，并且在飛行器姿態(tài)穩(wěn)定度不足的情況下，其能夠提供更加準(zhǔn)確的車控制信息，為飛行器飛行，功能實(shí)現(xiàn)提供飛行控制數(shù)據(jù)。在此，為實(shí)現(xiàn)比賽功能，選擇OV7620作為場(chǎng)地?cái)?shù)據(jù)采集模塊。

歐拉角Eulerian angles用來確定定點(diǎn)轉(zhuǎn)動(dòng)剛體位置的3個(gè)一組獨(dú)立角參量，由章動(dòng)角 θ、旋進(jìn)角（即進(jìn)動(dòng)角）ψ和自轉(zhuǎn)角j組成。為歐拉首先提出而得名。它們有多種取法，下面是常見的一種。如圖所示，由定點(diǎn)O作出固定坐標(biāo)系 Oxyz和固連于剛體的動(dòng)坐標(biāo)系Ox′y′z′。以軸Oz和Oz′為基本軸，其垂直面Oxy和Ox′y′為基本平面。由軸Oz量到Oz′的角θ稱章動(dòng)角。平面zOz′的垂線ON稱節(jié)線，它又是基本平面Ox′y′和Oxy的交線。在右手坐標(biāo)系中，由 ON 的正端看，角θ應(yīng)按逆時(shí)針方向計(jì)量。由固定軸 Ox 量到節(jié)線ON的角ψ稱旋進(jìn)角；由節(jié)線ON量到動(dòng)軸Ox′的角j稱自轉(zhuǎn)角。由軸 Oz 和Oz′正端看，角ψ和j也都按逆時(shí)針方向計(jì)量。若令 Ox′y′z′的初始位置與 Oxyz 重合，經(jīng)過相繼繞 Oz 、ON 和 Oz′的三次轉(zhuǎn)動(dòng)后，剛體將轉(zhuǎn)到圖示的任意位置。如果剛體繞通過定點(diǎn) O的某一軸線以角速度ω轉(zhuǎn)動(dòng)，而ω在動(dòng)坐標(biāo)系Ox′y′z′上的投影為ωx′、ωy′、ωz′，則它們可用歐拉角及其微商表示如下：ωx′ =sinθsinj+cosj，ωy′= sinθcosj－sinj，ωz′=cosθ+。如果已知 ψ、θ、j和時(shí)間的關(guān)系，則可用上式計(jì)算ω在動(dòng)坐標(biāo)軸上的 3個(gè)分量；反之，如已知任一瞬時(shí)t的ω各個(gè)分量，也可利用上式求出ψ、θ、j和時(shí)間t的關(guān)系，因而也就決定了剛體的運(yùn)動(dòng)。

根據(jù)歐拉定理，剛體繞固定點(diǎn)的位移也可以是繞該點(diǎn)的若干次有限轉(zhuǎn)動(dòng)的合成。在歐拉轉(zhuǎn)動(dòng)中，將參與坐標(biāo)系轉(zhuǎn)動(dòng)三次得到星體坐標(biāo)系。在三次轉(zhuǎn)動(dòng)中每次的旋轉(zhuǎn)軸是被轉(zhuǎn)動(dòng)坐標(biāo)系的某一坐標(biāo)軸，每次的轉(zhuǎn)動(dòng)角即為歐拉角。因此，用歐拉角確定的姿態(tài)矩陣是三次坐標(biāo)轉(zhuǎn)換矩陣的乘積。這些坐標(biāo)轉(zhuǎn)換矩陣都有如下標(biāo)準(zhǔn)形式：

RX(φ)=

Ry(θ)=

Rz(ω)=

最終的姿態(tài)矩陣還與三次轉(zhuǎn)動(dòng)的順序有關(guān)，即按照Z-Y-X軸的順序旋轉(zhuǎn)。于是可得姿態(tài)矩陣

A=RX(φ)Ry(θ)Rz(ω) =

式中，字符"c"，"s"分別為"cos"和"sin"的縮寫形式。

在剛體定點(diǎn)轉(zhuǎn)動(dòng)理論中有一個(gè)著名的歐拉定理：剛體繞固定點(diǎn)的任一位移，可由繞通過此點(diǎn)的某一軸轉(zhuǎn)過一個(gè)角度而得到。在單位時(shí)間間隔Δt內(nèi)假設(shè)剛體角速度為，則該轉(zhuǎn)動(dòng)軸的方向及繞該軸轉(zhuǎn)過的角度分別為：=/，φ=Δt，相應(yīng)四元數(shù)表示式為：

q=（q0，q1，q2，q3）T=   

滿足約束條件        q02+q12+q22+q32=1

以超復(fù)數(shù)形式表示有

q=cos(φ/2)+isin(φ/2)+jsin(φ/2)+ksin(φ/2)

滿足約束條件：      i2 =j2=k2=-1；ij=−ji=k ；jk=−kj=I;ki=-ik=j.

利用三角公式:      cosφ=2cos2(φ/2) −1，

sinφ=2sin(φ/2)cos(φ/2),

可將四元數(shù)轉(zhuǎn)化成姿態(tài)矩陣

ϕ=arctan()      

φ= arctan()

Φ=arctan()

φ=arctan()

在四旋翼自主飛行器設(shè)計(jì)中，主控MCU讀回飛行器在三軸上的加速度，角速度，通過四元數(shù)空間轉(zhuǎn)換得出飛行器當(dāng)前姿態(tài)，以此姿態(tài)數(shù)據(jù)，通過卡爾曼濾波和窗口濾波，得到穩(wěn)定可靠的數(shù)據(jù)，控制四個(gè)電機(jī)的轉(zhuǎn)速以調(diào)整飛行器的飛行姿態(tài)。

將電路從功能上進(jìn)行劃分，可以分為幾個(gè)部分：姿態(tài)檢測(cè)模塊、電機(jī)驅(qū)動(dòng)模塊及電源模塊、各芯片供電電壓產(chǎn)生電路、控制電路以及主控電路，下面對(duì)主要模塊電路進(jìn)行簡(jiǎn)要說明。

在此次電子設(shè)計(jì)競(jìng)賽中，直流電動(dòng)機(jī)是整個(gè)飛行器的動(dòng)力來源，其驅(qū)動(dòng)的穩(wěn)定性，驅(qū)動(dòng)能力，抗擾度能力直接關(guān)系到飛行器的功能實(shí)現(xiàn)，在如圖3.1所示電路中，D1作為直流電機(jī)的續(xù)流回路，在控制信號(hào)失效的情況下，能夠?qū)﹄姍C(jī)進(jìn)行續(xù)流，保護(hù)電路。

              姿態(tài)檢測(cè)模塊由MPU6050完成，其可以由IIC和串口讀取數(shù)據(jù)進(jìn)行運(yùn)算，圖3.2所示為其驅(qū)動(dòng)電路。

MCU采用組委會(huì)下發(fā)的R5F100LEA最小系統(tǒng)板，其電路圖如圖3.3所示:

軟件系統(tǒng)則主要由數(shù)據(jù)采集程序、工作狀態(tài)選擇子程序、尋跡飛行子程序、PID控制子程序、直流電機(jī)調(diào)速子程序等組成，在主函數(shù)中統(tǒng)一調(diào)用。在此不再一一說明，下面對(duì)軟件實(shí)現(xiàn)中的一些關(guān)鍵算法，進(jìn)行說明。

中值濾波對(duì)脈沖噪聲有良好的濾除作用，特別是在濾除噪聲的同時(shí)，能夠保護(hù)信號(hào)的邊緣，使之不被模糊。這些優(yōu)良特性是線性濾波方法所不具有的。此外，中值濾波的算法比較簡(jiǎn)單，也易于用硬件實(shí)現(xiàn)。所以，中值濾波方法一經(jīng)提出后，便在數(shù)字信號(hào)處理領(lǐng)得到重要的應(yīng)用。

中值濾波方法：對(duì)一個(gè)數(shù)字信號(hào)序列xj(-∞<j<∞)進(jìn)行濾波處理時(shí)，首先要定義一個(gè)長(zhǎng)度為奇數(shù)的L長(zhǎng)窗口，L=2N+1，N為正整數(shù)。設(shè)在某一個(gè)時(shí)刻，窗口內(nèi)的信號(hào)樣本為x(i-N)，…，x(i)，…，x(i+N)，其中x(i)為位于窗口中心的信號(hào)樣本值。對(duì)這L個(gè)信號(hào)樣本值按從小到大的順序排列后，其中值，在i處的樣值，便定義為中值濾波的輸出值。中值濾波是在“最小絕對(duì)誤差”準(zhǔn)則下的最優(yōu)濾波。

在實(shí)際應(yīng)用中，隨著所選用窗口長(zhǎng)度的增加，濾波的計(jì)算量將會(huì)迅速增加。因此，尋求中值濾波的快速算法，是中值濾波理論的一個(gè)重要研究?jī)?nèi)容。中值濾波的快速算法，一般采用下述三種方式：①直方圖數(shù)據(jù)修正法；②樣本值二進(jìn)制表示邏輯判斷法；③數(shù)字和模擬的選擇網(wǎng)絡(luò)法。

對(duì)中值濾波的理論研究，還集中于統(tǒng)計(jì)特性分析和根序列的描述方面。當(dāng)一個(gè)信號(hào)序列經(jīng)一特定窗口長(zhǎng)度的中值濾波反復(fù)處理后，它會(huì)收斂于某一個(gè)不再變化的序列，這個(gè)序列稱為中值濾波的根序列。根序列是描述中值濾波特性的一個(gè)重要概念。通過對(duì)根序列結(jié)構(gòu)的研究，可以確定原信號(hào)序列中，哪些成分可以經(jīng)中值濾波后保留下來，哪些成分將被抑制。這對(duì)確定中值濾波器的窗口長(zhǎng)度，提供了重要依據(jù)。用VLSI實(shí)現(xiàn)的中值濾波器芯片，可供實(shí)時(shí)處理中應(yīng)用。

PID控制是通過將被控變量與設(shè)定值進(jìn)行比較，產(chǎn)生一個(gè)與偏差信號(hào)有關(guān)的控制信號(hào)輸入到被控對(duì)象中，屬于一種負(fù)反饋閉環(huán)控制。

PID控制器是根據(jù)給定的r(t)與實(shí)際輸出量y(t)構(gòu)成控制偏差，再對(duì)偏差進(jìn)行比例、積分、微分調(diào)節(jié)的一種方法，其控制規(guī)律為：

            

將上式變?yōu)閭鬟f函數(shù)為：

            

其中，e(t)=r(t)-y(t)為系統(tǒng)誤差，Kp為比例增益，Ti為積分時(shí)間常數(shù)，Td為微分時(shí)間常數(shù)。

PID控制是綜合了比例、積分、微分三種控制方式的優(yōu)點(diǎn)，利用比例控制快速抵消干擾的影響，利用積分控制由于外界干擾或者建立的模型與實(shí)際系統(tǒng)之間不匹配而引起的穩(wěn)態(tài)誤差，同時(shí)微分控制具有某種程度的預(yù)見性，改善系統(tǒng)穩(wěn)定性，提高控制精度。

PID控制是通過姿態(tài)采集模塊發(fā)送回的數(shù)據(jù)與期望姿態(tài)進(jìn)行比較，若存在誤差，則通過比例、積分、微分進(jìn)行調(diào)節(jié)，使調(diào)整后的值被作用于電機(jī)上，使之恢復(fù)正常姿態(tài)。通過PID控制，可以考慮到整個(gè)系統(tǒng)的過去、現(xiàn)在、未來，使之達(dá)到穩(wěn)定。

通過軟件Cubesuite++仿真實(shí)時(shí)監(jiān)控傳感器MPU6050所測(cè)角速度、角度及角加速度來驗(yàn)證代碼的準(zhǔn)確性，下圖為Cubesuite++仿真測(cè)試界面。

圖5.1 仿真測(cè)試界面

圖5.2 作品正面圖

圖5.3 內(nèi)部芯片圖

表1和表2所示分別為飛行器從起飛區(qū)出發(fā)，自主飛行到指定區(qū)域并降落以及飛行器在攜帶鐵片的條件下飛行到指定區(qū)域空投鐵片后返回降落于起飛區(qū)位置和耗時(shí)情況。

由表1可知系統(tǒng)工作在A、B區(qū)直接降落時(shí)完成動(dòng)作平均約耗時(shí)5.2秒，與中心位置偏差平均為5厘米，飛行器系統(tǒng)響應(yīng)速度較快，滿足任務(wù)要求，遠(yuǎn)遠(yuǎn)超過任務(wù)要求的指標(biāo)。

由表2可知系統(tǒng)工作在A區(qū)起飛、B區(qū)降落（以下簡(jiǎn)稱任務(wù)二）時(shí)完成動(dòng)作平均約耗時(shí)21.3秒，與中心位置偏差平均為19.8厘米。由此可見，與飛行器A、B區(qū)直接降落（以下簡(jiǎn)稱任務(wù)一）相比，執(zhí)行一次任務(wù)二所需時(shí)間遠(yuǎn)超過執(zhí)行任務(wù)一兩次的時(shí)間，飛行器降落后與中間位置偏差遠(yuǎn)遠(yuǎn)超過任務(wù)一平均偏差5厘米，為19.8厘米。分析原因可知，任務(wù)一為單向直接降落，而任務(wù)二為往返降落。在任務(wù)二中要求飛行器飛過1米示高線，相比任務(wù)一飛行距離有所增加，在任務(wù)二中投放鐵片的瞬間會(huì)導(dǎo)致飛行器整體重量減輕，此時(shí)需要及時(shí)調(diào)整電機(jī)轉(zhuǎn)速來保證飛行器姿態(tài)的穩(wěn)定并繼續(xù)執(zhí)行飛行動(dòng)作，這需要一定的調(diào)整時(shí)間，所以飛行器執(zhí)行任務(wù)二的總時(shí)間會(huì)相應(yīng)的增長(zhǎng)；任務(wù)二返回起飛區(qū)時(shí)同樣要求飛行器飛過示高線降落，與任務(wù)一相比需要從更高的高度降落，由于降落的調(diào)節(jié)距離較短，從而增加了與中心位置的偏差。即使飛行任務(wù)如此復(fù)雜、困難，本系統(tǒng)仍很好的完成任務(wù)，并超過任務(wù)所要求的指標(biāo)。

圖5.4 實(shí)飛場(chǎng)地圖

本文基于最早MPU6050整合性6軸運(yùn)動(dòng)處理組在件，在R5F100LEA CPU中對(duì)測(cè)量數(shù)據(jù)進(jìn)行整定，并利用其內(nèi)部資源，進(jìn)行運(yùn)算完成四元數(shù)矩陣轉(zhuǎn)換及姿態(tài)解算、融和、矯正，設(shè)計(jì)實(shí)現(xiàn)了四旋翼飛行器自主飛行工作系統(tǒng)。MPU6050傳感器使得飛行器姿態(tài)信號(hào)的獲取十分準(zhǔn)確、便捷，移動(dòng)平均濾波法能有效濾除采樣干擾信號(hào)，有助于實(shí)現(xiàn)系統(tǒng)姿態(tài)的閉環(huán)控制。使用直流空心杯電機(jī)作為飛行器的運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)，可以使飛行器姿態(tài)控制更為精確、PID控制算法的實(shí)現(xiàn)更為容易。采用比能量高的磷酸鐵鋰電池組作為能源系統(tǒng)能提供穩(wěn)定長(zhǎng)效的輸出電流、延長(zhǎng)系統(tǒng)的續(xù)航時(shí)間。在不同的工作環(huán)境中，通過合理調(diào)整PID控制參數(shù)和濾波器參數(shù)仍能完成控制任務(wù)。實(shí)驗(yàn)結(jié)果表明，系統(tǒng)各項(xiàng)性能指標(biāo)都達(dá)到或超過控制任務(wù)要求，飛行器定位精度高、響應(yīng)速度快。

完整的Word格式文檔51黑下載地址：

http://www.torrancerestoration.com/bbs/dpj-115522-1.html