曾慶化，王敬賢，孟 騫，熊 智，錢偉行

（1. 南京航空航天大學 導航研究中心，南京 210016；2. 衛(wèi)星通信與導航協(xié)同創(chuàng)新中心，南京 210016；

3. 南京師范大學 電氣與自動化工程學院，南京 210094）

基于UWB優(yōu)化配置的室內(nèi)行人導航方法

曾慶化1,2，王敬賢1,2，孟 騫1,2，熊 智1,2，錢偉行3

（1. 南京航空航天大學 導航研究中心，南京 210016；2. 衛(wèi)星通信與導航協(xié)同創(chuàng)新中心，南京 210016；

3. 南京師范大學 電氣與自動化工程學院，南京 210094）

在衛(wèi)星信號無法覆蓋的室內(nèi)條件下，行人的精確導航問題是當前研究的熱點。為解決基于MEMS的微慣性導航系統(tǒng)誤差隨時間發(fā)散、航向角發(fā)散快的問題，提出了一種利用UWB輔助修正慣性導航系統(tǒng)，實現(xiàn)室內(nèi)較高精度定位的方法。該方法采用基于零速修正的微慣導系統(tǒng)進行導航，以抑制慣導誤差隨時間發(fā)散，并在建筑內(nèi)拐角、樓梯口等關鍵節(jié)點處優(yōu)化配置UWB設備，采用UWB信息與微慣導數(shù)據(jù)進行卡爾曼濾波，實現(xiàn)對微慣導航向及位置的修正。與大規(guī)模使用UWB系統(tǒng)進行室內(nèi)定位相比，該方法降低了系統(tǒng)布設成本，避免了UWB出現(xiàn)問題時對整體導航結果的影響，有效地保證了系統(tǒng)的定位精度。行走實驗表明：直線行走時，微慣導最終定位誤差為1.8%；轉(zhuǎn)彎行走時，在UWB輔助定位下，微慣導最終定位誤差小于1.0%。

室內(nèi)定位；慣性導航系統(tǒng)；超寬帶無線技術；組合導航

基于衛(wèi)星定位的行人導航系統(tǒng)已廣泛應用于定位領域[1]，由于衛(wèi)星信號容易被建筑物阻隔，產(chǎn)生多路徑、非視距、時變性等問題，因此衛(wèi)星導航接收機很難實現(xiàn)室內(nèi)的高精度定位應用。美國Draper實驗室于20世紀末提出將慣性測量器件安裝在人體的足部實現(xiàn)人體定位功能，其中應用零速修正技術較好地解決了低成本慣導系統(tǒng)誤差發(fā)散快的問題。國內(nèi)外學者對該算法進行了大量的改進研究[2-4]。

由于室內(nèi)航向角很難精確獲得，因此行人慣性導航系統(tǒng)中的航向角誤差已成為慣性導航系統(tǒng)誤差的主要來源之一[5-6]。當前解決航向角發(fā)散問題的方法主要有磁航向輔助算法[7]、陀螺旋轉(zhuǎn)修正算法[8]等。磁航向輔助算法主要運用于室外，室內(nèi)磁場環(huán)境復雜，很難應用該算法；陀螺旋轉(zhuǎn)修正算法多用于機器，行人無法滿足算法所需的陀螺勻速旋轉(zhuǎn)的條件。對室內(nèi)行人慣性導航系統(tǒng)航向進行修正的重要性日益突出。

UWB（超寬帶無線技術）是當前國內(nèi)外定位領域研究熱點之一[9-10]。在室內(nèi)定位過程中，UWB具有厘米級的定位精度，良好的抗多路徑性能，較低的發(fā)射功率及一定的穿透能力，與WiFi、ZigBee、RFID等室內(nèi)定位技術相比優(yōu)勢顯著。目前，國內(nèi)外研究者對慣導/UWB組合的研究主要集中在UWB與IMU組合實現(xiàn)無人機的室內(nèi)定位[11]，UWB與PDR/SLU （Pedestrian dead reckoning /Step length update）組合實現(xiàn)行人導航[12-13]。PDR/SLU對行人的步長、步數(shù)精度要求較高，而基于足部微慣性系統(tǒng)的零速修正算法則不需獲知行人的精確步長，因此本文選擇UWB與使用零速修正的慣導系統(tǒng)組合，以UWB修正慣導系統(tǒng)的航向與位置，同時在UWB受干擾定位出現(xiàn)偏差時，利用慣導保證定位的準確性實現(xiàn)低成本高精度的室內(nèi)導航。

1 慣性導航系統(tǒng)定位方法

本文選用低成本的微慣性傳感器是消費級別的，因此傳統(tǒng)捷聯(lián)慣性導航算法無法使之滿足行人慣導系統(tǒng)的需要。在足部運動時，足部的慣性傳感器數(shù)據(jù)使用捷聯(lián)慣導算法遞推得到導航結果；在足部靜止時，采用零速修正算法將速度置零，并重新計算姿態(tài)角以修正導航結果、抑制誤差發(fā)散。具體流程如圖1所示。

圖1 基于零速修正算法的行人慣性導航算法原理圖Fig. 1 Pedestrian navigation algorithm principle based on zero-velocity update algorithm

準確判斷足部的零速時刻是零速修正算法最重要的條件。由圖2可以看出，足部處于零速時刻時，慣性器件有一段較為明顯的平穩(wěn)輸出區(qū)間。本文采取了以陀螺儀輸出為主，加速度計輸出為輔的滑動窗口判斷方法，對足部的零速時刻進行判定?；诓煌男凶郀顟B(tài)將滑動窗口寬度設置為變量N，該值的大小與行走狀態(tài)有關，相關變量定義如式(1)所示：

圖2 行人行走時慣性器件輸出數(shù)據(jù)Fig.2 The output data of inertial device for pedestrian

為了避免零速判定的條件過于嚴格導致慣導誤差的積累，本文適度放寬零速判定的條件，當上述4個變量有一個滿足所給定的限定門限值時，就認為處于零速時刻，并對慣性器件進行零速修正。當慣性器件處于零速修正時刻，利用式(2)可以使用加速度計輸出信息求出該器件的橫滾角與俯仰角。

低成本慣性器件穩(wěn)定性、重復性都比較差，每次使用前均需要對其進行標定，以扣除陀螺儀和加速度計的零偏誤差，提高傳感器的精度。陀螺儀取靜止時刻的輸出作為零偏，但加速度計無法僅由某一位置的輸出求出零偏，因此本文使用一種三位置加速度計標定法，通過傳感器在三個不同位置的靜態(tài)數(shù)據(jù)，求出加速度計的零偏值。該方法簡單實用，是一種適用于安裝完成后的現(xiàn)場標定方法，其算法如式(3)所示：

慣性導航系統(tǒng)通過慣性器件獲得加速度計與陀螺儀的測量信息，去除零偏后解算出自身的位置與姿態(tài)，同時在足部靜止時刻引入零速修正以抑制誤差的發(fā)散，從而保證一定時間內(nèi)的精確導航。

2 UWB定位方法

常用的無線定位算法有以下四種：測量信號的到達時間（TOA，Time of Arrival）；信號的到達時間差（TDOA，Difference of Time Arrival）；信號的到達角度（AOA，Angel of Arrival）；接收信號的強度（RSSI，Received Signal Strength）。由于UWB技術支持使用納秒級的同步時鐘，因此能夠確保時間的準確性，故本文中選用 TOA方法：精確測量定位芯片發(fā)出的無線脈沖信號到達各基站所經(jīng)過的時間，乘以光速即獲得芯片與各基站之間的精確距離，從而解算出較高精度的芯片位置[14]。

圖3 TOA定位方法的幾何模型Fig.3 Geometric model of TOA location method

然而在大部分情況下，由于1r、r2、r3的測量值存在誤差，三球并不會正好相交于一點，因此需要N個（3個以上）基站才能完成定位，如式(4)所示：

式(4)通過平方和求差可得式(5)：

N個定位基站可得N-1個類似式(5)的方程，寫成如式(6)所示的矩陣形式：

其中，

根據(jù)基站的數(shù)據(jù)，獲得M和b的值，帶入式(6)，利用最小二乘法，即可求出定位芯片MS的坐標I。

3 慣導/UWB組合定位方法

3.1 慣導/UWB組合定位原理

基于零速修正的慣性導航技術通過自身加速度計與陀螺儀測得的信息計算出行人的位置和航向，同時在足部靜止時刻引入零速修正算法，以修正導航結果。雖然零速修正算法在一定程度上能抑制誤差的發(fā)散，保證一定時間的獨立工作能力，但是誤差隨著行人走動仍會不斷累積，最后無法滿足導航的需求。同時在行人行進時航向角計算會出現(xiàn)偏差，若不對其進行及時修正，會對后續(xù)定位造成很大的影響。

基于TOA方法的UWB技術采用測量無線脈沖信號準確的飛行時間，從而解得厘米級的精確位置，可以為行人提供準確的位置與航向。但是UWB信號易被人體所吸收，尤其在對快速行走的人體進行定位時，這種影響所帶來的誤差將達到分米級[15]。另外 UWB基站成本相對微慣性器件來說較高，很多室內(nèi)導航系統(tǒng)的預算無法支持UWB的全面覆蓋。

慣導/UWB組合定位相輔相成，可以實現(xiàn)定位精度與成本之間的平衡。在直線行走時，采用慣性傳感器自主導航，在樓道關鍵節(jié)點處（如拐角）布設UWB基站予以輔助定位，既修正了慣性傳感器的航向，同時又消除了慣導之前行走過程所積累的位置誤差，還避免了建筑內(nèi)大規(guī)模布設UWB基站的高昂成本，在滿足定位需求的同時降低了成本的投入。

本文采用的是慣性/UWB的松組合導航系統(tǒng)，具體原理如圖4所示，組合過程分為以下三步：

1）實時由慣性導航系統(tǒng)進行導航解算，同時依靠零速修正算法對其誤差進行修正。

2）UWB模塊嘗試接收UWB信號，當能獲取到可用的UWB定位信息時，轉(zhuǎn)到步驟3）。

圖4 慣導/UWB組合定位原理圖Fig.4 INS/UWB integrated positioning principle

3）使用UWB定位信息與慣導解算結果進行卡爾曼濾波，對慣導解算數(shù)據(jù)進行修正，返回步驟 1）繼續(xù)進行慣導解算。

3.2 卡爾曼濾波原理

基于對數(shù)據(jù)融合的有效性與算法的總體計算量的考慮，本文選擇僅在零速修正時刻進行基本線性卡爾曼濾波，其狀態(tài)方程與觀測方程為

式中：

卡爾曼濾波的狀態(tài)量如下所示：

卡爾曼濾波的量測量為

由于慣導傳感器采樣周期為200 Hz，UWB采樣周期為 20 Hz，因此本文所選量測量并非時刻存在，故僅在可以獲得有效量測量的零速時刻進行系統(tǒng)狀態(tài)更新和量測更新，其余時刻濾波器僅作狀態(tài)更新。經(jīng)過濾波器量測更新得到的導航誤差估計量被返回至慣導解算環(huán)節(jié)，慣導系統(tǒng)輸出修正后的導航結果。

4 實驗驗證與數(shù)據(jù)分析

為了對本文提出的UWB輔助慣導定位方法進行驗證，項目組在南京航空航天大學自動化學院樓群進行了室內(nèi)行走實驗。本次實驗將低成本微慣性器件MPU-6050置于足部，器件輸出頻率設為200 Hz，以測量行人行走過程中的足部運動信息，實現(xiàn)零速修正下的慣性導航，同時將UWB標簽置于上半身（本次實驗選擇肩部），避免下半身行走時抖動過大對UWB測量精度的影響。具體設備如圖5所示。

圖5 慣性傳感器與UWB標簽、基站示意圖Fig.5 Schematic of inertial sensor, UWB tag and UWB base station

本次實驗分為兩部分進行：第一部分實驗驗證慣導在長距離直線行走中的準確性；第二部分實驗驗證UWB輔助修正慣導的效果。

4.1 純慣導長距離直線行走實驗

本部分實驗在4號樓內(nèi)一條貫穿全樓的直線過道中行走，由激光測距儀測得行走距離長52.081 m，行走效果如圖6所示。

圖6 長距離直線行走實驗結果Fig.6 Experiment result of long distance walking straight

圖6中直線前后的紅點所示理論起始點為（0, 0），理論終點為（52.081, 0）。實際行走結果由藍色曲線表示，起始點為（0, 0），終點為（52.912, -0.437）。由數(shù)據(jù)可知，起點-終點位置差為 0.939 m，位置誤差占行走距離的1.8%?？梢姡疚乃捎玫幕诹闼傩拚膽T性導航算法，在直線行走時可以基本滿足室內(nèi)行人定位的需求。

4.2 UWB輔助慣導實驗

本UWB實驗室構型如圖7所示，室內(nèi)安裝4個UWB基站實時向計算機傳輸標簽定位數(shù)據(jù)。實驗區(qū)為邊長5 m的正方形，各點坐標由激光測距儀測得，將慣導與UWB的坐標原點均設于其左下角O點，X、Y軸如圖7所示。實驗路線設置為從O點起沿Y軸正向順時針繞行一圈回到O點，共20 m。實驗過程中：前15 m實驗者正常行走；后5 m設置一名人員位于實驗者與某一基站之間，起到對信號的阻擋作用。

圖7 設備位置及實驗路線示意圖Fig.7 Sketch map of equipment position and experimental route

實驗結果如圖8所示，其中：黑色點劃線為規(guī)劃行走路線；紅色虛線為UWB測量路線，由于遮擋的存在，在（0, 0）附近時候存在較大的誤差；淺藍色點線為慣導測量路線，存在一定的誤差累積情況；深藍色粗實線為慣導/UWB組合導航路線，與規(guī)劃行走路線貼合最好。

圖8 慣導、UWB導航與組合導航實驗結果對比Fig.8 Comparison on experimental results of inertial navigation, UWB navigation and integrated navigation

由圖8可得：

1）前15 m中，由于實驗者實際行走時肩部存在輕微晃動，不能保證時刻與規(guī)劃路線一致，UWB數(shù)據(jù)與規(guī)劃路線之間存在20 cm以內(nèi)的誤差，具有較高的可靠性。后5 m中，由于部分信號被阻擋，UWB信號產(chǎn)生漂移。

2）慣導數(shù)據(jù)由于誤差累積，隨著時間推移，后期偏差逐步擴大，最終位置為（0.6918, 0.1279），與原點位置差為0.7035 m，位置誤差占行走距離的3.52%，全程平均位置誤差為0.6437 m。

3）慣導/UWB組合導航數(shù)據(jù)最終位置為（-0.0738, -0.1588），與原點位置差為0.1751 m，位置誤差占行走距離的0.88%，全程平均位置誤差為0.0968 m。

由于本文設置僅在零速修正時刻進行卡爾曼濾波，因此每次剛進入零速修正時刻開始濾波時，輸出結果存在小幅度的跳變。

綜上，慣導/UWB組合導航相對于慣性導航單獨使用在繞圈行走的最終結果以及整個導航過程的平均誤差方面都有明顯的精度提升，同時與UWB單獨導航相比，基本接近UWB的正常導航精度，且在UWB定位出現(xiàn)問題時，系統(tǒng)依然能靠慣導保證定位的精度，能夠滿足較長時間導航的需要。

5 結 論

在一般室內(nèi)區(qū)域，本文采用基于零速修正的慣導系統(tǒng)進行導航，以抑制慣導誤差隨時間發(fā)散。同時，在關鍵節(jié)點（拐彎）處布設UWB設備，通過松組合的形式，將UWB/慣導組合在一起，則可以實現(xiàn)在零速修正時刻對UWB與慣導進行卡爾曼濾波，實現(xiàn)對慣導信息的修正。

從實驗結果可以看出，本文提出的算法能夠?qū)⑽闹惺覂?nèi)導航的定位精度從 3.52%提升到 0.88%。本方法修正了慣性傳感器之前行走過程所積累的誤差，解決了低精度消費級慣導無法較長時間準確導航定位的問題；與完全使用UWB進行室內(nèi)定位相比，系統(tǒng)既實現(xiàn)了定位精度與成本的平衡，又能夠避免UWB出現(xiàn)問題時對整體定位性能的影響。

（References）：

[1] Hsu L T, Gu Y, Kamijo S. 3D building model-based pedestrian positioning method using GPS/GLONASS/QZSS and its reliability calculation[J]. GPS Solutions, 2015: 1-16.

[2] Zhang Xin-xi, Zhang Rong, Guo Mei-feng, et al. Yaw error self-observation algorithm for pedestrian navigation via foot-mounted inertial navigation system[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 457-466.

[3] 萬駿煒. 多信息融合室內(nèi)外無縫個人定位導航系統(tǒng)實現(xiàn)研究[D]. 南京航空航天大學, 2015. Wan Jun-wei. Research on multi-information fusion based seamless outdoor/indoor personal navigation system [D]. Nanjing University of Aeronautics and Astronautics, 2015.

[4] Girisha R, Prateek G V, Hari K V S, et al. Fusing the navigation information of dual foot-mounted zero-velocity-update-aided inertial navigation systems[C]//2014 IEEE International Conference on Signal Processing and Communications. 2014: 1-6.

[5] 曾慶化, 萬駿煒, 劉建業(yè), 等. 基于蜂窩網(wǎng)格粒子濾波的行人導航航向估計方法[J]. 中國慣性技術學報, 2014, 22(5): 576-579. Zeng Qing-hua, Wan Jun-wei, Liu Jian-ye, et al. Heading compensation algorithm based on cellular structure particle filter for pedestrian navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(5): 576-579.

[6] 王云舒, 劉建業(yè), 曾慶化, 等. 結構光輔助的慣性/視覺室內(nèi)導航三維環(huán)境重構方法[J]. 中國慣性技術學報, 2016, 24(1): 51-58. Wang Yun-shu, Liu Jian-ye, Zeng Qing-hua, et al. 3D environment restructure method with structured light for indoor vision/inertial navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(1): 51-58.

[7] Zhang R, Hoflinger F, Reindl L. Inertial sensor based indoor localization and monitoring system for emergency responders[J]. IEEE Sensors Journal, 2013, 13(2): 838-848.

[8] 吳哲明, 孫振國, 張文增, 等. 基于慣性測量單元旋轉(zhuǎn)的陀螺漂移估計和補償方法[J]. 清華大學學報(自然科學版), 2014(9): 1143-1147. Wu Zhe-ming, Sun Zhen-guo, Zhang Wen-zeng, et al. Gyroscope bias estimation and compensation by rotation of the inertial measurement unit[J]. Journal of Tsinghua University(Science and Technology), 2014(9): 1143-1147.

[9] Suski W, Banerjee S, Hoover A. Using a map of measurement noise to improve UWB indoor position tracking[J]. IEEE Transactions on Instrumentation & Measurement, 2013, 62(8): 2228-2236.

[10] Tiemann J, Schweikowski F, Wietfeld C. Design of an UWB indoor-positioning system for UAV navigation in GNSS-denied environments[C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2015.

[11] Benini A, Mancini A, Longhi S. An IMU/UWB/Visionbased extended Kalman filter for mini-UAV localiza- tion in indoor environment using 802.15.4a wireless sensor network[J]. Journal of Intelligent & Robotic Systems, 2013, 70(1-4): 461-476.

[12] Ascher C, Zwirello L, Zwick T, et al. Integrity monitoring for UWB/INS tightly coupled pedestrian indoor scenarios [C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2011: 1-6.

[13] Zampella F, De Angelis A, Skog I, et al. A constraint approach for UWB and PDR fusion[C]//International Conference on Indoor Positioning and Indoor Navigation. 2012: 1-9.

[14] Irahhauten Z, Nikookar H, Klepper M. 2D UWB localization in indoor multipath environment using a joint ToA/ DoA technique[C]//IEEE Wireless Communications & Networking Conference. 2012: 2253-2257.

[15] Kilic Y, Ali A J, Meijerink A, et al. The effect of humanbody shadowing on indoor UWB TOA-based ranging systems[C]//The Workshop on Positioning Navigation & Communication. IEEE, 2012: 126-130.

Indoor pedestrian navigation method based on optimal allocation of UWB

ZENG Qing-hua1,2, WANG Jing-xian1,2, MENG Qian1,2, XIONG Zhi1,2, QIAN Wei-xing3
(1. Navigation Research Center, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China;
2. Satellite Communication and Navigation Collaborative Innovation Center, Nanjing 210016, China;
3. School of Electrical and Automation Engineering, Nanjing Normal University, Nanjing 210094, China)

In view that the heading and positioning errors of the MEMS micro-inertial navigation system is divergent with time, a method for realizing the indoor high-precision pedestrian positioning system is put forward based on an ultra-wideband-aided micro-inertial navigation system. A micro-inertial navigation system based on zero velocity update is adopted to suppress the divergence of inertial navigation error with time. The UWB (Ultra-wideband) equipment is optimally installed in key areas of the building, and a Kalman filter is used to modify the direction and the position of the micro-inertial navigation system with the help of the UWB signal and the inertial navigation data. Compared with the method based on a large number of UWB devices, the proposed method has such advantages as high positioning accuracy, low cost, high autonomy, etc. Meanwhile, this method can avoid the poor UWB information’s influences on the overall navigation results. The walking experiments indicate that the final positioning error of the micro-inertial navigation system is 1.8% in the linear walking, and the final positioning error is less than 1.0% when assisted with the UWB in the turning walking.

indoor positioning; inertial navigation; ultra-wideband radio technology; integrated navigation

U666.1

A

1005-6734(2017)02-0186-06

10.13695/j.cnki.12-1222/o3.2017.02.009

2017-01-04；

2017-03-28

國家自然科學基金（61533008，61374115，61328301，61304227）；江蘇省自然科學基金（BK20141453）；中央高?；究蒲袠I(yè)務費專項資金（NS2015037）

曾慶化（1979—），男，副教授，從事慣性導航及組合導航技術研究。E-mail: zengqh@nuaa.edu.cn