曹 峻，余科根，文 鎧

(1.中國(guó)礦業(yè)大學(xué)環(huán)境與測(cè)繪學(xué)院，徐州 221116； 2.武漢大學(xué)測(cè)繪學(xué)院，武漢 430072)

0 引言

基于位置的服務(wù)(Location Based Services，LBS)為個(gè)人定位和導(dǎo)航等帶來(lái)了極大的便利。智能手機(jī)內(nèi)的慣性測(cè)量單元(Inertial Measurement Unit，IMU)可以為不同環(huán)境下的行人跟蹤提供自主解決方案[1]。由于智能手機(jī)內(nèi)置微機(jī)電系統(tǒng)的IMU通常精度較差，因此，使用行人航位推算(Pedestrian Dead Reckoning，PDR)方法生成磁場(chǎng)序列的輪廓特征。

地球磁場(chǎng)由于信息量豐富、可以實(shí)時(shí)方便匹配、精度高、穩(wěn)定性好、成本低等，適合作為一種位置指紋來(lái)使用?；诖艌?chǎng)的室內(nèi)定位存在以下問(wèn)題[2]：1)目前使用的指紋采集方法普遍無(wú)法滿足快速建庫(kù)的要求，需要一種高效、可靠的室內(nèi)磁性指紋數(shù)據(jù)庫(kù)構(gòu)建方法；2)與WIFI定位和藍(lán)牙定位相比，地磁定位中由于智能移動(dòng)設(shè)備的任意姿態(tài)，可用的磁場(chǎng)特征從原來(lái)的三維特征變?yōu)橐痪S特征，匹配特征較少，如何快速進(jìn)行匹配操作是研究難點(diǎn)。國(guó)內(nèi)外基于慣性導(dǎo)航和地磁信息的室內(nèi)定位技術(shù)研究中，文獻(xiàn)[3]提出了基于磁約束的IMU定位算法,采用多級(jí)傅立葉變換方法提取特征，降低了軌跡累積誤差且提高了精度；文獻(xiàn)[4]則利用擴(kuò)展Kalman濾波器(Extended Kalman Filter，EKF)融合不同類型的數(shù)據(jù)，提出了精確區(qū)分姿態(tài)相位的聚類算法，采用零速更新方法最小化積分誤差。

本文IMU的輸出可用于設(shè)置慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)機(jī)械編排并檢測(cè)運(yùn)動(dòng)步數(shù)，如果步伐檢測(cè)成功，則執(zhí)行步長(zhǎng)估計(jì)來(lái)計(jì)算當(dāng)前步長(zhǎng)和行人的步行速度?；贗NS的PDR通過(guò)分別積分來(lái)自陀螺儀和加速度計(jì)的角速度和比力計(jì)算用戶的位置[5]。該方法使用估計(jì)的步長(zhǎng)和估算的航向產(chǎn)生移動(dòng)速度更新和位置增量更新，以改善PDR導(dǎo)航性能。前進(jìn)速度作為INS的速度更新，而位置增量則作為INS的位置更新，以改進(jìn)位置和航向精度。磁力計(jì)的測(cè)量結(jié)果與來(lái)自INS的軌跡輪廓相結(jié)合，被輸入到數(shù)據(jù)緩沖區(qū)中以設(shè)置磁場(chǎng)序列(Magnetic Field Sequence，MFS)[6]。如果磁場(chǎng)數(shù)據(jù)的緩沖長(zhǎng)度超過(guò)預(yù)設(shè)閾值，則對(duì)測(cè)量MFS進(jìn)行重新采樣，并使用高斯牛頓迭代法在磁場(chǎng)圖中找到與測(cè)量MFS具有最高相似性的參考MFS；如果地磁匹配定位算法成功，則參考MFS的位置將用作位置更新，以控制INS的導(dǎo)航誤差并提高軌跡輪廓的準(zhǔn)確性[7]。本文提出的C-INS方法可以很好地解決基于智能手機(jī)的PDR中未對(duì)準(zhǔn)角度估計(jì)的問(wèn)題。

1 捷聯(lián)慣性導(dǎo)航系統(tǒng)力學(xué)編排

1.1 捷聯(lián)慣性導(dǎo)航系統(tǒng)流程

通過(guò)載體坐標(biāo)系(簡(jiǎn)稱b系)和導(dǎo)航坐標(biāo)系(簡(jiǎn)稱n系)描述載體的運(yùn)動(dòng)狀態(tài)。在進(jìn)入解算前，先確立n系和b系的對(duì)應(yīng)關(guān)系，實(shí)現(xiàn)載體運(yùn)動(dòng)狀態(tài)從b系向n系的轉(zhuǎn)換。在初始對(duì)準(zhǔn)中，由于MEMS-IMU內(nèi)置移動(dòng)設(shè)備的陀螺精度較低，因此利用加速度計(jì)的輸出計(jì)算俯仰角和橫滾角，磁力計(jì)輸出的磁場(chǎng)強(qiáng)度計(jì)算航向角[8]。IMU的漂移誤差模型可采用一階馬爾科夫誤差模型，修正b系下的陀螺儀和加速度計(jì)的原始誤差[9]，測(cè)得載體相對(duì)慣性空間的角速度和比力數(shù)據(jù)；再通過(guò)解算常微分方程組，對(duì)加速度計(jì)和陀螺儀提供的加速度和角速度數(shù)據(jù)進(jìn)行多次積分,根據(jù)n系下的速度和時(shí)間間隔累計(jì)求得運(yùn)動(dòng)載體在n系中的姿態(tài)、位置及速度信息。

1.2 捷聯(lián)慣性導(dǎo)航系統(tǒng)力學(xué)編排

(1)

式中

(2)

由式(2)可以看出，當(dāng)姿態(tài)角已知并給定初值后即可求解?？紤]到載體的姿態(tài)變化快，利用四元數(shù)法表示b系相對(duì)n系的轉(zhuǎn)動(dòng)，進(jìn)行姿態(tài)解算

Q=q0+q1ib+q2jb+q3kb

(3)

式中，ib、jb、kb與b系的基底相同。

其微分方程為

(4)

(5)

當(dāng)載體和地球有相對(duì)運(yùn)動(dòng)時(shí)，加速度計(jì)計(jì)算的比力方程如下

(6)

式中，f為加速度計(jì)的數(shù)據(jù)值；ven表示載體和地球間的相對(duì)運(yùn)動(dòng)速度；wen×ven表示載體在地球表面運(yùn)動(dòng)時(shí)形成的向心加速度； 2wie×ven表示哥式加速度的數(shù)值。

因加速計(jì)測(cè)量值實(shí)際是沿b系的，所以要轉(zhuǎn)換到n系下[11]。設(shè)加速度計(jì)的輸出比力為

fb=[fx,fy,fz]T

(7)

則

(8)

(9)

(10)

導(dǎo)航坐標(biāo)系(即n幀)中離散INS機(jī)械編排算法方程的簡(jiǎn)化形式為

(11)

2 C-INS算法和EKF模型

2.1 C-INS算法設(shè)計(jì)

為了滿足行人導(dǎo)航應(yīng)用的需要，將步態(tài)模型中的前向行走速度和運(yùn)動(dòng)約束作為三維速度更新和二維位置增量應(yīng)用，以提高慣性導(dǎo)航系統(tǒng)的速度[12]，稱為C-INS。

n幀中速度觀測(cè)方程由式(12)給出

(12)

(13)

(14)

(15)

航向角即為

(16)

式中

c32φy)+b13(c13+c23φZ(yǔ)-c33φy)

(17)

c12φZ(yǔ))+b13(c23+c33φX-c13φZ(yǔ))

(18)

n幀中的航向觀測(cè)方程由式(19)給出

(19)

(20)

(21)

(22)

2.2 EKF系統(tǒng)模型和多級(jí)測(cè)量方程

使用基于INS的PDR具有更高的輸出頻率，可以提供相對(duì)軌跡輪廓替代方案，以改善磁場(chǎng)指紋的辨別力。陀螺儀和加速度計(jì)的測(cè)量值積分后會(huì)導(dǎo)致位置誤差，為此，基于IMU的誤差模型，在n系中利用速度誤差矢量、姿態(tài)誤差角矢量以及位置誤差矢量，使用EKF融合多源觀測(cè)量進(jìn)行濾波和狀態(tài)估計(jì),繼而修正系統(tǒng)誤差。所提出的方法中狀態(tài)變量定義如下[13]

(23)

δxk|k-1=φk-1δxk-1|k-1+wkδzk=Hkδxk|k-1+vk

(24)

式中，δxk-1|k-1、δxk|k-1代表先前的誤差狀態(tài)向量和預(yù)測(cè)的誤差狀態(tài)向量，下標(biāo)k-1和k代表時(shí)期；δzk代表測(cè)量誤差矢量；wk代表過(guò)程噪聲；Hk代表設(shè)計(jì)矩陣；vk代表測(cè)量噪聲；φk-1是15×15的量測(cè)矩陣，表示為

(25)

2.3 磁場(chǎng)匹配定位

1)離線階段

為確保磁力計(jì)的輸出真實(shí)反映周圍環(huán)境的磁場(chǎng)，必須在使用磁力計(jì)感知磁場(chǎng)之前,利用開(kāi)發(fā)的安卓應(yīng)用程序執(zhí)行傳感器校準(zhǔn)過(guò)程。在離線數(shù)據(jù)收集階段，使用INS機(jī)械編排產(chǎn)生的橫滾角和俯仰角獲得磁場(chǎng)強(qiáng)度的水平和垂直分量[14]。已知點(diǎn)(Known Points，KPs)的坐標(biāo)和在KPs處的磁場(chǎng)觀測(cè)是影響磁場(chǎng)圖精度的2個(gè)主要因素。為了平衡制圖精度、效率和人工成本，利用步行測(cè)量方法(WS)沿著與房間的x軸平行的線或與y軸平行的線收集磁場(chǎng)數(shù)據(jù)，以提高構(gòu)建磁場(chǎng)圖的工作效率[15]。使用由許多直線組成的路徑以確保KPs的測(cè)量密度，同時(shí)利用稀疏的具有已知坐標(biāo)的點(diǎn)和恒定的行走速度進(jìn)行約束，記下經(jīng)過(guò)每個(gè)指定點(diǎn)的時(shí)間，并在測(cè)試期間將它們寫入文件，此外，在高精度地圖上選擇相應(yīng)的點(diǎn)來(lái)獲得上述點(diǎn)的坐標(biāo)。最后，通過(guò)時(shí)間線性插值計(jì)算出相鄰指定點(diǎn)之間的KPs坐標(biāo)，生成分辨率為0.2m的柵格磁場(chǎng)圖。其中，Gi代表拐角點(diǎn)或支柱點(diǎn)，KPi代表KP位置，F(xiàn)Mi={Pi，Mi}代表KP的磁場(chǎng)指紋。G1和G2之間的KPi坐標(biāo)計(jì)算公式如下，其中T表示經(jīng)過(guò)路點(diǎn)的時(shí)間。

(26)

2) 在線定位階段

在線匹配時(shí)，最多3個(gè)分量不足以實(shí)現(xiàn)磁場(chǎng)指紋的精確匹配定位，使用歷史磁場(chǎng)數(shù)據(jù)和相應(yīng)的相對(duì)坐標(biāo)來(lái)構(gòu)造MFS，如下

(27)

式中，k代表磁場(chǎng)數(shù)據(jù)的長(zhǎng)度，而i是MFS的紀(jì)元。使用高斯牛頓迭代法執(zhí)行磁場(chǎng)輪廓匹配定位[16]，該算法不需要在磁場(chǎng)圖中遍歷一個(gè)固定的區(qū)域，可以根據(jù)磁場(chǎng)梯度快速找到最優(yōu)的轉(zhuǎn)換參數(shù)。

假定剛性變換為

ε=[px0,py0,Δφ]T

(28)

(29)

地磁匹配定位算法在n幀中的定位結(jié)果的誤差由式(30)給出

(30)

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

3.1 地磁匹配定位算法實(shí)驗(yàn)結(jié)果

為了評(píng)估所提出的室內(nèi)定位算法的性能，實(shí)驗(yàn)人員在武漢大學(xué)珞珈創(chuàng)意園16樓遵循預(yù)定軌跡進(jìn)行了實(shí)驗(yàn)。軌跡位于典型的室內(nèi)辦公樓環(huán)境(約300m2)中，走廊狹窄。實(shí)驗(yàn)人員手持小米8智能手機(jī)按標(biāo)紅路線走動(dòng)，并將智能手機(jī)保持在距離地面約1.2m的高度。智能手機(jī)的前進(jìn)方向和參與者的步行方向之間角度差小于10°。

1)磁場(chǎng)圖

利用磁場(chǎng)序列與軌跡相結(jié)合的方法，構(gòu)造了一種新的磁場(chǎng)特征以實(shí)現(xiàn)更好的磁場(chǎng)匹配。智能手機(jī)的內(nèi)置磁力計(jì)具有很高的采樣率，并且行人的正常步行速度通常約為1.2m/s。所以在在線定位階段可以用較低的計(jì)算成本，重新采樣所收集的磁場(chǎng)數(shù)據(jù)以生成稀疏的MFS。在本文中，使用長(zhǎng)度為10m的MFS進(jìn)行位置估計(jì)。如圖1所示，磁場(chǎng)強(qiáng)度在0.2m范圍內(nèi)的變化可以忽略不計(jì)。與動(dòng)態(tài)時(shí)間規(guī)整(Dynamic Time Warping，DTW)[8，13]算法相比，該算法使用了網(wǎng)格磁場(chǎng)圖，因此對(duì)用戶行走軌跡不敏感，而且結(jié)合來(lái)自PDR的軌跡輪廓具有更高的磁場(chǎng)指紋識(shí)別能力。該算法與基于粒子濾波(Particle Filter，PF)的定位算法精度接近，但計(jì)算復(fù)雜度低、收斂速度快。

圖1 磁場(chǎng)強(qiáng)度基準(zhǔn)圖Fig.1 Reference diagram of magnetic field intensity

2)位置估算效果分析

在試驗(yàn)區(qū)域進(jìn)行了實(shí)驗(yàn)軌跡測(cè)試，位置估計(jì)結(jié)果如圖2所示，累積誤差百分比(Cumulative Error Percentages，CEPs)如圖3所示。圖2中，紅色、藍(lán)色、綠色和紫色實(shí)線分別表示參考軌跡、INS、MM和MM/INS的位置軌跡。對(duì)于CEP圖形，天藍(lán)色、綠色和藍(lán)色線分別表示INS、MM和MM/INS。MM和MM/INS集成方法通過(guò)使用基于INS的PDR的軌跡輪廓改善MFS的識(shí)別。

圖2 位置估計(jì)結(jié)果Fig.2 Result of position estimation

圖3 累積誤差百分比(CEPs)Fig.3 Cumulative error percentage (CEPs)

所提出的MM算法可以在走廊環(huán)境中實(shí)現(xiàn)高精度的位置估計(jì)，測(cè)試的INS、MM、MM/INS集成方法的位置誤差如表1所示?；贛EMS-IMU的INS解決方案的均方根(Root Mean Square,RMS)位置誤差為3.65m。

表1 測(cè)試實(shí)驗(yàn)中的位置誤差

所提出的基于磁場(chǎng)指紋的MM解決方案在測(cè)試中的RMS位置誤差為1.64m，而匹配成功率為95.19%。匹配成功率指的是成功匹配的總數(shù)除以匹配的總數(shù)。集成解決方案的RMS位置誤差為1.52m。根據(jù)測(cè)試的結(jié)果，提出的MM算法具有良好的位置估計(jì)性能和較高的匹配成功率。

3.2 C-INS實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)環(huán)節(jié)設(shè)計(jì)了步行測(cè)試，并分析了所提出的C-INS算法與現(xiàn)有的E-PDR算法在手持設(shè)備、打電話模式和放在褲子口袋中3個(gè)基本位置的位置估計(jì)和航向估計(jì)方面的性能。

1)C-INS和E-PDR方法之間的航向估計(jì)

表2說(shuō)明了這兩種算法在3個(gè)位置具有一致的統(tǒng)計(jì)航向性能。E-PDR方法的RMS航向誤差保持位置分別為3.43°、6.22°和17.65°，而C-INS方法的保持位置分別為3.74°、6.52°和16.86°。C-INS方法因使用步態(tài)模型獲得了更好的航向估計(jì)。圖4所示為相應(yīng)的航向累積誤差百分比圖。

表2 E-PDR和C-INS在不同位置的航向角誤差

2)C-INS和E-PDR方法之間的位置估計(jì)

圖5和圖6顯示了C-INS和E-PDR解決方案實(shí)驗(yàn)中，3個(gè)位置的估計(jì)結(jié)果和位置累積誤差百分比。紅色、綠色和藍(lán)色實(shí)線分別表示參考軌跡、C-INS方法的軌跡和E-PDR方法的軌跡。E-PDR方法在不同位置的統(tǒng)計(jì)定位誤差分別為2.15m、1.94m和1.73m，C-INS方法的統(tǒng)計(jì)定位誤差分別為1.15m、1.84m和1.73m，如表3所示。因C-INS方法使用步態(tài)模型來(lái)提高滾轉(zhuǎn)角和俯仰角的精度，故在位置估計(jì)方面表現(xiàn)出了更好的性能。

(a)手持模式

(b)電話模式

(c)口袋模式圖4 航向角累積誤差百分比Fig.4 Cumulative error percentage of heading

表3 E-PDR和C-INS解決方案在3個(gè)不同 電話保持位置的定位誤差

(a)手持模式

(b) 電話模式

(c) 口袋模式圖5 E-PDR和C-INS在3個(gè)位置的估計(jì)結(jié)果Fig.5 Estimation results of two methods in three positions

(a)手持模式

(b) 電話模式

(c) 口袋模式圖6 E-PDR和C-INS在3個(gè)位置的累積誤差百分比Fig.6 Cumulative error percentage of E-PDR and C-INS in three positions

4 結(jié)論

1)基于環(huán)境磁場(chǎng)圖的匹配定位方法，提高了磁場(chǎng)匹配定位的位置估計(jì)精度。將磁場(chǎng)序列與來(lái)自基于INS的PDR的估計(jì)軌跡結(jié)合使用，提高了磁場(chǎng)指紋的可分辨性，并設(shè)計(jì)了高斯牛頓迭代法進(jìn)行磁場(chǎng)輪廓匹配定位。

2)基于智能手機(jī)內(nèi)置傳感器，利用步態(tài)模型和運(yùn)動(dòng)約束提供偽測(cè)量的C-INS方法，提供了更高頻率的輸出、更豐富的三維速度信息，以及更可靠的位置估計(jì)和航向估計(jì)。實(shí)際的步行測(cè)試證明了利用C-INS方法可以很好地解決基于智能手機(jī)的PDR中未對(duì)準(zhǔn)角度估計(jì)的問(wèn)題。