融合水平儀、氣壓計的行人位置估計算法*

王曉甜，付夢媛，毛永毅

(西安郵電大學電子工程學院，陜西 西安 710061)

隨著技術的發(fā)展，人們對室內(nèi)定位技術的精度、成本、普適性等方面提出了更高的要求[1-4]。 目前，室內(nèi)二維定位已經(jīng)無法滿足人們的日常需求。 現(xiàn)在的室內(nèi)環(huán)境以多樓層場景居多，針對多樓層的三維定位受到越來越多的關注[5-8]。

目前，三維定位技術大都利用接收信號衰落特征結合地圖環(huán)境建立指紋庫來完成定位[9-10]，由于信號穿墻衰減較大，前期工作復雜且建立指紋庫后需持續(xù)更新，該方法多用于單個樓層或相鄰兩個樓層的定位。 對于三層及以上的樓層，可以通過微機電系統(tǒng)(Micro Electromechanical System，MEMS)利用行人航跡推算(Pedestrian Dead Reckoning，PDR)算法完成定位[11-14]，通過對人員在行進過程中的加速度、角速度進行兩次積分得到行人的位置信息?，F(xiàn)有PDR 算法存在累積誤差大、樓層識別不夠準確等問題。 為了解決上述問題，本文融合水平儀、氣壓計和PDR 算法共同完成對室內(nèi)行人所處樓層位置的估計，同時水平儀為PDR 算法提供了新的數(shù)據(jù)來源，對PDR 算法累計誤差進行動態(tài)校正，從而提高多樓層三維定位算法的精度。

1 融合水平儀的行人航跡推算算法

PDR 算法如圖1 所示，在獲得上一時間狀態(tài)的位置信息的基礎上，通過推算或實測得到的運動距離和方向變化來計算當前的位置。

圖1 PDR 算法

PDR 算法需要獲得目標運動時的方位角變化、

步數(shù)與步長。 如圖2 所示，人體在運動時存在單復步的區(qū)別。 單步為雙腳交替的一個跨步，而復步則為單側足部進行的一個跨步。 在運動分析、步長推算時需要進行明確區(qū)分。 行走步數(shù)Nstride、步長Lstride，行人行走的距離L的關系為:

圖2 單復步關系

通過水平儀對步態(tài)周期進行檢測和記錄。 使用峰值檢測法對步伐進行檢測，通過步頻和一個步態(tài)周期內(nèi)的水平儀角度數(shù)據(jù)可以得到行人的步長。

式中:a，b和c是可依據(jù)環(huán)境進行設置參數(shù)；F為行走頻率；βmax和βmin為一個步態(tài)周期的水平儀數(shù)據(jù)的最值。 行人運動時除了需要獲得每次遞推時的步數(shù)和步長以外，還需要獲取遞推前后的方位角的變化，α為方位角，Δα為方位角變化值:

第k次遞推的坐標(xk，yk)可由前一次已知的坐標進行遞推得到，其公式如下，式中L為步行位移，α為方位角:

大氣壓與高度的關系模型如式(7)和式(8)所示:

Tm表示兩個等壓面的平均溫度，T0表示基點溫度，T則為測量值。 在室內(nèi)環(huán)境下可認為Tm＝T0＝T。 基準點高程為H0、壓強為P0及氣溫為T。

假設建筑中的樓梯具有不同的高度，且每個臺階高度可變。 通過式(7)行人起點和終點的高度獲得高差ΔH；若假設樓梯每個臺階高度相同，即每步高度都可以視為常數(shù)。 計算相鄰臺階的高度增量Hinc的平均值Ha和標準偏差DH，當｜Hinc-Ha｜＞DH時，通過相鄰的高度校正Hinc。 得到行人上下樓n步的高度變化為:

使用線性系統(tǒng)狀態(tài)方程，經(jīng)過系統(tǒng)地輸入輸出觀測數(shù)據(jù)，得到系統(tǒng)狀態(tài)的最優(yōu)估計的算法。 因為系統(tǒng)的噪聲影響包含在觀測數(shù)據(jù)中，所以此最優(yōu)估計過程也可作為濾波過程。 移動目標的狀態(tài)矢量表示如下:

移動目標狀態(tài)可由式(11)表示:

式中:wi(i∈1，2，3，4，5)為高斯系統(tǒng)噪聲。

行人在室內(nèi)多樓層位置估計原理圖如圖3 所示，流程如下:①按照行人不同運動狀態(tài)下的步頻區(qū)間設置水平儀單位時間內(nèi)的采樣數(shù)。 ②得到水平儀采樣數(shù)據(jù)后，對其截取整周期數(shù)據(jù)進行預處理。 濾波后分析提取特征值:角度區(qū)間，步頻和步長等。 ③通過磁力計參數(shù)和陀螺儀參數(shù)計算出方位角。 ④結合PDR 算法，經(jīng)氣壓計輔助，利用式(11)計算行人在室內(nèi)每一步的位置坐標，得到其所在建筑物內(nèi)的位置變化情況。

圖3 行人在室內(nèi)多樓層位置估計原理圖

2 實驗及結果分析

數(shù)據(jù)采集裝置為Lenovo 小新Air 計算機一臺，具有磁力計、陀螺儀、水平儀及加速度計軟件的手機一部，TP-LINK TP-WDR5620 路由器一個。 數(shù)據(jù)采集時將手機置于右腳腳踝內(nèi)側，采集的數(shù)據(jù)通過路由器傳送至計算機。 實驗選取5 名年齡在18 歲～50 歲之間，身高在160 cm～185 cm 之間的志愿者進行實驗，行走平均速度3 km/h，上下樓平均速度為0.5 步/s。 實驗分別在兩個地點進行，分別針對二維定位和樓層定位進行研究。

實驗地點一為學校2 號實驗樓五樓，如圖4 所示，主要對二維定位進行研究。 實驗分為兩組:①實驗者使用改進的基于水平儀的PDR 算法以東南角開始，固定起點與終點，沿著實驗樓走廊繞行，分析該過程中行人的位置，與計劃路線進行對比；②實驗者在同一地點同樣環(huán)境下使用傳統(tǒng)PDR 算法以相同的路徑繞行，與本文方法進行比較與分析。

圖4 實驗樓結構圖

實驗地點二為學校長智2 號宿舍樓，如圖5 所示，主要對三維定位進行研究。 宿舍樓共五層，相鄰樓層間的樓梯存在拐角，臺階高度為0.15 m，連續(xù)臺階數(shù)為10。 該地點實驗分為兩組:①在實驗樓內(nèi)按照計劃路線連續(xù)地運動，同時記錄運動過程中水平儀和氣壓計的變化情況，分析該過程行人的位置，與計劃路線進行對比。 ②在第一組實驗基礎上，記錄測試過程中實驗者的位置，得到實驗者位置坐標，分析室內(nèi)樓層定位精確度。

圖5 宿舍樓內(nèi)部結構圖

2.1 二維定位實驗結果

2.1.1 基于水平儀的二維位置估計

按照圖6 路徑，對實驗者行走路徑進行分段，實驗者從起點開始，依次通過①～⑥。 實驗進行20次，隨機選取其中一次測試的記錄數(shù)據(jù)，如圖7所示。

圖6 實驗地點路徑規(guī)劃及分段

圖7 測試中水平儀及航向變化數(shù)據(jù)

(1)根據(jù)實驗者的水平儀數(shù)據(jù)變化區(qū)間、周期及頻率等數(shù)據(jù)，結合運動狀態(tài)識別方法，可以判斷出實驗者運動狀態(tài)、步數(shù)和速度。

(2)根據(jù)水平儀記錄可看出實驗者在第23 s、88 s、107 s、145 s 處時的數(shù)據(jù)周期變化規(guī)律發(fā)生明顯改變，同時刻對應航向數(shù)據(jù)也發(fā)生明顯變化。 與行人實際運動進行比對，發(fā)現(xiàn)處于該時刻時行人發(fā)生明顯方向改變，改變的方向角度可通過陀螺儀直接測量得到。

(3)通過航向數(shù)據(jù)得到實驗者測試過程中的方向變化，通過對水平儀采樣數(shù)據(jù)進行處理后得到特征值步長，然后通過PDR 算法可以得到實驗者的詳細位置。

隨機選取其中一次測試的分段記錄數(shù)據(jù)如表1所示。 隨著實驗者行走距離的增加，對于其位置估計的誤差也隨之增大。 當行走的距離不超過30 m時誤差小于5%，對距離估計的精確度較高；隨著步數(shù)的增加，距離估計的誤差也隨之增加，當行走距離達到70 m 時，定位偏差達將達到7 m，誤差高于10%。

表1 測試過程中的數(shù)據(jù)及誤差對比

由圖8 可見，測試路徑中有三個直角轉彎，兩個銳角轉彎，角度不同的轉彎也會造成一定的誤差累積。 第1～2 段軌跡較為筆直，并未出現(xiàn)明顯地方向偏差和抖動；從第3 段開始，第二個轉彎后，軌跡發(fā)生輕微偏移；第5 段出現(xiàn)較為明顯的方向波動，導致第6 段也相應地出現(xiàn)偏移。 路徑長度相同的直線路徑實驗和帶有拐角的路徑實驗產(chǎn)生的測試結果會有差異，因為其與實驗時的路徑結構有關。 當路徑中存在拐角時，實驗者行走過程中會產(chǎn)生角度變化，由于在行走過程中實驗者很難保證在每一個轉彎時的轉彎半徑都相同，也會導致在路徑實驗中產(chǎn)生距離誤差。

圖8 實驗者測試軌跡圖

2.1.2 二維位置估計的性能分析對比

使用本文算法和傳統(tǒng)PDR 算法進行對比，以相同的路徑沿著實驗樓走廊繞行一周為一組，進行五組實驗。 通過加速度計、磁力計及陀螺儀數(shù)據(jù)得到測試過程中的步長、步頻及方向變化。 數(shù)據(jù)經(jīng)濾波處理后利用兩種算法得到運動軌跡，兩種算法和計劃行進路徑對比如圖9 所示。

圖9 兩種算法行人軌跡對比

實線代表實驗者測試的實際路徑，在整個的測試過程中實驗者做3 km/h 的勻速運動。 當實驗者經(jīng)過固定位置點時，將對其位置進行校對和修正。虛線對應為實驗者使用傳統(tǒng)PDR 算法得到的軌跡路線，行走過程中位置估計最大誤差達到7 m。 雙劃線對應的是實驗者使用本文算法得到的軌跡路線，該過程中的最大行人位置誤差為3 m。 從圖9的定位軌跡能夠看出，使用傳統(tǒng)PDR 算法進行定位的行人軌跡波動大且誤差分布不均勻。 融合水平儀與PDR 后，行人定位軌跡比較平滑，其測試軌跡與傳統(tǒng)PDR 算法相比更靠近真實路線。

多次重復該實驗，傳統(tǒng)PDR 算法的定位平均誤差在3m 左右，最大誤差為7 m。 本文算法的定位平均誤差在1.3 m 左右，最大誤差為3 m。 通過圖10(a)的誤差分析可知，傳統(tǒng)PDR 的定位誤差在行走距離達到20 m 時開始加速增大，且行走距離越遠，誤差的增加越大。 基于水平儀的PDR 算法的定位誤差整體較小，且其誤差增速在同等條件下也低于傳統(tǒng)PDR 算法。 三種算法的定位誤差累積概率分布如圖10(b)所示，本文算法的定位效果優(yōu)于傳統(tǒng)PDR 定位算法。 其中，本文算法定位的平均誤差為1.3 m，90%的定位誤差小于2 m，60%的誤差小于1.5 m。

圖10 兩種定位方法的誤差對比

2.2 多樓層定位實驗及結果

2.2.1 運動狀態(tài)識別

為驗證本文方法多樓層定位的效果，設計如下實驗:在圖5 宿舍樓內(nèi)，首先從三樓樓梯處開始上樓至五樓，后穿過40 m 走廊沿另一側樓梯下樓至四樓樓梯處結束，重復10 次實驗，隨機選取一次測試過程中的水平儀數(shù)據(jù)和氣壓計數(shù)據(jù)，如圖11 所示。 觀察水平儀數(shù)據(jù)、氣壓計和航向角數(shù)據(jù)變化，分析得到:

圖11 水平儀數(shù)據(jù)、氣壓計數(shù)據(jù)及方向變化數(shù)據(jù)

①根據(jù)水平儀記錄可看出實驗者在第10 s、22 s時的數(shù)據(jù)周期變化規(guī)律發(fā)生明顯改變，同時刻對應航向數(shù)據(jù)也發(fā)生明顯變化，變化在180°左右，32 s處航向變化在90°左右。 與行人實際運動進行比對，行人處于樓梯拐角處，發(fā)生明顯方向改變，觀察氣壓計的變化情況發(fā)現(xiàn)該時刻氣壓并未發(fā)生改變，因為行人處于拐角處時高度并未發(fā)生變化。 第45 s～75 s 處有方向上的波動，為行走時腿部抖動產(chǎn)生的噪聲所致，這段時間內(nèi)運動方向未發(fā)生改變。 第76 s、90 s 處航向數(shù)據(jù)也發(fā)生明顯變化。

②通過航向角數(shù)據(jù)得到實驗者測試過程中的方向變化、通過對水平儀采樣數(shù)據(jù)進行處理后得到特征值步長以及通過氣壓計得到行人運動時的高度變化，結合PDR 算法，可以得到行人在多層建筑物內(nèi)的詳細位置。

結合表2 分析可得:0～45 s 目標處于上樓狀態(tài)，連續(xù)上臺階步數(shù)為5 步，可知連續(xù)臺階數(shù)為10。且上樓過程中經(jīng)歷了3 個拐角，故而初步判定為上樓2 層；46 s～75 s 目標處于行走狀態(tài)，行走步數(shù)為30，水平儀角度變化無明顯波動，判斷此時目標穿過走廊；76 s 之后，目標開始下樓，途經(jīng)一個拐角，初步判定為下樓1 層。

表2 測試過程中的數(shù)據(jù)特征對比

2.2.2 多樓層定位實驗結果

綜合上下樓路徑數(shù)據(jù)，結合二維位置估計及氣壓計測高對行人行走過程中的位置進行估計，其結果如圖12 所示。 其中圖12(a)為實驗者在宿舍樓內(nèi)行走的軌跡圖。 從軌跡圖中可清晰觀察到行人的高度變化和移動趨勢。 采用基于水平儀和氣壓計的PDR 算法所估計出的路徑連續(xù)性更加明顯和平滑。圖12(b)為上樓過程中的軌跡圖，本文方法能夠很好地分析實驗者上樓和下樓時每一次腳部與臺階的接觸，能夠較為直觀地描述行人在樓梯上的腳部運動規(guī)律，可觀察出行人在上樓時連續(xù)的單側步數(shù)為5，依據(jù)人體上樓時雙腳交替的運動習慣，可得連續(xù)樓梯數(shù)為10。 所以通過行人上樓的軌跡可判斷測試者的軌跡中的臺階數(shù)量和位置。

圖12 室內(nèi)行人上下樓位置估計

上下樓過程中，行人的運動狀態(tài)切換較為頻繁，包括行走、上樓和下樓三種狀態(tài)，因此導致多樓層實驗中出現(xiàn)的位置誤差、方向誤差和距離誤差均比單樓層實驗中的誤差大。 雖然測試人員在每次實驗中都沿著同一路線行走，但由于運動的隨意性，在行走過程中難免出現(xiàn)輕微的方向偏移和速度的不均，導致偏離預設路徑，特別是在路徑中的轉彎處，行走過程中方向的改變更容易出引起偏航。

3 結束語

本文提出了結合水平儀和氣壓計的行人位置估計算法。 通過磁力計、陀螺儀結合水平儀閾值可以判定行人運動狀態(tài)及行人位置變化；結合行人運動狀態(tài)和氣壓計得到目標行人的高度及樓層；利用PDR 算法最終得到行人位于多層樓內(nèi)的位置。 實驗證明在噪聲環(huán)境下本文算法的定位精度比傳統(tǒng)PDR 算法有明顯提高。