地圖輔助行人航跡推算技術的室內定位方法

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

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

0 引言

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

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

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

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

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

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

圖1 融合定位系統結構框圖Fig.1 Block diagram of fusion positioning system

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

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

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

(1)

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

2.1 地標點檢測

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

(2)

(3)

其中，T為檢測窗口長度；n為數據長度。定義門的識別規則

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

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

(4)

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

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

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

Rcor=

(5)

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

(6)

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

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

樓梯或者電梯地標可通過氣壓計數據來識別。圖4所示為上下樓梯時氣壓計幅值變化。為避免誤檢，在一次樓梯運動期間只匹配樓梯入口位置，當行人位于樓梯入口時，運動狀態由水平移動變為垂直移動，定義樓梯入口的識別規則[13]

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

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

(7)

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

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

2.2 地標匹配

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

使用地標輔助室內行人定位的一個關鍵挑戰在于數據的關聯，即當前位置附近存在多個地標時，很難確定檢測到的地標與地標庫的匹配情況。為了解決該問題，本文提出了設置置信度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分別表示航向因素和距離因素在置信度中的權重系數。δ(Rk,Rt)為狄拉克函數，定義如下

(9)

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

(10)

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

(11)

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

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

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

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

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

(12)

若窗口內連續步長參考航向和走廊參考航向差的最大值在閾值β范圍內，如式(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為該墻面的起點和終點構成的向量

(17)

(18)

由墻面反射原理可知

(19)

即

=(δx,δy)

(20)

則校正點坐標為

(21)

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

4 濾波融合更新

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

(22)

其中，Xk為PDR解算結果，即三軸位置、速度和姿態;Zk為地標點或者地圖校準點三軸位置;Φk|k-1為狀態轉移矩陣;Hk為量測矩陣;Wk-1和Vk分別為系統的過程噪聲和量測噪聲，均為不相關白噪聲。卡爾曼濾波算法流程如圖6所示。

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

左側是系統預測回路，Q為系統過程噪聲Wk的協方差，按照卡爾曼濾波模型，首先建立狀態方程為

(23)

(24)

狀態轉移矩陣Φk|k-1為

(25)

Hk=[I3×303×303×3]

(26)

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

(27)

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

5 實驗與分析

5.1 實驗場景

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

表1 MTw Awinda設備參數指標

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

5.2 地標與路徑匹配結果

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

表2 算法參數設置

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

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

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

表3 地標匹配結果

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

5.3 軌跡分析

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

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

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

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

基于走廊和墻面修正算法的局部效果如圖11所示。結果顯示，在走廊場景下，若行人軌跡偏離通行路徑，校正算法能夠很好地保持估計路徑與走廊通行路徑平行(圖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；而經過地圖輔助算法約束之后，使平均定位誤差降至1.8435m，閉合誤差降至0.6146m，定位精度提高了51.2%，證明了本文提出的地圖信息輔助的定位方法在實驗后期效果顯著。

表4 定位誤差對比

6 結論

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

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

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

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