基于改進卡爾曼濾波算法的行人航向角估計

吳藝旋,張愛軍,楊 萌

(南京理工大學機械工程學院,南京 210094)

0 引言

行人航位推算(pedestrian dead reckoning,PDR)是一種基于慣性導航的用于未知環境定位的行人定位方法,具有使用便攜、成本低、不依賴基礎設施等諸多優勢,常用于消防救援、單兵作戰等場景[1-2]。PDR算法包括步數檢測、步長估計和航向估計3個步驟,通過上述步驟能夠獲取行人的實時定位。其中航向估計是PDR算法的最后一步,它的精度會直接影響最終定位結果的準確性,而單一依靠陀螺儀解算的航向角,由于存在漂移誤差并不能維持長時間的高精度,通常采用非線性的卡爾曼濾波融合算法,通過磁力計和加速計來校準陀螺儀估計的航向角,從而提高行人航向角的精度[3-4]。

擴展卡爾曼濾波(extended Kalman filter,EKF)是一種最為常見的非線性濾波算法,然而在EKF的線性化轉換過程中會產生模型誤差累積[5-6]。隨后提出的無跡卡爾曼濾波(unscented Kalman filter,UKF)[7]和容積卡爾曼濾波(cubature Kalman filter,CKF)[8]能夠彌補EKF的缺陷,其中當濾波模型的維度超過3時,UKF會損失掉部分Sigma點對后驗估計的統計特性,因此CKF的精度優于UKF。與此同時,CKF在遞推過程中存在計算量大、數值不穩定、正定性丟失等問題[9],因此文獻[10]提出了平方根容積卡爾曼濾波器(square root cubature Kalman filter,SRCKF),平方根濾波思想的引入確保了CKF過程中協方差矩陣的對稱性和半正定性。

然而,在實際的行走過程中,傳感器可能在行人行走狀態突變等突發情況下采集到異常的數據,這些極端噪聲會造成濾波精度下降甚至出現濾波發散的問題,H∞濾波器[11]能夠最小化異常值對估計輸出的影響,從而確保出現異常值時具有良好的魯棒性。為了使H∞濾波器適應于非線性系統,文獻[12]提出了H∞平方根容積卡爾曼濾波器(square-root cubatureH∞filter,SR-CHF)。

雖然SR-CHF提高了系統應對極端噪聲的魯棒性,但是對于外部噪聲未知、動力學模型的不準確性等情況,其精度仍有待提高。為了解決這一問題,多種自適應濾波的方法被提出。Sage-Husa自適應濾波[13]可以實時校正過程噪聲和測量噪聲的統計特性,但很難給出準確的噪聲特性,計算量大。文獻[14]采用模型預測濾波器的概念建立動態模型誤差估計器,將模型誤差估計補償到濾波器中。文獻[15]使用Huber方法補償過程不確定性。上述自適應方法都不能同時估計過程和測量噪聲特性,而周東華等[16-17]提出的基于新息正交的漸消因子和多漸消因子法能夠在不估計過程和測量誤差的情況下抑制模型不確定性的影響,通過在預測協方差陣中引入漸消因子,保持對實際系統的強跟蹤。

因此,MSTHSCKF算法通過將H∞濾波器、多漸消因子和SRCKF進行融合,能夠提高濾波結果的穩定性、魯棒性及其自適應能力。最后,通過對采集的實際行走數據進行驗證,實驗結果證明, MSTHSCKF在行人航向角估計中具有更好的魯棒性、準確性和優越性,經由該算法濾波后的航向角更加接近真實值。

1 SRCKF

在行人航向解算的非線性系統中,其狀態方程和測量方程表示如式(1)

(1)

其中,xk∈Rn,zk∈Rn;f(·)和h(·)分別為非線性狀態轉移函數和量測函數;wk和rk分別為獨立同分布的狀態和量測噪聲,其協方差分別為Qk和Rk。

以下方程為平方根卡爾曼濾波的算法流程:

1)獲取容積點

(2)

2)傳播容積點

(3)

3)計算預測狀態值

(4)

4)計算估計預測誤差協方差矩陣的平方根

(5)

5)傳播容積點,計算預測測量值

(6)

6)計算新息協方差矩陣的平方根

(7)

7)計算互協方差矩陣

(8)

(9)

8)計算增益

(10)

9)計算狀態估計值

(11)

10)計算誤差協方差的平方根系數

(12)

2 基于H∞和多漸消因子的SRCKF

2.1 H∞濾波

H∞濾波是卡爾曼濾波的特殊形式,它采用了博弈論的思想,能夠在噪聲最大時最小化輸出狀態誤差。定義非線性濾波的代價函數如下[18]

(13)

(14)

從式(14)可以看出,H∞濾波需要線性化測量模型,文獻[12]分別將H∞濾波融入了CKF和SRCKF,提出了不需要雅可比矩陣計算的H∞平方根容積卡爾曼濾波。SR-CHF在估計預測狀態、預測誤差協方差矩陣的平方根、預測測量值、新息協方差矩陣的平方根因子等步驟均與SRCKF一致。其求取SR-CHF誤差協方差的平方根因子的過程如下[12]

(15)

其中,ΘJ表示J-酉矩陣;SR,k表示測量噪聲協方差的平方根。

2.2 多漸消因子

針對系統模型的不確定性和狀態突變等問題,為了提高濾波算法的濾波精度和跟蹤性能,文獻[16-17]提出了漸消因子自適應濾波器。漸消因子自適應濾波器的主要思想是修改預測誤差協方差,使其滿足

(16)

(17)

其中,ηk表示k時刻的殘差,式(16)表示自適應濾波器的性能標準;式(17)表示新息序列不相關。通過在線調整預測協方差和增益矩陣,強制新息正交,以確保濾波器跟蹤能力。使用多漸消因子修改預測協方差相對于單漸消因子來說,能夠對多維狀態進行修正,更好地改善整個系統平均性能。

Pk|k-1=λk(Pk|k-1-Qk)+Qk

(18)

其中,λk表示多漸消因子,它的計算過程如下所示。

根據先驗經驗,采用近似確定比例系數,即

λk(1)∶λk(2)∶…∶λk(n)=α1∶α2∶…∶αn

(19)

λk=diag[λk(1)λk(2)…λk(n)]

(20)

λ′k(i)=αiCk

(21)

(22)

(23)

(24)

(25)

(26)

其中,0<ρ≤1為遺忘因子;αi為弱化因子,它可以使得狀態估計值更加平滑。由于Hk和Fk需要經過雅可比矩陣運算才能得到,為了使多漸消因子適用于SRCKF,可對Hk和Fk進行如下變換[20]

(27)

(28)

聯立式(20)～式(28)可以求得多漸消因子λk,使用λk修正式(5)中新息協方差矩陣平方根,完成狀態的預測。

(29)

2.3 MSTHSCKF

根據上述推導,建立了MSTHSCKF算法。基于式(1)的非線性系統,MSTHSCKF算法的詳細計算過程(見圖1)如下:

圖1 MSTHSCKF流程圖Fig.1 Flow chart of MSTHSCKF

1)初始化狀態。

3)計算預測過渡誤差協方差矩陣Pk|k-1。

(30)

5)計算預測自協方差。

(31)

6)根據式(20)～式(28)計算多漸消因子λk。

7)根據式(29)更新預測誤差協方差矩陣平方根Sk|k-1。

10)計算增益Kk。

Kk=Pxz,k|k-1+(Re)-T/2(Re)-1/2

(32)

其中,Re=Pzz,k|k-1+Rk。

12)計算誤差協方差矩陣的平方根因子Sk|k。

(33)

3 實驗與結果分析

3.1 實驗條件與數據采集

為了驗證所提出的融合算法的性能,進行了室內行走實驗,實驗采用荷蘭Xsens公司的MTi-30進行數據采集。MTi-30是一款高精度、高穩定性、具有動態快速響應等優勢的姿態-航向參考系統,其內部集成了加速度計、磁力計和陀螺儀,可以輸出每個傳感器的原始數據,MTi-30內置各傳感器性能指標如表1所示。

表1 MTi-30內置傳感器性能指標Tab.1 Performances of MTi-30 built-in sensors

實驗時將模塊固定在行人腳背(見圖2),連線另一端連接電腦,通過Xsens的上位機軟件MT Manager實時采集傳感器數據,采樣頻率為20 Hz,實驗地點為一直形走廊,全長約60 m,寬約2 m,如圖3所示,行人全程沿直線行走。

圖2 傳感器穿戴圖Fig.2 Picture of sensor wearing

圖3 實驗環境Fig.3 Experimental site

3.2 實驗結果分析

將采集的數據分別通過SRCKF、MSTSCKF、SR-CHF和MSTHSCKF進行傳感器數據融合,獲取行人運動時的航向角。將四元數以及陀螺儀漂移誤差作為狀態向量,將磁傳感器的輸出和加速度計的輸出分別校準后作為觀測向量[21]。在預測階段,四元數通過陀螺儀的角速率積分進行預測;在更新階段,使用當前四元數估計加速度計和磁力計的數值,并將其與加速度計和磁力計測試出來的原始數據進行對比,修正四元數的值。仿真過程的流程框圖如圖4所示。

圖4 基于四元數的實驗仿真流程圖Fig.4 Experimental simulation flow chart based on quaternion

其中,初始四元數由靜止狀態下的加速度計和磁力計數值計算得到。

過程噪聲協方差Q=diag[10-610-610-610-610-810-810-8],測量噪聲協方差R=diag[10-110-110-110-810-810-8]。

為了直觀地了解該算法的優越性,比較了經過不同濾波器濾波后的航向角與真實值的誤差,其中圖5、圖6、圖7分別代表經由不同濾波之后的航向角、航向角誤差和航向角絕對值誤差對比圖。從圖5可以看出,經由MSTHSCKF估計后的行人航向角圍繞著真實值上下平緩地波動,最為接近真實值。從圖6、圖7可以看出,MSTHSCKF在大多數情況下的誤差都保持在2°以內,SR-CHF和MSTSCKF大部分在3°以內,而SRCKF的誤差突破了6°。表2給出了各濾波器求得的行人航向角的誤差對比結果,通過表2能夠更直觀地看出,MSTHSCKF的誤差絕對值的均值相較于SRCKF、SR-CHF和MSTSCKF分別下降了56.72%、21.54%和22.91%。MSTHSCKF的均方差相較于SCKF、SR-CHF和MSTHCSKF分別下降了30.62%、23.36%和22.52%。因此,可以得出,所提出的MSTHSCKF能夠提供更為準確的行人航向估計。

圖5 航向角解算結果Fig.5 Calculation result of heading angle

圖6 航向角誤差Fig.6 Error of heading angle

圖7 航向角誤差的絕對值Fig.7 Absolute value of error of heading angle

表2 行人航向角的濾波結果對比Tab.2 Comparison of filtering results of pedestrian heading angle

為了進一步驗證MSTHSCKF的優越性,將經由不同濾波器濾波后得到的行人航向角應用于PDR算法中。在繪圖時以行人起點為原點,行人行走方向為Y方向,與行走方向垂直的方向為X方向,以此構建行人在圖3走廊上的二維行走軌跡,如圖8所示,其中黑色軌跡表示參考路徑。表3表示經由各濾波器獲得的航向角所估計的行人軌跡在X方向上的位置誤差,包括誤差絕對值的最大值、最小值和均值。其中MSTHSCKF的最接近參考值,其平均誤差在0.072 7 m,相較于SRCKF、SR-CHF和MSTSCKF分別下降了44.63%、24.97%和25.28%。同時MSTHSCKF對應的誤差絕對值最大值和最小值也都比其他濾波器要小,這說明行人航向角估計誤差確實很大程度地影響行人定位的精度,進一步驗證了所提出的MSTHSCKF的抗差能力好,穩定性和可靠性更高。

圖8 行人行走軌跡Fig.8 Pedestrian walking trajectories

表3 X方向的位移誤差絕對值Tab.3 Absolute value of position error in Xaxis

4 結論

本文提出了一種基于H∞濾波和多漸消因子的平方根容積卡爾曼濾波算法MSTHSCKF,用于提高PDR算法的航向角估計精度,解決因傳感器誤差累積、噪聲特性不確定、模型不確定等造成的行人航向角精度降低的問題,實驗結果證明:

1)采用MSTHSCKF融合加速度計、陀螺儀和磁力計數據獲得的航向角,解決了陀螺儀解算誤差積累的問題。

2)MSTHSCKF融合濾波求得的航向角相較于SRCKF、SR-CHF和MSTSCKF,誤差絕對值的均值分別下降了56.72%、21.54%和22.91%。均方根誤差分別下降了30.62%、23.36%和22.52%,證明該算法能夠提供精確穩定的航向角估計。

3)最后的行人定位實驗進一步驗證了該算法獲得的行人航向角為后續的行人定位和導航提供了更高的精度。