劉青文， 郭劍東， 浦黃忠， 甄子洋

(南京航空航天大學(xué),a.自動化學(xué)院; b.無人機研究院，南京 210001)

0 引言

近年來，隨著無人機市場的火熱，四旋翼無人機由于體積小、結(jié)構(gòu)簡單、操縱方便等優(yōu)點吸引著越來越多科研人員的關(guān)注[1]。無人機姿態(tài)解算是實現(xiàn)其自主飛行的前提條件，直接決定著無人機飛行的穩(wěn)定性與位置的精確性[2]，因此姿態(tài)解算是無人機研究的熱點領(lǐng)域。得益于微機電系統(tǒng)(MEMS)的飛速發(fā)展[3]，四旋翼無人機的姿態(tài)解算系統(tǒng)普遍采用低成本、重量輕，便于集成、攜帶的航姿參考系統(tǒng)(AHRS)，它主要由三軸陀螺儀、三軸加速度計和三軸磁羅盤組成，既可以應(yīng)用于小型無人機[4]，又可以應(yīng)用于車輛自主駕駛[5]。然而傳感器本身具有嚴(yán)重的時變漂移[6]，陀螺儀瞬態(tài)性能良好，但會隨著時間累積誤差，加速度計雖沒有積分誤差，但處于高動態(tài)時[4]，會受到附加平動或轉(zhuǎn)動加速度干擾，磁羅盤易受到周圍環(huán)境磁場的干擾，因此，單獨采用陀螺儀或者加速度計和磁羅盤都無法獨立給出精度高的姿態(tài)估計[7]，所以需要采用適當(dāng)?shù)淖藨B(tài)解算算法對傳感器數(shù)據(jù)進行誤差補償。

目前對姿態(tài)解算系統(tǒng)的研究常用的算法有卡爾曼濾波、互補濾波、梯度下降法等[6]。卡爾曼濾波應(yīng)用比較廣泛，但建立穩(wěn)定可靠的姿態(tài)方程、確定合適的量測噪聲以及過程噪聲協(xié)方差矩陣都比較困難[3];互補濾波由于對無人機姿態(tài)估計需要重構(gòu)的缺陷，且其精度低，姿態(tài)漂移嚴(yán)重，不適用于動態(tài)環(huán)境[8-9];本文設(shè)計了一種以STM32F427為主控制器，MPU6000和LSM303D為姿態(tài)傳感器的姿態(tài)估計系統(tǒng)。系統(tǒng)采用四元數(shù)法對姿態(tài)進行描述，利用梯度下降法有效提高了四旋翼無人機姿態(tài)解算的精度。

1 硬件設(shè)計

本文姿態(tài)估計系統(tǒng)選擇意法半導(dǎo)體(ST)公司的基于Cortex-M4內(nèi)核的32位微控制器STM32F427VIT6為主控制器，其主頻高達180 MHz,且?guī)в袉沃芷诔朔ê陀布?，滿足系統(tǒng)對數(shù)據(jù)運算速度的要求。航姿參考系統(tǒng)(AHRS)選用兩款高精度的姿態(tài)傳感器(內(nèi)部均集成高精度的ADC)，分別為MPU6000和LSM303D，構(gòu)成了一個低成本、高精度的九軸姿態(tài)估計系統(tǒng)。姿態(tài)估計系統(tǒng)如圖1所示。

圖1 姿態(tài)估計系統(tǒng)框圖Fig.1 Block diagram of attitude estimation system

主控制器通過SPI總線(速率為22.5 MHz)讀取姿態(tài)傳感器數(shù)據(jù)，然后進行單位換算、坐標(biāo)轉(zhuǎn)換、梯度下降法解算姿態(tài)誤差和融合陀螺儀角速度積分解算出歐拉角。姿態(tài)估計系統(tǒng)實物如圖2所示。

圖2 姿態(tài)估計系統(tǒng)實物Fig.2 The attitude estimation system

2 四旋翼無人機姿態(tài)解算

本文設(shè)計的算法步驟如下：首先，利用陀螺儀輸出的角速度和四元數(shù)微分方程計算出角速度微分四元數(shù)；然后,對測得的加速度計和磁羅盤數(shù)據(jù)進行預(yù)處理，根據(jù)加速度計和磁羅盤的誤差函數(shù)及其導(dǎo)數(shù)，得出姿態(tài)傳感器的梯度公式，再利用梯度下降法消除四元數(shù)誤差，得到精確的姿態(tài)四元數(shù)；最后,將陀螺儀角速度積分得到的四元數(shù)融合梯度下降法求出的姿態(tài)四元數(shù)，從而實現(xiàn)了姿態(tài)數(shù)據(jù)的補償修正，提高了姿態(tài)解算精度。在姿態(tài)解算過程中只有普通的乘法和加法運算[9]，這大大減輕了姿態(tài)解算的計算壓力，提高了姿態(tài)解算的速度。

2.1 四元數(shù)姿態(tài)描述

在對四旋翼無人機姿態(tài)描述前，必須先建立合適的坐標(biāo)系。無人機姿態(tài)描述中常用的兩種坐標(biāo)系分別為機體坐標(biāo)系b和地理坐標(biāo)系e。通常取機體的重心為機體坐標(biāo)系原點，X,Y,Z軸分別與機體的縱軸、橫軸和豎軸相互重合，定義無人機繞機體坐標(biāo)系的Z軸轉(zhuǎn)動為偏航角ψ；繞X軸轉(zhuǎn)動為滾轉(zhuǎn)角φ，繞Y軸轉(zhuǎn)動為俯仰角θ。在忽略地球自轉(zhuǎn)的情況下，可將地面坐標(biāo)系看成慣性坐標(biāo)系，則兩個坐標(biāo)系的轉(zhuǎn)換可由轉(zhuǎn)換矩陣表示為[4]

由于歐拉角在表示姿態(tài)時會出現(xiàn)“奇異點”現(xiàn)象，故不能實現(xiàn)全姿態(tài)解析。四元數(shù)在姿態(tài)描述方面成功避免了“奇異點”現(xiàn)象，且計算量小，實時性高，滿足飛行控制的要求。四元數(shù)描述的一般形式為[10]

q=q1+q2i+q3j+q4k

(1)

忽略地球運動對四旋翼無人機的影響，四元數(shù)矩陣表達式的微分方程為[11]

(2)

(3)

(4)

結(jié)合三角函數(shù)變換矩陣和四元數(shù)變換矩陣得出四旋翼無人機的姿態(tài)角四元數(shù)表達形式為[14]

(5)

2.2 基于梯度下降法的四元數(shù)估計

梯度下降法是沿梯度下降的方向求解表達式的最小值,其迭代表達式為

xn+1=xn-δ▽F(xn)

(6)

可知梯度下降法屬于一階收斂。式中：▽F(xn)是在xn處的梯度；負號表示梯度的負方向(即當(dāng)前最快下降收斂速度)；δ為梯度方向上的步長，誤差函數(shù)取函數(shù)▽F(xn)的方向，即▽F(xn)/‖▽F(xn)‖。當(dāng)誤差函數(shù)無限接近于0時，可以認(rèn)為式(6)達到了穩(wěn)定解，梯度法計算完成。具體到本文中，誤差函數(shù)為多元向量函數(shù)，自變量為四元數(shù)向量。算法初始化前設(shè)定經(jīng)過量化后的陀螺儀、加速度計以及磁羅盤輸出分別為ωb=[0ωxωyωz]，ab=[0axayaz]，mb=[0mxmymz]，當(dāng)?shù)刂亓κ噶亢痛艌鍪噶糠謩e表示為ge=[0 0 0 1]，ρe=[0ρx0ρz]。

2.2.1 加速度計、磁羅盤預(yù)處理

僅利用陀螺儀角速度積分獲得的四元數(shù)隨著時間增加，誤差逐漸增大。為減少誤差，可以利用四元數(shù)表征的誤差函數(shù)等于0實現(xiàn)。由文獻[15]可得加速度計表征的四元數(shù)矩陣誤差函數(shù)為

(7)

上式為重力加速度在慣性坐標(biāo)系中的值通過四元數(shù)法旋轉(zhuǎn)到機體坐標(biāo)系中的值，再減去當(dāng)前加速度計的測量值。為使誤差函數(shù)等于0，對式(7)進行四元數(shù)偏導(dǎo)，得到的導(dǎo)數(shù)即為雅克比矩陣[15]，即

(8)

因此加速度計的梯度公式可表達為

(9)

同理可得磁羅盤的誤差函數(shù)及其雅克比矩陣分別為

(10)

(11)

故磁羅盤的梯度公式為

(12)

根據(jù)式(6)，設(shè)經(jīng)過梯度下降法預(yù)處理加速度計和磁羅盤后計算的姿態(tài)四元數(shù)為q▽,t，則

(13)

(14)

式中：Δt表示采樣時間；η表示加速度計和磁羅盤的測量噪聲。

2.2.2 融合策略

設(shè)式(2)求解的陀螺儀角速度姿態(tài)四元數(shù)為

(15)

加速度計以及磁羅盤預(yù)處理后的姿態(tài)四元數(shù)為q▽,t，目標(biāo)姿態(tài)四元數(shù)為qest,t，則融合策略為

qest,t=αq▽,t+(1-α)qω,t0≤α≤1

(16)

式中，等式右邊由兩部分組成，前者適用于高速運動，后者適用于低速運動[15]，因此α是一個動態(tài)值。文獻[15]給出了一種α的定義，即

(17)

式中，β為四元數(shù)微分方程求解的姿態(tài)算法的發(fā)散速度，也是陀螺儀的測量誤差，可通過查詢傳感器手冊獲得。

由于梯度下降法收斂速度與運動速度有關(guān)，因此用于高速運動時α必須盡可能大(過大容易導(dǎo)致靜態(tài)性能差)。此時μt/Δt比較大，且β很小，故

(18)

將式(13)、式(15)、式(18)代入式(16)并簡化得出最終的梯度下降法的姿態(tài)融合公式為

(19)

綜上所述,本文的算法步驟如下：

1) 初始化，姿態(tài)傳感器和陀螺儀輸入?yún)?shù)單位規(guī)范化，確定β值；

2) 根據(jù)陀螺儀角速度利用其四元數(shù)微分方程求得角速度微分四元數(shù)；

3) 根據(jù)加速度測得值求出其誤差函數(shù)及其導(dǎo)數(shù)；

4) 根據(jù)磁羅盤測得值求出其誤差函數(shù)及其導(dǎo)數(shù)；

5) 利用步驟3)，4)中的誤差函數(shù)及其導(dǎo)數(shù)表征梯度公式，再利用梯度下降法求出姿態(tài)四元數(shù)q▽,t；

6) 將步驟2)中角速度微分四元數(shù)和步驟5)中姿態(tài)四元數(shù)利用融合策略推導(dǎo)出最終梯度下降法的姿態(tài)融合公式；

7) 將步驟6)中解算的四元數(shù)結(jié)合式(5)更新出最新姿態(tài)角；

8) 重復(fù)以上步驟，不斷更新姿態(tài)角。

3 實驗結(jié)果與分析

為驗證本文算法，搭建四旋翼無人機飛行平臺，并通過靜態(tài)與動態(tài)兩組實驗分析姿態(tài)估計系統(tǒng)的有效性。實驗中本文實物系統(tǒng)MPU6000采樣頻率為1000 Hz；陀螺儀角速度測量范圍為±1000 (°)/s，加速度計的測量范圍為±4g,磁羅盤的測量范圍為±4 Gauss；數(shù)傳頻率為433 MHz；梯度下降法計算周期為1 ms；經(jīng)過測試從傳感器數(shù)據(jù)采集到完成一次梯度下降解算用時980 μs左右。

3.1 靜態(tài)分析

靜態(tài)分析即地面狀態(tài)對姿態(tài)估計系統(tǒng)數(shù)據(jù)進行分析，以俯仰角為例，數(shù)據(jù)采集頻率為10 Hz。圖3a為系統(tǒng)水平放置時俯仰角的輸出值，圖3b為施加外力時加速度計測得的俯仰角和本文算法解算的俯仰角。

圖3 靜態(tài)測試Fig.3 Static test

由圖3可得，水平測試時俯仰角在0.08°～0.2°之間振蕩，且沒有發(fā)散，有效消除了陀螺儀積分等誤差；外力加載時，加速度計測得值曲線毛刺較多，本文算法解算值不僅能快速跟蹤上加速度計解算值，而且曲線相對平滑，這大大削弱了加速度計帶來的測量白噪聲，平均跟蹤偏差為0.26°。

3.2 動態(tài)分析

動態(tài)分析是將本文實物系統(tǒng)與Pixhawk飛控一起安裝于飛行平臺，通過實時飛行分析姿態(tài)估計數(shù)據(jù)，其中，Pixhawk飛控數(shù)傳頻率為915 MHz，姿態(tài)采樣頻率均為10 Hz。將Pixhawk飛控飛行日志和串口調(diào)試助手保存的數(shù)據(jù)導(dǎo)入Execl表中，并通過Matlab選取試飛數(shù)據(jù)進行比較分析。

當(dāng)四旋翼無人機處于懸停狀態(tài)時，以俯仰角為例,其輸出結(jié)果如圖4所示。

圖4 懸停實驗Fig.4 Hovering test

由于機體振動，俯仰角基本在(-0.5°,0.3°)之間變化，滿足實際飛行時姿態(tài)解算的精度；在162 s時因風(fēng)力變化導(dǎo)致機體振動變化，俯仰角也隨之變大，12 s后機體恢復(fù)正常，俯仰角也回落到正常值。

當(dāng)四旋翼無人機做俯仰、滾轉(zhuǎn)、偏航運動時，如圖5所示,梯度下降法測得的俯仰角、滾轉(zhuǎn)角、偏航角為曲線A，Pixhawk飛控測得的俯仰角、滾轉(zhuǎn)角、偏航角為曲線B。

圖5 俯仰、滾轉(zhuǎn)、偏航運動時角度對比Fig.5 Angle contrast when pitching,rolling,or yawing

從圖5可以看出，在0～360 s內(nèi)，梯度下降法解算的姿態(tài)角沒有發(fā)散現(xiàn)象，有效降低了因陀螺儀積分帶來的誤差；同時兩種測試系統(tǒng)的俯仰角偏差在±2.5°以內(nèi)，滾轉(zhuǎn)角偏差在±3°以內(nèi)，偏航角平均偏差為0.8°。因此，本文設(shè)計的姿態(tài)解算系統(tǒng)能夠滿足四旋翼無人機實際飛行的需求。

4 結(jié)論

本文在基于四元數(shù)姿態(tài)描述的基礎(chǔ)上，設(shè)計了一種以STM32F427為主控制器，MPU6000和LSM303D為姿態(tài)傳感器，利用梯度下降法預(yù)處理加速度計和磁羅盤的初始數(shù)據(jù)，并與陀螺儀角速度積分相融合的姿態(tài)估計系統(tǒng)。實驗結(jié)果表明：梯度下降法彌補了陀螺儀角速度積分導(dǎo)致的累積誤差，削弱了加速度計的測量白噪聲；經(jīng)過融合解算后的姿態(tài)角能夠快速可靠地估計無人機的飛行姿態(tài)。因此，本文算法解算后的姿態(tài)角滿足四旋翼無人機姿態(tài)控制的要求，為后續(xù)四旋翼無人機完成各種飛行任務(wù)奠定基礎(chǔ)。

參 考 文 獻

[1] PHAM S T,CHEW M T.Sensor signal filtering in quadrotor control[C]//IEEE Sensors Applications Symposium, 2014:293-298.

[2] 楚仕彬，袁亮.小型四旋翼無人機姿態(tài)測量仿真研究[J].計算機仿真，2015，32(2):67-73.

[3] 李文鵬，唐海洋.基于STM32的四旋翼飛行器姿態(tài)解算的研究[J].單片機與嵌入式系統(tǒng)應(yīng)用，2016(6)：13-16.

[4] 王宏昊，陳明，張坤.基于四元數(shù)與卡爾曼濾波的四旋翼飛行器姿態(tài)估計[J].微型機與應(yīng)用，2016，35(14):71-73，76.

[5] 賈瑞才.基于四元數(shù)EKF的低成本MEMS姿態(tài)估計算法[J].傳感技術(shù)學(xué)報，2014，27(1):90-95.

[6] 何川，李智，王勇軍.基于STM32的四旋翼飛行器的姿態(tài)最優(yōu)估計研究[J].電子技術(shù)應(yīng)用，2015，41(12):61-64.

[7] 張浩，任芊.四旋翼飛行器航姿測量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動化，2013，32(1):28-31.

[8] 梁延德，程敏，何福本，等.基于互補濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng)，2011，30(11)：56-61.

[9] 陳亮，楊柳慶，肖前貴.基于梯度下降法和互補濾波的航向姿態(tài)參考系統(tǒng)[J].電子設(shè)計工程，2016，24(24)：38-41.

[10] 蔣鈺，諶海云，岑汝平.基于四元數(shù)的四旋翼飛行器姿態(tài)解算算法[J].制造業(yè)自動化，2015，37(12):77-80.

[11] 曹延超.基于STM32的四旋翼飛行器姿態(tài)測量系統(tǒng)設(shè)計[J].軟件，2015，36(1)：104-109.

[12] KRAJNIK T,VONASEK V,FISER D,et al.AR-drone as a platform for robotic research and education[C]//International Conference on Reseach & Education in Robotics, 2011:172-186.

[13] 劉建業(yè)，曾慶化，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，2010.

[14] 龍云露，陳洋，滕雄.四旋翼飛行器姿態(tài)解算與濾波[J].計算機測量與控制，2016，24(10)：194-201.

[15] MADGWICK S O H,HARRISON A J L,VAIDYANATHAN R.Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//IEEE International Conference on Rehabilitation Robotics (ICORR),2011:1-7.