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

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

0 引 言

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

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

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

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

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

2 零速檢測(cè)算法

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

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

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

(1)

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

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

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

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

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

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

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

(2)

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

(3)

傳感器權(quán)重設(shè)定為wi,i=1,2,3，表示第i只傳感器采樣的權(quán)重，遵循關(guān)于m個(gè)采樣信號(hào)標(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ù)后，需要利用卡爾曼濾波器和零速檢測(cè)算法對(duì)數(shù)據(jù)進(jìn)行處理，以得到行人軌跡。對(duì)數(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個(gè)方位上除去重力效應(yīng)的加速度值；[aXaYaZ]為載體坐標(biāo)系下加速度計(jì)(x,y,z)三個(gè)軸上的加速度數(shù)據(jù)；gn為重力加速度的值。獲得f的值之后，利用牛頓運(yùn)動(dòng)定律即可得到行人軌跡信息。

(8)

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

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

(9)

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

(10)

(11)

(12)

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

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

(13)

Zk=HXk+τk

(14)

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

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

(15)

(16)

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

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

(17)

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

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

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

圖6 攝像頭測(cè)距原理

(18)

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

(19)

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

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

5 仿真結(jié)果

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

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

5.1 單傳感器與多傳感器數(shù)據(jù)的行人軌跡對(duì)比實(shí)驗(yàn)

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

5.2 參數(shù)優(yōu)化對(duì)比實(shí)驗(yàn)

圖7 實(shí)驗(yàn)效果對(duì)比

圖8 融合軌跡與實(shí)際軌跡的對(duì)比

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

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

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

5.3 融合路標(biāo)數(shù)據(jù)的對(duì)比實(shí)驗(yàn)

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

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

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

6 結(jié)束語

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

[1] 劉 琦,陳慶文,張智超.消防員組合定位技術(shù)的研究[J].自動(dòng)化技術(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)零速檢測(cè)算法的性能分析[J].山東科技大學(xué)學(xué)報(bào):自然科學(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.