基于多傳感器數(shù)據(jù)融合的室內(nèi)行人航位推算法*

柳勝凱， 蘇婷立， 王彬彬， 金學(xué)波， 彭世禹

(北京工商大學(xué) 計算機(jī)與信息工程學(xué)院,北京 100048)

0 引 言

行人導(dǎo)航系統(tǒng)對于營救、軍事行動等一些場合起著越來越大的作用[1]，考慮室內(nèi)和室外的環(huán)境因素，行人導(dǎo)航的技術(shù)主要分為兩大板塊：使用全球定位系統(tǒng)(global positioning system,GPS)[2]和使用慣性傳感器。由于在建筑物附近或隧道中GPS的信號衰減特別嚴(yán)重，因此，GPS不適用于室內(nèi)環(huán)境下的導(dǎo)航。目前，室內(nèi)的環(huán)境中進(jìn)行行人導(dǎo)航主要基于慣性傳感器，由三軸加速度計和三軸陀螺儀組成,利用航位推算方法即可獲得行人的位置信息。由于慣性傳感器的自身存在較大的漂移誤差，嚴(yán)重制約了室內(nèi)行人的導(dǎo)航精度。李金鳳等人[3]提出了一種基于微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)慣性傳感器的行人航位推算系統(tǒng)，可以在短距離之內(nèi)對行人的軌跡優(yōu)化調(diào)整，但在長時間工作的情況下，得出的行人軌跡誤差將會快速加大。此外有研究提出了在室內(nèi)布設(shè)局域網(wǎng)絡(luò)[4～6],但網(wǎng)絡(luò)部署困難，而且其定位精度容易受環(huán)境的影響。

本文針對慣性傳感器的工作機(jī)理提出了一種多傳感器數(shù)據(jù)融合的方案，利用多個慣性傳感器進(jìn)行數(shù)據(jù)融合，并結(jié)合攝像頭提取的二維碼信息進(jìn)行行人運(yùn)動軌跡重建，實驗仿真證明，方案可以提供更加精準(zhǔn)的行人軌跡信息。

1 系統(tǒng)算法實現(xiàn)過程

1)讀取多個慣性測量單元(inertial measurement unit,IMU)傳感器的數(shù)據(jù)，利用零速檢測算法對數(shù)據(jù)進(jìn)行同步處理;2)利用處理后的數(shù)據(jù)求解姿態(tài)矩陣以及使用零速檢測算法標(biāo)定行人運(yùn)動狀態(tài)，并將處理的數(shù)據(jù)通過卡爾曼濾波器進(jìn)行參數(shù)優(yōu)化計算；3)將攝像頭解析的路標(biāo)信息和IMU傳感器的信息進(jìn)行融合，得到最終的行人軌跡信息。具體實現(xiàn)步驟如圖1所示。

圖1 系統(tǒng)算法實現(xiàn)步驟

2 零速檢測算法

零速檢測[6]通過將檢測到的零速時段作為系統(tǒng)速度誤差的外部測量信息，對系統(tǒng)的各項狀態(tài)進(jìn)行控制校正，以提高系統(tǒng)定位精度。零速檢測的方法主要分為3個過程：

1)采集IMU的加速度和陀螺儀返回的數(shù)據(jù)，設(shè)定為

2)利用式(1)[7]對行人的狀態(tài)進(jìn)行判別，行人的狀態(tài)分為靜止和運(yùn)動2個狀態(tài)

(1)

3)設(shè)定閾值η，與式(1)T(zn)值進(jìn)行比較，若T(zn)<η，則系統(tǒng)處于靜止?fàn)顟B(tài);反之,系統(tǒng)處于運(yùn)動狀態(tài)。η值的經(jīng)過多次實驗篩選得到，本文設(shè)定η=3×104。

3 基于多只IMU傳感器的數(shù)據(jù)融合方案

本文采用3只IMU傳感器對行人的位姿進(jìn)行跟蹤，傳感器的采樣周期為0.01s，傳感器安裝在鞋面位置。

對傳感器的數(shù)據(jù)采取融合操作之前，需要保證3只慣性傳感器所采集的數(shù)據(jù)同步，否則融合后的數(shù)據(jù)將會產(chǎn)生巨大的誤差。本文利用零速檢測算法找到每只傳感器的起始點位置，如圖2所示，起始點之前的數(shù)據(jù)直接丟棄，將起始點之后的3只慣性傳感器的數(shù)據(jù)使用最大反比例方法進(jìn)行融合。經(jīng)過數(shù)據(jù)同步后的3只慣性傳感器的數(shù)據(jù)對齊情況如圖3所示，滿足對行人軌跡進(jìn)行跟蹤的要求。數(shù)據(jù)融合方式以陀螺儀傳感器為例，如圖4所示。

圖2 數(shù)據(jù)同步處理

圖3 處理后的數(shù)據(jù)同步效果

圖4 數(shù)據(jù)融合方式

(2)

式中ri,j為線性配置的第i個傳感器中第j個采樣數(shù)據(jù)，由式(3)計算出i只傳感器進(jìn)行m個連續(xù)采樣的平均值

(3)

傳感器權(quán)重設(shè)定為wi,i=1,2,3，表示第i只傳感器采樣的權(quán)重，遵循關(guān)于m個采樣信號標(biāo)準(zhǔn)差的反比例原則：即標(biāo)準(zhǔn)差越大誤差越大，傳感器數(shù)據(jù)所占的權(quán)重就越小，說明發(fā)生漂移；標(biāo)準(zhǔn)差越小誤差越小，傳感器數(shù)據(jù)所占的權(quán)重就越大，說明更接近真值。經(jīng)過推理權(quán)值wi為

(4)

(5)

經(jīng)過最大反比例方法得到的效果如圖5所示。

圖5 數(shù)據(jù)融合效果

4 卡爾曼濾波更新位置狀態(tài)

獲得融合的數(shù)據(jù)后，需要利用卡爾曼濾波器和零速檢測算法對數(shù)據(jù)進(jìn)行處理，以得到行人軌跡。對數(shù)據(jù)處理的過程可以分為如下過程：

(6)

式中 [xn,yn,zn]為導(dǎo)航坐標(biāo)系下的數(shù)據(jù)；[xb,yb,zb]為載體坐標(biāo)系下的數(shù)據(jù)。因此，通過姿態(tài)旋轉(zhuǎn)矩陣可以將載體下的加速度數(shù)據(jù)轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下，轉(zhuǎn)換方式為

(7)

式中f為導(dǎo)航系下3個方位上除去重力效應(yīng)的加速度值；[aXaYaZ]為載體坐標(biāo)系下加速度計(x,y,z)三個軸上的加速度數(shù)據(jù)；gn為重力加速度的值。獲得f的值之后，利用牛頓運(yùn)動定律即可得到行人軌跡信息。

(8)

式中wx，wy和wz分別為陀螺儀三個軸輸出的角速度值。

利用畢卡逼近方法得到四元數(shù)的更新迭代為

(9)

式中q(0)為初始的四元數(shù)矩陣，可通過初始加速度計的輸出值計算得到;Δθ0為角度的變化量，可以通過式(10)計算到，其中,Δθx，Δθy，Δθz代表陀螺儀三個軸上的角度變化量，由式Δθx=wx×Δt,Δθy=wy×Δt,Δθz=wx×Δt計算

(10)

(11)

(12)

2)由于慣性傳感器在長時間工作中會產(chǎn)生積分漂移，對行人軌跡的運(yùn)算產(chǎn)生較大影響，因此,需要利用零速檢測算法和卡爾曼濾波器對行人的軌跡進(jìn)行優(yōu)化，即在行人腳掌落地時的一瞬間對行人的軌跡進(jìn)行矯正。

卡爾曼濾波的模型結(jié)構(gòu)為

(13)

Zk=HXk+τk

(14)

使用卡爾曼濾波[9]算法對狀態(tài)進(jìn)行更新

3)利用牛頓第二定律，由式(15)和式(16)求得行人軌跡

(15)

(16)

定義D=k,k-k,k-1，為一個9維度的向量D=[D(1)D(2)D(3)…D(9)]，D(1-3)為D的前3個元素，D(4-6)代表D的4～6個元素。

這種方法得到的行人軌跡存有較大的誤差，基于此本文提出了卡爾曼濾波器的參數(shù)優(yōu)化方法，即在位置更新時加入一個參數(shù)調(diào)節(jié)，如式(17)，λ值根據(jù)實驗人員的身高體重比進(jìn)而得到的一個實驗優(yōu)化參數(shù)，通過實驗可以看出在短時間內(nèi)可有效提高行人的室內(nèi)導(dǎo)航精度

(17)

4)融合室內(nèi)二維碼信息

在實驗中發(fā)現(xiàn)慣性傳感器長時間工作時，參數(shù)優(yōu)化方案對行人軌跡誤差的抑制作用有限，為解決這個問題，本文中引入將路標(biāo)信息嵌入二維碼的方式，通過攝像頭相應(yīng)位置的二維碼標(biāo)簽進(jìn)行解析，并將路標(biāo)(信息)位置和經(jīng)過參數(shù)優(yōu)化后的行人位置信息融合，以達(dá)到對慣性傳感器定點校正的效果。室內(nèi)識別二維碼信息直接采用OpenCV下的zbar解析包，對二維碼的信息進(jìn)行解算，其中,二維碼承載的信息為室內(nèi)全局地圖的坐標(biāo)信息。

利用攝像頭解析出二維碼的路標(biāo)信息后，需要計算二維碼相對于攝像頭的距離，標(biāo)定攝像頭。單目攝像頭的測距原理如圖6所示。

圖6 攝像頭測距原理

(18)

實際應(yīng)用中，攝像頭解析二維碼時，總會出現(xiàn)二維碼相對于攝像頭的角度偏移現(xiàn)象。在zbar包里可以直接使用angle庫函數(shù)計算即可得到二維碼實際中相對于攝像頭的距離和方位。攝像頭相對于二維碼的位姿為

(19)

式中xb,yb為攝像頭相對于二維碼所在的坐標(biāo)；xqr,yqr為利用攝像頭解析出的二維碼的路標(biāo)信息；d由式(18)計算得到;ψ為二維碼相對于攝像頭的偏轉(zhuǎn)角度。

將攝像頭解算出的位置信息xb,yb與慣性傳感器解算得到位置信息Sk進(jìn)行一個求和平均以得到最終的行人(軌跡)位置信息。通過實驗表明:該方法可以在很大程度上降低行人的軌跡誤差，驗證了本文算法的有效性。

5 仿真結(jié)果

實驗所采用的裝置為MTI—3慣性傳感器，分別在小型室內(nèi)和大型室內(nèi)的場合進(jìn)行實驗操作。IMU傳感器的具體參數(shù)如表1。

表1 慣性傳感器配置參數(shù)

5.1 單傳感器與多傳感器數(shù)據(jù)的行人軌跡對比實驗

將3只慣性傳感器綁定在腳上，行人在室內(nèi)小范圍行走，并記錄實驗數(shù)據(jù)。通過處理所獲得傳感器的數(shù)據(jù)可以得到行人的軌跡信息，對比2種實驗方案的結(jié)果如圖7，經(jīng)多傳感器融合得到的數(shù)據(jù)和單傳感器數(shù)據(jù)之間的軌跡對比情況，可以看出：圖7(b)更貼近于行人的實際行走軌跡。圖8為融合數(shù)據(jù)得到的軌跡和實際軌跡的對比情況。表2給出了經(jīng)過計算得出的軌跡信息和實際軌跡信息之間的誤差情況。

5.2 參數(shù)優(yōu)化對比實驗

圖7 實驗效果對比

圖8 融合軌跡與實際軌跡的對比

傳感器軌跡誤差/m軌跡誤差占總長度比/%傳感器112．5857033．5傳感器24．2399211．3傳感器36．9851818．6融合后的傳感器1．356253．6

在室內(nèi)進(jìn)行大范圍行走軌跡測試，將融合后的數(shù)據(jù)經(jīng)過參數(shù)優(yōu)化和未經(jīng)過參數(shù)優(yōu)化作為對比實驗，結(jié)果如圖9，對比可以看出：經(jīng)過參數(shù)優(yōu)化的誤差在3 m左右，而未經(jīng)參數(shù)優(yōu)化的誤差達(dá)到了4 m。說明經(jīng)過參數(shù)優(yōu)化之后的行人軌跡精度更高。

圖9 參數(shù)優(yōu)化軌跡對比

5.3 融合路標(biāo)數(shù)據(jù)的對比實驗

通過上述2個實驗可以看出多傳感器數(shù)據(jù)融合和參數(shù)優(yōu)化可以將誤差縮小，但3 m的誤差依舊很大。為了能將誤差最小化，本文利用攝像頭識別的二維碼路標(biāo)信息，對行人的軌跡再次進(jìn)行優(yōu)化。

將攝像頭安裝于行人側(cè)部且盡可能固定住攝像頭，此外需要墻壁上每隔一定的距離粘貼二維碼。經(jīng)過實驗，得到結(jié)果如圖10，通過對比可以看出：加入攝像頭之后誤差縮小到了0.5 m之內(nèi)，因此,該方法可以有效提高行人軌跡的精度。

圖10 加入攝像頭前后的行人軌跡結(jié)果對比

6 結(jié)束語

在現(xiàn)有使用IMU傳感器的室內(nèi)導(dǎo)航算法的基礎(chǔ)上，提出了利用多傳感器數(shù)據(jù)融合及參數(shù)優(yōu)化的方法，推算出將行人的軌跡誤差控制在0.5 m之內(nèi)，對于研究慣性定位和組合導(dǎo)航技術(shù)有很好的參考和應(yīng)用價值。

[1] 劉 琦,陳慶文,張智超.消防員組合定位技術(shù)的研究[J].自動化技術(shù)與應(yīng)用,2017,36(7):34-37.

[2] Chen L,Hu H.IMU/GPS-based pedestrian localization[C]∥Computer Science and Electronic Engineering Conference,IEEE,2012:23-28.

[3] 李金鳳,王慶輝,劉曉梅,等.基于MEMS慣性傳感器的行人航位推算系統(tǒng)[J].傳感器與微系統(tǒng),2014,33(12):85-87.

[4] 孫璧文,樊啟高,武亞恒,等.基于PDR/UWB緊耦合的足綁式行人導(dǎo)航技術(shù)[J].傳感器與微系統(tǒng),2017,36(3):43-47.

[5] Li Y,Zhuang Y,Zhang P,et al.An improved inertial/WiFi/magnetic fusion structure for indoor navigation[J].Information Fusion,2017,34(C):101-119.

[6] Skog I,Handel P,Nilsson J O,et al.Zero-velocity detection—an algorithm evaluation[J].IEEE Transactions on Biomedical Engineering,2010,57(11):2657-2666.

[7] 石 波,李耀宗,程 敏,等.不同慣導(dǎo)系統(tǒng)零速檢測算法的性能分析[J].山東科技大學(xué)學(xué)報:自然科學(xué)版,2016,35(2):57-63.

[8] Grigorie L T,Botez R M,Sandu D.Experimental testing of a data fusion algorithm for miniaturized sensors in redundant configurations[C]∥The 2014 International Conference on Mathematical Methods,Mathematical Models and Simulation in Science and Engineering,MMSSE 2014,2014.

[9] Yi J,Zhang L,Shu R,et al.Initial alignment for SINS based on low-cost IMU[J].Journal of Computers,2011,6(6):1080-1085.