FSAC賽道地圖構(gòu)建算法研究

蘭建平,郭文韜,湯文靖,佘依函

(湖北汽車工業(yè)學(xué)院 汽車工程師學(xué)院,湖北 十堰 442002)

0 引言

無人駕駛汽車是未來汽車的發(fā)展方向[1],汽車智能化的飛速發(fā)展推動了眾多企業(yè)進(jìn)軍該領(lǐng)域。為培養(yǎng)智能網(wǎng)聯(lián)汽車方向的創(chuàng)新型人才,中國汽車工程學(xué)會在大學(xué)生電動方程式大賽的基礎(chǔ)上[2],舉辦了中國大學(xué)生無人駕駛方程式大賽(FSAC)。無人駕駛汽車主要分為感知定位、決策規(guī)劃和執(zhí)行控制組成。感知定位是無人駕駛的第一步也是最為重要的一步,其結(jié)果直接影響之后的決策規(guī)劃和執(zhí)行控制部分[3]。通過相機和雷達(dá)等各類傳感器感知周圍環(huán)境[4],決策規(guī)劃基于感知系統(tǒng)確定最優(yōu)行駛路徑,通過控制部分沿著最優(yōu)行駛路徑行駛。通常,感知定位[5]系統(tǒng)會通過構(gòu)建地圖的方式為路徑規(guī)劃系統(tǒng)提供決策依據(jù)。

國內(nèi)外學(xué)者對無人駕駛的地圖構(gòu)建算法進(jìn)行了大量研究,常見的建圖方式有基于激光雷達(dá)和基于視覺方法兩大類,其中基于視覺的算法又可分為特征點匹配和深度學(xué)習(xí)方法。方哲等[6]提出了一種融合語義特征傳播模型的前景對象感知定位算法,實現(xiàn)了一定精度和實時性的定位,但占用資源較大。Krylov等[7]提出一種使用街景圖像自動檢測和計算重復(fù)靜態(tài)感興趣對象坐標(biāo)的方法,利用MRF估計物體的地理位置,能確定目標(biāo)位置,但精度偏低。商磊等[8]提出了一種基于MeanShift聚類選取自適應(yīng)KNN的混合相似度加權(quán)KNN定位算法,顯著提高了算法的定位精度和穩(wěn)定性。ORB-SLAM系列為主的稀疏點云地圖由于缺乏一些必要的細(xì)節(jié)[9],不能作為路徑規(guī)劃的依據(jù)。對于稠密地圖[10]的構(gòu)建,雖然細(xì)節(jié)保留比較完全,但是建圖速度較慢,難以滿足實時性要求,對無人駕駛汽車構(gòu)成安全隱患。

FSAC無人方程式賽車感知對象為不同顏色的錐桶,目的是獲得錐桶的顏色和位置,并為之后的決策規(guī)劃提供關(guān)鍵信息。通過對FSAC賽道錐桶地圖構(gòu)建方法進(jìn)行研究,利用單目相機與組合慣導(dǎo)設(shè)計并實現(xiàn)了一種具備高實時性和高準(zhǔn)確性的錐桶地圖構(gòu)建算法,解決賽場環(huán)境中賽道地圖構(gòu)建的問題。

1 實驗平臺及系統(tǒng)框架

1.1 實驗平臺

采用湖北汽車工業(yè)學(xué)院FSAC方程式賽車,Basler ace-acA1920-40gc相機;邁普時空M39組合慣性導(dǎo)航系統(tǒng)。計算單元采用Intel i7-10870H處理器和NVIDIA 2060顯卡(6G顯存)。使用機器人操作系統(tǒng)(robot operating system,ROS)作為中間件進(jìn)行通信,通過話題發(fā)布和訂閱完成信息交互和控制。將相機水平朝向賽車正前方,安裝于賽車主環(huán)下方,便于其獲取車輛前方障礙物,組合慣導(dǎo)安裝于電池箱上方,計算單元安裝于賽車右側(cè)。FSAC方程式賽車及關(guān)鍵設(shè)備安裝位置如圖1所示。

圖1 FSAC方程式賽車及關(guān)鍵設(shè)備安裝位置

1.2 系統(tǒng)框架

首先對單目相機進(jìn)行標(biāo)定,使用張氏標(biāo)定法利用7×8的棋盤格對圖像進(jìn)行標(biāo)定,去除圖像畸變,并獲取相機的內(nèi)參;接著對錐桶在相機坐標(biāo)系下的位置進(jìn)行標(biāo)定,錐桶均放置在水平地面上,使錐桶分布滿足平面方程,完成錐桶與相機的聯(lián)合標(biāo)定。當(dāng)系統(tǒng)運行時,通過相機獲取錐桶圖像,通過YOLOv3算法進(jìn)行目標(biāo)檢測,得到錐桶的目標(biāo)檢測框,取其下邊界中點作為該錐桶在圖像中的位置,通過相機內(nèi)參和平面方程求出錐桶在相機坐標(biāo)系下的空間位置;通過組合慣導(dǎo)解算得到賽車位置,進(jìn)一步確定錐桶在車輛坐標(biāo)系下的位置,并利用KNN算法進(jìn)行濾波,進(jìn)一步篩選錐桶在車輛坐標(biāo)系下的位置。定義賽車起駛位置為賽道地圖坐標(biāo)系的原點,并將錐桶坐標(biāo)轉(zhuǎn)換到賽道地圖坐標(biāo)系下,最終得到賽道地圖。設(shè)計流程如圖2所示。

圖2 系統(tǒng)設(shè)計流程

2 相關(guān)理論

2.1 相機標(biāo)定方法

2.1.1相機畸變?nèi)コ?/p>

相機內(nèi)包含凸透鏡,在實際使用中安裝廣角鏡頭以增大視角,但透鏡會引起圖像失真,為獲取較好的成像效果,首先進(jìn)行圖像畸變?nèi)コ?。徑向畸變多項式關(guān)系為：

(1)

在安裝過程中,鏡頭不能與成像平面完全平行,否則會導(dǎo)致切向畸變,切向畸變多項式關(guān)系為：

(2)

2.1.2相機內(nèi)參獲取

單目相機的成像過程可以簡化為小孔成像[11],建立相機坐標(biāo)系O-X-Y-Z和成像坐標(biāo)系O′-X′-Y′-Z′,其中f為相機焦距。設(shè)空間內(nèi)任意一點在相機坐標(biāo)系下的坐標(biāo)為P(x,y,z),通過光心O投影到成像坐標(biāo)系下為P′(x′,y′),相機投影模型如圖3所示。

對小孔成像進(jìn)行分析,建立相似三角形,得式(3)：

(3)

同理,可得：

(4)

將物理成像平面翻轉(zhuǎn)放置到光心O點的f處,得到：

(5)

成像平面和像素坐標(biāo)存在式(6)的關(guān)系：

(6)

式中：u和v為P′在像素坐標(biāo)系的位置;α和β為像素坐標(biāo)系軸方向和軸方向上的比例系數(shù);cx和cy為偏移量。

結(jié)合式(5)和式(6),將結(jié)果轉(zhuǎn)化為矩陣形式：

(7)

其中：fx為αf,fy為βf,K為相機的內(nèi)參矩陣。通過內(nèi)參可以求得像素點與對應(yīng)空間點的歸一化空間坐標(biāo)點之間的關(guān)系。

2.2 YOLOv3算法介紹

卷積神經(jīng)網(wǎng)絡(luò)(convolutional neural networks,CNN)是典型的深度學(xué)習(xí)算法,根據(jù)CNN原理不同,基于CNN的目標(biāo)檢測算法可分為二階段檢測和一階段檢測2種。經(jīng)典的二階段檢測算法有R-CNN、Fast-RCNN和Faster-RCNN等,有代表性的一階段檢測算法有YOLO[12]和SSD(single shot mutibox detector)等算法。YOLO算法相較于傳統(tǒng)的二階段檢測算法繞開了候選區(qū)域分類和評估,極大程度提升了運算速度。

YOLO算法在保持速度優(yōu)勢的前提下提升了預(yù)測精度,加強了對小物體的檢測能力,在工業(yè)界應(yīng)用廣泛。其中,YOLOv3算法既能同時提取多個通道的特征,又能保證一定準(zhǔn)確性,滿足無人駕駛方程式賽車的檢測需求。與此同時,相比于當(dāng)前新版本的YOLO檢測算法,YOLOv3自2018年提出以來,已推廣近5年時間,更為成熟可靠,適用于無人駕駛賽車等實際工程[13]。

YOLOv3[14]主體由特征提取網(wǎng)絡(luò)與多尺度檢測器組成,前者用于圖像特征提取,后者在前者基礎(chǔ)上進(jìn)行抽象和融合,得到3個尺度預(yù)測特征。進(jìn)一步對這3個尺度進(jìn)行檢測,得到檢測目標(biāo)中錐桶類別及檢測框?qū)?yīng)坐標(biāo)[9]。

YOLOv3為YOLO算法的改進(jìn)版本,對YOLOv2[15]和YOLOv1改進(jìn)時借鑒了特征金字塔(feature pyramid network,FPN)思想,利用多個卷積層和池化層來提取圖像特征,并將這些特征組合成一個特征金字塔,用于對各類尺寸圖像檢測。

YOLOv3算法將輸入的待檢圖片劃分為S×S的網(wǎng)格,每個網(wǎng)格預(yù)測一定數(shù)量的初始邊界框和其對應(yīng)的置信度,預(yù)測每個狀態(tài)下目標(biāo)物的類別概率,即邊界框中存在目標(biāo)物的條件概率。將每個狀態(tài)下目標(biāo)物的類別概率和置信度相乘,得到該狀態(tài)下的分類概率。最后,將所有狀態(tài)下的分類概率相加,得到最終的分類概率。對于每個類別,可以計算其邊界框的置信度,然后將分類概率和邊界框的置信度相乘,得到該類別的置信度。YOLOv3算法檢測原理[16]如圖4所示。

圖4 YOLOv3算法檢測原理示意圖

2.3 K近鄰算法介紹

K近鄰算法[17]是基礎(chǔ)的機器學(xué)習(xí)算法之一,可用于分類和回歸。它通過測量不同特征值之間的距離來進(jìn)行分類。對于任意維度輸入向量,輸出為該特征向量所對應(yīng)的類別標(biāo)簽或預(yù)測值。相較于最近鄰算法,K近鄰算法的定位準(zhǔn)確性有一定提升,缺點是在進(jìn)行濾波處理時,直接采用取平均值確定定位點的位置可能導(dǎo)致誤差增大。

對K近鄰算法進(jìn)行改進(jìn),引入權(quán)重概念[18],在K個位置直接平均賦值的基礎(chǔ)上,對每個近鄰目標(biāo)點的貢獻(xiàn)程度分配權(quán)重值,對K個近鄰的錐桶位置點進(jìn)行加權(quán)平均,得到估計位置坐標(biāo)：

(8)

(9)

式中：ωi為第i個參考點所占的權(quán)重;di為K個近鄰點中第i個錐桶點到待定位點的距離;ε為無限趨近于0的數(shù),防止ωi在di為0時成為無窮大。采用改進(jìn)版的K近鄰算法降低遠(yuǎn)處點對定位點的影響,提高定位的準(zhǔn)確性。

2.4 M39組合導(dǎo)航系統(tǒng)

M39組合慣導(dǎo)由武漢邁普時空導(dǎo)航科技有限公司生產(chǎn),包含IMU和GNSS兩個主要部分,在GPS信號良好時,能夠準(zhǔn)確對無人駕駛車輛進(jìn)行定位。M39連接示意圖如圖5所示。

圖5 M39連接示意圖

對M39組合導(dǎo)航系統(tǒng)進(jìn)行標(biāo)定,使用M39對組合導(dǎo)航系統(tǒng)進(jìn)行位姿解算。在位姿解算中心,輸入的數(shù)據(jù)被轉(zhuǎn)換成慣性空間中的導(dǎo)航信息,包括速度、加速度和位置信息。這些信息被用來計算當(dāng)前位置的姿態(tài),并將這些姿態(tài)傳輸?shù)杰囕v的控制系統(tǒng)中,最終獲得當(dāng)前位置的經(jīng)緯度、頭指向、俯仰角、橫滾角和偏航角。

3 錐桶空間定位

以無人駕駛賽車起駛點建立賽道地圖坐標(biāo)系O0-X0-Y0-Z0,定義k時刻相機坐標(biāo)系為Oc-Xc-Yc-Zc,組合導(dǎo)航系統(tǒng)為該時刻車輛坐標(biāo)系原點,即：Ov-Xv-Yv-Zv為車輛坐標(biāo)系,賽道地圖坐標(biāo)系如圖6所示。

圖6 賽道地圖坐標(biāo)系示意圖

3.1 相機與錐桶標(biāo)定方法

相機與錐桶標(biāo)定方法通過相機對錐桶所在平面進(jìn)行標(biāo)定[19]。獲取錐桶在單目相機下的平面參數(shù),設(shè)A錐桶在相機坐標(biāo)系下坐標(biāo)為(xA,yA,zA),H為A錐桶到相機坐標(biāo)系原點的距離[20],如圖7所示。

圖7 相機與錐桶標(biāo)定圖

設(shè)錐桶所在平面方程滿足：

axA+byA+czA=1

(10)

(11)

結(jié)合式(7)、(10)和(11),通過測量獲得u、v和H,對式(10)中系數(shù)a、b和c進(jìn)行求解。

3.2 錐桶空間定位計算

相機通過YOLOv3獲取錐桶檢測框,以檢測框下方框線中點為錐桶位置。設(shè)空間中存在B錐桶,代入組合方程(12),即可得到B錐桶的空間位置。

(12)

求得B錐桶在相機坐標(biāo)系下的空間位置(xB,yB,zB)后,相機朝向為賽車的正前方,錐桶投影在Oc-Xc-Zc平面的坐標(biāo)為(xB,zB)。為方便表示,令該坐標(biāo)為(xcone,ycone)。錐桶在該平面投影如圖8所示,左側(cè)為目標(biāo)檢測,右側(cè)為對應(yīng)的錐桶位置。

圖8 錐桶投影示意圖

3.3 車輛位置獲取

定義起始時刻為0時刻,通過經(jīng)緯度對車輛位置進(jìn)行解算,獲取當(dāng)前車輛位置(x0,y0)和頭指向θ0,在k時刻,通過經(jīng)緯度進(jìn)行車輛的位置解算,獲取k時刻車輛位置(xk,yk)和頭指向θk,車輛的位置(xv,yv)在賽道地圖坐標(biāo)系的位置為：

(13)

3.4 錐桶空間定位

(14)

θ0k=θk-θ0

(15)

由此求得賽車前方障礙物在賽道坐標(biāo)系中對應(yīng)位置。

3.5 基于KNN的濾波算法

若每一次獲取到錐桶都放入地圖內(nèi),會導(dǎo)致每個錐桶在地圖上對應(yīng)多個位置,影響規(guī)劃判斷,通過改進(jìn)的KNN算法對所得錐桶進(jìn)行濾波處理。

當(dāng)目標(biāo)檢測系統(tǒng)檢測到一個新錐桶時,分為3種情況進(jìn)行處理：

1) 若該錐桶附近2 m沒有之前建立的錐桶時,設(shè)該錐桶權(quán)重為1;

2) 若新錐桶附近2 m有1個之前建立權(quán)重為n的錐桶時,對2個錐桶位置進(jìn)行加權(quán)平均,設(shè)置新得到錐桶的權(quán)重為n+1;

3) 若新錐桶附近2 m有2個及以上之前建立的錐桶時,取離該錐桶最近的權(quán)重為m的錐桶,對該錐桶位置進(jìn)行加權(quán)平均,設(shè)置新得到錐桶權(quán)重為m+1。

通過OpenCV制作一個錐桶全局坐標(biāo)顯示,可以得到錐桶全局坐標(biāo)位置和初始經(jīng)緯度坐標(biāo),獲得錐桶在對應(yīng)賽道地圖坐標(biāo)系下的二維錐桶賽道地圖。

4 實驗與分析

使用湖北汽車工業(yè)學(xué)院無人駕駛方程式賽車對提出的算法進(jìn)行測試驗證,共檢測3種物體類型：紅色錐桶、藍(lán)色錐桶和黃色錐桶,并獲取錐桶空間位置,構(gòu)建錐桶賽道平面地圖。

4.1 目標(biāo)檢測算法驗證

采用由北京理工大學(xué)、福州大學(xué)、湖南大學(xué)、湖北汽車工業(yè)學(xué)院等10余所高校共同構(gòu)建的FSAC-COCO錐桶數(shù)據(jù)集,如圖9所示,從中選取 6 000張圖片(包含56 857個錐桶)作為訓(xùn)練集,2 000張圖片(包含17 694個錐桶)作為測試集。在不同地點、不同時間、不同光照及不同賽道進(jìn)行采集,錐桶分為3個目標(biāo)類別,分別為紅色錐桶、藍(lán)色錐桶和黃色錐桶。為增加圖像多樣性,通過輕微平移、縮放和添加隨機顏色噪聲對數(shù)據(jù)集進(jìn)行增強,再將圖像歸一化后送入目標(biāo)檢測模型進(jìn)行特征提取。

圖9 FSAC-COCO錐桶數(shù)據(jù)集示意圖

采用YOLOv3算法對FSAC-COCO錐桶數(shù)據(jù)集進(jìn)行訓(xùn)練測試,訓(xùn)練過程參數(shù)曲線如圖10所示。

圖10 訓(xùn)練過程參數(shù)曲線

圖10(a)為損失曲線,訓(xùn)練過程前35輪下降最快,在265次迭代訓(xùn)練之后,損失曲線下降趨于平緩,最終穩(wěn)定在最小值。圖10(b)為在驗證集測試的平均精度均值曲線,最終平均檢測精度為96.2%。

訓(xùn)練完成后,加載獲取的權(quán)重文件,對獲取的圖像進(jìn)行檢測,檢測結(jié)果如圖11所示。通過ROS發(fā)布錐桶在圖像中的位置信息,供建圖使用。

圖11 目標(biāo)檢測結(jié)果示意圖

4.2 賽道地圖實驗結(jié)果

在校園環(huán)境中模擬高速循跡賽道,設(shè)定FSAC方程式賽車以30 km/h速度行駛[21],使用ROS中Rviz可視化界面進(jìn)行測試結(jié)果檢測[22]。賽車以30 km/h速度行駛時,圖12(a)和圖12(b)為高速循跡測試中不同時刻測試結(jié)果,左側(cè)為錐桶檢測結(jié)果,右側(cè)為實時建圖結(jié)果?？梢钥吹?錐桶的顏色信息和空間位置匹配良好,完成高速循跡測試賽道地圖構(gòu)建,具有較高準(zhǔn)確度和實時性。

圖12 模擬高速避障賽道實車測試示意圖

高速循跡賽道共設(shè)置70個錐桶實例,其中有34個紅色錐桶和36個藍(lán)色錐桶,獲取錐桶地圖結(jié)果如表1所示,真陽性(true positive,TP)為68個,召回率(recall)為98.6%,精準(zhǔn)度(precision)為97.1%。對比文獻(xiàn)[7]中的MRF算法,召回率提升了3.1%,精準(zhǔn)度提升了4.6%,且每幀都可以輸出一組錐桶位置,即使在圖像目標(biāo)少檢情況下,仍能較好地實現(xiàn)賽道構(gòu)建目標(biāo),減小假陽性(true positive,FP)發(fā)生概率。同時,較多錐桶位置結(jié)果也能更好地剔除假陰性(false negative,FN)發(fā)生概率。從實驗結(jié)果可以看出,通常假陰性結(jié)果在改進(jìn)KNN濾波時僅占很小權(quán)重,通過設(shè)置閾值進(jìn)行剔除。

表1 獲取錐桶地圖結(jié)果

對建圖結(jié)果進(jìn)行分析,定位精度如表2所示,平均誤差為0.245 m,中值誤差為0.142 m,相比MRF算法降低了52.8%,中值誤差降低了43.4%,順利完成循跡任務(wù),結(jié)果滿足競賽要求,能夠為無人駕駛賽車提供可靠的環(huán)境感知信息,獲取實時準(zhǔn)確的錐桶賽道地圖。

表2 定位精度

5 結(jié)論

1) 為滿足實際環(huán)境中無人駕駛賽車地圖構(gòu)建實時性和準(zhǔn)確性,提出了一種基于相機和組合導(dǎo)航系統(tǒng)的錐桶地圖構(gòu)建算法,可在不依賴激光雷達(dá)的情況下滿足賽場環(huán)境下無人駕駛賽車賽道地圖的構(gòu)建。

2) 實驗結(jié)果表明,相比基于MRF的地圖構(gòu)建算法,所提出算法在提升精度和召回率、降低誤差等方面有較好表現(xiàn),為FSAC無人方程式賽車的決策規(guī)劃提供了更為準(zhǔn)確的判斷依據(jù)。

3) 所提出算法可擴展到行人地圖構(gòu)建及車輛地圖構(gòu)建等,將其部署于車輛前置相機時,可操作性高且易于部署。

4) 所提出算法主要針對單目相機與組合慣導(dǎo)融合的錐桶地圖構(gòu)建進(jìn)行研究,未來可從提高算法檢測精度、速度及輕量化與可移植性等多方面對檢測算法進(jìn)一步迭代提升,以滿足算法的商用化及量產(chǎn)化需求。