基于GPS和視覺(jué)同步定位與建圖的無(wú)人駕駛車輛定位方法研究＊

高揚(yáng) 徐永貴 劉江

（長(zhǎng)安大學(xué)，西安 710054）

主題詞：無(wú)人駕駛車輛 視覺(jué)同步定位與建圖 角點(diǎn)均勻分布策略 GPS

1 前言

定位是在環(huán)境中尋找相對(duì)于參考系的自我位置的任務(wù)[1]，是無(wú)人駕駛汽車感知的重要一環(huán)。GPS 定位技術(shù)是常用的絕對(duì)定位技術(shù)，其定位精度可達(dá)10～15 m[2]，且沒(méi)有累積誤差，但在城市環(huán)境中，其定位精度受到環(huán)境影響，甚至可能無(wú)法定位[3]。盡管實(shí)時(shí)動(dòng)態(tài)載波相位差分技術(shù)能夠在大范圍環(huán)境中提供厘米級(jí)高精度定位[4]，但在GPS 信號(hào)薄弱甚至缺失的環(huán)境下同樣無(wú)法正常工作。同步定位與建圖（Simultaneous Localization and Mapping，SLAM）技術(shù)主要采用地圖匹配實(shí)現(xiàn)定位，自行繪制地圖，不依賴于先驗(yàn)地圖信息，且定位精度可以達(dá)到毫米級(jí)。然而，其需要同時(shí)完成定位與繪圖，計(jì)算量較大，且在大范圍移動(dòng)場(chǎng)景中存在累積誤差[5]。視覺(jué)同步定位與建圖（Visual Simultaneous Localization and Mapping，VSLAM）技術(shù)是SLAM 技術(shù)的一種，依賴視覺(jué)傳感器作為主要感知手段，具有成本低、能實(shí)現(xiàn)三維位姿估計(jì)與建圖等優(yōu)點(diǎn)。

GPS 與SLAM 的定位方法具有各自的局限性，但兩者之間也具備鮮明的互補(bǔ)性。事實(shí)上，GPS 與VSLAM技術(shù)融合定位在許多研究中都有涉及。Markus Schreiber[6]提出一種GPS 與VSLAM 緊耦合的無(wú)人駕駛汽車低成本定位方案，在GPS信號(hào)差的區(qū)域依然能保持系統(tǒng)的魯棒性，但其定位精度不高。Mutz[7]引入GPS 信號(hào)幫助SLAM 算法進(jìn)行閉環(huán)檢測(cè)，減少SLAM 算法的計(jì)算量，但其未融合二者的定位信息，GPS 定位信息利用率低。許豪[8]提出一種GPS 與地圖匹配的組合定位方法，結(jié)合了二者的優(yōu)點(diǎn)，能夠?yàn)闊o(wú)人駕駛汽車在室外環(huán)境提供較高精度的定位，但只適用于提前構(gòu)建好地圖的區(qū)域。Chen[9]等提出一種基于GPS與ORB-SLAM[10]緊耦合的無(wú)人駕駛定位方法，可以實(shí)現(xiàn)無(wú)人駕駛汽車在室外大范圍環(huán)境中的定位，但其在動(dòng)態(tài)環(huán)境中魯棒性不足。

目前，GPS 與VSLAM 系統(tǒng)融合的方案大多對(duì)GPS定位信息利用率低或系統(tǒng)魯棒性不高，進(jìn)而無(wú)法應(yīng)用于無(wú)人駕駛汽車的定位。因此，本文提出一種GPS 與VSLAM 技術(shù)融合的無(wú)人駕駛汽車定位方法，首先基于VINS-Mono 算法提出了改進(jìn)算法VINS-FAST，將已有的Shi-Tomasi 角點(diǎn)檢測(cè)替換為魯棒性更強(qiáng)的FAST 角點(diǎn)。同時(shí)，針對(duì)FAST 角點(diǎn)檢測(cè)所識(shí)別的特征存在集中度較高的問(wèn)題，本文進(jìn)一步提出一種FAST 角點(diǎn)均勻分布策略，使得在紋理弱的區(qū)域也能檢測(cè)到FAST角點(diǎn)，最后提出一種基于GPS與VINS-FAST融合的定位算法。

2 基于VINS-Mono改進(jìn)的VINS-FAST算法

VINS-Mono 是香港科技大學(xué)沈劭劼團(tuán)隊(duì)提出的一種VSLAM 算法[11]，具有在室內(nèi)環(huán)境中定位精度高等優(yōu)點(diǎn)。本文在VINS-Mono的基礎(chǔ)上進(jìn)行改進(jìn)，提出VINSFAST 算法，運(yùn)行框架如圖1 所示，可以輸出相機(jī)的6 自由度位姿和地圖三維點(diǎn)。

圖1 VINS-FAST算法流程

VSLAM 系統(tǒng)中常用的角點(diǎn)包括Shi-Tomasi 角點(diǎn)、尺度不變特征轉(zhuǎn)換（Scale-invariant feature transform，SIFT）和FAST（Features from Accelerated Segment Test）角點(diǎn)。FAST角點(diǎn)檢測(cè)算法于2006年提出，以檢測(cè)速度快著稱[12]。2015年，R.Mur-Artal進(jìn)一步改進(jìn)了FAST角點(diǎn)，添加了尺度和旋轉(zhuǎn)的描述，進(jìn)而使FAST角點(diǎn)具有尺度不變性和方向不變性[10]。由于Shi-Tomasi 角點(diǎn)無(wú)法描述圖像尺度信息，僅對(duì)圖像方向信息進(jìn)行了描述，而FAST 角點(diǎn)同時(shí)描述了尺度信息與方向信息，從而大幅提升了FAST角點(diǎn)在不同圖片中進(jìn)行光流跟蹤時(shí)的魯棒性。因此，本文首先針對(duì)VINS-Mono 算法進(jìn)行改進(jìn)，將其采用的Shi-Tomasi角點(diǎn)替換為魯棒性更強(qiáng)的FAST角點(diǎn)，提出一種VINS-FAST視覺(jué)同步定位與建圖算法，其流程如圖2所示。

圖2 VINS-FAST特征檢測(cè)與追蹤流程

通過(guò)FAST角點(diǎn)檢測(cè)算法檢測(cè)到得的角點(diǎn)容易集聚在紋理、亮度特征變化明顯的區(qū)域。如圖3 所示，圖中類圓形陰影表示檢測(cè)到的FAST 角點(diǎn)。此時(shí)檢測(cè)出的FAST 角點(diǎn)大多聚集在特征明顯的位置，而在紋理弱的區(qū)域幾乎沒(méi)有檢測(cè)到角點(diǎn)。這導(dǎo)致很多FAST角點(diǎn)描述的環(huán)境信息過(guò)于單調(diào)，未能充分代表周圍環(huán)境，不利于后續(xù)匹配、定位等任務(wù)的實(shí)現(xiàn)。

圖3 FAST角點(diǎn)檢測(cè)集中現(xiàn)象

因此，本文提出一種FAST角點(diǎn)均勻分布策略，利用四叉樹(shù)結(jié)構(gòu)改進(jìn)FAST 角點(diǎn)檢測(cè)方法，具體流程如圖4所示，其中，L為圖像金字塔中圖像的層數(shù)，Wima為圖像的寬度，Hima為圖像的高度。該策略在提取FAST 角點(diǎn)時(shí)，需提前設(shè)定FAST 閾值Na，即整張圖像提取FAST 角點(diǎn)的數(shù)量，最后基于四叉樹(shù)均勻，按照Harris 響應(yīng)品質(zhì)的優(yōu)劣選取數(shù)量為Na的FAST角點(diǎn)。

圖4 FAST角點(diǎn)均勻分布策略流程

算法設(shè)計(jì)了2 個(gè)角點(diǎn)檢測(cè)閾值：Vini為FAST 角點(diǎn)檢測(cè)的初始閾值，即每個(gè)格子中的初始角點(diǎn)數(shù)量；Vmin為最小閾值，即每個(gè)格子中的最少角點(diǎn)數(shù)量。如果每個(gè)格子中的角點(diǎn)數(shù)量小于Vini，則認(rèn)為該區(qū)域紋理較弱，無(wú)法檢測(cè)到足夠數(shù)量的角點(diǎn)，此時(shí)，按照Vmin檢測(cè)角點(diǎn)。因此，Vmin的設(shè)置應(yīng)保證在圖片中紋理弱的區(qū)域也能提取到FAST 角點(diǎn)。針對(duì)分辨率為640 像素×480 像素的圖片（如圖3），取每格邊長(zhǎng)P=30 像素。對(duì)圖3 采用FAST 角點(diǎn)均勻分布策略后的角點(diǎn)檢測(cè)結(jié)果如圖5所示。

圖5 FAST角點(diǎn)均勻分布效果

3 基于GPS與VINS-FAST融合的VINS-GPS

為進(jìn)一步消除SLAM算法中累計(jì)誤差的影響，提出一種GPS 與VINS-FAST 算法融合的VINS-GPS 算法。GPS與VINS-FAST融合步驟如圖6所示，將GPS的定位結(jié)果與VINS-FAST經(jīng)過(guò)后端位姿優(yōu)化的定位結(jié)果進(jìn)行融合，最后采用非線性優(yōu)化的方式求解融合后的位姿。

圖6 GPS與VINS-FAST融合框架

假設(shè)已經(jīng)將VINS-FAST 與GPS坐標(biāo)系轉(zhuǎn)換至同一坐標(biāo)系，構(gòu)造非線性優(yōu)化的目標(biāo)函數(shù)：

式中，Rgv、Tgv分別為VINS-FAST坐標(biāo)系到GPS坐標(biāo)系轉(zhuǎn)換的旋轉(zhuǎn)矩陣和平移矩陣；N為在VINS-FAST坐標(biāo)系與GPS 坐標(biāo)系下可用于融合的無(wú)人駕駛汽車位姿估計(jì)結(jié)果數(shù)量；分別為i時(shí)刻無(wú)人駕駛汽車在GPS 坐標(biāo)系和VINS-FAST坐標(biāo)系下的坐標(biāo)。

通過(guò)求解式（1）可以求得Rgv和Tgv，獲得GPS 與VINS-FAST的坐標(biāo)系轉(zhuǎn)換關(guān)系。

本文將融合定位算法VINS-GPS 的優(yōu)化目標(biāo)函數(shù)設(shè)為：

式中，χ=[ξ1,…,ξN]T為待優(yōu)化的變量；Ev(i,j,ki,kj)為VINSFAST 的約束；ki、kj分別為i時(shí)刻和j時(shí)刻VINS-FATS 估計(jì)的位姿；Eg(i)為i時(shí)刻GPS的約束；ξi為i時(shí)刻無(wú)人駕駛汽車的位姿；wg為GPS定位結(jié)果進(jìn)行融合時(shí)的權(quán)重；M、N分別為融合時(shí)VINS-FAST 輸出的定位結(jié)果數(shù)量和GPS定位結(jié)果數(shù)量。

無(wú)人駕駛汽車i時(shí)刻位姿估計(jì)值為：

本文將Ev(i,j,ki,kj)定義為：

式中，z(i,j,ki,kj)為直接由VINS-FAST 得到的i時(shí)刻到j(luò)時(shí)刻無(wú)人駕駛汽車位姿估計(jì)結(jié)果變化量；為i時(shí)刻到j(luò)時(shí)刻無(wú)人駕駛汽車真實(shí)位姿位姿結(jié)果變化量。

沒(méi)有GPS 定位結(jié)果時(shí)，本文設(shè)Eg(i)=0，有GPS 定位結(jié)果時(shí)，Eg(i)定義為：

GPS 定位的幾何精度因子（Geometric Dilution Of Precision，GDOP）是衡量GPS 定位精度的重要參數(shù)，其值越小，表示GPS定位精度越高，通常，該值可由GPS信號(hào)接收機(jī)給出。因此，本文采用wg作為定位結(jié)果融合權(quán)重設(shè)置的依據(jù)：

式中，κ為幾何精度因子。

4 試驗(yàn)結(jié)果分析

為了驗(yàn)證本文所提出算法的有效性，首先在室內(nèi)數(shù)據(jù)集Euroc[13]上驗(yàn)證了VINS-FAST 在室內(nèi)等遮擋環(huán)境的定位效果，并在自動(dòng)駕駛數(shù)據(jù)集KITTI 上驗(yàn)證了VINS-GPS 融合定位算法在室外大范圍環(huán)境下的定位效果[14]。本文所有試驗(yàn)均基于ROS系統(tǒng)kinetic版進(jìn)行，硬件設(shè)備為戴爾游匣G3，處理器為i5-7300HQ。

4.1 VINS-FAST角點(diǎn)檢測(cè)測(cè)試

本文選取Euroc 數(shù)據(jù)集中的MH04_difficlut 和MH05_difficlut 子集作為測(cè)試數(shù)據(jù)。試驗(yàn)參數(shù)設(shè)置為：Vini=15個(gè)，Vmin=5個(gè)，圖像金字塔比例系數(shù)為1.2，F(xiàn)AST角點(diǎn)檢測(cè)數(shù)量Na=150 個(gè)。圖7 所示為系統(tǒng)啟動(dòng)階段在MH04_difficlut和MH05_difficlut子集上的角點(diǎn)檢測(cè)效果。

圖7 MH04_difficlut和MH05_difficlut子集中角點(diǎn)檢測(cè)效果

不難看出，圖7a 和圖7c 中提取所得角點(diǎn)存在明顯的聚集現(xiàn)象，且多集中于特征明顯的區(qū)域，而圖7b和圖7d 中角點(diǎn)檢測(cè)分布更加均勻，證明了本文角點(diǎn)分散策略的有效性。

4.2 VINS-GPS在室外大范圍環(huán)境中的測(cè)試

為測(cè)試本文提出的VINS-GPS 定位算法在室外大范圍環(huán)境中的表現(xiàn)，采用KITTI 數(shù)據(jù)集07 和09 子集進(jìn)行了測(cè)試。KITTI 數(shù)據(jù)集07 和09 子集的環(huán)境如圖8 所示，是典型的室外環(huán)境。

圖8 KITTI數(shù)據(jù)集07和09子集部分試驗(yàn)環(huán)境

由于KITTI數(shù)據(jù)集只提供了差分GPS定位結(jié)果，本文在該結(jié)果的基礎(chǔ)上分別添加均值為3 m 的高斯白噪聲以模擬無(wú)差分GPS 定位結(jié)果。圖9 所示為09 子集中GPS、差分GPS 定位結(jié)果與車輛真實(shí)軌跡（Ground Truth）對(duì)比結(jié)果，其局部放大圖如圖10 所示。由圖10不難看出，差分GPS 定位結(jié)果比較接近真值，而無(wú)差分GPS定位結(jié)果與真值相差較大，且含有抖動(dòng)。

圖9 GPS、差分GPS定位結(jié)果與真實(shí)軌跡對(duì)比結(jié)果

圖10 GPS、差分GPS定位結(jié)果和真實(shí)軌跡局部放大圖

采用與4.1 節(jié)相同的參數(shù)設(shè)置，作為對(duì)比，引入VINS-Mono 算法進(jìn)行了定位試驗(yàn)。設(shè)置Shi-Tomasi角點(diǎn)檢測(cè)閾值為30 個(gè)，Shi-Tomasi 角點(diǎn)檢測(cè)數(shù)量為150 個(gè)。本試驗(yàn)采用KITTI 自帶的評(píng)測(cè)工具KITTI_odometry_evaluation_tool 評(píng)測(cè)2 個(gè)系統(tǒng)的定位精度。

圖11給出了VINS-Mono算法所得車輛軌跡與真實(shí)軌跡對(duì)比結(jié)果。由圖11 不難看出，VINS-Mono 在起始階段位姿估計(jì)結(jié)果與真值相差較小，定位精度較高。然而，隨著移動(dòng)距離增加，系統(tǒng)累積誤差逐漸增大，導(dǎo)致VINS-Mono的定位精度下降。

圖11 VINS-Mono算法定位結(jié)果與真實(shí)軌跡對(duì)比結(jié)果

圖12 給出了采用VINS-GPS 定位算法所得車輛軌跡與真實(shí)軌跡的對(duì)比結(jié)果。

圖12 VINS-GPS算法定位結(jié)果與真實(shí)軌跡對(duì)比結(jié)果

由圖12可知，本文提出的VINS-GPS定位算法估計(jì)的定位結(jié)果基本接近真實(shí)軌跡，同時(shí)VINS-GPS 在KITTI 數(shù)據(jù)集07 和09 子集的均方根誤差較VINS-Mono分別降低了18.16%和33.08%，較VINS-Mono 定位精度有了很大的提升。這是由于本文提出的VINS-GPS 定位算法融合了VINS-Mono 與GPS 兩者的定位優(yōu)點(diǎn)，能夠在室外大范圍環(huán)境中獲得良好表現(xiàn)。本文還統(tǒng)計(jì)了VINS-GPS在09子集上各模塊的耗費(fèi)時(shí)間和運(yùn)行頻率，如表1所示。

表1 VINS-GPS各模塊運(yùn)行時(shí)間和頻率

不難看出，VINS-GPS 的角點(diǎn)檢測(cè)與跟蹤可以穩(wěn)定在10 Hz運(yùn)行，位姿優(yōu)化與GPS融合耗時(shí)也相對(duì)較少。

綜上所述，本文提出的VINS-GPS 融合定位算法，不僅能夠改善VINS-Mono 在室外大范圍環(huán)境表現(xiàn)較差的問(wèn)題，同時(shí)還可以提高GPS的定位精度。

5 結(jié)束語(yǔ)

本文基于VINS-Mono 提出一種GPS 與VINS-FAST算法融合的VINS-GPS 算法，改善了FAST 角點(diǎn)檢測(cè)集中的現(xiàn)象，使得在紋理弱的區(qū)域也能檢測(cè)到FAST角點(diǎn)，同時(shí)進(jìn)一步消除了SLAM算法運(yùn)行過(guò)程中的累計(jì)誤差，能夠?yàn)闊o(wú)人駕駛汽車在室外大規(guī)模復(fù)雜環(huán)境下提供較高精度的定位。