一種基于無線信號輔助的室內(nèi)無死角定位算法

劉 宇，李俊林，路永樂，張 旭，方 針

(1.重慶郵電大學(xué)光電信息感測與傳輸技術(shù)重慶市重點實驗室,重慶 400065；2.中國電子科技集團公司第二十六研究所,重慶 400065)

0 引言

室內(nèi)定位技術(shù)發(fā)展已久，但常用的WIFI、藍牙、射頻等基于信標(biāo)的室內(nèi)定位技術(shù)均需預(yù)先部署大量的基礎(chǔ)設(shè)施且定位精度受環(huán)境所限[1]，而慣性導(dǎo)航定位技術(shù)不受外界環(huán)境所限制，在室內(nèi)定位中有著天然的優(yōu)勢，但存在累計誤差大的缺點[2]。因此，目前還沒有某種單一的室內(nèi)定位技術(shù)能夠提供精確、穩(wěn)健和一致的定位與跟蹤服務(wù)[3]。所以采用各種輔助方式增強慣導(dǎo)系統(tǒng)的實用性，實現(xiàn)室內(nèi)無死角定位是當(dāng)前的研究熱點。

國內(nèi)外許多研究人員對該技術(shù)已經(jīng)開展了大量的研究工作。劉春燕等[4]提出了一種自適應(yīng)模型噪聲的擴展卡爾曼濾波(Extended Kalman Filter,EKF)算法，實現(xiàn)行人航位推算(Pedestrian Dead Reckoning，PDR)與WIFI定位源的濾波融合，有效抑制了慣性定位誤差的累積，將整體誤差控制在5m左右。Chen P等[5]使用卡爾曼濾波對慣性定位和超帶寬(Ultra Wideband，UWB)定位數(shù)據(jù)進行融合，得到的融合定位系統(tǒng)精度高，實時性能好；Zou Han等[6]基于手機傳感器將WIFI與PDR融合，并采用新興的iBeacon技術(shù)矯正WIFI覆蓋盲區(qū)的PDR漂移誤差，采用粒子濾波(Particle Filter，PF)融合得到的定位精度可以達到亞米級，但兩者均局限于二維定位。周寶定等[7]通過智能手機內(nèi)置的傳感器獲取用戶行為感知來輔助進行室內(nèi)行人定位，以消除PDR的累積誤差；Liu Mengyun等[7]在智能手機上利用視覺、WIFI和慣導(dǎo)來實現(xiàn)多源室內(nèi)無縫定位，95%的幾率可以達到1.32m的定位精度。王敬賢等[8]采用手機自身光、磁、慣性傳感器模塊和衛(wèi)星模塊進行組合，實現(xiàn)行人室內(nèi)外無縫導(dǎo)航，避免了導(dǎo)航過程中路徑中斷現(xiàn)象的產(chǎn)生。中國礦業(yè)大學(xué)等高校的學(xué)者們在相關(guān)課題的支持下，進行了基于智能終端平臺的以WIFI和慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)為主的室內(nèi)無縫定位技術(shù)研究[9-14]，如Li Xin等[15]融合了藍牙和PDR技術(shù)，在不增加基礎(chǔ)設(shè)施的情況下提供米級室內(nèi)定位精度，但同樣地，該機構(gòu)的研究結(jié)果都是基于二維空間實驗得出的。

上述國內(nèi)外研究機構(gòu)及研究學(xué)者的努力都有很大收獲，提高了系統(tǒng)定位精度或穩(wěn)定性，極大地促進了無縫定位技術(shù)的發(fā)展,但無須昂貴的基礎(chǔ)設(shè)施定位系統(tǒng)，如智能手機終端受傳感器精度限制總體定位精度不高，而能實現(xiàn)高精度定位的定位系統(tǒng)硬件開銷較大，系統(tǒng)的普及性差，且大部分處于二維空間定位的研究。本文提出了一種基于WIFI信號輔助的室內(nèi)定位算法，以慣性定位為主導(dǎo)，采用WIFI信號進行位置和航向的修正，能夠利用現(xiàn)有的無線路由器，減少部署開銷，提高慣性定位的精度，便攜高效地實現(xiàn)室內(nèi)無死角定位。

1 系統(tǒng)架構(gòu)

本文實驗采用實驗室自主研發(fā)的微慣性測量單元(Micro Inertial Measurement Unit，MIMU)，內(nèi)部集成了陀螺儀等傳感器和ESP8266。在數(shù)據(jù)采集過程中將其佩戴在測試人員腰部脊椎位置，載體坐標(biāo)系即微慣性測量單元坐標(biāo)的定義如圖1所示，X軸正向向右，Y軸正向指向前進方向，Z軸正向垂直地面向上。實驗中數(shù)據(jù)采集頻率設(shè)定為50Hz，輸出數(shù)據(jù)保存在筆記本電腦上。

圖1 微慣性測量單元坐標(biāo)定義Fig.1 Definition of MIMU coordinate

本文算法首先利用了微慣性測量單元中的陀螺儀、加速度計、磁力計和氣壓計四種傳感器實現(xiàn)三維的慣導(dǎo)定位。然后利用無線信號對行人的位置和航向進行糾正，實現(xiàn)室內(nèi)無死角定位。算法框圖如圖2所示。

圖2 算法框圖Fig.2 Algorithm block diagram

2 無線信號輔助定位算法

目前，大量室內(nèi)建筑都廣泛布設(shè)了WIFI無線路由器，眾所周知無線信號的強弱與距離信號發(fā)射端的遠近有關(guān)。若提前知道無線訪問接入點(Access Point,AP)的物理位置，則完全可以利用這些信息進行無線信號輔助慣導(dǎo)定位，實時糾正INS定位的位置，消除位置上的累計誤差，增強INS的適用性。

為了實際應(yīng)用操作簡單，本文不考慮根據(jù)計算傳播損耗模型公式進行接收功率與距離之間的轉(zhuǎn)化，而是直接進行實驗測試來得出接收信號強度指示(Received Signal Strength Indicator，RSSI)與距離的對應(yīng)關(guān)系。在室內(nèi)活動過程中，行人佩戴的定位終端會檢測到附近的無線AP發(fā)射出的信號，信號強度通常處于-110～-20dBm區(qū)間。實際中，一般把-90dBm看成臨界值，若檢測到比-90dBm還低的信號強度值，則判定此時無信號；上限以-20dBm為臨界值，凡是高于-20dBm則認為信號極好，判定其所處位置應(yīng)該在AP附近。設(shè)置AP的RSSI功率輻射值為10dBm，分別在距離AP1m、2m、3m、4m、5m、6m、7m、8m、9m、10m的位置處采集無線信號30次，統(tǒng)計每個位置上的RSSI的最大值、最小值和平均值。最后得到RSSI與距離的變化特性如圖3所示。

圖3 RSSI與距離的變化特性Fig.3 Characteristics of RSSI-distance variation

從圖3可以明顯地看出，RSSI的平均值隨著定位終端與AP間的距離的增加而遞減。具體地，當(dāng)RSSI在-10～-20dBm范圍內(nèi)時，定位終端與AP的距離不超過1m；當(dāng)RSSI在-20～-25dBm的范圍內(nèi)時，定位終端與AP的距離在1～2m之間。隨著2個端點之間距離的增大，會出現(xiàn)RSSI數(shù)值大幅度變化的情況，說明RSSI與位置之間只在距離較短時才存在較好的對應(yīng)關(guān)系，當(dāng)距離逐漸增大后，這種對應(yīng)關(guān)系將變得不再可靠，此時采集的RSSI無法用于輔助定位。

根據(jù)上述特性，設(shè)置定位終端接收到的信號強度閾值RTh為-6dBm，當(dāng)定位終端接收到的某個AP的RSSI超過設(shè)定閾值RTh，就可推算出行人處于距離該AP半徑為1m的單位圓內(nèi)，就將行人當(dāng)前位置更新為已知的該AP的物理位置。無線信號輔助矯正實時位置方法如圖4所示，INS由于長時間誤差累計，對行人的實時定位出現(xiàn)明顯偏差，當(dāng)行人佩戴定位終端經(jīng)過某AP時，就通過閾值法將實時位置更新為已知的該AP的物理位置，而且由所設(shè)的閾值和距離之間的對應(yīng)關(guān)系可知，真實位置與校正位置之間的誤差小于1m，滿足室內(nèi)定位精度的需求。

圖4 無線信號輔助矯正實時位置示意圖Fig.4 Schematic diagram of wireless signal assisted correction for real-time location

3 航向最優(yōu)估計算法

3.1 基于陀螺增量的航向優(yōu)化算法

定位終端佩戴在行人腰部，并不是完全固定不動的，因此，正常行走過程中終端隨人體運動帶來的抖動誤差都會在航向上有所表現(xiàn)。根據(jù)多次的仿真分析，這種航向誤差在一般在5°～10°，隨著本來的誤差累積，將會對最終的定位精度造成極大的影響。為了抑制這種航向誤差帶來的不良影響，本文提出了利用角速率變化量進行航向誤差的修正。

受限于目前建筑物的規(guī)則矩形結(jié)構(gòu)，整個室內(nèi)無死角定位過程中，將行人的正常行走分為兩種情況：1)直線行走，航向基本保持一致；2)轉(zhuǎn)向行走，航向發(fā)生劇烈變化，所以對航向誤差的修正也是從這兩方面進行考慮。剛好在INS中，使用陀螺儀進行姿態(tài)解算，動態(tài)響應(yīng)良好，短期內(nèi)精度高，但漂移會因長時間工作而逐漸變大，使用加速度計和磁力計組合進行解算，靜態(tài)解算時精度高，而動態(tài)解算時由于加速度計的動態(tài)誤差而導(dǎo)致精度降低，即可利用這兩種不同的姿態(tài)解算組合的優(yōu)點來對兩種行走情況的航向誤差做補償。

若是行人航向發(fā)生劇烈改變，仍舊利用EKF融合算法進行解算，將載體坐標(biāo)轉(zhuǎn)換中的四元數(shù)Q作為狀態(tài)矢量，將測量的三軸加速度計值和三軸磁力計值分別進行歸一化，得到ak和mk，作為觀測量，構(gòu)建卡爾曼濾波方程，如式(1)：

(1)

最后，通過更新后的四元數(shù)(q0,q1,q2,q3)，得到目標(biāo)航向角：

(2)

若是行人航向未發(fā)生變化，即基本保持直線行走，但存在抖動誤差，則需要對航向進行優(yōu)化處理。其中關(guān)鍵是要對行人是否發(fā)生劇烈的航向變化進行判別，本文以陀螺短時積分為依據(jù)，通過閾值法進行判斷。若是陀螺積分大于設(shè)定閾值，則認為此時航向發(fā)生劇烈變化，不需要進行航向誤差補償；若是陀螺積分小于設(shè)定閾值，則對誤差進行修正。

一般當(dāng)行人直線行走時，定位終端會隨人體運動發(fā)生輕微的移位。陀螺儀良好的動態(tài)性能對這種極小幅度的擺動也會作出響應(yīng)，直接體現(xiàn)在EKF求解的航向角上會有10°左右的誤差波動。本文選擇滑動均值濾波進行誤差處理，具體措施為，在用陀螺積分作閾值判斷航向變化時，若判定行人航向未發(fā)生劇烈變化或變化較小，且持續(xù)一段時間Δh，則判定行人處于平穩(wěn)行走狀態(tài)，使用該平穩(wěn)時間區(qū)間內(nèi)的航向均值代替當(dāng)前航向作為行人運動的前進方向；若是行人航向發(fā)生了劇烈變化，則不做濾波處理，等待下一個平穩(wěn)狀態(tài)的出現(xiàn)。綜上，整個航向優(yōu)化的具體流程如圖5所示。

圖5 航向優(yōu)化流程Fig.5 Heading optimization process

圖6所示為航向優(yōu)化算法修正前后的航向?qū)Ρ葓D。從圖中可以看出，航向改變時，EKF準(zhǔn)確地解算出航向角，符合實際情況；而當(dāng)航向未發(fā)生改變，即保持平穩(wěn)運動狀態(tài)時，優(yōu)化后的航向更為平滑，減小了腰部抖動誤差對航向的影響。

圖6 航向優(yōu)化前后對比圖Fig.6 Comparison of heading angle before and after optimization

3.2 在線實時航向糾正

航向誤差產(chǎn)生的根本原因在于陀螺儀長時間工作會產(chǎn)生較大的漂移，這也是慣導(dǎo)系統(tǒng)最根本的誤差所在。當(dāng)通過INS定位得到的位置出現(xiàn)了偏差時，一般是距離和方向兩方面的誤差累積所導(dǎo)致的。

同樣地，可以利用已知物理位置的無線路由器及其所處位置的建筑物幾何結(jié)構(gòu)信息進行實時的航向糾正。目前，室內(nèi)建筑大部分是規(guī)則的矩形結(jié)構(gòu)，所以一般AP的安裝位置不外乎直線走廊和拐角兩處。凡是路過AP進行了位置矯正，那么一定是從走廊的左右兩邊或者拐角的兩邊朝AP靠近，所以就算解算的航向角出現(xiàn)誤差，卻可以根據(jù)無線信號輔助定位的位置糾正點確定行人行走過程中的真實航向，完成航向誤差的實時糾正。

以無線節(jié)點被裝置在狹長的走廊上為例，行人經(jīng)過該AP被糾正了定位位置，行走方向受走廊地理條件限制，只能是沿走廊從AP的右邊或者左邊走過來，如圖7所示。用Hright表示從AP右邊走過來的方向，用Hleft表示從AP左邊走過來的方向。AP在導(dǎo)航坐標(biāo)系下的物理位置已知，那么方向Hright和Hleft在導(dǎo)航坐標(biāo)系中的具體角度也是確定的。接下來只需判斷行人是從左邊還是右邊路過的該AP，即可確定行人的實際航向。雖然慣導(dǎo)長時間工作航向會有偏差，但該偏差是一步一步發(fā)散的，所以可以根據(jù)短時間內(nèi)的平均航向和該已知AP的左右2個方向做差值，比較判斷行人行走方向，表示如下：

(3)

圖7 行人經(jīng)過AP行走方向示意圖Fig.7 Schematic diagram of pedestrians walking through the AP

當(dāng)無線路由器被裝置在走廊的拐角處，判別方法仍沿用上述的閾值法。

將矯正后的航向信息輸入到EKF中進行下一步迭代，即可有效地消除慣導(dǎo)累計誤差。

在已知無線路由器位置的前提下，利用無線信號輔助矯正位置和航向進行在線調(diào)整后，不僅提高了定位精度，更清空了慣導(dǎo)的累計誤差，相當(dāng)于對傳感器件進行了初始化，提高了定位系統(tǒng)的定位時間，增強了慣導(dǎo)系統(tǒng)的實用性。

4 實驗驗證

為驗證本文提出的室內(nèi)無死角定位算法的可行性與可靠性，本文采用實驗室自主研發(fā)的微慣性測量單元進行實驗驗證，該測量單元內(nèi)部集成1個三軸加速度計、1個三軸磁力計、3個單軸陀螺及1個微處理器。其輸出的橫滾角和俯仰角的動態(tài)精度為0.5°，航向角的動態(tài)精度為3.0°，加速度量程為50m/s2。

選擇重慶郵電大學(xué)第一教學(xué)樓作為實驗測試環(huán)境，圖8所示為第一教學(xué)樓的樓層結(jié)構(gòu)圖及無線路由器的布點方案。藍色的無線路由器代表實際的無線信號發(fā)射端，AP2表示布置在2樓左邊走廊的無線路由，AP3表示布置在3樓中間走廊的無線路由，AP4表示布置在4樓右邊走廊拐角的無線路由。中間的走廊行走距離為50m，圖示右邊走廊行走距離為18m，左邊走廊行走距離為22m，樓梯長度10m左右。

圖8 第一教學(xué)樓平面圖及測試路線Fig.8 Floor plan of the 1st teaching building and test route

測試人員腰部佩戴定位終端從1樓起點出發(fā)，沿路線每層樓上下重復(fù)行走，如此循環(huán)至少行走30min回到1樓起點，分別在沒有無線信號和有無線信號兩種情況下行走，即對純慣導(dǎo)定位算法和本文提出的室內(nèi)無死角定位算法進行測試。純慣導(dǎo)航跡推算得到的定位結(jié)果和本文提出的室內(nèi)無死角定位算法得到的定位結(jié)果分別如圖9和10所示。從圖中可以看出，隨著時間的推移，純慣導(dǎo)定位出現(xiàn)明顯的航向漂移，而本文提出的室內(nèi)無死角定位算法仍然一直提供著精準(zhǔn)定位，航向漂移極小。可見本文提出的室內(nèi)無死角定位算法可在室內(nèi)提供長期有效的三維定位信息。

圖9 純慣導(dǎo)定位結(jié)果Fig.9 Results of the inertial navigation

圖10 本文提出的室內(nèi)無死角定位結(jié)果Fig.10 Results of the proposed indoor no-dead-space positioning

根據(jù)實驗統(tǒng)計，測試人員沿著一教行走至少30min后回到起點，能達到樓內(nèi)(2樓走到4樓又回到2樓)重復(fù)行走5次，整個行走距離大約2100m。記錄純慣導(dǎo)定位和本文提出的室內(nèi)無死角定位兩種算法的定位參數(shù)，統(tǒng)計結(jié)果如表1所示。由表1可知，純慣導(dǎo)技術(shù)定位過程中航向出現(xiàn)漂移，航向誤差達到35.61°，水平距離誤差高達36.01m，定位誤差為1.73%；而使用無線信號輔助的室內(nèi)無死角定位整個測試過程無軌跡發(fā)散跡象，最終航向誤差為9.97°，水平距離誤差為12.57m，定位誤差為0.61%，明顯小于純慣導(dǎo)定位誤差，證明了本文研究的室內(nèi)無死角定位技術(shù)與純慣性傳感器定位技術(shù)相比，能夠提供更持久和準(zhǔn)確的位置信息。

表1 純慣導(dǎo)和本文提出的室內(nèi)無死角定位實驗結(jié)果對比表Tab.1 Comparison of pure inertial navigation and the proposed indoor no-dead-space positioning based on wireless signal aided location experimental results

為了測試室內(nèi)無縫定位算法的穩(wěn)定性，首先對多次實驗下的系統(tǒng)定位精度進行了評估，選擇10位測試人員仍然按照上述路線行走，同樣要求至少行走30min后，回到出發(fā)點，行走距離大約2100m。一般地，慣導(dǎo)的定位效果主要是看閉環(huán)行走起點和終點的距離偏差與實際行走距離的誤差比。統(tǒng)計測試人員回到原點后的觀測結(jié)果，計算出垂直誤差、水平誤差、航向誤差、閉環(huán)誤差等，結(jié)果如表2所示。

表2 三維定位測試結(jié)果Tab.2 3D positioning test results

從表2中可以看出，實驗?zāi)繕?biāo)距離為2100m，起點與終點的最大三維距離誤差為19.95m，最大閉環(huán)誤差為0.95%，平均閉環(huán)誤差為0.79%。所以本文所研究的室內(nèi)無死角定位算法的閉環(huán)誤差小于1%，滿足室內(nèi)的定位精度需求。而且10位測試人員通過上位機得到的定位結(jié)果均與實際行走軌跡相似，航向角漂移較小，保持在15°以內(nèi)，并未出現(xiàn)傳統(tǒng)慣導(dǎo)普遍存在的明顯航向漂移與軌跡發(fā)散的情況，以上多次實驗測試證明了該算法的穩(wěn)定性。

然后選擇圖書館、信息科技大廈和第四教學(xué)樓三種不同于第一教學(xué)樓的室內(nèi)環(huán)境進行實驗測試。每種測試場景中都規(guī)定起始點，并根據(jù)不同室內(nèi)建筑物的幾何構(gòu)造，進行不一樣的AP布點方案，每種環(huán)境下測試20次，每次測試依舊不少于30min，統(tǒng)計每個環(huán)境下的定位閉環(huán)誤差，結(jié)果如表3所示。測試結(jié)果表明，不同室內(nèi)環(huán)境下的平均誤差與0.79%相比，偏移均在±5%以內(nèi)，多次測試的數(shù)據(jù)標(biāo)準(zhǔn)差較小，證明了該算法具有較高的穩(wěn)定性，可以穩(wěn)健地實現(xiàn)室內(nèi)無死角定位。

表3 不同應(yīng)用場景測試結(jié)果Tab.3 Different application scenarios test results

通過同一室內(nèi)定位場景的多次重復(fù)性三維定位實驗測試，本文提出的室內(nèi)無死角定位算法定位誤差小于1%，再通過不同室內(nèi)定位場景的多次重復(fù)性實驗測試，平均定位誤差偏移不超過±5%，證明了本文算法的可靠性。因此，驗證了本文提出的使用無線信號輔助的室內(nèi)無死角定位算法的可行性與可靠性。

5 結(jié)論

針對純慣性導(dǎo)航系統(tǒng)累計誤差大、無法長時間工作的問題，本文提出了一種利用無線信號輔助定位的室內(nèi)無死角定位算法。該算法利用無線信號對慣導(dǎo)定位的位置進行實時的標(biāo)定，然后針對不同的運動狀態(tài)，對無信號時的航向做平滑處理，再利用無線信號對航向進行實時地糾正，從位置和航向兩方面消除慣導(dǎo)的累積誤差，提高定位精度。通過實驗證明，與純慣導(dǎo)定位技術(shù)相比，本文提出的室內(nèi)無死角定位算法定位精度更高，可靠性更好，進一步提高了慣性導(dǎo)航定位技術(shù)的實用價值。