融合UWB測距信息的室外高精度定位算法

周愛國 ，楊思靜 ，沈 勇 ，王嘉立

0 引言

智能駕駛汽車需要高精度的穩(wěn)定定位信息，以滿足各場景的應用需求，如碰撞預警、并線等[1-2]。目前的智能駕駛汽車定位系統(tǒng)高度依賴于差分定位的全球衛(wèi)星導航系統(tǒng)（global navigation satellite system，GNSS），不僅價格昂貴而且在城市環(huán)境中極易因信號遮擋而引起誤差，造成車載定位系統(tǒng)在城市環(huán)境下定位誤差的增大和穩(wěn)定性的降低[3]。慣性導航系統(tǒng)（inertial navigation system，INS）利用慣性敏感元件來推算載體的位置、速度和姿態(tài)，可實現(xiàn)自主定位，但其誤差會隨時間累積以致定位精度隨時間發(fā)散。作為GNSS定位技術(shù)的1種，單點定位模式的全球定位系統(tǒng)（global positioning system，GPS）定位精度較低，但能夠提供全方位、全天候的定位信息且價格較低?；跀U展卡爾曼濾波的GPS/INS組合定位算法結(jié)合GPS和INS的誤差特性保證定位信號的穩(wěn)定輸出，但其定位誤差難以達到分米級[4]。

地圖匹配是1種軟件糾錯技術(shù)，將車輛行駛軌跡與高精度電子地圖進行匹配，從而確定車輛在地圖中的位置[5]。但現(xiàn)有地圖匹配技術(shù)存在魯棒性差、可用路標有限等缺點，且大部分地圖匹配技術(shù)主要修正沿車道方向的縱向定位誤差，而對相對于車道的橫向定位研究還較少[6]。實際上，橫向定位精度對車輛的行駛安全起著至關重要的作用，如超車和并道過程中的 2車橫向間距一般不超過1 m?？紤]到車輛沿車道行駛時，行駛軌跡基本平行于車道線，且與車道線距離保持穩(wěn)定，因此可以通過車輛的歷史軌跡推算出車輛的未來行駛軌跡，即航跡。本文將提出1種推算航跡的方法，并融合航跡對車輛進行橫向定位。

在室內(nèi)定位領域，超寬帶（ultra wideband，UWB）技術(shù)作為 1種無線電技術(shù)，將信號傳輸時間轉(zhuǎn)換為距離信息，可以實現(xiàn)厘米級的定位精度，但主流的基于到達時間（time of arrival，TOA）和基于達到時間差（time difference of arrival，TDOA）的定位技術(shù)都至少需要設置 3個基站，硬件成本較高且系統(tǒng)復雜[7]。在立交橋、樹蔭遮擋等復雜城市道路環(huán)境下，GNSS定位精度嚴重下降，本文將利用 UWB技術(shù)輔助 GPS/INS組合定位系統(tǒng)，既降低成本又實現(xiàn)高精度的室外定位。

綜上所述，本文在高精度車道線數(shù)據(jù)庫和低精度GPS/INS組合定位的基礎上提出1種融合UWB測距信息的室外高精度定位算法。以 GPS/INS組合定位信號作為穩(wěn)定原始輸出，針對沿車道行駛和路口轉(zhuǎn)彎 2種情況分別提出單基站航跡匹配和擴展卡爾曼濾波（extended Kalman filter，EKF）組合定位算法。

1 定位算法方案與框架

UWB是1種無載波通信技術(shù)，利用微秒至納秒級的非正弦波窄脈沖傳輸數(shù)據(jù)，可達到厘米級的測距精度，并且還具有處理增益高、輸出功率低、抗干擾能力強、傳輸距離遠等特點，因此可將其應用于室外定位中。融合UWB測距信息的室外高精度定位算法方案如圖1所示。

圖1 定位技術(shù)方案

為了保證穩(wěn)定輸出，將 GPS/INS組合定位信號作為原始輸出，其中包括位置、速度和航向角。因為車輛在路口轉(zhuǎn)彎時不存在起引導作用的車道線，因此其定位方案與沿車道行駛時有所不同。實車實驗中的航向角變化率如圖2所示。

圖2 航向角變化率

由實驗結(jié)果可知，當車輛沿車道行駛時，大部分行駛時間內(nèi)航向角變化率穩(wěn)定在2(°)/s以內(nèi)，因此可將其作為判斷車輛行駛狀態(tài)的基準。當航向角保持穩(wěn)定或微小變化時，可判定車輛處于沿車道行駛階段，通過車輛行駛歷史軌跡計算車輛在車道上的橫向偏移量，根據(jù)車道線數(shù)據(jù)庫確定車輛行駛航跡，最后結(jié)合UWB測距信息進行單基站航跡匹配[8]；當航向角發(fā)生較大變化時，可判定車輛處于路口轉(zhuǎn)彎階段，由于UWB測距信息與車輛的位置存在非線性關系，因此通過EKF結(jié)合原始定位數(shù)據(jù)和UWB測距信息進行組合定位[9]。

另外，為了保證UWB的測距精度，應將UWB基站布置在相對空曠的環(huán)境下，抑制其非視距誤差。

2 單基站航跡匹配

本文根據(jù)UWB的高精度測距特性提出了1種應用于室外定位的單基站航跡匹配算法，主要包括融合橫向位移的航跡推算和融合縱向位移的圓交點求解。

2.1 航跡推算

由駕駛員駕駛習慣可知，沿車道行駛時其行駛軌跡平行于車道線，且與車道線的橫向距離保持穩(wěn)定，因此本算法設計在每個路口放置1個UWB基站，同時在車輛頂部安裝UWB移動站，通過計算車輛在進入車道后穩(wěn)定行駛 1段距離的平均橫向距離推算出未來行駛的航跡。

為了提高航跡推算的簡便性和精確性，根據(jù)行駛策略將處于交叉路口的車輛分為 2種情況：直行（如路徑1）和路口轉(zhuǎn)彎（如路徑2），如圖3所示。由 UWB基站的經(jīng)緯度坐標與車道線數(shù)據(jù)庫坐標點集計算UWB基站與車道線之間的距離 d2。

圖3 路口行駛策略

2.1.1 直行時的航跡推算

以圖3中路徑1為例，車輛直行通過路口時，車輛進入推算區(qū)域 1后穩(wěn)定行駛，其 UWB基站與車輛 UWB移動站之間的距離特征曲線如圖 4所示?？扇【嚯x特征曲線的最低點作為行駛航跡與 UWB基站之間的距離 d1，則行駛航跡與車道中心線之間的距離為d=d2-d1（設定車輛在車道中心線以右時d為正值）。最后將車道中心線向右橫向平移距離d，即可得到行駛航跡，即路徑1（虛線部分）。

圖4 直行時距離特征曲線

2.1.2 路口轉(zhuǎn)彎后的航跡推算

以圖3中路徑2為例，車輛轉(zhuǎn)彎通過路口后，車輛進入推算區(qū)域2后穩(wěn)定行駛。此時，UWB基站、行駛航跡和車輛之間的幾何關系可以簡化為圖5所示幾何關系。

圖5 幾何關系示意圖

根據(jù)勾股定理可得

式中：L(t)為t時刻UWB基站與車輛的距離；d1為UWB基站與行駛航跡的距離；l(t)為t時刻車輛與UWB基站在行駛航跡上的投影點的距離；t為行駛時間。

由式（2）可得UWB基站與行駛航跡的距離為

根據(jù) d1可求得行駛航跡與車道中心線之間的距離為d=d2-d1。最后將車道中心線向右橫向平移距離 d，即可得到行駛航跡，即路徑 2（虛線部分）。

2.2 圓交點求解

車輛確定行駛航跡后，即可通過圓交點求解確定其縱向位移。如圖2所示，在t時刻，以UWB基站坐標(xUWB，yUWB)為圓點，UWB的測距特征L(t)為半徑作圓，與行駛航跡的交點即為車輛位置。聯(lián)立方程為

式中：(x(t)，y(t))為t時刻車輛在行駛航跡上的路點坐標，即車輛定位結(jié)果；{(xm(i)，ym(i))}為高精度車道線數(shù)據(jù)庫坐標點集（i為點集序列），由高精度定位模塊測得；k(i)為車道線在第 i個車道線點處的(xUWB，yUWB)為 UWB基站的路點坐標；hUWB為UWB基站的安裝高度；h(x(t)、y(t))為t時刻UWB移動站在路點(x(t)、y(t))的安裝高度分布函數(shù)，考慮到城市環(huán)境下大部分道路為平整路段，即h(x(t)，y(t))≈常數(shù) h'，h'的值可通過實驗獲得。當車輛通過橋梁或立交橋路段時，道路高程在短時間內(nèi)發(fā)生變化，因此可利用 INS解算車輛高程上的變化 h"，此時 h(x(t)，y(t))≈h'+ h"。

通常情況下，上述方法將求得多個圓交點。根據(jù) d的正負性判斷車輛在車道中心線的左側(cè)還是右側(cè)。在匹配的初始化階段，將靠近 GPS/INS定位坐標的交點作為匹配起始點；在后續(xù)的匹配過程中，則采用與上1次匹配位置最近的交點。

3 EKF組合定位算法

當車輛路口轉(zhuǎn)彎時，不存在起引導作用的車道線，這就需要利用GPS/INS組合定位原始輸出和 UWB的測距信息進行數(shù)據(jù)融合，獲得精確的定位信息?？紤]到UWB的測距信息 L (k)=（為了表述方便，和車輛位置(x(t)，y(t))成非線性關系，且其相應的雅可比矩陣較易求得，因此采用計算量較小的EKF濾波進行參數(shù)估計。EKF濾波算法建立在線性卡爾曼濾波的基礎上，其核心思想為：對一般的非線性系統(tǒng)，融合濾波值X ?(k)將非線性函數(shù) f(*)和 p(*)展開成泰勒級數(shù)并略去 2階及以上項，得到 1個近似的線性化模型，然后利用線性卡爾曼濾波完成對目標的濾波估計[10]。

根據(jù)車輛運動學分析，取狀態(tài)量 X(k)=[x(k)，?(k)，y(k)，?(k)]T，采用 CV 模型，則系統(tǒng)狀態(tài)方程為

式中：x(k)和y(k)為k時刻車輛在東向和北向的位置；? (k)和 ? (k)為 k時刻車輛在東向和北向的速度；T為UWB采樣周期；w(k)為k時刻的系統(tǒng)噪聲，是均值為零的高斯白噪聲，其對應的協(xié)方差矩陣為Q，其值可由實驗確定。

以 UWB測距信息 L(k)、車輛的位置(x(t)，y(t))和速度(? (t)，? (t))作為觀測量，則觀測方程為

式中，v(k)=[vL(k)，vx(k)，v?(k)，vy(k)，v?(k)]T為 k 時刻的測量噪聲，是均值為零的高斯白噪聲。其中vL(k)為 UWB 的測距誤差，vx(k)、v?(k)、vy(k)和 v?(k)為GPS/INS組合定位系統(tǒng)的測量誤差。v(k)對應的協(xié)方差矩陣為R，其值可由實驗確定。

由式（6）可知，系統(tǒng)觀測方程為非線性方程，根據(jù)EKF的泰勒（Taylor）級數(shù)展開方法，求得對應的雅可比矩陣為

式中I4為4階單位矩陣。

最終可將觀測矩陣簡化為線性的形式，即

式中：X(k) 和X (k)為 k時刻狀態(tài)量的濾波值和預測值； P?(k)和P(k~)為 k時刻誤差協(xié)方差矩陣的濾波值和預測值；K(k)為 k時刻濾波增益矩陣。

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

為了驗證本文提出的定位算法的合理性及其高定位精度，在4個路口各安裝1個UWB基站且安裝高度與車載 UWB移動站一致，圍繞同濟大學機械學院進行實車實驗。其中，GPS接收機為 u-blox LEA-6H，MEMS傳感器為 MPU6050，UWB模塊為DW1000。實驗時以RT3000高精度定位模塊的定位數(shù)據(jù)作為車輛的參考定位軌跡，進而比較融合本文定位算法的定位軌跡與標定軌跡的偏差。

圖6和圖7分別為GPS/INS組合定位算法和本文定位算法的橫向和縱向誤差曲線。GPS/INS組合定位算法的橫向誤差和縱向誤差平均值分別為1.07和 4.73 m。而本文定位算法大部分橫向誤差和縱向誤差均在1 m以內(nèi)，且平均誤差分別為0.24和0.4 m，滿足智能駕駛中的分米級高精度定位要求。但在彎道處有較大的偏差，這是因為在沿車道行駛時，車輛的定位精度高度依賴于UWB的厘米級測距精度和高精度車道線數(shù)據(jù)庫，所以最終的精度也能保證在40 cm以下。而在鄰近彎道處車輛的定位精度由低精度的 GPS/INS組合定位信號和UWB模塊共同決定，因此相對于沿車道行駛的定位精度較低。

圖6 GPS/INS組合定位算法曲線

圖7 本文定位算法誤差曲線

實車實驗軌跡如圖8所示，其局部放大圖如圖 9所示。GPS/INS組合定位算法與標定軌跡存在較大偏差，融合UWB測距信息的室外高精度定位軌跡與軌跡基本重合，證明了本文定位算法的優(yōu)越性和可靠性。

圖8 實車實驗軌跡結(jié)果

圖9 實車實驗軌跡結(jié)果局部放大圖

5 結(jié)束語

本文在 GPS/INS組合定位和 UWB技術(shù)的基礎上提出了 1種融合 UWB測距信息的室外高精度定位算法。在沿車道線行駛情況下，綜合駕駛員的駕駛習慣和 UWB的高精度測距特性提出了單基站航跡匹配；在彎道情況下，利用 EKF融合GPS/INS組合定位原始輸出和UWB的測距信息。實車實驗結(jié)果表明，該算法準確可靠，能廣泛應用于智能駕駛的導航定位中。