基于自適應(yīng)PDR補償?shù)腢WB/MIMU組合行人定位算法

王 偉,馬晞茗,王敬民,佘嘯天,黃 平

(哈爾濱工程大學(xué)智能科學(xué)與工程學(xué)院,黑龍江哈爾濱 150001)

0 引言

目前基于超寬帶(UWB)[1]和微型慣性測量單元(MIMU)器件[2]的組合方案在行人室內(nèi)定位領(lǐng)域取得了較大進展。UWB可以修正MIMU的累計誤差,MIMU則可以解決UWB在非視距環(huán)境下定位結(jié)果不可靠的問題。楊海等[3]在UWB/MIMU組合定位模型基礎(chǔ)上引入決策樹容錯判斷機制,進而構(gòu)建出容錯組合定位系統(tǒng),能夠較好檢測出UWB定位系統(tǒng)出現(xiàn)的定位數(shù)據(jù)丟失,但尚未考慮到噪聲先驗信息錯誤對定位精度的影響。S.Zhang等[4]采用最小方差無偏有限脈沖響應(yīng)算法,從噪聲測量中獲得準確的位置和速度估計,提高了噪聲先驗信息錯誤情況下的定位精度,但該算法在零速修正不準確時性能受到較大影響。李榮冰等[5]將整個系統(tǒng)分為零速修正、UWB/MIMU、MIMU/磁力計3個子濾波器模型,提高了系統(tǒng)的容錯性,但是當磁力計失效時,整體定位效果受到的影響較大。Q.Tian等[6]提出一種基于行人相對于基站的行走方向角與阻塞程度關(guān)系的模型,一定程度上降低了對磁力計的依賴,但該算法對結(jié)構(gòu)性強的室內(nèi)環(huán)境依賴性較強,同時PDR計步準確性較差時定位精度受干擾較大。K.Yu等[7]針對航跡丟失問題,提出一種新的粒子重置方法,利用UWB的距離測量值計算粒子群的均方根誤差,提高了UWB/MIMU行人組合導(dǎo)航系統(tǒng)的穩(wěn)定性,但UWB信號僅存在于局部區(qū)域?qū)е略撍惴ù嬖谝欢ǖ木窒扌?。Y.Wang等[8]提出一種基于UWB和慣性測量單元(IMU)緊組合的定位算法,提高了定位精度,但算法計算量較大,且PDR計步精準度有待提升,從而導(dǎo)致定位延遲較大?？挛跽萚9]基于卡爾曼濾波算法將MIMU和磁力計進行航向估計,提高了PDR過程中的航向角精度,但是行人航向角在定位中后期容易偏離真實的航向。

由此可知,現(xiàn)有的PDR以及UWB/MIMU組合方案存在著定位不連續(xù)和定位誤差累計的問題。針對該問題,本文提出了一種基于自適應(yīng)PDR補償?shù)腢WB/MIMU組合定位方案。首先,對傳統(tǒng)PDR計步方案[10]進行改進,引入步行周期閾值條件和后驗策略,提出一種結(jié)合閾值和計步周期的計步方案,降低了對閾值設(shè)置的準確性要求,并消除了偽波峰和偽波谷對計步結(jié)果的影響; 引入ZUPT和ZARU組合的方法輔助慣導(dǎo)來修正誤差,一定程度上抑制了MIMU在行走過程中的速度位置發(fā)散;將UWB和MIMU獲取的定位信息統(tǒng)一至同一坐標系下,實現(xiàn)了UWB和MIMU的速度位置組合,并利用組合后的定位結(jié)果對PDR計步過程中識別的行人步長進行訓(xùn)練。當UWB信號丟失時,則利用訓(xùn)練的步長進行PDR得到位置量測信息,然后與MIMU進行速度位置組合,計算得到組合系統(tǒng)在UWB信號丟失時的定位精度。最后,通過具體實驗驗證了本文提出的方案對室內(nèi)環(huán)境下行人定位的精確度有了顯著提高。

1 基于MIMU的多約束PDR計步方案

1.1 定位系統(tǒng)結(jié)構(gòu)設(shè)計

當UWB信號接收正常時,利用UWB為MIMU提供位置參考信息,并采用卡爾曼濾波進行速度位置組合,利用組合后的定位結(jié)果進行行人PDR步長的在線訓(xùn)練;當UWB信號受干擾丟失時,則利用UWB信號丟失前訓(xùn)練的步長結(jié)果輔助MIMU進行PDR,然后將PDR的結(jié)果與MIMU進行速度與位置的組合,組合定位系統(tǒng)的整體結(jié)構(gòu)如圖1所示。

圖1 組合定位系統(tǒng)整體結(jié)構(gòu)

1.2 PDR計步方案設(shè)計

對于計步環(huán)節(jié)而言,當行人從靜止狀態(tài)轉(zhuǎn)為行走狀態(tài)時,行人第1次邁步過程中z軸加速度計數(shù)據(jù)往往會出現(xiàn)幅值較小的情況。此時如果計步方案所選閾值太大,則可能會出現(xiàn)漏計現(xiàn)象;若閾值太小,則后續(xù)由于偽波峰偽波谷的干擾,可能出現(xiàn)多計現(xiàn)象。本文設(shè)計了一種多約束的PDR計步方案,可以在設(shè)置閾值較小的情況下實現(xiàn)準確地計步。圖2為第i-1步PDR計步流程圖。其中約束條件包括:極值條件、峰值條件以及后驗周期閾值條件。

圖2 第i-1步PDR計步流程圖

約束條件的具體含義為:

(1)極值條件:設(shè)置1個2N+1的滑動窗口,若第N個元素大于滑動窗口內(nèi)的其他元素,則該元素為極大值;同理,若第N個元素小于滑動窗口內(nèi)的其他元素,則該元素為極小值。

(2)峰值條件:當滿足極值條件的點大于波峰閾值或小于波谷閾值才可能是真正的波峰或者波谷。

(3)后驗周期閾值條件:滿足極大值條件和峰值條件的點可能是偽波峰,此時需計算當前波峰與上一個波峰之間的時間差,若大于步長閾值,則上一個波峰是上周期內(nèi)真正的波峰;同理,若滿足極小值條件和波谷閾值,則計算當前點與上一個波谷之間的時間差,若大于步長閾值,則上一個波谷可確認是真正的波谷。

若當前狀態(tài)為波谷,則需尋找符合極大值條件和峰值條件的波峰。若在尋找波峰的過程中檢測到波谷,則比較該波谷與當前狀態(tài)波谷峰值的大小關(guān)系。若目前的波谷值更小,則更新當前狀態(tài)的波谷值,否則繼續(xù)尋找波峰。找到波峰后,則計算當前檢測到的波峰與上一個波峰之間的時間間隔,若時間間隔大于周期閾值,即可確認上一個波峰是上一步行周期內(nèi)的實際波峰,并把系統(tǒng)切換為尋找波谷的狀態(tài)。

若當前狀態(tài)為波峰,則需要尋找符合極值條件和峰值條件的波谷。若尋找波谷過程中又檢測到波峰,則將該波峰與當前狀態(tài)的波峰進行比較,若當前的波峰峰值更大,則更新當前的波峰值,否則繼續(xù)尋找波谷。找到波谷后,則計算當前檢測到的波谷與上一個波谷之間的時間間隔,若時間間隔大于周期閾值,則可確認上一個波谷是上一步行周期內(nèi)真正的波谷。至此,上一步行周期內(nèi)真正的波峰和波谷都已被確認,則計步加1,并把系統(tǒng)狀態(tài)重新更新為尋找波峰的狀態(tài),然后開始下一步尋找。

通過上述過程,可在波峰和波谷閾值設(shè)置較小時實現(xiàn)偽波峰和偽波谷的剔除,閾值的判斷方法如式(1)所示:

(1)

本文通過長度為W的時間窗計算波峰閾值Tcre與波谷閾值Ttro。

(2)

式中:aN為窗內(nèi)豎直加速度集合;maxloc(aN)與minloc(aN)分別為豎直方向線性加速度的極大值與極小值集合;mean和var分別為窗內(nèi)豎直加速度集合中所有數(shù)值的平均值和方差;α為權(quán)重因子。

設(shè)置波峰閾值為1.5g,波谷閾值為0.5g。

以某次實測結(jié)果為例,實驗過程為從靜止開始,每次走20步則停頓靜止一段時間,一共4次。圖3為整體步態(tài)檢測結(jié)果。其中縱坐標比力(fb)表示載體相對慣性空間的絕對加速度和引力加速度之差。根據(jù)圖4可知,在由靜止變?yōu)檫\動的第一步過程中,z軸的加速度計數(shù)據(jù)幅值明顯小于后續(xù)步行過程中的偽波峰幅值,但仍能被正確識別為第一步的波峰;第4、5、6、7、9步過程中出現(xiàn)的偽波谷也能被正確剔除。整個實驗過程中一共行走了80步,實驗的計步結(jié)果也為80步。實驗結(jié)果表明本方案可準確實現(xiàn)PDR計步。

圖3 整體步態(tài)檢測結(jié)果

圖4 局部步態(tài)檢測結(jié)果放大圖

2 自適應(yīng)PDR補償?shù)腢WB/MIMU組合模型

2.1 ZUPT與ZARU修正誤差

在零速檢測過程中,由于MIMU慣性器件的誤差較大,因此零速修正在行人導(dǎo)航過程中非常重要。本文通過ZARU將陀螺儀實際角速度測量值與零角速度觀測量作差后的結(jié)果作為量測量,進而進行卡爾曼濾波的方法修正偏航角誤差。

在著地靜止階段有式(3)與式(4)成立:

(3)

(4)

根據(jù)式(3)、式(4)以及SINS的誤差方程[10-11]可知,基于ZUPT和ZARU組合的連續(xù)卡爾曼濾波的狀態(tài)方程為:

(5)

量測方程為:

(6)

對上述連續(xù)時間卡爾曼濾波方差進行離散化處理。離散化方程如式(7)與式(8)所示:

Xk=Φk/k-1Xk-1+Γk-1Wk-1

(7)

Zk=HkXk+Vk

(8)

式中:Xk為k時刻的狀態(tài);Xk-1為k-1時刻的狀態(tài);Zk為k時刻的量測;Φk/k-1為k-1時刻到k時刻的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣;Γk-1為系統(tǒng)噪聲驅(qū)動陣;Wk-1為方差為Qk的系統(tǒng)噪聲;Hk為量測矩陣;Vk為方差為Rk量測噪聲。

若零速檢測結(jié)果為非靜止,則只進行時間更新操作,利用式(9)與式(10)可求狀態(tài)的一步預(yù)測以及協(xié)方差陣的一步預(yù)測:

(9)

(10)

若零速檢測結(jié)果為靜止,則需要在進行時間更新的基礎(chǔ)上進一步量測更新,利用式(11)～式(13)對狀態(tài)的一步預(yù)測進行加權(quán),對狀態(tài)變量的值以及協(xié)方差矩陣進行更新:

(11)

(12)

(13)

為避免校正過度及提高反饋校正后的平滑度,本文采用部分反饋策略,具體表示為:

(14)

(15)

2.2 室內(nèi)定位信息坐標統(tǒng)一

由于傳統(tǒng)的UWB/MIMU組合方案需要額外考慮UWB所在室內(nèi)坐標系、東北天坐標系和MIMU載體系與東北天坐標系之間的關(guān)系,一定程度上影響了整個定位方案的實用性,因此本文通過將室內(nèi)定位結(jié)果統(tǒng)一至室內(nèi)局部坐標系,有效避免了對于東北天坐標系的討論與分析,從而提高了整個室內(nèi)定位系統(tǒng)的實用性。圖5為系統(tǒng)坐標系示意圖。

圖5 坐標系示意圖

在圖5中,l系為用于室內(nèi)定位的UWB所在的室內(nèi)局部坐標系;N系為行人初始靜止時刻MIMU載體坐標系重合的導(dǎo)航坐標系。首先利用UWB和MIMU單獨進行定位,分別獲取初始位置在l系下的坐標(x0,y0)和N系下的坐標(0,0),運動后的新位置在l系下的坐標(x1,y1)和N系下的坐標(Δx,Δy),則有:

(16)

將式(16)代入圖5坐標系中則有如下方程成立:

(17)

解得:

(18)

式中:ψ為N系相對于l系的偏航角,以符合右手系的方向為正方向,根據(jù)上式即可確定ψ值;(x2,y2)為下一時刻的坐標;Δx、Δy為x坐標與y坐標的變化。

至此,可以將UWB和MIMU的定位結(jié)果在UWB室內(nèi)局部坐標系l系與MIMU導(dǎo)航坐標系N系之間互相轉(zhuǎn)換。

2.3 UWB/MIMU組合模型

在整個行人運動過程中會出現(xiàn)3種情況:僅存在位置參考信息的時刻、僅存在速度參考信息的時刻以及位置和速度參考信息同時存在的時刻。

當檢測到?jīng)]有任何量測信息時,僅做卡爾曼濾波時間更新;當檢測到ZUPT速度參考信息時,進行速度組合的卡爾曼濾波量測更新;當檢測到位置參考信息時,則繼續(xù)進行位置組合的卡爾曼濾波量測更新,整個模型的執(zhí)行流程如圖6所示。

圖6 UWB/MIMU組合模型執(zhí)行流程

(19)

(20)

(21)

(22)

3 實驗結(jié)果與分析

本文使用傳感器的加速度計與陀螺儀部分參數(shù)如表1所示。

表1 加速度計與陀螺儀部分參數(shù)

在實驗中,將MIMU模塊安裝在行人足部,且z軸指向天向,在后續(xù)實驗中僅對z軸的加速度計數(shù)據(jù)進行分析。本文基于FIR濾波對z軸加速度計的原始數(shù)據(jù)進行預(yù)處理,設(shè)計了一個25階的FIR低通濾波器,采樣頻率fs為100 Hz,截止頻率fc為10 Hz。圖7為FIR濾波后的z軸加速度計數(shù)據(jù)。

圖7 加速度計三軸比力數(shù)據(jù)

根據(jù)圖7和圖8可知,z軸加速度計的數(shù)據(jù)經(jīng)過FIR濾波后,在去除了高頻擾動的同時較好地保留了原始數(shù)據(jù)的峰值特征和零速特征,進而便于后續(xù)PDR計步算法和零速檢測算法的執(zhí)行。

圖8 FIR濾波后的z軸加速度計數(shù)據(jù)

3.1 MIMU數(shù)據(jù)修正

實驗場景如圖9所示,在無衛(wèi)星信號且未使用磁力計數(shù)據(jù)的情況下,直接以MIMU初始時刻的載體坐標系作為導(dǎo)航坐標系進行行人導(dǎo)航解算,并利用ZUPT和ZARU進行修正。

圖9 實驗場景圖

具體實驗步驟為:基于初始靜止時刻的MIMU載體坐標系,沿著長為134.52 m、寬為86.15 m的矩形行走一圈后回到起始點,接著沿著反方向繼續(xù)行走,最終形成如圖10所示的運動軌跡,其中橫縱坐標分別表示實際實驗中x和y方向的坐標值,其閉合誤差為2.59 m,占總行走距離的0.29%。

圖10 定位軌跡結(jié)果

經(jīng)過ZUPT和ZARU的修正后,運動軌跡整體趨勢正確。但在行走過程中,存在5～10 m的位置偏差,分別占各邊長行走距離的5.80%和7.43%,這是由于MIMU器件的精度較低,長時間行走會累計較大的位置誤差。圖11(a)中φ表示航向角,圖11(b)中θ表示俯仰角,圖11(c)中γ表示橫滾角。圖12(a)～圖12(c)中的vNx,vNy,vNz分別表示載體x,y,z方向的速度。根據(jù)圖11以及圖12可知,經(jīng)過ZUPT修正后的俯仰角和橫滾角更加平穩(wěn),經(jīng)過ZARU修正后的偏航角平穩(wěn)度也有所提升,較好地抑制了速度誤差的發(fā)散。

(a)航向角結(jié)果對比

(b)俯仰角結(jié)果對比

(c)橫滾角結(jié)果對比圖11 姿態(tài)角對比結(jié)果

(a)x方向速度

(b)y方向速度

(c)z方向速度圖12 速度計算結(jié)果

由此可知,當行人處于未知絕對方向的室內(nèi)環(huán)境時,基于本文提出的方案,可以較為準確地得到連續(xù)的行人相對運動信息,從而降低了行人室內(nèi)導(dǎo)航對磁力計的依賴。

3.2 PDR補償UWB/MIMU組合定位實驗

本實驗中MIMU模塊安裝在行人足部,Y軸朝前,X軸朝右。UWB定位標簽則安裝在行人帽子處,采用的MIMU數(shù)據(jù)為100 Hz,包括電腦接收時間、片內(nèi)時間、三軸加速度計數(shù)據(jù)和三軸陀螺儀數(shù)據(jù)。UWB數(shù)據(jù)為10 Hz,標簽側(cè)UWB數(shù)據(jù)通過串口傳到計算機。采集完數(shù)據(jù)后統(tǒng)一將時間轉(zhuǎn)換為時間戳,并將UWB數(shù)據(jù)與最近時間的MIMU數(shù)據(jù)進行匹配,以實現(xiàn)UWB與MIMU數(shù)據(jù)的軟件同步,實驗場景如圖13所示,實驗的參考點和參考軌跡如圖14所示,行人沿逆時針方向運動直至遍歷完整個大廳,形成的運動軌跡為矩形。

圖13 實驗場景圖

圖14 實驗參考點和參考軌跡示意圖

3.2.1 ZUPT和ZARU仿真驗證

利用軌跡發(fā)生器模擬行人運動,并輸出對應(yīng)的位置、速度、姿態(tài)信息以及MIMU的角速度和比力數(shù)據(jù)。設(shè)置行人步長為0.7 m,步行周期為0.8 s。行人每一步的運動包括0.2 s勻加速運動、0.2 s勻減速運動以及0.4 s零速階段,每次行人轉(zhuǎn)向時則靜止5 s,圖15顯示了行人運動的仿真軌跡。

圖15 行人運動的仿真軌跡

圖16 導(dǎo)航仿真軌跡對比圖

(a)航向角誤差對比

(b)俯仰角誤差對比

(c)橫滾角誤差對比圖17 姿態(tài)角誤差對比圖

(a)x方向速度誤差對比

(b)y方向速度誤差對比

(c)z方向速度誤差對比圖18 導(dǎo)航速度誤差對比圖

圖17(a)中δΨ表示航向角誤差,圖17(b)中δθ表示俯仰角誤差,圖17(c)中δγ表示橫滾角誤差。帶有圓圈的曲線為慣導(dǎo)解算結(jié)果,無圓圈曲線為ZUPT+ZARU得出結(jié)果。圖18(a)～圖18(c)中δve、δvn、δvu分別表示x、y、z方向的速度誤差。結(jié)果表明,經(jīng)過ZUPT和ZARU組合方法修正后,行人運動的軌跡與參考軌跡幾乎重合;偏航角誤差從4°下降到2°,較好地抑制了MIMU速度誤差的發(fā)散。北向位置誤差0.1 m,東向位置誤差0.04 m,天向速度誤差與SINS相比發(fā)散速度得到了明顯的減緩。

3.2.2 PDR輔助下UWB/MIMU組合方案實驗

為驗證組合方案的定位性能,分別測試僅使用ZUPT、僅適用UWB以及組合方案情況下的定位軌跡,圖19為室內(nèi)環(huán)境下各方案的定位軌跡對比圖。

圖19 室內(nèi)環(huán)境下各方案的定位軌跡對比圖

根據(jù)圖19可知,僅使用ZUPT的MIMU定位結(jié)果雖然形狀正確,但是由于位置誤差逐漸積累,定位結(jié)果慢慢偏離真實軌跡;僅適用UWB模塊的定位結(jié)果長期穩(wěn)定性較好,但是定位結(jié)果跳變較為嚴重。與僅使用MIMU模塊的定位結(jié)果相比,基于UWB/MIMU的組合導(dǎo)航方案修正了MIMU的位置累計誤差;與僅使用UWB模塊的定位結(jié)果相比,基于UWB/MIMU的組合導(dǎo)航方案有效提高了定位頻率,降低了UWB定位結(jié)果的跳變。實驗結(jié)果表明,本文提出的方案是有效的。

整個模擬信號丟失過程持續(xù)的時間為6 s,圖20顯示了信號丟失前的計步結(jié)果和步長估計結(jié)果。

(a)PDR計步結(jié)果

(b)PDR步長估計結(jié)果圖20 UWB信號丟失前MIMU計步結(jié)果和步長估計結(jié)果

在模擬UWB信號丟失前,行人共行走67步,圖20(a)中計步結(jié)果為66步,漏記步數(shù)為1步。在圖20(b)中,存在4種步長結(jié)果,可按步長大小分為:短步、單步、長步、雙步。

利用線性步長模型對UWB信號丟失前的步長模型進行估計,線性步長模型為

SLi=A+B·Ti+C·Pi

(23)

式中:SLi為第i步行人行走的步長;Ti為從第i-1步到第i步所用的時間;Pi為加速度計幅值;A、B以及C為待估計的步長模型。

本文基于最小二乘法對UWB信號丟失前的步長模型進行估計,得到的線性步長模型為

SLi=0.832 6-0.319 2×Ti+0.033 0×Pi

(24)

結(jié)合MIMU模塊輸出的航向來推算行人航位,將航位推算得到的位置用于MIMU模塊的速度位置組合。圖21顯示了僅使用UWB模塊、加入PDR補償?shù)慕M合方案以及無補償?shù)慕M合方案的定位軌跡對比圖。

圖21 各方案定位軌跡對比圖

根據(jù)圖21可知,當UWB信號丟失后,僅使用MIMU模塊進行導(dǎo)航得到的軌跡定位誤差逐漸增大,偏離真實軌跡。在PDR輔助下的MIMU定位軌跡誤差增大趨勢得到了抑制,軌跡更接近真實軌跡,表明本文提出的組合方案是有效的。

4 結(jié)論

本文提出了一種基于自適應(yīng)PDR補償?shù)腢WB/MIMU組合行人定位方案,較好地解決了現(xiàn)有方案存在的定位不連續(xù)和定位誤差累計問題。首先通過ZUPT和ZARU組合的方法進行零速修正,有效抑制了速度誤差的發(fā)散,使得航向角誤差降低了50%;然后引入一種自適應(yīng)濾波模型,利用線性步長模型結(jié)合MIMU模塊輸出的航向進行行人航位推算,較好地抑制了行人軌跡的發(fā)散,提高了行人定位的連續(xù)性。在未來的工作中,將在所提方案的基礎(chǔ)上,進一步利用UWB信息來修正航向角的誤差,同時考慮量測噪聲方差陣的變化,進一步提高UWB/MIMU組合導(dǎo)航系統(tǒng)的定位精度。