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

(南京理工大學(xué)自動(dòng)化學(xué)院,南京 210094)

0 引言

隨著智能手機(jī)等便攜式移動(dòng)終端的快速發(fā)展,相關(guān)的基于位置的服務(wù)成為人們關(guān)注的重點(diǎn)[1]。其中,慣性測(cè)量單元(IMU)具有尺寸小、自主性高等特點(diǎn),成為行人導(dǎo)航領(lǐng)域研究的核心,在消防救援、單兵作戰(zhàn)、商場(chǎng)等室內(nèi)定位中具有廣闊的應(yīng)用前景。

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

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

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

1 基于IMU 和磁力計(jì)的行人導(dǎo)航算法模塊設(shè)計(jì)

本文提出的導(dǎo)航算法結(jié)構(gòu)如圖1所示。

圖1 基于IMU 和磁力計(jì)的行人導(dǎo)航算法Fig.1 Pedestrian navigation algorithm based on IMU and magnetometer

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

建立15維的狀態(tài)矢量模型Xk=[δΦkδvkδPkεbkΔbk]T,分別表示k時(shí)刻行人的姿態(tài)誤差、速度誤差、位置誤差、陀螺儀零偏誤差和加速度計(jì)零偏誤差,選擇導(dǎo)航坐標(biāo)系為東北天坐標(biāo)系。將捷聯(lián)慣導(dǎo)算法的更新結(jié)果作為卡爾曼濾波的預(yù)測(cè)量,其余約束算法提供誤差值的虛擬觀測(cè)量,進(jìn)行卡爾曼濾波融合后對(duì)行人的位置和姿態(tài)進(jìn)行估計(jì)。

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

圖2 算法流程Fig.2 Algorithm flow

2 算法模塊實(shí)現(xiàn)

2.1 零速檢測(cè)算法

零速檢測(cè)算法用于檢測(cè)行人的零速狀態(tài),在算法中具有很重要的作用。零速檢測(cè)的準(zhǔn)確程度直接關(guān)系到INS對(duì)導(dǎo)航誤差的修正,本文中采用加速度模值、角速度模值和加速度移動(dòng)方差三個(gè)方法[12-13]共同檢測(cè)確定行人的零速區(qū)間。假設(shè)k時(shí)刻三個(gè)方法得到的檢測(cè)結(jié)果分別為T(mén)1,T2和T3,則最終的檢測(cè)結(jié)果如式(1)所示

零速檢測(cè)算法輸出的結(jié)果為0和1的運(yùn)動(dòng)靜止序列,考慮到行人行走時(shí)運(yùn)動(dòng)和靜止?fàn)顟B(tài)在時(shí)間上是連續(xù)的,并且持續(xù)一段時(shí)間。但是由于實(shí)際運(yùn)動(dòng)過(guò)程中存在傳感器抖動(dòng)等外部因素的影響,算法得到的零速區(qū)間并不連續(xù)。本文對(duì)于零速檢測(cè)結(jié)果做二進(jìn)制均值濾波處理,消除類似圖3 中186～188 s處的偽靜止區(qū)間。

圖3 二進(jìn)制均值濾波前后的零速檢測(cè)結(jié)果對(duì)比Fig.3 Comparison of zero-speed detection results before and after binary mean filtering

從圖3中可以看出,經(jīng)過(guò)處理后,算法得到的結(jié)果在時(shí)間上是連續(xù)的,符合行人運(yùn)動(dòng)的規(guī)律。

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

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

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

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

當(dāng)行人處于零速狀態(tài)時(shí),只受到重力加速度的影響,此時(shí)導(dǎo)航坐標(biāo)系下的加速度數(shù)據(jù)為[0,0,g]T,假設(shè)為載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的方向余弦陣,此時(shí)的加速度表示如式(6)所示

根據(jù)零速狀態(tài)的橫滾角和俯仰角信息對(duì)姿態(tài)誤差進(jìn)行修正,將此時(shí)計(jì)算得到的橫滾角和俯仰角與捷聯(lián)慣導(dǎo)更新的估計(jì)值作差,輸入到卡爾曼濾波器對(duì)誤差進(jìn)行修正,觀測(cè)量的計(jì)算如式(7)和式(8)所示

2.4 改進(jìn)的穩(wěn)定狀態(tài)磁場(chǎng)檢驗(yàn)算法

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

2.4.1 地磁場(chǎng)與航向角

地磁場(chǎng)的分布隨時(shí)間和空間的變化非常緩慢[16],在一個(gè)小范圍區(qū)域內(nèi),地磁偏角、磁傾角和磁場(chǎng)強(qiáng)度可以視為常值。地磁場(chǎng)的模型如圖4所示。

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

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

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

2.4.2 改進(jìn)的穩(wěn)定狀態(tài)磁場(chǎng)檢驗(yàn)算法

在實(shí)際環(huán)境中,地磁信息很容易受到干擾,如何在凌亂的磁場(chǎng)信息中提取可用信息成為融合磁力計(jì)的關(guān)鍵。由于行人運(yùn)動(dòng)是小范圍的低速運(yùn)動(dòng),地磁場(chǎng)的磁場(chǎng)強(qiáng)度和磁傾角都是近似不變的。因此,本文設(shè)計(jì)了一種基于磁場(chǎng)強(qiáng)度、地磁傾角、IMU與磁力計(jì)解算的航向角之差的廣義似然比檢驗(yàn)算法,對(duì)穩(wěn)定狀態(tài)磁場(chǎng)的信息進(jìn)行獲取。

1)檢驗(yàn)量的獲取

a)根據(jù)式(9)可以計(jì)算得到磁場(chǎng)強(qiáng)度。

b)傳統(tǒng)的磁傾角的求取如式(10)所示,但是該求取過(guò)程需要使用方向余弦陣Cnb,k/k-1將載體坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下,而方向余弦陣由估算的歐拉角得到,使用估算的數(shù)據(jù)會(huì)引入額外的誤差。進(jìn)一步研究發(fā)現(xiàn)在靜止?fàn)顟B(tài)時(shí),磁傾角的大小與加速度計(jì)和磁力計(jì)數(shù)據(jù)的乘積有關(guān),相關(guān)證明如下:

當(dāng)行人靜止時(shí),載體坐標(biāo)系下的加速度計(jì)和磁力計(jì)的數(shù)據(jù)分別為ab和mb,假設(shè)導(dǎo)航坐標(biāo)系下的加速度和磁場(chǎng)分別為an和mn,則兩者的關(guān)系可以表示為

已知在一定的范圍內(nèi),重力加速度g和磁場(chǎng)強(qiáng)度均可視為常值,因此結(jié)論得證。

c)當(dāng)磁場(chǎng)處于穩(wěn)定時(shí),地磁信息得到的航向角和慣導(dǎo)更新得到的航向角理論上是相等的。因此使用兩個(gè)航向角的差值作為檢驗(yàn)量,完成對(duì)穩(wěn)定狀態(tài)磁場(chǎng)的檢驗(yàn)。

2)基于廣義似然比的穩(wěn)定狀態(tài)磁場(chǎng)檢驗(yàn)算法

行人運(yùn)動(dòng)第k步的觀測(cè)模型如式(13)所示。

其中,sk=分別表示相鄰兩步之間的磁傾角變化、磁場(chǎng)強(qiáng)度變化和當(dāng)前時(shí)刻IMU 和磁力計(jì)得到的航向角的差值;rk為量測(cè)噪聲。

可以將穩(wěn)定狀態(tài)磁場(chǎng)的檢驗(yàn)轉(zhuǎn)換為二元假設(shè)問(wèn)題。

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

構(gòu)建廣義似然比檢驗(yàn)量如式(16)所示

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

其中,w為滑動(dòng)窗口長(zhǎng)度;γ'Z為檢驗(yàn)門(mén)限閾值;和為噪聲方差。

如果式(17)成立,則磁場(chǎng)處于穩(wěn)定狀態(tài),表示此時(shí)的地磁信息可以用來(lái)計(jì)算得到航向角信息,當(dāng)行人處于無(wú)主方向進(jìn)行約束時(shí),可以根據(jù)地磁信息計(jì)算航向角并通過(guò)卡爾曼濾波器對(duì)行人航向進(jìn)行修正。

2.5 自適應(yīng)航向角漂移修正算法

通過(guò)提取磁場(chǎng)數(shù)據(jù)中的可用信息,可以對(duì)航向角漂移進(jìn)行抑制。但是當(dāng)環(huán)境干擾很大不存在穩(wěn)定狀態(tài)磁場(chǎng)時(shí),使用磁力計(jì)的數(shù)據(jù)會(huì)帶來(lái)額外的偏差,需要其他的方法來(lái)獲取正確的航向信息。

傳統(tǒng)的HDE 算法,需要先驗(yàn)知識(shí)預(yù)知建筑物以及環(huán)境的主方向,在陌生環(huán)境中無(wú)法工作。本文對(duì)HDE算法進(jìn)行改進(jìn),對(duì)直行和轉(zhuǎn)彎行走的檢測(cè)嚴(yán)格限制,提出了一種實(shí)時(shí)建立主方向的a HDE 算法,通過(guò)相鄰四步的步幅航向確定行人是否直行,直行的情況下實(shí)時(shí)建立主方向,根據(jù)建立的主方向庫(kù)完成對(duì)后續(xù)航向角誤差的修正,該方法不需要提前預(yù)知建筑物或者環(huán)境的主方向,可以根據(jù)行人的行走狀態(tài),實(shí)時(shí)建立主方向庫(kù),在后續(xù)直行時(shí)對(duì)航向角誤差進(jìn)行約束。

1)步幅航向

當(dāng)IMU 固定于行人右腳時(shí),定義右腳離開(kāi)地面、接觸地面到再次離開(kāi)地面為一個(gè)復(fù)步,步幅航向?yàn)檫B續(xù)兩步之間的角度[17]。定義零速點(diǎn)為零速區(qū)間的初始時(shí)刻,在零速點(diǎn)計(jì)算步幅航向。設(shè)第k個(gè)零速點(diǎn)的采樣序列為s(k),此時(shí)東北方向位移分別為x(s(k))和y(s(k)),則第k步的步幅航向θs(k)如式(18)

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

設(shè)定步長(zhǎng)的閾值tth為0.3 m,若SSL(k)＜tth,則認(rèn)為此時(shí)的零速點(diǎn)為偽零速點(diǎn),應(yīng)該舍去,進(jìn)一步提高零速檢測(cè)的準(zhǔn)確度。

2)嚴(yán)格的直行轉(zhuǎn)彎?rùn)z測(cè)算法

使用a HDE 算法的前提是行人處于直行狀態(tài),因此正確檢測(cè)出直行或者轉(zhuǎn)彎是關(guān)鍵。行人行走可以分為直線行走和轉(zhuǎn)彎兩種,無(wú)論是小曲率半徑行走還是大曲率半徑行走都可以在有限步內(nèi)完成。本文基于歷史時(shí)刻的步幅航向提出了一種嚴(yán)格的直行和轉(zhuǎn)彎?rùn)z測(cè)方法。

假設(shè)此時(shí)為第k步,相鄰四步的步幅航向?yàn)棣萻(j),j∈(k-3,k),當(dāng)行人處于直行狀態(tài)時(shí),步幅航向應(yīng)該保持一致,利用歷史三步的步幅航向的差值進(jìn)行判斷。

如果Δφ＜tth1,則表示步幅航向變化不明顯,行人處于直線行走或者沿大曲率半徑曲線行走階段。此時(shí)單純利用步幅航向差值無(wú)法區(qū)分,分析發(fā)現(xiàn),當(dāng)行人沿直線行走時(shí),步幅航向在一個(gè)常值附近波動(dòng),當(dāng)行人沿曲線行走時(shí),相鄰幾步的步幅航向有相同的變化趨勢(shì),因此進(jìn)一步對(duì)直行和轉(zhuǎn)彎進(jìn)行限制。

首先,判斷行人是否處于轉(zhuǎn)彎狀態(tài)。

其中,tth1為步幅航向角度檢驗(yàn)的閾值。當(dāng)T5=0時(shí),表示行人處于轉(zhuǎn)彎狀態(tài)。定義相鄰兩步的步幅航向差值為Δθs,計(jì)算如下

當(dāng)轉(zhuǎn)彎時(shí),相鄰幾步的步幅航向有相同的變化趨勢(shì),因此,做以下判斷

設(shè)定直行標(biāo)識(shí)符為SSLP(k),則有

只有以上條件同時(shí)滿足時(shí),認(rèn)為行人處于直行狀態(tài)。

3)a HDE算法流程

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

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

首先對(duì)行人的運(yùn)動(dòng)狀態(tài)進(jìn)行檢測(cè),當(dāng)檢測(cè)到行人當(dāng)前狀態(tài)為直行時(shí),則遍歷主方向數(shù)據(jù)庫(kù),如果主方向數(shù)量為0,直接建立新的主方向θnew,新的主方向θnew計(jì)算如式(25)所示

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

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

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

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

為了減少行人在非主方向上直行造成的誤修正,使用自適應(yīng)量測(cè)噪聲,觀測(cè)量δφ(k)的量測(cè)噪聲標(biāo)準(zhǔn)差如式(27)所示

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

2.6 融合濾波更新

卡爾曼濾波的狀態(tài)方程與量測(cè)方程分別為

其中,Xk為系統(tǒng)的狀態(tài)變量,具體實(shí)現(xiàn)在前文已經(jīng)提及;Φk/k-1為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣;Qk-1為系統(tǒng)白噪聲;Zk為觀測(cè)量;Hk為量測(cè)矩陣;Wk表示傳感器數(shù)據(jù)的噪聲。具體的更新過(guò)程如文獻(xiàn)[18]和[19]所示。

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

則對(duì)應(yīng)的量測(cè)矩陣的設(shè)置如式(32)所示。

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

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

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

實(shí)驗(yàn)所使用的傳感器型號(hào)為MTI-300,采樣頻率為100 Hz,實(shí)驗(yàn)時(shí)傳感器固定于實(shí)驗(yàn)人員右腳。傳感器的參數(shù)如表1所示。

表1 MTI-300性能參數(shù)Tab.1 MTI-300 performance parameters

3.1 室外實(shí)驗(yàn)

實(shí)驗(yàn)地點(diǎn)為自動(dòng)化學(xué)院籃球場(chǎng),實(shí)驗(yàn)人員從起點(diǎn)出發(fā),繞籃球場(chǎng)周圍道路行走,終點(diǎn)位置與起點(diǎn)位置相同,使用GPS得到行人運(yùn)動(dòng)的參考軌跡。實(shí)驗(yàn)路徑長(zhǎng)約660 m,包含8條直行路徑。參考軌跡如圖6所示。

圖6 室外復(fù)雜環(huán)境參考軌跡圖Fig.6 Outdoor complex environment reference trajectory diagram

分別使用IEZ、IEZ+ZARU、IEZ+ZARU+QSF和本文的算法對(duì)實(shí)驗(yàn)人員的導(dǎo)航信息進(jìn)行求取,得到的軌跡如圖7所示。

圖7 室外復(fù)雜環(huán)境實(shí)驗(yàn)結(jié)果圖Fig.7 Experimental results of outdoor complex environment

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

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

本文算法實(shí)時(shí)建立的8個(gè)主方向與理論上的8個(gè)主方向?qū)Ρ热绫?所示。

表3 實(shí)時(shí)建立主方向與理論主方向?qū)Ρ萒ab.3 Comparison of the main direction established in real time with the main direction of theory

根據(jù)表2和表3的數(shù)據(jù)可知,行人航向?qū)π腥硕ㄎ徽`差的影響很大,提高行人航向的準(zhǔn)確度可以有效提升行人定位精度。本文提出的算法在終點(diǎn)誤差和定位誤差方面優(yōu)于其他的方法,可以對(duì)行人的航向角誤差進(jìn)行有效修正,得到的行人軌跡與參考軌跡重合度高,更加符合真實(shí)軌跡。在660 m 的實(shí)驗(yàn)中,本文算法得到終點(diǎn)誤差為4.545 4 m,實(shí)驗(yàn)平均誤差為1.747 3 m,相比較沒(méi)有航向角修正的ZUPT,ZARU 算法和單獨(dú)使用磁力計(jì)數(shù)據(jù)進(jìn)行修正的QSF 算法,本文方法的定位誤差分別降低了91.02%,82.93%和61.39%,并且可以實(shí)時(shí)建立的主方向完成對(duì)航向角的修正,定位精度符合行人定位的需求。

3.2 室內(nèi)定位實(shí)驗(yàn)

實(shí)驗(yàn)地點(diǎn)為自動(dòng)化學(xué)院院辦樓,實(shí)驗(yàn)人員從起點(diǎn)出發(fā),繞教學(xué)樓走廊行走兩圈,終點(diǎn)位置與起點(diǎn)位置相同,使用激光測(cè)距儀得到建筑物的輪廓信息,實(shí)驗(yàn)路徑長(zhǎng)約320 m。傳感器的安裝及實(shí)驗(yàn)環(huán)境如圖8所示。

圖8 傳感器安裝及室內(nèi)實(shí)驗(yàn)環(huán)境Fig.8 Sensor installation and indoor experimental environment

通過(guò)對(duì)室內(nèi)磁力計(jì)采集得到的數(shù)據(jù)進(jìn)行分析,發(fā)現(xiàn)室內(nèi)磁場(chǎng)比較紊亂,實(shí)驗(yàn)中發(fā)現(xiàn)存在的穩(wěn)定狀態(tài)磁場(chǎng)數(shù)據(jù)較少。磁力計(jì)采集得到的室內(nèi)數(shù)據(jù)和室外數(shù)據(jù)經(jīng)過(guò)校正后的磁場(chǎng)強(qiáng)度對(duì)比如圖9(a)和(b)所示。

圖9 室內(nèi)外實(shí)驗(yàn)磁場(chǎng)強(qiáng)度對(duì)比圖Fig.9 Comparison of indoor and outdoor experimental magnetic field intensity

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

圖10 室內(nèi)環(huán)境兩種算法實(shí)驗(yàn)結(jié)果圖Fig.10 Experimental results of two algorithms in indoor environment

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

4 結(jié)論

本文基于IMU 和磁力計(jì)這兩種傳感器進(jìn)行實(shí)驗(yàn),提出了改進(jìn)的穩(wěn)定狀態(tài)磁場(chǎng)檢驗(yàn)方法和自適應(yīng)航向角漂移修正算法,使用兩種方法協(xié)同完成行人航向角的估計(jì)和修正。解決了行人定位無(wú)航向角修正和避免了單一使用磁力計(jì)由于磁場(chǎng)失真嚴(yán)重?zé)o法進(jìn)行航向角修正的問(wèn)題。通過(guò)實(shí)驗(yàn)得出以下結(jié)論:

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

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