四旋翼姿態(tài)檢測及數(shù)據(jù)處理系統(tǒng)優(yōu)化設(shè)計

江 杰,劉 娜,杜永興

(內(nèi)蒙古科技大學(xué)信息工程學(xué)院,內(nèi)蒙古包頭 014010)

四旋翼姿態(tài)檢測及數(shù)據(jù)處理系統(tǒng)優(yōu)化設(shè)計

江 杰,劉 娜,杜永興

(內(nèi)蒙古科技大學(xué)信息工程學(xué)院,內(nèi)蒙古包頭 014010)

姿態(tài)角作為四旋翼飛行器重要飛控參數(shù),對穩(wěn)定飛行起著至關(guān)重要作用。針對姿態(tài)的檢測問題,搭建了以STM32F407VG、MPU6050和AK8975組成的姿態(tài)檢測系統(tǒng)。采取四元數(shù)法描述飛行器姿態(tài),通過共軛梯度法對加速度計和電子羅盤測量數(shù)據(jù)做最優(yōu)處理,得出的數(shù)據(jù)融合參數(shù)修正陀螺儀累積漂移誤差,從而達到最佳融合效果。結(jié)果表明該檢測及數(shù)據(jù)處理系統(tǒng)簡單可靠,性能穩(wěn)定,計算量小,能夠滿足四旋翼飛行器對精度和實時性的要求。

姿態(tài)檢測系統(tǒng);四元數(shù);共軛梯度法;互補濾波

0 引言

四旋翼飛行器因其具有較強的機動性能等優(yōu)勢而被廣泛應(yīng)用在工業(yè)和軍事上。飛行器最終的飛行效果與控制系統(tǒng)的性能密切相關(guān),而高效、精確的姿態(tài)檢測是保障優(yōu)質(zhì)控制系統(tǒng)的關(guān)鍵環(huán)節(jié)[1]。

集成度高、性價比好、功耗低的MEMS傳感器是完成微型姿態(tài)測量系統(tǒng)的最佳選擇,但同時這類傳感器有一定的缺陷。陀螺儀動態(tài)響應(yīng)好,但溫度漂移較嚴(yán)重;加速度計精度高、靜態(tài)性能好,但其動態(tài)響應(yīng)比較慢;電子羅盤在測量地磁場的過程中易受周邊環(huán)境中外部磁場的干擾而無法獨立給出測量結(jié)果。因此考慮綜合陀螺儀、加速度計和電子羅盤各自的優(yōu)勢,將三者采集的數(shù)值融合處理,不但可提高系統(tǒng)的動態(tài)性能,還可以減小偏差,使測量的角度與真實的角度更吻合。

系統(tǒng)由主控制器STM32F407VG、9軸運動處理器MPU6050和3軸磁力計AK8975組成。由陀螺儀檢測角速度,經(jīng)過數(shù)學(xué)運算得出角度,將加速度計和電子羅盤測量出的加速度和地磁場這兩個不相關(guān)的三維矢量,作為四旋翼姿態(tài)的觀測矢量來校準(zhǔn)陀螺儀的測量值[2]。

在數(shù)據(jù)融合方面,利用四元數(shù)描述姿態(tài),結(jié)合計算量少且效果較好的共軛梯度法和互補濾波法。結(jié)果表明該檢測及數(shù)據(jù)處理系統(tǒng)能夠滿足四旋翼對精度和實時性的要求。

1 系統(tǒng)設(shè)計

姿態(tài)檢測系統(tǒng)由上位機和下位機兩部分組成,下位機是硬件架構(gòu),主要由STM32F407VG、MPU6050和AK8975及無線模塊等構(gòu)成。傳感器主要作用是實時測量飛行器的原始姿態(tài)信息,并將檢測的模擬測量信號轉(zhuǎn)化為數(shù)字信號輸出,主控制器及其外圍電路的主要作用是實時采集傳感器輸出的數(shù)字測量信號以及融合原始姿態(tài)數(shù)據(jù),得到姿態(tài)信息。無線通信模塊[3]將收到的數(shù)據(jù)通過USB傳送給上位機,實現(xiàn)整個系統(tǒng)的通信。系統(tǒng)硬件電路結(jié)構(gòu)如圖1所示。

圖1 姿態(tài)檢測系統(tǒng)硬件電路結(jié)構(gòu)

系統(tǒng)總體方案如圖2所示。

圖2 系統(tǒng)總體方案

基于ARM CortexM4內(nèi)核的STM-32F407VG處理器,處理速率快,擁有128個外設(shè)通用的輸入輸出接口,故選其作為飛行器數(shù)據(jù)處理的核心單元。角速度和加速度數(shù)據(jù)的測量選用6軸運動處理器MPU6050,該芯片集成了3軸陀螺儀和3軸加速度計[4],其高分辨率能夠測量到不到1°的傾角變化,電子羅盤選用3軸磁強計AK8975,分辨率達0.3 μT/LSB,滿足飛行器對于精度的要求。

2 姿態(tài)檢測

2.1 姿態(tài)檢測原理

加速度傳感器可用來測量三軸加速度值。把傳感器固定在四旋翼的主控板上,在處于相對靜止時,當(dāng)四旋翼姿態(tài)改變,相對于重力場加速度計X、Y、Z三個方向的感應(yīng)軸產(chǎn)生變化,從而產(chǎn)生重力的分量。當(dāng)四旋翼處于變速運動時,加速度計測出的加速度值是物體的重力加速度和線性加速度的總和。但因為加速度計動態(tài)響應(yīng)速度慢,存在累積漂移誤差,并且在飛行器運動的過程中給測量帶來白噪聲,所以其不能獨立使用測量系統(tǒng)的姿態(tài)。

電子羅盤通過磁場在Z軸與Y軸的分布,測量相對于地磁場的方向,提供絕對航向姿態(tài)信息,前提是電子羅盤必須保持水平才可以測出精確的偏航角。飛行器處于三維空間中,故通過磁場傳感器難以單獨實現(xiàn)姿態(tài)檢測。

陀螺儀檢測出三軸角速度,通過對其積分得到姿態(tài)角。然而由于其本身的局限性,積分運算傾向于形成漂移,從而導(dǎo)致誤差累計。因此需要結(jié)合MEMS傳感器的優(yōu)勢,發(fā)揮加速度計及磁力計在穩(wěn)定下的姿態(tài)角測量,以及陀螺儀在動態(tài)條件下的姿態(tài)角測量,通過對其結(jié)果的融合,得出三維空間下準(zhǔn)確的姿態(tài)角。

2.2 飛行器姿態(tài)描述

飛行器在三維空間中有3種不同的姿態(tài),即俯仰、橫滾、偏航。將陀螺儀、加速度計和電子羅盤的輸出值定義在一個坐標(biāo)系,記飛行器的載體坐標(biāo)系為b(X,Y,Z),選擇東北天右手坐標(biāo)系作為導(dǎo)航坐標(biāo)系n(E,N,U)[5]。飛行器在運動過程中,保持n不變,b相對于n經(jīng)過一次或數(shù)次的旋轉(zhuǎn)之后得到新的載體坐標(biāo)系。飛行器坐標(biāo)b系與坐標(biāo)n系之間關(guān)系如圖3所示。

圖3 飛行器載體坐標(biāo)系與導(dǎo)航坐標(biāo)系的關(guān)系示意圖

其中,角ψ、θ、φ表述載體相對于導(dǎo)航坐標(biāo)系的轉(zhuǎn)角,即偏航角、俯仰角、橫滾角[6]。由3個姿態(tài)角可表述b系到n系的關(guān)系為:

(1)

一個單位四元數(shù)可以描述為:

(2)

四元數(shù)可以看作繞一個軸的一個旋轉(zhuǎn),其關(guān)系為:

(3)

式中:μx、μy和μz是角矢量的分量,μ是角矢量大小。用四元數(shù)表述式(1)為:

(4)

由式(1)和式(4)得出由四元數(shù)轉(zhuǎn)換姿態(tài)角的關(guān)系式為:

(5)

3 數(shù)據(jù)處理

3.1 姿態(tài)角度源值獲取

(6)

從而解出φ和θ分別是:

(7)

(8)

(9)

利用三角函數(shù)關(guān)系計算航向角:

(10)

陀螺儀檢測出的是角速度值,經(jīng)過積分運算,獲得相應(yīng)的角度值。設(shè)陀螺儀輸出為ωb=[ωxωyωz]T,則得姿態(tài)角為:

(11)

3.2 多傳感器的數(shù)據(jù)融合

姿態(tài)四元數(shù)q的微分方程用矩陣形式表述為:

(12)

陀螺儀在坐標(biāo)b系中輸出的X、Y、Z軸的角速度對應(yīng)式(12)中ωx、ωy、ωz。設(shè)采樣周期為T,每隔T讀取ωx、ωy、ωz,代入式(12)得出更新后的四元數(shù),從而得出更新后的方向余弦矩陣[7]。然而,陀螺儀的輸出值因漂移而產(chǎn)生誤差,所以需要合理的補償算法校正誤差,文中融合加速度計和電子羅盤的數(shù)據(jù),采用共軛梯度法進行數(shù)據(jù)處理,以誤差分析和噪聲補償獲得高精度校正結(jié)果。

共軛梯度法顧名思義是將共軛性與最速下降方法聯(lián)合,根據(jù)已知點處的梯度建造一組共軛方向,沿著這組方向查找,直到找到目標(biāo)函數(shù)的極小點[8]。相對于其它優(yōu)化算法優(yōu)點顯現(xiàn)在獲得高精度的校正結(jié)果的同時極大的提高了計算效率。

對于該系統(tǒng)中,加速度計與磁感器在坐標(biāo)b系的輸出,經(jīng)過歸一化計算,得出由坐標(biāo)n系到坐標(biāo)b系投影的四元數(shù)表示為:

(13)

(14)

式中:

將式(13)、式(14)中轉(zhuǎn)換得出的數(shù)據(jù)與傳感器測量數(shù)據(jù)相減得到測量誤差。定義誤差函數(shù):

對e(q)取2范數(shù)的平方,設(shè)為共軛梯度法的目標(biāo)函數(shù)f(q),對四元數(shù)的優(yōu)化即為對f(q)求最小值。按照共軛梯度法,載體更新的方程為:

Qm(k+1)=Qm(k)+λkdk

(15)

式中:λk為最優(yōu)步長;dk為搜索方向。根據(jù)共軛梯度算法,定義:f=JT(q)e(q),式中JT(q)為e(q)的雅克比矩陣。

d1=-b1,fk=gk,則λk=-,βk=,dk=-gk+βkdk-1。

當(dāng)利用四元數(shù)得到當(dāng)前時刻的姿態(tài)四元數(shù)后,用式(15)進行次數(shù)迭代得到四元數(shù)最優(yōu)值,為了降低運算量,文中實際程序中只進行一次迭代。加速度計與電子羅盤的輸出值經(jīng)過共軛梯度法修正后,獲得優(yōu)化的四元數(shù)記為Qgh。將陀螺儀測量數(shù)據(jù)進行解算后記為Qω。由于Qgh和Qω在頻域上的互補特性,因此使用互補濾波方法融合兩者,得到新姿態(tài)四元數(shù)Q,能夠達到準(zhǔn)確測量載體的姿態(tài)信息?；パa濾波的公式為:Q=αQgh+(1-α)Qω(0≤α≤1),當(dāng)步長為0.03時,姿態(tài)靜態(tài)值穩(wěn)定。

圖4 融合的共軛梯度下降法框圖

4 實驗分析和算法驗證

為了使傳感器輸出的結(jié)果更加準(zhǔn)確,在數(shù)據(jù)采集前,通過軟件編程對器件的零漂進行處理,對加速度計采取滑動平均濾波法、電子羅盤運用限幅平均濾波法、陀螺儀使用均值濾波法[9],采集的數(shù)據(jù)通過優(yōu)化處理實現(xiàn)軟件修正,最大可能的減少擾動。通過硬件電路采集各個傳感器的信號,檢測的數(shù)據(jù)經(jīng)過簡單的處理傳送到STM32F407上,再將處理后的數(shù)據(jù)進行姿態(tài)解算和數(shù)據(jù)融合。通過串口發(fā)送到PC機上,實時顯示姿態(tài)信息,最終運用MATLAB實現(xiàn)數(shù)據(jù)曲線的繪制。系統(tǒng)驗證主要是通過四旋翼處于靜止和運動時的反復(fù)測試結(jié)果進行比較,驗證算法的有效性。

4.1 靜止?fàn)顟B(tài)

如圖5所示,根據(jù)四旋翼飛行器在采用數(shù)據(jù)融合前后姿態(tài)角度的變化曲線得出,未經(jīng)過數(shù)據(jù)處理回到初始位置有明顯的角度偏移誤差,而且誤差范圍較大。經(jīng)過數(shù)據(jù)融合處理后的數(shù)據(jù)能夠保持在0°左右?；パa濾波前后誤差比較對比如表1所示。

表1 靜止?fàn)顟B(tài)濾波前后誤差角度

4.2 運動狀態(tài)

如6圖所示,姿態(tài)角度在數(shù)據(jù)融合前的起始時間均在0°左右,經(jīng)過一定時間的滾動后,三個角度均有一定范圍的偏差角度,無法實現(xiàn)實時準(zhǔn)確的姿態(tài)角測量。經(jīng)過融合濾波后的數(shù)據(jù),在飛行器經(jīng)過一段時間的轉(zhuǎn)動后,橫滾角、俯仰角、偏航角均能保持在0°?；パa濾波前后誤差比較對比如表2所示。

表2 運動狀態(tài)濾波前后誤差角度

圖5 靜止?fàn)顟B(tài)下濾波前后對比圖

圖6 運動狀態(tài)下濾波前后對比圖

5 結(jié)論

由上述實驗驗證分析可以看出,該系統(tǒng)能夠有效的檢測無人機的姿態(tài)變化。通過無線模塊實現(xiàn)數(shù)據(jù)高效傳輸,有效融合了加速度計、陀螺儀、電子羅盤在測量過程中的優(yōu)勢。實驗結(jié)果表明,這種數(shù)據(jù)融合算法處理,可以使姿態(tài)檢測系統(tǒng)具有較好的靜態(tài)特性和動態(tài)特性,能夠滿足四旋翼飛行器對精度和實效性的要求。

[1] 楊永. 基于PD控制的航天飛行器控制系統(tǒng)的設(shè)計與實現(xiàn) [J]. 計算機測量與控制, 2014, 22(6): 1738-1740.

[2] 劉春陽, 徐軍領(lǐng), 程洪濤, 等. MPU9250傳感器的姿態(tài)檢測與數(shù)據(jù)融合 [J]. 河南科技大學(xué)學(xué)報(自然科學(xué)版), 2015, 36(4): 14-17.

[3] 王俊生, 馬宏緒, 蔡文瀾, 等. 基于ADRC的小型四旋翼無人直升機控制方法研究 [J]. 彈箭與制導(dǎo)學(xué)報, 2008, 28(3): 31-34.

[4] 符秀輝, 趙茂鑫, 周文俊. 基于互補濾波算法的移動機器人姿態(tài)檢測 [J]. 測控技術(shù), 2015, 34(6): 31-33.

[5] 劉鳳鳴. 重力梯度輔助導(dǎo)航定位技術(shù)研究 [D]. 哈爾濱: 哈爾濱工程大學(xué), 2010.

[6] 龍達峰, 劉俊, 張曉明, 等. 高速旋轉(zhuǎn)彈飛行姿態(tài)磁測解算方法 [J]. 彈道學(xué)報, 2013, 25(2): 69-73.

[7] QNUNKA Chiemela, BRIGHT Glen, STOPFORTH Riaan. USV attitude estimation: an approach using quaternion in direction cosine matrix [J]. Robotica, 2016,34(5):995-1009.

[8] 喬熔巖, 趙新國. 改進共軛梯度法求解無約束二次凸規(guī)劃問題 [J]. 大學(xué)數(shù)學(xué), 2014, 30(6): 38-42.

[9] 何瑜. 四軸飛行器控制系統(tǒng)設(shè)計及其姿態(tài)解算和控制算法研究 [D]. 成都: 電子科技大學(xué), 2015.

Optimization Design of Attitude Detection and Data Processing System of a Quad-rotor Aircraft

JIANG Jie,LIU Na,DU Yongxin

(Information Engineering College, Inner Mongolia University of Science and Technology, Inner Mongolia Baotou 014010, China)

As the importance flight control parameter of the four rotor aircraft, attitude angle played an important role in stable flight. For gesture detection problem, attitude detection system was built with STM32F407VG, MPU6050 and AK8975. Spacecraft attitude was described using quaternion method, and the data of the accelerometer and the electronic compass were optimized by using the conjugate gradient method, then the data fusion parameters were used to correct the gyro drift error, so as to achieve the best fusion effect. Results showed that the detection and data processing system was simple, reliable, stable, easy to calculate and could satisfy the requirements of the accuracy and instantaneity of four rotor aircraft.

attitude detection system；quaternion；conjugate gradient method; complementary filter

2016-09-29

國家自然科學(xué)基金(61661044)資助

江杰(1959-),男,內(nèi)蒙古包頭人,教授,研究方向:嵌入式系統(tǒng)、工業(yè)控制網(wǎng)絡(luò)、智能機器人。

TP216

A