基于多傳感器數據融合的室內行人航位推算法*

柳勝凱， 蘇婷立， 王彬彬， 金學波， 彭世禹

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

0 引 言

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

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

1 系統算法實現過程

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

圖1 系統算法實現步驟

2 零速檢測算法

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

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

2)利用式(1)[7]對行人的狀態進行判別，行人的狀態分為靜止和運動2個狀態

(1)

3)設定閾值η，與式(1)T(zn)值進行比較，若T(zn)<η，則系統處于靜止狀態;反之,系統處于運動狀態。η值的經過多次實驗篩選得到，本文設定η=3×104。

3 基于多只IMU傳感器的數據融合方案

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

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

圖2 數據同步處理

圖3 處理后的數據同步效果

圖4 數據融合方式

(2)

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

(3)

傳感器權重設定為wi,i=1,2,3，表示第i只傳感器采樣的權重，遵循關于m個采樣信號標準差的反比例原則：即標準差越大誤差越大，傳感器數據所占的權重就越小，說明發生漂移；標準差越小誤差越小，傳感器數據所占的權重就越大，說明更接近真值。經過推理權值wi為

(4)

(5)

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

圖5 數據融合效果

4 卡爾曼濾波更新位置狀態

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

(6)

式中 [xn,yn,zn]為導航坐標系下的數據；[xb,yb,zb]為載體坐標系下的數據。因此，通過姿態旋轉矩陣可以將載體下的加速度數據轉換到導航坐標系下，轉換方式為

(7)

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

(8)

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

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

(9)

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

(10)

(11)

(12)

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

卡爾曼濾波的模型結構為

(13)

Zk=HXk+τk

(14)

使用卡爾曼濾波[9]算法對狀態進行更新

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個元素。

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

(17)

4)融合室內二維碼信息

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

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

圖6 攝像頭測距原理

(18)

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

(19)

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

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

5 仿真結果

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

表1 慣性傳感器配置參數

5.1 單傳感器與多傳感器數據的行人軌跡對比實驗

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

5.2 參數優化對比實驗

圖7 實驗效果對比

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

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

在室內進行大范圍行走軌跡測試，將融合后的數據經過參數優化和未經過參數優化作為對比實驗，結果如圖9，對比可以看出：經過參數優化的誤差在3 m左右，而未經參數優化的誤差達到了4 m。說明經過參數優化之后的行人軌跡精度更高。

圖9 參數優化軌跡對比

5.3 融合路標數據的對比實驗

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

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

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

6 結束語

在現有使用IMU傳感器的室內導航算法的基礎上，提出了利用多傳感器數據融合及參數優化的方法，推算出將行人的軌跡誤差控制在0.5 m之內，對于研究慣性定位和組合導航技術有很好的參考和應用價值。

[1] 劉 琦,陳慶文,張智超.消防員組合定位技術的研究[J].自動化技術與應用,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慣性傳感器的行人航位推算系統[J].傳感器與微系統,2014,33(12):85-87.

[4] 孫璧文,樊啟高,武亞恒,等.基于PDR/UWB緊耦合的足綁式行人導航技術[J].傳感器與微系統,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] 石 波,李耀宗,程 敏,等.不同慣導系統零速檢測算法的性能分析[J].山東科技大學學報:自然科學版,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.