基于慣性傳感器與WiFi的室內(nèi)行人軌跡推算

趙世華，趙 磊

( 山東理工大學(xué) 計算機科學(xué)與技術(shù)學(xué)院，山東 淄博 255049)

室內(nèi)行人軌跡推算即實現(xiàn)室內(nèi)環(huán)境中行人的實時位置確定與行進(jìn)軌跡的計算與優(yōu)化，在學(xué)術(shù)上屬于同步定位與建圖(Simultaneous Localization And Mapping:SLAM[1-2])的范疇.由于室內(nèi)環(huán)境復(fù)雜，動態(tài)變化頻繁，在工程上還沒有一種統(tǒng)一成熟的方案.SLAM主要在貝葉斯的框架下進(jìn)行討論，多使用卡爾曼濾波器或者粒子濾波器[3-4].傳統(tǒng)的SLAM算法多基于激光測距傳感器并應(yīng)用在機器人上，例如美國斯坦福人工智能實驗室(Thrun實驗室) 設(shè)計的FastSLAM算法[5].由于精度高的傳感器比較昂貴并且不適用于行人，本文算法設(shè)計僅使用慣性傳感器與WiFi信號，在沒有精密的激光測距傳感器的情況下，需要重點解決慣性傳感器無邊界的誤差積累問題.德國宇航中心(Deutsches Zentrum für Luft-und Raumfahrt ：DLR)的通信和導(dǎo)航研究所設(shè)計并實現(xiàn)了FootSLAM[6]，該項技術(shù)基于粒子濾波且僅使用慣性測量裝置，以行人位置狀態(tài)變化的概率分布作為建圖的基礎(chǔ)，在沒有視覺和距離測量傳感器的情況下實現(xiàn)了行人軌跡推算，并將誤差控制在1-3m.本文粒子濾波與底圖設(shè)計部分參考借鑒了該項技術(shù).與該項技術(shù)不同，在傳感器數(shù)據(jù)采集上使用了安卓智能手機，更有利于應(yīng)用和推廣.在WiFi數(shù)據(jù)的使用上，DLR在FootSLAM的基礎(chǔ)上設(shè)計了WiSLAM[7]，在行進(jìn)的過程中使用WiFi信號強度衰減模型推算AP(Wireless Access Point，無線訪問接入點)的位置，并使用AP位置進(jìn)行后驗概率計算提升粒子濾波的效果.本文所述算法后驗概率的產(chǎn)生同樣基于WiFi信號，但使用了一種基于WiFi信號強度變化趨勢的WiFi地標(biāo)策略，不需要推算AP位置，規(guī)避了因AP位置推算而產(chǎn)生的誤差.綜上所述，本文所述算法面向行人，在僅使用慣性傳感器與WiFi信號的情況下，控制傳感器誤差積累，使軌跡收斂，實現(xiàn)行人的軌跡推算.

1 傳感器的使用

使用了目前市面上普遍流行的智能手機的慣性傳感器，包括加速度計與陀螺儀，根據(jù)加速度計和陀螺儀的數(shù)據(jù)分別可以獲得行人的步態(tài)信息與每步的旋轉(zhuǎn)角度.手機在使用時，要求平放于手中或者固定于胸前.

實驗采用了平行于地面的手機手持姿勢，手機傳感器坐標(biāo)系如圖1所示，實驗中x，y所在平面與地面平行，z軸正方向指向天空，y軸正方向指向身體正前方.由于手機姿態(tài)固定，步態(tài)檢測使用了手機三軸加速度傳感器的z軸數(shù)據(jù)，旋轉(zhuǎn)角度推算使用了手機三軸陀螺儀的z軸數(shù)據(jù).

圖1 Android手機傳感器坐標(biāo)系Fig.1 Coordinate system of Android phone′s sensor

1.1 使用加速度計進(jìn)行步態(tài)檢測

由于手機的加速度計精度不高，并且行走狀態(tài)下對加速度計的影響因素比較多，對加速度計進(jìn)行積分求步長誤差比較大.本文所述的步態(tài)檢測為確定行人的步伐狀態(tài)，即計算每一步的起止時間.使用手機三軸加速度傳感器z軸數(shù)據(jù)進(jìn)行分步，如圖2所示，實驗人員行進(jìn)過程中，加速度計z軸數(shù)據(jù)呈現(xiàn)規(guī)律性變化(為了便于計算，已去除重力影響)，該變化規(guī)律反映了行走狀態(tài)下身體上升和下降姿態(tài)的加速度變化.據(jù)此進(jìn)行分步，即獲得每一步的起始時間和結(jié)束時間.而步長部分采用了固定步長加噪聲的方法來表示每一步的移動距離，固定步長由實驗人員根據(jù)實際情況進(jìn)行手動設(shè)置.

圖2 加速度數(shù)據(jù)步態(tài)劃分Fig.2 Gait analysis of accelerometer sensor data

利用加速度計可將整個移動過程按步分段，每一步的屬性包括移動距離與旋轉(zhuǎn)角度.旋轉(zhuǎn)角度是基于手機三軸陀螺儀z軸數(shù)據(jù)進(jìn)行推算的.

1.2 使用陀螺儀計算旋轉(zhuǎn)角度

手機陀螺儀返回的數(shù)據(jù)表示當(dāng)前時刻的角速度.通過對陀螺儀進(jìn)行積分可以推算出某段時間的行進(jìn)角度變化.根據(jù)加速度計的步態(tài)檢測提供的每一步的起始時間t1與結(jié)束時間t2對陀螺儀進(jìn)行積分可得該步旋轉(zhuǎn)角度為

(1)

通過加速度和陀螺儀分別獲得了行人的步態(tài)信息和方位信息.給定實驗人員初始位置、初始方位和步長的情況下，利用加速度計和陀螺儀可以對實驗人員進(jìn)行粗糙的軌跡推算.如圖3所示，白色區(qū)域為走廊，黑色表示不可通行區(qū)域，真實的行走軌跡為繞圖中走廊行走三圈.由于陀螺儀誤差的積累，軌跡偏離嚴(yán)重.

圖3 直接使用慣性傳感器的粗糙定位Fig.3 Rough positioning using sensor data directly

本節(jié)通過慣性傳感器獲得的步態(tài)信息與旋轉(zhuǎn)角度作為粒子濾波中狀態(tài)與狀態(tài)之間轉(zhuǎn)換的驅(qū)動，是產(chǎn)生先驗概率的必要條件.狀態(tài)的后驗概率由該位置的WiFi環(huán)境計算得來.

1.3 利用WiFi信號構(gòu)造WiFi地標(biāo)

在基于WiFi的室內(nèi)定位算法中，大多數(shù)需要設(shè)計WiFi強度衰減模型，推算AP位置，在確定AP位置后，需要至少三個AP進(jìn)行三角定位確定手機位置作為系統(tǒng)觀測，提升算法效果，如WiSLAM[7].然而準(zhǔn)確地對WiFi信號進(jìn)行建模是十分困難的，一方面需要考慮不同設(shè)備的發(fā)射功率，另一方面還要考慮實際空間的阻擋情況對模型的影響.同時不同的手機由于其WiFi接收模塊的差異，不能使用同一模型.在WiFi信號的使用方面，采用一種基于WiFi信號強度變化趨勢的WiFi地標(biāo)[8]，該地標(biāo)不需要推算AP位置并且避免了設(shè)備不同或手機不同而造成的影響.

1.3.1 WiFi地標(biāo)的定義

WiFi地標(biāo)的三元組定義格式如下

WiFi-Mark：[BSSID，Orientation，Neighboring APS]

單一WiFi地標(biāo)形成情況如圖4所示，如果WiFi地標(biāo)的定義僅使用一個WiFi，將會在多個位置形成相同的WiFi地標(biāo)，這樣不能滿足每個位置地標(biāo)的獨特性.所以在地標(biāo)的定義時引入臨近WiFi，三元組WiFi地標(biāo)定義如圖5所示，中心標(biāo)有數(shù)字1的點即為一個WiFi地標(biāo)(WiFi-Marks)，其所在長條形區(qū)域為路，貫穿該點的箭頭表示運動的軌跡和方向，各圓形陰影區(qū)域即圖中AP Coverage所示區(qū)域表示各AP的覆蓋情況，MasterAP為對地標(biāo)生成起決定性作用的一個無線訪問節(jié)點，當(dāng)檢測到某個WiFi信號的峰值時，WiFi地標(biāo)生成，該WiFi對應(yīng)的AP即為MasterAP.BSSID表示AP的物理地址，三元組中的第一項BSSID為MasterAP的物理地址.其中山峰線表示運動過程中MasterAP的強度變化，其橫坐標(biāo)(Displacement)表示行人位置，縱坐標(biāo)(RSS)表示收到的WiFi信號強度，圖中曲線的峰值點即WiFi趨勢變化的臨界點(Tipping point of RSS trend).WiFi采樣時間間隔為500 ms，由于對WiFi信號進(jìn)行了濾波平滑，所以幾乎沒有出現(xiàn)連續(xù)峰值的情況，算法中認(rèn)為一旦出現(xiàn)連續(xù)峰值則取最后一個作為峰值點. Orientation表示行人通過信號峰值位置前后時刻的面向角.Neighboring APS表示當(dāng)前位置臨近(按強度判斷是否臨近)AP的物理地址以及該AP的信號強度與Master AP的強度差值，為了減小計算復(fù)雜度使用兩個臨近AP，即取相鄰AP中信號強度最大的兩個.當(dāng)行人在包含WiFi信息的環(huán)境中行進(jìn)時，一旦檢測到WiFi峰值便會對當(dāng)前WiFi環(huán)境就地生成一個WiFi地標(biāo).

圖4 單一WiFi地標(biāo)形成情況Fig.4 The situation of using a single WiFi for the creation of landmarks

圖5 三元組WiFi地標(biāo)定義示意圖 Fig.5 Three-element tuple for describing the WiFi-mark

1.3.2 WiFi峰值檢測

實驗使用的手機型號是華為Honor6，由于噪聲的存在需要先對WiFi信號進(jìn)行平滑.然后根據(jù)其變化趨勢進(jìn)行峰值檢測.使用了截止頻率為0.3 Hz的低通濾波器，并使用斜率檢測確定WiFi強度峰值，若出現(xiàn)連續(xù)峰值則取最后一個，WiFi信號總數(shù)為27個，其中一個WiFi的峰值檢測效果如圖6所示，行走軌跡為閉環(huán)矩形路徑繞行三圈，可將WiFi模塊記錄的時間戳與慣性傳感器記錄的時間戳進(jìn)行映射，進(jìn)而鎖定在哪個位置檢測到峰值.實驗人員的行走軌跡為繞閉環(huán)走廊行走三圈，由圖可見該WiFi信號亦出現(xiàn)三次規(guī)律性變化，峰值也為三個.

圖6 其中一個WiFi信號的平滑與峰值檢測Fig.6 Smooth and peak detection of a single WiFi

2 粒子濾波

本文算法的核心部分采用了粒子濾波的方法.將人在空間中的狀態(tài)表示成概率問題，在整個運動過程中，撒下粒子群，所有粒子模擬同一人的運動.每個粒子攜帶自己的高斯白噪聲和傳感器漂移.將空間映射為一張柵格底圖，柵格大小為0.1 m×0.1 m，柵格底圖大小默認(rèn)為1 000 m×1 000 m.通過傳感器提供的驅(qū)動信息與柵格底圖產(chǎn)生先驗概率，每個狀態(tài)所處位置的WiFi信息提供后驗概率(傳感器信號和WiFi信號在行走過程中同時獲得)，并在此基礎(chǔ)上進(jìn)行算法設(shè)計，以使系統(tǒng)可以抑制陀螺儀誤差積累，較好的擬合真實的運動軌跡，并具有良好的魯棒性.如1.1、1.2所述，使用了智能手機來提供加速度計和陀螺儀的數(shù)據(jù)支持，為狀態(tài)的改變提供驅(qū)動信息，同時使用了一種基于WiFi信號強度變化趨勢的地標(biāo).

2.1 系統(tǒng)狀態(tài)初始化

2.2 狀態(tài)轉(zhuǎn)移

就單一粒子而言，假設(shè)k-1時刻的坐標(biāo)為(xk-1,yk-1)，面向角度為θk-1，任意時刻t向下一時刻位置過度的移動距離為

lt=d+st

式中：st為t時刻的步長白噪聲，d為固定步長.

故k-1時刻向k時刻過度的步長為lk-1=d+sk-1.

有運動模型

(2)

k時刻的面向角為

系統(tǒng)的狀態(tài)方程為

(3)

每次狀態(tài)傳遞后都需要將每個粒子的當(dāng)前狀態(tài)加入到歷史狀態(tài)隊列.

2.3 權(quán)重更新

粒子的每一狀態(tài)轉(zhuǎn)換都需要更新其權(quán)重，粒子的權(quán)重變化由其上一個狀態(tài)的權(quán)重和該狀態(tài)下的先驗概率和后驗概率決定.在柵格地圖上，每一個柵格權(quán)重的含義為該柵格可以被通過的概率.被通過次數(shù)多的柵格更有可能被通過[6].所以每一次的狀態(tài)轉(zhuǎn)移，都會更新底圖柵格的權(quán)重wmap，而該權(quán)重即為粒子所處狀態(tài)的先驗概率.若對實際環(huán)境有所了解，或者有空間地圖存在時，我們可以初始化底圖柵格權(quán)重.障礙物區(qū)域的柵格權(quán)重為0，一旦粒子運動到該區(qū)域，柵格權(quán)重不會被更新，而且粒子權(quán)重會被置位0，即殺死該粒子.粒子權(quán)重是由柵格權(quán)重提供的先驗概率和WiFi提供的后驗概率計算得來，每個粒子和每個柵格都有各自的權(quán)重.

本文使用的WiFi地標(biāo)，在地圖上提供了由相互獨立的地標(biāo)組成的地標(biāo)集，即在運動過程中會生成一系列如1.3.1 WiFi地標(biāo)定義所示的WiFi信號特征、轉(zhuǎn)角和地標(biāo)坐標(biāo)的集合.粒子在某時刻的運動結(jié)束后，對手機在該時刻收到的WiFi信號集進(jìn)行地標(biāo)合成，若發(fā)現(xiàn)新WiFi地標(biāo)則進(jìn)行地標(biāo)位置記錄，地標(biāo)位置為當(dāng)前時刻系統(tǒng)狀態(tài)的位置坐標(biāo).當(dāng)某個時刻發(fā)生重采樣時，初始時刻到該時刻的所有狀態(tài)將進(jìn)行調(diào)整，WiFi地標(biāo)的位置也隨之進(jìn)行調(diào)整.

WiFi地標(biāo)的位置時刻隨系統(tǒng)狀態(tài)進(jìn)行調(diào)整，實則將WiFi地標(biāo)與生成該地標(biāo)的時刻進(jìn)行綁定，再次檢測到該WiFi地標(biāo)，則認(rèn)為這兩個時刻的位置應(yīng)該在同一點.即當(dāng)前粒子群根據(jù)產(chǎn)生該WiFi地標(biāo)時刻的位置坐標(biāo)進(jìn)行權(quán)重更新.

(4)

(5)

2.4 重采樣

2.5 更新系統(tǒng)狀態(tài)

本文所述系統(tǒng)狀態(tài)是指從運動開始到當(dāng)前時刻的所有狀態(tài). 即從運動開始到當(dāng)前時刻的整條軌跡.每次發(fā)生重采樣，由于新粒子會繼承其父粒子的歷史狀態(tài)并且粒子權(quán)重分布發(fā)生了變化，故整條軌跡也會優(yōu)化調(diào)整.k時刻的系統(tǒng)狀態(tài)更新即為0～k每個時刻的位置坐標(biāo)更新，每個時刻位置坐標(biāo)為該時刻的粒子群的位置坐標(biāo)的加權(quán)平均，即

(6)

K時刻的系統(tǒng)狀態(tài)為Xk={s0，s1，…，sk}

當(dāng)m時刻發(fā)生重采樣后，相同數(shù)目的新粒子群產(chǎn)生，新粒子會繼承其父粒子保存的歷史軌跡，然而很多新粒子的父粒子是同一權(quán)重比較大的粒子，因此，粒子群保存的路徑差異變小.繼續(xù)運動后粒子發(fā)散，粒子保存的軌跡差異開始變大，但是差異主要體現(xiàn)在m時刻到當(dāng)前時刻的一段.所以再次發(fā)生重采樣后，該段差異縮小明顯.反應(yīng)到實驗現(xiàn)象上，可以看到之前的軌跡調(diào)整微弱，偏離的軌跡被拉回正確位置.

3 實驗效果

在采集時要求采集人員步伐盡量均勻.將固定步長視為可調(diào)參數(shù)，本次實驗設(shè)為0.7 m，白噪聲的幅值在[-0.1,0.1]之間，粒子群可以消除這方面引起的誤差.實驗環(huán)境為一段閉環(huán)的走廊，圍繞該閉環(huán)路徑繞行三圈，手機的手持姿勢如本文1所述，平放于手中，全程姿態(tài)比較穩(wěn)定.

如圖7所示，柵格底圖的權(quán)重都為相同的初值，使用2 000粒子運行，可用的WiFi地標(biāo)為13個.帶有圓點的線為僅使用加速度計進(jìn)行的粗略軌跡推算，每個圓點代表一個狀態(tài)即兩個圓點之間為一步，可以發(fā)現(xiàn)軌跡偏離嚴(yán)重.帶有星號的線為使用本文算法進(jìn)行軌跡推算的結(jié)果，每個星號為一個狀態(tài)，兩個星號之間為一步.根據(jù)實際的行走路徑，本文所述算法可以實現(xiàn)控制傳感器誤差積累，達(dá)到推算軌跡與行人軌跡較好擬合的目的.

圖7 行人軌跡推算效果 Fig.7 The effect of pedestrian trajectory estimation

4 結(jié)束語

基于智能手機的硬件環(huán)境，在僅使用慣性傳感器和WiFi信號的情況下，實現(xiàn)了室內(nèi)環(huán)境中實驗人員固定手機姿態(tài)的運動軌跡推算.其中基于WiFi信號強度變化趨勢的WiFi地標(biāo)策略，有效地規(guī)避了傳統(tǒng)WiFi信號強度衰減模型由不同機型和發(fā)射設(shè)備以及墻體等障礙物所引起的誤差，使用該地標(biāo)策略的粒子濾波算法可以有效地控制陀螺儀誤差無邊界的積累，實現(xiàn)推算軌跡與真實運動軌跡的擬合.在數(shù)據(jù)采集方面，只需要實驗人員攜帶手機在室內(nèi)環(huán)境中行走，規(guī)避了一些特定環(huán)境中所涉及的隱私等問題，基于智能手機傳感器的設(shè)計更有利于該算法的推廣.

設(shè)計非固定姿態(tài)手機的室內(nèi)行人軌跡推算策略，應(yīng)用上探討多人協(xié)作模式的室內(nèi)地圖的構(gòu)建，在智能手機的環(huán)境下融合更多的信息以提高軌跡推算的準(zhǔn)確度，這都是今后研究的方向.

[1]SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Machine Intelligence and Pattern Recognition, 1988, 5(5):435-461.

[2] DISSANAYAKE M W M G, NEWMAN P, DURRANT-WHYTE H F, et al. An experimental and theoretical investigation into simultaneous localisation and map building[C]// CORKE P,TREVELYAN J.The Sixth International Symposium on Experimental Robotics VI.London:springer-verag,2000:265-274.

[3]張毅, 鄭瀟峰, 羅元,等. 基于高斯分布重采樣的rao-blackwellized粒子濾波SLAM算法[J]. 控制與決策, 2016, 31(12):2 299-2 304.

[4]NASURIWONG S, YUWAPOOSITANON P. Gaussian kernel posterior elimination for fast look-ahead rao-blackwellised particle filtering for SLAM[J]. Applied Mechanics & Materials, 2015, 781:555-558.

[5]MONTEMERLO M S. Fastslam: a factored solution to the simultaneous localization and mapping problem with unknown data association[J]. Archives of Environmental Contamination & Toxicology, 2015, 50(2):240-248.

[6] ANGERMANN M, ROBERTSON P. FootSLAM: pedestrian simultaneous localization and mapping without exteroceptive sensors—hitchhiking on human perception and cognition[J]. Proceedings of the IEEE, 2012, 100(5):1 840-1 848.

[7] BRUNO L, ROBERTSON P. WiSLAM: improving FootSLAM with WiFi[C]//MOREIRA A, MAUTZ R. 2011 International conference on Indoor Positioning and Indoor Navigation. Piscataway:IEEE,2011:1-10.

[8]SHEN G, CHEN Z, ZHANG P, et al. Walkie-Markie: indoor pathway mapping made easy[C]// FEAMSTER N,MOGUL J.nsdi′13 proceedings of the 10th Usenix Conference on Networked Systems Design and Implementation. CA:USENIX Association Berkeley, 2013:85-98.