徐建龍，王 慶，張 昊

(東南大學 儀器科學與工程學院，南京 210000)

近年來，隨著人們對室內(nèi)精準位置信息獲取的需求，室內(nèi)定位服務(wù)產(chǎn)業(yè)發(fā)展迅猛。實現(xiàn)室內(nèi)定位服務(wù)的首要工作是基站的部署，基站的部署主要是結(jié)合定位場所的物理空間，通過測繪手段來實現(xiàn)定位基站的標定。例如超寬帶(Ultra-Wideband，UWB)節(jié)點的部署[1-2]、藍牙信標的部署[3]、Wi-Fi節(jié)點的部署[4]、偽衛(wèi)星節(jié)點的部署[5]等，這些環(huán)境區(qū)域大部分較為理想，便于測量與部署。然而需要位置服務(wù)的非合作環(huán)境也是普遍存在的，例如應(yīng)急定位的室內(nèi)救援區(qū)域、室內(nèi)外無縫定位的室內(nèi)區(qū)域、自組網(wǎng)定位的未標定區(qū)域等，這也暴露出了目前室內(nèi)定位技術(shù)必須基于理想環(huán)境并事先標定坐標的情況下才能提供精準位置服務(wù)的不足[6]。在非合作室內(nèi)環(huán)境下，由于缺乏先驗信息無法對室內(nèi)環(huán)境進行了解和認知，從而難以開展定位基站的標定與定位模型的構(gòu)建工作[7]。因此，如何利用易獲取信息對非合作區(qū)域進行快速標定基站，并實現(xiàn)精準定位服務(wù)，成為了應(yīng)急定位以及室內(nèi)外無縫定位的研究重點。

在室內(nèi)外無縫定位、應(yīng)急定位以及組網(wǎng)方式國內(nèi)外學者也開展了相應(yīng)的研究。文獻[8]針對室內(nèi)外無縫定位的場景，提出了一種全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)輔助UWB基站位置估計的方法，利用GNSS提供的實時位置坐標值與UWB測距值，利用擴展卡爾曼濾波緊組合求解UWB基站在大地坐標系下的坐標位置，但相對來說精度較低；文獻[9]針對地下UWB自組網(wǎng)定位場景提出了一種動態(tài)的UWB組網(wǎng)坐標估計方法，UWB模塊既為基站又為標簽，在合作區(qū)域動態(tài)的將已知的坐標系過渡到非合作區(qū)域，利用TLS-Taylor迭代法對非合作區(qū)域的UWB標簽進行位置估計，逐步過渡，由于不斷遞進的重定位，累積誤差也會不斷增大。文獻[10]提出了一種應(yīng)急定位場景下，利用所有可視UWB基站互相之間的測距值進行總體約束，基于梯度下降算法進行最優(yōu)化估計，完成UWB基站組網(wǎng)的自校準的方法，精度較高，但其只能生成自身建立的坐標系，無法與合作區(qū)域的已有坐標系聯(lián)系起來；文獻[11]針對UWB室內(nèi)定位場景，提出了一種基于冗余距離篩選的UWB定位優(yōu)化方法，該方法以多個定位標簽的間距為約束條件，對冗余距離設(shè)置權(quán)重進行篩選，使用機器學習方法優(yōu)化Caffery方法計算的坐標初值，得到最優(yōu)坐標值，方法新穎但約束條件較多，計算較為復雜，收斂時間較長，不適用于應(yīng)急定位場景。

針對以上研究內(nèi)容存在的問題，本文提出一種基于待建基站之間距離約束的非合作環(huán)境下單標簽快速延拓建站方法，旨在快速、靈活且準確地標定待建基站，以便在非合作環(huán)境下快速開展應(yīng)急定位工作，提高救援效率。該方法利用合作環(huán)境下定位系統(tǒng)獲得移動平臺軌跡坐標，結(jié)合UWB標簽獲得的與各待建基站之間的測距信息，對軌跡坐標數(shù)據(jù)和測距數(shù)據(jù)進行時間軟同步，即利用測距數(shù)據(jù)與軌跡坐標數(shù)據(jù)的UTC時間戳，以時間戳最近的原則進行數(shù)據(jù)幀匹配，本文以時間戳之差小于6 ms作為條件閾值匹配出相近數(shù)據(jù)，進行時間軟同步。將同步好的數(shù)據(jù)采用Caffery方法進行線性化，后用最小二乘法初步計算出待建基站坐標初始值，再利用各待建基站之間的距離作為約束，通過梯度下降(Gradient descent,GD)算法對坐標初值進行最優(yōu)化估計，進一步得到最優(yōu)結(jié)果。

1 快速建站方法原理

文中提出的非合作環(huán)境快速建站方法的總體方案如下：在合作環(huán)境下已有定位系統(tǒng)范圍內(nèi)，將UWB移動站(Tag)綁定已有可定位的移動站組成移動平臺，在保持移動平臺與非合作環(huán)境中待標定基站處于視距狀態(tài)的情況下，以任意軌跡運動。移動平臺可獲取非合作環(huán)境中各待標定的UWB基站到移動站的距離di，亦可獲取移動平臺基于合作環(huán)境的軌跡坐標(x,y)。非合作環(huán)境快速建站方法方案如圖1所示。

圖1 非合作環(huán)境快速建站方法方案

1.1 UWB定位原理

文中使用的UWB定位系統(tǒng)采用的是雙邊雙向測距DS-TWR(Double-sided Two-way Ranging)方法[12]，此方法測距原理是：同時記錄兩個往返消息的時間戳，通過計算得到飛行時間，進而得到測距值。此方法響應(yīng)時間比單邊雙向測距方法長，但其測距誤差更低，亦可消除基站與移動站之間時鐘不同步帶來的誤差，測距精度更高[13-14]。

文中采用三邊定位法實現(xiàn)待標定基站的快速建站，以3個位置點A，B，C為圓心做圓，其坐標值分別為(x1,y1),(x2,y2),(x3,y3)，3個圓周相交于1點D，交點D即為待建立基站的坐標位置，A，B，C為移動平臺移動軌跡中的3點，A，B，C與交點D的距離分別為d1,d2,d3。三邊定位原理如圖2所示。

圖2 三邊定位原理

由上述定位原理可知，已知4個軌跡點坐標(xi,yi,zi)與其到待標定基站的距離di，即可通過Caffery方法，方程間相減即可進行線性化處理，從而可采用最小二乘方法求得待標定基站坐標(xt,yt,zt)。4個軌跡點到待標定基站之間的距離方程如式(1)所示。

(1)

1.2 最小二乘方法

由Caffery方法可將以上非線性方程組轉(zhuǎn)化為線性方程組，將式(1)中第n個方程減去第(n+1)個方程，即可得到關(guān)于待標定基站Pt的坐標(xt,yt,zt)的線性方程組，如式(2)所示。

(2)

根據(jù)式(2)的線性方程組可以得到待建基站坐標的最小二乘解，如式(3)所示:

X=(ATA)-1ATb.

(3)

1.3 Taylor展開法

由Caffery方法線性化之后，最小二乘解算得到待標定基站Pt的坐標初始值(xt,yt,zt)，但是單純由最小二乘法解算得到的坐標值定位誤差較大，因此常常基于Pt(xt,yt,zt)初始坐標值使用Taylor級數(shù)法對其進行優(yōu)化[15]，即將式(1)在定位初值(xt,yt,zt)處進行Taylor級數(shù)展開，通過循環(huán)迭代消除誤差。

假設(shè)式(3)由最小二乘方法得到的待標定基站Pt的坐標初值與真值(xt′,yt′,zt′)之間的誤差為(δx,δy,δz)，即可得到：

(xt′,yt′,zt′)=(xt,yt,zt)+(δx,δy,δz).

(4)

將式(1)在定位初值Pt,(xt,yt,zt)處進行Taylor級數(shù)展開，并忽略二階及二階以上的分量，即可得到線性方程組式(5)。

(5)

由最小二乘法進行解算，即可得到式(5)的解，即如式(6)所示。

δ=(GTG)-1GTh.

(6)

1.4 梯度下降法

在應(yīng)急救援建站過程中，可用本方法同時對多個基站進行標定，而多個基站之間的距離可以通過基站之間的測距得知。為解決實際測距誤差、移動軌跡交叉等對標定結(jié)果的影響，文中提出使用待建基站之間的距離對標定結(jié)果進行約束的方法實現(xiàn)對標定結(jié)果的最優(yōu)化估計，方法中待建基站之間的距離由待建基站之間通信測距得到，同組進行標定的待建基站之間需在視距情況下；如存在非視距情況，則需要人員手動進行直線距離的測量。

文中使用GD方法[16]，以最小二乘法得到的定位初值為迭代的起點，以同時標定的兩個或多個待建基站之間的距離di,j為約束條件，通過構(gòu)建損失函數(shù)，目標是最小化所有待建基站損失函數(shù)的總和，以達到最好的定位效果，優(yōu)化待建基站的定位坐標值。

構(gòu)建梯度下降法的損失函數(shù)如式(7)所示。

(7)

式中：Pi(xi,yi)，Pj(xj,yj)為待建基站坐標值；m為待建基站個數(shù)；di,j為Pi與Pj之間的距離。

計算式(7)損失函數(shù)的對xi和yi的偏導數(shù)，即：

(8)

定義步長Δd為梯度δJ與學習率α的乘積，即：

Δd=δJ·α.

(9)

迭代計算新坐標值，即：

(10)

2 仿真實驗驗證

2.1 誤差統(tǒng)計方法

文中設(shè)計了仿真實驗，對上述非合作環(huán)境下單標簽快速延拓建站方法進行驗證。其中，實驗的定位精度用待建基站坐標真實值與坐標計算值之間的歐拉距離誤差來進行衡量。歐拉距離誤差計算方法如式(11)所示。

(11)

在非合作環(huán)境放置多個待建基站，采用多個基站標定結(jié)果的均方根誤差(RMSE)來表示標定穩(wěn)定性，其計算方法如式(12)所示。

(12)

2.2 仿真實驗

指定合作環(huán)境下用于標定的移動標簽移動軌跡坐標真值(xi,yi)和非合作環(huán)境下的15個待建基站真實坐標值Pt(xt′,yt′)。結(jié)合以上坐標真值，根據(jù)式(11)即可計算出移動軌跡各點與各個待建基站Pt的測距真值集合St。

文中實測使用的UWB設(shè)備是基于雙邊雙向飛行時間DS-TWR測距方法進行測距，文獻[17]指出此方法的測距誤差滿足均值為0、標準差為0.02 m的正態(tài)分布。但在實測實驗中，測距誤差會受到室內(nèi)復雜環(huán)境中各種遮擋物造成的多徑效應(yīng)的干擾，無法保證最佳的實驗效果，因此，仿真實驗中設(shè)置UWB的測距誤差δ滿足均值為0，標準差為0.10 m的正態(tài)分布，以最大程度模擬實測的場景。同樣，本方法的移動平臺的移動軌跡定位精度由合作環(huán)境的定位系統(tǒng)決定，文中實驗中，合作環(huán)境的定位系統(tǒng)為動捕相機定位系統(tǒng)，其定位精度可達mm級，但考慮實際場景中普遍已有定位系統(tǒng)(如GNSS定位、UWB定位等)，仿真實驗設(shè)置移動軌跡定位誤差滿足均值為0，標準差為0.10 m的正態(tài)分布。

仿真實驗合作環(huán)境移動軌跡設(shè)置為(2 m,2 m)-(2 m,10 m)-(10 m,10 m)-(2 m,10 m)3段直線軌跡，在非合作環(huán)境設(shè)置15個待建基站，進行快速建站仿真驗證。仿真實驗示意圖如圖3所示。

圖3 仿真實驗示意圖

將非合作環(huán)境15個待建基站真實坐標、仿真軌跡散點坐標值與計算得到的UWB測距值輸入到LS-Taylor方法與LS-GD方法，經(jīng)兩種算法計算得到的優(yōu)化后的待建基站坐標值，最后，根據(jù)待建基站坐標真值與計算得到的基站坐標值計算各個點的歐拉距離誤差與均方根誤差。

2.3 仿真結(jié)果分析

文中提出的LS-GD方法利用多個待建基站之間的距離來約束各個待建基站的標定結(jié)果，提高了待建基站標定精度。以各待建基站標定的定位誤差、均方根誤差來衡量定位方法的穩(wěn)定性與精確性。其中，考慮待建基站距離合作環(huán)境的距離不同，以(16 m,2 m)、(20 m,6 m)、(24 m,10 m)3個待建基站的標定結(jié)果為例，其標定結(jié)果、定位誤差對比如圖4所示，15個待建基站標定結(jié)果均方根誤差圖如圖5所示。

根據(jù)圖4中3個待建基站的實驗結(jié)果可以看出，待建基站與合作環(huán)境距離越遠，其定位結(jié)果誤差越大，這一特征也符合實際規(guī)律。根據(jù)圖5誤差統(tǒng)計結(jié)果可以看出，使用待建基站之間的距離作為約束，對其標定結(jié)果來進行優(yōu)化估計，能夠明顯提高標定精度。由圖5中15個待建基站定位誤差均值可以看出，LS-GD方法相較于LS-Taylor方法定位精度更高，受距離影響也相對較小，魯棒性更優(yōu)。LS_GD方法定位誤差約在0.2 m，而LS-Taylor方法定位誤差在0.3～0.4 m波動，說明此方法可行且效果較好。

圖4 待建基站標定結(jié)果及定位誤差

圖5 15個待建基站標定結(jié)果誤差均值及RMSE分布

3 真實環(huán)境實驗驗證

本實驗使用的UWB模塊為NoopLoop公司的LinkTrackP模塊，合作環(huán)境的定位系統(tǒng)為動態(tài)捕捉相機定位系統(tǒng)[18]，使用1個UWB定位標簽(Tag)、1個紅外反光標記點、1臺實驗小推車、1臺筆記本電腦組成移動平臺。實驗中動態(tài)捕捉相機定位系統(tǒng)作為真值系統(tǒng)，為移動平臺提供移動軌跡坐標值，以合作環(huán)境的動態(tài)捕捉相機定位系統(tǒng)坐標系作為坐標基準，標定非合作環(huán)境多個待建UWB基站(Anchor)位置。實驗平臺組成如圖6所示。

圖6 實驗平臺組成

3.1 實驗過程

實驗過程中保證移動平臺與待建基站之間無障礙物遮擋，完全為視距(LOS)環(huán)境。使用移動平臺在合作環(huán)境動態(tài)捕捉相機定位系統(tǒng)區(qū)域下以任意軌跡行進一段時間。過程中，筆記本電腦用串口助手，以6 ms/fps的速率接收UWB標簽到待建UWB基站的測距數(shù)據(jù)，以25 ms/fps的速率接收動態(tài)捕捉定位系統(tǒng)輸出的軌跡坐標數(shù)據(jù)。同時，利用電腦的串口程序?qū)γ繋瑪?shù)據(jù)進行授時，給每幀數(shù)據(jù)添加時間戳(UTC時間)，保證在后面進行數(shù)據(jù)處理時，對動態(tài)捕捉相機定位系統(tǒng)輸出的坐標和UWB測距值進行時間同步。

利用動態(tài)捕捉相機定位系統(tǒng)提供實驗平臺移動過程的高精度室內(nèi)定位坐標值，實驗平臺移動軌跡以及待建基站分布如圖7所示，紅色點即為非合作環(huán)境待建基站。

圖7 實驗平臺移動軌跡及待建基站分布

3.2 結(jié)果分析

1)實驗數(shù)據(jù)預處理。首先，動態(tài)捕捉相機輸出的軌跡坐標數(shù)據(jù)為[-6 m,6 m]×[-6 m,6 m]區(qū)間，為方便計算與成圖，將定位坐標數(shù)據(jù)進行平移，x軸和y軸數(shù)據(jù)均進行+6 m處理。

其次，因為動捕相機定位系統(tǒng)每25 ms輸出 1 fps軌跡坐標數(shù)據(jù)，UWB每6 ms輸出1 fps測距數(shù)據(jù)，兩者數(shù)據(jù)輸出頻率不相同，所以需對軌跡坐標數(shù)據(jù)和測距數(shù)據(jù)進行時間同步，可以通過插值算法或者時間戳最近原則進行時間戳的對應(yīng)，文中時間同步的策略采用了時間戳最近原則進行軟同步，若兩fps數(shù)據(jù)時間戳(UTC時間)差值不超過6 ms，則視為同1 fps數(shù)據(jù)，從而進行一一對應(yīng)。

2)實驗結(jié)果。將以上處理好的數(shù)據(jù)代入Caffery-LS算法方法中，進行待建基站坐標的初步解算，得到坐標初值后，將坐標初值及待建基站之間的真實距離值帶入到泰勒展開算法與梯度下降算法中進行最優(yōu)化估計，得到最終的待建基站坐標。得到的待建UWB基站坐標解算結(jié)果以及估計誤差如圖8所示，包含LS-Taylor算法與LS-GD算法解算的結(jié)果。

圖8 待測UWB基站坐標解算結(jié)果

根據(jù)圖8中各待建基站解算坐標結(jié)果可以看出，基站(a)和基站(b)解算結(jié)果都比較準確，但由于現(xiàn)場環(huán)境較為復雜，有較多遮擋物，并且可以加入約束的其他待建基站較少，導致基站(c)解算結(jié)果出現(xiàn)了少量的偏差。根據(jù)整體的誤差統(tǒng)計來看，LS-GD方法較LS-Taylor方法解算精度有了較大的提升。

由LS-GD方法解算出多組解算結(jié)果后，對優(yōu)化后的多組數(shù)據(jù)取平均值即可確定最終的坐標值，亦可以得到其標定誤差均值。圖9為12個待建基站標定誤差均值；表1列出了各待建UWB基站解算誤差均值。

圖9 待建基站標定誤差均值

表1 待建UWB基站坐標解算誤差均值表 m

由于1組數(shù)據(jù)損失的原因，有4個基站的數(shù)據(jù)不完整，只對12個待建基站進行了解算，驗證。由圖9可以直觀看出LS-GD方法的標定誤差均值普遍低于LS-Taylor方法，只有基站8的解算結(jié)果誤差相對較大，主要是因為基站8的同時解算的其他基站數(shù)據(jù)異常，未達到約束的效果?？傮w來看，LS-GD方法解算的誤差均值僅為LS-Taylor方法的70%，且解算誤差相對較為穩(wěn)定，魯棒性更好。表1列出了各基站解算的誤差均值，LS-GD方法解算誤差均值普遍穩(wěn)定在0.2 m以內(nèi)。以上實驗結(jié)果表明,LS-GD方法相較于傳統(tǒng)的LS-Taylor方法標定精度、魯棒性方面都有明顯的提升。

3.3 定位效果驗證

文中分別以LS-Taylor方法得到的四角基站坐標值和LS-GD方法解算得到四角基站坐標值，將其組成新的UWB定位系統(tǒng)，對其進行定位效果驗證。在定位區(qū)域內(nèi)，用移動平臺小車推行，按直線軌跡移動，兩方法得到的定位軌跡與實際軌跡圖對比如圖10所示，兩方法得到的定位誤差圖對比如圖11所示。

圖10 定位軌跡圖與實際軌跡

圖11 定位誤差

由圖10可以看出，LS-GD方法得到的UWB基站坐標位置組成的UWB定位系統(tǒng)定位效果較LS-Taylor方法定位精度更高，LS-GD方法解算出的基站坐標組成的UWB定位系統(tǒng)解算出的定位軌跡與實際運動軌跡圖基本吻合。由圖11定位結(jié)果誤差統(tǒng)計圖得出，其定位誤差在17 cm以內(nèi)，定位效果較好，可以較好地滿足應(yīng)急定位、室內(nèi)外無縫定位的需求。

4 結(jié)束語

針對應(yīng)急定位等領(lǐng)域，基于合作環(huán)境下的已有位置服務(wù)對非合作區(qū)域進行快速建站、完成位置服務(wù)的共享建立的問題，本文提出了一種單標簽快速延拓建站的方法。該方法實施簡單、定位精度較已有方法更高。該方法將合作環(huán)境定位系統(tǒng)移動站與UWB標簽綁定組成移動平臺，在合作環(huán)境定位系統(tǒng)區(qū)域按任意軌跡移動，得到運動軌跡坐標值及UWB測距值，根據(jù)UTC時間戳進行時間軟同步后，利用Caffery-LS算法解算出待建基站坐標初始值，后利用同時標定的多個待建基站之間的距離作為約束條件，用梯度下降法對待建基站的坐標進行最優(yōu)化估計，即可快速精確計算出待建UWB基站坐標值，完成非合作環(huán)境多個待建基站的快速標定。實驗結(jié)果表明：該方法可以快速、精確完成非合作環(huán)境多個待建基站的標定工作，LS-GD方法相較于傳統(tǒng)LS-Taylor方法定位精度更高、魯棒性更好，仿真實驗與實測實驗的定位誤差結(jié)果約為傳統(tǒng)LS-Taylor方法的60%～70%。對新標定的定位系統(tǒng)進行定位效果測試，其定位誤差可控制在15 cm以內(nèi)，可較好滿足應(yīng)急定位等領(lǐng)域的應(yīng)用。