徐愛功，劉 韜，隋 心,2，王長強(qiáng)

(1.遼寧工程技術(shù)大學(xué) 測繪與地理科學(xué)學(xué)院, 遼寧 阜新 123000； 2.武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心, 武漢 430079)

?

UWB/INS緊組合的室內(nèi)定位定姿方法

徐愛功1，劉 韜1，隋 心1,2，王長強(qiáng)1

(1.遼寧工程技術(shù)大學(xué) 測繪與地理科學(xué)學(xué)院, 遼寧 阜新 123000； 2.武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心, 武漢 430079)

針對室內(nèi)環(huán)境下UWB定位精度受NLOS誤差影響明顯，且不能提供載體姿態(tài)信息的問題，提出UWB/INS緊組合的室內(nèi)定位定姿方法：利用經(jīng)過標(biāo)準(zhǔn)時間偏差改正后的RTT測距信息與INS推算的距離信息之差作為量測信息，按設(shè)定的閾值剔除NLOS誤差，通過擴(kuò)展卡爾曼濾波進(jìn)行室內(nèi)定位定姿。實(shí)驗(yàn)結(jié)果表明，該方法能有效抑制NLOS誤差的影響，較好提升在NLOS環(huán)境下的定位定姿精度，適用于高精度的室內(nèi)移動測量。

UWB/INS緊組合；NLOS；室內(nèi)定位定姿

0 引言

在室外環(huán)境中，全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system，GNSS)以及與慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)組成的GNSS/INS組合導(dǎo)航系統(tǒng)能進(jìn)行較高精度的導(dǎo)航定位服務(wù)；在室內(nèi)封閉環(huán)境中，由于無法接收到衛(wèi)星信號，GNSS及GNSS/INS組合技術(shù)已無法進(jìn)行有效的導(dǎo)航定位。相較于射頻識別(radio frequency identification，RFID)、ZigBee、無線局域網(wǎng)絡(luò)(wireless local area networks，WLAN)、藍(lán)牙等室內(nèi)定位技術(shù)，超寬帶(ultra-wide band，UWB)技術(shù)由于具有極大的帶寬和超高的時間分辨率，能提供高達(dá)cm級的定位精度，且具有更好的穿透能力，更適用于室內(nèi)封閉環(huán)境下的高精度導(dǎo)航定位服務(wù)[1-2]。

UWB定位一般包括基于角度到達(dá)(angle of arrival，AOA)、基于接收信號強(qiáng)度(received signal strength，RSS)和基于到達(dá)時間(time/time difference of arrival，TOA/TDOA)等方法，其中：AOA方法需要天線陣列，額外增加硬件成本；RSS方法對信道環(huán)境極為敏感，魯棒性不強(qiáng)[3]；TOA方法需UWB基準(zhǔn)站和流動站之間保持嚴(yán)格的時間同步；TDOA方法需UWB基準(zhǔn)站之間保持嚴(yán)格的時間同步[2]，在不增加硬件的條件下，很難通過算法進(jìn)行精確的時間同步；基于往返時間(round trip time，RTT)方法可以間接測量UWB基準(zhǔn)站和流動站的距離，不需要UWB基準(zhǔn)站和流動站保持嚴(yán)格的時間同步[4-5]。在非視距(non line of sight，NLOS)環(huán)境下，由于障礙物的遮擋，UWB定位精度會大幅降低。利用Kalman濾波對原始測距信息平滑進(jìn)而減少NLOS誤差的影響，如文獻(xiàn)[5]提出了有色噪聲自適應(yīng)的卡爾曼濾波方法消除NLOS誤差，仿真效果顯著，但計算量較大；利用AOA/RTT混合測距方式減輕NLOS誤差的影響[6]，算法簡單、計算量小，但會在RTT測距的方式上增加AOA量測的硬件成本。

INS利用陀螺儀和加速度計量測載體的角速率和比力信息，依據(jù)初始位置和初始方位進(jìn)行二次積分獲取載體的位置、速度和方位角信息，短期導(dǎo)航定位精度高；但其誤差隨時間迅速累積[4,7]。

UWB/INS組合既能利用INS解算的結(jié)果減少UWB中NLOS誤差的影響，又能利用UWB的測距或解算信息抑制INS中誤差隨時間的迅速累積，從而獲得更高的導(dǎo)航定位精度以及更豐富的導(dǎo)航信息[7-11]。UWB/INS組合方式可分為松組合和緊組合，其中松組合利用UWB解算的位置(或位置和速度)信息與INS推算的位置(或位置和速度)信息進(jìn)行組合濾波[7-8]，其導(dǎo)航結(jié)果的精度依賴于UWB解算的精度，且算法中包括UWB導(dǎo)航解算部分，算法冗余度較高；而緊組合利用UWB的原始測距信息與INS推算的距離信息進(jìn)行組合[9-11]，能較好地利用INS推算的距離信息鑒別UWB中的NLOS誤差，從而減少NLOS誤差的影響。

本文采用UWB/INS緊組合的方式，以RTT測距信息與INS推算的距離信息進(jìn)行組合，并提出分級量測方程，充分利用RTT測距中的有用信息；同時利用樣本統(tǒng)計確定RTT測距中的時延模型。

1 RTT測距及INS量測模型

1.1 RTT測距模型

RTT測距是通過UWB脈沖信號從UWB流動站到UWB基準(zhǔn)站的往返時間確定二者之間的距離，其不需要UWB基準(zhǔn)站及與流動站間的時間同步[1,5]，可以消除TOA/TDOA測距中的時間同步誤差。如圖1所示，UWB流動站向UWB基準(zhǔn)站發(fā)送加入標(biāo)識的脈沖信號，UWB基準(zhǔn)站識別標(biāo)識并接收到脈沖信號后再向UWB流動站發(fā)送響應(yīng)脈沖，UWB流動站接收到響應(yīng)脈沖信號，其模型為

(1)

那么，UWB流動站與UWB基準(zhǔn)站間的距離為

(2)

標(biāo)準(zhǔn)時間偏差所造成的測距誤差rD不僅包括脈沖信號在UWB流動站和UWB基準(zhǔn)站中的固定時延誤差、UWB器件誤差和UWB逐次啟動誤差，而且還與脈沖信號傳播的距離、溫度等外界環(huán)境有關(guān)[10,12]，模型可表示為

rD=cn+φ(s)+en。

(3)式中：cn為固定時延誤差、UWB器件誤差、UWB逐次啟動誤差和外界環(huán)境所造成的常值誤差項(xiàng)；φ(s)為不同UWB測距距離形成的誤差，可以看作是與距離s構(gòu)成的多項(xiàng)式函數(shù)[10]；en為系統(tǒng)噪聲。

可知需要逐一對UWB設(shè)備進(jìn)行標(biāo)定，確定每個UWB基準(zhǔn)站和流動站間的標(biāo)準(zhǔn)時間偏差誤差項(xiàng)，獲得更精確的RTT測距信息。假定溫度等大氣環(huán)境恒定，在完全的LOS環(huán)境下，將UWB流動站與UWB基準(zhǔn)站放置于多組固定距離上，將大量RTT測距信息的平均值與真實(shí)測距信息的差值作為標(biāo)準(zhǔn)時間偏差誤差rD，最后通過擬合可確定標(biāo)準(zhǔn)時間偏差的誤差模型。

1.2 INS量測模型

INS通過3軸的陀螺儀和加速度計量測載體在載體坐標(biāo)系b系下的角速率信息和比力信息[12-13]。經(jīng)過轉(zhuǎn)臺標(biāo)定并進(jìn)行誤差補(bǔ)償之后，陀螺儀和加速度計輸出信息為：

(4)

(5)

2 UWB/INS組合算法

2.1 狀態(tài)方程

本文采用的UWB/INS緊組合是指直接利用UWB的RTT距離量測值和INS推算距離之間的差值作為量測信息，以UWB的RTT距離量測值作為約束輔助INS進(jìn)行位置、速度和姿態(tài)更新。

在大多數(shù)室內(nèi)應(yīng)用(如機(jī)場、商場)中，平面定位的需求超過高程定位，且橫滾角和俯仰角的變化為0°左右[7-9]；因而本文只考慮室內(nèi)二維導(dǎo)航定位。以UWB坐標(biāo)系作為導(dǎo)航系n系，利用陀螺儀和加速度計輸出的角速率信息和比力信息，UWB輔助INS從而獲得載體的位置、速度和方位角，其模型為：

(6)

(7)

(8)

(9)

以載體的位置、速度和航向角為狀態(tài)向量，則可以得到UWB/INS緊組合的狀態(tài)方程為

(10)

2.2 量測方程

在室內(nèi)平面的導(dǎo)航定位中，需要將UWB的RTT測距信息中的垂直方向差值減去；同時，需將INS解算的平面坐標(biāo)推算至載體與UWB基準(zhǔn)站間的距離，即：

i=1,2,…,M；

(11)

對于UWB輔助INS的組合定位定姿，以INS推算的距離信息與UWB的RTT測距信息的差值作為組合濾波器的量測信息。由于障礙物的遮擋，RTT測距信息中會存在NLOS誤差，則需要將NLOS改正或消除；但UWB基準(zhǔn)站相互間的NLOS誤差是相互獨(dú)立的。為了充分利用良好的RTT測距信息，本文提出分級量測方程，即量測方程的維數(shù)是變化的，即：

(13)

(14)

對式(11)進(jìn)行泰勒一階展開可得

同時，式(12)可表示為

(16)

式中：vi,k為第k時刻第i個RTT距離量測噪聲。將式(15)和式(16)作差，即可得到UWB/INS緊組合的量測方程為

Z(k)=HX(k)+v(k)。

(17)

根據(jù)狀態(tài)方程和量測方程，通過Kalman濾波進(jìn)行時間更新和量測更新獲得載體的導(dǎo)航信息[14]。圖2給出了UWB輔助INS室內(nèi)導(dǎo)航定位的整體流程。

3 實(shí)驗(yàn)及結(jié)果分析

3.1UWB標(biāo)準(zhǔn)時間偏差標(biāo)定

為了確定每個UWB的標(biāo)準(zhǔn)時間偏差函數(shù)，在本次實(shí)驗(yàn)中，以TimeDomain公司的PulsON400通信與測距模塊為UWB硬件，以LeicaTS06全站儀測距結(jié)果為真實(shí)值。實(shí)驗(yàn)場景為完全的LOS條件下，即實(shí)驗(yàn)過程中UWB流動站和基準(zhǔn)站間無障礙物遮擋，且實(shí)驗(yàn)中無人員穿行，其中UWB流動站保持不動，將UWB基準(zhǔn)站放置于固定距離上，按距離3～30m分為10組，每組間隔3m，在每組距離上RTT測距1 000次，將平均RTT測距信息作為當(dāng)前距離上的RTT測距值。每組距離的RTT測距值與真實(shí)值的差作為RTT測距誤差，在本次實(shí)驗(yàn)中認(rèn)為是標(biāo)準(zhǔn)時間偏差誤差(如圖3所示)。

圖3是針對0～35m的測距范圍，對3個UWB基準(zhǔn)站的10組標(biāo)準(zhǔn)時間偏差誤差進(jìn)行分析并用4階多項(xiàng)式進(jìn)行擬合，確定3個UWB基準(zhǔn)站統(tǒng)一的標(biāo)準(zhǔn)時間偏差誤差模型，用于RTT測距中的UWB標(biāo)準(zhǔn)時間偏差改正。

3.2UWB/INS組合實(shí)驗(yàn)及分析

由圖可以看出，由于UWB信號的遮擋，3組UWB的RTT測距中存在大量的NLOS誤差或RTT測距異常值，如果單依靠UWB進(jìn)行導(dǎo)航定位會存在一定困難。本次實(shí)驗(yàn)中，UWB解算采用2步最小二乘[15](2WLS)方法，圖5、圖6分別為UWB和UWB/INS解算結(jié)果和航向角結(jié)果。

從圖5可以看出：當(dāng)3組RTT測距值均良好時，UWB和UWB/INS組合解算結(jié)果都能貼近參考軌跡，定位精度均能達(dá)到cm級；當(dāng)3組RTT量測值中任意一組出現(xiàn)NLOS誤差或測距異常時，UWB解算的結(jié)果中存在大量的發(fā)散值，其定位結(jié)果已不可靠，其最大定位精度超過10m，而此時UWB/INS組合解算結(jié)果良好，其最大定位精度(圖5(b)中圓圈部分)為0.63m。從圖6可以看出，UWB/INS組合還能獲得載體的航向角信息，其航向角精度為0.05°左右。表1給出了UWB和UWB/INS組合解算結(jié)果的誤差統(tǒng)計，其中LOS環(huán)境指3組RTT測距值均良好時的平均誤差值。

表1 UWB及UWB/INS組合誤差統(tǒng)計

經(jīng)分析：在LOS環(huán)境下，UWB/INS組合和單獨(dú)UWB都能達(dá)到cm級的定位精度；在NLOS環(huán)境下，UWB/INS組合能大幅提升定位精度，具有更穩(wěn)定的導(dǎo)航定位能力，同時還能提供載體的姿態(tài)信息，能夠適用于高精度的室內(nèi)定位定姿需求。

4 結(jié)束語

GNSS/INS組合可以在室外獲得高精度的定位定姿結(jié)果；而在室內(nèi)環(huán)境下，需要采用其他室內(nèi)定位系統(tǒng)來替代GNSS，UWB是一個較好的選擇：因此本文針對基于UWB/INS緊組合的室內(nèi)定位定姿方法進(jìn)行研究。標(biāo)準(zhǔn)時間偏差和NLOS誤差是UWB測距的主要誤差源。本文采用4階多項(xiàng)式擬合模型對UWB的標(biāo)準(zhǔn)時間偏差進(jìn)行標(biāo)定，利用INS推算的距離信息對UWB NLOS誤差進(jìn)行鑒別；以經(jīng)典的RTT測距及INS量測模型為基礎(chǔ)，推導(dǎo)出UWB/INS緊組合的狀態(tài)方程和量測方程，通過擴(kuò)展卡爾曼濾波進(jìn)行參數(shù)解算，最終獲得室內(nèi)二維環(huán)境下的高精度定位定姿信息。利用實(shí)測實(shí)驗(yàn)對以上算法進(jìn)行驗(yàn)證，結(jié)果表明UWB/INS緊組合具有在LOS和NLOS環(huán)境下的高精度定位定姿能力，有較好的穩(wěn)定性，可用于高精度室內(nèi)移動測量。

[1] HUI L, HOU S, PAT B, et al. Survey of wireless indoor positioning techniques and systems[J]. IEEE Transaction on System, Man, and Cybernetics-Part C: Applications and Reviews，2007, 37(6):1067-1080.

[2] ABDULRAHMAN A, ABDULMALIK A S, MANSOUR A, et al.Ultra wideband indoor positioning technologies analysis and recent advances[J]. Sensors, 2016, 16(5):707.

[3] 肖竹,王勇超,田斌.超寬帶定位研究與應(yīng)用:回顧和展望[J].電子學(xué)報, 2011, 39(1):133-141.

[4] JOHN O N, ALESSIO D A, ISAAC S, et al. Signal processing issues in indoor positioning by ultra wide band radio aided inertial navigation[EB/OL].(2009-08-07)[2017-02-18].http://www.eurasip.org/Proceedings/Eusipco/Eusipco2009/contents/papers/1569187242.pdf.

[5] 李奇越,吳忠,黎潔,等.基于改進(jìn)卡爾曼濾波的NLOS誤差消除算法[J].電子測量與儀器學(xué)報,2015,29(10):1513-1519.

[6] 毛科技,鄔錦彬,金洪波,等.面向非視距環(huán)境的室內(nèi)定位算法[J].電子學(xué)報,2016,44(5):1174-1179.

[7] RISANG G Y, FREDERIK P, FLANDERS M.Sensor fusion for indoor navigation and tracking of automated guided vehicles[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of International Conference on Indoor Positioning and Indoor Navigation (IPIN 2015). Banff, Canada: IEEE, 2015: 1-5.

[8] 劉見輝,王冬,劉鳳英,等.高精度UWB/SINS組合室內(nèi)定位系統(tǒng)研究[J]. 導(dǎo)航定位學(xué)報,2016,4(2):53-56.

[9] ALESSIO D A, JOHN O N, ISAAC S, et al. Indoor positioning by ultrawide band radio aided inertial navigation[J]. Metrology and Measurement Systems, 2010, 17(3):447-460.

[10]ZHANG RUI，H?FLINGER F，REINDL L.Inertial sensor based indoor localization and monitoring system for emergency responders[J]. IEEE Sensors Journal, 2013, 13(2):838-848.

[11]TINGCONG Y, SALVATORE T, MICHAEL W, et al. Fully-coupled hybrid IEEE 802.15.4a UWB/IMU position estimation in indoor environments[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings International Conference on Information and Communications Technologies (ISSC/CIICT 2014). Limerick: IEEE, 2013:53-58.

[12]MANON K, JEROEN D H, THOMAS B S. Indoor positioning using ultrawideband and inertial measurements[J]. IEEE Transactions on Vehicular Technology, 2015, 64(4):1293-1303.

[13]YOUSSEF T, PHILLIP T, CYRIL B, et al. Implementation and performance of a GPS/INS tightly coupled assisted PLL architecture using MEMS inertial sensors[J]. Sensors, 2014, 14(2):3678-3796.

[14]秦永元. 張洪鉞. 汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社,2015:33-49.

[15]CHAN Y T, HO K C. A simple and efficient estimator for hyperbolic location[J]. IEEE Transactions on Signal Processing, 1994, 42(8):1905-1915.

Indoor positioning and attitude determination method based on UWB/INS tightly coupled

XUAigong1,LIUTao1,SUIXin1,2,WANGChangqiang1

(1.School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China;2.Research Center of GNSS, Wuhan University, Wuhan 430079, China)

Aiming at the problem that the accuracy of UWB positioning was obviously affected by NLOS errors and it cannot provide attitude information in indoor environment, the paper proposed an indoor positioning and attitude determination method of UWB/INS tightly coupled: the difference between the RTT ranging information after the correction of standard deviation time and the distance information calculated by INS was used as measurement information, and the NLOS error was rejected with preset threshold, finally the position and attitude was determinted by extended Kalman filter.Experimental result showed that this method could effectively restrain the influences of NLOS error and greatly improve the positioning and attitude accuracy in NLOS environment, which would be suitable for high-precise traverse measurement in indoor environment.

UWB/INS tightly coupled; NLOS; indoor positioning and attitude determination

2016-08-15

國家重點(diǎn)研發(fā)計劃項(xiàng)目(2016YFC0803102)；遼寧省高等學(xué)校創(chuàng)新團(tuán)隊(duì)項(xiàng)目(LT2015013)。

徐愛功(1963—)，男，山東日照人，教授，博士，研究方向?yàn)槿蛐l(wèi)星定位系統(tǒng)與地理信息系統(tǒng)及智能交通系統(tǒng)的集成與應(yīng)用。

徐愛功，劉韜，隋心，等.UWB/INS緊組合的室內(nèi)定位定姿方法[J].導(dǎo)航定位學(xué)報,2017,5(2):14-19.(XU Aigong, LIU Tao, SUI Xin,et al.Indoor positioning and attitude determination method based on UWB/INS tightly coupled[J].Journal of Navigation and Positioning,2017,5(2):14-19.)

10.16547/j.cnki.10-1096.20170203.

P228

A

2095-4999(2017)02-0014-06