基于地標點匹配的高精度室內定位算法

周凌柯，鮮 委，龔文龍，李 勝

（南京理工大學 自動化學院，南京 210094）

行人導航系統又稱單兵導航系統，用于實時提供行人的速度、位置等導航信息[1]，是單兵數字化系統的重要組成部分。全球定位系統（Global Positioning System,GPS）在室外定位中具有廣泛的應用，但是在室內環境下，衛星信號會由于遮擋和多路徑效應而不可用，這就使得行人室內定位具有挑戰性?；谖C電系統（Micro-Electro-Mechanical System,MEMS）的慣性測量單元（Inertial Measurement Unit,IMU）具備自主性好、體積小、質量小、便于攜帶等特點[2]，因此被廣泛應用于行人導航系統?；贗MU 的行人的室內定位廣泛使用零速檢測（Zero Velocity Update,ZUPT）與行人航跡推算（Pedestrian Dead Reckoning,PDR）算法，但這些定位算法只能得到行人的相對位置與相對航向，并且定位誤差會隨著時間而累積，導致定位效果變差[3]。

為了抑制行人室內定位中航向角漂移的問題，Borenstein 提出了啟發式航向角削減（Heuristic Drift Elimination,HDE）算法?；诮ㄖ酁榫匦芜@一事實，將建筑方向定義為主方向，通過主方向來約束航向角[4]；Jimenez 則對HDE 算法進行改進，只有檢測到行人正在直行時才會執行HDE 算法，從而減小行人在非主方向上行走時HDE 的過度修正[5]；針對HDE方法中判斷直行需要設定閾值的問題，Muhammad 提出了一種無閾值的轉彎檢測技術[6]。然而傳統的HDE方法只是將航向角的主方向在360 °內等間隔地分割為4 個或8 個，若行人在非預設的主方向上直行，則HDE 方法無法起到修正作用。

HDE 方法僅能對行人的航向角進行約束，可以增添超寬帶[7]、藍牙[8]、WIFI[9]、音頻[10]等外接設備來輔助定位，但這些額外設備的部署與維護進一步增加了人力與財力的消耗。使用地標點輔助定位則具備成本低的優勢，在室內定位領域，傳感器輸出呈現規律性變化的位置點被稱為地標點[11]。Jimenez 將建筑物中出現的斜坡作為地標，事先建立包含斜坡位置信息的數據庫，若檢測到斜坡則對行人位置進行修正[12]。胡倩針對不同類型的地標點設定了置信度的計算公式，提高了匹配的準確率[13]。然而傳統的地標點輔助定位需要事先將地標與位置相關聯，并不適用于未知環境。

考慮到純慣導解算時高度通道并不穩定，因此需要額外的信息源去約束高度信息，通常使用氣壓計進行約束。Zhao 將氣壓計和PDR 方法融合，通過氣壓計來計算垂直位移，解決了PDR 只能實現平面二維定位的問題[14]；Jao 提出了一種結合氣壓計和超聲波的混合高度計，并在樓梯、坡道等常見室內地形進行實驗。在該高度計的輔助下，ZUPT 的導航精度相較于無輔助時提升91%[15]，但是氣壓計解算的高度在溫度、氣壓變化的情況下會急劇惡化。

在分析上面所提問題的基礎上，本文研究了行人室內行走時的特點，提出了一種基于地標點匹配的高精度室內定位算法。該算法使用IMU 這一單一傳感器，包含速度約束、航向角約束、水平位置約束和高度約束四個模塊，其中零速檢測算法用來約束速度；增強型啟發式偏移消除（Enhanced Heuristic Drift Elimination,eHDE）算法實時檢測和建立主方向以約束航向角；實時建立地標點約束水平位置，并設計匹配度的計算公式以區分不同的地標點；基于階梯高度的高度約束算法約束高度，實現了行人室內的高精度定位。

1 融合濾波器設計

本文采用卡爾曼濾波算法融合四個模塊，該濾波器的框架如圖1 所示。

圖1 融合濾波器架構Fig.1 The structure of fusion filter

陀螺儀和加速度計的測量值被用來進行捷聯慣導解算。由于慣性傳感器的誤差會隨著時間迅速增長，為了更好地描述慣導誤差的傳播規律，建立15 維誤差狀態向量模型δXk。

其中，δψk為k時刻的三個姿態角誤差；為東向、北向、天向的速度誤差；為東向、北向、天向的位置誤差；為陀螺儀零偏向量；為加速度計零偏向量。

將慣導的解算結果當做預測量，不同的運動約束算法將會提供相應誤差值的虛擬觀測量，將預測量與觀測量融合以對行人運動時的導航信息進行約束。

2 模塊設計

2.1 速度約束

在足綁式行人慣性導航系統中，零速狀態時支撐腳相對于地面是靜止的，支撐腳上的速度應該為0，但是由于傳感器本身以及慣導算法所帶來的誤差影響，導致此時的速度并不為0，可以使用零速檢測算法檢測零速狀態以對速度誤差進行修正。本文使用文獻[16]的姿態假設最優檢測（Stance Hypothesis Optimal Detection,SHOE）算法進行零速檢測，關于該算法本文不再贅述。若檢測到當前時刻為零速狀態，可得速度的虛擬觀測量為：

為了獲得更完整的零速區間，對零速檢測結果進行二進制均值濾波。圖1 中的“零速點”對應每個零速區間的第一個采樣點，當前時刻為零速點是進行航向角約束、地標點約束以及高度約束的前提。

2.2 航向角約束

2.2.1 傳統的HDE

HDE 算法需事先計算零速點時刻的步幅航向，設第m個零速點在所有采樣點中的序列號為s(m)，s(m)時刻慣導解算的東向、北向位移分別為x(s(m))和y(s(m))，則第m步的步幅航向θs(m)可由相鄰兩個零速點的東向、北向位移表示(m＞ 1)：

該步的對應步長SL(m)可以表示為：

由“靜止”向“運動”過渡可能會出現偽零速點，這些偽零速點的特征是計算的步長過小。設定步長閾值thSL=0.15 m，篩除步長小于該閾值的零速點。

使用主方向的前提是當前時刻為直行狀態，設定直行標識符為SLP(m)，角度閾值thθ1=8°，由相鄰四個步幅航向判斷當前是否為直行狀態(m＞ 3)：

取走廊方向為主方向，直行標識符為“1”時進行主方向匹配。設與步幅航向最接近的主方向為θb1(m)，角度閾值thθ2=8°，滿足 abs (θs(m) -θb1(m))＜thθ2時匹配成功，此時HDE 算法提供的虛擬觀測量為：

2.2.2 改進的HDE 算法

傳統的HDE 算法只能在預定義的直線路徑上生效，而本文eHDE 算法無需預設主方向，能夠在行走過程中實時建立主方向，新的主方向θnew可由相鄰四步的步幅航向確定(m＞ 3)：

檢測到行人正在直行，則遍歷主方向數據庫。如果主方向數量為0，則直接建立新的主方向θnew。如果主方向的數量不為0，則搜索與當前步幅航向最接近的主方向為θb1(m)，滿足 abs (θs(m) -θb1(m))＜thθ2時用θb1(m)對航向角進行修正。如果不滿足修正條件，則搜索與θnew最接近的主方向為θb2(m)，滿足abs (θnew-θb2(m))≥2thθ2時建立新的主方向θnew，否則不建立新的主方向。

為了減小行人在非主方向上直行造成的誤修正影響，使用自適應量測噪聲進行航向角誤差的量測更新，該量測噪聲的標準差如式(8)所示。

其中，σeHDE=0.013；β=7。

δφ的絕對值越小，卡爾曼濾波對該觀測量的置信度越高，即當前的步幅航向與主方向越接近，eHDE的修正作用越明顯。

eHDE 算法的流程圖如圖2 所示。

圖2 eHDE 算法流程圖Fig.2 The algorithm flow chart of eHDE

2.3 地標點約束

2.3.1 改進的地標點算法

行人在室內建筑物行走時，在某些特定的區域傳感器輸出會出現規律性的變化。例如在兩條直型走廊相交的拐角處航向角會出現90 °左右的變化，而在兩個樓梯梯段交接處的平臺會出現180 °左右的變化，可以把這些位置點視為地標點，對行人導航的位置信息進行修正。經過對建筑結構的分析，本文設定90 °與180 °這兩種類型的地標點。

相較于傳統的地標點約束算法，本文的地標點約束算法包含兩個改進。一是基于航向角的變化實時檢測并建立地標點，無需預知地標點的位置。檢測到的地標點可以分為初次訪問和重復訪問兩種情況：將初次訪問的地標點納入地標點數據庫，與IMU 解算的位置進行關聯；重復訪問時則用已有的地標點信息進行位置修正。雖然IMU 解算的位置存在誤差，但是該位置由先前時刻的慣導數據解算，其不確定程度小于重復訪問時刻，因此基于該信息進行位置修正可以提高定位精度。第二個改進則是設計了一種地標點匹配函數，檢測到地標點后取匹配度最高者進行位置修正，避免了單純以位置區分地標點造成的區分度低、誤匹配率高的問題，提高了地標匹配的準確率。

考慮到建筑物的結構在不同的樓層上存在強對稱性，本文的地標點僅關注東向、北向這二維位置信息，行走過程中高度方向的位移則通過其他算法進行約束。

2.3.2 角度條件

本文的地標點約束算法通過角度條件去檢測地標點。地標點檢測需要分析相鄰兩個零速點步幅航向的角度增量，已知第m步與第m-1步的步幅航向分別為θs(m)與θs(m-1)，則第m步的步幅航向增量Δθs(m)可以表示為：

假設在地標點處經歷的步數為N，事先進行的拐角實驗表明該步數滿足N?{ 4,5,3,6,2}。易得地標點處的運動滿足兩個條件，一是對應N步的步幅航向增量具有相同的符號，逆時針經過拐角時，N步步幅航向增量的符號均為正，順時針則均為負。設條件一的標識符為LF1(m)，則LF1(m)可以表示為：

二是對應N步的步幅航向增量的總和應該接近特定的角度θsum，本文θsum為90 °或180 °。設定條件二的角度閾值為thθ3=8°，則條件二的標識符LF2(m)可以表示為：

第m步是否滿足地標點檢測的角度條件標識符為LF(m)，只有條件一與條件二均滿足時LF(m)才為1。

由于角度條件是根據步幅航向是否變化特定的度數來檢測地標點，故檢測到地標點的時刻滯后于到達地標點的時刻，所以納入數據庫的地標點不能與檢測到地標點時輸出的位置信息進行關聯。設轉向中途點h為N步中滿足的第一個零速點，則新納入的地標點應該與h時刻輸出的位置相關聯。

2.3.2 地標點匹配度計算

為了進一步提高地標點匹配的準確度，在角度條件的基礎上，引入匹配度的概念。納入新地標點時不僅會存儲位置信息(xl,yl)，也會存儲初始步幅航向θs(m-N)、末尾步幅航向θs(m)、地標點類型LT以及轉向中途點h時刻卡爾曼濾波器輸出的東向、北向位置誤差的噪聲方差、以及該條地標點信息入庫時的序列號ID。初始和末尾步幅航向將會用來進行匹配度計算，而噪聲方差將會在重復訪問地標點進行位置修正時作為東向、北向位置誤差的量測噪聲。

設第m個零速點滿足角度條件，此時遍歷地標點數據庫。如果此時數據庫為空，該地標點肯定是初次訪問；如果非空，遍歷所有地標點信息，對于數據庫中的第c個地標點，可得轉向中途點h與其位置的歐式距離為dc。若起始航向角偏差為ΔθSTART，末尾航向角偏差為ΔθEND，第c條地標點信息的類型為LTc，則可根據式(13)計算第c個地標點的匹配度mc。

其中，wd表示距離因素在匹配度中的權重系數；wθ1和wθ2表示方向因素所占的權重系數，并且滿足wd+wθ1+wθ2=1。本文取wd=1/2，wθ1=wθ2=1/4。

式(13)表明匹配的前提是地標點類型相同。設定匹配度閾值thm=0.65，若數據庫中的最大匹配度mmax大于該閾值，表明匹配成功，可用相應位置信息進行修正；反之則為初次訪問，需建立新的地標點。地標點匹配成功時，地標點約束算法輸出的虛擬觀測量可表示為：

此時地標點約束算法的流程圖如圖3 所示。

圖3 地標點約束算法流程圖Fig.3 The algorithm flow chart of landmark constraint algorithm

2.4 高度約束

當行人僅通過樓梯實現高度變化時，只會包含平地走、上樓梯和下樓梯這三種運動狀態，由于每一級階梯的高度固定，相鄰兩個零速點的高度增量應該接近階梯高度的某個整數倍。本文關注相對高度，引入累積階梯變化級數ntotal，ntotal表示當前時刻相對于初始時刻階梯變化級數的總和，初始值為0。在零速點時刻計算高度觀測量，設定每一級階梯的固定高度為zstair=0.15 m，高度約束的閾值thz=0.05 m，每一步對應的階梯變化級數為k，則高度誤差的觀測量δz的求解過程可以用表1 的偽代碼實現。

表1 高度約束算法流程Tab.1 The process of height constraint algorithm

3 融合濾波更新

卡爾曼濾波的狀態方程與量測方程分別為：

其中，Φk/k-1為系統的狀態轉移矩陣；Wk-1為k-1時刻的系統白噪聲；Zk為觀測量；Hk為k時刻系統的量測矩陣；Vk為k時刻的量測噪聲。

根據慣導的機械編排可以得到系統的狀態轉移矩陣Φk/k-1為：

其中，Δt為采樣間隔為載體坐標系到導航坐標系的坐標轉換矩陣為加速度計在導航坐標系上的反對稱矩陣；I 為3 × 3的單位陣；0 為3 × 3的零陣。

慣導的每個采樣時刻均會進行時間更新，若當前時刻運動約束算法生效，則進一步進行量測更新，此時量測矩陣Hk由當前虛擬觀測量的組合情況決定。

4 實驗與分析

4.1 實驗場景介紹

開展多樓層行走實驗來驗證本文高精度三維室內定位算法，該實驗路徑的起點位于自動化學院的六樓，實驗人員在六樓按照L1-L2-L3-L4-L5 的順序行走，通過樓梯依次進入五樓和四樓，并在五樓和四樓的走廊各行走一周，整個實驗歷時448 s，起點與終點的水平位置相同，實驗人員在六樓的軌跡示意圖如圖4 所示。

圖4 六樓行走軌跡示意圖Fig.4 Schematic diagram of walking trajectory on the sixth floor

實驗使用荷蘭Xsens 公司生產的MTI-300 傳感器采集加速度計以及陀螺儀數據，采集數據期間將該傳感器固定于人右腳的腳面上，并利用USB 口將數據傳輸到電腦中，采樣頻率為100 Hz。該傳感器的性能參數如表2 所示。

表2 MTI-300 性能參數Tab.2 The performance parameters of MTI-300

4.2 水平定位結果分析

通過水平定位效果分析各種算法對航向角的約束作用。ZUPT 和ZUPT+HDE 算法的二維軌跡如圖5 所示，本文算法的二維軌跡如圖6 所示，其中起點位置記為(0,0)，墻面信息事先通過激光測距儀獲得。

圖5 ZUPT 和ZUPT+HDE 算法的多樓層行走實驗的二維軌跡Fig.5 The 2D trajectories of ZUPT and ZUPT+HDE algorithm obtained from the multi-floor walking experiment

圖6 本文算法多樓層行走實驗的二維軌跡Fig.6 The 2D trajectory of the proposed algorithm obtained from the multi-floor walking experiment

本文算法、ZUPT 算法和ZUPT+HDE 算法得到的終點誤差分別為0.2548 m、1.4186 m 和3.4772 m。根據實驗的實際情況可知，行走軌跡應該被約束在兩側墻壁之間，但是傳統的ZUPT 算法航向角發散嚴重，導致出現了很多軌跡穿墻的情況，而HDE 算法能夠抑制航向角漂移，減少軌跡穿墻的情況，降低了水平定位的終點誤差。由圖6 可知，相較于ZUPT+HDE 算法，本文算法中的地標點約束算法產生了作用，進一步減少了軌跡穿墻的情況，水平定位的定位誤差也隨之降低。

4.3 地標點約束算法驗證

為了驗證實時建立地標點的可行性，用黑色的五角星標記初次訪問的地標點，藍色的圓圈標記重復訪問的地標點，同時在初次訪問的地標點旁用數字標記其建立的先后順序，本文算法多樓層行走實驗的三維軌跡圖如圖7 所示。

圖7 本文算法多樓層行走實驗的三維軌跡Fig.7 The 3D trajectory of the proposed algorithm obtained from the multi-floor walking experiment

根據實驗結果可知，整個實驗過程共建立了9 個不同的地標點，初次訪問的地標點均被正確地建立，而重復訪問的地標點也能被正確地檢測。為了更好地分析實驗結果，地標點約束算法建立的地標點數據庫如表3所示，一部分地標點的實際環境圖如圖8所示。在環境圖中用箭頭表示對應地標點步幅航向的變化情況，地標點數據庫中的序號與環境圖中的序號均代表該地標點的建立次序。

表3 多樓層行走實驗建立的地標點數據庫Tab.3 The landmark database established in the multifloor walking experiment

圖8 部分地標點的實際環境圖Fig.8 Actual environmental map of some landmarks

本文根據航向角的變化情況設定了兩種類型的地標點，航向角變化接近90 °的地標點為類型1，航向角變化接近180 °的地標點為類型2。由數據庫信息可知，僅有地標點7 為類型2，這與規劃的路徑相符。雖然地標點5 和9、6 和8 的地標點類型相同，水平位置也非常接近，但是仍被劃分為不同的地標點，這是因為匹配度計算時不僅會考慮水平位置因素，而且也會考慮轉向初始和末尾的角度因素，由于地標點5 和9、6 和8 的初始角度和末尾角度均相異，因此被劃分為不同的地標點，表明匹配度公式發揮了正確的區分作用。

4.4 高度約束效果分析

為了驗證高度約束算法的優越性，本文算法解算高度與ZUPT 解算高度的對比結果如圖9 所示。

圖9 本文算法與ZUPT 算法解算的高度對比Fig.9 Comparison between the proposed algorithm and ZUPT algorithm in height calculation

由圖9 可知，ZUPT 算法解算的高度發散明顯，而高度約束算法即使沒有氣壓計的輔助，也能準確反應行走過程中的高度變化情況。通過終點位置的高度誤差評判高度估計的準確度，ZUPT 算法的終點高度誤差為0.5773 m，而本文算法的終點高度誤差為0.0174 m，算法性能提高了96.98%。

4.5 誤差分析

由于終點位置已知，可以計算不同算法的終點誤差，結果如表4 所示。由表4 可知，相較于ZUPT 算法和ZUPT+HDE 算法，本文算法的終點誤差分別減小了82.6%和68.3%，驗證了本文算法的有效性。

表4 多樓層行走實驗不同算法的終點誤差Tab.4 Terminal point error of different algorithms in the multi-floor walking experiment

5 總結

針對室內行人導航系統定位誤差大的問題，本文提出了一種基于地標點匹配的行人高精度三維室內定位算法。該算法僅使用IMU 這一單一傳感器，其濾波框架包含速度約束、航向角約束、水平位置約束和高度約束四個模塊。多樓層行走實驗表明該算法能夠提高行人室內定位精度，滿足室內定位的需求。