基于PDR/UWB緊耦合的足綁式行人導航技術(shù)*

孫璧文， 樊啟高， 武亞恒， 孫 艷

(江南大學 物聯(lián)網(wǎng)工程學院，江蘇 無錫 214122)

基于PDR/UWB緊耦合的足綁式行人導航技術(shù)*

孫璧文， 樊啟高， 武亞恒， 孫 艷

(江南大學 物聯(lián)網(wǎng)工程學院，江蘇 無錫 214122)

為了解決低成本微機電慣性導航系統(tǒng)存在的累積誤差問題，提出一種基于融合行人航跡推算(PDR)和超寬帶(UWB)無線定位的實時室內(nèi)行人導航系統(tǒng)。利用加速度計和磁強計進行初始姿態(tài)對準；考慮濾波誤差估計，推導了慣性導航算法；依靠加速度計和陀螺儀的“與”邏輯進行行人步態(tài)檢測；實施零速更新(ZUPT)提供速度誤差觀測量，利用UWB系統(tǒng)提供位置誤差觀測量；設計具有野值辨識機制的擴展卡爾曼濾波器進行數(shù)據(jù)融合。對提出的行人導航算法進行實驗驗證，結(jié)果表明該行人導航算法與傳統(tǒng)定位方法相比能夠有效提高行人定位精度。實驗中，該行人導航算法能夠獲取低于0.2 m的定位誤差，且穩(wěn)定、不發(fā)散。

室內(nèi)定位； 行人航跡推算； 超寬帶； 擴展卡爾曼濾波

0 引 言

行人導航系統(tǒng)已經(jīng)被用于定位服務、營救、運動以及軍事應用等方面[1]。在室外，全球定位系統(tǒng)(GPS)得到廣泛應用。但由于建筑物的阻擋和衛(wèi)星信號衰減，GPS無法在室內(nèi)進行有效定位。現(xiàn)有的室內(nèi)定位技術(shù)可以分為兩大類：無線定位系統(tǒng)和傳感器定位系統(tǒng)。無線定位系統(tǒng)(wireless location system,WLS)通過測量距離或者角度的三角測量法來估算物體坐標。常見的無線定位系統(tǒng)[2]如藍牙、WiFi、RFID,Zig Bee等，它們的定位精度易受外部環(huán)境影響，如非視距因素和多徑因素。

與無線定位方式不同，傳感器定位系統(tǒng)并不需要依附于安裝好的外界設備。在導航應用方面，慣性導航系統(tǒng)(inertial navigation system,INS)是最常見的傳感器定位系統(tǒng)。在過去的幾年，常使用行人航跡推算(pedestrian dead reckoning,PDR)法[3]進行行人定位，需要將慣性傳感器安裝在身體的某個部位，通過加速度計數(shù)據(jù)與陀螺儀數(shù)據(jù)計算步長、步態(tài)以及航向等信息，來估算行人最終位置。最近微機電系統(tǒng)(micro-electro-mechanical system,MEMS)技術(shù)發(fā)展使得慣性測量元件更加適用于PDR方法。但是MEMS慣性傳感器存在隨機漂移誤差以及噪聲誤差，長時間定位會產(chǎn)生很大累積誤差。為了減小累積誤差，部分學者提出各自的步態(tài)檢測算法、步長與航向估計算法等[4～6]。這些研究都集中利用運動學規(guī)律，設計精妙算法抑制(INS)的誤差，但是依舊無法避免誤差累積問題。

為了徹底消除累積誤差，一些學者使用INS與WiFi、RFID等[7,8]無線定位系統(tǒng)進行融合。這種INS+WLS的融合概念已經(jīng)應用于室外GPS與INS定位，并能夠提供無漂移的定位結(jié)果。在無線定位領域，超寬帶(ultra wideband，UWB)系統(tǒng)擁有獨特的定位優(yōu)勢，具有時間分辨率高、穿透能力強、發(fā)射功率低和數(shù)據(jù)傳輸速率高等特點[9]。因此，可以使用UWB無線技術(shù)來消除INS的累積誤差。一些學者試圖將UWB與INS技術(shù)進行融合[10]，但他們研究方面只適用于車輛定位，而且INS的定位優(yōu)勢無法最大化利用。

本文提出一種基于足綁式慣性測量單元(inertial mea-surement unit,IMU)與UWB室內(nèi)行人組合定位系統(tǒng)，該系統(tǒng)包括初始靜態(tài)對準算法、IMU導航算法、行人步態(tài)檢測算法、系統(tǒng)濾波方程以及野值辨識算法。在封閉環(huán)境下進行多種步態(tài)實驗，結(jié)果表明提出的緊耦合模型算法能夠獲取低于0.2 m的定位精度。

1 基于IPZE/UWB算法的定位模型

利用加速度計和磁強計進行初始姿態(tài)對準；考慮濾波誤差估計，推導了慣性導航算法；依靠加速度計和陀螺儀的“與”邏輯進行行人步態(tài)檢測；在站立相時刻實施零速更新(zero velocity update,ZUPT)提供速度誤差觀測量，利用UWB系統(tǒng)提供位置誤差觀測量；設計具有野值辨識機制的擴展卡爾曼濾波器(extended Kalman filter,EKF)，對位置、速度、姿態(tài)、加速度以及角速度進行最優(yōu)估計，形成IPZE(INS-PDR-ZUPT-EKF)定位與UWB定位的緊耦合算法。如圖1為提出的IPZE/UWB緊耦合模型。

圖1 IPZE/UWB 緊耦合模型

1.1 靜態(tài)初始對準

在不考慮慣性傳感器誤差的情況下，加速度計的輸出為

(1)

(2)

(3)

Mx=Mbxcosθ+Mbysinφ+Mbzcosφ

(4)

My=Mbycosφ-Mbz

(5)

式中Mbx，Mby和Mbz分別為磁強計的三軸輸出值。Mx和My分別為磁場在n系的x軸和y軸分量。所以，航向角的計算公式

ψ=tan-1(My/Mx)-Md

(6)

式中Md為IMU所在地點的地球磁偏角。

1.2 慣性導航解算

(7)

(8)

2)姿態(tài)更新：基于k時刻的角速度和k-1時刻的姿態(tài)預測k時刻的姿態(tài)，用方向余弦矩陣表示

(9)

(10)

3)重力補償：消除補償加速度中重力的影響

(11)

4)位姿計算：位置、速度和姿態(tài)的估計。k時刻速度和位置的預測

(12)

r(k,k-1)=r(k-1,k-1)+v(k,k-1)·Δt

(13)

式中 v(k-1, k-1)和r(k-1,k-1)分別為k-1時刻的速度和位置估計。k時刻位置、速度和姿態(tài)的估計分別是

r(k,k)=r(k,k-1)-δrk

(14)

v(k,k)=v(k,k-1)-δvk

(15)

(16)

(17)

1.3 步態(tài)檢測算法

當行人足部被檢測到在地面靜止時(站立相)，EKF算法會獲取到誤差觀測量。大部分步態(tài)檢測算法僅僅依靠對加速度或者角速度設定閾值來進行站立相檢測，但是在慢走或者隨意行走時，這些方法會存在一些誤差。本文基于多條件判別算法進行站立相判定，以提高步態(tài)檢測的魯棒性。具體判定如下：

1)加速度閾值判定條件

(18)

式中 |ak|表示k時刻加速度幅值，|ak| =[ak(1)2+ak(2)2+ak(3)2]0.5。min(Ta)和max(Ta)為設定的最小和最大加速度閾值，即|ak|在閾值范圍之內(nèi)，則表示C1為真。

2)角速度閾值判定條件

(19)

式中 |ωk|為k時刻角速度幅值，|ωk| =[ωk(1)2+ωk(2)2+ωk(3)2]0.5。Tω為設定的最大角速度閾值，即|ωk|小于Tω，則表示C2為真。

3)局部加速度方差判定條件

(20)

式中 σa為局部加速度方差，其具體計算方法如式(21)所示。Tσ為設定的最大閾值，即當σa小于Tσ時，表示C3為真。式(21)中，s為定義的移動窗口

(21)

(22)

只有當C1,C2和C3均為真時，則判定此時為站立相。具體的閾值設定與步態(tài)檢測結(jié)果如實驗部分所示。

1.4 濾波方程

由于系統(tǒng)狀態(tài)轉(zhuǎn)移模型為非線性狀態(tài)函數(shù)，常規(guī)卡爾曼濾波無法處理非線性問題，因此本文使用EKF算法進行數(shù)據(jù)處理，設計一個包含15個狀態(tài)量的擴展卡爾曼濾波器，其中位置誤差δP、速度誤差δV、姿態(tài)角(翻滾角、俯仰角以及航向角)誤差δΦ、加速度誤差δa以及角速度誤差δω。濾波方程包括狀態(tài)方程和觀測方程。

1)狀態(tài)方程

令狀態(tài)量：X= [δP,δV,δΦ,δa,δω]，系統(tǒng)狀態(tài)方程為

δXk/k-1=FkδXk-1/k-1+Wk-1

(23)

(24)

(25)

(26)

2)觀測方程

觀測方程模型為

Zk=HδXk/k+Vk

(27)

δXk/k=δXk/k-1+Kk·[Zk-HδXk/k-1]

(28)

式中Kk為卡爾曼增益，δXk/k-1是預測狀態(tài)量。在本文中，系統(tǒng)觀測量選用線速度誤差Δvk以及位置誤差ΔLi。

在速度誤差補償方面，當系統(tǒng)檢測到站立相時，認定此時腳的真實速度為0 m/s。將慣性導航測量的實際速度與真實速度之差Δvk作為此時速度的觀測量，其相應的觀測矩陣如下

H1=[03×3I3×303×303×303×3]

(29)

在位置誤差補償方面，使用UWB定位系統(tǒng)對位置進行修正，因此需建立相應的位置觀測方程。設置UWB錨節(jié)點的矢量坐標為pi=[xi,yi]T,i=1,2,…,N，其中N是定位基站總數(shù)。在k時刻，盲節(jié)點的矢量坐標是puwb=[xuwb,yuwb]T，慣性導航測量出行人的位置是pins=[xins,yins]T。由于盲節(jié)點安裝在行人身上，因此，盲節(jié)點到第i個錨節(jié)點間的距離為

duwb,i=‖puwb-pi‖

(30)

dins,i=‖pins-pi‖

(31)

由式(30)和式(31)可得

(32)

將第2～N個等式依次減去第1個等式，得到UWB定位誤差觀測量

ΔLi=Δli-Δl1=-2(p1pins-p1pins-pipuwb+p1puwb)=

-2(pi-p1)×Δp

(33)

式中Δp=pins-puwb，i=2…N。結(jié)合式(29)和式(33)可得系統(tǒng)觀測量和觀測矩陣

Zk=[Δvk,ΔL2,…,ΔLN]T

(34)

(35)

(36)

本系統(tǒng)只考慮二維平面坐標定位，因此Z軸方向上的觀測量設為0。

1.5 野值辨識機制

在實際運動過程中，UWB系統(tǒng)實時測量當前行人位置。而UWB測量值很容易受到環(huán)境干擾，產(chǎn)生野值。所以需要對野值進行辨識并修正，這樣才能使得EKF濾波器穩(wěn)定與收斂。因此基于新息正交性理論，有如下等式

(37)

記

(38)

根據(jù)式(38)兩邊矩陣的對角線元素，對量測量Zk的位置誤差分量Zi,k是否為野值進行假設判斷，判別如下

Bi,k∈[Mi,k-εi,Mi,k+εi]

(39)

(40)

在每個濾波周期內(nèi)，對量測量Zk中各個分量分別進行野值辨識。當Zk不是野值時，權(quán)重因子為單位值，不改變新息序列；而當Zk為野值時，則使用小于1的活化函數(shù)進行加權(quán)限制，保持新息的正交性。使用具有抗野值機制的EKF算法對PDR/UWB組合系統(tǒng)進行融合濾波，提高整個系統(tǒng)的容錯能力以及定位精度。

2 實 驗

2.1 實驗設置

為了評估提出的緊耦合模型的定位效果，需要對實驗環(huán)境進行相關配置。本文使用Xsens公司的MTi微慣性航姿模塊以及一套UBW無線定位系統(tǒng)作為實驗裝置。其中，微慣性航姿模塊包含三軸加速度計、三軸陀螺儀以及三軸磁強計等傳感器，輸出速率為100 Hz/s，通過USB數(shù)據(jù)線與電腦通信；UBW無線系統(tǒng)的輸出速率為10 Hz/s，通過無線裝置與電腦進行通信，并使用電腦對數(shù)據(jù)進行分析；EKF的更新頻率為100 Hz。將IMU模塊固定在腳上，接收慣性數(shù)據(jù)；將超寬帶定位標簽固定在肩膀上。實驗裝置如圖2所示。

分別進行三種不同實驗來證明算法的有效性。第一種是普通行走實驗，包括直線行走、拐彎等步態(tài)；第二種是快速跑步實驗，軌跡為一個矩形；而第三種是按照圓形軌跡行走。設定實驗環(huán)境：實驗是在8 m×5 m的室內(nèi)進行。步行實驗是從(1,1)點開始行走，經(jīng)過(3,1),(5,4),(7,4),(7,1),(5,1),(3,1),(1,3)，最終回到(1,1)點；跑步實驗是從(1,1)點開始走，經(jīng)過(7,1),(7,4),(1,4)點，最后回到(1,1)點；最后的圓形軌跡行走實驗是以(3,2.5)為圓心，以1 m為半徑進行的。為了使得位置誤差累積效果明顯，進行多次重復行走。本文中步行實驗重復了4圈相同軌跡，跑步實驗重復了5圈相同軌跡，圓形行走實驗重復了10圈相同軌跡。

圖2 實驗設備圖

為了檢測站立相，設定閾值max(Ta)=11m/s2，min(Ta)=9m/s2，Tσ=2m/s2，Tω=50°/s，S=17。站立相檢測過程如圖3所示。只有當C1,C2和C3全部為真時，才屬于站立相時刻。從圖中可以觀察出單獨的線加速度或者角速度無法準確捕捉到站立相，因此，這種“和”邏輯的方式能夠提高系統(tǒng)的魯棒性。

圖3 步態(tài)檢測圖

2.2 測試與分析

普通行走、快速跑步以及圓形軌跡行走三種步態(tài)實驗軌跡如圖4。可以觀察出:MEMS慣性導航單獨定位效果最差，定位軌跡與實際最不相符合；IPZE算法解算偏差隨行走的圈數(shù)增多而不斷變大；UWB定位效果整體上誤差很小，但存在個別位置誤差較大的野值；而本文提出的IPZE/UWB算法定位效果明顯優(yōu)于前面提到的定位方法，且能夠應對各種環(huán)境的影響。接下來會利用數(shù)據(jù)進行更進一步驗證。

圖4 不同步態(tài)下軌跡

在實驗中，進行了直線行走、拐彎、跑步、圓形行走等多種步態(tài)測試，均為封閉軌跡。實驗誤差曲線如圖5所示。由于MEMS單獨定位誤差太大，故在此不對其進行誤差分析。圖6為不同步態(tài)、不同定位策略下的平均誤差圖。

由圖可知，步行狀態(tài)下，IPZE算法的誤差范圍為-0.293 3～1.886 7m，平均誤差為0.196 5m，平均殘差率為8.21 %，置信度為91.79 %；UWB定位的誤差范圍為-0.316 5～0.298 2m，平均誤差為0.142 3m，平均殘差率為7.09 %，置信度為92.91 %；而提出的IPZE/UWB方法的誤差范圍為-0.516 5～0.108 2m，平均誤差為0.100 1m，平均殘差率為4.06 %，置信度為95.94 %。IPZE/UWB算法的定位精度比IPZE算法提高了49.06 %，比UWB定位提高了29.65 %。

圖5 不同狀態(tài)下位置誤差

跑步狀態(tài)下，IPZE算法的誤差范圍為-0.756 6～0.252 2m，平均誤差為0.166 8m，平均殘差率為9.32 %，置信度為90.68 %；UWB定位的誤差范圍為-0.400 0～1.140 0m，平均誤差為0.133 1m，平均殘差率為7.39 %，置信度為92.61 %；而提出的IPZE/UWB算法的誤差范圍為-0.355 6～0.313 0m，平均誤差為0.070 2m，平均殘差率為4.42 %，置信度為95.58 %。IPZE/UWB算法的定位精度比IPZE算法提高了57.91 %，比UWB定位提高了47.26 %。

繞圓運動時，IPZE算法的誤差范圍為-0.198 6～1.158 1m，平均誤差為0.360 9m，平均殘差率為9.44 %，置信度為90.56%；UWB定位的誤差范圍為-0.196 5～0.610 2m，平均誤差為0.127 9m，平均殘差率為4.47 %，置信度為95.53 %；而提出的IPZE/UWB算法的誤差范圍為-0.196 9～0.519 9m，平均誤差為0.110 0m，平均殘差率為4.00 %，置信度為96.00 %。IPZE/UWB算法的定位精度比IPZE算法提高了69.52 %，比UWB定位提高了13.99 %。

圖6 不同步態(tài)下平均誤差圖

2.3 討 論

一方面，對于不同的步態(tài)來說，判斷站立相的理論條件上應當是相同的，但是由于運動的劇烈性和方向性不同，判斷站立相的依據(jù)也應該有所改變，以適應步態(tài)的變化；另一方面，UWB定位結(jié)果易受多徑因素和非視距因素影響，且成本較大，定位范圍較小，運動空間受局限，所以，應該盡量提高純慣性導航定位精度或可以在必要的通道或者節(jié)點處安放無線定位裝置進行校正，提高此方法的實用性。

3 結(jié) 論

實驗結(jié)果表明提出的緊耦合算法定位效果穩(wěn)定且不發(fā)散，能夠獲取到低于0.2 m的定位誤差。對于進行研究慣性定位技術(shù)以及其組合定位技術(shù)具有很好的參考和應用價值。

[1] Foxlin E.Pedestrian tracking with shoe-mounted inertial sensor-s[J].IEEE Computer Graphics and Applications,2005,25(6):38-46.

[2] Liu H,Darabi H,Banerjee P,et al.Survey of wireless indoor positioning techniques and systems[J].IEEE Transactions on Systems Man and Cybernetics：Part C—Applications and Reviews,2007,37(6):1067-1080.

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

[4] Zhang Z Q,Meng X L.Use of an inertial/magnetic sensor module for pedestrian tracking during normal walking[J].IEEE Transactions on Instrumentation and Measurement,2015,64(3):766-783.

[5] Fourati H.Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[J].IEEE Transactions on Instrumentation and Measurement,2015,64(1):221-229.

[6] Diaz E M.Inertial pocket navigation system:Unaided 3D positioning[J].Sensors,2015,15(4):9156-9178.

[7] Zhuang Y,Lan H,Li Y,et al.PDR/INS/WiFi integration based on handheld devices for indoor pedestrian navigation[J].Micromachines,2015,6(6):793-812.

[8] Jimenez Ruiz A R,Seco Granja F,Prieto Honorato J C,et al.Accurate pedestrian indoor navigation by tightly coupling foot-mounted IMU and RFID measurements[J].IEEE Transactions on Instrumentation and Measurement,2012,61(1):178-189.

[9] 鄔春明,耿 強,劉 杰,等.DGPS與UWB混合精確無縫定位技術(shù)研究[J].傳感器與微系統(tǒng),2012,31(3):74-77.

[10] Angelis A de,Nilsson J O,Skog I,et al.Indoor positioning by ultrawide band radio aided inertial navigation[J].Metrology and Measurement Systems,2010,17(3):447-460.

Foot-mounted pedestrian navigation technology based on tightly coupled PDR/UWB*

SUN Bi-wen, FAN Qi-gao, WU Ya-heng, SUN Yan

(School of IoT Engineering,Jiangnan University,Wuxi 214122,China)

In order to solve accumulated error problem of low cost micro-electro-mechanical inertial navigation system,a new real-time indoor pedestrian navigation system based on fusion of pedestrian dead reckoning (PDR)and ultra wideband(UWB)wireless positioning is proposed.The accelerometer and magnetometer are used to calculate the initial attitude;inertial navigation algorithm is deduced by considering the filtering error estimation;the“and”logic is used in step detection algorithm by relying on the accelerometer and gyroscope;zero velocity update(ZUPT)is carried out to provide velocity error measurements and UWB system is used to provide position error measurements for the filter;an extended Kalman filter with the outliers identification mechanism is designed for data fusion.The proposed pedestrian navigation algorithm is verified,and the proposed algorithm can effectively improve the positioning position of pedestrian compared with the traditional method.In the experiment,the pedestrian navigation algorithm can obtain the positioning error which is less than 0.2 m.The proposed navigation system is stable and divergence problem does not exist.

indoor positioning; pedestrian dead reckoning(PDR); ultra wideband(UWB); extended Kalman filtering(EKF)

10.13873/J.1000—9787(2017)03—0043—05

2016—03—29

國家自然科學基金資助項目(51405198)；中央高校專項自主科研項目(JUSRP11464)；江蘇省2015年度普通高校研究生實踐創(chuàng)新計劃項目(SJLX15_0567)

TP 212

A

1000—9787(2017)03—0043—05

孫璧文(1991-)，男，碩士研究生，研究方向為封閉環(huán)境下導航定位。

樊啟高(1986-)，男，通訊作者,博士，講師，從事智能傳感與測量系統(tǒng)研究工作，E-mail:qgfan@jiangnan.edu.cn。