非合作環境下單標簽快速延拓建站方法

徐建龍，王 慶，張 昊

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

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

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

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

1 快速建站方法原理

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

圖1 非合作環境快速建站方法方案

1.1 UWB定位原理

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

文中采用三邊定位法實現待標定基站的快速建站，以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方法可將以上非線性方程組轉化為線性方程組，將式(1)中第n個方程減去第(n+1)個方程，即可得到關于待標定基站Pt的坐標(xt,yt,zt)的線性方程組，如式(2)所示。

(2)

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

X=(ATA)-1ATb.

(3)

1.3 Taylor展開法

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

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

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

(4)

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

(5)

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

δ=(GTG)-1GTh.

(6)

1.4 梯度下降法

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

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

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

(7)

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

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

(8)

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

Δd=δJ·α.

(9)

迭代計算新坐標值，即：

(10)

2 仿真實驗驗證

2.1 誤差統計方法

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

(11)

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

(12)

2.2 仿真實驗

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

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

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

圖3 仿真實驗示意圖

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

2.3 仿真結果分析

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

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

圖4 待建基站標定結果及定位誤差

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

3 真實環境實驗驗證

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

圖6 實驗平臺組成

3.1 實驗過程

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

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

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

3.2 結果分析

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

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

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

圖8 待測UWB基站坐標解算結果

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

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

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

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

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

3.3 定位效果驗證

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

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

圖11 定位誤差

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

4 結束語

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