一種基于UWB TDOA定位模式的迭代最小二乘算法

何成文 袁運(yùn)斌 潭冰峰

1 中國(guó)科學(xué)院精密測(cè)量科學(xué)與技術(shù)創(chuàng)新研究院大地測(cè)量與地球動(dòng)力學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，武漢市徐東大街340號(hào)，430077 2 中國(guó)科學(xué)院大學(xué)，北京市玉泉路19號(hào)甲，100049

對(duì)TDOA定位模式下的LOS信號(hào)場(chǎng)景的算法研究很多[1-14]，包括兩步加權(quán)最小二乘(two-step weighted least squares，TSWLS)算法[1]、線性修正最小二乘(linear-correction least-squares，LCLS)算法[2]、約束加權(quán)最小二乘(constrained weighted least squares，CWLS)算法[3-4]、分離約束加權(quán)最小二乘(separated CWLS，SCWLS)算法[5]和迭代約束加權(quán)最小二乘(iterative CWLS，ICWLS)算法[6]等。但這些方法難以同時(shí)兼顧精度和抗噪性能。為此，本文提出一種簡(jiǎn)單有效的非約束迭代優(yōu)化算法，并用實(shí)驗(yàn)驗(yàn)證其效果。

1 算法描述

結(jié)合室內(nèi)定位的特點(diǎn)，考慮采用N個(gè)UWB基準(zhǔn)站去定位UWB標(biāo)簽的2維坐標(biāo)。假設(shè)si=[xi,yi]T為已知UWB基站坐標(biāo)，u0=[x,y]T為待求標(biāo)簽位置，通常選擇第1個(gè)基站作為參考站，則常規(guī)TDOA-LOS定位方程為：

(1)

式中，di,1為標(biāo)簽到第i個(gè)基站和到第1個(gè)基站之間的距離差，ηi為均值為0的高斯白噪聲。對(duì)式(1)移項(xiàng)后進(jìn)行平方展開(kāi)，忽略高斯白噪聲的影響，可將其轉(zhuǎn)化為線性形式Gu1=h，其中，

(2)

u1=[x,y,R]T

(3)

(4)

由于R與u0和s1之間存在如下關(guān)系：

R2=(u0-s1)T(u0-s1)

(5)

因此，傳統(tǒng)CWLS算法[3-5]的表達(dá)形式可寫(xiě)為：

min(h-Gu1)TW(h-Gu1)

s.t.R2=(u0-s1)T(u0-s1)

(6)

式中，W為加權(quán)矩陣。

CWLS算法已有較為快速的解法，但在定位精度方面仍存在較大的提升空間。為進(jìn)一步提高CWLS算法的定位精度和運(yùn)算速度，并解決約束方程的非凸問(wèn)題，Qu等[6]提出迭代約束加權(quán)最小二乘(ICWLS)算法。該算法通過(guò)新的等式變換，將具有非凸特性的CWLS算法表達(dá)式轉(zhuǎn)化成具有凸性的新表達(dá)式。盡管仿真實(shí)驗(yàn)證實(shí)了該算法的有效性，但在大噪聲環(huán)境下卻存在定位發(fā)散的缺點(diǎn)。

為解決定位發(fā)散的問(wèn)題，本文提出一種簡(jiǎn)單且適用于大噪聲環(huán)境下的TDOA定位算法。首先假設(shè)基準(zhǔn)站坐標(biāo)位于笛卡爾坐標(biāo)系原點(diǎn)(即便真實(shí)場(chǎng)景中基準(zhǔn)站坐標(biāo)不在原點(diǎn)，也可通過(guò)坐標(biāo)變換來(lái)實(shí)現(xiàn))，因此，式(1)可被重新寫(xiě)為：

(7)

通常在室內(nèi)環(huán)境中，觀測(cè)值不會(huì)很大且白噪聲較小，而白噪聲的分析常用于計(jì)算加權(quán)矩陣，由于本文算法不屬于加權(quán)類算法，因此可忽略式(7)中噪聲項(xiàng)的影響。利用勾股定理，可將式(7)轉(zhuǎn)化為：

(di,1+R)2=(x-xi)2+(y-yi)2

(8)

展開(kāi)移項(xiàng)后可得到：

(9)

令

(10)

(11)

(12)

則式(9)可寫(xiě)為如下線性形式：

2AR=C-2BX*

(13)

(14)

(15)

當(dāng)式(15)多次迭代后達(dá)到式(16)的迭代停止準(zhǔn)則時(shí)，即可停止迭代，從而輸出最終定位結(jié)果：

(16)

式中，ε為誤差閾值，數(shù)值一般比較小。

(17)

通常初值可以由最小二乘算法得到，而最小二乘解是無(wú)偏的，因此式(17)中每步迭代解的期望都等于最小二乘解，是無(wú)偏估計(jì)解。

2 仿真實(shí)驗(yàn)

通常情況下，室內(nèi)環(huán)境相對(duì)復(fù)雜，視距條件下的基站總數(shù)相對(duì)有限。為保證UWB定位算法的可行性，僅采用4個(gè)UWB基站定位標(biāo)簽的位置，坐標(biāo)分別為A1(0,0)、A2(10,0)、A3(10,10)和A4(0,10)，標(biāo)簽接收來(lái)自各個(gè)基站的信號(hào)中含有均值為0、方差為σ2的高斯白噪聲ηi。由于本文的主要目的是評(píng)價(jià)算法在大噪聲環(huán)境下的性能，故將噪聲方差分別設(shè)置為0.1、0.25、0.5、0.75和1以進(jìn)行綜合評(píng)價(jià)，將每個(gè)噪聲方差下的蒙特卡洛仿真實(shí)驗(yàn)次數(shù)和迭代閾值ε分別設(shè)置為2 000和0.000 01。

基于Qu等[6]的仿真實(shí)驗(yàn)結(jié)論可知，ICWLS算法的定位精度優(yōu)于TSWLS算法、SDR算法、CWLS算法和迭代似然估計(jì)法，因此本文僅選擇該算法與本文算法進(jìn)行對(duì)比。在定位初值方面，2種算法均采用最小二乘解作為迭代初值；在精度評(píng)價(jià)指標(biāo)方面，本文采用均方根誤差(RMSE)來(lái)評(píng)估2種算法的性能，計(jì)算公式為：

2.1 實(shí)驗(yàn)1

為充分對(duì)比2種算法在邊緣點(diǎn)和中心點(diǎn)處的定位精度，在LOS信號(hào)環(huán)境下選擇近基站點(diǎn)L1(1, 1)和近中心點(diǎn)L2(5, 6)進(jìn)行測(cè)試，2種算法在測(cè)試點(diǎn)處的定位精度見(jiàn)圖1。由圖可知，當(dāng)噪聲方差σ2≤0.25時(shí)，ICWLS算法的精度性能與本文算法大致相同。然而，當(dāng)σ2>0.25時(shí)，ICWLS算法的性能誤差開(kāi)始急劇上升，而本文算法則緩慢上升，且誤差低于ICWLS算法。因此，總體而言，本文算法在定位精度和抗噪聲性能方面表現(xiàn)更好。

圖1 2種算法在不同點(diǎn)處的定位精度

2.2 實(shí)驗(yàn)2

為進(jìn)一步測(cè)試2種算法的定位精度，在4個(gè)基站圍成的矩形區(qū)域中隨機(jī)生成標(biāo)簽的位置，實(shí)驗(yàn)結(jié)果見(jiàn)圖2。可以看出，本文算法顯著優(yōu)于ICWLS算法，在數(shù)值方面，本文算法的相應(yīng)誤差大約只有ICWLS算法的一半。此外，實(shí)驗(yàn)發(fā)現(xiàn)，本文算法迭代到收斂的平均迭代次數(shù)為15次。

圖2 2種算法在隨機(jī)點(diǎn)環(huán)境下的定位精度

2.3 實(shí)驗(yàn)3

基于實(shí)驗(yàn)1和實(shí)驗(yàn)2的仿真結(jié)果，通過(guò)定位散點(diǎn)圖來(lái)探究ICWLS算法定位性能較差的原因。將標(biāo)簽位置分別固定在L1(1,1)和L2(5,6)處，并將其噪聲方差分別設(shè)為低噪聲σ2=0.1和高噪聲σ2=0.8，結(jié)果見(jiàn)圖3。從圖中可以看出，在低噪聲條件下，2種算法的定位坐標(biāo)基本分布在同一區(qū)域內(nèi)，變化不大；在高噪聲條件下，本文算法的定位結(jié)果接近真值，但I(xiàn)CWLS算法的定位結(jié)果分布在2個(gè)不同的區(qū)域，產(chǎn)生嚴(yán)重的發(fā)散。結(jié)合2種實(shí)驗(yàn)場(chǎng)景的定位結(jié)果認(rèn)為，ICWLS算法存在一定的發(fā)散現(xiàn)象，這種發(fā)散現(xiàn)象在低噪聲環(huán)境下不明顯，在高噪聲環(huán)境下相對(duì)明顯，而本文提出的定位算法具有良好的收斂性和抗噪聲性能。

圖3 2種算法在不同點(diǎn)處的定位坐標(biāo)

由此認(rèn)為：1)本文提出的算法優(yōu)于ICWLS算法；2)無(wú)論噪聲方差是多少，本文算法的定位結(jié)果都與真實(shí)位置更接近，說(shuō)明本文算法具有良好的抗噪聲性能；3)ICWLS算法存在定位發(fā)散的缺點(diǎn)，這可能是其定位性能不如本文算法的根本原因。

3 結(jié) 語(yǔ)

針對(duì)傳統(tǒng)TDOA算法因抗噪性能弱而導(dǎo)致定位發(fā)散的缺點(diǎn)，本文提出一種簡(jiǎn)單的TDOA定位算法。該算法首先將TDOA定位方程轉(zhuǎn)化為L(zhǎng)S形式來(lái)得到初始解，然后通過(guò)迭代思想得到收斂坐標(biāo)。本文算法原理簡(jiǎn)單、抗噪聲性能好、易于實(shí)現(xiàn)，可推廣到工業(yè)領(lǐng)域，且仿真結(jié)果也驗(yàn)證了本文算法具有可行性和有效性，定位精度優(yōu)于ICWLS算法。