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

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

0 引言

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

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

1 基于MIMU的多約束PDR計(jì)步方案

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

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

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

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

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

圖2 第i-1步PDR計(jì)步流程圖

約束條件的具體含義為:

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

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

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

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

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

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

(1)

本文通過(guò)長(zhǎng)度為W的時(shí)間窗計(jì)算波峰閾值Tcre與波谷閾值Ttro。

(2)

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

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

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

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

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

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

2.1 ZUPT與ZARU修正誤差

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

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

(3)

(4)

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

(5)

量測(cè)方程為:

(6)

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

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

(7)

Zk=HkXk+Vk

(8)

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

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

(9)

(10)

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

(11)

(12)

(13)

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

(14)

(15)

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

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

圖5 坐標(biāo)系示意圖

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

(16)

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

(17)

解得:

(18)

式中:ψ為N系相對(duì)于l系的偏航角,以符合右手系的方向?yàn)檎较?根據(jù)上式即可確定ψ值;(x2,y2)為下一時(shí)刻的坐標(biāo);Δx、Δy為x坐標(biāo)與y坐標(biāo)的變化。

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

2.3 UWB/MIMU組合模型

在整個(gè)行人運(yùn)動(dòng)過(guò)程中會(huì)出現(xiàn)3種情況:僅存在位置參考信息的時(shí)刻、僅存在速度參考信息的時(shí)刻以及位置和速度參考信息同時(shí)存在的時(shí)刻。

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

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

(19)

(20)

(21)

(22)

3 實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

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

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

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

圖9 實(shí)驗(yàn)場(chǎng)景圖

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

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

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

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

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

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

(a)x方向速度

(b)y方向速度

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

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

3.2 PDR補(bǔ)償U(kuò)WB/MIMU組合定位實(shí)驗(yàn)

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

圖13 實(shí)驗(yàn)場(chǎng)景圖

圖14 實(shí)驗(yàn)參考點(diǎn)和參考軌跡示意圖

3.2.1 ZUPT和ZARU仿真驗(yàn)證

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

圖15 行人運(yùn)動(dòng)的仿真軌跡

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

(a)航向角誤差對(duì)比

(b)俯仰角誤差對(duì)比

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

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

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

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

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

3.2.2 PDR輔助下UWB/MIMU組合方案實(shí)驗(yàn)

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

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

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

整個(gè)模擬信號(hào)丟失過(guò)程持續(xù)的時(shí)間為6 s,圖20顯示了信號(hào)丟失前的計(jì)步結(jié)果和步長(zhǎng)估計(jì)結(jié)果。

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

(b)PDR步長(zhǎng)估計(jì)結(jié)果圖20 UWB信號(hào)丟失前MIMU計(jì)步結(jié)果和步長(zhǎng)估計(jì)結(jié)果

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

利用線性步長(zhǎng)模型對(duì)UWB信號(hào)丟失前的步長(zhǎng)模型進(jìn)行估計(jì),線性步長(zhǎng)模型為

SLi=A+B·Ti+C·Pi

(23)

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

本文基于最小二乘法對(duì)UWB信號(hào)丟失前的步長(zhǎng)模型進(jìn)行估計(jì),得到的線性步長(zhǎng)模型為

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

(24)

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

圖21 各方案定位軌跡對(duì)比圖

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

4 結(jié)論

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