基于室內合作場景智能識別的行人導航算法

朱超群，賴際舟，呂 品，葉素芬，袁 誠

(1.南京航空航天大學自動化學院, 南京 211106；2.物聯網與控制技術江蘇省高校重點實驗室, 南京 211106)

0 引言

行人導航系統是導航定位領域的一個重要分支，近年來得到了越來越多研究人員的關注，被廣泛應用于救援搶險、軍事作戰等領域。傳統行人導航主要采用全球定位系統(Global Positioning System，GPS)定位技術，但GPS信號在室內以及城市環境下存在信號丟失現象，且民用精度較差，無法滿足人們室內的導航需求[1-3]。隨著微機電系統(Micro-Electro-Mechanical System, MEMS)技術的發展，MEMS慣性測量單元(Inertial Measure-ment Unit，IMU)的體積小、功耗低、質量小、便于攜帶等優點逐漸凸顯[4]，以MEMS-IMU為基礎的室內行人導航系統的研究也成為了熱點。然而慣性傳感器存在著漂移誤差，是行人導航位置航向發散的主要誤差來源。通過零速修正(Zero Velocity Update, ZUPT)算法，可以抑制速度誤差發散并提高導航精度[5-8]。由于航向角誤差的可觀測性差[9]，ZUPT算法無法對航向角誤差進行修正，行人航向及位置會隨著時間的累積而發散，最終使得定位結果偏離真實軌跡。

為了降低航向及位置漂移對行人導航的影響，國內外學者對室內行人導航系統進行了廣泛的研究。文獻[10]提出了利用建筑物走向來約束航向角偏移的算法。文獻[11]提出了一種偏航角誤差自觀測的算法，當行人沿直線行走時，該算法可以用于約束航向角漂移。文獻[12]利用WIFI輔助行人導航，并使用無跡卡爾曼濾波對航向角進行了修正。以上方案都存在一定的局限性，即行人需沿直線行走或需額外添置傳感器，在較為復雜的室內環境下無法對位置航向誤差進行有效約束。文獻[13]提出了一種基于斜坡檢測的行人導航算法，通過檢測行人行走過程中的斜坡場景并與合作環境進行地圖匹配來對行人導航定位結果進行校正。文獻[14]提出了一種多運動模式下的自適應閾值ZUPT算法，即通過訓練機器學習模型，并將該模型用于判別行人當前運動模式，通過判別結果動態調整ZUPT閾值以提高零速判別精度。

考慮到在室內環境下，主要合作場景有上樓梯及下樓梯。因此，本文提出了一種基于室內合作場景智能識別的行人導航算法，通過隨機森林算法對行人在室內行走的步態進行學習與分類，從而完成對平地步行及上下樓梯場景的識別。在此基礎上，將場景識別結果與室內的先驗地圖信息進行關聯，并利用該關聯位置信息對行人軌跡進行校正優化。通過該算法可以有效抑制行人導航誤差累積，提升定位精度。

1 基于隨機森林的行走場景識別算法設計

當行人于室內步行時，主要行走場景有平地步行、上樓梯和下樓梯。為了實現對行走場景的高精度辨識，本文提出了一種基于隨機森林的行走場景識別算法。該算法主要分為模型訓練與場景關聯2個步驟。

1.1 模型訓練

1.1.1 數據分割

對采集到的數據進行精確地分割及截取，可以有效地提高場景識別的精度。在行人步行過程中，足部的運動具有周期性。并且對于單個步態周期而言，通過零速檢測可將其分為運動及靜止2個狀態。因此，通過足部的周期性特征，可以完成對數據的分割，取單個步態周期內足部的運動狀態作為該步態的特征取值范圍。在利用MEMS-IMU獲取得到原始數據(ax,ay,az,a,ωx,ωy,ωz,ω)后，通過捷聯慣導解算及ZUPT算法，還可獲取得到(px,py,pz,vx,vy,vz,γk,θk,φk)狀態量。通過提取以上數據基于時域的特征及基于空間特性的特征，可以獲取需要的機器學習特征向量。其中，利用MEMS-IMU進行數據采集的坐標系定義如圖1所示。

圖1 坐標系定義Fig.1 Definition of coordinate system

1.1.2 步態特征提取

由于行人在穿越樓梯場景時，足部的運動步態特性相較于平地步行明顯不同，且在實際測試中發現，MEMS-IMU的各軸輸出都會產生不同程度的變化，其中陀螺儀z軸的輸出變化最為顯著。因此，本文基于時域的具體特征選取如表1所示。

表1 基于時域的特征選取

在不同場景中步行時，由于行人足部高度及水平速度會產生明顯變化，因此選擇高度和速度等反映空間特性的變化量作為特征量。基于空間特性的特征選取如表2所示。

表2 基于空間特性的特征選取

其中，h(k+1)表示第k+1個步態的初始足部高度，h(k)表示第k個步態的初始足部高度；|v(k)max|表示在第k個步態內的水平速度最大值。

1.1.3 基于隨機森林的樓梯場景識別算法

本文通過行人步態特征來完成對場景的識別，由于不同行人的行走步態特征不同，因此若只采用單一閾值無法對場景進行高精度識別。為了提高場景識別精度，本文提出了一種基于隨機森林的場景識別算法。隨機森林是一種以多棵決策樹為基礎，并通過集成學習后得到的一個強分類器，其最終結果是通過森林中的每棵決策樹投票決定的。隨機森林算法具有簡單、易于實現、計算開銷小等優點[15]。其原理示意圖如圖2所示。

根據圖2可知，該場景識別算法的構建主要分為3個步驟：

1)利用Bootstrap采樣為每棵決策樹生成訓練集

通過數據采集、數據分割及特征提取，獲取得到訓練集D。其中，D=(X,Y)={(xi,yi),i=1,2,…,m}，此處X為獲取的步態特征矩陣，Y為對應的期望輸出，m為輸入的樣本個數。xi=[xi1,xi2,…,xid]T，表示X中的第i個樣本,d為輸入樣本的維數。Bootstrap采樣方法即從原始數據集D中隨機有放回地抽取與D同樣大小的訓練樣本(D1,D2,…,Dn)，利用每個訓練樣本Di來構建一棵決策樹。

2)構建決策樹

圖2 隨機森林算法結構示意圖Fig.2 Schematic diagram of random forest algorithm

節點分裂是決策樹生成過程中的核心步驟。在本文的決策樹構建中，使用的為分類和回歸樹(Classification And Regression Trees， CART)算法。CART算法在進行節點分裂時，采用的分裂規則是Gini值最小原則，其計算公式為

(1)

式中，pi為樣本點屬于第i類的概率。于是，通過在候選屬性集合Ak中，選擇使得劃分后Gini指數最小的屬性作為最優劃分屬性，完成了節點的分裂。通過對該步驟進行循環迭代，便完成了一棵決策樹的訓練。

3)隨機森林的形成及算法執行

通過訓練多棵決策樹構成隨機森林算法。在本文中，將平地步行場景標記為0，上樓梯場景標記為1，下樓梯場景標記為2。在算法執行過程中，輸入當前行人的步態特征，利用訓練完成的決策樹對步態進行投票判別，最終，將得票最多的結果記為當前場景識別結果。

1.2 場景關聯

若當前的場景被識別為上下樓梯場景時，則需將行人位置與對應的樓梯位置進行關聯，因此需要事先建立樓梯位置數據庫。在該算法內，將樓梯位置定義為進入樓梯時第一個臺階中心的位置。獲取室內各個樓梯所對應的樓梯位置信息database={(xp,yp),p=1,2,…,s},其中s為室內樓梯的數量，將該數據以矩陣形式記錄，并記其為樓梯位置數據庫。對于同一個樓梯場景而言，上下樓梯位置點較為接近。為了減小誤關聯現象，將數據庫分為2個，1個為上樓梯的數據庫databaseu，1個為下樓梯的數據庫databased。

行人在室內步行過程中，利用訓練獲取的分類器對行人所在場景進行識別，當行人在步行中且場景識別到行人進入上下樓梯場景時，即

(2)

其中SR(k-1)代表上一個步態的場景識別結果，SR(k)代表當前步態的場景識別結果。式(2)表示行人由平地步行進入上下樓梯場景時，需識別行人所在的特定樓梯。獲取數據庫中與行人當前位置最近的位置坐標，選取該點作為關聯的樓梯位置點。

2 基于行走場景校正的行人導航算法設計

2.1 零速檢測算法

由人體運動學可知，行人在步行過程中，雙足的運動可近似為周期性運動。在一個步態周期內，主要可以分為5個階段：腳跟觸地，腳掌觸地，站立，腳跟離地和腳掌離地[16]。在站立時間段內，行人足部的速度趨近于0，因此，將站立時間段定義為行人步態中零速區間。靜止階段的持續時間約為0.15～0.35s[17]，在此期間內，加速度計的理論輸出在g值附近，陀螺儀的理論輸出趨近于0。

為了能夠精準判斷零速時刻，提高檢測精度，本文采用三條件判別法，即通過三軸總加速度檢測、三軸總加速度方差檢測和三軸總角速度檢測聯合實現零速時刻判別。以“1”表示檢測結果中的零速度狀態，“0”為非零速狀態。k時刻零速判定公式為

(3)

(4)

(5)

(6)

(7)

(8)

其中，thamin、thamax、thωmax和thσmax為判別閾值，N為滑動窗口寬度。以上三種判別條件中，采用單一判別均會出現不同條件的誤判別，無法得到精確的零速區間。因此，通過對其結果取與運算，即ZUPT(k)=C1(k)&C2(k)&C3(k),可以提高零速檢測的準確率。

2.2 融合濾波器設計

本文的融合濾波器結構圖如圖3所示。首先，系統采集行人足部的MEMS-IMU輸出，根據MEMS-IMU的輸出判別零速時刻并運用卡爾曼濾波對行人的速度進行修正；其次，當行人在室內行走時，利用訓練完成的模型對場景進行識別，若檢測到當前的場景為上下樓梯，即可進行場景關聯；最后利用行人當前位置與最近的樓梯位置的偏差作為觀測量，通過卡爾曼濾波進行誤差估計，提升定位精度。

圖3 融合濾波器架構Fig.3 Fusion filter architecture

本文建立的狀態方程及量測方程如下

Xk=φk,k-1Xk-1+Γk-1Wk-1

(9)

Zk=HkXk+Vk

(10)

式中：Xk=[δpkδvkδΨk],δΨk為3個姿態角誤差，δvk=[δvxδvyδvz]為三軸速度誤差，δpk=[δpxδpyδpz]為三軸位置誤差；φk,k-1為系統的狀態轉移矩陣；Γk-1為系統噪聲矩陣；Wk-1為k-1時刻的系統噪聲，Zk為觀測量；Hk為k時刻的量測矩陣；Vk為k時刻的量測噪聲矩陣。并有

(11)

(12)

在本文中，將傳統的ZUPT算法與基于場景識別的定點校正算法相結合，通過零速檢測及場景識別引入了速度誤差與水平位置誤差2個觀測值。當行人在行走過程中處于站立狀態時，速度誤差可以直接被觀測到，如式(10)所示

(13)

其量測方程如下所示

Zv_k=HvXk+Vv

(14)

(15)

(16)

當成功檢測到行人進入樓梯時，則利用行人當前位置與數據庫內取得位置點的偏差來估計水平位置誤差，如下

δPxy=Pxy(k)-Pdatabase(k)

(17)

其中，Pxy(k)為當前的位置，Pdatabase(k)為從數據庫取得的位置點。為了避免由于誤檢測而造成定位與樓梯的誤關聯，此處加入了一個防止誤檢測的步驟，即若檢測到進入上下樓梯場景時行人位置與數據庫中最近點的距離大于閾值thst，則不進行校正。此處，設置閾值thst為

thst=0.03SPD

(18)

其中，SPD為上一個位置修正點到行人當前位置的步行距離。若選取的位置點滿足要求，則將δPxy作為系統的觀測值，并利用卡爾曼濾波修正位置誤差。

其量測方程如下所示

Zp_k=HpXk+Vp

(19)

(20)

(21)

3 實驗驗證

3.1 實驗條件

本實驗采用的傳感器為荷蘭Xsens公司生產的型號為MTW awinda的慣性器件，其主要性能指標如表3所示。

表3 MTW awinda參數

在實驗時將該慣性器件安裝在腳背處，該MEMS器件及其安裝方式如圖4所示。

圖4 MEMS器件及其安裝方式 Fig.4 Installation of MEMS devices

在實驗過程中，通過繞行實驗樓一、二層樓走廊及樓梯進行數據采集，其中每層樓共有4個樓梯口。實驗場景如圖5所示。

圖5 實驗場景Fig.5 Experimental scene

3.2 實驗結果分析

3.2.1 行人行走場景識別試驗

為了利用RF算法訓練模型并測試其分類效果，在實驗場地進行了多組數據采集。本實驗中共有10名測試人員，其中有6名男性，4名女性。每位測試人員分別在實驗樓里步行、上樓梯和下樓梯。在數據采集過程中，不對測試人員進行步態約束，測試人員均按個人習慣步行。采用10-折交叉法對算法性能進行評估，即取9位測試人員的數據集作為訓練集，另外1位測試人員的數據集作為測試集，識別精度如表4所示。

表4 場景識別精度

根據表4中10-折交叉測試結果，其總識別率為99.8%，該識別成功率說明了本算法能夠對場景進行有效識別，進而根據其識別結果對行人導航進行位置修正。

3.2.2 行人定位精度試驗

在本組實驗中，由1名實驗人員在實驗場地內繞著走廊行走，并伴隨有平地步行、上樓梯和下樓梯，最終走回預設的起點處。分別在走廊不同位置設置標志點，行人在行進過程中需穿越標志點并記錄對應位置，用于與標志點真實位置進行對比評估導航精度。本次實驗共穿越了16次標志點，總行走里程為1100m。在相同的環境下，分別采用傳統算法和本文提出的算法分別進行解算，得到的結果分別如圖6及圖7所示。其中，上圖為解算結果平面示意圖，下圖為三維示意圖，實驗整體定位精度如表5所示。可以看出，因為傳統ZUPT算法無法對航向誤差進行補償修正，因此在行人行進過程中位置及航向漂移較大，使得整體導航軌跡圖誤差較大，偏離了真實軌跡。在加入了本文提出的基于室內合作場景智能識別的行人導航算法以后，定位精度顯著提高。該方法與傳統ZUPT算法相比，有效地解決了行人在室內長距離行走狀況下由于位置及航向發散導致定位不準的問題，顯著提升了導航系統的定位精度。

圖6 傳統算法解算結果Fig.6 The computed result of traditional algorithm

圖7 本文提出算法解算結果Fig.7 The computed result of improved algorithm in this paper

表5 定位精度對比

4 結論

針對傳統的基于足綁式MEMS-IMU的行人導航存在漂移誤差，導致行人定位誤差較大的問題，本文提出了一種基于室內合作場景智能識別的行人導航算法。在傳統 ZUPT算法的基礎上，通過訓練機器學習模型，并利用該模型對室內合作場景進行智能識別，根據樓梯位置對行人位置進行修正。在實驗中，利用該算法可以有效地將導航位置誤差由11.45m降為1.85m，驗證了算法的可行性，取得了較好的定位精度。