基于改進的磁場檢驗和主方向算法的行人航向估計

張永耀,周凌柯,黃海舟,蔡紫燁,李 勝

(南京理工大學自動化學院,南京 210094)

0 引言

隨著智能手機等便攜式移動終端的快速發展,相關的基于位置的服務成為人們關注的重點[1]。其中,慣性測量單元(IMU)具有尺寸小、自主性高等特點,成為行人導航領域研究的核心,在消防救援、單兵作戰、商場等室內定位中具有廣闊的應用前景。

基于慣導的定位技術通過積分運算或者航跡推算的方法得到運載體當前的位置、姿態等信息,但是隨著時間的積累,通過積分計算出來的誤差不斷累積,只能在短時間內得到準確的位置信息。對此,許多學者進行了改進。Foxlin[2]將零速更新(zero velocity update,ZUPT)和擴展卡爾曼濾波器(extended Kalman filter,EKF)結合起來,提出了慣性導航系統(inertial navigation system,INS)+EKF+ZUPT的行人導航框架,對速度誤差進行了修正;Rajagopal[3]在INS+EKF+ZUPT 框架中融入零角速度更新算法(zero angular rate update,ZARU)對陀螺儀的誤差進行補償;Jimenez等[4]使用地磁修正算法和啟發式航向漂移修正算法(heuristic drift elimination,HDE)對航向角誤差進行修正。

Jimenez提出的算法提供了航向角作為額外的觀測量,但是如何正確獲取航向信息十分重要。在實際應用中,地磁信息很容易受到干擾,對于磁力計測量的數據,如果直接使用,會引入額外的誤差,因此需要對原始磁場信息進行校正。Afzal等[5]提出了穩定狀態磁場(quasi-static magnetic field,QSF)的概念,并根據磁場強度構建檢驗量,對磁場信息進行檢驗;劉少兵等[6]針對QSF檢驗容易出現誤判的情況,利用磁場強度、磁力計解算的航向角的差值、磁場強度變化量構造檢驗量,但是該方法只適用于直線行走時的磁場檢驗,在曲線行走時無法判斷。針對行人室內定位中存在航向角漂移的問題,Borenstein等[7]提出了HDE算法,基于人行走一般沿著走廊等直線行走這一思想,將建筑物的方向設定為主方向,通過主方向來約束航向角;Jimenez等[8]則對HDE 算法進行改進,只有檢測到行人正在直行時才會執行HDE 算法,從而減小行人在非主方向上行走時HDE的過度修正;針對HDE 算法在非主導方向上容易錯誤校正的缺點,Zhu等[9]提出一種實時擴充主方向數量的HDE 算法,使得航向修正在非預設主方向上時仍然能夠正常執行;針對HDE 方法中判斷直行需要設定閾值的問題,Muhammad等[10]提出了一種無閾值的轉彎檢測技術;鄧平等[11]根據航向角變化的時域特征對航向角進行分類,確定行人的直行狀態,并設定16個主方向,進行航向角的修正。

在分析以上算法的基礎上,本文針對長時間的慣性導航存在航向失真從而導致定位誤差大的問題,提出了一種融合IMU 和磁力計的行人導航算法,獲取準確的航向信息。根據行人運動過程中步幅航向的變化,將行人的行走分為直線行走和曲線行走兩部分,分別采用不同的方法對航向角信息進行修正。當檢測到行人沿曲線行走時,使用地磁信息輔助導航,針對傳統的QSF方法在磁場受到持續穩定干擾時容易出現誤判的情況,設計了一種基于磁場強度、地磁傾角、INS與磁力計解算的航向角之差這三個特征值的廣義似然比檢驗算法,進行穩定狀態磁場的檢驗,獲取純凈磁場從而計算得到航向信息;當檢測到行人沿直線行走時,針對傳統的HDE算法需要提前預知實驗環境信息并人為設定主方向的缺點,提出了一種自適應航向角漂移修正(adaptive heuristic drift elimination,a HDE)算法,在直行時實時建立主方向,對行人的航向誤差進行修正,提高行人導航定位的精度。

1 基于IMU 和磁力計的行人導航算法模塊設計

本文提出的導航算法結構如圖1所示。

圖1 基于IMU 和磁力計的行人導航算法Fig.1 Pedestrian navigation algorithm based on IMU and magnetometer

圖中,abk和wbk表示k時刻加速度計和陀螺儀在載體坐標系下的采樣結果;Cnb,k/k-1表示k時刻的方向余弦陣;Δα,Δγ和Δφ分別表示行人的俯仰角、橫滾角和航向角誤差;b和n分別表示載體坐標系和導航坐標系。

建立15維的狀態矢量模型Xk=[δΦkδvkδPkεbkΔbk]T,分別表示k時刻行人的姿態誤差、速度誤差、位置誤差、陀螺儀零偏誤差和加速度計零偏誤差,選擇導航坐標系為東北天坐標系。將捷聯慣導算法的更新結果作為卡爾曼濾波的預測量,其余約束算法提供誤差值的虛擬觀測量,進行卡爾曼濾波融合后對行人的位置和姿態進行估計。

當行人行走時,從足部接觸地面到抬起足部這一段時間,稱為零速狀態,本文所有的量測更新都是基于零速狀態進行的。根據傳感器采集的加速度計和陀螺儀數據,判斷行人是否處于零速狀態,如果行人處于零速狀態,則由不同的約束算法分別得到行人的姿態角(橫滾角、俯仰角、航向角)和速度虛擬觀測量及誤差,將誤差輸入到卡爾曼濾波器對行人的導航誤差進行修正,得到更為準確的誤差信息。算法流程如圖2所示。

圖2 算法流程Fig.2 Algorithm flow

2 算法模塊實現

2.1 零速檢測算法

零速檢測算法用于檢測行人的零速狀態,在算法中具有很重要的作用。零速檢測的準確程度直接關系到INS對導航誤差的修正,本文中采用加速度模值、角速度模值和加速度移動方差三個方法[12-13]共同檢測確定行人的零速區間。假設k時刻三個方法得到的檢測結果分別為T1,T2和T3,則最終的檢測結果如式(1)所示

零速檢測算法輸出的結果為0和1的運動靜止序列,考慮到行人行走時運動和靜止狀態在時間上是連續的,并且持續一段時間。但是由于實際運動過程中存在傳感器抖動等外部因素的影響,算法得到的零速區間并不連續。本文對于零速檢測結果做二進制均值濾波處理,消除類似圖3 中186～188 s處的偽靜止區間。

圖3 二進制均值濾波前后的零速檢測結果對比Fig.3 Comparison of zero-speed detection results before and after binary mean filtering

從圖3中可以看出,經過處理后,算法得到的結果在時間上是連續的,符合行人運動的規律。

2.2 速度修正算法和陀螺儀誤差修正

行人處于零速狀態時,只受重力加速度的影響,相應的速度和角速度理論值應該為零,但是測量得到的速度和角速度的實際值并不為零。此時將速度誤差和陀螺儀零偏誤差作為卡爾曼濾波的觀測量,即ZUPT 和ZARU 算法。誤差計算如式(2)和式(3)所示

2.3 俯仰角和橫滾角的誤差修正

假設加速度計的采樣為(abx,aby,abz),則俯仰角和橫滾角的計算如式(4)和式(5)所示

當行人處于零速狀態時,只受到重力加速度的影響,此時導航坐標系下的加速度數據為[0,0,g]T,假設為載體坐標系到導航坐標系的方向余弦陣,此時的加速度表示如式(6)所示

根據零速狀態的橫滾角和俯仰角信息對姿態誤差進行修正,將此時計算得到的橫滾角和俯仰角與捷聯慣導更新的估計值作差,輸入到卡爾曼濾波器對誤差進行修正,觀測量的計算如式(7)和式(8)所示

2.4 改進的穩定狀態磁場檢驗算法

磁力計可以測量得到環境中的磁場信息,但是地磁場非常微弱,很容易受到外界的干擾,因此使用磁力計解算航向角之前必須進行校準。本文采取橢球擬合[14-15]的方法對原始磁場數據進行補償。

2.4.1 地磁場與航向角

地磁場的分布隨時間和空間的變化非常緩慢[16],在一個小范圍區域內,地磁偏角、磁傾角和磁場強度可以視為常值。地磁場的模型如圖4所示。

圖4 地球磁場與航向角模型圖Fig.4 Model diagram of the Earth's magnetic field and heading angle

其中B表示地磁場;BH為水平分量;Bx,By,Bz為地磁場在北東地三個方向上的投影。磁場幅值和地磁傾角ψI的計算如式(9)和式(10)所示

航向角的計算如式(11)所示,其中磁偏角ψD在一定區域內是不變的。

2.4.2 改進的穩定狀態磁場檢驗算法

在實際環境中,地磁信息很容易受到干擾,如何在凌亂的磁場信息中提取可用信息成為融合磁力計的關鍵。由于行人運動是小范圍的低速運動,地磁場的磁場強度和磁傾角都是近似不變的。因此,本文設計了一種基于磁場強度、地磁傾角、IMU與磁力計解算的航向角之差的廣義似然比檢驗算法,對穩定狀態磁場的信息進行獲取。

1)檢驗量的獲取

a)根據式(9)可以計算得到磁場強度。

b)傳統的磁傾角的求取如式(10)所示,但是該求取過程需要使用方向余弦陣Cnb,k/k-1將載體坐標系下的數據轉換到導航坐標系下,而方向余弦陣由估算的歐拉角得到,使用估算的數據會引入額外的誤差。進一步研究發現在靜止狀態時,磁傾角的大小與加速度計和磁力計數據的乘積有關,相關證明如下:

當行人靜止時,載體坐標系下的加速度計和磁力計的數據分別為ab和mb,假設導航坐標系下的加速度和磁場分別為an和mn,則兩者的關系可以表示為

已知在一定的范圍內,重力加速度g和磁場強度均可視為常值,因此結論得證。

c)當磁場處于穩定時,地磁信息得到的航向角和慣導更新得到的航向角理論上是相等的。因此使用兩個航向角的差值作為檢驗量,完成對穩定狀態磁場的檢驗。

2)基于廣義似然比的穩定狀態磁場檢驗算法

行人運動第k步的觀測模型如式(13)所示。

其中,sk=分別表示相鄰兩步之間的磁傾角變化、磁場強度變化和當前時刻IMU 和磁力計得到的航向角的差值;rk為量測噪聲。

可以將穩定狀態磁場的檢驗轉換為二元假設問題。

其中,H1表示穩定狀態磁場;Ωn= {ζ∈w:n≤ζ≤n+w-1},則概率密度函數的表示如式(15)所示

構建廣義似然比檢驗量如式(16)所示

最終得到的廣義似然比檢驗結果如式(17)所示

其中,w為滑動窗口長度;γ'Z為檢驗門限閾值;和為噪聲方差。

如果式(17)成立,則磁場處于穩定狀態,表示此時的地磁信息可以用來計算得到航向角信息,當行人處于無主方向進行約束時,可以根據地磁信息計算航向角并通過卡爾曼濾波器對行人航向進行修正。

2.5 自適應航向角漂移修正算法

通過提取磁場數據中的可用信息,可以對航向角漂移進行抑制。但是當環境干擾很大不存在穩定狀態磁場時,使用磁力計的數據會帶來額外的偏差,需要其他的方法來獲取正確的航向信息。

傳統的HDE 算法,需要先驗知識預知建筑物以及環境的主方向,在陌生環境中無法工作。本文對HDE算法進行改進,對直行和轉彎行走的檢測嚴格限制,提出了一種實時建立主方向的a HDE 算法,通過相鄰四步的步幅航向確定行人是否直行,直行的情況下實時建立主方向,根據建立的主方向庫完成對后續航向角誤差的修正,該方法不需要提前預知建筑物或者環境的主方向,可以根據行人的行走狀態,實時建立主方向庫,在后續直行時對航向角誤差進行約束。

1)步幅航向

當IMU 固定于行人右腳時,定義右腳離開地面、接觸地面到再次離開地面為一個復步,步幅航向為連續兩步之間的角度[17]。定義零速點為零速區間的初始時刻,在零速點計算步幅航向。設第k個零速點的采樣序列為s(k),此時東北方向位移分別為x(s(k))和y(s(k)),則第k步的步幅航向θs(k)如式(18)

則第k步的步長可以表示如式(19)

設定步長的閾值tth為0.3 m,若SSL(k)＜tth,則認為此時的零速點為偽零速點,應該舍去,進一步提高零速檢測的準確度。

2)嚴格的直行轉彎檢測算法

使用a HDE 算法的前提是行人處于直行狀態,因此正確檢測出直行或者轉彎是關鍵。行人行走可以分為直線行走和轉彎兩種,無論是小曲率半徑行走還是大曲率半徑行走都可以在有限步內完成。本文基于歷史時刻的步幅航向提出了一種嚴格的直行和轉彎檢測方法。

假設此時為第k步,相鄰四步的步幅航向為θs(j),j∈(k-3,k),當行人處于直行狀態時,步幅航向應該保持一致,利用歷史三步的步幅航向的差值進行判斷。

如果Δφ＜tth1,則表示步幅航向變化不明顯,行人處于直線行走或者沿大曲率半徑曲線行走階段。此時單純利用步幅航向差值無法區分,分析發現,當行人沿直線行走時,步幅航向在一個常值附近波動,當行人沿曲線行走時,相鄰幾步的步幅航向有相同的變化趨勢,因此進一步對直行和轉彎進行限制。

首先,判斷行人是否處于轉彎狀態。

其中,tth1為步幅航向角度檢驗的閾值。當T5=0時,表示行人處于轉彎狀態。定義相鄰兩步的步幅航向差值為Δθs,計算如下

當轉彎時,相鄰幾步的步幅航向有相同的變化趨勢,因此,做以下判斷

設定直行標識符為SSLP(k),則有

只有以上條件同時滿足時,認為行人處于直行狀態。

3)a HDE算法流程

在實際的環境中存在著許多不規則路徑,為了讓HDE方法在這些路徑上生效,本文對HDE 算法進行改進,改進后的方法可以實時建立主方向來約束航向角。a HDE算法流程如圖5所示。

圖5 aHDE算法流程圖Fig.5 Flowchart of the aHDE algorithm

首先對行人的運動狀態進行檢測,當檢測到行人當前狀態為直行時,則遍歷主方向數據庫,如果主方向數量為0,直接建立新的主方向θnew,新的主方向θnew計算如式(25)所示

其中,k表示當前行人處于第k步,即文中所定義的第k個采樣點;θs(j)表示第j步時行人的步幅航向;θnew表示新的主方向,由當前步幅航向和歷史三步步幅航向計算得到。

如果主方向的數量不為0,則在數據庫中搜索與當前步幅航向θs(k)距離最近的主方向θb1(k)。如果滿足條件,使用主方向θb1(k)對航向角漂移進行約束。

如果不滿足條件,則搜索與θnew最近的主方向θb2(k),滿足條件時將θnew作為新的主方向添加入庫。這樣可以保證建立的主方向不會重復,否則如果在相同的路徑上重復多次,解算得到的航向角略有差異,實時建立的主方向會有多條,增加匹配的難度。

直行狀態下,航向角的觀測量如式(26)所示

為了減少行人在非主方向上直行造成的誤修正,使用自適應量測噪聲,觀測量δφ(k)的量測噪聲標準差如式(27)所示

其中,σeHDE和β1的設置與傳感器性能相關。當前時刻的步幅航向與主方向越接近,算法的修正作用越明顯。

2.6 融合濾波更新

卡爾曼濾波的狀態方程與量測方程分別為

其中,Xk為系統的狀態變量,具體實現在前文已經提及;Φk/k-1為系統的狀態轉移矩陣;Qk-1為系統白噪聲;Zk為觀測量;Hk為量測矩陣;Wk表示傳感器數據的噪聲。具體的更新過程如文獻[18]和[19]所示。

本文所涉及的量測值如式(30)和式(31)所示

則對應的量測矩陣的設置如式(32)所示。

其中,I3×3為三行三列的單位矩陣,03×3為三行三列的全零矩陣。

3 實驗與結果

為了驗證本文算法在不同環境中的有效性,分別設計室外實驗和室內實驗兩部分,在學校內部采集行人運動信息,實驗設計行人運動的起點位置和終點位置重合,便于對導航結果進行評價。室外實驗行人攜帶GPS設備獲取運動的參考軌跡,分別采用IEZ、IEZ+ZARU、IEZ+ZARU+QSF和本文的算法對實驗人員的導航信息進行求取,從軌跡的重合程度、終點誤差、終點定位誤差和平均誤差四個維度對本文算法的優越性進行分析;室內實驗,由于GPS信號受到遮擋而無法使用,采用激光測距儀得到走廊兩側的墻壁信息,分析室內外磁場的差異,得知室內磁場由于疊加了建筑物的異常磁而不可用,因此室內實驗本文算法結果僅與有速度修正的零速更新算法ZUPT 相比較,從軌跡是否穿墻、終點誤差和終點定位誤差三個維度進行比較,說明本文算法可以提高行人的定位精度。

實驗所使用的傳感器型號為MTI-300,采樣頻率為100 Hz,實驗時傳感器固定于實驗人員右腳。傳感器的參數如表1所示。

表1 MTI-300性能參數Tab.1 MTI-300 performance parameters

3.1 室外實驗

實驗地點為自動化學院籃球場,實驗人員從起點出發,繞籃球場周圍道路行走,終點位置與起點位置相同,使用GPS得到行人運動的參考軌跡。實驗路徑長約660 m,包含8條直行路徑。參考軌跡如圖6所示。

圖6 室外復雜環境參考軌跡圖Fig.6 Outdoor complex environment reference trajectory diagram

分別使用IEZ、IEZ+ZARU、IEZ+ZARU+QSF和本文的算法對實驗人員的導航信息進行求取,得到的軌跡如圖7所示。

圖7 室外復雜環境實驗結果圖Fig.7 Experimental results of outdoor complex environment

不同方法得到的終點誤差、終點定位誤差和平均誤差如表2所示。

表2 室外復雜路徑下定位誤差Tab.2 Positioning error in outdoor complex path

本文算法實時建立的8個主方向與理論上的8個主方向對比如表3所示。

表3 實時建立主方向與理論主方向對比Tab.3 Comparison of the main direction established in real time with the main direction of theory

根據表2和表3的數據可知,行人航向對行人定位誤差的影響很大,提高行人航向的準確度可以有效提升行人定位精度。本文提出的算法在終點誤差和定位誤差方面優于其他的方法,可以對行人的航向角誤差進行有效修正,得到的行人軌跡與參考軌跡重合度高,更加符合真實軌跡。在660 m 的實驗中,本文算法得到終點誤差為4.545 4 m,實驗平均誤差為1.747 3 m,相比較沒有航向角修正的ZUPT,ZARU 算法和單獨使用磁力計數據進行修正的QSF 算法,本文方法的定位誤差分別降低了91.02%,82.93%和61.39%,并且可以實時建立的主方向完成對航向角的修正,定位精度符合行人定位的需求。

3.2 室內定位實驗

實驗地點為自動化學院院辦樓,實驗人員從起點出發,繞教學樓走廊行走兩圈,終點位置與起點位置相同,使用激光測距儀得到建筑物的輪廓信息,實驗路徑長約320 m。傳感器的安裝及實驗環境如圖8所示。

圖8 傳感器安裝及室內實驗環境Fig.8 Sensor installation and indoor experimental environment

通過對室內磁力計采集得到的數據進行分析,發現室內磁場比較紊亂,實驗中發現存在的穩定狀態磁場數據較少。磁力計采集得到的室內數據和室外數據經過校正后的磁場強度對比如圖9(a)和(b)所示。

圖9 室內外實驗磁場強度對比圖Fig.9 Comparison of indoor and outdoor experimental magnetic field intensity

從圖9可以看出,室外磁場在運動的過程中變化較小,基本保持在正常范圍0.6～0.85 Gs,此時磁力計數據是可用的;對于室內實驗得到的數據分析,初始時行人處于靜止狀態,磁場變化不明顯,之后運動時由于室內的磁場疊加了建筑物的異常磁,在同一空間內分布不均勻,磁場強度變化劇烈,此時環境中的磁場數據幾乎不可用。本文提出的算法主要使用a HDE 算法完成對航向角漂移誤差的修正,所以僅使用零速修正方法和本文提出的算法分別對實驗人員的軌跡進行獲取,得到的結果如圖10(a)和(b)所示。

圖10 室內環境兩種算法實驗結果圖Fig.10 Experimental results of two algorithms in indoor environment

從圖10中可以看出,本文算法可以對航向角進行約束,相比于只對速度誤差進行修正的ZUPT 方法,本文的算法結果將行人軌跡約束在兩個墻面之間,減少了軌跡穿墻現象的發生。通過計算本文算法得到的終點誤差為0.543 6 m,終點定位誤差為總路程的0.17%,而ZUPT 方法的誤差為1.533 4 m,本文算法與之相比定位誤差降低了64.56%,定位精度符合行人室內定位的需求。

4 結論

本文基于IMU 和磁力計這兩種傳感器進行實驗,提出了改進的穩定狀態磁場檢驗方法和自適應航向角漂移修正算法,使用兩種方法協同完成行人航向角的估計和修正。解決了行人定位無航向角修正和避免了單一使用磁力計由于磁場失真嚴重無法進行航向角修正的問題。通過實驗得出以下結論:

1)本文根據行人運動過程中步幅航向的變化,將行人的行走分為直線行走和曲線行走兩部分,分別采用改進的基于廣義似然比的穩定狀態磁場檢驗算法和自適應航向角漂移修正(a HDE)算法得到行人的航向信息,而且a HDE 方法不需要提前預知實驗環境的主方向,相比較傳統的HDE 算法,在陌生環境中也可以應用,應用局限更小。

2)室內外的多次實驗結果說明,本文提出的算法提高了行人定位的精度,在室外和室內終點定位誤差分別為4.545 4 m 和0.543 6 m,分別占總路徑的0.69%和0.17%。并且本文的算法結果相比較沒有航向角修正的ZUPT,ZARU 算法和單獨使用磁力計數據進行修正的QSF算法,定位誤差分別降低了91.02%,82.93%和61.39%。