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

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

0 引言

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

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

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

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

1 定位算法方案與框架

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

圖1 定位技術方案

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

圖2 航向角變化率

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

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

2 單基站航跡匹配

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

2.1 航跡推算

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

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

圖3 路口行駛策略

2.1.1 直行時的航跡推算

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

圖4 直行時距離特征曲線

2.1.2 路口轉彎后的航跡推算

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

圖5 幾何關系示意圖

根據勾股定理可得

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

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

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

2.2 圓交點求解

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

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

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

3 EKF組合定位算法

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

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

式中：x(k)和y(k)為k時刻車輛在東向和北向的位置；? (k)和 ? (k)為 k時刻車輛在東向和北向的速度；T為UWB采樣周期；w(k)為k時刻的系統噪聲，是均值為零的高斯白噪聲，其對應的協方差矩陣為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組合定位系統的測量誤差。v(k)對應的協方差矩陣為R，其值可由實驗確定。

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

式中I4為4階單位矩陣。

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

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

4 實驗與結果分析

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

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

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

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

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

圖8 實車實驗軌跡結果

圖9 實車實驗軌跡結果局部放大圖

5 結束語

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