一種多傳感器融合的行人多模式自主定位方法

亓 林, 劉 宇, 芶志平, 楊 勇, 路永樂, 鄒新海

(1.重慶郵電大學(xué) 智能傳感技術(shù)與微系統(tǒng)重慶市高校工程研究中心，重慶 400065；2.中國電子科技集團(tuán)公司第二十六研究所，重慶 400060)

0 引言

隨著網(wǎng)絡(luò)的發(fā)展，行人可以通過全球定位系統(tǒng)迅速獲取位置及其路徑信息，但在全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)拒止的環(huán)境里，行人的蹤跡變得“捉摸不透”。為解決這一難題，室內(nèi)定位技術(shù)應(yīng)運(yùn)而生[1]。目前常用的室內(nèi)定位方法主要有基于紅外、超聲波、射頻、Wi-Fi、超寬帶(UWB)等[2-5]。紅外探測功耗大，需要貼附電子標(biāo)識，實(shí)用性較低；超聲波精度較高，但衰減明顯，需要若干電子標(biāo)簽；射頻對網(wǎng)絡(luò)穩(wěn)定性要求較高，易受環(huán)境干擾；Wi-Fi價格低，精度較高，但需要預(yù)知定位環(huán)境的地圖；UWB精度能達(dá)到厘米級，但成本極高，不適合普遍推廣?；谑謾C(jī)內(nèi)置的慣性傳感器組的航位推算[6]，因其短時間精度高、連續(xù)性、低成本等特點(diǎn)，被廣泛應(yīng)用于行人自主定位中。

基于慣性傳感組件的方法主要采集智能手機(jī)自帶的慣性測量單元(IMU)的傳感器數(shù)據(jù)[7]，利用步長檢測和步數(shù)統(tǒng)計方法，結(jié)合基于陀螺儀、磁力計和加速度計的航向角解算方法，在短時間內(nèi)準(zhǔn)確地計算行人路徑。在姿態(tài)更新過程中，傳統(tǒng)航跡推算法更適合的步態(tài)模式為前進(jìn)模式，在后向、左側(cè)、右側(cè)行進(jìn)更新中會出現(xiàn)反向或垂直誤差，造成最后姿態(tài)更新軌跡偏離實(shí)際行徑[8]。為解決這一難題，本文改進(jìn)傳統(tǒng)姿態(tài)更新算法，增加了步態(tài)模式系數(shù)，算法可根據(jù)行人步態(tài)的不同，自主完成路徑跟蹤，使行人在正常行走時，可不借助外界輔助標(biāo)記點(diǎn)，完成包含前、后、左、右步態(tài)的行走路徑跟蹤。

1 行人多模式自主定位方法

1.1 自主定位基本原理

基于多傳感器融合的行人多模式自主定位算法(MSMDR)，其原理框圖如圖1所示。

圖1 MSMDR系統(tǒng)框架

圖1中，ax,ay,az分別為加速度計x、y、z軸的輸出；gx,gy,gz分別為陀螺儀x、y、z軸的輸出；mx,my,mz分別為磁力計x、y、z軸的輸出；Lk為步長;φ為航向角;Pk為前、后步態(tài)系數(shù)；Nk為左、右為步態(tài)系數(shù)；k為第k個采樣點(diǎn)，k≤N，N為總的采樣點(diǎn)數(shù)。

整體思路為：

1) 采集移動終端的IMU數(shù)據(jù)，進(jìn)行數(shù)據(jù)預(yù)處理得到9軸的傳感器數(shù)據(jù)。

2) 利用加速度計數(shù)據(jù)進(jìn)行步態(tài)檢測和步長估計，分別得到Pk和Nk。

3) 通過四元數(shù)法獲取φ。

4) 采用改進(jìn)的位置更新算法得到行人的位置信息。

1.2 步態(tài)檢測與步長估計

在實(shí)時性要求較高的步態(tài)檢測過程中，采用加速度計的時域特征來識別步態(tài)模式。行人在行進(jìn)過程中，加速度計數(shù)據(jù)的垂直分量呈現(xiàn)周期變化，根據(jù)此階躍特征可以檢測行人的步數(shù)。由于加速度原始數(shù)據(jù)中含有噪聲，需要采用低通濾波平滑處理。手機(jī)朝向如圖2所示。

圖2 智能手機(jī)的手持姿勢

在向前或向后行走時，y軸作為方向軸，提取y、z軸的數(shù)據(jù)，定義K1和K2(z軸波峰或波谷時刻對應(yīng)的y軸加速度數(shù)據(jù)的斜率)的值；在向左或向右行走時，x軸作為方向軸，提取x、z軸的數(shù)據(jù)，定義K3和K4(z軸波峰或波谷時刻對應(yīng)的x軸加速度數(shù)據(jù)的斜率)的值，利用波峰波谷檢測方法檢測步態(tài)[9]，即

(1)

(2)

式中m,n均為在1個周期內(nèi)z軸波峰或波谷檢測時刻。

1) 測試者手持智能機(jī)向前走6 m，再向后走6 m回到起點(diǎn)。加速度的采樣頻率設(shè)置為50 Hz，由于加速度計原始數(shù)據(jù)疊加高頻隨機(jī)噪聲，首先采用滑動平均濾波進(jìn)行低通濾波，窗口長度設(shè)置為10。圖3為前進(jìn)、后退數(shù)據(jù)。

圖3 前進(jìn)、后退數(shù)據(jù)

2) 測試者手持智能機(jī)向左走5 m，再向右走5 m回到起點(diǎn)。圖4為左移、右移數(shù)據(jù)。

圖4 左移、右移數(shù)據(jù)

當(dāng)滿足K1>0與K2>0時，向前行走，m值取波峰到來時刻，向后行走，m值取波谷到來的時刻；當(dāng)滿足K3<0與K4<0時，向左行走，n值取波峰到來時刻；當(dāng)滿足條件K3>0與K4>0時，向右行走，n值取波峰到來時刻。

步長估計常用的方法是將一段測得的行走距離除以計算得到的步數(shù)，稱為平均步長。但實(shí)際上人在行走時的姿勢會有所變化，步長也隨之改變，本文采用Weinberg模型[10]來計算實(shí)時步長：

(3)

式中：amax，amin分別為步態(tài)檢測中1個周期內(nèi)加速度的最大值和最小值；C為實(shí)際距離和估計距離的比例系數(shù)，通過多次試驗(yàn)可以獲取其經(jīng)驗(yàn)值。

1.3 姿態(tài)解算

四元數(shù)法[11]能夠唯一確定兩個坐標(biāo)系之間的位置關(guān)系，且可以實(shí)現(xiàn)全姿態(tài)測量，計算量較小，實(shí)際生活中被廣泛采用。四元數(shù)矩陣由4個元素構(gòu)成：

Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k

(4)

式中：q0,q1,q2,q3為實(shí)數(shù)；i,j,k為單位向量且相互正交。

姿態(tài)矩陣四元數(shù)的微分方程為

(5)

(6)

本文采用四階龍格-庫塔法[12]求解四元數(shù)的微分方程。設(shè)四元數(shù)的更新時間為T，則有

(7)

式中ki為求解過程中積分區(qū)間內(nèi)插值的斜率，i=1,2,3,4。

其中，

(8)

(9)

使用四元數(shù)法進(jìn)行姿態(tài)更新,其計算簡單，精度和實(shí)時性較好，多用于姿態(tài)測量領(lǐng)域。

1.4 位置更新

在坐標(biāo)系xOy中(見圖5)，傳統(tǒng)的行人位置更新的基本原理[13]是：設(shè)行人的起始位置為A(xk-1,yk-1)，姿態(tài)角為φk-1，A到B行人步長為Lk，則行人下一步達(dá)到的位置為B(xk,yk)，則有

(10)

圖5 位置更新傳統(tǒng)模型

但是，在傳統(tǒng)航位推算中(見圖6)，如果行人從A走到B，再由B走到C，則位置更新會在C；如果行人從A到B，再返回A，傳統(tǒng)航位推算的位置更新有時會定位在C，將產(chǎn)生反向誤差；如果行人從A到B，再到D或E，傳統(tǒng)航位推算的位置更新結(jié)果依然在C，將產(chǎn)生垂直方向的誤差。

圖6 本文位置更新模型

根據(jù)本文步態(tài)檢測將行人步態(tài)分為前、后、左、右4個方向的步行姿態(tài)，引入Pk和Nk，位置更新公式變?yōu)?/p>

(11)

融合步態(tài)系數(shù)的位置更新方式執(zhí)行條件如表1所示。

表1 步態(tài)系數(shù)賦值表

2 實(shí)驗(yàn)結(jié)果

根據(jù)式(1)、(2)測試行人步態(tài)，經(jīng)過10次的步態(tài)檢測統(tǒng)計結(jié)果如表2所示。根據(jù)前、后、左、右步態(tài)測試結(jié)果，可計算其檢測準(zhǔn)確率≥92%，由此可驗(yàn)證本文步態(tài)檢測算法的準(zhǔn)確性較高。

表2 步態(tài)模式檢測結(jié)果

后面測試者將選擇地下車庫。按照圖2所示的方式，通過數(shù)據(jù)提取軟件獲得如圖7所示的原始數(shù)據(jù)結(jié)果。

圖7 IMU原始數(shù)據(jù)

估算出測試者的每一步步長，其結(jié)果如圖8所示，按照四元數(shù)解算的航向角如圖9所示。

圖8 步長估計結(jié)果

圖9 航向角解算結(jié)果

測試者的步長為0.584 5～0.773 9 m，此數(shù)據(jù)集在經(jīng)驗(yàn)統(tǒng)計范圍內(nèi)，驗(yàn)證了本文步長估計算法的可行性。由圖9可知，在每個環(huán)形步行周期內(nèi)，航向角解算結(jié)果均較平滑，準(zhǔn)確性較好，但在每個周期開始前，由于陀螺儀靜止時易受零漂等因素的影響，數(shù)據(jù)相對不可靠，由此解算的航向角有偏差。最終對行人的位置進(jìn)行更新，其結(jié)果如圖10所示。

圖10 位置更新結(jié)果

通過本文位置更新算法的改進(jìn)，行人可不在任何其他地標(biāo)或標(biāo)簽輔助的前提下，單靠IMU可完成自主路徑跟蹤，驗(yàn)證了本文方法的可行性。對比實(shí)際路徑與參考路徑的數(shù)據(jù)，其位置距離誤差為2.8 m，如圖11所示。

圖11 路徑偏離誤差

3 結(jié)束語

在GNSS拒止的環(huán)境里，行人位置不定，但借助移動設(shè)備中配備的IMU，利用航位推算法可實(shí)時地獲取行人的位置信息。針對傳統(tǒng)航位推算法的缺點(diǎn)，本文增加了步態(tài)模式系數(shù)，改進(jìn)了位置更新方程，最終獲得單獨(dú)依靠IMU自主定位的效果，其定位精度較好。由于市面上使用基于MEMS的IMU較多，其精度相對較低，為提高定位精度，我們需要不斷改進(jìn)算法，移動設(shè)備給我們提供了一個開放測試平臺。