周愛國(guó) ，楊思靜 ，沈 勇 ，王嘉立

0 引言

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

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

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

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

1 定位算法方案與框架

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

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

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

圖2 航向角變化率

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

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

2 單基站航跡匹配

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

2.1 航跡推算

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

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

圖3 路口行駛策略

2.1.1 直行時(shí)的航跡推算

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

圖4 直行時(shí)距離特征曲線

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

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

圖5 幾何關(guān)系示意圖

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

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

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

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

2.2 圓交點(diǎn)求解

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

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

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

3 EKF組合定位算法

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

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

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

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

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

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

式中I4為4階單位矩陣。

最終可將觀測(cè)矩陣簡(jiǎn)化為線性的形式，即

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

4 實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

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

圖8 實(shí)車實(shí)驗(yàn)軌跡結(jié)果

圖9 實(shí)車實(shí)驗(yàn)軌跡結(jié)果局部放大圖

5 結(jié)束語

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