基于UWB的LSM-Taylor級聯車輛定位算法

許秀峰， 蒲家坤， 周愛國， 楊思靜， 魏榕慧

(同濟大學 機械與能源工程學院，上海 201804)

智能駕駛汽車需要高精度的穩定定位信息，以滿足各場景的應用需求，如碰撞預警、并線等[1-2]。除了常見的室外定位場景外，自動駕駛要求在室內場景(如地下車庫、隧道等)也能得到精確的車輛位置，從而實現全方位的高精度定位。無線定位利用無線通信和測量參數(幅度、傳輸時間、相位角和到達角等)確定車輛位置，主要用于室內定位。常見的無線定位技術包括超聲波定位、藍牙定位、射頻識別定位、WiFi定位、ZigBee定位和超寬帶(Ultra Wideband，UWB)定位等。

UWB是一種以極低的功率在短距離內進行高速傳輸的無線技術，具有速率高、功耗低、測距精度高等特點[3]。發送機在發射無線信號時，將分散信號加載到寬頻帶中，然后接收機接收并還原這些信號，同時產生擴頻增益。UWB信號自身抗干擾性強且不會干擾其他設備上的無線信號。

UWB定位技術在國外的發展較早，且已有不少學者對其進行了深入的研究。Maalek等[4]研究了UWB定位技術在施工現場次優條件下的定位性能，進行了一系列的實驗，通過控制變量來模擬每種場景對UWB定位性能的影響。Zhou等[5]提出了一種高精度的遠距離UWB定位算法，以Cramer-Rao下界為設計工具解決了測距信息融合的問題，并研究了到達時間差(Time Difference of Arrival,TDOA)定位系統中基于相關測距信息的錨點最佳幾何分布算法。我國從1999年開始研究UWB技術的應用，2003年國家“863”計劃開啟了東南大學、清華大學、中國科技大學等國內高校的自主研發高潮[6]。王志鎧[7]針對傳統導航方式在無人搬運車(Automated Guided Vehicle，AGV)機器人上的不足，提出了一種基于UWB的室內AGV定位算法。

UWB定位系統工作時，首先建立UWB模塊間的通信機制，然后通過基站和移動站之間的通信信息獲得定位所需的必要參數，建立相應的系統估算模型，最后解算出移動站的位置。UWB的定位方式有4種：基于接收信號強度、基于達到角度、基于到達時間和基于到達時間差(TDOA)。其中基于TDOA的定位方式只需保證基站之間的時鐘嚴格同步，且單程測距即可獲得所需信息，降低了基站的硬件成本和定位系統的通信容量，是一種優良的定位方式。

在實際的定位過程中，多徑效應和非視距環境會帶來很大的誤差。多徑效應是指在復雜的室內環境下，UWB信號在傳播過程中會發生反射、折射、衍射或散射等現象，信號會通過多條路經到達UWB基站。非視距(Non Line of Slight，NLOS)是指當UWB基站和移動站之間存在障礙物時，UWB信號傳播過程中就不存在直達路徑。一方面，UWB會通過反射、折射或衍射等方式到達UWB基站，造成多徑效應；另一方面，UWB信號在穿透障礙物時，傳播速度小于在空氣中的傳播速度，造成傳播時間上的正偏差，從而影響測距精度。

為了滿足智能車在室內的高精度定位要求，解決多徑效應和非視距產生的測量誤差對定位精度的影響，針對室內的偽三維定位場景，提出了一種基于UWB的LSM-Taylor級聯車輛定位算法。該算法以TDOA為定位方式，以多基站LSM定位算法的計算結果為初始值，通過Taylor級數迭代估計車輛的精確位置，為車輛提供精確的定位信息。

1 定位算法方案與框架

1.1 UWB信號分析

在無干擾的靜止環境下，實驗采集TDOA信息，數據結果如圖1所示。由圖1可知，TDOA的測量值在真值±20 cm的范圍內波動，其誤差服從高斯分布，均值為0，標準差為6.5 cm。

圖1 TDOA誤差分布示意圖

1.2 定位算法方案與框架設計

傳統的TDOA定位算法有Fang算法[8]、Chan算法[9]等。Fang算法通過線性化雙曲線方程來求解位置，但其只能應用于二維平面的定位，且只能利用3個基站的TDOA信息，不能綜合多基站的TDOA信息，因此精確度較低。Chan算法采用兩次迭代的求解方法，使用多基站求解三維空間下移動站坐標，但NLOS等因素造成的較大測距誤差對Chan算法的定位精度影響很大。

在實際的智能車室內定位場景下，安裝在車上的UWB移動站高度變化很小，可將其近似為常值，并且高度方向上的位移對車輛導航的意義不大。因此，可將移動站的運動簡化為高度未知的水平二維運動，定位系統通常只輸出車輛的水平二維坐標。考慮到定位解算的準確性和簡便性，通常將UWB基站布置在同一高度上。

在三維立體空間內，每兩個基站的TDOA信息可以構建出一個非線性方程，本文提出的UWB定位算法為忽略高度的偽三維定位，但由于基站和移動站之間存在高度差，因此仍需要3組TDOA信息才能求解位置，即需要4個基站。在實際情況中，UWB測距信息存在一定的誤差和噪聲，還需要更多的基站，利用冗余信息對UWB的定位結果進行優化。筆者提出的基于TDOA的LSM-Taylor級聯定位算法，首先利用最小二乘法(Least Square Method,LSM)得到非理想狀態下的移動站位置初始值，再通過Taylor展開遞歸獲取精確的位置估計，直至收斂。算法流程圖如圖2所示。

圖2 級聯定位算法流程圖

2 多基站LSM定位算法

理想情況下，偽三維定位系統需要4個基站才能獲得移動站的位置坐標，但測量過程中存在NLOS、多徑效應或溫漂等引起的誤差，因此四基站定位系統并不能準確求解出移動站的位置。TDOA定位方式為變量耦合程度較高的非線性系統，本文采用多基站LSM定位算法，將非線性方程求解問題轉換為非線性系統的最優估計問題[10]，其中基站數量≥4。

為了簡化定位系統模型，假設定位系統中包含一個高度為h的移動站(x,y,h)和N個高度為0，三維坐標已知的基站(x1,y1,0;x2,y2,0;…;xN,yN,0),第i個基站(xi,yi,0)到移動站的距離di可以由UWB信號飛行時間τi計算得到：

(1)

式中，c為UWB信號傳播速度，近似于光速，可取c=299792485 m/s。

Lij表示移動站到第i個和第j個基站之間的TDOA值，即

Lij=di-dj=c×(τi-τj)

(2)

假定UWB定位系統的原始輸入數據為{L21,L32,…,LN(N-1),L1N}，可由UWB模塊測得。序號相連的3個基站組成一個位置解算子單元，設基站序號分別為i，j和k(i

(3)

將式(3)線性化可得

(4)

aijkx+bijky=dijk

(5)

式中，

(6)

由式(5)可知，每個位置解算子單元可以得到兩組非線性方程，由此可以得到一個包含移動站的直線約束。若想求出移動站的位置，需要更多的位置解算子單元來提供直線約束，最少直線約束數為2。

將所有的定位解算子單元和TDOA測量值代入式(5)，可得

Ap=d

(7)

式中，

(8)

當N≥4時，通過最小二乘法得到移動站的估計位置：

(9)

式(5)并不包含高度項h，說明基站LSM定位算法可以在高度未知的情況下求解移動站的水平位置坐標。與傳統的Fang算法相比較，多基站LSM定位算法結合多個基站的TDOA信息聯合估計移動站的位置初始值，降低了個別誤差較大的TDOA測量值對定位結果的影響，提高了定位系統的穩定性和準確性。

3 Taylor級數定位算法

多基站LSM定位算法計算量小且精度較高，但得到的定位結果并非最優解，還需通過Taylor級數定位算法得到精確解。Taylor級數定位算法是一種迭代的求解方法[11]。每次迭代中，對誤差項在估計值附近進行Taylor展開，并忽略高階項，通過局部的最小二乘法來更新移動站的位置。定位算法的迭代次數和定位精度很大程度上取決于位置初始值的計算。當初始值偏差較大時，迭代極易陷入局部最優中，定位算法將難以收斂至準確值。

(10)

(11)

式中，Δx和Δy為修正量；

(12)

整合式(11)可得

(13)

式中，

(14)

忽略e，并通過最小二乘法得到位置校正量的估計值：

(15)

式中，Φ為TDOA測量值的協方差矩陣。利用校正值更新估計量：

(16)

重復上述過程直至收斂，收斂條件為

|Δx|+|Δy|<ε

(17)

式中，ε為修正量的最小閾值。

4 UWB定位算法仿真分析

為了驗證上述定位算法的準確性和穩定性，下面以Matlab(CPU為Intel i7-8565U)為仿真環境對算法進行仿真分析。

4.1 仿真方案設計

首先，假設TDOA測量誤差的標準差σUWB為3 cm，利用LSM-Taylor級聯定位算法和多基站LSM定位算法分別進行兩次仿真，每次仿真包含100組數據。

4.2 仿真結果

仿真結果俯視圖如圖3和圖4所示，圖3和圖4中每個點的橫縱坐標代表了定位算法得到的位置。由圖3和圖4可知，兩種算法的移動站位置都聚集在真實值附近，且LSM-Taylor級聯定位算法的結果分布更加緊密，定位精度更高。多基站LSM定位算法解算每組數據的時間為12 μs，而LSM-Taylor級聯定位算法采用多次迭代的方式，定位精度較高但計算量會相對較大，其解算每組數據的時間為58 μs。由此可見，兩種算法都能滿足UWB實時定位的要求。

圖3 定位點(1 m,3 m,0)仿真結果俯視圖

圖4 定位點(5 m,2 m,0)仿真結果俯視圖

4.3 定位精度的評估

均方根誤差(Root Mean Squared Error，RMSE)可以評價數據的變化程度，RMSE的值越小，說明定位系統的精確度越高。其計算公式為

(18)

接下來，針對定位點(1 m,3 m,0)，以RMSE為評估標準，定量分析TDOA測量誤差的標準差σUWB對兩種算法定位精度的影響，其結果如圖5所示。由圖5可知，隨著誤差標準差σUWB的增大，兩種算法的定位誤差均逐漸增大，但LSM-Taylor級聯定位算法的誤差較小，保持在10 cm以內，且增大的幅度也較小，受誤差標準差σUWB的影響較小，具有更高的精度和魯棒性。

圖5 TDOA測量誤差的標準差對定位精度的影響

5 UWB定位算法實驗與分析

5.1 實驗環境設計

定位元件選擇DW1000芯片，它是由Decawave公司推出的超寬帶定位芯片，符合IEEE 802.15.4-2011超寬帶標準。DW1000作為一款定位芯片，其功耗低，輸出頻率為100 Hz，可雙向測距和定位，測距噪聲標準差為6 cm，最大測距距離可達200 m，定位精度達到cm級。

實驗場地設置在同濟大學機械與能源工程學院地下車庫。為了減小室內物體遮擋造成的NLOS誤差，將UWB基站的高度設置為3 m。如圖6所示，4個UWB基站分別布置在正方形的4個頂點上，其坐標依次為 (0,0,3 m)，(0,10 m,3 m)，(10 m,10 m,3 m)，(10 m,0,3 m)。7個靜態定位點布置在高度為2 m的水平面內，其中4個均勻分布在正方形區域內，另外4個分別布置在正方形區域的中心、邊上、頂點附近和外部。

圖6 靜態定位實驗UWB布置俯視圖

5.2 實驗結果與分析

UWB模塊的輸出頻率為100 Hz，在每個定位點靜止采集10 s的TDOA信息，并利用LSM-Taylor級聯定位算法對其解算，以距離真實位置的最大偏差和RMSE為定位精度的評估標準。定位結果統計如表1所示。

表1 靜態定位實驗統計結果

由表1可知，1#～4#定位點的最大偏差和RMSE近似，且明顯大于5#定位點，小于6#、7#和8#定位點。隨著定位點與正方形中心的距離變大，定位點的定位精度下降。當定位點距離中心較近時，其RMSE在10 cm以內，符合室內定位的精度要求；當定位點位于頂點附近或正方形外部時，其精度大幅度下降。因此，移動站的運動范圍應盡量處于UWB基站多邊形區域的內部，從而保證室內定位的高精度。

6 結束語

以TDOA為定位方式，提出了一種基于UWB的LSM-Taylor級聯車輛定位算法。首先利用TDOA構造非線性方程組，然后通過LSM求解車輛的估計位置，最后通過Taylor級數迭代獲得車輛的精確位置。在仿真結果中，相比LSM定位算法，LSM-Taylor級聯定位算法的定位結果分布更加緊密，定位精度更高。實際測試結果表明，該定位算法的RMSE在10 cm以下，能夠滿足智能駕駛中的室內定位要求，驗證了該方法的有效性，能廣泛應用于智能駕駛的室內定位中。