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

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

0 引言

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

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

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

1 實(shí)驗(yàn)平臺(tái)及系統(tǒng)框架

1.1 實(shí)驗(yàn)平臺(tái)

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

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

1.2 系統(tǒng)框架

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

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

2 相關(guān)理論

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

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

相機(jī)內(nèi)包含凸透鏡,在實(shí)際使用中安裝廣角鏡頭以增大視角,但透鏡會(huì)引起圖像失真,為獲取較好的成像效果,首先進(jìn)行圖像畸變?nèi)コ较蚧兌囗?xiàng)式關(guān)系為：

(1)

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

(2)

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

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

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

(3)

同理,可得：

(4)

將物理成像平面翻轉(zhuǎn)放置到光心O點(diǎn)的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為相機(jī)的內(nèi)參矩陣。通過(guò)內(nèi)參可以求得像素點(diǎn)與對(duì)應(yīng)空間點(diǎn)的歸一化空間坐標(biāo)點(diǎn)之間的關(guān)系。

2.2 YOLOv3算法介紹

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

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

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

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

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

圖4 YOLOv3算法檢測(cè)原理示意圖

2.3 K近鄰算法介紹

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

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

(8)

(9)

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

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

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

圖5 M39連接示意圖

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

3 錐桶空間定位

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

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

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

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

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

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

axA+byA+czA=1

(10)

(11)

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

3.2 錐桶空間定位計(jì)算

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

(12)

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

圖8 錐桶投影示意圖

3.3 車輛位置獲取

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

(13)

3.4 錐桶空間定位

(14)

θ0k=θk-θ0

(15)

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

3.5 基于KNN的濾波算法

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

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

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

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

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

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

4 實(shí)驗(yàn)與分析

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

4.1 目標(biāo)檢測(cè)算法驗(yàn)證

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

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

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

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

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

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

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

4.2 賽道地圖實(shí)驗(yàn)結(jié)果

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

圖12 模擬高速避障賽道實(shí)車測(cè)試示意圖

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

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

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

表2 定位精度

5 結(jié)論

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

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

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

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