FSAC賽道地圖構建算法研究

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

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

0 引言

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

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

FSAC無人方程式賽車感知對象為不同顏色的錐桶,目的是獲得錐桶的顏色和位置,并為之后的決策規劃提供關鍵信息。通過對FSAC賽道錐桶地圖構建方法進行研究,利用單目相機與組合慣導設計并實現了一種具備高實時性和高準確性的錐桶地圖構建算法,解決賽場環境中賽道地圖構建的問題。

1 實驗平臺及系統框架

1.1 實驗平臺

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

圖1 FSAC方程式賽車及關鍵設備安裝位置

1.2 系統框架

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

圖2 系統設計流程

2 相關理論

2.1 相機標定方法

2.1.1相機畸變去除

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

(1)

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

(2)

2.1.2相機內參獲取

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

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

(3)

同理,可得：

(4)

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

(5)

成像平面和像素坐標存在式(6)的關系：

(6)

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

結合式(5)和式(6),將結果轉化為矩陣形式：

(7)

其中：fx為αf,fy為βf,K為相機的內參矩陣。通過內參可以求得像素點與對應空間點的歸一化空間坐標點之間的關系。

2.2 YOLOv3算法介紹

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

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

YOLOv3[14]主體由特征提取網絡與多尺度檢測器組成,前者用于圖像特征提取,后者在前者基礎上進行抽象和融合,得到3個尺度預測特征。進一步對這3個尺度進行檢測,得到檢測目標中錐桶類別及檢測框對應坐標[9]。

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

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

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

2.3 K近鄰算法介紹

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

對K近鄰算法進行改進,引入權重概念[18],在K個位置直接平均賦值的基礎上,對每個近鄰目標點的貢獻程度分配權重值,對K個近鄰的錐桶位置點進行加權平均,得到估計位置坐標：

(8)

(9)

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

2.4 M39組合導航系統

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

圖5 M39連接示意圖

對M39組合導航系統進行標定,使用M39對組合導航系統進行位姿解算。在位姿解算中心,輸入的數據被轉換成慣性空間中的導航信息,包括速度、加速度和位置信息。這些信息被用來計算當前位置的姿態,并將這些姿態傳輸到車輛的控制系統中,最終獲得當前位置的經緯度、頭指向、俯仰角、橫滾角和偏航角。

3 錐桶空間定位

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

圖6 賽道地圖坐標系示意圖

3.1 相機與錐桶標定方法

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

圖7 相機與錐桶標定圖

設錐桶所在平面方程滿足：

axA+byA+czA=1

(10)

(11)

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

3.2 錐桶空間定位計算

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

(12)

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

圖8 錐桶投影示意圖

3.3 車輛位置獲取

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

(13)

3.4 錐桶空間定位

(14)

θ0k=θk-θ0

(15)

由此求得賽車前方障礙物在賽道坐標系中對應位置。

3.5 基于KNN的濾波算法

若每一次獲取到錐桶都放入地圖內,會導致每個錐桶在地圖上對應多個位置,影響規劃判斷,通過改進的KNN算法對所得錐桶進行濾波處理。

當目標檢測系統檢測到一個新錐桶時,分為3種情況進行處理：

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

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

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

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

4 實驗與分析

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

4.1 目標檢測算法驗證

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

圖9 FSAC-COCO錐桶數據集示意圖

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

圖10 訓練過程參數曲線

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

訓練完成后,加載獲取的權重文件,對獲取的圖像進行檢測,檢測結果如圖11所示。通過ROS發布錐桶在圖像中的位置信息,供建圖使用。

圖11 目標檢測結果示意圖

4.2 賽道地圖實驗結果

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

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

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

表1 獲取錐桶地圖結果

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

表2 定位精度

5 結論

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

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

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

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