基于Mahony濾波器和PID控制器的四旋翼飛行器姿態(tài)控制

黃坡　馬艷　楊萬扣

摘要：四旋翼飛行器由于其簡單的氣動布局和復雜的動力學模型在控制領域引起了研究熱潮，姿態(tài)估計與控制器設計一直是實現(xiàn)四旋翼飛行器穩(wěn)定飛行的難點。為實現(xiàn)精確的四旋翼飛行器姿態(tài)估計，首先分析了IMU傳感器示值組成和誤差存在的原因，然后在方向余弦矩陣（DCM）和重正交化的基礎上，具體給出了Mahony濾波器的實現(xiàn)流程。通過與擴展卡爾曼濾波器對比表明，該算法不僅能保證很高的姿態(tài)估計精度，而且計算時間小于擴展卡爾曼濾波器，有助于提高系統(tǒng)姿態(tài)估計的實時性。結合Mahony濾波后的姿態(tài)信息，采用嵌套PI-PID控制策略設計了控制器。最后，將姿態(tài)估計算法和控制算法應用到實驗平臺上，可以實現(xiàn)四旋翼飛行器懸停和角度跟蹤功能。

關鍵詞：四旋翼飛行器；Mahony濾波器；姿態(tài)估計；DCM；嵌套PI-PID控制器

中圖分類號：TP273 文獻標識碼：A 文章編號：1009-3044（2014）07-1611-07

近年來，隨著MEMS傳感器，嵌入式微處理器以及無刷直流電機技術上的突破，四旋翼飛行器研究獲得了飛速發(fā)展[1]。四旋翼飛行器在民用和軍用方面具有廣闊的應用前景，在一些人類無法涉及或應該避免的環(huán)境中，比如自然災害和工業(yè)災害，可以由它來代替人類完成工作；另外它的低空預警和輕物運輸功能，具備很大商業(yè)潛力和商業(yè)價值，目前正被逐漸地投入使用。

雖然四旋翼飛行器的結構和原理并不復雜，但是傳感器信息獲取和控制方法選用卻帶來了較大的難度[2]。四旋翼飛行器作為一個非線性強耦合系統(tǒng)，有四個輸入六個輸出。獨立輸入數(shù)量小于輸出自由度數(shù)量，因此它屬于欠驅(qū)動系統(tǒng)。輸出的6個自由度包括位置和姿態(tài)角度（俯仰角，滾轉角，偏航角）。實現(xiàn)四旋翼飛行器的自主飛行需要實現(xiàn)位置控制與姿態(tài)控制。姿態(tài)控制是四旋翼飛行器實現(xiàn)各項工作的核心，位置控制建立在完善的姿態(tài)控制的基礎之上。因此本文關心的主要是姿態(tài)控制。

獲取準確的姿態(tài)角度值是研究四旋翼飛行器的一個重要方面。對陀螺儀積分或者依據(jù)加速度計得到姿態(tài)角盡管是一種直觀的想法，但是陀螺儀工作一段時間會出現(xiàn)零點漂移，積分帶來的誤差隨時間將越來越大，而加速度計則更容易受到噪聲或者震動的干擾，因此依據(jù)兩種方式獲取的姿態(tài)角必定與真實的姿態(tài)角相差很大。如上所述，單用傳感器無法獲得準確的姿態(tài)角信息，必須結合濾波算法對姿態(tài)角進行估計。

為整體上解決四旋翼飛行器的姿態(tài)控制，該文提出了基于Mahony濾波器[3-4]的姿態(tài)估計和嵌套PI-PID[5]控制器。首先對各傳感器的示值組成和特性進行了分析，在此基礎上，根據(jù)Mahony濾波器數(shù)學原理，給出了基于方向余弦矩陣（DCM）的濾波器詳細實現(xiàn)。Mahony濾波器運用了互補濾波器的思想，但增加了旋轉角速度修正環(huán)節(jié)，在本文中，為了得到更加準確的結果，增加了對每次計算得到的DCM重新正交化這一環(huán)節(jié)。對比卡爾曼濾波器[6-7]，由于卡爾曼濾波器基于復雜矩陣運算，計算時間過長將導致姿態(tài)角更新時間變長，容易出現(xiàn)延時，Mahony濾波器在良好的姿態(tài)估計基礎上耗費時間更少，因此實時性更好。接著，姿態(tài)控制器采用了嵌套PI-PID控制器，盡管有很多非線性的方法被提出運用到四旋翼飛行器，但是由于模型的不確定性，它們實際表現(xiàn)反而不如PID控制器好。嵌套PI-PID控制器由一個速率控制內(nèi)環(huán)和一個位置控制外環(huán)組成。

1 四旋翼飛行器的氣動原理

四旋翼飛行器結構一般呈十字型，為獲得較輕機體質(zhì)量，宜采用碳纖維作為材料。十字型端點處各有一個用電機驅(qū)動的旋翼，相對旋翼旋轉方向一致，如圖1所示，若前后旋翼順時針（逆時針）旋轉，則左右旋翼逆時針（順時針）旋轉。

2 基于Mahony濾波器的姿態(tài)估計

5 總結與展望

本文提出基于Mahony濾波器的姿態(tài)估計和嵌套PI-PID控制器設計。首先設計了Mahony濾波器，不僅可以得到準確的姿態(tài)估計，而且計算耗費時間對比卡爾曼濾波器更少，對實時迅速地控制飛行器姿態(tài)變化非常有利。在良好的姿態(tài)估計基礎上，控制器設計部分采用嵌套PI-PID控制，最終，四旋翼飛行器實現(xiàn)懸停和姿態(tài)跟蹤。后續(xù)的工作包括在姿態(tài)控制的基礎上，實現(xiàn)位置速度控制和路點飛行。

參考文獻：

[1] Mahony R，Kumar V，Corke P.Multirotor Aerial Vehicles Modeling， Estimation and Control of Quadrotor[J]. IEEE Robotics & Automation Magazine，2012，19（3）： 20-32.

[2] 王偉，馬浩，孫長銀.四旋翼姿態(tài)控制系統(tǒng)設計[J].科學技術與工程，2013，13（19）：5513-5519.

[3] Mahony R，Hamel T，Pflimlin Jean-Michel.Complementary Filters on the Special Orthogonal Group[C].IEEE Transactions on Automatic Control，Europe，2008，53（5）：1203-1218.

[4] Mahony R，Hamel T，Pflimlin Jean-Michel.Complementary filter design on the Special orthogonal group SO（3）[C].2005 The 44th IEEE Conference on Decision and Control. Spain， Serville，2005：1477-1484.

[5] BouabdallaH S，Noth A，Siegwart R.Techniques Applied to an Indoor Micro Quadrotor[C].Proceedings of 2004 IEEE International Conference on Intelligent Robots and Systems，Japan， Sendal，2004.3：2451-2456.

[6] Hoffmann F，Goddemeier N，Bertram T.Attitude estimation and control of a quadrotor[C].2010 IEEE International Conference on Intelligent Robots and Systems，Taiwan， Taipei，2010：1072-1077.

[7] Chan A L，Tan S L，Kwek C L.Sensor data fusion for attitude stabilization in a low cost Quadrotor system[C]. 2011 IEEE15th International Symposium on Consumer Electronics.Singapore，2011： 34-39.

[8] Hamel T，Mahony R.Attitude estimation on SO（3）based on direct inertial measurements[C]. Proceedings of 2006 IEEE International Conference on Robotics and Automation. Florida， Orlando，2006：2170-2175.

[9] Diebel J.Representing Attitude： Euler Angles， Unit Quaternions， and Rotation Vectors[EB/OL].[2006-10-20]. http：//citeseerx.ist.psu.edu/viewdoc/download？doi=10.1.1.110.5134&rep=rep1&type=pdf.