高精度UWB/SINS組合室內(nèi)定位系統(tǒng)研究

劉見輝，王　冬，劉鳳英，王　靜

(1.山東科技大學(xué) 測繪科學(xué)與工程學(xué)院，山東　青島　266590；2.貴州省交通規(guī)劃勘察設(shè)計(jì)研究院股份有限公司，貴陽　550000)

高精度UWB/SINS組合室內(nèi)定位系統(tǒng)研究

劉見輝1，王冬1，劉鳳英1，王靜2

(1.山東科技大學(xué) 測繪科學(xué)與工程學(xué)院，山東青島266590；2.貴州省交通規(guī)劃勘察設(shè)計(jì)研究院股份有限公司，貴陽550000)

摘要：針對水利工程泥沙沉積規(guī)律模擬測量實(shí)驗(yàn)，提出INS和掃描儀相結(jié)合的測量系統(tǒng)，同時(shí)在該測量系統(tǒng)中又加入了基于UWB的室內(nèi)定位技術(shù)來解決INS誤差積累的問題。通過軌跡發(fā)生器和松組合形式的卡爾曼濾波器，對組合定位系統(tǒng)進(jìn)行模擬仿真。結(jié)果表明該組合定位系統(tǒng)中INS和UWB優(yōu)勢互補(bǔ)，抑制誤差的同時(shí)提高了室內(nèi)定位系統(tǒng)的定位精度。

關(guān)鍵詞：UWB；捷聯(lián)慣性導(dǎo)航系統(tǒng)；卡爾曼濾波；組合導(dǎo)航；室內(nèi)定位

0引言

為了研究河流和水庫等水利工程的泥沙沉積規(guī)律，許多相關(guān)科研機(jī)構(gòu)建立了水利工程泥沙沉積實(shí)驗(yàn)室。這種類型的實(shí)驗(yàn)工作中一個(gè)重要工作就是測量河床或者水庫在放水前后的幾何尺寸變化大小，從而建立水利工程的泥沙沉積模型[1-2]。對于水利設(shè)施模型的測繪，如果采用水準(zhǔn)儀和經(jīng)緯儀或全站儀，按照傳統(tǒng)的測量方法進(jìn)行實(shí)地測量，會存在諸多問題，比如采樣點(diǎn)分布不連續(xù)、精度低、作業(yè)時(shí)間長、工作量浩大等缺點(diǎn)。特別是在測量過程中，制約了水動(dòng)力、泥沙沉積的過程研究，直接影響泥沙沉積模型的科學(xué)結(jié)論。

針對此問題曾有學(xué)者提出基于攝影測量的定軌移動(dòng)掃描系統(tǒng)，但該系統(tǒng)存在諸多不足之處：首先需要大量的像控點(diǎn)，而在像控點(diǎn)的測量中往往存在較大的誤差；其次移動(dòng)設(shè)備的位置具有很大的不確定性，攝影測量設(shè)備在軌道上行進(jìn)的過程中會產(chǎn)生晃動(dòng)、傾斜問題，特別是在周期性觀測過程中由于姿態(tài)、位置的不確定性給后續(xù)的解析計(jì)算帶來很大不利因素[2]。

由于傳統(tǒng)測量方式對于水利模型測量的種種不適，所以迫切需要研究出一種新的測量方式。在實(shí)驗(yàn)室前幾代研究模型的基礎(chǔ)上提出慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)與掃描儀相結(jié)合的測量方式，用INS提供的位置、姿態(tài)信息來實(shí)現(xiàn)掃描儀的精確定位。INS可以與全球定位系統(tǒng)(global positioning system，GPS)組合定位；但是由于室內(nèi)GPS信號質(zhì)量太差，INS不能如同室外情況下使用GPS/INS組合系統(tǒng)來減小隨時(shí)間而逐漸積累增大的定位誤差，無法單獨(dú)實(shí)現(xiàn)室內(nèi)精確定位的要求。要實(shí)現(xiàn)水利模型的高精度測量，必須為掃描儀提供準(zhǔn)確的位置、姿態(tài)信息；因此就有了室內(nèi)情況下精確定位、定姿問題。本文針對這個(gè)問題特提出超寬帶與INS相結(jié)合的室內(nèi)定位組合定位系統(tǒng)。

1組合定位系統(tǒng)介紹

超寬帶(ultra-wide band，UWB)以其隱蔽性好、截獲率低、處理增益高、多徑分辨能力強(qiáng)、傳輸速率高、系統(tǒng)容量大、功耗低等特點(diǎn)逐漸引起人們重視，成為無線定位的熱點(diǎn)[3]。UWB信號的這幾種特點(diǎn)，注定了它在室內(nèi)定位研究上的優(yōu)勢。因此，基于UWB超寬帶技術(shù)的室內(nèi)定位技術(shù)逐漸成為人們的一個(gè)研究重點(diǎn)。但是測距信號易受周圍環(huán)境影響，特別是非視距傳播(non-line of sight，NLOS)很大程度上降低了系統(tǒng)的定位精度。捷聯(lián)慣性導(dǎo)航系統(tǒng) (strapdown inertial navigation system，SINS)是一種自主式導(dǎo)航定位系統(tǒng)，并且有較高的數(shù)據(jù)更新率，其主要缺點(diǎn)是隨著時(shí)間的積累，本身的誤差將會變大，即只能夠在短時(shí)間內(nèi)保持高精度[4]。本文選擇將慣性導(dǎo)航技術(shù)和UWB室內(nèi)定位技術(shù)相結(jié)合的組合定位方式，來提高定位精度。

組合定位原理如圖1，系統(tǒng)有2個(gè)獨(dú)立的測量系統(tǒng)，即SINS和UWB測距設(shè)備構(gòu)成的定位系統(tǒng)：INS采用基于微機(jī)電系統(tǒng)(microelectromechanical systems，MEMS)的SINS，實(shí)時(shí)輸出INS的位置，姿態(tài)信息；UWB系統(tǒng)測量移動(dòng)站到基站的距離，然后通過到達(dá)時(shí)間差(time difference of arrival，TDOA)算法，進(jìn)行定位解算獲取移動(dòng)站的坐標(biāo)。選取2個(gè)系統(tǒng)的位置差值作為觀測量，設(shè)計(jì)采取松組合方式的擴(kuò)展卡爾曼濾波器進(jìn)行解算，將解算出來的結(jié)果再反饋給INS，用來保證INS的定位精度。

圖1　組合導(dǎo)航定位系統(tǒng)原理

2UWB定位技術(shù)

2.1TDOA定位原理

TDOA定位算法的基本思想是測量移動(dòng)臺發(fā)射信號到達(dá)不同基站的時(shí)間差來計(jì)算出一系列距離差，再利用雙曲線的特性——線上的點(diǎn)到2焦點(diǎn)距離之差為定值計(jì)算出移動(dòng)站位置信息[5]。TDOA 定位的雙曲線模型如圖2所示。

圖2　TDOA定位原理

假設(shè)有3個(gè)參考節(jié)點(diǎn)BS1、BS2和BS3，移動(dòng)站到各參考節(jié)點(diǎn)的距離分別是d1、d2和d3。移動(dòng)站到BS1和BS2之間的距離差d21=d2-d1，依據(jù)雙曲線原理，移動(dòng)站位于以BS1、BS2為焦點(diǎn)，以d21為焦距的雙曲線上；同理可知移動(dòng)站也必然位于以BS1、BS3為焦點(diǎn)，以d31為焦距的雙曲線上[6]：因此2條雙曲線的交點(diǎn)就是移動(dòng)站的位置。需要注意的是，2條雙曲線的焦點(diǎn)會有2個(gè)，需要通過先驗(yàn)信息篩選出正確坐標(biāo)值。

此外，在2維坐標(biāo)系里，TDOA定位利用至少4個(gè)基站接收到移動(dòng)站發(fā)出的信號來確定移動(dòng)站的位置坐標(biāo)，3空間里則至少需要5個(gè)參考基站[7]。參考基站的位置很重要，盡量從上下2個(gè)方向使基站的覆蓋范圍包圍試驗(yàn)場，而不要使所有的參考基站都位于同1個(gè)平面范圍內(nèi)。

2.2影響UWB定位精度的因素

影響UWB無線定位精度的個(gè)重要因素是在無線電波的傳播過程中，發(fā)射機(jī)和接收機(jī)之間由于障礙物的阻礙會使得信號無法按照直射路徑進(jìn)行傳播，而只能通過由衍射、折射、反射等方式部分被接收機(jī)所接收而產(chǎn)生的NLOS傳播誤差[8]；如果應(yīng)用NLOS情況下得到的測量值對目標(biāo)進(jìn)行估計(jì)，必然會存在很大的不準(zhǔn)確性從而影響到達(dá)時(shí)間(time of arrival，TOA)或TDOA的定位精度，使得基于TOA或TDOA算法的定位估計(jì)成為有偏估計(jì)：因此就需要根據(jù)需求對測量值進(jìn)行處理再實(shí)施定位。多徑效應(yīng)和NLOS傳播是影響室內(nèi)定位精度的主要因素，而采用UWB方式極短時(shí)間的窄脈沖則時(shí)間、空間分辨率都很強(qiáng)，并且具有較好的多徑分辨率。在定位系統(tǒng)中，如何減小由于NLOS傳播產(chǎn)生的誤差是提高定位精度的關(guān)鍵之一[8-9]。

本文采用基于卡爾曼濾波的TDOA定位算法首先對來自不同基站的距離測量值進(jìn)行濾波處理，采用Wylie方法對含有非視距誤差的距離測量值進(jìn)行判別或重構(gòu)；然后使用正弦濾波對使用線性化最小二乘法計(jì)算出的移動(dòng)臺的位置進(jìn)行處理，從而得到平滑軌跡[10]。

利用Kalman濾波器對相應(yīng)范圍內(nèi)各個(gè)基站BSi(i=1，2，…，M)與移動(dòng)臺MS之間的距離值進(jìn)行處理，其狀態(tài)方程與觀測方程為:

X(k+1)=ΦX(k)+Γu(k);

(1)

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

(2)

v、u為零均值的觀測噪聲和過程噪聲；T為采樣周期；在卡爾曼濾波算法中：

狀態(tài)一步預(yù)測為

(3)

狀態(tài)估計(jì)為

(4)

濾波增益為

(5)

一步預(yù)測均方誤差為

(6)

估計(jì)均方差為

Pk=(I-KkHk)Pk|k-1。

(7)

3誤差模型

3.1SINS誤差模型

陀螺是INS的主要部件之一，SINS的誤差主要來源于陀螺的漂移。短時(shí)間內(nèi)，白噪聲和量化噪聲對角誤差的影響最大[11-12]。陀螺建模誤差包括3項(xiàng)，分別為常值誤差、隨機(jī)誤差和一階馬爾科夫過程誤差[9]。陀螺誤差模型為

ε=εe+εr+ωg。

(8)

在組合導(dǎo)航設(shè)計(jì)中，加速度計(jì)誤差模型一般只考慮分量相對較小的偏置誤差i(i=x，y，z)[12-13]。所以加速度計(jì)誤差模型為

i=bi+ωai(i=x，y，z)。

(9)

在考慮到SINS測量誤差的同時(shí)，還要考慮到儀器本身的誤差，包括儀器刻度誤差、INS元件的安裝誤差等。

3.2UWB測距設(shè)備誤差模型

由于本文采用了TDOA算法，各測距設(shè)備的常值誤差將抵消；并且通過分析TDOA的定位原理和實(shí)驗(yàn)測量數(shù)據(jù)可以發(fā)現(xiàn)UWB測距設(shè)備的噪聲信號的相關(guān)性比較小，馬爾科夫相關(guān)時(shí)間較大，故UWB測距設(shè)備的誤差模型為隨機(jī)誤差。

3.3Kalman濾波器模型

本文濾波器模型采用在非線性系統(tǒng)中應(yīng)用最廣的擴(kuò)展卡爾曼濾波算法。擴(kuò)展卡爾曼濾波的基本思想是通過對非線性系統(tǒng)進(jìn)行線性化，并且忽略其高階項(xiàng)誤差，將系統(tǒng)模型轉(zhuǎn)變?yōu)檎`差形式的狀態(tài)方程和測量方程；然后再利用標(biāo)準(zhǔn)卡爾曼濾波方程對狀態(tài)誤差進(jìn)行估計(jì)[14]。

假設(shè)非線性的觀測模型為

Z(t)=h(X(t))+V(t)，

(10)

式中h(x)為狀態(tài)向量的非線性函數(shù)，對其進(jìn)行線性化并寫出離散化形式，則

(11)

經(jīng)過線性化后的誤差狀態(tài)模型和觀測模型可以采用標(biāo)準(zhǔn)卡爾曼濾波方程進(jìn)行濾波解算[15]。

4系統(tǒng)仿真與結(jié)果分析

為了評定組合定位系統(tǒng)的性能，根據(jù)學(xué)校3D實(shí)驗(yàn)室實(shí)際情況本文設(shè)置如下仿真環(huán)境：仿真目標(biāo)物運(yùn)動(dòng)軌跡如圖3，實(shí)驗(yàn)場地為長150 m、寬10 m的矩形區(qū)域，軌跡起始點(diǎn)為(0，3)，沿圖中曲線勻速運(yùn)動(dòng)，速率為0.3 m/s，運(yùn)動(dòng)到點(diǎn)(120，3) 結(jié)束，共用時(shí)379.26 s；4個(gè)基站在矩形區(qū)域的4個(gè)角上，測量移動(dòng)站到各個(gè)基站的距離為UWB定位解算提供數(shù)據(jù)源；由微型慣性測量組合(miniature inertial measurement unit，MIMU)組成的微SINS位于移動(dòng)站上，分別感知載體的角速度和加速度，為INS解算提供數(shù)據(jù)源。

INS仿真條件與文獻(xiàn)[10]實(shí)驗(yàn)條件一致。UWB測距隨機(jī)零偏均方根為0.1 m。設(shè)置的3個(gè)工作階段，見表1。

表1　組合定位系統(tǒng)子系統(tǒng)工作狀態(tài)

按照以上條件進(jìn)行仿真，定位效果如圖3，圖4所示。

圖3　仿真目標(biāo)運(yùn)動(dòng)路線

圖4　仿真定位效果

將定位誤差分解成東向和北向2個(gè)方向，誤差分布圖如圖5、圖6所示。在第1階段，MINS正常工作，定位軌跡較平滑，但是由于僅僅是MINS在運(yùn)行，所以INS的定位誤差隨時(shí)間逐漸累積，在42.03 s時(shí)，定位誤差累積到了1 m；在第2階段，MINS和UWB共同正常工作，2個(gè)系統(tǒng)進(jìn)行組合定位，UWB間斷定位抑制MINS的誤差累計(jì)，MINS對UWB進(jìn)行平滑處理，抑制UWB的信號跳變，將定位精度控制在5 cm左右；在第3階段，MINS出現(xiàn)故障，UWB單獨(dú)定位，由于缺乏MINS的平滑處理，UWB的單獨(dú)定位出現(xiàn)了較大的波動(dòng)性。

圖5　東向定位誤差曲線

圖6　北向定位誤差曲線

經(jīng)結(jié)果分析，比較3個(gè)階段的定位效果可知：第1、3階段當(dāng)2個(gè)子系統(tǒng)單獨(dú)工作時(shí)定位結(jié)果都存在較大的誤差；第2階段當(dāng)采用INS和超寬帶組合定位時(shí)，2個(gè)子系統(tǒng)通過互補(bǔ)使定位精度得到很大提高，實(shí)現(xiàn)了較高精度室內(nèi)定位的要求。

5結(jié)束語

本文通過構(gòu)建松組合形式的卡爾曼濾波器，對UWB/INS組合定位系統(tǒng)進(jìn)行仿真，結(jié)果表明此組合系統(tǒng)充分發(fā)揮各自的優(yōu)勢，削弱了INS和UWB系統(tǒng)的自有誤差，大大提高了系統(tǒng)的定位精度。另外，在測距階段，通過提高在NLOS情況下非視距測量值的判別能力和重構(gòu)能力，優(yōu)化定位算法，減小采樣周期，可以使定位結(jié)果更加準(zhǔn)確、平滑。本文所研究的組合定位系統(tǒng)不僅可用于水利模型實(shí)驗(yàn)室，同樣可用于其他類型的對室內(nèi)定位有較高要求的環(huán)境中。

參考文獻(xiàn)

[1]石波，李國玉，景冬，等.基于RFID技術(shù)的POS輔助室內(nèi)定軌移動(dòng)攝影測量方法研究[J].大地測量與地球動(dòng)力學(xué)，2012，4(32)：129-134.

[2]李國玉.室內(nèi)定軌移動(dòng)攝影測量控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].青島：山東科技科技大學(xué)，2011：12-14.

[3]陳思，張邦寧，郭道省.超寬帶技術(shù)的抗干擾抗截獲性能研究[C]//中國電子學(xué)會.軍事電子信息學(xué)術(shù)會議論文集.武漢：中國電子學(xué)會，2006：313-316.

[4]喬洋，趙育善，趙金才.慣性導(dǎo)航/雙星組合導(dǎo)航的可行性研究[J].上海航天，2003，20(4)：34-37.

[5]蔚娜.蜂窩網(wǎng)絡(luò)中動(dòng)態(tài)選擇基站的無線定位算法[J].現(xiàn)代電子信息技術(shù)理論與應(yīng)用，2005(1)：361-364.

[6]杜海兵，王民北.數(shù)據(jù)融合技術(shù)在無線移動(dòng)定位中的應(yīng)用[J].計(jì)算機(jī)工程與應(yīng)用，2005，6(1)：153-155.

[7]海燕，張葡青.無線定位技術(shù)的物理基礎(chǔ)及其關(guān)鍵技術(shù)分析[J].中山大學(xué)學(xué)報(bào)：自然科學(xué)版，2005，44(3)：42-45.

[8]史巖，朱濤，傅軍.基于WPS/GPS/MIMU組合的無縫定位技術(shù)研究[J].計(jì)算機(jī)測量與控制，2014，22(10)：3388-3391.

[9]賀廣宇.UWB高精度定位中的信道估計(jì)及均衡技術(shù)研究[D].西安：西安電子科技大學(xué)，2010：31-34.

[10]傅軍，朱濤，史巖.高精度RNS/SINS組合定位算法研究[J].計(jì)算機(jī)仿真，2014，31(6)：115-119.

[11]楊彬，金毅，高娜.INS軌跡發(fā)生器誤差引入研究[J].導(dǎo)彈與航天運(yùn)載技術(shù)，2015(2)：102-106.

[12]盧舒勃.民用飛機(jī)導(dǎo)航傳感器濾波算法研究[D].上海：上海交通大學(xué)，2011：32-36.

[13]王明昊，王仕成，牟建華.SINS/天文組合導(dǎo)航[J].彈箭與制導(dǎo)學(xué)報(bào)，2009，4(2)：87-90.

[14]楊靜，張洪鉞，李驥.預(yù)測濾波器理論在INS非線性對準(zhǔn)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，2003，12(6)：44-52.

[15]李鵬程.組合導(dǎo)航及其濾波算法研究[D].西安：西安電子科技大學(xué)，2010：42-45.

Study on indoor locating system of high-accuracy UWB/SINS combination

LIU Jianhui1，WANG Dong1，LIU Fengying1，WANG Jing2

(1.College of Geosciences and Technology，Shandong University of Science and Technology，Shandong Qingdao 266590，China；2.Guizhou Transportation Planning Survey & Design Academy Co.，Ltd.，Guiyang 550000，China)

Abstract：Through the simulation measuring experiment of sediment deposition rule in water conservancy project，this paper puts forward a measurement system which combines inertial navigation with scanner.Meanwhile，the indoor positioning system technology based on the UWB was joined in the measurement system to solve the problem of inertial navigation error accumulation.Then a simulation of combined positioning system was carried out according to the track generator and the loosely coupled integration Kalman filter.The result indicated that the inertial navigation of combined positioning system and UWB would have complementary advantages by depressing the error accumulation and improving the positioning accuracy of indoor positioning system as well.

Keywords：UWB；SINS；Kalman filtering；integrated navigation；indoor positioning

收稿日期：2016-01-05

第一作者簡介：劉見輝(1986—)，男，河南周口人，碩士研究生，研究方向?yàn)?S集成與應(yīng)用。

中圖分類號：P228

文獻(xiàn)標(biāo)志碼：A

文章編號：2095-4999(2016)02-0053-05

引文格式：劉見輝，王冬，劉鳳英，等.高精度UWB/SINS組合室內(nèi)定位系統(tǒng)研究[J].導(dǎo)航定位學(xué)報(bào)，2016，4(2)：53-56,86.(LIU Jianhui，WANG Dong，LIU Fengying，et al.Study on indoor locating system of high-accuracy UWB/SINS combination[J].Journal of Navigation and Positioning，2016，4(2)：53-56,86.)DOI：10.16547/j.cnki.10-1096.20160211.