基于加速度補償的慣性行人導航非零速區間姿態估計CKF算法

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

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

0 引 言

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

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

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

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

1 行人導航系統

1.1 系統簡介

行人導航算法結構圖如圖1所示。

在圖1所示的導航算法結構上,本文進行了如下改進:根據加速度計的測量結果判斷加速度擬合區間,在擬合區間內使用擬合結果對加速度計量測值進行補償,補償后采用CKF算法估計姿態,進而進行速度和位置的解算。算法結構如圖2所示。導航解算包含姿態解算、速度解算和位置解算。由于姿態解算的誤差,導致加速度的解算出現誤差,通過積分進而導致速度和位置出現累積誤差。為了減小累積誤差,需要通過零速檢測和零速修正算法對誤差進行修正。當前的研究多集中于零速檢測和零速修正[20-34],對于行人導航算法中的姿態解算方法的研究較少,如果能夠提高姿態解算的精度,進而提升速度和位置的解算精度,將會提高算法的整體精度。考慮到CKF算法在非線性系統中有較高的估計精度和適應性,本文設計了基于CKF的姿態估計算法。

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)為由四元數q構成的方向余弦矩陣;G和f分別為當地重力加速度和外部加速度在N系中的坐標值;δa為加速度計隨機零偏;ym為磁力計的輸出;m為地球磁場在N系中的坐標值,δm為磁傳感器隨機噪聲[38]。

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

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中的紅圈可以發現,在非零速區間,有一段加速度的擬合誤差與零速區間相近。在這段運動過程中,可以利用擬合結果對加速度計進行補償,并將這一區間稱為擬合區間。對每一個步態周期中擬合區間的擬合誤差進行分析,其誤差分布圖如圖5所示。

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

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

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

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

(8)

(9)

(10)

2.2 擬合區間判斷

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

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

圖7 擬合區間Fig.7 Fitting interval

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

圖8 基于運動加速度補償和CKF的行人導航算法結構圖Fig.8 Structure diagram of pedestrian navigation algorithm based on motion acceleration compensation and CKF

3 姿態估計的CKF器設計

3.1 參數選擇

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

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

(11)

其共軛四元數為

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

(12)

而用于描述姿態的四元數需要滿足歸一性:

(13)

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

(14)

歐拉角的微分方程為

(15)

式中:

(16)

(17)

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

(18)

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

(19)

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

3.2 狀態方程

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

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) 一步狀態預測和協方差預測

(32)

(33)

(3) 量測更新方程

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

(34)

(35)

(36)

c.計算預測量測值、量測協方差矩陣和交叉協方差矩陣為

(37)

(38)

(39)

2) 更新增益矩陣、狀態量和誤差協方差矩陣

(40)

(41)

(42)

CKF原理圖如圖9所示。

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

4 試驗驗證

4.1 仿真試驗

傳感器的數據采樣周期設定為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。為了模擬行人導航中的足部運動情況,以檢測算法的性能,加入的運動加速度如表3所示。

表3 不同時間區間內加入的運動加速度Table 3 Motion acceleration added in different time intervals

為了對比算法的估計精度和計算復雜度,在仿真中對比了未加入CKF和加入CKF后的姿態解算誤差,對比結果如圖10所示。從圖10可以發現,在加速度擬合區間,加速度補償的CKF算法的精度較高,誤差有明顯減小。假定導航解算時間為T,為了更精確地比較算法的精度,采用均方根誤差,其表達式如下:

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

(43)

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

由表4姿態解算的均方根誤差數據可知,加入CKF的解算后,俯仰角誤差降低了38.8%,滾轉角誤差降低了39.8%,航向角誤差降低了29.0%,平均精度提高了35.3%。

表4 姿態解算的RMSETable 4 RMSE of attitude calculation

4.2 試驗驗證

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

圖11 水平位置和高度解算結果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算法能夠顯著提高行人導航的精度。

5 結 論

本文設計了運動加速度擬合模型和擬合區間判別方法,提出了基于運動加速度補償的CKF算法,用于非零速區間部分時段的姿態解算,并與純三子樣旋轉矢量法進行了精度對比。理論分析和試驗驗證表明,經過加速度補償的CKF算法在應用于行人導航的姿態估計時,相比于純三子樣旋轉矢量法具有更高的解算精度,具有一定的理論和實際價值。在后續研究中,還將針對多種步態和復雜地形的情況,對算法進行改進,以增強算法的適應能力。