基于UKF的環(huán)境自適應(yīng)UWB/DR室內(nèi)定位方法①

周 琳,李曉明,江先志

(浙江理工大學(xué) 機(jī)械與自動(dòng)控制學(xué)院,杭州 310018)

隨著無(wú)線(xiàn)定位技術(shù)的不斷發(fā)展,基于位置服務(wù)的研究越來(lái)越受到了人們的關(guān)注[1].目前,全球定位系統(tǒng)(Global Positioning System,GPS)已被廣泛應(yīng)用于室外定位[2],但由于墻體等建筑物對(duì)衛(wèi)星信號(hào)遮擋,其在室內(nèi)環(huán)境下定位效果較差.因此,室內(nèi)定位技術(shù)不斷涌現(xiàn),比如Zigbee、UWB、WIFI、RFID 等定位技術(shù)[3].其中UWB 是一種穿透力強(qiáng)、抗干擾能力強(qiáng)的無(wú)線(xiàn)定位技術(shù),比WIFI、ZigBee、RFID 等室內(nèi)無(wú)線(xiàn)定位技術(shù)有著更高的定位精度,在視距(los of sight,LOS)情況下可達(dá)厘米級(jí)別定位精度且不存在誤差累積,然而在復(fù)雜的室內(nèi)環(huán)境中,UWB 信號(hào)在傳播過(guò)程中不可避免地產(chǎn)生多徑效應(yīng),從而導(dǎo)致明顯的NLOS 定位誤差[4–8].而DR 是一種具有自主定位導(dǎo)航能力并且短時(shí)定位高精度的技術(shù),但其定位結(jié)果將會(huì)隨著時(shí)間的推移發(fā)生飄移,即存在定位誤差累積的問(wèn)題[9].

針對(duì)上述問(wèn)題,本文提出了一種基于UKF 的環(huán)境自適應(yīng)UWB/DR 室內(nèi)定位方法,將UWB 定位和DR定位的優(yōu)勢(shì)聯(lián)合,用DR 定位降低UWB 定位出現(xiàn)的NLOS 誤差,用UWB 定位更新DR 定位的當(dāng)前位置,減少DR 定位的累積誤差.該方法通過(guò)建立自適應(yīng)UKF 濾波模型,將UWB 定位信息和DR 定位信息進(jìn)行融合.在此過(guò)程中,利用新息和高斯分布的 3σ原則來(lái)對(duì)UWB 定位結(jié)果進(jìn)行非視距檢測(cè),再通過(guò)新息的實(shí)時(shí)估計(jì)協(xié)方差和理論協(xié)方差來(lái)構(gòu)建環(huán)境適應(yīng)系數(shù),進(jìn)而用此系數(shù)動(dòng)態(tài)修正UWB 定位的觀測(cè)噪聲,使得觀測(cè)噪聲自適應(yīng)真實(shí)環(huán)境,更大程度地降低NLOS 誤差對(duì)融合定位結(jié)果的影響.為了驗(yàn)證本文定位方法的有效性和穩(wěn)定性,設(shè)計(jì)差速移動(dòng)機(jī)器人在多遮擋室內(nèi)環(huán)境中進(jìn)行折線(xiàn)運(yùn)動(dòng)實(shí)驗(yàn)并實(shí)時(shí)采集和處理數(shù)據(jù).實(shí)驗(yàn)效果表明,該方法能夠有效減小UWB 定位的NLOS誤差和DR 定位的累積誤差,并且由于環(huán)境適應(yīng)系數(shù)的創(chuàng)新引入,比UKF 定位方法和PF 定位方法具有更高的定位精度和更強(qiáng)的抗NLOS 誤差性能.

1 DR 定位

DR 定位是一種精度較高的自主式定位方法,尤其適用于短時(shí)短距定位.其核心思想可概述為根據(jù)當(dāng)前時(shí)刻的位置、速度和航向推算下一時(shí)刻的位置.航位推算的實(shí)現(xiàn)原理如圖1所示.

已知當(dāng)前時(shí)刻的初始位置 (xk,yk)、航向角θk和速度vk,移動(dòng)機(jī)器人在下一時(shí)刻移動(dòng)到(xk+1,yk+1)時(shí),航向角的變化為 ωk,根據(jù)當(dāng)前的速度vk乘以間隔時(shí)間tk+1?tk得到的航行距離lk,從而tk+1時(shí)刻的位置和航向角可以表示為:

圖1 航位推算原理圖

采用的數(shù)據(jù)采集移動(dòng)平臺(tái)如圖2所示.該移動(dòng)平臺(tái)采用差速驅(qū)動(dòng),左右兩個(gè)為直流電機(jī)驅(qū)動(dòng)的主動(dòng)輪,前后兩個(gè)為用于支撐的萬(wàn)向輪.本文通過(guò)安裝在電機(jī)軸端的光電編碼器輸出來(lái)進(jìn)行航位推算.

設(shè)車(chē)輪的直徑為D,兩車(chē)輪間距為L(zhǎng),左右兩個(gè)驅(qū)動(dòng)輪的轉(zhuǎn)速分別為ωl,ωr,根據(jù)差速移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)關(guān)系[10]可知速度和航向變化的計(jì)算表達(dá)式為:

圖2 數(shù)據(jù)采集移動(dòng)平臺(tái)圖

因此k時(shí)刻根據(jù)的位置和航向推得的下一時(shí)刻的位置和航向可表達(dá)為:

其中,ωRk,ωLk分 別表示左右驅(qū)動(dòng)輪ti時(shí)刻的轉(zhuǎn)速,由光電編碼器測(cè)得.

2 基于TOA 的UWB 定位

到達(dá)時(shí)間方法(Time Of Arrive,TOA)的核心在于精確地測(cè)量出信號(hào)從發(fā)射機(jī)到達(dá)接收機(jī)的飛行時(shí)間.已知信號(hào)的傳播速度即可精確地計(jì)算出發(fā)送機(jī)和接收機(jī)的之間的距離,但是發(fā)射機(jī)和接收機(jī)采用不同的時(shí)鐘源,即兩者存在影響飛行時(shí)間測(cè)量值的時(shí)鐘差,又因?yàn)樾盘?hào)傳播速度非常快,TOA 所測(cè)距離準(zhǔn)確度難以保證.因此采用DS 測(cè)距(Double-Sided two-way ranging)消除時(shí)鐘差影響,其原理圖如圖3所示.

圖3 DS 測(cè)距原理圖

圖3中,Tc1,Tc2分別表示設(shè)備1 和設(shè)備2 發(fā)出信號(hào)到收到回應(yīng)信號(hào)所耗費(fèi)的時(shí)間,Tr1,Tr2則分別表示設(shè)備1 和設(shè)備2 處理收到的信號(hào)所耗費(fèi)的時(shí)間,c為光速,從而可得待測(cè)節(jié)點(diǎn)到四周各個(gè)基站的距離為:

該方法等效于分別用設(shè)備發(fā)射機(jī)和接收機(jī)的時(shí)鐘計(jì)算一次來(lái)回路程花費(fèi)的時(shí)間,再四均等分總時(shí)間求取測(cè)量時(shí)間.這可以有效減少不同設(shè)備之間的時(shí)鐘差導(dǎo)致的飛行時(shí)間誤差,極大提高了TOA 的精度.

本文所采用的UWB 定位系統(tǒng)的原理如圖4所示,移動(dòng)站安裝在數(shù)據(jù)采集移動(dòng)平臺(tái)上,定位系統(tǒng)的基準(zhǔn)錨點(diǎn)為四周的4 個(gè)定位基站,基站基于TOA 方法實(shí)時(shí)獲取其到移動(dòng)站的距離.

令UWB 移動(dòng)站(Moving Station,MS)和基站(Base Station,BS)的位置分別為(x,y)和(xi,yi),MS 和BS 的距離為di,因此可得方程組為:

圖4 UWB 定位系統(tǒng)原理圖

用該方程組的最后一個(gè)方程減去前n–1 個(gè)方程即可得方程組如下:

然而在實(shí)際情況下,通過(guò)測(cè)量所得d1,d2···di都是存在誤差的,因此使用最小二乘法去估計(jì)待測(cè)節(jié)點(diǎn)的位置,則可以得到式(6)的解為:

3 基于UKF 的環(huán)境自適應(yīng)UWB/DR 定位

3.1 選用UKF 的合理性分析

線(xiàn)性系統(tǒng)疊加原理表明線(xiàn)性系統(tǒng)的多個(gè)輸入信號(hào)的總響應(yīng)等于各個(gè)信號(hào)單獨(dú)作用產(chǎn)生響應(yīng)的代數(shù)和.由式(3)可知,DR 定位涉及三角函數(shù)運(yùn)算,因此本文提出的室內(nèi)定位模型是非線(xiàn)性的,不符合疊加原理.一般采用擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)或無(wú)跡卡爾曼濾波處理非線(xiàn)性模型的濾波問(wèn)題.其中EKF 通過(guò)將非線(xiàn)性函數(shù)進(jìn)行Taylor 級(jí)數(shù)展開(kāi),并保留其一階近似項(xiàng)實(shí)現(xiàn)線(xiàn)性化,難免引入線(xiàn)性化誤差,而UKF 利用無(wú)跡變換(Unscented Transform,UT)處理均值和協(xié)方差的非線(xiàn)性傳遞問(wèn)題,直接基于非線(xiàn)性模型進(jìn)行解算,免除線(xiàn)性化誤差,精確度更高[11–14].因此本文選用UKF 方法來(lái)進(jìn)行融合濾波.

3.2 UKF 預(yù)測(cè)方程和觀測(cè)方程確定

根據(jù)DR 定位建立系統(tǒng)的狀態(tài)預(yù)測(cè)方程如下:

式中,X=[x,y,θ]T為運(yùn)動(dòng)狀態(tài),(x,y)為機(jī)器人當(dāng)前位置,θ為當(dāng)前航向,ωRk?1,ωLk?1為電機(jī)編碼器測(cè)得的電機(jī)轉(zhuǎn)速,作為運(yùn)動(dòng)狀態(tài)的控制變量,Qk為三維的過(guò)程噪聲.根據(jù)式(5)可得f(Xk?1,ωRk?1,ωLk?1)的表達(dá)式為:

根據(jù)UWB 的TOA 定位方法可以獲得位置坐標(biāo),根據(jù)電子羅盤(pán)可測(cè)得航向,因此以位置和航向作為觀測(cè)量從而建立測(cè)量方程如下:

式中,Rk為三維得到觀測(cè)噪聲,H為觀測(cè)矩陣,其表達(dá)式為:

3.3 UKF 方法

無(wú)跡卡爾曼濾波的大體思路是結(jié)合Kalman 線(xiàn)性濾波框架和UT 變換來(lái)處理均值和協(xié)方差的非線(xiàn)性傳遞問(wèn)題,提高非線(xiàn)性問(wèn)題的濾波效果.UKF 計(jì)算步驟如下:

(1)初始化

計(jì)算系統(tǒng)狀態(tài)和方差的初始值,在導(dǎo)航開(kāi)始階段多次采集UWB 定位坐標(biāo)和電子羅盤(pán)航向來(lái)計(jì)算系統(tǒng)狀態(tài)和方差的初始值.

(2)Sigma 點(diǎn)和對(duì)應(yīng)權(quán)值計(jì)算

其中,λ=α2(n+κ)?n,n為系統(tǒng)狀態(tài)X的維數(shù),α 和κ 確定采樣點(diǎn)在均值附近的分布,通常 α取一個(gè)較小的正值,κ=3?n,P(k|k)i為協(xié)方差矩陣的第i列.β 是非負(fù)的權(quán)系數(shù),如果x服從高斯分布,β 最優(yōu)值為2.

(3)狀態(tài)預(yù)測(cè)

(4)量測(cè)預(yù)測(cè)

(5)狀態(tài)更新

3.4 環(huán)境適應(yīng)系數(shù)構(gòu)建

由于室內(nèi)環(huán)境復(fù)雜,往往存在著諸多障礙,采用UWB 進(jìn)行室內(nèi)定位時(shí),其信號(hào)傳播過(guò)程中將會(huì)發(fā)生反射和折射,從而導(dǎo)致NLOS 誤差,此時(shí)LOS 狀態(tài)下確認(rèn)的UWB 定位結(jié)果的觀測(cè)噪聲已無(wú)法正確反饋.而利用穩(wěn)定噪聲統(tǒng)計(jì)特性的UKF 濾波不能得知這種變化,因而大大影響其濾波的精度和穩(wěn)定性.為了優(yōu)化濾波的效果,構(gòu)建環(huán)境適應(yīng)系數(shù)來(lái)對(duì)觀測(cè)噪聲實(shí)時(shí)修正,使得觀測(cè)噪聲能根據(jù)環(huán)境變化自適應(yīng).

新息定義為:

理論上,最優(yōu)濾波的殘差服從均值為零的高斯分布,其協(xié)方差為:

但當(dāng)BS 和MS 出現(xiàn)NLOS 環(huán)境的時(shí)候,殘差將會(huì)發(fā)生顯著的變化.因此可以根據(jù)高斯分布的 3σ原則,利用新息的理論協(xié)方差的均方根構(gòu)建校驗(yàn)信息來(lái)對(duì)UWB 得到的定位結(jié)果進(jìn)行非視距檢測(cè).

結(jié)合開(kāi)窗估計(jì)法和極大似然估計(jì)法可得到新息的實(shí)時(shí)估計(jì)協(xié)方差為[15,16]:

則利用實(shí)時(shí)估計(jì)協(xié)方差和理論協(xié)方差構(gòu)建環(huán)境適應(yīng)系數(shù)為:

式中,RLOS為UWB 在LOS 情況下定位結(jié)果的協(xié)方差,則UWB 定位的觀測(cè)噪聲在每一次進(jìn)行無(wú)跡卡爾曼濾波的時(shí)候都進(jìn)行動(dòng)態(tài)修正為:

4 實(shí)驗(yàn)分析

實(shí)驗(yàn)環(huán)境如圖5所示,在場(chǎng)地內(nèi)共放置了4 個(gè)UWB 基站,坐標(biāo)分別為A(0,0),B(840,0),C(0,840),D(840,840),單位為cm.數(shù)據(jù)采集移動(dòng)平臺(tái)在移動(dòng)過(guò)程中的可以實(shí)時(shí)采集航向和車(chē)速信息,并通過(guò)ESP8266不斷向外傳輸,UWB 基站A 通過(guò)串口不斷向外發(fā)送移動(dòng)站和各個(gè)基站的距離.通過(guò)自主開(kāi)發(fā)的C#軟件同頻率實(shí)時(shí)記錄以上數(shù)據(jù).

圖5 實(shí)驗(yàn)環(huán)境圖

為了更好驗(yàn)證所提出方法的定位效果,設(shè)計(jì)途經(jīng)NLOS 誤差路段的折線(xiàn)路徑實(shí)驗(yàn),即采集平臺(tái)在空間阻擋的區(qū)域中按計(jì)劃路徑勻速運(yùn)行時(shí)的信息.由于NLOS 誤差的存在,觀測(cè)噪聲模型必然呈現(xiàn)非高斯特性.至此可知,室內(nèi)定位模型是非線(xiàn)性且非高斯的.而粒子濾波是一種適于處理此類(lèi)模型的成熟濾波方法.因此本文分別通過(guò)UKF、PF 和環(huán)境自適應(yīng)UKF 方法處理數(shù)據(jù),并比對(duì)和分析定位效果得出實(shí)驗(yàn)結(jié)果.

實(shí)驗(yàn)結(jié)果如圖6～圖9所示,分別分析基于最小二乘法得到的測(cè)量值、UKF 方法、PF 方法以及本文方法的運(yùn)動(dòng)軌跡復(fù)現(xiàn)、定位誤差、定位累計(jì)誤差以及定位誤差累積概率分布.

圖6對(duì)比了不同方法的運(yùn)動(dòng)軌跡復(fù)現(xiàn)結(jié)果,可以看出,在LOS 情況下測(cè)量值也有著較高的定位精度,此時(shí)基于非視距檢測(cè)構(gòu)建的環(huán)境適應(yīng)系數(shù)為I,即環(huán)境自適應(yīng)UKF 和UKF 具有相同濾波效果;當(dāng)存在NLOS測(cè)距異常值時(shí),測(cè)量值的定位精度明顯降低,UKF、PF 和環(huán)境自適應(yīng)UKF 方法均表現(xiàn)出較強(qiáng)的抗NLOS性能.

圖6 運(yùn)動(dòng)軌跡復(fù)現(xiàn)

圖7和圖8中50～60 s 可以看出,當(dāng)出現(xiàn)NLOS 誤差的時(shí)候,測(cè)量值的定位誤差明顯增大,UKF 方法和PF 方法一定程度抑制了誤差,本文方法能夠顯著提高NLOS 情況下的定位精度,使之與LOS 情況下的定位誤差變化與累計(jì)誤差增長(zhǎng)趨勢(shì)相近.

圖7 定位誤差

圖8 定位累積誤差

圖9所示為不同方法的誤差累積概率分布,進(jìn)一步驗(yàn)證了在有遮擋的路徑下,本文方法相比于其它方法具有更優(yōu)的定位性能.

表1是NLOS 誤差量化表,從中可以看出在NLOS情況下,本文方法的最大誤差值、平均誤差值相較其他方法最低,NLOS 降低率最高.此外用Matlab 統(tǒng)計(jì)得,UKF、PF、本文方法的運(yùn)算時(shí)間分別是0.0117 s,0.6094 s,0.0125 s.因此PF 方法定位效果略?xún)?yōu)于UKF方法,但運(yùn)算效率過(guò)低;環(huán)境自適應(yīng)UKF 通過(guò)消耗低成本運(yùn)算效率換得顯著優(yōu)于UKF 方法的定位效果,進(jìn)一步表明在NLOS 情況下,本文方法相比于其它方法具有更好的性能.

圖9 定位誤差累積概率分布

表1 NLOS 誤差量化表

總之,實(shí)驗(yàn)數(shù)據(jù)和結(jié)果表明,本文方法在LOS 情況下定位效果與UKF 方法、PF 方法相近;在NLOS情況下,由于環(huán)境適應(yīng)系數(shù)的動(dòng)態(tài)修正,比UKF 方法和PF 方法具有更高的定位性能.

5 結(jié)論

針對(duì)復(fù)雜室內(nèi)環(huán)境下UWB 信號(hào)傳播的非視距NLOS 誤差問(wèn)題,本文提出基于UKF 的環(huán)境自適應(yīng)UWB/DR 室內(nèi)定位方法.該方法從UWB 定位環(huán)境的誤差產(chǎn)生原因出發(fā),通過(guò)建立環(huán)境自適應(yīng)UKF 濾波模型,在UWB/DR 定位融合過(guò)程中對(duì)UWB 定位信息進(jìn)行非視距檢測(cè)并且引入環(huán)境適應(yīng)系數(shù)動(dòng)態(tài)修正UWB定位觀測(cè)噪聲.實(shí)驗(yàn)效果表明,該方法具有較高的抗NLOS 誤差性能,定位精度優(yōu)于PF 方法和UKF 方法,是一種具有較強(qiáng)實(shí)用性且高精度的室內(nèi)定位方法.