視覺與單路側單元輔助的車輛定位方法*

盛樹軒，荊崇波，蔣朝陽

（北京理工大學機械與車輛學院，北京 100081）

前言

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

V2V 協同定位方法存在的不足限制了其應用。首先，V2V 協同定位的效果很大程度取決于VANET中車輛先驗位置估計的準確性，如果協同車輛位置估計的誤差很大，自車與其協同后反而導致定位精度降低。其次，由于車輛運動受到道路和交通規則的強約束，而道路的縱向長度遠遠大于橫向車道寬度（例如3.7 m），VANET 拓撲構型通常沿著與道路共線的方向變形，導致與道路垂直方向的誤差持續變大并在網絡中傳播，進一步惡化協同定位的效果。

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

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

（1）只須部署單個RSU，成本低。

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

（3）其有效性得到實車實驗的驗證。

1 方法概述

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

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

圖1 定位方法框架圖

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

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

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

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

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

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

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

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

采用滑動窗口法從圖像底端向上提取車道線像素點集。選取矩形窗口，窗口的寬設為100，高設為60。首先將使sum取最大值的作為窗口的滑動起始點，記錄滑動窗口內的非零值像素點（即車道線）坐標，并統計像素點橫坐標的平均值作為滑動窗口下一階段的起始點，重復上一階段操作直至到達圖像頂端。車道線提取結果如圖3（c）所示。

（3）車道線擬合 對滑動窗口提取到的像素點坐標進行最小二乘多項式擬合：

式中：、和為圖像坐標中多項式的系數，其中原點位于圖像的左上角；軸指向右側，軸指向下方。車道線擬合結果如圖3（d）所示。

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

（4）橫向距離計算 車道線擬合后可得到以像素為單位的橫向距離，然后乘以一個從像素距離到現實距離的換算系數，得到以米為單位的橫向距離。換算系數的計算公式為

式中：為相機中心到車道線的橫向距離，m；為從圖像中心到車道線橫跨圖像最底行的像素數，選擇最底行是因為它具有最高的分辨率。

2.2 RSU測距

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

圖4 RSU測距示意圖

式中：為OBU 發出請求的時刻；、分別為RSU 接收請求和發出數據包的時刻；為OBU 接收到數據包的時刻；為信號傳播速度。

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

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

名義狀態的遞推方程為

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

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

定義誤差狀態變量為

假設失準角很小，且不考慮地球重力模型誤差，則誤差狀態微分方程可寫為

在上述誤差狀態微分方程的基礎上，考慮系統過程噪聲，得到誤差狀態方程為

1) 通過Simulink對簡化的起重機起升機構的模型進行系統仿真，得出了該系統的仿真結構圖，為系統的仿真分析與研究奠定了基礎。

其中

設角度隨機游走為n，速度隨機游走為n，則系統噪聲協方差矩陣定義為

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

2.3.2 誤差狀態修正

利用GNSS、RSU 和相機觀測信息對誤差狀態的先驗估計進行修正。要將車輛到車道線的橫向距離觀測納入濾波算法中，必須根據當前車輛位置找到車道線上的相關路點，本文假設車輛已知所處車道和關聯車道線路點地圖，各路點坐標事先已知，根據名義位置估計在路點坐標集內進行搜索，找到歐式距離最近的路點作為相關路點，如圖5所示。

圖5 相關路點查找示意圖

考慮GNSS、RSU 和相機觀測信息，得到誤差狀態的觀測方程為

其中

其中

對系統觀測噪聲協方差矩陣建模為

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

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

2.3.3 誤差狀態注入與重置

將經過濾波后的誤差狀態注入到名義狀態中，對名義狀態進行修正：

誤差狀態注入完成后，將誤差狀態根據式（32）進行重置，并用于下一次估計。

3 實驗與結果分析

3.1 實驗設備與測試場景

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

圖6 車輛測試平臺

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

圖7 測試場景說明

3.2 實驗結果分析

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

圖8 給出3 種方法在“NED”坐標系下對應的軌跡和RSU 的位置。開始時只有GNSS、IMU 可用，3種方法對應的軌跡是重合的。當車輛駛過一個路口后，轉為由東向西行駛，此時由于道路兩側的樹木及高樓的遮擋，GNSS測量開始變差，GNSS/IMU 方法對應軌跡與真實軌跡明顯分離。駛入區域A 后，RSU距離測量可用，在RSU 測距信息輔助下車輛軌跡很快接近真實值。車輛繼續向前行駛，逐漸遠離RSU，GNSS/IMU/DIS 方法對應的軌跡逐漸偏離真實值，而GNSS/IMU/DIS/CAM 方法對應的軌跡始終接近真實值。

圖8 “NED”坐標系下的車輛軌跡圖

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

圖9 “NED”坐標系下車輛北向位置變化圖

圖10 “NED”坐標系下車輛東向位置變化圖

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

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

進一步對比不同橫向距離觀測精度對定位結果的影響。在橫向距離觀測真值的基礎上添加均值為0、標準差為0.2 的高斯噪聲生成模擬的橫向距離觀測，實際橫向距離觀測、橫向距離觀測真值和模擬橫向距離觀測三者的對比如圖13 所示。可以看出，模擬橫向距離觀測存在很大誤差，用于表征橫向距離觀測特別不準確的情況。

圖13 橫向距離觀測對比圖

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

圖12 定位誤差圖

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

表1 定位誤差

4 結論

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

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

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

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

最后需要指出的是，受到實驗條件的限制，僅在校園園區內進行測試，今后須在其他道路環境中開展更廣泛的測試。