張承岫，李鐵鷹，王耀力

（太原理工大學(xué)信息工程學(xué)院，太原030024）

基于MPU6050和互補(bǔ)濾波的四旋翼飛控系統(tǒng)設(shè)計(jì)*

張承岫，李鐵鷹*，王耀力

（太原理工大學(xué)信息工程學(xué)院，太原030024）

針對(duì)四軸飛行器飛行性能不穩(wěn)定和慣性測(cè)量單元（IMU）易受干擾、存在漂移等問(wèn)題，利用慣性傳感器MPU6050采集實(shí)時(shí)數(shù)據(jù)，以經(jīng)典互補(bǔ)濾波為基礎(chǔ)，提出一種可以自適應(yīng)補(bǔ)償系數(shù)的互補(bǔ)濾波算法，該算法在低通濾波環(huán)節(jié)加入PI控制器，依據(jù)陀螺儀測(cè)得的角速度實(shí)時(shí)調(diào)節(jié)PI控制器補(bǔ)償系數(shù)。飛行器姿態(tài)控制系統(tǒng)采用雙閉環(huán)PID控制方法，姿態(tài)解算的歐拉角作為系統(tǒng)外環(huán)，陀螺儀角速度作為系統(tǒng)內(nèi)環(huán)。最后，搭建以NI myRIO為核心控制器的四軸飛行器，通過(guò)LabVIEW實(shí)現(xiàn)算法和仿真，實(shí)驗(yàn)結(jié)果表明，自適應(yīng)互補(bǔ)濾波算法可以準(zhǔn)確解算姿態(tài)信息，雙閉環(huán)PID控制超調(diào)量小、反應(yīng)靈敏，控制系統(tǒng)基本滿足飛行要求。

四旋翼飛行器；慣性傳感器MPU6050；自適應(yīng)互補(bǔ)濾波；雙閉環(huán)PID；LabVIEW語(yǔ)言

EEACC:2575；1270F；7320Edoi:10.3969/j.issn.1004-1699.2016.07.011

四旋翼飛行器因機(jī)動(dòng)性能強(qiáng)、體積小、可垂直升降、隱蔽性好等優(yōu)點(diǎn)，廣泛應(yīng)用于國(guó)內(nèi)外軍事和工業(yè)領(lǐng)域，國(guó)內(nèi)尤以大疆無(wú)人飛行器控制系統(tǒng)領(lǐng)先。近年來(lái)，隨著飛行器功能的不斷革新，用戶體驗(yàn)性逐步增強(qiáng)，操作變得更加簡(jiǎn)單，已受到廣大航模航拍愛(ài)好者和科研領(lǐng)域研究者的青睞，并被使用在影視廣告中，展現(xiàn)出很大的商業(yè)價(jià)值。

四軸飛行器的設(shè)計(jì)主要由姿態(tài)解算和飛行控制兩部分組成。姿態(tài)解算中的姿態(tài)信息主要通過(guò)慣性測(cè)量單元IMU（Inertial Measurement Unit）測(cè)量獲得，包括三軸加速度計(jì)和三軸陀螺儀。由于微機(jī)電系統(tǒng)MEMS（Micro-Electro-Mechanical System）具有低功耗、易于集成等優(yōu)點(diǎn)，已成為IMU的首選，然而慣性傳感器易受噪聲干擾且存在漂移誤差，姿態(tài)解算精度降低，影響飛控效果，因此需要通過(guò)數(shù)據(jù)融合提高解算的準(zhǔn)確性。針對(duì)飛行器的姿態(tài)估計(jì)問(wèn)題，研究者們提出了一系列的算法，文獻(xiàn)［1-2］采用卡爾曼濾波［1-2］進(jìn)行數(shù)據(jù)融合，針對(duì)非線性系統(tǒng)，Bucy，Sunahara等人又提出了擴(kuò)展卡爾曼濾波［3-4］，然而卡爾曼濾波需要對(duì)噪聲建模，建模的不精確會(huì)直接影響姿態(tài)估計(jì)的準(zhǔn)確性；文獻(xiàn)［5-6］采用粒子濾波，該方法可以解決非線性的問(wèn)題，但是由于計(jì)算量大，在低成本的飛控系統(tǒng)中應(yīng)用較少［5-6］；互補(bǔ)濾波則是從頻域的角度，對(duì)數(shù)據(jù)進(jìn)行融合，消除信號(hào)干擾，從根本上避免了噪聲建模誤差對(duì)姿態(tài)估計(jì)的影響。飛行控制方法主要包括反步法［7-8］、滑?？刂疲?-10］、組合法和PID控制［11］等。

本文在經(jīng)典互補(bǔ)濾波的基礎(chǔ)上，在加速度計(jì)的低通濾波環(huán)節(jié)加入PI控制器，根據(jù)陀螺儀角速度實(shí)時(shí)調(diào)節(jié)PI控制器補(bǔ)償系數(shù)，提出一種可以自適應(yīng)補(bǔ)償系數(shù)的互補(bǔ)濾波算法，對(duì)姿態(tài)數(shù)據(jù)進(jìn)行融合，并通過(guò)雙閉環(huán)PID控制對(duì)四旋翼飛行器實(shí)現(xiàn)飛行控制，搭建飛行器硬件和控制系統(tǒng)仿真平臺(tái)，并將采集的數(shù)據(jù)通過(guò)Wifi發(fā)送到上位機(jī)上，通過(guò)LabVIEW［12］界面實(shí)時(shí)顯示波形曲線，對(duì)四旋翼飛行器控制系統(tǒng)穩(wěn)定性和動(dòng)態(tài)性能進(jìn)行測(cè)試。

1　系統(tǒng)建模與姿態(tài)估計(jì)

定義導(dǎo)航坐標(biāo)系（N系）和四旋翼載體坐標(biāo)系（B系），XYZ導(dǎo)航坐標(biāo)系為“東-北-天”坐標(biāo)，以X軸正方向?yàn)轭^，X'Y'Z'為載體坐標(biāo)系，N系和B系角度轉(zhuǎn)換關(guān)系如圖1所示，飛行器的姿態(tài)分別用橫滾角?，俯仰角θ，偏航角ψ描述［13］。

圖1　導(dǎo)航坐標(biāo)系與載體坐標(biāo)系的角度轉(zhuǎn)換關(guān)系

IMU固定在飛行器上，慣性傳感器測(cè)得的數(shù)據(jù)基于載體坐標(biāo)系，飛行器在飛行過(guò)程中，B系相對(duì)于N系會(huì)形成相應(yīng)的角度，在姿態(tài)解算時(shí)，需將B系的數(shù)據(jù)轉(zhuǎn)換到N系下，對(duì)應(yīng)的歐拉角形式下的方向余弦矩陣可用式（1）表示。

慣性測(cè)量單元由三軸陀螺儀和三軸加速度計(jì)組成，陀螺儀測(cè)量系統(tǒng)旋轉(zhuǎn)角速度，用ω表示，加速度計(jì)測(cè)量三軸加速度，用a表示。飛行器在靜止或勻速飛行狀態(tài)下的姿態(tài)角估計(jì)值可表示如式（2）所示，其中分別為在一段時(shí)間內(nèi)沿三軸的加速度平均值［14］。

不考慮陀螺儀的漂移，并假設(shè)積分周期足夠小的情況下，陀螺儀的角速度可以通過(guò)積分運(yùn)算直接得到角度，如式（3），其中ωm為陀螺儀測(cè)得的旋轉(zhuǎn)角速度。

由式（2）和式（3）得到的分別是理想情況下的角度估計(jì)值，在實(shí)際過(guò)程中，由于測(cè)量噪聲、陀螺儀漂移的存在，通過(guò)以上方法得到的姿態(tài)解算值會(huì)存在很大誤差，需要將兩傳感器的姿態(tài)數(shù)據(jù)加以融合，獲得準(zhǔn)確的姿態(tài)角。

2　自適應(yīng)互補(bǔ)濾波原理及算法

慣性測(cè)量單元中的加速度計(jì)主要測(cè)量機(jī)體的線加速度，在靜止?fàn)顟B(tài)下，可以準(zhǔn)確測(cè)得加速度值，具有良好的靜態(tài)特性，但在飛行過(guò)程中，易受噪聲和震動(dòng)的影響，導(dǎo)致測(cè)量不準(zhǔn)確，動(dòng)態(tài)特性較差；陀螺儀主要測(cè)量旋轉(zhuǎn)角速度，跟蹤性能好，但是由于角度是通過(guò)陀螺儀角速度經(jīng)積分得到，而陀螺儀本身易受溫度的影響存在零點(diǎn)漂移，隨時(shí)間的積累，角度值存在很大誤差。如果以此角度值作為解算的姿態(tài)角，對(duì)飛行器進(jìn)行PID控制，將無(wú)法恢復(fù)飛行器的水平姿態(tài)，造成飛行失控。

為消除慣性傳感器的漂移和噪聲干擾，獲得準(zhǔn)確的姿態(tài)信息，綜合加速度計(jì)和陀螺儀各自的優(yōu)缺點(diǎn)可以發(fā)現(xiàn)，應(yīng)用互補(bǔ)濾波算法，從頻域的角度分別加入低通和高通濾波器，將兩傳感器的姿態(tài)信息加以融合，可去除干擾，消除零位誤差，提高解算精度［15］。

其中θ為根據(jù)式（2）計(jì)算得到的姿態(tài)角預(yù)估值。通過(guò)式（4）可以消除高頻、低頻干擾，該濾波能否達(dá)到預(yù)期效果的關(guān)鍵在于參數(shù)K的選取，但是在噪聲較大時(shí)，由于低通的阻帶衰減較慢，濾波效果并不好。本文在上述互補(bǔ)濾波原理的基礎(chǔ)上，在低通環(huán)節(jié)增加PI控制器，在角速度的不同階段，動(dòng)態(tài)調(diào)整補(bǔ)償系數(shù)KP、Ki構(gòu)成自適應(yīng)互補(bǔ)濾波器，提高姿態(tài)數(shù)據(jù)融合精度，解算出準(zhǔn)確的姿態(tài)角，自適應(yīng)互補(bǔ)濾波原理見(jiàn)下圖（2）所示。

圖2　自適應(yīng)互補(bǔ)濾波算法原理框圖

根據(jù)原理框圖可知，加速度計(jì)測(cè)量的加速度，經(jīng)式（2）得到姿態(tài)估計(jì)值，再經(jīng)PI控制與陀螺儀的角速度經(jīng)積分得到的角度融合，解算出飛行器當(dāng)前姿態(tài)角，融合的角度同時(shí)作為負(fù)反饋，實(shí)時(shí)更新飛行器歐拉角，姿態(tài)更新算法流程見(jiàn)圖3所示。

圖3　姿態(tài)更新算法流程圖

自適應(yīng)補(bǔ)償系數(shù)可以通過(guò)式（5）得到，其中ωmax為陀螺儀的最大量程，ωc為陀螺儀的截止角速度（ωc＜ωmax）。

3　雙閉環(huán)PID控制

飛行控制算法是決定飛行器性能的關(guān)鍵因素［16］，飛行器在飛行時(shí)，如果突遇外力或電磁干擾，會(huì)產(chǎn)生激烈震蕩，此時(shí)單環(huán)PID無(wú)法快速恢復(fù)系統(tǒng)穩(wěn)定，本文在傳統(tǒng)PID控制的基礎(chǔ)上，引入角速度反饋，搭建雙閉環(huán)PID控制系統(tǒng)，提高系統(tǒng)抗干擾能力，加強(qiáng)系統(tǒng)的魯棒性。

雙閉環(huán)PID控制原理框圖見(jiàn)下圖4，經(jīng)互補(bǔ)濾波算法解算得到的姿態(tài)角與期望姿態(tài)角的差值記為姿態(tài)誤差經(jīng)角度PID控制得到角度控制量，將更新的歐拉角作為系統(tǒng)外環(huán)反饋，而陀螺儀測(cè)量的角速度變化則作為內(nèi)環(huán)反饋，進(jìn)行角速度PID控制［17］。

圖4　雙閉環(huán)PID控制原理框圖

雙閉環(huán)PID控制算法采用位置式PID：

為防止油門突然增大或減小，引起飛行器震蕩，導(dǎo)致系統(tǒng)不穩(wěn)定，需要對(duì)PID控制量先進(jìn)行限幅處理，并根據(jù)十字形飛行器模式將角度控制量轉(zhuǎn)換為油門輸出值。

4　實(shí)驗(yàn)測(cè)試

本文搭建了以NI（National Instruments）公司生產(chǎn)的NI myRIO-1900為核心控制器，InvenSense公司生產(chǎn)的MPU6050芯片作為慣性測(cè)量單元的硬件平臺(tái)，并配以LM3510SM（700 kV）電機(jī)驅(qū)動(dòng)、TATTU聚合物鋰電源和機(jī)械組成模塊；采用NI公司開發(fā)的LabVIEW軟件編寫算法和仿真程序，用于四旋翼飛行器軟硬件系統(tǒng)測(cè)試。

四旋翼起飛前的姿態(tài)是決定飛行器能否平穩(wěn)起飛的關(guān)鍵，因此測(cè)量到準(zhǔn)確的姿態(tài)信息至關(guān)重要，圖5為飛行器在靜止時(shí)刻經(jīng)自適應(yīng)互補(bǔ)濾波后的歐拉角曲線。

由圖5可知，由于試驗(yàn)臺(tái)并非絕對(duì)水平，初始時(shí)刻的歐拉角不處于0°，經(jīng)過(guò)互補(bǔ)濾波后，飛行器在靜止時(shí)刻得歐拉角分別為：橫滾角為-0.2°，俯仰角為-3.6°，偏航角為0.2°。

圖5　自適應(yīng)互補(bǔ)濾波后歐拉角曲線

四旋翼飛行器自適應(yīng)互補(bǔ)濾波前后的動(dòng)態(tài)測(cè)試對(duì)比，如圖6所示。四旋翼在飛行過(guò)程中，載體坐標(biāo)系繞導(dǎo)航坐標(biāo)系旋轉(zhuǎn)會(huì)產(chǎn)生相應(yīng)的角度，MPU6050實(shí)時(shí)采集加速度和角速度。由圖可見(jiàn)，互補(bǔ)濾波前，初始時(shí)刻三軸歐拉角均為10°，經(jīng)過(guò)一段時(shí)間的轉(zhuǎn)動(dòng)后，橫滾角、俯仰角、偏航角均產(chǎn)生偏移誤差，無(wú)法滿足姿態(tài)解算的可靠性要求。而經(jīng)過(guò)自適應(yīng)互補(bǔ)濾波后的曲線則不會(huì)發(fā)生偏移，仍保持在10°。

圖6　自適應(yīng)互補(bǔ)濾波前后的動(dòng)態(tài)測(cè)試對(duì)比

依據(jù)自適應(yīng)互補(bǔ)濾波法進(jìn)行測(cè)試，陀螺儀的最大量程ωmax=2000°/s，截止角速度ωc=2πf，其中 f為陀螺儀截止頻率，約為100 Hz，KP0，KP1經(jīng)PID參數(shù)工程整定獲得，分別為5和20。由于系統(tǒng)未加磁力計(jì)，四旋翼在飛行過(guò)程中無(wú)法修正Z軸的姿態(tài)信息，偏航角存在偏差，在不考慮飛行器偏航的情況下，可以忽略。

合理選擇比例、積分、微分系數(shù)是決定PID控制器能否快速跟蹤四旋翼姿態(tài)變化，及時(shí)調(diào)整電機(jī)輸出，恢復(fù)飛行器平衡的關(guān)鍵。在對(duì)搭建的飛行器進(jìn)行測(cè)試之前，先在LabVIEW上設(shè)計(jì)飛行器模型PID控制器仿真平臺(tái)，模擬四旋翼在飛行過(guò)程中的慣性效應(yīng)、重力和隨機(jī)干擾的影響和電機(jī)的推力。程序運(yùn)行后，調(diào)節(jié)setpoint給定四旋翼單軸姿態(tài)角度，調(diào)整KP，Ki，Kd參數(shù)，觀察階躍輸入的響應(yīng)曲線、響應(yīng)時(shí)間和超調(diào)量的變化，慢慢調(diào)整PID增益，直到得到合適的響應(yīng)曲線。經(jīng)過(guò)測(cè)試，當(dāng)setpoint給定20，即橫滾角為20度時(shí)，最終選取四旋翼飛行器的PID增益KP為0.85，Ki為26，Kd為0.003，此時(shí)PID輸出為0.19，響應(yīng)時(shí)間為0.25 s，超調(diào)量為0.0333。飛行器模型PID控制器仿真界面如圖7所示。

圖7　前面板顯示四旋翼飛行器模型單軸仿真界面

根據(jù)仿真確定的PID增益和自適應(yīng)互補(bǔ)濾波算法測(cè)試的結(jié)果，對(duì)搭建的四旋翼飛行器進(jìn)行測(cè)試。如圖8所示，將飛行器裝有1號(hào)、3號(hào)電機(jī)的X軸固定，即可以改變橫滾角，給定橫滾角為20°，期望角為0°，逐漸增加油門，觀察傳輸?shù)缴衔粰C(jī)顯示的波形曲線以及四旋翼飛行器恢復(fù)平衡的情況。

圖8　姿態(tài)控制調(diào)試

經(jīng)過(guò)多次軟硬件測(cè)試，飛行器與上位機(jī)通訊良好，無(wú)信號(hào)中斷或失控現(xiàn)象，飛行器能夠快速恢復(fù)平衡并穩(wěn)定下來(lái)，結(jié)果表明系統(tǒng)的設(shè)計(jì)合理，基本滿足飛行需求。

5　結(jié)束語(yǔ)

本文針對(duì)姿態(tài)精確解算和飛行控制不穩(wěn)定問(wèn)題，介紹了四旋翼飛行器飛控系統(tǒng)的設(shè)計(jì)方法，采用自適應(yīng)互補(bǔ)濾波算法和雙閉環(huán)PID控制實(shí)現(xiàn)姿態(tài)信息的實(shí)時(shí)解算和飛行控制，搭建以NI myRIO為核心控制器的四旋翼飛行器硬件平臺(tái)，并在Lab-VIEW軟件上實(shí)現(xiàn)單軸飛行器模型仿真、油門控制以及曲線的實(shí)時(shí)顯示。本設(shè)計(jì)能夠準(zhǔn)確解算姿態(tài)信息，控制系統(tǒng)快速進(jìn)入穩(wěn)定狀態(tài)，基本滿足飛行要求。下一步將在系統(tǒng)中加入磁力儀和高度傳感器，實(shí)現(xiàn)對(duì)偏航角的修正和定點(diǎn)懸停。

［1］ 王帥，魏國(guó).卡爾曼濾波在四旋翼飛行器姿態(tài)測(cè)量中的應(yīng)用［J］.兵工自動(dòng)化，2011，30（1）：73-75.

［2］ 吳友強(qiáng)，紀(jì)浩，鄒力涵.基于卡爾曼濾波的四旋翼飛行器懸?？刂蒲芯浚跩］.自動(dòng)化與儀表，2013，28（11）：1-5.

［3］ 郭曉鴻，楊忠，陳喆，等.EKF和互補(bǔ)濾波器在飛行姿態(tài)確定中的應(yīng)用［J］.傳感器與微系統(tǒng)，2011，30（11）：149-152.

［4］ 賈瑞才.基于四元數(shù)EKF的低成本MEMS姿態(tài)估計(jì)算法［J］.傳感技術(shù)學(xué)報(bào)，2014，27（1）：90-95.

［5］ 夏楠，邱天爽，李景春，等.一種卡爾曼濾波與粒子濾波相結(jié)合的非線性濾波算法［J］.電子學(xué)報(bào)，2013，41（1）：148-152.

［6］ 曲仕茹，馬志強(qiáng).改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用［J］.飛行力學(xué)，2013，31（5）：458-461，466.

［7］ 趙元偉，盧京朝.四旋翼飛行器的建模及基于反步法的控制［J］.科學(xué)技術(shù)與工程，2013，13（34）：10425-10430.

［8］ 陳奕梅，陳增輝，修春波.四旋翼無(wú)人飛行器反步法的控制［J］.天津工業(yè)大學(xué)學(xué)報(bào)，2015，34（5）：32-36.

［9］ 李波波，賈秋玲.基于滑模控制的四旋翼飛行器控制器設(shè)計(jì)［J］.電子設(shè)計(jì)工程，2013，21（16）：76-78.

［10］薛鵬，任鵬飛，曹學(xué)儒.四旋翼飛行器滑?？刂频姆€(wěn)定性分析［J］.河南工程學(xué)院學(xué)報(bào)（自然科學(xué)版），2015，27（1）：33-35.

［11］Yibo L，Qiling X，Jianda H.Modeling and PID Control of Tethered Unmanned Quadrotor Helicopter［C］//Mechatronic Sciences，Electric Engineering and Computer（MEC），Proceedings 2013 International Conference on.IEEE，2013：58-262.

［12］王玉剛，王恒新，陳曄，等.基于LabVIEW的某型航箭發(fā)射檢測(cè)儀設(shè)計(jì)［J］.現(xiàn)代電子技術(shù)，2015，38（20）：81-83.

［13］萬(wàn)曉鳳，康利平，余運(yùn)俊，等.互補(bǔ)濾波算法在四旋翼飛行器姿態(tài)解算中的應(yīng)用［J］.測(cè)控技術(shù)，2015，34（2）：8-11.

［14］史智寧，韓波，徐玉，等.基于重力場(chǎng)自適應(yīng)互補(bǔ)濾波的無(wú)人直升機(jī)水平姿態(tài)估計(jì)方法［J］.傳感技術(shù)學(xué)報(bào)，2009，22（7）：993-996.

［15］傅忠云，劉文波，孫金秋，等.自適應(yīng)混合濾波算法在微型飛行器姿態(tài)估計(jì)中的應(yīng)用［J］.傳感技術(shù)學(xué)報(bào)，2014，27（5）：698-703.

［16］葉樹球，詹林.基于PID的四旋翼飛行器姿態(tài)控制系統(tǒng)［J］.計(jì)算機(jī)與現(xiàn)代化，2015，20（5）：117-120.

［17］Cavalcante Sa R，De Araujo A L C，Varela A T，et al.Construction and PID Control for Stability of an Unmanned Aerial Vehicle of the Type Quadrotor［C］//Robotics Symposium and Competition（LARS/LARC），2013 Latin American.IEEE，2013：95-99.

張承岫（1989-），女，黑龍江齊齊哈爾人，碩士研究生，主要研究方向?yàn)樗男盹w行器控制算法優(yōu)化及智能控制，316567231@qq.com；

李鐵鷹（1956-），男，山西太原人，教授，博士，碩士生導(dǎo)師，院黨委書記，長(zhǎng)期從事智能控制與自動(dòng)化研究，Lty@tyut. edu.cn；

王耀力（1965-），男，山西太原人，副教授，博士，碩士生導(dǎo)師，長(zhǎng)期從事信息系統(tǒng)設(shè)計(jì)、人機(jī)視覺(jué)分析與處理以及嵌入式系統(tǒng)電路設(shè)計(jì)理論的研究，wangyaoli@tyut. edu.cn。

Design of Quad-Rotor Aircraft Flight Control System Based on MPU6050 and Adaptive Complementary Filter*

ZHANG Chengxiu，LI Tieying*，WANG Yaoli
（College of Information Engineering，Taiyuan University of Technology，Taiyuan 030024，China）

Against the flight performance instability of quad-rotor aircraft and Inertial Measurement Unit（IMU）'s defects，such as lack of anti-interference and drifting etc，this paper presents a complementary filtering algorithm that adaptive to compensation coefficient，based on classic complementary filter.Collecting real-time data via inertial sensor MPU6050，this algorithm adds PI controller into the low-pass filter link，and adjusts the compensation coefficient of PI controller in real-time mode，based on the angular velocity of the gyroscope.Aircraft attitude control system adopts double closed loop PID control method，which takes the Euler angles of the attitude algorithm as its outer loop and the angular velocity of the gyroscope as its inner loop.In the final test，a quad-rotor aircraft is built with NI myRIO as a core controller，and implement the algorithm and simulation through the LabVIEW.Experimental result demonstrates that adaptive complementary filter algorithm can calculate attitude information accurately，with the advantages of little control overshoot and the sensitive response of the double closed loop PID control，the control system basically meets the requirements of flight.

quad-rotor aircraft；inertial sensor MPU6050；adaptive complementary filter；double closed loop PID；LabVIEW language

TP273

A

1004-1699（2016）07-1011-05

項(xiàng)目來(lái)源：山西省自然科學(xué)基金項(xiàng)目（2013011015-1）

2016-01-26修改日期：2016-03-07