基于MPU6050和互補濾波的四旋翼飛控系統設計*

張承岫，李鐵鷹，王耀力

（太原理工大學信息工程學院，太原030024）

基于MPU6050和互補濾波的四旋翼飛控系統設計*

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

（太原理工大學信息工程學院，太原030024）

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

四旋翼飛行器；慣性傳感器MPU6050；自適應互補濾波；雙閉環PID；LabVIEW語言

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

四旋翼飛行器因機動性能強、體積小、可垂直升降、隱蔽性好等優點，廣泛應用于國內外軍事和工業領域，國內尤以大疆無人飛行器控制系統領先。近年來，隨著飛行器功能的不斷革新，用戶體驗性逐步增強，操作變得更加簡單，已受到廣大航模航拍愛好者和科研領域研究者的青睞，并被使用在影視廣告中，展現出很大的商業價值。

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

本文在經典互補濾波的基礎上，在加速度計的低通濾波環節加入PI控制器，根據陀螺儀角速度實時調節PI控制器補償系數，提出一種可以自適應補償系數的互補濾波算法，對姿態數據進行融合，并通過雙閉環PID控制對四旋翼飛行器實現飛行控制，搭建飛行器硬件和控制系統仿真平臺，并將采集的數據通過Wifi發送到上位機上，通過LabVIEW［12］界面實時顯示波形曲線，對四旋翼飛行器控制系統穩定性和動態性能進行測試。

1　系統建模與姿態估計

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

圖1　導航坐標系與載體坐標系的角度轉換關系

IMU固定在飛行器上，慣性傳感器測得的數據基于載體坐標系，飛行器在飛行過程中，B系相對于N系會形成相應的角度，在姿態解算時，需將B系的數據轉換到N系下，對應的歐拉角形式下的方向余弦矩陣可用式（1）表示。

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

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

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

2　自適應互補濾波原理及算法

慣性測量單元中的加速度計主要測量機體的線加速度，在靜止狀態下，可以準確測得加速度值，具有良好的靜態特性，但在飛行過程中，易受噪聲和震動的影響，導致測量不準確，動態特性較差；陀螺儀主要測量旋轉角速度，跟蹤性能好，但是由于角度是通過陀螺儀角速度經積分得到，而陀螺儀本身易受溫度的影響存在零點漂移，隨時間的積累，角度值存在很大誤差。如果以此角度值作為解算的姿態角，對飛行器進行PID控制，將無法恢復飛行器的水平姿態，造成飛行失控。

為消除慣性傳感器的漂移和噪聲干擾，獲得準確的姿態信息，綜合加速度計和陀螺儀各自的優缺點可以發現，應用互補濾波算法，從頻域的角度分別加入低通和高通濾波器，將兩傳感器的姿態信息加以融合，可去除干擾，消除零位誤差，提高解算精度［15］。

其中θ為根據式（2）計算得到的姿態角預估值。通過式（4）可以消除高頻、低頻干擾，該濾波能否達到預期效果的關鍵在于參數K的選取，但是在噪聲較大時，由于低通的阻帶衰減較慢，濾波效果并不好。本文在上述互補濾波原理的基礎上，在低通環節增加PI控制器，在角速度的不同階段，動態調整補償系數KP、Ki構成自適應互補濾波器，提高姿態數據融合精度，解算出準確的姿態角，自適應互補濾波原理見下圖（2）所示。

圖2　自適應互補濾波算法原理框圖

根據原理框圖可知，加速度計測量的加速度，經式（2）得到姿態估計值，再經PI控制與陀螺儀的角速度經積分得到的角度融合，解算出飛行器當前姿態角，融合的角度同時作為負反饋，實時更新飛行器歐拉角，姿態更新算法流程見圖3所示。

圖3　姿態更新算法流程圖

自適應補償系數可以通過式（5）得到，其中ωmax為陀螺儀的最大量程，ωc為陀螺儀的截止角速度（ωc＜ωmax）。

3　雙閉環PID控制

飛行控制算法是決定飛行器性能的關鍵因素［16］，飛行器在飛行時，如果突遇外力或電磁干擾，會產生激烈震蕩，此時單環PID無法快速恢復系統穩定，本文在傳統PID控制的基礎上，引入角速度反饋，搭建雙閉環PID控制系統，提高系統抗干擾能力，加強系統的魯棒性。

雙閉環PID控制原理框圖見下圖4，經互補濾波算法解算得到的姿態角與期望姿態角的差值記為姿態誤差經角度PID控制得到角度控制量，將更新的歐拉角作為系統外環反饋，而陀螺儀測量的角速度變化則作為內環反饋，進行角速度PID控制［17］。

圖4　雙閉環PID控制原理框圖

雙閉環PID控制算法采用位置式PID：

為防止油門突然增大或減小，引起飛行器震蕩，導致系統不穩定，需要對PID控制量先進行限幅處理，并根據十字形飛行器模式將角度控制量轉換為油門輸出值。

4　實驗測試

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

四旋翼起飛前的姿態是決定飛行器能否平穩起飛的關鍵，因此測量到準確的姿態信息至關重要，圖5為飛行器在靜止時刻經自適應互補濾波后的歐拉角曲線。

由圖5可知，由于試驗臺并非絕對水平，初始時刻的歐拉角不處于0°，經過互補濾波后，飛行器在靜止時刻得歐拉角分別為：橫滾角為-0.2°，俯仰角為-3.6°，偏航角為0.2°。

圖5　自適應互補濾波后歐拉角曲線

四旋翼飛行器自適應互補濾波前后的動態測試對比，如圖6所示。四旋翼在飛行過程中，載體坐標系繞導航坐標系旋轉會產生相應的角度，MPU6050實時采集加速度和角速度。由圖可見，互補濾波前，初始時刻三軸歐拉角均為10°，經過一段時間的轉動后，橫滾角、俯仰角、偏航角均產生偏移誤差，無法滿足姿態解算的可靠性要求。而經過自適應互補濾波后的曲線則不會發生偏移，仍保持在10°。

圖6　自適應互補濾波前后的動態測試對比

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

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

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

根據仿真確定的PID增益和自適應互補濾波算法測試的結果，對搭建的四旋翼飛行器進行測試。如圖8所示，將飛行器裝有1號、3號電機的X軸固定，即可以改變橫滾角，給定橫滾角為20°，期望角為0°，逐漸增加油門，觀察傳輸到上位機顯示的波形曲線以及四旋翼飛行器恢復平衡的情況。

圖8　姿態控制調試

經過多次軟硬件測試，飛行器與上位機通訊良好，無信號中斷或失控現象，飛行器能夠快速恢復平衡并穩定下來，結果表明系統的設計合理，基本滿足飛行需求。

5　結束語

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

［1］ 王帥，魏國.卡爾曼濾波在四旋翼飛行器姿態測量中的應用［J］.兵工自動化，2011，30（1）：73-75.

［2］ 吳友強，紀浩，鄒力涵.基于卡爾曼濾波的四旋翼飛行器懸停控制研究［J］.自動化與儀表，2013，28（11）：1-5.

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

［4］ 賈瑞才.基于四元數EKF的低成本MEMS姿態估計算法［J］.傳感技術學報，2014，27（1）：90-95.

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

［6］ 曲仕茹，馬志強.改進的粒子濾波在四旋翼姿態估計中的應用［J］.飛行力學，2013，31（5）：458-461，466.

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

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

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

［10］薛鵬，任鵬飛，曹學儒.四旋翼飛行器滑模控制的穩定性分析［J］.河南工程學院學報（自然科學版），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的某型航箭發射檢測儀設計［J］.現代電子技術，2015，38（20）：81-83.

［13］萬曉鳳，康利平，余運俊，等.互補濾波算法在四旋翼飛行器姿態解算中的應用［J］.測控技術，2015，34（2）：8-11.

［14］史智寧，韓波，徐玉，等.基于重力場自適應互補濾波的無人直升機水平姿態估計方法［J］.傳感技術學報，2009，22（7）：993-996.

［15］傅忠云，劉文波，孫金秋，等.自適應混合濾波算法在微型飛行器姿態估計中的應用［J］.傳感技術學報，2014，27（5）：698-703.

［16］葉樹球，詹林.基于PID的四旋翼飛行器姿態控制系統［J］.計算機與現代化，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-），女，黑龍江齊齊哈爾人，碩士研究生，主要研究方向為四旋翼飛行器控制算法優化及智能控制，316567231@qq.com；

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

王耀力（1965-），男，山西太原人，副教授，博士，碩士生導師，長期從事信息系統設計、人機視覺分析與處理以及嵌入式系統電路設計理論的研究，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

項目來源：山西省自然科學基金項目（2013011015-1）

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