多旋翼自主飛行器（電賽四軸設計論文）MPU6050陀螺儀+R5F100LEA單片機

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

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

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

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

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

方案一：利用鉛蓄電池，電極主要由鉛制成，電解液是硫酸溶液的一種蓄電池。其通過氧化還原反應對用電設備進行供電。

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

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

方案一：OV7620

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

圖2.1  OV7620攝像頭

方案二：TSL1401CL線性CCD

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

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

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

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

RX(φ)=

Ry(θ)=

Rz(ω)=

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

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

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

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

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

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

以超復數(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ù)轉化成姿態(tài)矩陣

ϕ=arctan()      

φ= arctan()

Φ=arctan()

φ=arctan()

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

將電路從功能上進行劃分，可以分為幾個部分：姿態(tài)檢測模塊、電機驅動模塊及電源模塊、各芯片供電電壓產生電路、控制電路以及主控電路，下面對主要模塊電路進行簡要說明。

在此次電子設計競賽中，直流電動機是整個飛行器的動力來源，其驅動的穩(wěn)定性，驅動能力，抗擾度能力直接關系到飛行器的功能實現(xiàn)，在如圖3.1所示電路中，D1作為直流電機的續(xù)流回路，在控制信號失效的情況下，能夠對電機進行續(xù)流，保護電路。

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

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

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

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

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

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

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

PID控制是通過將被控變量與設定值進行比較，產生一個與偏差信號有關的控制信號輸入到被控對象中，屬于一種負反饋閉環(huán)控制。

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

            

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

            

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

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

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

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

圖5.1 仿真測試界面

圖5.2 作品正面圖

圖5.3 內部芯片圖

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

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

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

圖5.4 實飛場地圖

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

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

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