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

江 杰,劉 娜,杜永興

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

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

江 杰,劉 娜,杜永興

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

2 姿態(tài)檢測(cè)

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

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

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

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

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

飛行器在三維空間中有3種不同的姿態(tài),即俯仰、橫滾、偏航。將陀螺儀、加速度計(jì)和電子羅盤的輸出值定義在一個(gè)坐標(biāo)系,記飛行器的載體坐標(biāo)系為b(X,Y,Z),選擇東北天右手坐標(biāo)系作為導(dǎo)航坐標(biāo)系n(E,N,U)[5]。飛行器在運(yùn)動(dòng)過程中,保持n不變,b相對(duì)于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)系示意圖

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

(1)

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

(2)

四元數(shù)可以看作繞一個(gè)軸的一個(gè)旋轉(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)系計(jì)算航向角:

(10)

陀螺儀檢測(cè)出的是角速度值,經(jīng)過積分運(yùn)算,獲得相應(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軸的角速度對(duì)應(yīng)式(12)中ωx、ωy、ωz。設(shè)采樣周期為T,每隔T讀取ωx、ωy、ωz,代入式(12)得出更新后的四元數(shù),從而得出更新后的方向余弦矩陣[7]。然而,陀螺儀的輸出值因漂移而產(chǎn)生誤差,所以需要合理的補(bǔ)償算法校正誤差,文中融合加速度計(jì)和電子羅盤的數(shù)據(jù),采用共軛梯度法進(jìn)行數(shù)據(jù)處理,以誤差分析和噪聲補(bǔ)償獲得高精度校正結(jié)果。

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

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

(13)

(14)

式中:

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

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

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

(15)

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

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

4 實(shí)驗(yàn)分析和算法驗(yàn)證

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

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

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

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

4.2 運(yùn)動(dòng)狀態(tài)

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

表2 運(yùn)動(dòng)狀態(tài)濾波前后誤差角度

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

圖6 運(yùn)動(dòng)狀態(tài)下濾波前后對(duì)比圖

5 結(jié)論

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

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

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

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

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

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

[6] 龍達(dá)峰, 劉俊, 張曉明, 等. 高速旋轉(zhuǎn)彈飛行姿態(tài)磁測(cè)解算方法 [J]. 彈道學(xué)報(bào), 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ìn)共軛梯度法求解無(wú)約束二次凸規(guī)劃問題 [J]. 大學(xué)數(shù)學(xué), 2014, 30(6): 38-42.

[9] 何瑜. 四軸飛行器控制系統(tǒng)設(shè)計(jì)及其姿態(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

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

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

TP216

A