盛樹軒，荊崇波，蔣朝陽

（北京理工大學(xué)機(jī)械與車輛學(xué)院，北京 100081）

前言

智能運(yùn)輸系統(tǒng)因其在改善道路安全和提高交通運(yùn)行效率方面的巨大潛力而受到廣泛關(guān)注，車輛定位是其中的一項(xiàng)關(guān)鍵技術(shù)。目前，全球?qū)Ш叫l(wèi)星系統(tǒng)（global navigation satellite system，GNSS）是應(yīng)用最廣泛的車輛定位工具，它能提供車輛的絕對位置信息，但易受到環(huán)境干擾且定位頻率低。慣性測量單元（inertial measurement unit，IMU）能高頻率提供相對定位信息，但定位結(jié)果存在累計(jì)誤差，利用二者互補(bǔ)性的組合導(dǎo)航方法得到了廣泛的研究與應(yīng)用。然而在城市峽谷等環(huán)境下，由于多路徑和非視距等原因?qū)е翯NSS定位精度低，IMU得不到及時(shí)的校正時(shí)，算法很快失效。隨著通信技術(shù)的發(fā)展，V2X（vehicle-to-everything）協(xié)同定位方法提供了一種新的解決思路。Rohani等提出了一種集中式車-車（V2V）協(xié)同定位方法，自車與周圍車輛構(gòu)成一個(gè)車載自組織網(wǎng)絡(luò)（vehicular ad-hoc network ，VANET），VANET 中的車輛之間可互相通信與測距，利用擴(kuò)展卡爾曼濾波融合車輛各自的GNSS 測量和車間距離測量，仿真結(jié)果表明可獲得比單純GNSS 測量更優(yōu)的定位結(jié)果。Hoang 等提出了類似的方法并采用超寬帶（ultra wideband，UWB）進(jìn)行車輛間距離測量，通過實(shí)驗(yàn)測試了UWB 測距的可行性。Alam 等進(jìn)一步考慮了IMU 測量，并提出通過交換GNSS 多普勒頻移數(shù)據(jù)來計(jì)算車輛之間的距離，取代測距傳感器的直接測量，但利用GNSS 多普勒頻移數(shù)據(jù)間接獲取車輛之間的距離是不可靠的，誤差有時(shí)會(huì)超過1 m，這對于協(xié)同定位來說不可接受。

V2V 協(xié)同定位方法存在的不足限制了其應(yīng)用。首先，V2V 協(xié)同定位的效果很大程度取決于VANET中車輛先驗(yàn)位置估計(jì)的準(zhǔn)確性，如果協(xié)同車輛位置估計(jì)的誤差很大，自車與其協(xié)同后反而導(dǎo)致定位精度降低。其次，由于車輛運(yùn)動(dòng)受到道路和交通規(guī)則的強(qiáng)約束，而道路的縱向長度遠(yuǎn)遠(yuǎn)大于橫向車道寬度（例如3.7 m），VANET 拓?fù)錁?gòu)型通常沿著與道路共線的方向變形，導(dǎo)致與道路垂直方向的誤差持續(xù)變大并在網(wǎng)絡(luò)中傳播，進(jìn)一步惡化協(xié)同定位的效果。

一些學(xué)者考慮在V2V 的基礎(chǔ)上引入基礎(chǔ)設(shè)施輔助定位并挖掘VANET 中更加豐富的協(xié)作信息，如相對角度和相對速度。Hoang 等提出一種基于粒子濾波的數(shù)據(jù)融合框架，利用IMU 和輪速計(jì)測量進(jìn)行航跡推算，然后和車間距離測量、車輛到基礎(chǔ)設(shè)施距離測量共同進(jìn)行濾波得到最終的定位結(jié)果，該方法須部署多個(gè)路側(cè)單元。Zhu 等采用UWB 作為測距傳感器，部署在路側(cè)的多個(gè)UWB 跟車載UWB 進(jìn)行測距，基于三點(diǎn)定位法得到車輛的位置，用來提高GNSS受限區(qū)域內(nèi)的定位精度。Zhang等提出一種V2I 協(xié)同定位方法，利用IMU 進(jìn)行航跡推算，同時(shí)估計(jì)車輛與RSU 之間的相對距離和相對角度，利用加權(quán)最小二乘法計(jì)算車輛的位置，但僅進(jìn)行了仿真驗(yàn)證，沒有給出可以實(shí)際應(yīng)用的角度傳感器。Fan等在考慮距離測量的基礎(chǔ)上，進(jìn)一步考慮VANET中車輛間相對速度的測量，將協(xié)同定位看作一個(gè)非線性優(yōu)化問題，借助擬牛頓算法進(jìn)行求解，同樣只進(jìn)行了數(shù)值仿真來驗(yàn)證方法的有效性。

綜上所述，目前大多數(shù)協(xié)同定位方法都處于仿真模擬階段，仍存在一些不足而限制了其實(shí)際應(yīng)用，包括：①實(shí)際應(yīng)用過程中，相對角度和相對速度等信息難以準(zhǔn)確測量；②密集部署RSU 導(dǎo)致成本過高；③一些可用于車輛定位的交通語義信息未能得到充分利用。針對這些不足，本文中提出了一種視覺與單RSU 輔助的車輛定位方法，利用相機(jī)觀測車輛到車道線的橫向距離，利用單RSU 與車輛進(jìn)行測距與通信，結(jié)合車載GNSS、IMU 測量，借助誤差狀態(tài)卡爾曼濾波（error state Kalman filtering，ESKF）算法融合多傳感器信息得到車輛位姿。該方法的特點(diǎn)如下：

（1）只須部署單個(gè)RSU，成本低。

（2）利用RSU 測距信息降低縱向定位誤差，利用車輛到車道線的橫向距離觀測降低橫向定位誤差，二者優(yōu)勢互補(bǔ)，取得水平定位誤差均方根小于10 cm的定位精度。

（3）其有效性得到實(shí)車實(shí)驗(yàn)的驗(yàn)證。

1 方法概述

本文中提出的車輛定位方法基于這樣一個(gè)事實(shí)：IMU 產(chǎn)生不易受環(huán)境干擾的帶有噪聲的測量且很少出現(xiàn)離群值，但存在累計(jì)定位誤差，若能利用GNSS等其他傳感器觀測對累計(jì)誤差進(jìn)行修正，即可得到比較準(zhǔn)確的定位結(jié)果。但GNSS 在城市峽谷等環(huán)境下定位精度低，無法對IMU 累計(jì)誤差起到有效修正，因此，本文進(jìn)一步考慮視覺與單RSU 觀測輔助，并采用ESKF 算法融合IMU、GNSS、RSU 和相機(jī)觀測信息得到車輛位姿。

ESKF算法將系統(tǒng)狀態(tài)分為真實(shí)狀態(tài)、名義狀態(tài)和誤差狀態(tài)。其中，不考慮傳感器測量噪聲和系統(tǒng)擾動(dòng)的情況下得到的系統(tǒng)狀態(tài)稱為名義狀態(tài)，真實(shí)狀態(tài)與名義狀態(tài)之間的差值為誤差狀態(tài)。算法運(yùn)行時(shí)，名義狀態(tài)預(yù)測模塊根據(jù)IMU 測量和遞推方程進(jìn)行名義狀態(tài)（包括姿態(tài)、速度等）預(yù)測，誤差狀態(tài)預(yù)測模塊根據(jù)誤差狀態(tài)預(yù)測方程對誤差狀態(tài)進(jìn)行預(yù)測（包括姿態(tài)誤差、速度誤差等），二者并行。當(dāng)GNSS、RSU 和相機(jī)觀測可用時(shí)，根據(jù)觀測值基于卡爾曼濾波算法對誤差狀態(tài)估計(jì)進(jìn)行修正，然后將修正后的誤差狀態(tài)注入到名義狀態(tài)中，修正后的名義狀態(tài)作為定位結(jié)果輸出，當(dāng)其他傳感器不可用時(shí)直接輸出名義狀態(tài)預(yù)測。由于利用其他傳感器觀測對IMU累計(jì)誤差進(jìn)行了周期性修正，故可取得良好的定位結(jié)果。方法框圖如圖1所示。

圖1 定位方法框架圖

2 視覺與單RSU輔助的定位方法

首先介紹如何基于視覺觀測車輛到車道線的橫向距離以及RSU 與車載單元（on board unit，OBU）之間的測距過程，然后給出利用ESKF 算法融合多傳感器信息得到車輛位姿的推導(dǎo)過程。

2.1 基于視覺的橫向距離觀測

相機(jī)面向前方安裝在車輛頂部，位于車輛縱向中心面中且向下傾斜朝向路面，如圖2 所示。橫向距離觀測從閾值化處理開始，然后進(jìn)行逆透視變換，并使用滑動(dòng)窗口提取車道線對應(yīng)的像素點(diǎn)坐標(biāo)，使用多項(xiàng)式擬合出車道線方程，最后根據(jù)標(biāo)定參數(shù)計(jì)算出車輛到車道線的橫向距離。

圖2 車載單目相機(jī)觀測橫向距離示意圖

（1）閾值化處理 采用梯度閾和顏色閾相結(jié)合的方法進(jìn)行閾值化處理。由于車道線沿著圖像縱向分布，故選取Sobel 算子進(jìn)行橫向梯度檢測，閾值的下限為10，閾值的上限為50。我國公路上的車道線分為白色和黃色兩種，考慮到色相-飽和度-亮度（hue-saturation-lightness，HSL）顏色空間下的L通道和Lab 顏色空間下的b 通道分別對白色和黃色敏感，因此在HSL 顏色空間下的L 通道處理白色車道線，在Lab 顏色空間下的b 通道處理黃色車道線，閾值的下限皆為220，上限皆為255。最后根據(jù)式（1）將閾值化處理后的二值化圖像進(jìn)行合并。

式中：(，)為閾值化處理后的圖像；(，)為原始圖像；(，)為像素點(diǎn)坐標(biāo)；為設(shè)定的梯度閾值區(qū)間和顏色閾值區(qū)間的交集。圖像閾值化處理后如圖3（b）所示。

（2）車道線提取 對經(jīng)過閾值化處理的圖片進(jìn)行逆透視變換，使車道線以平行的方式呈現(xiàn)在鳥瞰圖中。經(jīng)過逆透視變換后得到尺寸為×的圖像，其中坐標(biāo)為(，)的像素點(diǎn)的灰度值用(，)表示，對每一列像素點(diǎn)的灰度值進(jìn)行求和計(jì)算：

采用滑動(dòng)窗口法從圖像底端向上提取車道線像素點(diǎn)集。選取矩形窗口，窗口的寬設(shè)為100，高設(shè)為60。首先將使sum取最大值的作為窗口的滑動(dòng)起始點(diǎn)，記錄滑動(dòng)窗口內(nèi)的非零值像素點(diǎn)（即車道線）坐標(biāo)，并統(tǒng)計(jì)像素點(diǎn)橫坐標(biāo)的平均值作為滑動(dòng)窗口下一階段的起始點(diǎn)，重復(fù)上一階段操作直至到達(dá)圖像頂端。車道線提取結(jié)果如圖3（c）所示。

（3）車道線擬合 對滑動(dòng)窗口提取到的像素點(diǎn)坐標(biāo)進(jìn)行最小二乘多項(xiàng)式擬合：

式中：、和為圖像坐標(biāo)中多項(xiàng)式的系數(shù)，其中原點(diǎn)位于圖像的左上角；軸指向右側(cè)，軸指向下方。車道線擬合結(jié)果如圖3（d）所示。

圖3 基于視覺的橫向距離觀測

（4）橫向距離計(jì)算 車道線擬合后可得到以像素為單位的橫向距離，然后乘以一個(gè)從像素距離到現(xiàn)實(shí)距離的換算系數(shù)，得到以米為單位的橫向距離。換算系數(shù)的計(jì)算公式為

式中：為相機(jī)中心到車道線的橫向距離，m；為從圖像中心到車道線橫跨圖像最底行的像素?cái)?shù)，選擇最底行是因?yàn)樗哂凶罡叩姆直媛省?/p>

2.2 RSU測距

本文中重點(diǎn)關(guān)注如何利用RSU 提供的測距信息設(shè)計(jì)定位算法以提高車輛的定位精度，不關(guān)心RSU采用何種通信協(xié)議等具體的硬件實(shí)現(xiàn)細(xì)節(jié)。因此僅簡要描述RSU 與OBU 之間的測距過程，如圖4所示。RSU 部署在路側(cè)，OBU 向RSU 廣播請求信號，RSU接收請求信號并進(jìn)行相關(guān)處理，然后RSU向OBU 發(fā)送包括ID、位置坐標(biāo)、請求信號接收時(shí)刻和數(shù)據(jù)包發(fā)出時(shí)刻等在內(nèi)的數(shù)據(jù)包。OBU根據(jù)接收到的數(shù)據(jù)包的信息，基于雙向測量的方法計(jì)算得出與RSU之間的距離：

圖4 RSU測距示意圖

式中：為OBU 發(fā)出請求的時(shí)刻；、分別為RSU 接收請求和發(fā)出數(shù)據(jù)包的時(shí)刻；為OBU 接收到數(shù)據(jù)包的時(shí)刻；為信號傳播速度。

2.3 基于ESKF算法的多傳感器信息融合

利用ESKF 算法融合IMU、GNSS、RSU 和相機(jī)觀測信息得到車輛位姿的推導(dǎo)過程。

名義狀態(tài)的遞推方程為

式中：為地球自轉(zhuǎn)角速率，其值常取7.2921151467 × 10rad/s；、分別為所在位置的緯度、高度；v、v分別為導(dǎo)航坐標(biāo)系下東向和北向的速度；R、R的表達(dá)式見式（12）和式（13）。

式中：R和分別為地球被近似描述為參考旋轉(zhuǎn)橢球體的長半徑和扁率，按WGS-84標(biāo)準(zhǔn)，地球長半徑等于6 378 137 m，扁率等于1/298.257。

定義誤差狀態(tài)變量為

假設(shè)失準(zhǔn)角很小，且不考慮地球重力模型誤差，則誤差狀態(tài)微分方程可寫為

在上述誤差狀態(tài)微分方程的基礎(chǔ)上，考慮系統(tǒng)過程噪聲，得到誤差狀態(tài)方程為

1) 通過Simulink對簡化的起重機(jī)起升機(jī)構(gòu)的模型進(jìn)行系統(tǒng)仿真，得出了該系統(tǒng)的仿真結(jié)構(gòu)圖，為系統(tǒng)的仿真分析與研究奠定了基礎(chǔ)。

其中

設(shè)角度隨機(jī)游走為n，速度隨機(jī)游走為n，則系統(tǒng)噪聲協(xié)方差矩陣定義為

式中：- 1、為相鄰的兩個(gè)時(shí)刻；△為兩個(gè)時(shí)刻的間隔；為單位矩陣，維度與F相同。

2.3.2 誤差狀態(tài)修正

利用GNSS、RSU 和相機(jī)觀測信息對誤差狀態(tài)的先驗(yàn)估計(jì)進(jìn)行修正。要將車輛到車道線的橫向距離觀測納入濾波算法中，必須根據(jù)當(dāng)前車輛位置找到車道線上的相關(guān)路點(diǎn)，本文假設(shè)車輛已知所處車道和關(guān)聯(lián)車道線路點(diǎn)地圖，各路點(diǎn)坐標(biāo)事先已知，根據(jù)名義位置估計(jì)在路點(diǎn)坐標(biāo)集內(nèi)進(jìn)行搜索，找到歐式距離最近的路點(diǎn)作為相關(guān)路點(diǎn)，如圖5所示。

圖5 相關(guān)路點(diǎn)查找示意圖

考慮GNSS、RSU 和相機(jī)觀測信息，得到誤差狀態(tài)的觀測方程為

其中

其中

對系統(tǒng)觀測噪聲協(xié)方差矩陣建模為

式中σ、σ、σ、σ分別為GNSS 速度觀測誤差、GNSS位置觀測誤差、RSU 測距誤差和視覺橫向距離觀測誤差，根據(jù)實(shí)驗(yàn)分別取值為0.5 m/s、5 m、0.1 m、0.1 m。

式中：K為時(shí)刻的卡爾曼矩陣；為單位矩陣，維度與KH一致。

2.3.3 誤差狀態(tài)注入與重置

將經(jīng)過濾波后的誤差狀態(tài)注入到名義狀態(tài)中，對名義狀態(tài)進(jìn)行修正：

誤差狀態(tài)注入完成后，將誤差狀態(tài)根據(jù)式（32）進(jìn)行重置，并用于下一次估計(jì)。

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

3.1 實(shí)驗(yàn)設(shè)備與測試場景

為驗(yàn)證所提出方法的有效性，進(jìn)行實(shí)車測試。選用UWB 傳感器來實(shí)現(xiàn)RSU 與OBU 之間的測距和通信功能。UWB 分為錨和標(biāo)簽兩種角色，其中標(biāo)簽安裝在車輛上隨車輛運(yùn)動(dòng)充當(dāng)OBU，錨部署在路側(cè)充當(dāng)RSU，二者可以實(shí)時(shí)測距與通信。具體地說，測試車輛配備了Novatel PwrPak7D-E1 高精度慣性導(dǎo)航系統(tǒng)、司南M300 衛(wèi)星導(dǎo)航系統(tǒng)、XSENS MTi670 IMU、Nooploop UWB、大恒MER-231-41U3C 彩色相機(jī)和筆記本電腦。PwrPak7D-E1接收千尋公司基站發(fā)送的RTK 差分?jǐn)?shù)據(jù)，其輸出被認(rèn)為是車輛行駛過程中定位結(jié)果的真實(shí)值。所有傳感器通過串口與電腦相連。所有傳感器的數(shù)據(jù)在機(jī)器人操作系統(tǒng)（robot operating system，ROS）框架下進(jìn)行采集，不同傳感器之間的時(shí)間同步也通過ROS 提供的時(shí)間戳實(shí)現(xiàn)。車輛測試平臺(tái)如圖6所示。

圖6 車輛測試平臺(tái)

選擇典型校園場景進(jìn)行測試，如圖7 所示。實(shí)車實(shí)驗(yàn)路線包含基本道路要素和場景要素，能代表大部分道路，同時(shí)道路兩側(cè)有許多樹和高樓，給車輛定位帶來挑戰(zhàn)。車輛由靜止開始，沿著道路由南向北行駛，此時(shí)只有GNSS 和IMU 可用，行駛一段距離左轉(zhuǎn)進(jìn)入測試區(qū)域A，此時(shí)GNSS、IMU 和RSU 可用，繼續(xù)向前行駛進(jìn)入?yún)^(qū)域B，GNSS、IMU、RSU 和視覺橫向距離觀測均可用。

圖7 測試場景說明

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

將考慮GNSS、IMU 信息的方法稱為GNSS/IMU，將考慮GNSS、IMU 和RSU 測距信息的方法稱為GNSS/IMU/DIS，將考慮GNSS、IMU、RSU 測距和視覺橫向距離觀測信息的方法稱為GNSS/IMU/DIS/CAM。

圖8 給出3 種方法在“NED”坐標(biāo)系下對應(yīng)的軌跡和RSU 的位置。開始時(shí)只有GNSS、IMU 可用，3種方法對應(yīng)的軌跡是重合的。當(dāng)車輛駛過一個(gè)路口后，轉(zhuǎn)為由東向西行駛，此時(shí)由于道路兩側(cè)的樹木及高樓的遮擋，GNSS測量開始變差，GNSS/IMU 方法對應(yīng)軌跡與真實(shí)軌跡明顯分離。駛?cè)雲(yún)^(qū)域A 后，RSU距離測量可用，在RSU 測距信息輔助下車輛軌跡很快接近真實(shí)值。車輛繼續(xù)向前行駛，逐漸遠(yuǎn)離RSU，GNSS/IMU/DIS 方法對應(yīng)的軌跡逐漸偏離真實(shí)值，而GNSS/IMU/DIS/CAM 方法對應(yīng)的軌跡始終接近真實(shí)值。

圖8 “NED”坐標(biāo)系下的車輛軌跡圖

圖9和圖10 分別示出3 種方法在“NED”坐標(biāo)系下對應(yīng)的北向位置和東向位置隨時(shí)間的變化情況。GNSS/IMU/DIS 方法在RSU 測距信息的輔助下可修正北向和東向位置，且可以看出隨著離RSU 的距離逐漸增加，對北向位置的修正效果逐漸降低，而對東向位置始終能顯著修正。原因是車輛行駛時(shí)受到道路約束，而道路的縱向長度遠(yuǎn)遠(yuǎn)大于道路的寬度（單車道寬度一般為3.7 m）。當(dāng)采用布置在路側(cè)的單RSU 進(jìn)行測距時(shí)，隨車輛逐漸遠(yuǎn)離RSU，RSU 與車輛之間距離測量的橫向（本例中近似為北向）分量在整個(gè)距離測量中所占的比例逐漸降低，如圖11 所示。時(shí)刻相比于時(shí)刻，RSU距離測量的橫向分量在整個(gè)距離測量中所占比例遠(yuǎn)小于縱向分量所占比例，距離測量中所蘊(yùn)含的橫向信息逐漸被縱向信息稀釋，導(dǎo)致GNSS/IMU/DIS 方法對車輛橫向位置的修正作用隨遠(yuǎn)離RSU 逐漸降低。GNSS/IMU/DIS/CAM 方法基于視覺觀測車輛到車道線的橫向距離，結(jié)合車道線路點(diǎn)坐標(biāo)對車輛橫向位置進(jìn)行估計(jì)，始終對車輛橫向位置進(jìn)行有效修正，彌補(bǔ)了GNSS/IMU/DIS 方法的不足。

圖9 “NED”坐標(biāo)系下車輛北向位置變化圖

圖10 “NED”坐標(biāo)系下車輛東向位置變化圖

圖11 單RSU測距橫向分量稀釋示意圖

圖12（a）和圖12（b）分別示出車輛北向位置誤差和東向位置誤差變化圖。由圖可見，開始時(shí)只有GNSS、IMU 可用，誤差是相同的。從15 s 開始RSU測距可用，在測距信息的輔助下，北向和東向位置誤差均明顯降低。從21 s 開始，視覺橫向距離觀測可用，在橫向距離觀測的輔助下，GNSS/IMU/DIS/CAM方法對應(yīng)的北向位置誤差始終顯著降低，而GNSS/IMU/DIS 方法由于RSU 測距信息的橫向分量被不斷稀釋，對應(yīng)的北向位置誤差逐漸增大。這表明單RSU測距信息主要降低縱向（本例中近似東向）定位誤差，橫向距離觀測可以有效降低橫向（本例中近似北向）定位誤差，二者優(yōu)勢互補(bǔ)。

進(jìn)一步對比不同橫向距離觀測精度對定位結(jié)果的影響。在橫向距離觀測真值的基礎(chǔ)上添加均值為0、標(biāo)準(zhǔn)差為0.2 的高斯噪聲生成模擬的橫向距離觀測，實(shí)際橫向距離觀測、橫向距離觀測真值和模擬橫向距離觀測三者的對比如圖13 所示?？梢钥闯?，模擬橫向距離觀測存在很大誤差，用于表征橫向距離觀測特別不準(zhǔn)確的情況。

圖13 橫向距離觀測對比圖

3 種不同橫向距離觀測精度對應(yīng)的位置誤差如圖12（a）和圖12（b）所示，CAM、CAM（真值）和CAM（Std=0.2）分別代表橫向距離的實(shí)際觀測值、真值和模擬值，并用以區(qū)分不同橫向距離觀測精度對應(yīng)的定位誤差?？梢钥闯觯?dāng)橫向距離觀測為真值，車輛的橫向定位誤差最小，幾乎為零。當(dāng)橫向距離觀測為視覺系統(tǒng)實(shí)際測得值，橫向距離觀測存在較小的誤差，車輛的橫向定位誤差略有增加。當(dāng)橫向距離觀測為模擬值，模擬的橫向距離觀測存在較大誤差，其對車輛橫向定位誤差的降低作用減弱，但相比于不考慮橫向距離觀測信息的GNSS/IMU、GNSS/IMU/DIS 方法，仍能有效降低橫向定位誤差，彌補(bǔ)單RSU在消除橫向定位誤差方面的不足。不同精度的橫向距離觀測對應(yīng)的縱向定位誤差則沒有明顯區(qū)別。

圖12 定位誤差圖

表1中匯總了GNSS、IMU、RSU 和視覺橫向距離觀測均可用區(qū)域內(nèi)3 種定位方法對應(yīng)的北向、東向位置誤差和總的水平位置誤差的平均絕對誤差（mean absolute error，MAE）以 及 均 方 根 差（root mean square error，RMSE）。以RMSE 指標(biāo)為例，在北向上，GNSS/IMU/DIS/CAM 方法的誤差比GNSS/IMU方法降低了95.9%，比GNSS/IMU/DIS 方法降低了81.9%。在東向上，GNSS/IMU/DIS 方法和GNSS/IMU/DIS/CAM 方法對應(yīng)的誤差均小于10 cm，后者的誤差比GNSS/IMU 方法降低了95.5%。同時(shí)，GNSS/IMU/DIS/CAM 方法的水平定位的MAE 和RMSE 誤差均小于10 cm，實(shí)現(xiàn)了厘米級定位。

表1 定位誤差

4 結(jié)論

提出一種視覺與單RSU 輔助的車輛定位方法，利用ESKF算法對GNSS測量、IMU測量、RSU測距和視覺橫向距離觀測信息進(jìn)行融合，實(shí)現(xiàn)對車輛位姿的準(zhǔn)確估計(jì)，并得到了實(shí)車測試的驗(yàn)證。研究結(jié)果表明：

（1）單RSU 測距信息可以有效降低車輛縱向定位誤差，但由于存在測距橫向分量的稀釋問題，對車輛橫向定位誤差的修正作用隨車輛遠(yuǎn)離RSU 逐漸降低。

（2）基于視覺觀測車輛到車道線的橫向距離，可以有效降低車輛橫向定位誤差，彌補(bǔ)單RSU 測距橫向分量稀釋問題，而無須借助密集部署RSU 來彌補(bǔ)。

（3）視覺與單RSU 輔助的定位方法的水平定位的MAE 和RMSE 誤差均小于10 cm，可實(shí)現(xiàn)厘米級定位。

最后需要指出的是，受到實(shí)驗(yàn)條件的限制，僅在校園園區(qū)內(nèi)進(jìn)行測試，今后須在其他道路環(huán)境中開展更廣泛的測試。