基于足間距信息輔助的行人三維慣性定位算法

路永樂,蘇 勝,楊 杰,羅 毅,粟 萍,惠嘉威

(重慶郵電大學智能傳感技術與微系統重慶市高校工程研究中心,重慶 400065)

0 引言

導航系統指為行人提供位置信息服務的系統,通常利用衛星定位系統實現室外導航定位[1],但人類大部分的活動都發生在室內環境中,衛星信號會受建筑物墻壁遮擋,導致信號衰弱嚴重,進而無法提供可靠的室內導航服務[2]。而慣性導航系統(inertial navigation system, INS)以慣性測量單元(inertial measurement unit, IMU)提供的加速度和角速度信息為基礎,利用捷聯慣導算法解算得到位姿信息,實現導航定位[3]。由于INS不需要外界信息輔助,具有自主性強、抗干擾能力強等優勢,在單兵作戰、火場救援及室內導航等領域成為了主要研究方向[4-5]。

但是IMU存在不可避免的誤差,導致INS定位結果出現嚴重漂移,無法提供準確的定位服務[6]。為抑制行人INS累積誤差,Foxlin等[7]采用零速修正(zero velocity update, ZUPT)算法,在靜止區間內利用零速條件修正系統誤差。但ZUPT算法無法解決航向角誤差漂移的問題,長航時定位結果仍會偏離真實情況[8]。

基于以上問題,研究人員對行人INS誤差修正進行了以下三方面的研究:1)利用多傳感器融合抑制航向漂移。孫猛等[9]利用磁力計測量磁場信息,并利用擴展卡爾曼濾波構建基于磁定位和行人航跡推算(pedestrian dead reckoning,PDR)的融合系統,實現室內定位;何成陽等[10]利用超寬帶(ultra wide band,UWB)無線通信技術輔助IMU定位,并利用聯邦導數容積卡爾曼濾波器對航向誤差進行修正。2)部署多IMU約束誤差。李興等[11]利用IMU陣列實現位置解算,并根據漂移誤差情況自適應融合陣列數據,提高行人導航精度;李曉東等[12]在行人雙腿關節處部署6個IMU,利用人體關節信息構建雙足約束模型來抑制航向誤差。3)利用外界信息實現高度解算。陸傳華等[13]利用氣壓計更新高度信息,并利用地圖信息限制高度更新的區域,最終實現行人高度解算;俞敏杰等[14]在大樓內部署信號源,根據接收來自各樓層信號強度差值的分布情況,確定行人所在樓層。

上述方法可以有效修正系統誤差,但引入其他傳感器增加了系統的復雜性和成本,并且導航系統只能在特定場景內工作,不具有普遍適用性?；谝陨蠁栴},本文提出了一種基于足間距離信息輔助的行人慣性定位算法。該算法利用隨機森林算法對行人的動作模式進行識別,并采用基于時間因子修正的自適應零速檢測算法,實現了對零速區間的高精度劃分;在平面運動中,通過利用超聲測距的足間距約束模型修正導航誤差,有效限制了航向誤差的發散;而在樓梯運動情況下,采用基于臺階高度和足間距信息的高度解算算法,實現了對高度信息的準確解算,有效避免了高度的發散。在保證系統自主性、便攜性和普遍適用性的同時,可以有效提高雙足IMU導航系統的定位精度。

1 基于足間距信息的位置解算

1.1 基于超聲測距的足間距約束模型

INS根據IMU采集的行人運動數據,建立“東北天”導航坐標系(n系)和“右前上”載體坐標系(b系),利用捷聯慣導解算獲得行人導航信息,導航信息解算方程如下[15]

(1)

將根據式(1)得到的任意時刻行人左右腳導航狀態表示為

(2)

(3)

圖1 行人單次邁步足部示意圖

(4)

(5)

(6)

(7)

當任意時刻左右腳位置關系不滿足足間距約束模型時,利用式(7)可以得到雙足位置誤差為

(8)

考慮到行人運動的每個步態周期中都存在一只靜止腳與一只擺動腳,利用零速修正算法修正靜止腳所處的導航子系統,并將靜止腳作為參考基準,利用足間距約束模型對擺動腳所處的導航子系統進行修正。在行人行走過程中雙足交替進行擺動,雙足IMU導航系統將不斷地對行人雙腳導航子系統交替進行零速修正和足間距約束修正,最終提高雙足IMU導航系統的定位精度。基于卡爾曼濾波的雙足導航子系統的誤差修正模型如下

(9)

(10)

(11)

其中,an表示n系下的加速度。

(12)

(13)

1.2 基于臺階高度與足間距的高度解算算法

圖2 行人上下樓梯運動示意圖

在已知臺階高度信息的情況下,利用單次邁步中初始位置與終點位置間的足間距信息,求取單次邁步跨越臺階數,便可以實現行人的高度解算。設單次邁步的起點和終點平面位置坐標分別如式(14)所示

(14)

由行人單次邁步的前后零速區間平面位置,可以計算出單次邁步跨越臺階數為

(15)

其中,N為單次邁步跨越臺階數;thstair為跨越臺階數判斷閾值;floor(·)為向下取整函數。當行人處于上樓運動時,N值為式(16)計算值;當行人處于下樓運動時,N值為式(16)計算值的相反數,具體表達式為

(16)

利用單次邁步跨越臺階數、臺階高度信息以及邁步總數便可以得到行人的高度信息

(17)

其中,hk為行人當前高度信息;i為邁步序號;n為總邁步數;Ni為第i次邁步跨越的臺階數。

2 足間距輔助IMU行人定位系統

2.1 總體框架

本研究在行人雙腳腳跟位置安裝IMU與超聲測距模塊,采集行人運動數據,利用捷聯慣導算法解算行人導航信息。由于行人在建筑物內存在平面運動與上下樓運動,因此首先利用隨機森林算法對行人運動模式進行識別,當行人處于平面運動時,根據超聲測距模塊測得的雙足相對距離信息,建立足間距約束模型,并結合零速修正算法,利用卡爾曼濾波修正雙足導航誤差;當行人處于上下樓運動時,利用臺階高度信息進行高度解算,最終實現三維導航定位。超聲測距輔助雙足IMU行人定位系統的總體框架如圖3所示,其中γ為零速修正閾值,ΔT為時間因子。

圖3 總體框架

2.2 零速檢測

如圖4所示,行人行走過程中足部特征由運動區間和靜止區間交替構成,在靜止區間中,足部完全接觸地面,此時足部理論速度為零,直到再次進入運動區間[16]。

圖4 足部運動特征

零速修正算法將靜止時捷聯慣導解算速度作為誤差觀測量,利用卡爾曼濾波修正導航誤差[17]。而使用零速修正算法的前提是正確地劃分足部運動狀態,本文采用廣義似然比檢測法(generalized likelihood ratio test, GLRT)對足部運動狀態進行劃分[18]

(18)

(19)

本文提出基于時間因子修正的零速檢測算法,用來消除初步劃分中的誤判情況。具體步驟為:

1)根據運動模式選擇γ閾值和時間因子ΔT,利用GLRT零速檢測算法對行人足部狀態進行初步劃分。將運動區間和靜止區間分別定義為Cmove,i和Cstatic,i,其中Cmove,i表示第i個運動區間內包含的數據數量,Cstatic,i表示第i個靜止區間內包含的數據數量。

2)將各區間的數據數量與時間因子ΔT進行比較。若區間的數據數量均大于時間因子,則表示區間劃分正確,反之,若某一區間的數據數量小于時間因子,則表明該區間劃分錯誤。

3)劃分錯誤區間進行狀態翻轉,即將偽靜止區間轉換為運動區間,偽運動區間轉換為靜止區間。對行人各步態區間狀態都進行時間因子檢測,減小誤判和漏判出現的次數,提高零速區間檢測精度。

2.3 運動模式識別

行人在室內運動時,主要運動模式包括靜止、行走、跑步及上下樓等。為實現不同運動場景算法以及零速檢測閾值的動態調整,引入隨機森林算法對行人運動模式進行識別。隨機森林算法是以決策樹為基礎的集成學習模型,其原理為利用boots-trap技術獲得多個樣本子集,并使用不同樣本子集構建決策樹,當輸入待分類數據時,各決策樹共同投票實現隨機森林分類的算法[19]。分類示意圖如圖5所示,圖中數字為各動作模式的類別標簽。

圖5 運動模式分類示意圖

本文使用的IMU數據輸出頻率為200 Hz,采集的原始數據表達式為

(20)

其中,Sorig表示原始運動數據;ax,ay,az分別為x軸,y軸,z軸的加速度數據;ωx,ωy,ωz分別為x軸,y軸,z軸的角速度數據。

在采集完測試人員的運動數據后,采用加窗法對數據進行分割,窗口寬度為140,重疊率為50%,并提取三軸加速度和三軸角速度的數據特征值,降低識別和分類的復雜性,提高效率,由此得到數據集D,數據集D的表達式為

D=(F,Y)={(fi,yi)},i=1,2,…,m

(21)

其中,m為樣本個數;Y表示樣本對應的運動類型,yi表示樣本i的類型;F表示運動數據的特征矩陣,具體表示為

(22)

其中,F為樣本的特征向量,共7組運動數據時域特征;max,min分別為最大值和最小值;μ,σ分別為均值和方差;kurt,skew分別為峰度和偏度;IQR為四分位距。利用樣本的特征向量對隨機森林模型進行訓練,實現行人運動模式分類。

根據運動模式分類情況,選取相應的算法進行導航解算。針對平面運動,采用1.1節提出的基于超聲測距的足間距約束模型,并結合零速修正算法,抑制INS誤差;針對上下樓運動,采用1.2節提出的基于臺階高度與足間距的高度解算算法,計算行人高度信息,最終實現高精度行人三維慣性定位系統。

3 實驗驗證

3.1 實驗平臺搭建

本文選擇大樓室內平面和樓梯走道作為算法驗證環境,實驗人員裝備本文搭建的超聲輔助雙足IMU定位系統,按照規劃路線行進,雙足IMU定位系統示意圖如圖6所示。

圖6 雙足IMU定位系統

系統中IMU為實驗室自研的慣性測量單元,輸出頻率為200 Hz,主要性能參數如表1所示;超聲測距模塊為逐飛科技公司的“有來有去”超聲模塊,對于短距測距的誤差情況如表2所示。

表1 IMU主要性能參數

表2 超聲測距誤差結果

雙足IMU和超聲測距傳感器利用AT32F4單片機實現數據采集與存儲。超聲波測距模塊進行短距測量時,平均測量誤差為6.4 mm,測距精度誤差為2.3%,該誤差能夠滿足本文搭建系統的精度需求。

圖7 足間距離測量結果

3.2 實驗驗證與分析

設定實驗人員所處的起點坐標為(0,0),行走路線的起點與終點坐標重合,使用閉環誤差(起點和終點的歐式距離)和閉環誤差占比(閉環誤差占總路程的百分比)作為衡量算法效果的依據,實驗場地如圖8所示。

圖8 實驗路線

實驗1:平面閉環行走實驗。為驗證本文超聲波足間距約束算法的有效性,選擇折返路線作為實驗的基準路線,以星形點作為起點,以棱形點為折返點,返回起點,行進距離總計184.00 m,實驗結果如圖9所示,軌跡誤差如表3所示。

表3 平面定位誤差比較

圖9 實驗1運動軌跡

圖9中淺藍色線與綠色線為零速修正后的左腳與右腳軌跡,在終點位置雙腳平均閉環誤差為2.66 m,誤差占比為1.44%。藍色線與紅色線為經本文提出的超聲波足間距約束算法修正后得到的左腳與右腳軌跡,在終點位置雙腳平均閉環誤差為1.17 m,誤差占比為0.64%,導航誤差與零速修正算法相比下降了55.56%。

實驗2:上下樓實驗。為驗證基于臺階高度與足間距的高度解算算法的有效性,選擇實驗1中的折返路線作為基準路線,實驗人員以樓梯A圓形點作為起點,經過樓梯A從二層到達四層,沿路線到達樓梯B,經過樓梯B從四層達到二層折返點,最后沿路線回到起點,該路線二層到四層總高度為7.52 m。高度解算結果如圖10所示,高度誤差結果如表4所示,三維軌跡如圖11所示,三維定位誤差如表5所示。

表4 高度解算誤差比較

表5 三維定位誤差比較

圖10 高度解算結果

圖11 實驗2三維運動軌跡

圖10中紅色線為本文的高度解算算法得到的高度結果,其終點高度為0.07 m,高度解算平均絕對誤差為0.06 m,每層樓的高度解算誤差比較穩定;藍色線為利用零速修正解算算法得到的高度結果,由于加速度計數據積分存在累計誤差,導致高度漂移,從第三層到達第四層解算高度誤差達到最大,由第四層到達第三層時,累計誤差正負抵消,高度誤差達到最小,其終點高度為0.43 m,高度解算平均絕對誤差為0.41 m。綠色線為利用氣壓計相對高度信息和零速修正進行解算得到的高度結果,由于氣壓計易受到外界環境因素影響,導致高度解算出現誤差,每層樓的高度解算誤差相差較大,其終點高度為0.35 m,高度解算平均絕對誤差為0.17 m?；谂_階高度與足間距的高度解算算法相較于零速修正和零速修正與氣壓計聯合算法高度解算平均絕對誤差分別下降了85.37%和64.70%。

圖11中紅色軌跡為本文算法得到的三維定位結果,三維定位誤差占總路程的0.39%,相較于零速修正與氣壓計聯合算法,誤差下降了77.06%,行人在大樓內的運動軌跡,航向穩定,軌跡高度誤差小,表明該算法可以有效修正航向誤差,提高行人在大樓內的高度計算精度,實現高精度行人室內三維定位。

4 結論

本文針對基于純慣性器件的行人導航系統誤差隨時間累積致使定位精度嚴重下降的問題,提出基于足間距信息輔助的行人三維慣性定位算法。在平面運動和上下樓運動分別進行對比實驗,實驗數據顯示:

1)行人圍繞平面閉環路徑行走,本文算法解算的雙足導航系統閉環誤差占比平均值為0.64%,閉環誤差占比與零速修正算法相比下降55. 56%,證明足間約束模型能有效減小航向誤差。

2)在上下樓高度實驗中,本文算法解算的高度信息絕對平均誤差為0.06 m,與零速修正算法和零速修正與氣壓計聯合算法解算高度信息相比,平均絕對誤差分別下降了85.37%和64.70%,證明通過臺階高度與足間距信息解算高度能有效減少高度漂移。

3)本文提出的算法能在保證系統自主性與適用性的同時,有效提升足綁式IMU三維INS定位精度,能夠實現導航誤差在總路程的0.50%以內的三維定位,具有良好的工程應用價值。