基于智能手機(jī)傳感器的林區(qū)行人定位算法

王瀚慶，武 剛，陳玥璐，陳飛翔

(北京林業(yè)大學(xué) 信息學(xué)院，北京 100083)

0 引 言

步行是人們?cè)诹謪^(qū)實(shí)施森林調(diào)查，培育和經(jīng)營(yíng)作業(yè)，執(zhí)行搶險(xiǎn)、救援任務(wù)，開展科學(xué)研究和進(jìn)行休閑游憩活動(dòng)的重要方式，林區(qū)行人導(dǎo)航存在廣泛的應(yīng)用需求[1]。定位是基于位置服務(wù)(location based service，LBS)的基礎(chǔ)，是行人導(dǎo)航的重要部分，因此實(shí)現(xiàn)林區(qū)精準(zhǔn)定位具有重要意義[2]。雖然全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system, GNSS)在定位、導(dǎo)航方面具有廣泛應(yīng)用，但由于衛(wèi)星容易受到多路徑效應(yīng)、地形與地物的遮擋等因素的影響，導(dǎo)致其精度降低，不能滿足絕大多數(shù)森林資源與環(huán)境監(jiān)測(cè)定位的精度要求[3-6]。因此，探索無信號(hào)情況下的林區(qū)行人定位具有研究?jī)r(jià)值和應(yīng)用意義。

在脫離GNSS信號(hào)與移動(dòng)網(wǎng)絡(luò)信號(hào)的前提下，可以采用的定位方式包括：有源定位技術(shù)和無源定位技術(shù)[7]。前者需要添加額外硬件設(shè)施，系統(tǒng)部署復(fù)雜，不適用于林區(qū)[8]。后者利用攜帶式慣性測(cè)量單元(inertial measurement unit, IMU),計(jì)算相對(duì)位置信息。隨著智能手機(jī)內(nèi)置傳感器技術(shù)的提升，有學(xué)者提出基于智能手機(jī)傳感器的定位方法[9]，其中，針對(duì)行人定位主要采用的方法為行人航位推算算法[10-12](pedestrian dead reckoning，PDR)。PDR算法以行人每步為單位進(jìn)行步長(zhǎng)和方向估計(jì)，以此計(jì)算每步相對(duì)位置。然而，受手機(jī)傳感器精度限制，基于手機(jī)傳感器的PDR算法不能直接適用于林區(qū)行人定位：一方面，在復(fù)雜且沒有外界參考的環(huán)境下獲得精準(zhǔn)的方向[13]和步長(zhǎng)[14]是PDR算法的難點(diǎn)；另一方面，林區(qū)路況復(fù)雜，PDR算法無法滿足獲取高程分量的定位需求。

本文基于改進(jìn)的PDR算法提出林區(qū)行人定位(forest-pedestrian location，F(xiàn)PL)算法。針對(duì)行人方位角和步長(zhǎng)測(cè)量精度低的問題，首先提出融合多傳感器輸出多濾波算法組合的方法，提高行人位移方位角求解精度；其次，采用K鄰近(K-nearest neighbor, KNN)算法估算行人實(shí)時(shí)步長(zhǎng)。在此基礎(chǔ)上，使用差分氣壓測(cè)高法獲取行人高程信息。

1 FPL算法的基本原理

FPL算法流程如圖1所示：①使用擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)融合算法融合加速度計(jì)和磁力計(jì)輸出，求解原始方位角；②使用卡爾曼濾波(Kalman filter, KF)融合算法，融合陀螺儀輸出，算出精準(zhǔn)方位角；③采用差分氣壓測(cè)高[15]，根據(jù)氣壓計(jì)輸出，測(cè)量行人每步高度變化；④結(jié)合計(jì)時(shí)器和加速度計(jì)輸出，使用KNN算法，估算行人步長(zhǎng)；⑤根據(jù)PDR算法，獲取行人2維定位位置；⑥最后利用氣壓計(jì)獲取行人在林區(qū)的高程信息。

圖1 FPL算法Fig.1 FPL algorithm

以行人GPS定位信息丟失前的最后一個(gè)定位點(diǎn)為起始點(diǎn)，持續(xù)跟蹤行人定位信息。2維坐標(biāo)采用投影坐標(biāo)系，北方向?yàn)閥軸正方向，記為N；東方向?yàn)閤軸正方向，記為E。位移方向與y軸正方向夾角為方位角。行人位置計(jì)算公式為

(1)

(1)式中：(Ei,Ni)為行人第i步的2維坐標(biāo)；(E0,N0)為行人丟失GPS定位信息的最后一個(gè)點(diǎn)的2維坐標(biāo)；li為第i步步長(zhǎng)；θi為第i步方向角。以起始點(diǎn)高度平面為基準(zhǔn)高度h0，第i步高度為：hi=h0+h，則第i步行人3維定位坐標(biāo)為(Ei,Ni,hi)。

2 FPL算法的設(shè)計(jì)

2.1 基于多傳感器的方位角估計(jì)

方位角誤差是PDR算法及FPL算法中位置估計(jì)誤差的最主要來源。方位角分為實(shí)時(shí)方位角和位移方位角，其中實(shí)時(shí)方位角為行人進(jìn)行一個(gè)跨步過程中每一時(shí)刻的方向值，位移方位角為行人完成一個(gè)跨步后相對(duì)上一個(gè)落腳點(diǎn)的方向值。為了能夠精準(zhǔn)測(cè)量實(shí)時(shí)方位角數(shù)據(jù)，采用EKF算法融合加速度計(jì)和磁力計(jì)輸出求實(shí)時(shí)方位角θ。側(cè)傾角φ，俯仰角ψ和實(shí)時(shí)方位角θ可由(2)式求解得到[16]為

(2)

(2)式中：ax,ay,az分別為加速度計(jì)沿手機(jī)坐標(biāo)系x軸,y軸,z軸方向輸出測(cè)量值；mx,my,mz為磁力計(jì)沿手機(jī)坐標(biāo)系x軸,y軸,z軸方向輸出測(cè)量值。

θk|k-1=θk-1+Δθ+Wk

(3)

狀態(tài)估計(jì)方程為

(4)

(3)—(4)式中：θk|k-1為方位角預(yù)測(cè)值;Wk為預(yù)測(cè)方差,Kk為KF增益矩陣;H是測(cè)量參數(shù)。

根據(jù)行人生理學(xué)特征，行人自然行走過程中，實(shí)時(shí)方位角的變化過程如圖2所示，以位移方位角為中軸，做周期性波動(dòng)，周期為1復(fù)步時(shí)間間隔。由圖3所示分位圖(quantile-quantile plot, Q-Q)可知，每一周期內(nèi)，實(shí)時(shí)方位角數(shù)值的概率分布滿足正態(tài)分布。因此，可利用t分布，根據(jù)小樣本容量的實(shí)時(shí)方位角估計(jì)1復(fù)步時(shí)間間隔之內(nèi)的位移方位角[17]。

圖2 行走過程中身體方向變化Fig.2 Changes in body direction during stepping process

圖3 方位角Q-Q圖Fig.3 Q-Q plot of azimuth

采集到的實(shí)時(shí)方位角包括：行人的朝向、行人身體的自然擺動(dòng)、行人手持手機(jī)時(shí)身體的抖動(dòng)、手機(jī)傳感器的測(cè)量誤差等。測(cè)量數(shù)據(jù)變化十分迅速，包含相當(dāng)窄的峰值，如果濾波器窗口長(zhǎng)度大于峰值間隔，會(huì)造成方向數(shù)據(jù)過分平滑。Savitzky-Golay(S-G)濾波[18]是一種有限脈沖響應(yīng)平均濾波器，可以保留信號(hào)的高頻成分，適用于行人導(dǎo)航中信號(hào)的過濾[19]。因此，根據(jù)實(shí)時(shí)方位角確定位移方位角步驟為

1)取1單步時(shí)間間隔內(nèi)，實(shí)時(shí)方位角均值；

2)采用S-G濾波，對(duì)1)所得數(shù)據(jù)進(jìn)行平滑處理；

3)使用KF濾波對(duì)2)所得數(shù)據(jù)進(jìn)行最后的平滑處理。

行人實(shí)時(shí)方位角如圖4所示，當(dāng)行人向北行走時(shí)，實(shí)時(shí)方位角在360°與0°之間快速跳躍，若直接使用上述步驟計(jì)算，將導(dǎo)致較大偏差。因此，當(dāng)行人位移方向朝向北方附近時(shí)，首先針對(duì)實(shí)時(shí)方位角求解正弦值與余弦值；其次，針對(duì)實(shí)時(shí)方位角計(jì)算三角函數(shù)值；最后，采用上述方法計(jì)算位移方位角三角函數(shù)值。

2.2 基于KNN算法步長(zhǎng)估計(jì)

本文采用KNN學(xué)習(xí)算法估計(jì)行人步長(zhǎng)。將行人行走時(shí)產(chǎn)生的行為數(shù)據(jù)形式化為特征向量，稱之為步長(zhǎng)特征K。K包括一單步之內(nèi)加速度均值amean、加速度方差avar、步頻f及高度變化h，即K=(amean,avar,f,h)。假設(shè)在林區(qū)不同的路面情況、不同的行走方式共有m種狀態(tài)，記L={l1,l1,…,lm}代表不同狀態(tài)下的步長(zhǎng)集合。按(K,li)鍵值對(duì)形式存儲(chǔ)數(shù)據(jù)，建立步長(zhǎng)模型數(shù)據(jù)庫，假定共有n項(xiàng)步長(zhǎng)特征，則步長(zhǎng)模型數(shù)據(jù)庫中不超過nm種步長(zhǎng)模型集合，其中l(wèi)iL。實(shí)驗(yàn)表明，為排除硬件誤差，保留(N/4,3N/4)區(qū)間內(nèi)的數(shù)據(jù)，可以提高數(shù)據(jù)精度且可以保證匹配精準(zhǔn)度[7]。

圖4 行走過程中實(shí)時(shí)方位角Fig.4 Azimuth during walking process

(5)

實(shí)驗(yàn)證明，步行特征中不同屬性的重要性呈現(xiàn)差異性，因此引入屬性權(quán)重ωi(i=1,2,…,k)，得到最終距離度量為

(6)

步長(zhǎng)估計(jì)步驟為：

1)計(jì)算K*與Kj之間的距離度量dist2(K*,Kj)，其中j=1,2,…,n；

2)按升序排列dist2(K*,Kj)，查找前k項(xiàng)對(duì)應(yīng)的K；

3)在步長(zhǎng)模型數(shù)據(jù)庫中，根據(jù)(K,li)查找前k項(xiàng)步長(zhǎng)l；

步驟4)中，權(quán)重為[20]

(7)

(7)式中：e是正無窮小量；1/(disti+e)為估計(jì)點(diǎn)到第i個(gè)距離最近的數(shù)據(jù)庫點(diǎn)的歐式距離倒數(shù)。

2.3 差分氣壓測(cè)高

智能手機(jī)中壓力傳感器精度較高，同時(shí)，林區(qū)步行環(huán)境滿足Laplace壓高方程成立條件，因此，可以采用差分氣壓測(cè)高，解決林區(qū)3維定位中高程測(cè)算的問題。差分氣壓測(cè)高利用Laplace公式：若已知參考點(diǎn)高度為h0，氣壓值為P0，溫度為t0，則有

(8)

(8)式中：Rd為氣體常數(shù)；g為所在位置的重力加速度值；h,P,t分別為待測(cè)點(diǎn)高度、氣壓值及溫度；T=(t+t0)/2為待測(cè)點(diǎn)與參考點(diǎn)之間的平均溫度。當(dāng)行人在林區(qū)行走時(shí)，行人所處位置重力加速度g、平均溫度T及初始?xì)鈮褐礟0有確定關(guān)系，因此(8)式可化簡(jiǎn)為

Δh=h-h0=a·lnP+b

(9)

由(9)式可建立針對(duì)確定位置確定溫度環(huán)境下的高度關(guān)于氣壓的一元線性估計(jì)模型。由于重力加速度變化較小，在行人行走高度上溫度幾乎不變，因此，在確定溫度條件下，該一元線性模型具有便捷度及普適性，根據(jù)(8)—(9)式可以獲得行人當(dāng)前位置高度值h。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 實(shí)驗(yàn)條件

本文以小米Note作為傳感器數(shù)據(jù)測(cè)量工具，通過Android傳感器編程調(diào)用SensorManger接口，獲取相應(yīng)傳感器數(shù)據(jù)，選擇北京鷲峰國(guó)家森林公園作為實(shí)驗(yàn)地點(diǎn)，進(jìn)行實(shí)驗(yàn)分析。傳感器數(shù)據(jù)包括加速度計(jì)輸出、磁力計(jì)輸出、陀螺儀輸出以及氣壓計(jì)輸出。行人自然步行的頻率為75～125步/min，普通人的步長(zhǎng)分布在50～90 cm[21-22]。因此，行走的速度為0.7～1.8 m/s，步行頻率為1.25～2.08 Hz，根據(jù)采樣定理，采樣頻率取60 Hz，可以保證信息不丟失[14]。本算法實(shí)驗(yàn)中，實(shí)驗(yàn)者手持智能手機(jī)，以正常行走速度、姿態(tài)進(jìn)行步行實(shí)驗(yàn)，同時(shí)以60 Hz獲取傳感器原始數(shù)據(jù)。

實(shí)驗(yàn)環(huán)境如圖5所示。實(shí)驗(yàn)時(shí)，先針對(duì)每一種路況分別進(jìn)行仿真分析，包括上坡、下坡、臺(tái)階、平面道路及林間無路環(huán)境。最后選擇實(shí)驗(yàn)路徑如圖5中黑色軌跡，進(jìn)行最終仿真實(shí)驗(yàn)分析，所選實(shí)驗(yàn)路況為下坡方向，包含直行轉(zhuǎn)彎、臺(tái)階、緩坡等路況，具有一定的林區(qū)步行環(huán)境代表性。

圖5 實(shí)驗(yàn)環(huán)境Fig.5 Experimental environment

3.2 實(shí)驗(yàn)結(jié)果分析

在PDR算法中，方向信息常用電子羅盤(磁力計(jì)與加速度計(jì)組合)輸出的方式獲??；步長(zhǎng)L常用非線性步長(zhǎng)模型估計(jì)，表達(dá)式為

(10)

(10)式中：amax,I,amin,i為單步加速度最大值與最小值；C為步長(zhǎng)相關(guān)系數(shù)。

本文將FPL算法與使用上述獲取方向、步長(zhǎng)信息方式的定位方法進(jìn)行對(duì)比。首先，討論步長(zhǎng)估算模型中KNN算法中K值與權(quán)重的選擇；其次，確定K值與權(quán)重后，依次選擇3.1節(jié)所述的5種步行環(huán)境作為步行條件，作為實(shí)驗(yàn)環(huán)境進(jìn)行實(shí)驗(yàn)，將FPL定位算法與如下3種定位方式對(duì)比：電子羅盤估計(jì)方向與KNN算法估計(jì)步長(zhǎng)組合法(記為MKNN)、電子羅盤估計(jì)方向與非線性模型估計(jì)步長(zhǎng)組合法(記為MNRA)、多傳感器多次融合法估計(jì)方向與非線性模型估計(jì)步長(zhǎng)組合法(記為SNRA)；最后，選擇復(fù)雜林區(qū)步行環(huán)境，進(jìn)行仿真分析。

表1是不同K值及權(quán)重模式下的步長(zhǎng)估計(jì)誤差。其中weights為KNN算法中取最近K項(xiàng)加權(quán)均值時(shí)的權(quán)重，weights=distance為(7)式所述權(quán)重，weights=uniform為所有數(shù)據(jù)項(xiàng)權(quán)重相同。

由表1可知，選擇加權(quán)KNN算法，當(dāng)K=5時(shí)，KNN算法對(duì)步長(zhǎng)的估算誤差較低，精度優(yōu)于其他K值情況，因此，本文KNN算法中，權(quán)重選擇(7)式，K取值為5。得到K值之后，后續(xù)仿真分析均以此參數(shù)進(jìn)行分析。

表1 不同K值下KNN對(duì)比

FPL定位實(shí)驗(yàn)前，進(jìn)行高程實(shí)驗(yàn)，驗(yàn)證氣壓計(jì)測(cè)高在實(shí)際中的可用性。選取林區(qū)不同路況，包括上坡、下坡以及臺(tái)階路段，以起始點(diǎn)為基準(zhǔn)高度，手持智能手機(jī)隨機(jī)以不同的速度進(jìn)行步行數(shù)據(jù)測(cè)量。圖6a為不同路況下氣壓計(jì)輸出情況，圖6b所示為行人行走時(shí)每時(shí)刻計(jì)算出的高度變化理想效果，平均行走100 m誤差在±5 m之內(nèi)，可以滿足林區(qū)行人定位的高程分量需求。

FPL算法分路況實(shí)驗(yàn)時(shí)，每種路況選擇約500 m進(jìn)行步行實(shí)驗(yàn)，所有定位方法采用同一份原始數(shù)據(jù)進(jìn)行誤差分析。實(shí)驗(yàn)中，以起始點(diǎn)高度為基準(zhǔn)點(diǎn)，根據(jù)氣壓計(jì)輸出，利用差分氣壓測(cè)高測(cè)量行人在林區(qū)的高程信息。3維定位誤差情況如表2所示。

表2的實(shí)驗(yàn)結(jié)果表明，在上坡、下坡、臺(tái)階、平面道路、林間無路環(huán)境下，F(xiàn)PL定位算法與其余定位方式相比均能表現(xiàn)出較高精度。

最后，本文選取包含多種路況的路徑進(jìn)行仿真分析。由圖7可知，本文提出的林區(qū)行人定位算法得到的行人軌跡更接近真實(shí)軌跡。PDR算法中誤差主要來源是方位角的誤差，本文提出的融合多傳感器輸出測(cè)量實(shí)時(shí)方位角，多濾波方法組合確定最終位移方向的方法，可以有效地提高方位角精度，從而提高定位精度。從圖8可知，本文所提出的FPL算法行走1 000 m最大誤差為41 m，誤差為4%，可以滿足定位需求。

圖6 不同路況下氣壓及高度變化Fig.6 Pressure changes and height changes under different road condition

Tab.2 positioning accuracy of FPL，MKNN，MNRA and SNRA under different pedestrian environment comparison m

圖7 行走軌跡結(jié)果Fig.7 Schematic diagram of pedestrian walking trajectories

圖8 行人定位誤差Fig.8 Pedestrian localization errors

4 結(jié) 語

本文提出的FPL定位算法使用智能手機(jī)傳感器作為實(shí)驗(yàn)數(shù)據(jù)測(cè)量工具。針對(duì)PDR算法精度不高的問題，從方位角和步長(zhǎng)2個(gè)方面進(jìn)行改進(jìn)。采用融合多傳感器輸出方法，提高實(shí)時(shí)方位角測(cè)量精度，同時(shí)使用多種濾波組合方法，提高行人位移方位角的估算精度；采用KNN算法估計(jì)行人步長(zhǎng)，以提高步長(zhǎng)估計(jì)的準(zhǔn)確性和適用性，在此基礎(chǔ)上，利用差分氣壓測(cè)高增加行人高程定位信息。實(shí)驗(yàn)證明，本算法可以有效提高2維定位精度，同時(shí)可以獲得完整的3維定位信息，最終，將定位誤差控制在5%以內(nèi)，適用于林區(qū)行人定位。未來的工作重點(diǎn)將從以下2個(gè)方面展開：①更有效的陀螺儀校正方法，提高方位角精度；②有效利用KNN算法降維的方法，提高步長(zhǎng)解算的效率和精度。