王希彬, 戴洪德, 全聞捷, 王 瑞, 賈臨生

(1. 海軍航空大學(xué)航空基礎(chǔ)學(xué)院, 山東 煙臺 264001;2. 中國人民解放軍92329部隊, 遼寧 葫蘆島 125000)

0 引 言

姿態(tài)估計是許多導(dǎo)航系統(tǒng)的重要任務(wù)之一,在衛(wèi)星控制、無人機控制、工業(yè)機械臂、智能機器人、行人導(dǎo)航中擁有廣泛的應(yīng)用[1]。靜止情況下的解決方案是利用體坐標系中互不平行的量測向量與對應(yīng)的已知慣性向量(恒星的方位、地球重力、地球磁場等)進行對比,從而完成姿態(tài)估計,如四元數(shù)估計器(quaternion estimator,QUEST)[2]利用最小二乘思想進行姿態(tài)解算,得到了廣泛關(guān)注,并且衍生出許多相關(guān)算法[3-4]。

在運動情況下,基于慣性測量單元的姿態(tài)估計算法中,可以利用陀螺儀的角速度測量值計算姿態(tài)變化量,但陀螺儀的漂移會使得估計誤差逐漸累積,因此需要使用加速度計和磁力計等多種傳感器的數(shù)據(jù)進行補償。姿態(tài)解算中需要面對非線性狀態(tài)估計問題,在非線性狀態(tài)估計領(lǐng)域中,最常用的算法是擴展卡爾曼濾波(extended Kalman filter, EKF)[3,5-6]算法。根據(jù)噪聲特性的不同,EKF分為加性EKF(additive EKF, AEKF)和乘性EKF(multiplicative EKF, MEKF)[7-8]兩種。當狀態(tài)向量為四元數(shù)時,AEKF采用加法對四元數(shù)進行修正,可能使四元數(shù)的模值不再為1,進失去四元數(shù)的規(guī)范性,因此需要對估計結(jié)果進行歸一化,而MEKF采用了四元數(shù)乘積進行修正,這兩種算法在本質(zhì)上是等價的[3, 8-9]。由于EKF的線性化處理過程會降低算法精度和穩(wěn)定性,為了解決這一問題, Julier等[10]開發(fā)出了無跡卡爾曼濾波(unscented Kalman filter, UKF), Arasaratnam等[11]開發(fā)出了容積卡爾曼濾波(cubature Kalman filter, CKF),它們都基于“對分布進行近似要易于對任意非線性函數(shù)進行近似[12]”的思想,采用Sigma點對高斯分布進行近似,降低了線性化過程中的精度損失,使得UKF和CKF的性能要優(yōu)于EKF[13-15]。UKF在進行無跡變換時可能出現(xiàn)中心權(quán)值為負的情況,且當狀態(tài)向量高于三維時,精度會出現(xiàn)下降。雖然都采用了Sigma點對高斯分布進行近似,但CKF的核心是球面-徑向容積法則,在精度和收斂性方面都要優(yōu)于UKF,且CKF采用的Sigma點比UKF少一個,具有相同的權(quán)值,避免了出現(xiàn)權(quán)值為負的狀況[16]。為了提升CKF的性能,國內(nèi)外學(xué)者對CKF提出了一些改進算法,如平方根CKF[8]、迭代自適應(yīng)CKF[17]、強跟蹤CKF[15]算法,但算法的計算量也會大幅提升。CKF算法在航空器和衛(wèi)星等飛行器中的應(yīng)用較為廣泛[16, 18-19],但在行人導(dǎo)航姿態(tài)解算中的應(yīng)用還未見到。

慣性行人導(dǎo)航算法主要包括零速檢測、零速修正和導(dǎo)航解算,對零速檢測[20-27]和零速修正[28-34]算法的研究較多[20-34],而對導(dǎo)航解算方面的研究較少。由于在非零速區(qū)間,足部的運動加速度較大,多采用純陀螺儀解算的方式,即僅依靠角速度測量數(shù)據(jù)解算姿態(tài),沒有充分挖掘和利用加速度計和磁力計測量數(shù)據(jù)中的姿態(tài)信息。Makni等[35]對運動加速度進行建模,假設(shè)運動加速度為一常值與白噪聲的疊加,進而設(shè)計了四元數(shù)描述符濾波器,在行人導(dǎo)航中進行了實驗,其解算精度要優(yōu)于EKF算法,但該算法存在對運動加速度的模型假設(shè)過于簡單的問題。

本文為了減小運動加速度對姿態(tài)解算的影響,首先對運動加速度進行預(yù)測補償,預(yù)測下一時刻的運動加速度并補償加速度計的測量結(jié)果,得到更準確的重力加速度測量值,既可以提高行人導(dǎo)航解算精度,也能避免由于模型過于簡單而帶來的誤差。接著,設(shè)計CKF估計行人導(dǎo)航姿態(tài)。最后,通過計算機仿真模擬試驗和行人導(dǎo)航試驗對本文所提算法性能進行測試,并與四元數(shù)描述符濾波器、EKF進行分析比較。仿真結(jié)果表明,本文所提算法在位置精度和航向精度上均有顯著的提高。

1 行人導(dǎo)航系統(tǒng)

1.1 系統(tǒng)簡介

行人導(dǎo)航算法結(jié)構(gòu)圖如圖1所示。

在圖1所示的導(dǎo)航算法結(jié)構(gòu)上,本文進行了如下改進:根據(jù)加速度計的測量結(jié)果判斷加速度擬合區(qū)間,在擬合區(qū)間內(nèi)使用擬合結(jié)果對加速度計量測值進行補償,補償后采用CKF算法估計姿態(tài),進而進行速度和位置的解算。算法結(jié)構(gòu)如圖2所示。導(dǎo)航解算包含姿態(tài)解算、速度解算和位置解算。由于姿態(tài)解算的誤差,導(dǎo)致加速度的解算出現(xiàn)誤差,通過積分進而導(dǎo)致速度和位置出現(xiàn)累積誤差。為了減小累積誤差,需要通過零速檢測和零速修正算法對誤差進行修正。當前的研究多集中于零速檢測和零速修正[20-34],對于行人導(dǎo)航算法中的姿態(tài)解算方法的研究較少,如果能夠提高姿態(tài)解算的精度,進而提升速度和位置的解算精度,將會提高算法的整體精度?？紤]到CKF算法在非線性系統(tǒng)中有較高的估計精度和適應(yīng)性,本文設(shè)計了基于CKF的姿態(tài)估計算法。

1.2 傳感器測量模型

傳感器測量模型如下[36-37]:

yg=ω+bg+δg

(1)

ya=C(q)(G+f)+δa

(2)

ym=C(q)m+δm

(3)

式中:yg,bg,δg分別為陀螺儀的輸出、常值漂移和隨機漂移;ω為真實角速度;ya為加速度計輸出;C(q)為由四元數(shù)q構(gòu)成的方向余弦矩陣;G和f分別為當?shù)刂亓铀俣群屯獠考铀俣仍贜系中的坐標值;δa為加速度計隨機零偏;ym為磁力計的輸出;m為地球磁場在N系中的坐標值,δm為磁傳感器隨機噪聲[38]。

根據(jù)文獻[1],由于陀螺儀的漂移變化比較緩慢,可以采用靜止狀態(tài)下陀螺儀的量測均值對漂移進行估計,而后對陀螺儀的測量值進行補償[38],此時陀螺儀的量測結(jié)果可以近似表達為

yg=ω+δg

(4)

圖3 采用的傳感器Fig.3 The used sensors

2 運動加速度補償

2.1 運動加速度擬合模型和誤差

(5)

(6)

(7)

圖4 三軸運動加速度擬合誤差和加速度計測量模值Fig.4 Fitting error of acceleration in three-axis motion and measurement modulus of accelerometer

從圖4中的紅圈可以發(fā)現(xiàn),在非零速區(qū)間,有一段加速度的擬合誤差與零速區(qū)間相近。在這段運動過程中,可以利用擬合結(jié)果對加速度計進行補償,并將這一區(qū)間稱為擬合區(qū)間。對每一個步態(tài)周期中擬合區(qū)間的擬合誤差進行分析,其誤差分布圖如圖5所示。

圖5 運動加速度擬合誤差Fig.5 Motion acceleration fitting error

從5圖可見,誤差分布近似為正態(tài)分布。誤差均值和方差如表1所示。這樣就近似得到了δp,k+1的統(tǒng)計特征。

表1 運動加速度擬合誤差均值和方差Table 1 Motion acceleration fitting error mean value and variance

將式(6)代入式(2)可得

(8)

(9)

(10)

2.2 擬合區(qū)間判斷

在行人導(dǎo)航算法中,要應(yīng)用該算法,還需要準確判斷出擬合區(qū)間。由圖4可見,該區(qū)間位于加速度模值的兩個峰值之間,因此只需找出步態(tài)周期內(nèi)的兩個峰值即可。對步態(tài)持續(xù)時間進行統(tǒng)計,圖6展示了步態(tài)周期長度和一個步態(tài)周期之內(nèi)加速度模值的分布。圖6(a)表明步態(tài)周期長度分布在1.3 s至1.6 s之間,圖6(b)表明可以進行擬合的區(qū)間位于步態(tài)周期的0.85 s(即第170個采樣點)附近。在檢測到走完一步后,檢測前170個采樣點和170個采樣點以后的峰值,為消除峰值附近的劇烈變化帶來的誤差,取出區(qū)間中間3/4的部分,如圖7所示。

圖6 步態(tài)周期長度分布直方圖與加速度模值分布Fig.6 Histogram of the gait cycle length and acceleration modulus distribution

圖7 擬合區(qū)間Fig.7 Fitting interval

由于在擬合區(qū)間對加速度進行補償后可以將擬合區(qū)間近似看作零速區(qū)間,因此可以采用CKF對陀螺儀、加速度計和磁力計的數(shù)據(jù)進行融合,對姿態(tài)進行估計。在項目組的前期研究中,非零速區(qū)間的姿態(tài)解算算法為三子樣旋轉(zhuǎn)矢量法,因此在非擬合區(qū)間,仍采用三子樣旋轉(zhuǎn)矢量法,僅使用角速度信息進行姿態(tài)更新。此時,含加速度補償?shù)腃KF的導(dǎo)航解算過程如圖8所示。

圖8 基于運動加速度補償和CKF的行人導(dǎo)航算法結(jié)構(gòu)圖Fig.8 Structure diagram of pedestrian navigation algorithm based on motion acceleration compensation and CKF

3 姿態(tài)估計的CKF器設(shè)計

3.1 參數(shù)選擇

在三維空間中,剛體的姿態(tài)可以用載體坐標系B(XB,YB,ZB)和地理坐標系N(XN,YN,ZN)的相對位置進行描述[39]。用于描述姿態(tài)的常用參數(shù)有方向余弦矩陣、歐拉角和四元數(shù)等。由于四元數(shù)的參數(shù)冗余小,且不存在奇異值,因此應(yīng)用非常廣泛。單位四元數(shù)的形式如下:

q=[q0,qT]T=[q0,q1,q2,q3]T∈R4

(11)

其共軛四元數(shù)為

q*=[q0, -qT]T=[q0, -q1, -q2, -q3]T∈R4

(12)

而用于描述姿態(tài)的四元數(shù)需要滿足歸一性:

(13)

考慮到構(gòu)造容積點時需采用加法,這使得四元數(shù)不能滿足式(13),歸一化后不能再保證容積點的對稱性,影響了后續(xù)計算,因此本文采用參數(shù)冗余小且無歸一化要求的歐拉角作為姿態(tài)參數(shù)。采用ZYX旋轉(zhuǎn)的歐拉角時,其定義如下:

(14)

歐拉角的微分方程為

(15)

式中:

(16)

(17)

ω=[ωx,ωy,ωz]T

(18)

ω為剛體在B系中的三軸旋轉(zhuǎn)角速度。在實際應(yīng)用中,可以采用一階畢卡算法進行近似計算:

(19)

由式(17)可知,在解算過程中應(yīng)避免θ接近90°,在行人導(dǎo)航中,一般不可能出現(xiàn)θ接近90°的情況。

3.2 狀態(tài)方程

選取歐拉角Φ和陀螺儀漂移b作為狀態(tài)量,即x=[ΦT,bT]T,根據(jù)式(19)和式(4)可以得到如下狀態(tài)方程:

xk=f(xk-1)+wk

(20)

式中:

(21)

(22)

wΦ,k=ΔtJδg,k-1

(23)

3.3 量測方程

(24)

(25)

vm,k=δm,k

(26)

3.4 CKF

(1) 濾波初始化

(27)

(28)

(2) 時間更新

(29)

(30)

2) 容積點更新

(31)

3) 一步狀態(tài)預(yù)測和協(xié)方差預(yù)測

(32)

(33)

(3) 量測更新方程

1) 容積點和協(xié)方差矩陣的計算

(34)

(35)

(36)

c.計算預(yù)測量測值、量測協(xié)方差矩陣和交叉協(xié)方差矩陣為

(37)

(38)

(39)

2) 更新增益矩陣、狀態(tài)量和誤差協(xié)方差矩陣

(40)

(41)

(42)

CKF原理圖如圖9所示。

圖9 CKF原理圖Fig.9 Schematic diagram of CKF’s principle

4 試驗驗證

4.1 仿真試驗

傳感器的數(shù)據(jù)采樣周期設(shè)定為0.01 s,由于CKF參與解算的時間周期在0.5 s左右,因此僅作2 s的模擬,剛體的三維運動角速度如表2所示。

表2 三維運動角速度Table 2 Three dimensional angular velocity

加速度計和磁力計噪聲標準差[38]分別為σacc=0.01 m/s2和σmag=0.01 Gauss,陀螺儀噪聲的標準差為σgyro=0.05 rad/s。為了模擬行人導(dǎo)航中的足部運動情況,以檢測算法的性能,加入的運動加速度如表3所示。

表3 不同時間區(qū)間內(nèi)加入的運動加速度Table 3 Motion acceleration added in different time intervals

為了對比算法的估計精度和計算復(fù)雜度,在仿真中對比了未加入CKF和加入CKF后的姿態(tài)解算誤差,對比結(jié)果如圖10所示。從圖10可以發(fā)現(xiàn),在加速度擬合區(qū)間,加速度補償?shù)腃KF算法的精度較高,誤差有明顯減小。假定導(dǎo)航解算時間為T,為了更精確地比較算法的精度,采用均方根誤差,其表達式如下:

圖10 兩種算法的歐拉角估計誤差Fig.10 Euler angles estimation errors of two algorithms

(43)

式中:δxθ(t)是t時刻的解算誤差,θ∈{pitch,theta,yaw}[38]。

由表4姿態(tài)解算的均方根誤差數(shù)據(jù)可知,加入CKF的解算后,俯仰角誤差降低了38.8%,滾轉(zhuǎn)角誤差降低了39.8%,航向角誤差降低了29.0%,平均精度提高了35.3%。

表4 姿態(tài)解算的RMSETable 4 RMSE of attitude calculation

4.2 試驗驗證

試驗路線為長18 m、寬10 m的矩形[39],導(dǎo)航解算采用了項目組成員先前研究的基于偽標準差和N-P準則的零速檢測算法[40]和卡爾曼濾波零速修正算法[41],最終位置解算結(jié)果如圖11所示。

圖11 水平位置和高度解算結(jié)果Fig.11 Horizontal position and height position calculation results

路徑的起終點水平方向和高度方向誤差如表5所示。

表5 解算路徑的起終點水平與高度誤差Table 5 Horizontal and height errors of starting and ending point of the calculation path

表5表明,加入CKF后起終點水平誤差減小了56.3%,起終點高度誤差減小了20.3%。這表明加入加速度補償后的CKF算法能夠顯著提高行人導(dǎo)航的精度。

5 結(jié) 論

本文設(shè)計了運動加速度擬合模型和擬合區(qū)間判別方法,提出了基于運動加速度補償?shù)腃KF算法,用于非零速區(qū)間部分時段的姿態(tài)解算,并與純?nèi)訕有D(zhuǎn)矢量法進行了精度對比。理論分析和試驗驗證表明,經(jīng)過加速度補償?shù)腃KF算法在應(yīng)用于行人導(dǎo)航的姿態(tài)估計時,相比于純?nèi)訕有D(zhuǎn)矢量法具有更高的解算精度,具有一定的理論和實際價值。在后續(xù)研究中,還將針對多種步態(tài)和復(fù)雜地形的情況,對算法進行改進,以增強算法的適應(yīng)能力。