基于梯度下降法的四旋翼無(wú)人機(jī)姿態(tài)估計(jì)系統(tǒng)

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

(南京航空航天大學(xué),a.自動(dòng)化學(xué)院; b.無(wú)人機(jī)研究院，南京 210001)

0 引言

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

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

1 硬件設(shè)計(jì)

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

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

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

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

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

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

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

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

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

q=q1+q2i+q3j+q4k

(1)

忽略地球運(yùn)動(dòng)對(duì)四旋翼無(wú)人機(jī)的影響，四元數(shù)矩陣表達(dá)式的微分方程為[11]

(2)

(3)

(4)

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

(5)

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

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

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

(6)

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

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

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

(7)

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

(8)

因此加速度計(jì)的梯度公式可表達(dá)為

(9)

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

(10)

(11)

故磁羅盤的梯度公式為

(12)

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

(13)

(14)

式中：Δt表示采樣時(shí)間；η表示加速度計(jì)和磁羅盤的測(cè)量噪聲。

2.2.2 融合策略

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

(15)

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

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

(16)

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

(17)

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

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

(18)

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

(19)

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

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

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

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

4) 根據(jù)磁羅盤測(cè)得值求出其誤差函數(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 實(shí)驗(yàn)結(jié)果與分析

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

3.1 靜態(tài)分析

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

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

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

3.2 動(dòng)態(tài)分析

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

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

圖4 懸停實(shí)驗(yàn)Fig.4 Hovering test

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

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

圖5 俯仰、滾轉(zhuǎn)、偏航運(yùn)動(dòng)時(shí)角度對(duì)比Fig.5 Angle contrast when pitching,rolling,or yawing

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

4 結(jié)論

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

參 考 文 獻(xiàn)

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

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

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

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

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

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

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

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

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

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

[11] 曹延超.基于STM32的四旋翼飛行器姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)[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] 龍?jiān)坡叮愌?，滕?四旋翼飛行器姿態(tài)解算與濾波[J].計(jì)算機(jī)測(cè)量與控制，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.