徐曉蘇，劉興華，楊 博，王 帥

（1.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

UWB（Ultra-wideband）技術(shù)近年來廣泛應(yīng)用于室內(nèi)定位，它具有頻帶寬 、空間和時(shí)間分辨率高、穿透性強(qiáng)、保密性好、定位精度較其他定位方法高、基本不干擾其他信號(hào)的傳播等優(yōu)點(diǎn)。UWB定位是一種基于無線電的定位技術(shù)。UWB的定位原理可分為基于達(dá)到時(shí)間的方法（Time of arrival,TOA）、基于到達(dá)時(shí)間差的方法（Time difference of arrival,TDOA）、基于到達(dá)角的方法（Angle of arrival,AOA）和基于接收信號(hào)強(qiáng)度的方法（Received signal strength indication,RSSI）等[1]。基于飛行時(shí)間的方法（Time of flight,TOF）最能發(fā)揮UWB的優(yōu)點(diǎn)，因此當(dāng)前的方法大多是基于TOA或TDOA。

當(dāng)前UWB定位使用較多的解析法定位技術(shù)方案是基于TOA的最小二乘法（Least Square,LS）、Taylor級(jí)數(shù)展開法，基于TDOA的Chan算法，三角形質(zhì)心法等[2-3]。在二維室內(nèi)空間環(huán)境下這些算法能夠由UWB測距信息快速解算得到目標(biāo)的定位初值，是其他方法的基礎(chǔ)。然而，在三維室內(nèi)環(huán)境中，上述方法在高度方向上提供的解算值具有很大的不確定性，往往難以得到較為準(zhǔn)確的高度坐標(biāo)值，甚至得到的解算值是錯(cuò)誤的，這限制了上述方法的應(yīng)用。此外，一些組合方法，如UWB與IMU（慣性測量單元，Inertial Measurement Unit）組合方法[4]或UWB與視覺SLAM（即時(shí)定位與地圖構(gòu)建，Simultaneous Localization and Mapping）組合方法[5]等，需要應(yīng)用傳統(tǒng)解析法得到的初值，這要求一種能夠在具有高度限制環(huán)境下給出正確初值的解析定位方法。文獻(xiàn)[6]對(duì)三維定位問題作了研究，針對(duì)Chan算法提出了基于Tikhonov正則化的魯棒嶺估計(jì)方法，但文獻(xiàn)沒有分析導(dǎo)致方程組性態(tài)差的原因，且文獻(xiàn)方法在選擇正則化參數(shù)時(shí)需要結(jié)合L-曲線法多次迭代確定，增加了應(yīng)用難度。由于嶺估計(jì)引入了偏差，文獻(xiàn)[6]方法在基站不存在布置限制問題時(shí)，將導(dǎo)致解算誤差比傳統(tǒng)解析法大。

本文通過對(duì)UWB室內(nèi)定位常用解析法的研究，分析了傳統(tǒng)解析法在高度方向定位精度較差的原因，并針對(duì)傳統(tǒng)解析法在三維室內(nèi)定位環(huán)境下由于基站布置具有限制而無法給出正確解算值的不足，提出一種二次解析方法（Reanalysis Method,RM）以改善高度方向的定位效果。該方法先根據(jù)傳統(tǒng)解析法求解出水平方向定位值的解析解，然后在二次解析過程中利用修正偏差的Tikhonov正則化方法（Tikhonov Regularization,TR），求解得到較為精確的高度方向的定位值。

1 針對(duì)傳統(tǒng)解析法的分析

1.1 傳統(tǒng)定位方程組的機(jī)理分析

定位解算時(shí)采用解析法求解UWB定位目標(biāo)值，實(shí)質(zhì)上是利用定位方程組直接求解目標(biāo)點(diǎn)坐標(biāo)。本文首先介紹傳統(tǒng)定位方程組的分析方法，然后利用該方法分析基于傳統(tǒng)解析法UWB定位精度不高的原因。傳統(tǒng)定位方程組一般為一組線性方程組，通過將UWB測距信息代入方程組，求解出目標(biāo)點(diǎn)的位置值。假設(shè)定位方程組有如下形式：

其中，A為系數(shù)矩陣，b為常值向量，θ為待求解的位置值。由于UWB測距值帶有誤差，求解上述方程組將不可避免地產(chǎn)生求解誤差。根據(jù)數(shù)值分析中方程組誤差分析的知識(shí)，在系數(shù)矩陣A與常值向量b分別有誤差δA和δb的情況下帶來的解θ的誤差范圍為

對(duì)于2條件數(shù)有：

其中，λmax(·)和λmin(·)分別表示矩陣的最大和最小特征值。若系數(shù)矩陣對(duì)稱正定，則有：

1.2 解析法常用的定位方法與定位方程組

將常用的UWB定位算法對(duì)應(yīng)的定位方程組表述在三維定位情況下，可得如式(6)～(9)形式。其中基于TOA的n(n≥4)基站最小二乘法定位方程組為

基于TOA的泰勒展開定位方程組為[7]：

基于TDOA的Chan算法第一次極大似然估計(jì)使用的定位方程組為

式中，ψ為誤差向量，ri1(i= 1,,n)為定位目標(biāo)點(diǎn)到第i(i= 2,,n)個(gè)基站的距離與到第 1個(gè)基站的距離的差，參數(shù)

Chan算法采用兩次極大似然估計(jì)減小測距差誤差對(duì)目標(biāo)定位的影響。

三角形全質(zhì)心算法是針對(duì)二維定位情況提出的，在二維定位情況下具有較好的魯棒性。該算法形式上滿足式(1)，因此可以采取相同的方法分析，故本文也對(duì)其進(jìn)行分析。三角形全質(zhì)心算法的定位方程組為

式中，R2=x2+y2。

由此可見，前面給出的幾組定位方程，均可寫成式(1)的形式，可以按照1.1節(jié)的方法討論基站測距誤差對(duì)解的影響。通常情況下，認(rèn)為UWB基站的位置是比較精確的，而基站的測距誤差或測距差誤差是影響解的主要因素。所以可以認(rèn)為，解的誤差主要是由誤差δb引入的。以下的討論也是基于此觀點(diǎn)。

1.3 室內(nèi)UWB定位高度方向精度不佳的原因分析

在UWB定位的實(shí)際環(huán)境中，通常在布置基站的時(shí)候，對(duì)平面方向（x、y方向）的限制比較小，可以布置在比較合理的范圍，而對(duì)高度方向（z方向）的限制比較大。這是因?yàn)槭覂?nèi)樓層的高度有限，一般的樓層大多在3～5 m的范圍內(nèi)。以下討論說明，這是導(dǎo)致UWB定位高度方向上精度不佳的主要原因。

對(duì)于任意目標(biāo)節(jié)點(diǎn)有如下關(guān)系式：

在式(10)中，分別利用前n-1式減去第n式，當(dāng)0時(shí)可得：

為方便得出其中的規(guī)律，假設(shè)各基站的測距誤差均相等，且到定位目標(biāo)點(diǎn)的距離大致相當(dāng)，可以得到：

式(13)描述的距離比例因子還可看作是各算法定位方程組的系數(shù)矩陣中的元素的數(shù)量級(jí)之間的關(guān)系，最明顯的是式(6)和式(8)。距離比例因子越大，說明系數(shù)矩陣中的元素?cái)?shù)量級(jí)差別越大，矩陣越有可能是病態(tài)的，因測量誤差帶來的解的不確定性越大。

2 解析法在高度方向定位精度不佳的解決方法

前面指出，解析法在高度方向上定位精度不佳的主要原因是基站在方向的距離差相比于基站在方向（高度方向）上的距離差較大。因此對(duì)于采用解析法提供初值的定位技術(shù)方案中，可以采取兩種思路解決上述問題：其一是盡量將基站布置在成球?qū)ΨQ位置或近似球?qū)ΨQ位置，這樣將使得各方向上的誤差最小且一致[8]；其二是采取別的辦法減小解算誤差。通常 UWB室內(nèi)定位無法將各基站布置成球?qū)ΨQ位置，因此本文將根據(jù)第二種思路求取解決辦法。

2.1 影響定位精度主要的分量分析

以下首先針對(duì)魯棒性較好適用范圍廣的最小二乘法進(jìn)行討論，然后提出一種提高解析法解算時(shí)高度方向的位置準(zhǔn)確度的方法。首先分別對(duì)UWB室內(nèi)定位的兩種情況做討論。二維定位情況（不包含高度方向）下，根據(jù)式(1)和式(3)最小二乘法的定位方程組系數(shù)矩陣的2條件數(shù)為

較大，其中A3是處理三維定位問題時(shí)對(duì)應(yīng)的系數(shù)矩陣。為簡單說明問題考慮以下系數(shù)矩陣：

其中：an-1,n-1為考慮三維定位問題時(shí)，因某個(gè)基站高度值變化使得系數(shù)矩陣維數(shù)增加而引入的參數(shù)；式(16)中的參數(shù)an-1,n-1= 2(zn-1-zn)；A2為對(duì)角元不為0，其余元素均為 0的矩陣。只有四個(gè)基站時(shí)，矩陣A2=diag{2(x2-xn),2(y3-yn)}為對(duì)角陣。由于水平方向基站布置限制少，因此可認(rèn)為 (x2-xn) ≈ (y3-yn)，于是

其中，max{?}表示集合中元素的最大值。

式(17)和式(18)表明，方程組的病態(tài)是由于高度方向的引入引起的。上述也可說明高度方向的解算誤差δz是引起解向量不確定性增大的主要因素，即δz在中起主要作用。因此可以考慮保留第一次解析解算初值中的前兩個(gè)變量(x,y)的解算值，而對(duì)高度方向的變量解算值再進(jìn)行二次解析解算。

2.2 修正偏差的Tikhonov正則化方法

根據(jù)Tikhonov正則化理論[9-10]，其估計(jì)準(zhǔn)則為

其中，μ為正則化參數(shù)，為θ的估計(jì)值，R為正則化矩陣，f(θ)為穩(wěn)定泛函，表示矩陣p-范數(shù)。當(dāng)R=I時(shí)，該估計(jì)方法被稱為嶺估計(jì)。求解式(1)描述的不適定問題的嶺估計(jì)公式為

但式(20)的正則化參數(shù)μ難以確定，且所得的估計(jì)結(jié)果是有偏的。以下根據(jù)Tikhonov正則化的思想，提出一種修正偏差的Tikhonov正則化方法，并介紹確定修正偏差Tikhonov正則化矩陣的簡單方法。

根據(jù)矩陣的冪級(jí)數(shù)展開有：

其中，F(xiàn)為方陣。若矩陣F的譜半徑ρ(F)＜1（或，則 Neumann級(jí)數(shù)收斂于則有：

考慮對(duì)稱正定矩陣ATA（A列滿秩）的Cholesky分解

則有：

上述推導(dǎo)過程中，要求譜半徑ρ((ST)-1R(S)-1)＜1，且要求 ((ST)-1R(S)-1)2?0。由式(24)可以得到：

根據(jù)式(19)描述的準(zhǔn)則，可得到估計(jì)值的計(jì)算式：

式(26)說明，表征等式右邊第二項(xiàng)的是與R、ATb相關(guān)的項(xiàng)。實(shí)際上ATA的逆矩陣包含較大的特征值，直接用式(26)計(jì)算會(huì)帶有較大計(jì)算誤差。將式(26)中的(ATA)-1進(jìn)行修正，得到：

式(27)的實(shí)質(zhì)是通過對(duì) (ATA)-1的進(jìn)一步近似，來修正引入正則化矩陣R帶來的偏差。當(dāng)式(27)的正則化矩陣R滿足式(19)時(shí)，不論高度方向是否存在限制，理論上均可取得最優(yōu)值。式(27)中R的取值一般要滿足前面矩陣譜半徑的要求，在實(shí)際中可取R=diag(0,0,γ)，其中的參數(shù)γ與ATA的最小特征值或前文所說的最大距離比例因子相近較好，前者能夠保證估計(jì)量整體能夠近似無偏且能夠較大程度地降低估計(jì)誤差，后者能夠盡量降低z方向的最大估計(jì)誤差。同時(shí)，基于最小二乘法的無偏性，可利用最小二乘法的估計(jì)均值，來近似確定Tikhonov正則化方法的整體偏差，以取得Tikhonov正則化的大范圍近似無偏解，有：

根據(jù)求解的精度要求，可以重復(fù)式(28)和式(29)進(jìn)一步優(yōu)化估計(jì)值，即

上述求解過程首先需要進(jìn)行一次 LS估計(jì)，其次需要根據(jù)式(27)再次對(duì)目標(biāo)點(diǎn)進(jìn)行估計(jì)，然后將兩次估計(jì)按式(28)相減得到偏差，最后修正估計(jì)值的偏差。

3 仿真與實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證本文觀點(diǎn)以及驗(yàn)證文中提出的二次解析法，本文通過 MATLAB仿真與樣機(jī)實(shí)驗(yàn)將本文提出的方法與傳統(tǒng)的Chan算法和最小二乘法進(jìn)行對(duì)比。由于Taylor展開已難以收斂，故未與其進(jìn)行對(duì)比。

3.1 仿真驗(yàn)證

仿真條件設(shè)置如下：UWB基站的距離測距誤差為 0.1 m，設(shè)置了 5個(gè) UWB基站，其位置分別為(0,0,0)、(6,0,0)、(0,5,0)、(3.5,3,0)、(3,2.5,0.5)。

然后對(duì)1350個(gè)獨(dú)立空間點(diǎn)各進(jìn)行一次仿真定位。1）距離比例因子與矩陣條件數(shù)Z方向誤差的影響

圖1是最小二乘法的仿真實(shí)驗(yàn)結(jié)果。圖1的實(shí)驗(yàn)中高度不為0的基站其高度依次為2.5 m、1.25 m、0.5 m。在此實(shí)驗(yàn)中，令某個(gè)基站的高度產(chǎn)生變化以改變距離比例因子和系數(shù)矩陣條件數(shù)。從圖1可以看出，距離比例因子越大，系數(shù)矩陣的條件數(shù)也越大，高度方向（z方向）的誤差也越大。同時(shí)可以看出，高度方向的誤差是影響室內(nèi)三維定位精度的關(guān)鍵。值得一提的是，相同基站布局下不同算法的系數(shù)矩陣條件數(shù)不同，且定位誤差對(duì)測距誤差的敏感程度也不同。由于Taylor展開法與三角形質(zhì)心法的系數(shù)矩陣與最小二乘法的類似，也僅與基站的坐標(biāo)相關(guān)，所以這兩種算法的結(jié)果可以參考最小二乘法。雖然 Chan算法第一次極大似然估計(jì)的系數(shù)矩陣不僅與基站的位置相關(guān)還與移動(dòng)節(jié)點(diǎn)的位置相關(guān)，但其定位誤差與系數(shù)矩陣條件數(shù)的關(guān)系依舊滿足最小二乘法的結(jié)論，限于篇幅沒有將結(jié)果放在文中。

圖1 最小二乘法系數(shù)矩陣與定位誤差的關(guān)系Fig.1 Relation between the least square coefficient matrix and the positioning error

2）修正偏差的Tikhonov正則化法與最小二乘法、Chan算法對(duì)比

圖2 幾種算法誤差分量對(duì)比Fig.2 Comparison on error components of the three algorithms

圖3 定位誤差效果對(duì)比Fig.3 Comparison on positioning error effects

從圖2、圖3可以看出，影響Chan算法與最小二乘法三維定位的主要因素是高度方向的誤差，與圖1實(shí)驗(yàn)所得結(jié)論一致。雖然基于二次解析的修正偏差的Tikhonov正則化法（TR）的主要誤差也是高度方向的誤差，但是相比于前面兩種算法，該TR方法能夠有效利用目標(biāo)點(diǎn)的一次定位信息，降低高度方向的誤差。TR方法總誤差比最小二乘法減小49.6%，比Chan算法減小43.9%，均超過40%。從圖2和圖3可以看出Chan算法部分定位誤差比最小二乘法定位誤差要大，這是因?yàn)?Chan算法易受基站分布與測距誤差影響，且在室內(nèi)三維定位的環(huán)境下，高度方向的誤差急劇增大，使Chan算法其他兩個(gè)方向的估計(jì)誤差增大。

上述仿真結(jié)果說明，具有嚴(yán)重高度限制的 UWB室內(nèi)三維定位的定位精度主要取決于高度方向的定位精度。在室內(nèi)環(huán)境中應(yīng)盡量增大基站的高度差，盡量減小距離比例因子以減弱方程組的病態(tài)。

3.2 實(shí)際樣機(jī)實(shí)驗(yàn)

1）樣機(jī)實(shí)驗(yàn)條件與實(shí)驗(yàn)環(huán)境

實(shí)驗(yàn)采用Decawave公司的DWM1000作為定位模塊，實(shí)驗(yàn)前先對(duì)定位模塊的測距值進(jìn)行標(biāo)定，定位實(shí)驗(yàn)時(shí)對(duì)目標(biāo)點(diǎn)（移動(dòng)節(jié)點(diǎn)）進(jìn)行獨(dú)立測量，然后利用測量數(shù)據(jù)對(duì)目標(biāo)點(diǎn)進(jìn)行定位解算。實(shí)驗(yàn)場所為環(huán)境復(fù)雜的實(shí)驗(yàn)室，實(shí)驗(yàn)采用激光測距儀測量移動(dòng)節(jié)點(diǎn)的真實(shí)起始位置。本實(shí)驗(yàn)環(huán)境中使用的激光測距儀其測距原理基于光的粒子性質(zhì)，其測量精度優(yōu)于±5 mm。實(shí)驗(yàn)中設(shè)置5個(gè)UWB基站，其位置（單位為m）分別為(0,0,0)、(5.035,0.640,0)、(1.942,1.881,0)、(0.010,3.951,0)、(1.745,1.989,0.5)，然后對(duì)搭載移動(dòng)節(jié)點(diǎn)的無人車在多個(gè)位置點(diǎn)進(jìn)行定位，其中無人車做勻速運(yùn)動(dòng)。

圖4為搭載移動(dòng)節(jié)點(diǎn)的無人車實(shí)物照片。

本實(shí)驗(yàn)采用研創(chuàng)物聯(lián)公司的UWB Mini3sPlus開發(fā)板對(duì)目標(biāo)點(diǎn)進(jìn)行測距。研創(chuàng)物聯(lián)公司的 UWB Mini3sPlus開發(fā)板以Decawave公司的DWM1000作為定位模塊。定位模塊與激光測距儀的性能指標(biāo)如表1所示。

實(shí)驗(yàn)環(huán)境以及基站布置的位置示意圖如圖5所示，其中基站BS3的高度與其他四個(gè)基站的高度不同，而其他四個(gè)基站的高度均相同。

實(shí)驗(yàn)時(shí)定位點(diǎn)所在的軌跡如圖6所示。

表1 定位模塊與激光測距儀的性能指標(biāo)Tab.1 Performance indexes of positioning module and laser rangefinder

圖4 搭載移動(dòng)節(jié)點(diǎn)的無人車Fig.4 Unmanned vehicle with mobile node

圖5 實(shí)驗(yàn)環(huán)境以及基站布置示意圖Fig.5 Schematic of experimental environment and base station layout

圖6 定位點(diǎn)所在的軌跡Fig.6 Trajectory with positioning points

2）樣機(jī)實(shí)驗(yàn)分析

樣機(jī)實(shí)驗(yàn)的結(jié)果如圖7和圖8所示。從圖7的實(shí)驗(yàn)結(jié)果可以看出，在基站高度方向布置受到限制的情況下，影響定位誤差的主要因素是高度方向的定位誤差，與文中分析和仿真實(shí)驗(yàn)結(jié)果一致。同時(shí)可以發(fā)現(xiàn)，Chan算法在前段時(shí)間出現(xiàn)誤差近似平滑的現(xiàn)象。這主要是因?yàn)?Chan算法解算時(shí)其解算過程值出現(xiàn)了平方值為負(fù)數(shù)的情況而被強(qiáng)制轉(zhuǎn)為零。從圖8中還可發(fā)現(xiàn)，用 Chan算法解算時(shí)，中間段部分位置的解算誤差突然變大。導(dǎo)致這一現(xiàn)象的原因是目標(biāo)點(diǎn)處在這些位置時(shí)（圖6中黑色圓圈的位置），目標(biāo)點(diǎn)到部分基站的距離近似相等，使得用 Chan算法解算時(shí)相應(yīng)的逆矩陣接近奇異。修正偏差的Tikhonov正則化法則不會(huì)出現(xiàn)Chan算法的情況。

綜合圖7和圖8可以得出，本文提出的修正偏差的Tikhonov正則化法能夠降低定位方程組的病態(tài)，能夠同時(shí)降低最大誤差，高度方向的定位誤差，從而有效降低三維定位的誤差。本文提出的修正偏差的Tikhonov正則化法相比于最小二乘法能在高度方向上提高53.0%，總體定位效果提高52.1%，相比于Chan算法能在高度方向上提高 41.1%，總體定位效果提高41.6%，均超過 40%。三種算法的定位均方根誤差（Root-mean-square error,RMSE）如表2所示。

表2 三種算法均方根誤差Tab.2 Root-mean-square errors of the three algorithms

圖7 誤差分量對(duì)比Fig.7 Comparison on error components

圖8 定位誤差效果對(duì)比Fig.8 Comparison on positioning error effects

4 結(jié) 論

在室內(nèi)環(huán)境中利用UWB進(jìn)行三維定位易受基站在高度方向布置限制的影響，使得該方向的定位誤差較大。本文提出一種二次解析方法以提高高度方向的定位效果和整體定位效果。首先采用傳統(tǒng)方法對(duì)目標(biāo)點(diǎn)進(jìn)行定位，獲取目標(biāo)點(diǎn)位置的一次解析值，然后使用修正偏差的 Tikhonov正則化方法再次對(duì)目標(biāo)點(diǎn)位置進(jìn)行估計(jì)，以提高高度方向定位精度。該方法能有效地提高高度方向的定位精度和整體定位精度。

從仿真分析與實(shí)驗(yàn)結(jié)果來看，本文提出的算法能夠?qū)鹘y(tǒng)解析法的定位精度提高40%以上，能夠顯著提高基站在某個(gè)方向具有布置限制時(shí)該方向的定位精度。其中實(shí)驗(yàn)結(jié)果表明，所提方法的定位效果比最小二乘法提高52.1%，比Chan算法提高41.6%。由于本方法采用的依然是基于解析法的算法，故其計(jì)算相比于傳統(tǒng)解析法只多一次判斷和一次解析時(shí)間，相比于Kalman濾波算法等沒有滯后效果，而且同樣可以作為其他方法的解算初值。