一種基于偽距殘差加權(quán)的最小二乘定位算法

秦明峰，胡麗格

(1. 衛(wèi)星導(dǎo)航系統(tǒng)與裝備國家重點(diǎn)實(shí)驗(yàn)室，河北石家莊 050081；2. 中國電子科技集團(tuán)公司第五十四研究所，河北石家莊 050081)

1 引言

導(dǎo)航與位置服務(wù)在物聯(lián)網(wǎng)、智慧城市、公共安全和救災(zāi)減災(zāi)等領(lǐng)域發(fā)揮著基礎(chǔ)性支撐作用，雖然全球衛(wèi)星導(dǎo)航定位系統(tǒng)(GNSS)可以在室外提供較高的導(dǎo)航定位精度，但面對(duì)復(fù)雜的室內(nèi)場(chǎng)景，GNSS信號(hào)由于遮擋和多徑，其定位效果不如室外[1，2]。但是現(xiàn)代社會(huì)人們四分之三以上的時(shí)間都是室內(nèi)活動(dòng)，室內(nèi)高精度、高穩(wěn)定性和高連續(xù)性的導(dǎo)航與位置服務(wù)有著廣闊的發(fā)展前景。為此，開展室內(nèi)復(fù)雜環(huán)境下的定位導(dǎo)航服務(wù)技術(shù)研究有著重要的意義[3]。

室內(nèi)定位需要考慮在室內(nèi)的復(fù)雜環(huán)境下高精度、低成本和實(shí)現(xiàn)復(fù)雜度低等方面的要求，常見的室內(nèi)定位技術(shù)有：紅外線[4]、超聲波[5]、藍(lán)牙[6]、WIFE[7]、ZigBee[8]、偽衛(wèi)星[9]、地磁[10]、慣導(dǎo)[11]、視覺[12]和UWB[13]等室內(nèi)定位技術(shù)。UWB定位技術(shù)[14]根據(jù)測(cè)得定位標(biāo)簽相對(duì)于兩個(gè)不同定位基站之間無線電信號(hào)傳播的時(shí)間，從而得出定位標(biāo)簽相對(duì)于定位基站的距離，通過多個(gè)距離進(jìn)行定位，由于UWB的信號(hào)體制能夠有效抑制多徑，因此可實(shí)現(xiàn)6-10cm的定位精度[15]，在室內(nèi)定位領(lǐng)域有較好的應(yīng)用前景。但是受室內(nèi)建筑布局、結(jié)構(gòu)材料、裝修等影響因素，UWB基站收到的測(cè)距信號(hào)未必是直達(dá)信號(hào)，這樣的信號(hào)參與定位必然會(huì)影響室內(nèi)定位的定位精度。應(yīng)用在UWB室內(nèi)定位中的定位方法主要有最小二乘算法[16]、迭代最小二乘算法[17]和卡爾曼濾波算法[18]，最小二乘和迭代最小二乘算法不能消除非直達(dá)信號(hào)對(duì)定位精度的影響，卡爾曼濾波算法需要首先建立復(fù)雜環(huán)境模型，當(dāng)環(huán)境發(fā)生變化時(shí)需要對(duì)模型進(jìn)行修改，實(shí)現(xiàn)較為復(fù)雜。

本文從UWB定位的定位原理出發(fā)，結(jié)合室內(nèi)導(dǎo)航定位的特點(diǎn)，提出一種基于偽距殘差的最小二乘加權(quán)算法，該算法基于偽距殘差通過自適應(yīng)調(diào)整最小二乘加權(quán)的權(quán)值，來降低非直達(dá)信號(hào)對(duì)最小二乘定位的影響，實(shí)驗(yàn)結(jié)果表明：該算法能夠有效提升室內(nèi)復(fù)雜環(huán)境下UWB定位的定位精度和定位穩(wěn)定性。

2 室內(nèi)UWB導(dǎo)航定位模型

UWB測(cè)距技術(shù)可分為兩類：單程測(cè)距和雙程測(cè)距，單程測(cè)距應(yīng)用于基站之間需要時(shí)鐘同步，基站構(gòu)建實(shí)現(xiàn)起來較為復(fù)雜，但是能夠支持更多的標(biāo)簽卡同時(shí)定位，該方式誤差更多來源于基站網(wǎng)絡(luò)的時(shí)鐘同步精度。雙程測(cè)距不需要基站進(jìn)行時(shí)鐘同步，采用計(jì)算收發(fā)時(shí)間的方式測(cè)定基站與標(biāo)簽卡之間的距離，該方式實(shí)現(xiàn)較為簡單，目前情況下實(shí)現(xiàn)的精度相比單程測(cè)距要高，但是能夠同時(shí)支持的定位標(biāo)簽卡有數(shù)量限制。下面簡要介紹一下UWB定位的定位原理。

通過測(cè)量從載體到三個(gè)或更多個(gè)UWB基站傳播時(shí)間的方法，可以獲得載體到基站的距離，分別以每個(gè)基站為中心、以到每個(gè)基站的距離為半徑繪制三個(gè)或多個(gè)圓，多個(gè)圓的交點(diǎn)即為載體的位置。如圖1所示。

圖1 基于TOA的UWB定位原理

載體與基站的距離關(guān)系可表述為

(1)

其中，(x，y，z)為載體位置坐標(biāo)，(xsi，ysi，zsi)為第i個(gè)基站的位置坐標(biāo)，c為電磁波傳播速度，ti為載體信號(hào)到達(dá)第i個(gè)基站的時(shí)間，ni為系統(tǒng)噪聲，N為基站數(shù)量，ri為載體到第ri個(gè)基站的距離。

將式(1)轉(zhuǎn)換成關(guān)于載體坐標(biāo)的線性模型，對(duì)式(1)兩邊分別求平方，進(jìn)行變換可以得到

(2)

令

(3)

T=(x2+y2+z2)

(4)

將式(3)和(4)代入到式(2)，進(jìn)一步展開可得

(5)

將N個(gè)基站的位置距離關(guān)系寫成矩陣可得

H=AX+e

(6)

其中，X=[x，y，z，T]T，e=[e1，e2，…，eN]。

(7)

(8)

3 最小二乘的定位算法

最小二乘算法[19，20]最早由最早是在1975年由高斯(Karl Gauss)在測(cè)量恒星軌道時(shí)提出，該算法通過最小化平方和求解最優(yōu)解，并使得這些求得的數(shù)據(jù)與實(shí)際數(shù)據(jù)之間誤差的平方和為最小，由于具有較好的普適性和穩(wěn)定性，且實(shí)現(xiàn)較為簡單，在信號(hào)處理中有著廣泛的應(yīng)用。

最小二乘算法是構(gòu)造使誤差平方和最小的函數(shù)，因此，構(gòu)造代價(jià)函數(shù)為

J=E(eTe)=(AX-H)T(AX-H)

(9)

進(jìn)一步展開可變?yōu)?/p>

J=XTATAX-2XTATH+HTH

(10)

使上述代價(jià)函數(shù)最小的解即為當(dāng)前載體的位置坐標(biāo)，即

(11)

對(duì)上式進(jìn)行求導(dǎo)，對(duì)應(yīng)極值即為載體位置坐標(biāo)，即

(12)

最后可得到方程的最優(yōu)解為

X=(ATA)-1AH

(13)

該解即為最小二乘算法得到的最優(yōu)解。

4 基于偽距殘差加權(quán)的最小二乘算法

當(dāng)UWB信號(hào)在室內(nèi)傳輸時(shí)，由于室內(nèi)環(huán)境的復(fù)雜性，容易受到反射、衍射和多徑等多種因素的影響，上節(jié)中介紹的最小二乘算法定位精度將嚴(yán)重降低，這是因?yàn)樽钚《怂惴ㄡ槍?duì)的是服從正態(tài)分布觀測(cè)樣本，當(dāng)觀測(cè)值有悖于正態(tài)分布假設(shè)、樣本遭受異常污染時(shí)，最小二乘估計(jì)不具有抗干擾性。為了改善最小二乘在費(fèi)直射情況下的定位精度，需要對(duì)算法進(jìn)行改進(jìn)，提升算法在個(gè)別基站信號(hào)非視距條件下的算法的穩(wěn)定性。

可通過對(duì)最小二乘算法的各個(gè)基站的數(shù)據(jù)進(jìn)行加權(quán)降低非直達(dá)信號(hào)對(duì)定位結(jié)果的影響。即將式(13)改寫為

X=(ATWA)-1AWTH

(14)

加權(quán)是為了降低非視距信號(hào)對(duì)最小二乘定位結(jié)果的影響，但是，環(huán)境是不斷變化的，傳統(tǒng)的固定加權(quán)或根據(jù)測(cè)距距離的加權(quán)無法消除非視距信號(hào)對(duì)定位的影響。當(dāng)某個(gè)基站的偽距測(cè)量值不是直達(dá)信號(hào)時(shí)，測(cè)量偽距變化和估算偽距變化之間的差值變大，因此，可用偽距變換率的殘差構(gòu)造隨殘差變化的權(quán)值矢量，自適應(yīng)降低非直達(dá)信號(hào)對(duì)最小二乘定位的影響。

偽距殘差的構(gòu)造方法可描述為：假設(shè)k時(shí)刻載體的位置坐標(biāo)為(xk，yk，zk)，N個(gè)基站的測(cè)量偽距分別為r1，k，…ri，k，…，rN，k，k+1時(shí)刻載體的位置坐標(biāo)為(xk+1，yk+1，zk+1)，N個(gè)基站的測(cè)量偽距分別為r1，k+1，…ri，k+1，…，rN，k+1。設(shè)

(15)

(16)

其中，Δxk=xk+1-xk，Δyk=yk+1-yk，Δzk=zk+1-zk。

(17)

其中，ρi，k=ri，k+1-ri，k為k到k+1時(shí)刻偽距的變化率。

建立偽距變化量的觀測(cè)方程

Q=Fv+ε

(18)

其中，ε為偽距變化和位置估算不一致引起的誤差。

定義估計(jì)的偽距殘差為

ε=Q-Fv

(19)

當(dāng)所有基站的偽距測(cè)量為直達(dá)徑時(shí)，ε應(yīng)為接近于0很小的值，當(dāng)收到的某個(gè)基站的信號(hào)為非直達(dá)徑時(shí)，ε對(duì)應(yīng)該基站的偽距殘差值變大，因此，可根據(jù)偽距殘差的變化構(gòu)造加權(quán)向量，約束最小二乘算法性能。

則偽距殘差的方差為

R=E[εεT]=(Q-Fv)(Q-Fv)T

(20)

令

(21)

構(gòu)造最小二乘加權(quán)的約束函數(shù)。

基于殘差加權(quán)的最小二乘法算法流程如圖2所示。

圖2 基于殘差的最小二乘算法流程

可描述為：

1) 獲取能收到節(jié)點(diǎn)的測(cè)距結(jié)果；

2) 判斷節(jié)點(diǎn)數(shù)量是否≥3，若≥3，進(jìn)行定位，否則該次數(shù)據(jù)無法實(shí)現(xiàn)定位；

3) 構(gòu)建觀測(cè)量F和Q，并計(jì)算偽距殘差協(xié)方差矩陣，并根據(jù)參加協(xié)方差矩陣構(gòu)建加權(quán)矩陣W；

4) 構(gòu)建觀測(cè)矩陣A和H，根據(jù)式(14)計(jì)算定位結(jié)果，并輸出定位結(jié)果。

5 室內(nèi)定位性能試驗(yàn)

5.1 實(shí)驗(yàn)環(huán)境介紹

在有走廊、門廳和實(shí)驗(yàn)室的樓內(nèi)布設(shè)UWB接收基站，具體位置如圖4紅色點(diǎn)位所示。由全站儀標(biāo)定基站的精確位置，標(biāo)定精度在毫米量級(jí)。區(qū)域內(nèi)的每個(gè)位置點(diǎn)至少有三個(gè)以上的基站能夠?qū)崿F(xiàn)視距接收。行走的真實(shí)軌跡事先由全站儀進(jìn)行標(biāo)定，圖中黑線基站無遮擋條件下定位精度的驗(yàn)證軌跡，紅線為部分基站有遮擋條件下的驗(yàn)證軌跡。

圖3 實(shí)驗(yàn)環(huán)境及設(shè)備安裝位置示意圖

圖3給出實(shí)驗(yàn)環(huán)境及設(shè)備安裝位置布局示意圖，圖中紅色圓點(diǎn)表示安裝的UWB基站。實(shí)驗(yàn)采用基于雙向測(cè)距的UWB定位設(shè)備，該測(cè)距方式不需要考慮基站和標(biāo)簽之間的時(shí)間同步問題，實(shí)現(xiàn)較為簡單，有助于降低硬件成本。為了消除系統(tǒng)誤差，在用全站儀標(biāo)定好基站位置后，再標(biāo)定一些點(diǎn)位，消除基站測(cè)距誤差的影響。每個(gè)點(diǎn)位采集100次測(cè)距結(jié)果，采用中值濾波計(jì)算距離觀測(cè)值的絕對(duì)誤差，通過不同點(diǎn)的最小二乘擬合得到每個(gè)基站的測(cè)距補(bǔ)償值。

5.2 室內(nèi)無遮擋簡單環(huán)境的導(dǎo)航定位實(shí)驗(yàn)

首先在一個(gè)室內(nèi)尺度較小的方形無遮擋區(qū)域架設(shè)4顆UWB接收基站，在定位中僅采用這四個(gè)基站的測(cè)距結(jié)果，在3m×4m的房間內(nèi)沿圖3中黑線運(yùn)動(dòng)，在無遮擋條件下UWB定位如圖4所示。

圖4 室內(nèi)無遮擋方形區(qū)域定位圖

從上圖可以看出：在無遮擋所有基站都是收到直達(dá)信號(hào)的條件下，最小二乘(LS)，非線性最小二乘(NLS)[21]和基于殘差加權(quán)最小二乘算法(RWLS)都具有較好的定位性能，定位性能并無明顯差別，定位精度均優(yōu)于10cm。

為了對(duì)比分析以上三種算法的定位性能，利用位置誤差的絕對(duì)值，誤差的均方根等進(jìn)行分析。首先分析在參與定位所有基站無遮擋環(huán)境下導(dǎo)航定位精度，如表1所示。

表1 定位誤差的實(shí)驗(yàn)結(jié)果

從上表中可以看出：在無遮擋條件下，三種算法定位精度相差不大，均優(yōu)于10cm。

5.3 室內(nèi)有遮擋復(fù)雜環(huán)境下的導(dǎo)航定位實(shí)驗(yàn)

實(shí)驗(yàn)2標(biāo)簽的運(yùn)行軌跡如圖3中紅線所示，由于實(shí)際運(yùn)行軌跡是由事先標(biāo)校的離散定位點(diǎn)連線形成的線段，為了分析本文提出算法的定位精度，將線段分為幾段直線并進(jìn)行線性插值，并讓總點(diǎn)數(shù)等于該運(yùn)行軌跡內(nèi)定位解算的點(diǎn)數(shù)，通過定位結(jié)果與最近的插值點(diǎn)比較確定算法的定位精度。如下圖所示：

圖5 室內(nèi)收到多個(gè)非直達(dá)信號(hào)的定位圖

從圖5中可以看出：最小二乘、非線性最小二乘算法在存在多個(gè)非直達(dá)信號(hào)的條件下，定位結(jié)果存在一定的發(fā)散情況，定位精度變差而本文提出的基于偽距殘差加權(quán)的最小二乘算法能夠有效降低非直達(dá)信號(hào)對(duì)定位精度的影響，在室內(nèi)復(fù)雜環(huán)境下具有更好的收斂性。

進(jìn)一步分析在有部分基站收不到直達(dá)信號(hào)的條件下的定位精度，如表2所示。

表2 定位誤差的實(shí)驗(yàn)結(jié)果

從上表中可以看出：在有遮擋條件下，最小二乘算法的均方誤差在59cm左右，非線性最小二乘算法的定位精度略優(yōu)于最小二乘，但是兩者定位精度基本在一個(gè)量級(jí)，本文提出的基于偽距殘差加權(quán)的最小二乘算法的定位精度為17.8cm，明顯優(yōu)于前兩種算法的定位精度。

6 結(jié)束語

針對(duì)室內(nèi)復(fù)雜環(huán)境下的導(dǎo)航定位，在分析最小乘算法的基礎(chǔ)上，結(jié)合UWB定位的特點(diǎn)，提出一種基于偽距殘差加權(quán)的最小二乘室內(nèi)導(dǎo)航定位算法，在有足夠參與定位測(cè)距值的條件下，該算法能夠有效消除非直達(dá)信號(hào)對(duì)UWB定位精度的影響，提升了算法的魯棒性。實(shí)驗(yàn)結(jié)果表明：該算法在室內(nèi)復(fù)雜環(huán)境下具有較好的定位精度，不存在非直達(dá)信號(hào)的條件下定位精度優(yōu)于10cm，存在多個(gè)非直達(dá)信號(hào)的情況下，定位精度優(yōu)于20cm，能夠滿足室內(nèi)高精度導(dǎo)航定位的需求。