胡 倩，張文超，魏東巖，袁 洪

(1. 中國科學院大學微電子學院，北京 100049； 2. 中國科學院空天信息創(chuàng)新研究院，北京 100094)

0 引言

室內(nèi)位置服務(wù)在人們?nèi)粘Ｉ詈凸ぷ髦邪l(fā)揮著重要作用。雖然全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Naviga-tion Satellite System, GNSS)可以提供比較可靠的室外定位導航服務(wù)，但不適用于室內(nèi)環(huán)境[1-3]。為了解決該問題，目前研究人員已經(jīng)提出許多室內(nèi)定位解決方案，如激光、超聲波定位技術(shù)、射頻識別技術(shù)和基于超寬帶的定位技術(shù)等，但是這些技術(shù)需要較大的前期基礎(chǔ)設(shè)施投入，人力和時間成本較高。與以上幾種定位方法不同，行人航跡推算(Pedestrian Dead Reckoning，PDR)技術(shù)僅依賴慣性傳感器，受外界干擾小，在復(fù)雜室內(nèi)環(huán)境下，短時間內(nèi)可以達到較好的定位效果。但是慣性導航解算算法屬于一種遞推式導航方法，通過連續(xù)測得載體的角度變化和加速度來計算其導航信息，大多數(shù)便攜式移動設(shè)備內(nèi)置的慣性傳感器工藝有限，且精度不高[4]，導致PDR的誤差隨時間迅速增長，定位結(jié)果變差。

為了減少這種累積誤差，通常需要融合其他定位方法，常用的方法是用WiFi數(shù)據(jù)輔助修正軌跡，如文獻[5]和文獻[6]分別采用極大似然和擴展卡爾曼濾波(Extended Kalman Filter，EKF)算法融合WiFi數(shù)據(jù)和PDR結(jié)果，但是該方法通常需要預(yù)先采集WiFi信號的強度數(shù)據(jù)。若使用室內(nèi)地圖信息參與導航解算過程，算法便捷且成本較低。從慣性傳感器數(shù)據(jù)中可以檢測出行人在室內(nèi)的移動行為信息，將其與地圖信息進行匹配，可以約束行人的運動軌跡，從而提高定位精度及穩(wěn)定性。

在地圖輔助定位相關(guān)研究中，文獻[7]基于條件隨機場融合室內(nèi)地圖信息和PDR定位結(jié)果，提出了一種輕量級的地圖匹配技術(shù)，采用維特比算法得到最終定位結(jié)果，但定位精度有待提升；文獻[8]提出了一種結(jié)合地圖信息的步數(shù)計算算法，可校準步距和方向，提高了步長算法的性能；文獻[9]提出了采用卡爾曼濾波融合PDR和室內(nèi)地標點的算法框架，同時為PDR算法提供新起點，以校正整個系統(tǒng)的累積誤差；文獻[10]通過對加速度計數(shù)據(jù)進行分析處理，識別站立、行走、上下樓梯和乘電梯等行為，并與相應(yīng)的地標進行匹配，對室內(nèi)行人位置進行校正，但存在較大誤匹配問題；文獻[11]基于地圖信息檢測室內(nèi)特殊位置，并將其定義為路網(wǎng)節(jié)點對PDR的累積誤差進行校正，但只是點位置校正；文獻[12]設(shè)計了一種級聯(lián)結(jié)構(gòu)的卡爾曼粒子濾波算法，將傳感器信息和室內(nèi)地圖信息融合，以約束行人軌跡發(fā)散問題，但是結(jié)構(gòu)復(fù)雜，計算量較大。

本文在分析上述問題的基礎(chǔ)上，通過研究行人運動和室內(nèi)地圖的特點，提出了一種地圖輔助的室內(nèi)定位方法。該方法采用EKF融合量測數(shù)據(jù)和PDR的定位結(jié)果，并結(jié)合地圖信息以優(yōu)化室內(nèi)行人運動軌跡。主要貢獻包括：提出了一種多地標匹配方法，能夠有效提高地標匹配的準確率；使用通行路徑和墻面信息輔助約束定位點航向和位置；采用EKF融合地圖校正點和PDR的定位結(jié)果。

1 地圖輔助的融合定位方案

本文采用卡爾曼濾波算法融合PDR和地圖校正點。圖1所示為融合定位系統(tǒng)結(jié)構(gòu)框圖。在PDR解算過程中，陀螺儀的測量值用于計算導航坐標系和載體坐標系之間的姿態(tài)轉(zhuǎn)換矩陣，加速度計測量值通過該姿態(tài)轉(zhuǎn)換矩陣投影到導航坐標系，然后經(jīng)過對時間的一次積分得到載體在導航坐標系中的速度，二次積分即可得到位置。但是，由于低成本微機電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)慣性器件(特別是陀螺儀)測量誤差隨時間增長迅速，導航誤差在短時間內(nèi)迅速累積。因此，引入地標信息進行位置修正，將PDR解算結(jié)果視為狀態(tài)量，地標信息視為量測量，更新解算結(jié)果。針對航向發(fā)散問題，通過檢測行人是否位于走廊等區(qū)域，判斷軌跡是否穿墻，采用地圖信息對定位結(jié)果進行約束。

圖1 融合定位系統(tǒng)結(jié)構(gòu)框圖Fig.1 Block diagram of fusion positioning system

2 基于地標檢測的位置約束算法

所謂地標點，即為傳感器讀數(shù)呈現(xiàn)獨特、穩(wěn)定、可識別變化模式的定位點。對于一個地標點，在數(shù)學上做如下定義

LP={(x,y,z),(R1,…,Rm)}

(1)

式中，(x,y,z)為地標點的三維坐標；(R1,…,Rm)為不同類型傳感器讀數(shù)下的檢測規(guī)則，一類地標點可以有多種檢測規(guī)則。

2.1 地標點檢測

傳感器是地標點檢測的基礎(chǔ)，不同傳感器輸出的數(shù)據(jù)類型、內(nèi)容和數(shù)據(jù)特征均不同。對傳感器數(shù)據(jù)進行特征提取，可定義不同的地標識別規(guī)則。行人的運動狀態(tài)在室內(nèi)環(huán)境下發(fā)生變化，可由加速度計幅值變化感知。如圖2所示，行人在經(jīng)過門這一地標時，由于存在推門動作，運動狀態(tài)會出現(xiàn)由動到靜再到動的狀態(tài)改變特征，提取合加速度acc的方差用于區(qū)分行人的靜態(tài)和動態(tài)，計算公式如下

(2)

(3)

其中，T為檢測窗口長度；n為數(shù)據(jù)長度。定義門的識別規(guī)則

Rdoor=(LPt|at-t1:t>aσ2&&at:t+t2

&&at+t2:t+t1+t2>aσ2)

(4)

t1和t2分別為動態(tài)和靜態(tài)的檢測時長，若某一時刻t滿足這一地標識別規(guī)則，則認為是一個可能的門地標點。

圖2 經(jīng)過門時加速度計幅值變化Fig.2 The change in the amplitude of acceleration when a user passes through a door

行人在轉(zhuǎn)彎或是經(jīng)過墻角時，行走方向會發(fā)生大幅度改變，這種變化特征可由陀螺儀數(shù)據(jù)幅值變化檢測出來。由于實驗采樣頻率較高易誤檢，故在不損失數(shù)據(jù)特征的條件下對數(shù)據(jù)進行降頻處理，將每10個數(shù)據(jù)點劃分為一組，取其均值作為當前陀螺儀數(shù)據(jù)。為提高檢測準確率，采用符號函數(shù)判定是否連續(xù)角度改變，定義轉(zhuǎn)彎點的識別規(guī)則

Rcor=

(5)

其中，θzi為某時刻陀螺儀z軸讀數(shù)；εgyro為轉(zhuǎn)彎閾值；sgn(θzi-εgyro)為符號函數(shù)，定義如下

(6)

若某時刻陀螺儀數(shù)據(jù)滿足式(5)，表示行人當前運動狀態(tài)是連續(xù)的航向改變過程，而不是偶然的航向改變，則認為當前位置點是一個可能的轉(zhuǎn)彎地標點，可用于進一步地標匹配。轉(zhuǎn)彎時陀螺儀z軸幅值變化如圖3所示。

圖3 轉(zhuǎn)彎時陀螺儀z軸幅值變化Fig.3 The change in the gyroscope readings on the z-axis when a user takes a turn

樓梯或者電梯地標可通過氣壓計數(shù)據(jù)來識別。圖4所示為上下樓梯時氣壓計幅值變化。為避免誤檢，在一次樓梯運動期間只匹配樓梯入口位置，當行人位于樓梯入口時，運動狀態(tài)由水平移動變?yōu)榇怪币苿?，定義樓梯入口的識別規(guī)則[13]

Rsta=(LPt|(|pi-pi-1|)<εbar_lev

&& |pi+Kp-pi|)>εbar_ver)

(7)

其中，pi表示以t時刻為中心檢測窗口內(nèi)的氣壓均值；εbar_lev表示行人水平移動的閾值；εbar_ver表示行人垂直運動的閾值；Kp為一個動態(tài)變化的量，初值可設(shè)為1，若符號函數(shù)值不變，表示當前行人運動狀態(tài)不變，Kp值逐漸增大。

圖4 上下樓梯時氣壓計幅值變化Fig.4 The changes in air pressure when a user takes stairs or an elevator

2.2 地標匹配

地標匹配是指根據(jù)約定好的行為判定規(guī)則，當數(shù)據(jù)特征滿足某類地標的特征時，判斷當前的地標類型，得到該地標點坐標，再與PDR定位點進行融合計算。對實驗樓層存在的所有地標點建庫，并存儲對應(yīng)的地標類型、地標序號和三維位置坐標。

使用地標輔助室內(nèi)行人定位的一個關(guān)鍵挑戰(zhàn)在于數(shù)據(jù)的關(guān)聯(lián)，即當前位置附近存在多個地標時，很難確定檢測到的地標與地標庫的匹配情況。為了解決該問題，本文提出了設(shè)置置信度Val的方法，以表示當前地標點與地標庫的匹配程度。Val的計算方法如下

Val(LPk)=δ(Rk,Rt)·(ωθ·h(θk,θt)+

ωd·d(lk,lt))

(8)

其中，k為地標索引；Rk為地標庫中參考地標LPk的地標類型；Rt為t時刻檢測到的地標類型；θk和θt分別為t時刻PDR定位點與參考地標的參考航向和PDR前后定位點的估計航向；lk和lt分別為t時刻參考地標位置和PDR估計位置；ωθ和ωd分別表示航向因素和距離因素在置信度中的權(quán)重系數(shù)。δ(Rk,Rt)為狄拉克函數(shù)，定義如下

(9)

h(θk,θt)為航向函數(shù)，定義如下

(10)

其中，εθ為航向閾值，實驗中取0.4rad；d(lk,lt)為距離函數(shù)，定義如下

(11)

其中，δl為距離閾值，實驗中取5m。當檢測點附近有多個可能地標時，選擇Val值最大的庫地標點作為當前地標點的匹配點，用于濾波更新，若最大Val值為0，則認為當前檢測地標為偽地標，將其舍棄。

3 基于地圖信息航向約束算法

采用地標約束行人定位軌跡能夠一定程度上優(yōu)化室內(nèi)定位結(jié)果，但仍存在航向發(fā)散和穿墻問題，需要結(jié)合室內(nèi)地圖信息來解決。室內(nèi)地圖數(shù)字化采用矢量法表示，矢量包括點、線和面，點表示室內(nèi)某點位置，用三軸坐標表示;線表示室內(nèi)的墻和通行路徑等，包括起終點坐標，本文選取走廊中線為通行路徑;面表示房間和走廊等區(qū)域。本文提出了兩種基于地圖信息的航向約束方法，這兩種方法均為平面位置校正，不涉及高度。

3.1 基于路徑信息的航向校正

在建筑物中的走廊等區(qū)域，行人通常沿直線行走，這種情況下，連續(xù)的步長向量大致平行，且與最近的可通行路徑角度平行。通過提取相鄰步長定位點，計算滑動窗口內(nèi)航向，連續(xù)步長參考航向可利用式(12)計算

(12)

若窗口內(nèi)連續(xù)步長參考航向和走廊參考航向差的最大值在閾值β范圍內(nèi)，如式(13)所示，但該時刻PDR估計位置偏離當前路徑，則朝向當前通行路徑校正PDR軌跡。

max(|θ(ki)|)<β

(13)

某一時刻定位點與附近可通行路徑的距離Dis可用式(15)計算[14]，(x0,y0)為當前PDR的定位坐標，(x1,y1)和(x2,y2)分別為該通行路徑的起終點坐標，則該通行路徑方程為

(y2-y1)x-(x2-x1)y+x2y1-x1y2=0

(14)

由點到直線距離公式可知

Dis=

(15)

選取Dis值最小的候選路徑作為匹配參考路徑，將當前定位點向匹配通行路徑中軸線做投影。校正點坐標(xp,yp)計算如下

(16)

3.2 基于墻面信息的航向校正

圖5 基于墻面信息的軌跡校正Fig.5 Trajectory correction based on wall information

若α小于角度閾值，即為小角度穿墻，將當前定位點反射至墻面前方的點(如圖5中點C)，N為該墻面的起點和終點構(gòu)成的向量

(17)

(18)

由墻面反射原理可知

(19)

即

=(δx,δy)

(20)

則校正點坐標為

(21)

若α大于角度閾值，即為大角度穿墻的情況。此時認為行人由門進入房間，采用地標匹配方法在地標庫中檢索距離當前定位點最近的門地標點，作為校正點以約束行人定位軌跡。

4 濾波融合更新

通過文中第2節(jié)和第3節(jié)提出的修正方法，可以獲得當前時刻用于校正PDR定位結(jié)果的三軸位置信息(包含地標點和地圖校正點)，然后采用EKF進行修正。選取前-右-下為載體坐標系(b系)，北-東-地為導航坐標系(n系)。卡爾曼濾波的狀態(tài)方程和量測方程如式(22)所示

(22)

其中，Xk為PDR解算結(jié)果，即三軸位置、速度和姿態(tài);Zk為地標點或者地圖校準點三軸位置;Φk|k-1為狀態(tài)轉(zhuǎn)移矩陣;Hk為量測矩陣;Wk-1和Vk分別為系統(tǒng)的過程噪聲和量測噪聲，均為不相關(guān)白噪聲?？柭鼮V波算法流程如圖6所示。

圖6 卡爾曼濾波過程Fig.6 Kalman filtering process

左側(cè)是系統(tǒng)預(yù)測回路，Q為系統(tǒng)過程噪聲Wk的協(xié)方差，按照卡爾曼濾波模型，首先建立狀態(tài)方程為

(23)

(24)

狀態(tài)轉(zhuǎn)移矩陣Φk|k-1為

(25)

Hk=[I3×303×303×3]

(26)

由預(yù)測回路得到的協(xié)方差矩陣P及量測矩陣Hk，可計算EKF濾波增益Kk如下

(27)

其中，R為量測噪聲Vk的協(xié)方差，可根據(jù)經(jīng)驗調(diào)整其值以達到最好的校正效果，進而更新當前狀態(tài)向量和協(xié)方差。只需使用當前的量測向量和上一時刻計算的狀態(tài)向量及其協(xié)方差，濾波算法即可遞歸運行。

5 實驗與分析

5.1 實驗場景

實驗地點為中國科學院光電研究院，設(shè)計測試路徑包含門、樓梯和轉(zhuǎn)彎點三類地標點，以及走廊等典型室內(nèi)場景，采用MapInfo軟件處理該實驗區(qū)域室內(nèi)地圖，建立規(guī)劃路徑上的地標點數(shù)據(jù)庫，并確立地標點類型、序號和三維位置之間的索引關(guān)系。實驗設(shè)備包括荷蘭Xsens公司生產(chǎn)的MTw Awinda系列慣性器件，用于采集行走過程中的加速度計、陀螺儀和氣壓計數(shù)據(jù)，表1所示為傳感器相關(guān)參數(shù)指標，采樣頻率100Hz。將器件固定于實驗人員腳面，如圖7所示，數(shù)據(jù)通過無線傳至電腦端接收軟件，實驗過程中假設(shè)行人已知其目的地，并沿最短路徑到達。

表1 MTw Awinda設(shè)備參數(shù)指標

圖7 慣性器件及穿戴方式Fig.7 Inertial device and wearing method

5.2 地標與路徑匹配結(jié)果

實驗開始時靜止10s，用于陀螺儀減零偏，航向和距離因素在置信度中的權(quán)重系數(shù)ωθ和ωd分別取0.4和0.6。表2所示為地標檢測和航向約束過程中的參數(shù)設(shè)置。

表2 算法參數(shù)設(shè)置

根據(jù)傳感器數(shù)據(jù)特征，檢測用戶行走過程中周圍地標信息，匹配成功后進行位置修正。在實驗設(shè)計路徑中，共用到8個地標點，其中4個門地標點，用字母d表示，2個轉(zhuǎn)彎地標點，用字母c表示，2個樓梯或電梯地標點，用字母s表示，數(shù)字表示各種類型地標點的序號。地標點分布情況如圖8所示，走廊部分的通行路徑取其中軸線，用粉色實線表示。

圖8 地標及路徑修正Fig.8 Landmark and path correction

一次試驗中，地標匹配置信度Val值如表3所示，匹配結(jié)果顯示，相較于傳統(tǒng)匹配方法，本文所提地標匹配方法采用地標類型、航向和距離三類限制條件，能夠有效地提高多地標情況下的匹配正確率。

表3 地標匹配結(jié)果

實驗中通行路徑設(shè)計較為簡單，不涉及最優(yōu)匹配路徑選擇問題，故Dis值最小候選路徑即為正確的參考路徑。

5.3 軌跡分析

基于同一組慣性數(shù)據(jù)，使用地圖信息賦值初始位置和航向，圖9所示為僅零速修正的PDR定位軌跡(藍色)和本文所提方法修正后的軌跡(紅色)。實驗人員從光電樓一層大廳出發(fā)，經(jīng)過門、走廊、電梯到達三層，再經(jīng)過走廊和樓梯回到一層出發(fā)點?？梢钥闯?，PDR短期定位效果較好，但是隨時間會出現(xiàn)誤差累積現(xiàn)象，后期定位軌跡發(fā)散嚴重，最終沒有回到出發(fā)點。而本文所提地圖輔助PDR修正算法得到的軌跡，由于修正了PDR的累積誤差，且使用室內(nèi)地圖校正了路徑和軌跡穿墻現(xiàn)象，定位結(jié)果明顯優(yōu)于PDR原始軌跡，閉合誤差較小，接近真實軌跡。

圖9 PDR定位軌跡和修正軌跡Fig.9 PDR trajectory and modified trajectory

為了驗證地標匹配的有效性，圖10所示為部分地標匹配局部放大結(jié)果，藍色為僅零速修正的PDR定位軌跡，可以看出，三類地標點都顯示出較好的地標匹配修正效果，其中s1電梯地標點由于使用氣壓計數(shù)據(jù)檢測，故使用其三樓的平面坐標作為校正點。地標點s2和d4距離較近，采用本文所提地標匹配方法仍然能夠正確匹配，并將偏離軌跡通過EKF校正至地標點附近。

圖10 地標點校正效果局部圖Fig.10 The partial maps of landmark correction effect

基于走廊和墻面修正算法的局部效果如圖11所示。結(jié)果顯示，在走廊場景下，若行人軌跡偏離通行路徑，校正算法能夠很好地保持估計路徑與走廊通行路徑平行(圖11(a))；當行人軌跡小角度穿墻時，能將當前位置校正為遠離墻面的位置(圖11(b))；當行人軌跡大角度穿墻時，認為行人進入房間，校正至最近的門的位置(圖11(c))，從而使校正后的軌跡更加合理和準確。

圖11 路徑及墻面校正效果局部圖Fig.11 The partial maps of path and wall correction effect

5.4 誤差分析

本文采用平均定位誤差和閉合誤差作為定位性能的度量標準，定位誤差是指實際位置和估計位置之間的歐式距離，平均定位誤差是所有測試位置定位誤差的均值，實驗中實際位置坐標點通過地圖得到。兩種方法的誤差如表4所示。實驗中PDR平均定位誤差為3.7854m，閉合誤差為6.6877m；而經(jīng)過地圖輔助算法約束之后，使平均定位誤差降至1.8435m，閉合誤差降至0.6146m，定位精度提高了51.2%，證明了本文提出的地圖信息輔助的定位方法在實驗后期效果顯著。

表4 定位誤差對比

6 結(jié)論

室內(nèi)定位服務(wù)應(yīng)用十分廣泛，當前室內(nèi)定位方法缺乏統(tǒng)一規(guī)范，精度與成本難以兼顧。傳統(tǒng)的PDR定位方法會出現(xiàn)誤差累積的問題，不能應(yīng)用于實際復(fù)雜的室內(nèi)定位場景。本文提出了一種地圖信息輔助PDR的融合定位方法，實驗結(jié)果表明：

1)對室內(nèi)地標點進行建庫，匹配后采用卡爾曼濾波融合PDR定位結(jié)果和地標點，能夠有效優(yōu)化定位軌跡。

2)基于配準后的室內(nèi)地圖，將室內(nèi)地圖信息融合到定位算法中，采用走廊和墻面等信息，減少軌跡航向發(fā)散，顯著提高了最終的定位精度，實現(xiàn)了室內(nèi)場景下行人連續(xù)、穩(wěn)定的定位功能。

3)本文提出的融合定位方法能夠有效提高室內(nèi)定位精度，具有重要的工程應(yīng)用價值。下一步的研究將考慮更復(fù)雜的實驗環(huán)境，更有效地利用地圖信息約束室內(nèi)行人定位結(jié)果。