基于UWB的LSM-Taylor級聯(lián)車輛定位算法

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

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

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

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

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

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

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

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

1 定位算法方案與框架

1.1 UWB信號分析

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

圖1 TDOA誤差分布示意圖

1.2 定位算法方案與框架設(shè)計

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

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

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

圖2 級聯(lián)定位算法流程圖

2 多基站LSM定位算法

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

為了簡化定位系統(tǒng)模型，假設(shè)定位系統(tǒng)中包含一個高度為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定位系統(tǒng)的原始輸入數(shù)據(jù)為{L21,L32,…,LN(N-1),L1N}，可由UWB模塊測得。序號相連的3個基站組成一個位置解算子單元，設(shè)基站序號分別為i，j和k(i

(3)

將式(3)線性化可得

(4)

aijkx+bijky=dijk

(5)

式中，

(6)

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

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

Ap=d

(7)

式中，

(8)

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

(9)

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

3 Taylor級數(shù)定位算法

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

(10)

(11)

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

(12)

整合式(11)可得

(13)

式中，

(14)

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

(15)

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

(16)

重復(fù)上述過程直至收斂，收斂條件為

|Δx|+|Δy|<ε

(17)

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

4 UWB定位算法仿真分析

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

4.1 仿真方案設(shè)計

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

4.2 仿真結(jié)果

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

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

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

4.3 定位精度的評估

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

(18)

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

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

5 UWB定位算法實驗與分析

5.1 實驗環(huán)境設(shè)計

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

實驗場地設(shè)置在同濟大學機械與能源工程學院地下車庫。為了減小室內(nèi)物體遮擋造成的NLOS誤差，將UWB基站的高度設(shè)置為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個靜態(tài)定位點布置在高度為2 m的水平面內(nèi)，其中4個均勻分布在正方形區(qū)域內(nèi)，另外4個分別布置在正方形區(qū)域的中心、邊上、頂點附近和外部。

圖6 靜態(tài)定位實驗UWB布置俯視圖

5.2 實驗結(jié)果與分析

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

表1 靜態(tài)定位實驗統(tǒng)計結(jié)果

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

6 結(jié)束語

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