基于地磁與改良粒子濾波算法的室內(nèi)定位方法

倪振心，殷鋒，袁平

（1.四川大學(xué)計(jì)算機(jī)學(xué)院，成都610065；2.西南民族大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，成都610041；3.重慶第二師范學(xué)院數(shù)學(xué)與信息工程學(xué)院，重慶400067）

0 引言

隨著現(xiàn)代交通的日漸發(fā)達(dá)，人們的活動(dòng)范圍也越來越廣，因此在任何地方都有對(duì)基于位置服務(wù)（Loca?tion-Based Service，LBS）的需求。不得不說成熟的衛(wèi)星導(dǎo)航（GPS導(dǎo)航、北斗星導(dǎo)航）給戶外活動(dòng)帶來了極大的便利，米級(jí)的定位精度使得人們?cè)诔鲂械穆吠局胁辉倜允Х较颉Ｈ欢谑覂?nèi)環(huán)境中衛(wèi)星導(dǎo)航技術(shù)的信號(hào)會(huì)被建筑物屏蔽，室內(nèi)的LBS只能另尋他法。現(xiàn)有的室內(nèi)定位方案也比較豐富，包括Wi-Fi、RFID、藍(lán)牙、地磁和慣導(dǎo)等技術(shù)。考慮到多方面的因素，基于地磁的定位方案綜合比較后更顯優(yōu)勢(shì)。地磁與類似Wi-Fi這樣的傳統(tǒng)射頻技術(shù)不同，不需要部署額外的基礎(chǔ)設(shè)施，且由于室內(nèi)建筑結(jié)構(gòu)的干擾和異常，使得室內(nèi)的磁場(chǎng)有可以用于位置估計(jì)的可辨性[1]。且地磁信號(hào)也比較穩(wěn)定，長時(shí)間間隔也不會(huì)發(fā)生較大變化。在保證定位精度的前提下，設(shè)備功耗也相對(duì)較低。綜上，使用地磁信號(hào)進(jìn)行定位是一種用于室內(nèi)定位的較好方案。

1 算法可行性分析

室內(nèi)定位系統(tǒng)的核心便是其定位算法，現(xiàn)有的算法大致可分為五類。Wang等人采用地標(biāo)匹配的方法[2]；Shu等人采用DTW方法進(jìn)行定位且有較好效果[3]；機(jī)器學(xué)習(xí)中的HMM方法被Ma等人使用進(jìn)行定位[4]；Xie等人采用粒子濾波方法精度也較高[5]；也有Lee等人使用深度學(xué)習(xí)方法進(jìn)行相關(guān)研究[6]。在定位系統(tǒng)或目標(biāo)跟蹤系統(tǒng)中，由于定位環(huán)境的復(fù)雜多樣性和定位目標(biāo)行為的不可預(yù)知性，對(duì)目標(biāo)的準(zhǔn)確定位就存在較大的困難。對(duì)于這種實(shí)時(shí)非線性動(dòng)態(tài)運(yùn)動(dòng)模型通常用轉(zhuǎn)移模型和觀測(cè)模型來進(jìn)行分析。模型是描述目標(biāo)狀態(tài)隨時(shí)間變化的過程，觀測(cè)模型則是將實(shí)時(shí)目標(biāo)的狀態(tài)與目標(biāo)的輸出信息關(guān)聯(lián)起來[7]。通過將兩種模型結(jié)合可更直觀描述對(duì)目標(biāo)的定位跟蹤過程。

因?yàn)槟繕?biāo)的運(yùn)動(dòng)是一個(gè)時(shí)序的過程，將整個(gè)過程離散化后假設(shè)f是目標(biāo)從t-1時(shí)刻到t時(shí)刻的狀態(tài)轉(zhuǎn)移函數(shù)，h是目標(biāo)在t時(shí)刻的輸出觀測(cè)函數(shù)，xt表示該目標(biāo)在t時(shí)刻的狀態(tài)，yt表示目標(biāo)在t時(shí)刻的輸出觀測(cè)值，ut和vt則表示目標(biāo)的轉(zhuǎn)移噪聲和觀測(cè)噪聲。

本文采用的粒子濾波算法是一種基于馬爾可夫模型和蒙特卡羅方法的解決非線性濾波問題的算法[8]。主要思想就是在面對(duì)數(shù)值方法不容易解決的問題時(shí)，用類似統(tǒng)計(jì)采樣的方法來近似的估算出其概率，通過進(jìn)一步計(jì)算得到一個(gè)最優(yōu)近似解。

假設(shè)目標(biāo)狀態(tài)數(shù)據(jù)集為x0:t={ }xn,n=0,…,t，在t時(shí)刻目標(biāo)狀態(tài)的先驗(yàn)條件概率為p(xt)，假設(shè)到t時(shí)刻為止獲取到的觀測(cè)數(shù)據(jù)集為y0:t={ }yn,n=0,…,t。則t時(shí)刻目標(biāo)狀態(tài)的后驗(yàn)概率p(xt|y0:t)，且假設(shè)重要性概率密度為q(xt|y0:t)。算法是基于統(tǒng)計(jì)采樣的方法，每個(gè)粒子都有其對(duì)應(yīng)的權(quán)值{ωi,i=1,…,N} 。假設(shè)重要性概率密度用目標(biāo)狀態(tài)的先驗(yàn)概率來代替，通過運(yùn)算可以將后驗(yàn)概率表示為：

粒子的權(quán)值更新公式為：

因此目標(biāo)狀態(tài)的后驗(yàn)概率可通過所有粒子的加權(quán)求和求解。

應(yīng)用到實(shí)際定位系統(tǒng)中，基本粒子濾波算法可以分為以下步驟：

（1）對(duì)采樣粒子集進(jìn)行初始化，保證采樣粒子均勻分布于位置范圍內(nèi)，初始化各個(gè)粒子權(quán)值

（3）根據(jù)觀測(cè)模型獲取到的觀測(cè)值yt更新各個(gè)粒子的權(quán)值

（5）進(jìn)行重采樣，在權(quán)值較高的分布中重新采樣N個(gè)粒子。

（6）重復(fù)（2）到（5）直到粒子群收斂，計(jì)算出最終位置。

從粒子濾波的整個(gè)流程上可以看出，通過蒙特卡羅的思想對(duì)粒子群進(jìn)行不斷的采樣、計(jì)算和估計(jì)，最終迭代更新到最趨近于真實(shí)情況的某幾個(gè)粒子上，得到目標(biāo)的定位結(jié)果。但也不難看出轉(zhuǎn)移模型、觀測(cè)模型的精度對(duì)結(jié)果有較大影響。

2 改良粒子濾波算法

型的跟蹤與定位，但也存在一些不夠完美之處。首先，采用蒙特卡羅方法雖然在一定程度上增加了整個(gè)系統(tǒng)的靈活性，但也變相地增大了整個(gè)系統(tǒng)的計(jì)算量。如果能在保證定位精度的前提下減少定位所需的粒子數(shù)，相當(dāng)于也就減輕了系統(tǒng)的運(yùn)算負(fù)載。Xie等人采用類似Wi-Fi的粗定位系統(tǒng)先確定一個(gè)大致的范圍，再從這個(gè)范圍開始計(jì)算收斂[9]。這種方法不僅需要額外的外設(shè)進(jìn)行輔助，增加了系統(tǒng)的部署成本，且Wi-Fi本身就有不夠穩(wěn)定易受干擾的特點(diǎn)，粗定位帶來的誤差必定會(huì)對(duì)后續(xù)的定位結(jié)果造成影響。

2.1 方向可行性采樣模型

粒子濾波算法的傳統(tǒng)采樣方法是將定位地圖劃分為一定單位大小，用手機(jī)對(duì)各個(gè)單位格點(diǎn)進(jìn)行地磁信號(hào)數(shù)據(jù)采樣。為了保證系統(tǒng)的定位精度，劃分的單位必定不能太大，不能遠(yuǎn)大于人的平均步長約50cm左右。獲取到各個(gè)單位格點(diǎn)的地磁值后，再用三次樣條插值或者克里金插值等方法計(jì)算出整個(gè)地圖的完整地磁指紋庫[7]。不同的室內(nèi)建筑對(duì)應(yīng)不同的室內(nèi)地圖，假設(shè)在一個(gè)一百平米的室內(nèi)環(huán)境進(jìn)行采樣，以1m2為單位進(jìn)行劃分也需要采集一百個(gè)左右的方格地磁數(shù)據(jù)，可見采集數(shù)據(jù)的成本過大。

本文提出一種基于方向可行性的采樣模型。同樣將地圖以60cm×60cm大小的方格進(jìn)行劃分，每個(gè)方格都如下進(jìn)行表示：

其中x,y表示該方格在地圖上的二維坐標(biāo)。M表示該方格分布范圍內(nèi)的地磁值，且由于不同的手機(jī)內(nèi)置不同型號(hào)的地磁傳感器，導(dǎo)致不同的手機(jī)的地磁讀數(shù)會(huì)存在一些的差異。對(duì)比實(shí)驗(yàn)后可發(fā)現(xiàn)只有把手機(jī)讀取的數(shù)值作為相對(duì)值才能使用。假設(shè)方格一條邊上的地磁值為m1，對(duì)邊的地磁值為m2，取兩個(gè)值的差值作為在該方格分布內(nèi)的地磁值，可以解決不同手機(jī)間采樣數(shù)據(jù)存在差異的問題。D表示該方格分布內(nèi)的方向可行性，取地圖中的路徑的主要方向?yàn)閛1,o2,o3,o4，若該方格可通行的方向?yàn)閛1,o3則d1=1,d3=1,D=(1,0,1,0)，所有的方格可構(gòu)成一個(gè)方向可行性矩陣：

基本粒子濾波算法盡管能夠滿足對(duì)非線性動(dòng)態(tài)模

2.2 改良算法與定位實(shí)驗(yàn)

對(duì)上述數(shù)據(jù)進(jìn)行收集首先需要將采樣地圖按照路徑的方向可行性劃分為多條采樣路徑，然后分別對(duì)每一條劃分路徑進(jìn)行采樣。每次采樣將手機(jī)放置于智能小車上，操縱小車從頭至尾走完該路徑，置于手機(jī)中的地磁傳感器會(huì)讀取到整個(gè)路徑的方向和地磁時(shí)序數(shù)據(jù)。由于小車可以在某路徑上做勻速運(yùn)動(dòng)，可以認(rèn)為地磁在時(shí)序上的取值與在路徑上的取值一一對(duì)應(yīng)。以四川大學(xué)計(jì)算機(jī)學(xué)院4樓地圖為例，方向可行性劃分后的路徑圖如圖1。

圖1方向可行性劃分路徑

由圖1可以看出主要的路徑分為兩種相互垂直的路徑，令正北方向?yàn)?°，順時(shí)針方向增加，經(jīng)實(shí)測(cè)，該建筑較長端的方向約為西北方向310°，由此可得該地圖路徑的主要方向o1,o2,o3,o4分別約為40°、130°、220°和310°。假設(shè)取其中一條路徑收集數(shù)據(jù)，首先讀取地磁值序列，將數(shù)據(jù)轉(zhuǎn)換到每一個(gè)方格上并求出其對(duì)應(yīng)的地磁值M。同時(shí)讀取傳感器的方向數(shù)據(jù)，計(jì)算出與其差值最小的主方向（若不是路徑的開始和結(jié)尾通常還包括其反方向）并更新該方格的可行性數(shù)據(jù)。

基于方向可行性的改良粒子濾波算法分為以下步驟：

（1）根據(jù)方向值和方向可行性矩陣U計(jì)算出采樣坐標(biāo)分布范圍，對(duì)N個(gè)采樣粒子進(jìn)行初始化，保證采樣粒子均勻分布于位置范圍內(nèi)，初始化各個(gè)粒子權(quán)值

（3）根據(jù)觀測(cè)模型獲取到的觀測(cè)值yt更新各個(gè)粒子的權(quán)值

（5）進(jìn)行重采樣，為防止出現(xiàn)粒子退化的現(xiàn)象，Liu JS等人[10]提出一種計(jì)算有效粒子數(shù)的方法：

Nthreshold為判斷是否需要重采樣的閾值。若需要重采樣則將權(quán)值較低的Nlow個(gè)粒子舍棄，通過方向可行性矩陣U計(jì)算出重采樣位置分布，重采樣Nlow個(gè)粒子。

（6）重復(fù)（2）到（5）直到粒子群收斂，計(jì)算出最終位置，。

相比于基本粒子濾波算法，這種基于方向可行性的粒子濾波算法不僅通過方向可行性的判斷縮小了采樣范圍，減少了采樣總粒子數(shù)，進(jìn)而降低了系統(tǒng)整體的運(yùn)算量；還在重采樣階段通過快速生成重采樣位置分布，增加了粒子多樣性防止出現(xiàn)粒子退化的現(xiàn)象。

為了通過實(shí)驗(yàn)驗(yàn)證該理論的有效性，同樣以四川大學(xué)計(jì)算機(jī)學(xué)院4樓地圖為例。在已經(jīng)對(duì)地圖進(jìn)行采樣并構(gòu)建完地圖數(shù)據(jù)集的前提下，定位初始化時(shí)實(shí)際測(cè)得的初始方向?yàn)?7°，與路徑主要方向o1最為接近，因此通過方向可行性矩陣U可得在o1方向上可行的所有方格集{ }Sj,j=1,…,k，顯示在地圖上如圖2所示。

圖2某可行方向上方格集

假設(shè)地圖總方格數(shù)為K，o1方向上可行方個(gè)數(shù)為k,k∈K，在保證采樣粒子分布密度不變的條件下，改良后采樣粒子數(shù)與方格數(shù)成正相關(guān)N'∝k。采樣粒子節(jié)省率為：

已知地圖總共方格數(shù)為416個(gè)，實(shí)際測(cè)得o1,o2,o3,o44個(gè)主路徑方向上方格數(shù)分別為176,242,183,239，粒子節(jié)省率如圖3所示。可見通過使用方向可行性采樣方法，可有效減少實(shí)際所需的采樣粒子數(shù)，進(jìn)而提升系統(tǒng)整體的計(jì)算速度。但不難發(fā)現(xiàn)采樣粒子節(jié)省率與實(shí)際室內(nèi)場(chǎng)景的路徑結(jié)構(gòu)有較大關(guān)系，若路徑單一可能達(dá)不到理想的效果。

圖3各個(gè)方向采樣粒子節(jié)省率

實(shí)驗(yàn)采用Huawei P30作為實(shí)驗(yàn)手機(jī)，分別采用基本粒子濾波算法和改良粒子濾波算法在四川大學(xué)計(jì)算機(jī)學(xué)院3樓和4樓進(jìn)行實(shí)地定位。在構(gòu)建好兩層樓的地磁數(shù)據(jù)庫后，進(jìn)行了反復(fù)的實(shí)驗(yàn)，最終定位精度對(duì)比如圖4所示。可見改良粒子濾波算法有效地縮小了定位誤差距離，提高了定位精度。

圖4基本粒子濾波算法與改良粒子濾波算法定位結(jié)果

3 結(jié)語

本文的目的是對(duì)地磁室內(nèi)定位中現(xiàn)有的基本粒子濾波方法進(jìn)行改良，首先介紹了基本粒子濾波算法存在的計(jì)算量大和粒子退化的問題，因此提出了一種基于方向可行性的采樣方法，在不借助其他外設(shè)的條件下，能有效地減少系統(tǒng)定位所需的粒子數(shù)，降低了系統(tǒng)計(jì)算量和時(shí)間消耗。且在重采樣階段增加了采樣粒子的多樣性，提升了系統(tǒng)的定位精度。但該方法比較依賴地圖的整體路徑結(jié)構(gòu)情況，在一些空曠單一或者路徑不規(guī)則且復(fù)雜的場(chǎng)景下可能達(dá)不到理想的效果，因此后面的工作將從其通用性上去考慮，在多種室內(nèi)場(chǎng)景定位中減少系統(tǒng)計(jì)算量，并嘗試能否進(jìn)一步提升定位精度。