王宏昊， 陳明， 張坤

　　（哈爾濱理工大學 測控技術與通信工程學院， 黑龍江 哈爾濱 150080）

　　摘要：設計了一款基于嵌入式處理器STM32的四旋翼飛行器，利用低成本傳感器測量加速度和機體的角速率，提出一種結合算法，從而實現飛行器姿態的精確測量。采用四元數法描述飛行器的姿態，在測量過程中結合互補濾波算法進行測量數據的矯正；同時針對出現的隨機噪聲干擾，采用卡爾曼濾波算法，實現姿態的準確測量，最終在實際的飛行器平臺上得到了驗證。

　　關鍵詞：四旋翼；互補濾波；卡爾曼濾波；姿態控制

0引言

　　近年來，無人飛行器逐漸成為人們研究的熱點。無人飛行器的種類繁多，其中四旋翼無人飛行器是一個重要的研究方向［1］。與其他無人飛行器相比，四旋翼飛行器可以實現垂直起降、定點懸停，具有體積小、機動性能好等優勢，尤其適用于災害搜救、航空拍攝和特殊環境的巡視偵查等方面［2］。隨著無人飛行器在民用領域的發展，四旋翼飛行器更以其成本低廉的優點得到了廣泛的應用。

　　飛行器的位姿估計是實現UAV自主能力飛行的基礎。由于微機電系統（Micro Electro Mechanical System，MEMS）具有成本低廉、體積小、功耗低的優點，MEMS被廣泛應用于無人機的慣性導航領域。然而，低成本的傳感器具有嚴重的時變漂移，因此需要采用有效的算法實時估計傳感器的漂移，抑制姿態誤差的累計。至今，國內外已經有很多學者在這方面做了很多工作，參考文獻［3］使用卡爾曼濾波算法對加速度傳感器數據和陀螺儀數據進行融合，實時地改變測量噪聲協方差的值，避免了動態噪聲對加速度傳感器的影響，提高了飛行器姿態測量的精度；參考文獻［4］提出了基于四元數的姿態估計方法，并采用了低功耗的傳感器。本文設計了一款低成本的四旋翼飛行器，結合四元數與互補濾波與卡爾曼濾波算法，實現了對飛行器位姿的有效與精確估計，并在實際中得到了驗證。

1硬件設計

　　為了設計體積小、重量輕、功耗低的姿態航向參考系統，本文選擇集成度高的嵌入式處理器STM32F103作為導航核心處理器，實現姿態和航向解算以及輸出。使用集成加速度計和陀螺儀的MPU6050測量加速度和機體的角速率。加速度計比較敏感、變化速度快,在低動態下可以解算得到比較準確的姿態角；而在高速狀態下，加速度計在飛行過程中釆集到的數據帶有大量的噪聲(主要是由震動產生的)，解算傳來的姿態角也就有了較大的誤差。在加速度較大的情況下，加速度計解算的姿態角變得不可靠，需要使用陀螺儀測量的角速率積分來修正姿態陀螺儀傳感器輸出變化緩慢,但是時間長了有較大的累積誤差,而且還有溫度漂移。利用陀螺儀進行物體姿態檢測需要考慮到累計誤差的消除［5］。因此在四軸飛行器的飛行姿態控制系統中,必須將陀螺儀和加速度計的數據通過數據濾波算法進行融合和濾除噪聲干擾,以此來得到正確的姿態數據。

2飛行器姿態估計算法

　　2.1四元數法

　　四旋翼無人飛行器姿態解算是將飛行器上慣性單元的輸出實時轉換成飛行器的姿態，即飛行器的機體坐標系(xB,yB,zB)相對于導航坐標系(XE,YE,ZE)的角位置。

　　歐拉角是飛行器的3個姿態角，即俯仰角（pitch）、橫滾角（roll）、偏航角（yaw）。根據歐拉旋轉定律，可用3次旋轉使得機體坐標系與導航坐標系重合，每一次旋轉都是以導航標系的x、y、z軸中的一個坐標軸來轉動，轉過的角就是歐拉角，每次旋轉后坐標關系可由一旋轉矩陣來表示，即方向余弦矩陣：

　　式中φ、、θ分別代表偏航角、橫滾角、俯仰角。為避免歐拉角在表示姿態時可能出現的奇異問題，四元數在飛行器的姿態表示方面得到了廣泛的應用。設描述四旋翼飛行器姿態的四元數為：

　　導航坐標系與機體坐標系之間的坐標關系可用方向余弦矩陣表示，其四元數形式為：

　　導航坐標系到機體坐標系的旋轉過程中坐標系始終保持直角坐標系，所以CBE為正交矩陣，即CBE=(CBE)－1=(CBE)T。可得飛行器的姿態角為：

　　將四元數代入可得到:

　　四元數的微分方程為=12Ωbnbq，即:

　　式中wx、wy、wz為機體坐標系下的角速度。在已知初始四元數的情況下，通過三軸陀螺儀測量的3個軸的角速度就可以實時更新四元數的值，進而更新姿態角獲得姿態信息［6］。

　　2.2互補濾波補償算法

　　陀螺儀存在積分誤差，所以解算出來的姿態角也會出現偏差。為了解決這一問題，引入互補濾波算法，利用加速度計來修正陀螺儀的誤差［7］。設加速度計測出來的重力向量為ax、ay、az，陀螺儀積分后的姿態推算出來的重力向量為vx、vy、vz，則有:

　　vx=2(q1q3+q2q4)

　　vy=2(q2q3－q0q1)

　　vz=(q20－q21－q22+q23)（7）

　　從而陀螺儀積分后的姿態結合加速度計數據得到的姿態誤差為:

　　ex=(ayvz－azvy)

　　ey=(azvx－axvz)

　　ez=(axvy－azvx)（8）

　　此姿態誤差與陀螺儀積分誤差成正比，使用互補濾波算法來修正陀螺儀角速度積分誤差：

　　xint=exint+ex*ki

　　yint=eyint+ey*ki

　　zint=ezint+ez*ki （9）

　　其中xint、yint、zint是對陀螺儀的糾正量的積分項，ki為積分系數。

　　x=ωx+kpex+exint

　　y=ωy+kpey+eyint

　　z=ωz+kpez+ezint （10）

　　其中x、y、z為陀螺儀修正后的輸出角速度，kp為比例系數。

　　2.3基于姿態角的卡爾曼濾波算法

　　在實際測試中，上位機接收到姿態角的數據仍然存在少許的噪聲干擾，為了濾除噪聲的干擾，引入卡爾曼濾波算法［8］。設姿態角為α，對其離散化并構造狀態方程和觀測方程：

　　αk=Aαk－1+Wk－1

　　Zk=Hkαk+Vk（11）

　　算法步驟如下。

　　（1）狀態一步預測。通過第k－1時刻的α值，預測第k時刻的α值：

　　αk|k－1=αk

　　（2）一步預測均方誤差。根據第k－1時刻的系統誤差估計第k時刻的系統預測誤差Pk k-1 ：

　　Pk|k－1=Pk－1+Q

　　（3）濾波增益計算：

　　Kk=Pk|k－1/(Pk|k－1+R)

　　（4）狀態更新，計算系統最優估算值：

　　αk=αk－1+K(Zk－αk－1)

　　（5）濾波均方誤差更新。在卡爾曼濾波運算中，要實現對均方誤差的更新，以便下一刻計算卡爾曼增益，更新方程如下：

　　Pk=(1－Kk)Pk|k－1

3軟件設計思路

　　在STM32和MPU6050等傳感器上電后，程序首先進行系統的初始化，在初始化中配置MCU以及傳感器，設定卡爾曼濾波器參數，然后獲取傳感器數據，傳感器的數據經過換算得到陀螺儀和加速度計的測量值。利用加速度的測量值修正陀螺儀的輸出，再通過修正后的角速度更新四元數，進而解算得到姿態角。由于仍然有噪聲的干擾，因此采用卡爾曼濾波算法對姿態角進行濾波，從而得到正確的姿態角［9］。具體軟件流程如圖1所示。

4實驗結果與分析

　　在現有的四旋翼飛行器平臺上，運用上述算法進行實驗。初始時刻，將四旋翼置于水平，然后改變四旋翼的航向，獲取俯仰角、滾轉角、偏航角的數據，通過藍牙傳送到上位機，得到3個姿態角的輸出，如圖2~圖4所示。

　　其中，a、b、c分別表示加速度計解算出來的姿態角輸出、四元數法解算出來的姿態角輸出和卡爾曼濾波后的姿態角輸出。

5結論

　　本文設計了由低成本傳感器組成的四旋翼飛行器，針對傳感器測量數據誤差的復雜性與累積性，通過使用四元數法描述姿態，避免了用歐拉角描述姿態的奇異值問題，同時引入互補濾波與卡爾曼濾波算法，從而更加精確地獲得飛行器的實時精確姿態信息，并在實際的觀測中得到了驗證，結果十分理想，為有效地控制飛行器平臺姿態并最終實現無人飛行打下了良好的基礎。

參考文獻

［1］ KUMAR V, MICHAEL N. Opportunities and challenges with autonomous micro aerial vehicles［J］． The International Journal of Robotics Research, 2012, 31(11) : 12791291．

［2］ BOUABDALLAH S. Design and control of quadrotors with application to autonomous flyin［D］. Lausanne: Ecole Polytechnique Federale De Lausanne, 2007.

［3］ Wang Shaohua, Yang Ying. Quadrotor aircraft attitude estimation and control based on Kalman filter［C］. Proceedings of the 31st Chinese Control Conference, 2012,30(9)：56345639.

［4］ Chao Haiyang, COMMPMANS C, Di Long, et al. A comparative evaluation of lowcost IMUs for unmanned autonomous systems［C］. IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, 2010:211216.

［5］ LAM  Q M, STAMATAKOS N, WOODRUFF C, et al. Gyro modeling and estimation of its random noise sources［C］. AIAA Guidance, Navigation, and Control Conference and Exhibit, Austin, Texas, 2003.

［6］ KRAJNIK T, VONASEK V, FISER D, et al. Ardrone as a platform for robotic research and education［A］. OBDRZALEK D， GOTTSCHEBER A. Communications in Computer and Information Science［C］. Springer, 2011, 161： 172186.

［7］ BRISTEAU P J, CALLOU F, VISSIERE D, et al. The navigation and control technology inside the ar.drone micro uav［C］. Proceedings of the 18th IFAC World Congress, Milan, Italy, AugustSeptember 2011, 2011，18：14771484.

［8］ Wu Xingming, Duan Li, Chen Weihai. A Kalman filter approach based on random drift data of fiber optic gyro［C］. 2011 6th IEEE Conference on Industrial Electronics and Applications (ICIEA), 2011:19331937.

［9］ 劉曉杰，趙曉輝，顧海軍. 微小型四旋翼無人機實時嵌入式控制系統設計與實現［J］.電子技術應用，2009，35（5）：3637.

［10］ 楊萌，雷建和，胡廷軒，等. 基于STM32的四旋翼飛行器控制系統設計［J］.微型機與應用，2015,34（12）：7679.