，，

(1.海軍工程大學(xué) a.兵器工程系；b.導(dǎo)航工程系，武漢 430033；2.海軍海洋測(cè)繪研究所，天津 300061)

隨著MEMS技術(shù)的成熟，以及對(duì)姿態(tài)測(cè)量中低功耗、低成本的需求，微機(jī)械陀螺、加速度計(jì)和磁傳感器在無(wú)人機(jī)、智能機(jī)器人等領(lǐng)域得到廣泛的應(yīng)用[1]。針對(duì)單一傳感器測(cè)量姿態(tài)存在零漂，受環(huán)境因素影響大等問(wèn)題，多傳感器融合測(cè)姿已成為一種發(fā)展趨勢(shì)，并且隨著MEMS傳感器的迅速發(fā)展以及向各個(gè)學(xué)科領(lǐng)域的滲透，其各個(gè)方面性能如精度、魯棒性、動(dòng)態(tài)響應(yīng)等都得到了很大的提高，因此，如何在多MEMS傳感器系統(tǒng)中實(shí)現(xiàn)數(shù)據(jù)融合進(jìn)而進(jìn)行姿態(tài)測(cè)量成為主要研究方向。Marins基于四元數(shù)擴(kuò)展的卡爾曼濾波，研究使用MARG傳感器進(jìn)行姿態(tài)檢測(cè)[2]，然而該算法計(jì)算量較大，難以在硬件上實(shí)現(xiàn)。宋東等人[3]提出了一種基于MEMS加速度計(jì)、磁阻傳感器的方法，該方法簡(jiǎn)化了計(jì)算量，但無(wú)法抑制動(dòng)態(tài)環(huán)境下數(shù)據(jù)中的噪聲。Mahony等人[4]利用MEMS器件及互補(bǔ)濾波器建立姿態(tài)估計(jì)方法，得出互補(bǔ)濾波器截止頻率設(shè)計(jì)方法，但該算法PI參數(shù)只能通過(guò)試湊，無(wú)法實(shí)時(shí)調(diào)整PI參數(shù)。針對(duì)以上姿態(tài)測(cè)量方法的缺陷，基于GY-85九軸傳感器模塊，設(shè)計(jì)一種以單片機(jī)MSP430F5438為核心的姿態(tài)檢測(cè)系統(tǒng)，利用梯度下降法對(duì)傳感器進(jìn)行數(shù)據(jù)融合，彌補(bǔ)單一傳感器測(cè)姿的不足。

1 硬件組成

設(shè)計(jì)的系統(tǒng)以MSP430F5438為核心，通過(guò)與集成了三軸加速度計(jì)ADXL345、三軸地磁傳感器HMC5883L以及陀螺儀ITG3205的九軸傳感器GY-85進(jìn)行I2C通信以采集各個(gè)傳感器的輸出數(shù)據(jù)，并將解算出的各個(gè)姿態(tài)角通過(guò)無(wú)線串口發(fā)送給上位機(jī)。系統(tǒng)設(shè)計(jì)的原理框圖如圖1所示。

圖1 系統(tǒng)設(shè)計(jì)原理框圖

MSP430F5438是德州儀器近兩年推出的MSP43054XX系列單片機(jī)中的最高型號(hào)，其資源充足，功能強(qiáng)大，非常適用于多傳感器的姿態(tài)檢測(cè)。ADXL345是一款超低功耗的三軸加速度計(jì)，分辨率高(13位)，測(cè)量范圍達(dá)±16 g，能測(cè)量到小于1°的傾斜角度變化[5]。HMC5883L磁場(chǎng)傳感器分辨率達(dá)0.002 G，最大輸出頻率達(dá)160 Hz。陀螺儀ITG3205內(nèi)部具有3個(gè)整合16位模數(shù)轉(zhuǎn)換器，提供陀螺儀同步取樣，并帶有可選擇的數(shù)字低通濾波器以及快速模式(400 kHz)的I2C接口。

2 軟件流程

系統(tǒng)的主流程圖如圖2所示。為了便于移植，對(duì)所設(shè)計(jì)程序進(jìn)行模塊化處理，通過(guò)調(diào)用各個(gè)子函數(shù)完成所需的功能。程序首先完成MSP430F5438初始化，并對(duì)各個(gè)傳感器的工作模式進(jìn)行初始化設(shè)定，接著對(duì)四元數(shù)初始化及各個(gè)傳感器的初始偏移進(jìn)行校正。隨后在中斷函數(shù)程序中完成數(shù)據(jù)采集與濾波，并解算出姿態(tài)角發(fā)送給上位機(jī)。

圖2 系統(tǒng)流程

3 系統(tǒng)校正

加速度計(jì)校正的方法為將傳感器水平放置，使z軸在1 g的重力場(chǎng)，x、y軸在0 g的重力場(chǎng)，取一系列樣本的平均值，記為x0、y0、z1。這些值即為相對(duì)于各軸的偏移量，通過(guò)減去這些值以獲得實(shí)際加速度的值。

(1)

式中：Xacc、Yacc、Zacc為加速度計(jì)實(shí)際值，Xmea、Ymea、Zmea為加速度計(jì)測(cè)量值，SZ為加速度計(jì)z軸在1 g的重力場(chǎng)下的理想靈敏度。

陀螺儀校正時(shí)需采集初始時(shí)的一組數(shù)據(jù)求平均值后作為零位偏移，此后將測(cè)量值減去偏移即可。

ωacc=ωmea-ωbias

(2)

式中：ωacc為陀螺儀實(shí)際值；ωmea為陀螺儀測(cè)量值；ωbias為零位偏移值。

在理想狀態(tài)下，地磁傳感器三軸的輸出磁場(chǎng)矢量應(yīng)構(gòu)成一個(gè)圓心在原點(diǎn)的正球體，而在實(shí)驗(yàn)的過(guò)程中，地磁傳感器受到周?chē)泊鸥蓴_和軟磁干擾，導(dǎo)致該球體變成了一個(gè)偏離圓心的橢球體，所以需對(duì)地磁傳感器進(jìn)行校正。圖3和圖4為采用AntMag軟件對(duì)地磁傳感器校正前后的對(duì)比示意圖。校正后的地磁傳感器各軸的零位偏移和均方誤差有了明顯的改善，如表1所示。

圖3 校正前地磁傳感器輸出

圖4 校正后地磁傳感器輸出

校正X零位偏移Y零位偏移Z零位偏移均方誤差校正前0.08660.09380.11930.059校正后0.00080.00180.00330.029

4 姿態(tài)解算

4.1 坐標(biāo)系建立、四元數(shù)與歐拉角之間的轉(zhuǎn)換

分別建立導(dǎo)航坐標(biāo)系N和載體坐標(biāo)系B，N系采用東北天坐標(biāo)系，B系固連在載體上，傳感器的三軸分別安裝在B系的x、y、z軸上。設(shè)α、β、γ分別為俯仰角，滾轉(zhuǎn)角、航向角。則由N系到B系的旋轉(zhuǎn)矩陣可表示為

(3)

用四元數(shù)Q=q0+q1i+q2j+q3k(|Q|=1)表示該旋轉(zhuǎn)矩陣為

(4)

通過(guò)上述2種表示形式，將歐拉角用四元數(shù)表示為

(5)

4.2 四元數(shù)微分方程求解

根據(jù)四元數(shù)微分方程定義，對(duì)應(yīng)的矩陣[6]為

采用一階龍格庫(kù)塔求解四元數(shù)微分方程。

Q(t+h)=Q(t)+hk1

(7)

得到四元數(shù)更新方程為

式中：ωx、ωy、ωz為△軸輸出角速度；h為采樣周期。

4.3 單傳感器解算姿態(tài)角

ADXL345加速度計(jì)在靜態(tài)或者低動(dòng)態(tài)的情況下利用重力在各軸上的投影分量求解姿態(tài)角[7]。

(9)

式中：α、β、γ分別為俯仰角、翻滾角、z軸與重力加速度的夾角。

ITG3205陀螺儀測(cè)得繞各個(gè)軸的角速度，通過(guò)積分便可以得到繞各個(gè)軸旋轉(zhuǎn)的角度[8]:

θi=θi-1+ωdt

(10)

式中：θi為i時(shí)刻的角度值；θi-1為i-1時(shí)刻的角度值；ω為修正后的角速度；dt為積分時(shí)間。

HMC5883L地磁傳感器通過(guò)測(cè)得地磁場(chǎng)在其各軸上的投影分量解算出航向角。當(dāng)?shù)卮艂鞲衅魈幱谒綘顟B(tài)時(shí)，可通過(guò)式(11)對(duì)航向角進(jìn)行檢測(cè)，而當(dāng)其處于傾斜狀態(tài)時(shí)，則需通過(guò)加速度計(jì)所測(cè)俯仰角α和翻滾角β代入式(12)，對(duì)其進(jìn)行傾斜補(bǔ)償后解算出航向角。

γ=arctan(Hy/Hx)

(11)

(12)

式中：Hx、Hy、Hz為地磁傳感器各軸所測(cè)數(shù)據(jù)。

4.4 梯度下降法融合解算姿態(tài)

通過(guò)陀螺儀積分解算姿態(tài)，由于受到溫度和零漂的影響，長(zhǎng)時(shí)間使用誤差較大[9]。而加速度計(jì)不僅對(duì)重力，同時(shí)對(duì)外力產(chǎn)生的加速度十分敏感，動(dòng)態(tài)下噪聲明顯，且其不能對(duì)陀螺儀所測(cè)的航向角進(jìn)行修正，故無(wú)法使用單傳感器對(duì)姿態(tài)進(jìn)行測(cè)量。本文基于Madgwick所提出的梯度下降法，將九軸傳感器中的加速度計(jì)、陀螺儀、地磁傳感器數(shù)據(jù)進(jìn)行融合修正旋轉(zhuǎn)矩陣，從而得到姿態(tài)的近似最優(yōu)解。

(13)

構(gòu)造目標(biāo)函數(shù)為

F(Q)=(egx)2+(egy)2+(egz)2+

(emx)2+(emy)2+(emz)2

將四元數(shù)旋轉(zhuǎn)矩陣的優(yōu)化轉(zhuǎn)化為求F(Q)的最小值。

由梯度下降法得到其迭代公式為[10]

(14)

式中：m為迭代步長(zhǎng)。

將其與由四元數(shù)微分方程得到的四元數(shù)Qω(t)相結(jié)合，得到最終的梯度下降法的姿態(tài)融合公式：

(15)

式中：n為迭代步長(zhǎng)；Δt為采樣時(shí)間；Q(t)為所求姿態(tài)四元數(shù)。

通過(guò)加速度計(jì)對(duì)地磁傳感器進(jìn)行傾斜補(bǔ)償后求得的歐拉角，帶入式(16)轉(zhuǎn)化為初始四元數(shù)，以解決初始對(duì)準(zhǔn)時(shí)梯度下降法收斂過(guò)慢的問(wèn)題。當(dāng)四元數(shù)初始化后，便可利用上述方法解算姿態(tài)的近似值。

(16)

5 實(shí)驗(yàn)與分析

5.1 靜態(tài)實(shí)驗(yàn)

利用角度分度裝置將九軸傳感器靜止放置水平，同時(shí)采集通過(guò)加速度計(jì)、陀螺儀、梯度下降法解算出的俯仰角，實(shí)驗(yàn)結(jié)果如圖5所示。通過(guò)對(duì)比可以看出：加速度計(jì)存在明顯的噪聲，陀螺儀存在較明顯的偏移，隨著時(shí)間的增加，累積誤差逐漸增大。梯度下降法能有效的抑制加速度計(jì)的噪聲，俯仰角測(cè)量誤差大約在±0.3°以內(nèi)。

圖5 俯仰角靜止實(shí)驗(yàn)

5.2 動(dòng)態(tài)實(shí)驗(yàn)

將傳感器初始水平，此后繞x軸旋轉(zhuǎn)30°得到的圖像如圖6所示。由圖像看出，梯度下降法能及時(shí)跟蹤姿態(tài)的變化，同時(shí)一定程度上抑制了加速度計(jì)的噪聲和陀螺儀的漂移，所測(cè)的俯仰角誤差在±1°以內(nèi)。

圖6 俯仰角動(dòng)態(tài)實(shí)驗(yàn)

5.3 四元數(shù)初始化對(duì)比試驗(yàn)

初始時(shí)將傳感器x軸指向東方，然后旋轉(zhuǎn)90°使其指向北方，分別將前文所提出的對(duì)四元數(shù)初始化的方法與一般采用的將初始四元數(shù)設(shè)為(1，0，0，0)所測(cè)的航向角進(jìn)行比較，實(shí)驗(yàn)效果如圖7、8所示。由實(shí)驗(yàn)結(jié)果看出，進(jìn)行初始對(duì)準(zhǔn)后，梯度下降法初始收斂時(shí)間由原來(lái)的5 s降到了1 s以內(nèi)，改善了其姿態(tài)角初始時(shí)刻不穩(wěn)的問(wèn)題，采用該系統(tǒng)測(cè)的航向角誤差在±2°以內(nèi)。

圖7 未初始對(duì)準(zhǔn)航向角實(shí)驗(yàn)結(jié)果

圖8 初始對(duì)準(zhǔn)航向角實(shí)驗(yàn)結(jié)果

6 結(jié)論

基于MSP430F5438與九軸傳感器GY- 85搭建姿態(tài)測(cè)量系統(tǒng)，實(shí)現(xiàn)了姿態(tài)角的檢測(cè)。針對(duì)地磁傳感器的所測(cè)數(shù)據(jù)受周?chē)艌?chǎng)干擾比較嚴(yán)重的問(wèn)題，使用AntMag軟件對(duì)其進(jìn)行了校正，實(shí)驗(yàn)結(jié)果表明該軟件簡(jiǎn)單可行，正確有效。另外為解決加速度計(jì)測(cè)量噪聲、陀螺儀漂移誤差及加速度計(jì)無(wú)法對(duì)航向角進(jìn)行修正等問(wèn)題，引入梯度下降法，在角度分度裝置上進(jìn)行了動(dòng)靜態(tài)實(shí)驗(yàn)。結(jié)果表明，該系統(tǒng)克服了航向角初始難以對(duì)準(zhǔn)的問(wèn)題，具有響應(yīng)快、實(shí)時(shí)性好的特點(diǎn)，并可應(yīng)用于平衡小車(chē)、無(wú)人機(jī)姿態(tài)檢測(cè)、慣性導(dǎo)航等領(lǐng)域。

[1] 夏圣，許勇.基于MEMS組合模塊的姿態(tài)檢測(cè)系統(tǒng)設(shè)計(jì)[J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用,2011(4):52- 55.

[2] MARINS J L,YUN X,BACHAMARM E,et al. An extended Kalman filter for quaternion based orientation rst- imation using MARG sensors[C]∥Proceedings of the 2001 IEEE/RSJ I- nternational Conference on Intel- ligent Robots and systems.Maui,Hawaii:IEEE,2001(8):2003- 2011.

[3] 宋東,章侃,王彥文,等.基于MEMS傳感器的飛行姿態(tài)指示系統(tǒng)[J].測(cè)控技術(shù),2009,28(3):1- 4.

[4] MAHONY R,HAMEL T,PFILMLIN J M,et al.Nonlinear complementary filte- rs on the special orthogonal gro- up[J].IEEE trans.on autom atic control,2008,53(5):1203- 1217.

[5] 陳新雋，劉曉平，戴若犁，等.多MEMS傳感器的嵌入式姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)[A].單片機(jī)與嵌入式系統(tǒng)：應(yīng)用天地,2011(10):58- 61.

[6] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,2014.

[7] KIMBERLY T.Tilt sensing using linear Accelerometers[J].Freescale Semiconductor Application Note,2007(6):1- 7.

[8] 李偉，何鵬舉，高社生.多傳感器加權(quán)信息融合算法研究[J].西北工業(yè)大學(xué)學(xué)報(bào)，2010,28(5):674 - 678.

[9] 畢盛.閔華清.李淳，等.姿態(tài)傳感器采集測(cè)試系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)測(cè)量與控制，2011,19(7):1562- 1564.

[10] MADGWICK S.An efficient orientation filter for intertial and inertial/magnetic sensor arrays [R].Report xio and University of Bristol(UK),2010.