基于多模態(tài)感知與融合的無人車韌性導(dǎo)航系統(tǒng)

仲訓(xùn)昱，武東杰，陳登龍，莊明溪，吳汶鴻，彭俠夫

(廈門大學(xué)航空航天學(xué)院，廈門 361005)

0 引言

在衛(wèi)星拒止、物理空間變化等復(fù)雜環(huán)境下，為了使地面無人車具備全天候、全時(shí)域的韌性導(dǎo)航能力，需要解決基于視覺、激光雷達(dá)、慣性測量單元(Inertial Measurement Unit，IMU)、輪式里程計(jì)及磁力計(jì)等多模態(tài)環(huán)境感知與信息獲取、基于信息融合的最優(yōu)位姿估計(jì)與導(dǎo)航規(guī)劃等問題[1-2]。

針對最優(yōu)位姿估計(jì)問題(基于微機(jī)電系統(tǒng)低精度IMU)，國內(nèi)外學(xué)者主要開展了基于卡爾曼濾波的融合定位方法研究。其中擴(kuò)展卡爾曼濾波(Extended Kal-man Filter，EKF)是低延遲、高效狀態(tài)估計(jì)的典型選擇，已用于履帶機(jī)器人的IMU和輪式里程計(jì)融合定位(精度達(dá)1.2%D)[3]，還用于美國國防部高級研究計(jì)劃局(Defense Advanced Research Projects Agency, DARPA) LS3四足機(jī)器人的IMU、相機(jī)、足式里程計(jì)融合定位(精度達(dá)1.0%D)[4]，以及無人機(jī)組合導(dǎo)航[5]等。近年，誤差狀態(tài)EKF(Error State EKF，ESEKF)和不變EKF(Invariant EKF，IEKF)憑借其在提高精度、收斂性及實(shí)時(shí)性方面的優(yōu)勢，在多源融合定位中不斷得到應(yīng)用[6-8]。

針對導(dǎo)航規(guī)劃(路徑規(guī)劃或運(yùn)動(dòng)規(guī)劃)問題，在以往的研究中提出了基于環(huán)境的先驗(yàn)3D幾何信息以選擇最佳通行區(qū)域[9]，但在越野環(huán)境中如果只依賴幾何信息則會(huì)導(dǎo)致令人失望的結(jié)果，例如可通行的草叢或細(xì)小植物等都會(huì)被認(rèn)為是不可通過的障礙物，這顯然不是類似人類的感知行為。近年來，隨著深度學(xué)習(xí)技術(shù)的發(fā)展，文獻(xiàn)[10]提出了使用語義分割實(shí)時(shí)構(gòu)建環(huán)境的2.5D語義地圖，同時(shí)考慮環(huán)境的語義和幾何信息使得無人車在導(dǎo)航過程中具備類似人的感知能力，例如可以自主選擇是否穿越草叢行駛。但是2.5D語義地圖對環(huán)境的表示能力不足，因?yàn)闊o人車在感知最真實(shí)的3D環(huán)境時(shí)，仍然需要重建完整的3D環(huán)境模型；文獻(xiàn)[9]使用的局部規(guī)劃器過于簡單，無法滿足復(fù)雜場景;并且語義分割模型的速度也有待提高，這些缺陷極大地限制了無人車在未知越野環(huán)境中的自主導(dǎo)航能力。

為了解決這些問題，本文提出了一種新穎的無人車自主導(dǎo)航系統(tǒng)。首先，為了盡量提高語義分割的速度，對實(shí)時(shí)網(wǎng)絡(luò)LedNet[11]進(jìn)行裁剪以加快模型推斷速度，這樣可以進(jìn)一步提高后續(xù)環(huán)境模型構(gòu)建的實(shí)時(shí)性。其次，使用了一種相較于傳統(tǒng)貝葉斯算法[12]速度更快的局部語義3D地圖構(gòu)建算法，并結(jié)合語義和幾何信息實(shí)時(shí)投影生成可通行性代價(jià)地圖，為規(guī)劃提供必要的實(shí)時(shí)性條件。最后，改進(jìn)了傳統(tǒng)動(dòng)態(tài)窗口(Dynamic Window Approach，DWA)局部規(guī)劃算法[13]，為其增加一個(gè)語義約束，使得DWA算法無需全局規(guī)劃器的指導(dǎo)即可具有實(shí)時(shí)感知前方最低通行難度區(qū)域的能力。

1 基于濾波的融合定位

1.1 融合框架與子系統(tǒng)設(shè)計(jì)

基于濾波的多模態(tài)信息融合位姿估計(jì)的總體框架如圖1所示，包括四部分：1)具備多種信號感知能力的傳感器集合；2)單一模態(tài)位姿估計(jì)子系統(tǒng)；3)基于ESEKF的位姿估計(jì)器；4)用于提高韌性的多模態(tài)測量信息管理器。

圖1 基于ESEKF的多模態(tài)信息融合總體方案圖Fig.1 Overall scheme of multi-modal information fusion based on ESEKF

在各位姿估計(jì)子系統(tǒng)中，IMU和旋轉(zhuǎn)式編碼器可感知無人車的運(yùn)動(dòng)學(xué)或動(dòng)力學(xué)狀態(tài)(如角速度、加速度、輪速等)；在磁場均勻不受干擾的條件下，IMU中的磁力計(jì)可以提供一個(gè)絕對的偏航角參考；相機(jī)和激光雷達(dá)等傳感器可以采用同步定位與構(gòu)圖(Simultaneous Localization and Mapping，SLAM)算法(如VINS[14]、LeGO-LOAM[15]等)，獲得相對位姿信息及速度測量信息。

1.2 ESEKF多模態(tài)信息融合位姿估計(jì)器

設(shè)計(jì)的ESEKF融合算法實(shí)現(xiàn)流程如圖2所示，主要包括全狀態(tài)預(yù)測、誤差狀態(tài)EKF預(yù)測、誤差狀態(tài)EKF更新和誤差補(bǔ)償與重置等模塊。

圖2 基于ESEKF的多模態(tài)融合定位架構(gòu)Fig.2 Multi-modal fusion positioning architecture based on ESEKF

全狀態(tài)x∈R16定義為

(1)

(2)

其中，位置誤差δp∈R3、速度誤差δv∈R3、加速度計(jì)測量偏差誤差δab∈R3和陀螺儀測量偏差誤差δωb∈R3由線性空間減法定義；姿態(tài)誤差δθ∈R3由四元數(shù)乘法定義[16]。

ESEKF多模態(tài)信息融合位姿估計(jì)器的思路是[17]：1)利用高頻IMU數(shù)據(jù)預(yù)測當(dāng)前時(shí)刻的全狀態(tài)和誤差值；2)基于EKF算法，利用多模態(tài)子系統(tǒng)的測量值獲得系統(tǒng)誤差的最優(yōu)估計(jì)值；3)將誤差估計(jì)值和全狀態(tài)預(yù)測值合成，獲得系統(tǒng)全狀態(tài)的最優(yōu)估計(jì)。

(1)全狀態(tài)預(yù)測估計(jì)過程

設(shè)am和ωm分別為加速度計(jì)和陀螺儀的測量值，利用數(shù)值積分知識可得到全狀態(tài)的預(yù)測方程，設(shè)為

(3)

(4)

其中，R∈SO(3)表示姿態(tài)的旋轉(zhuǎn)矩陣；g為重力加速度矢量；q{}表示將旋轉(zhuǎn)向量轉(zhuǎn)換為相應(yīng)的四元數(shù)；?表示四元數(shù)乘法。

(2)誤差狀態(tài)EKF估計(jì)過程

δxk+1=Fkδxk+wF,k

(5)

其中

Fk=

wF,k～N(0,QF)

其中，I為單位矩陣；0為零矩陣；[]×表示將向量轉(zhuǎn)換為對應(yīng)的反對稱矩陣；wF,k為過程噪聲，其協(xié)方差設(shè)為QF,k。

當(dāng)前時(shí)刻的全狀態(tài)測量模型設(shè)為

yk+1=h(xk+1)+wh,k+1

(6)

其中，函數(shù)h的具體形式由子系統(tǒng)的輸出性質(zhì)決定；wh,k+1表示測量噪聲，其協(xié)方差記為Rh,k+1。

根據(jù)EKF框架，誤差狀態(tài)最優(yōu)估計(jì)過程為[16]

(7)

其中，P為濾波協(xié)方差矩陣；Hk+1為測量雅可比矩陣，計(jì)算公式如下

(8)

其中

位置測量方程為

pm,k+1=pk+1+wp,k+1

(9)

其中，pm,k+1為測量值；wp,k+1為位置測量噪聲；測量雅可比為三維單位矩陣。

姿態(tài)測量方程為

qm,k+1=qk+1+wq,k+1

(10)

其中，qm,k+1為測量值；wq,k+1為姿態(tài)測量噪聲；測量雅可比為四維單位矩陣。

(3)全狀態(tài)更新估計(jì)過程

1)誤差補(bǔ)償

(11)

2)誤差重置

當(dāng)完成對全狀態(tài)預(yù)測值的補(bǔ)償后，需要對EKF誤差狀態(tài)估計(jì)器中的變量進(jìn)行重置，過程如下[16]

(12)

其中

2 環(huán)境感知與語義3D地圖構(gòu)建

2.1 基于深度語義分割的環(huán)境感知

為提高實(shí)時(shí)性，對LedNet編碼器端進(jìn)行修改，得到計(jì)算速度更高的語義分割網(wǎng)絡(luò)(如圖3(a)所示)，稱為MiniLedNet。MiniLedNet在LedNet的基礎(chǔ)上，引入瓶頸型深度可分離卷積模塊(SS-Next-bt，如圖3(b)所示)，相較于LedNet的SS-nbt模塊添加了具有擴(kuò)張率的深度可分離卷積以及MobileNext提出來的SandGlass模塊[18]。由于1×1的點(diǎn)卷積對通道數(shù)進(jìn)行了縮小導(dǎo)致部分信息的丟失，所以又引入了MobileNext的線性瓶頸模塊，再將這三者組合得到SS-Next-bt，該模塊可在高通道特征層中大大減少運(yùn)算量，同時(shí)僅小幅降低準(zhǔn)確率，從而對LedNet模型進(jìn)行壓縮以提高推斷速度。

2.2 融合激光點(diǎn)云的語義3D地圖

如圖4所示，雙目相機(jī)RGB圖像經(jīng)MiniLedNet得到語義圖像，與 3D 激光雷達(dá)的點(diǎn)云融合得到語義點(diǎn)云，再結(jié)合融合定位信息，通過八叉樹存儲(chǔ)結(jié)構(gòu)的占有更新、語義更新建立局部和全局語義3D地圖[19]。此外，為了獲得更高精度的語義3D地圖，通過語義標(biāo)簽分布修正和運(yùn)動(dòng)目標(biāo)去除算法對地圖進(jìn)行糾錯(cuò)和修正，為地面無人車平臺執(zhí)行復(fù)雜任務(wù)提供準(zhǔn)確的導(dǎo)航特征地圖。

(a) MiniLedNet

(b) SS-Next-bt圖3 MiniLedNet 編碼器及其Split-Shuffle-Next-bottleneck模塊Fig.3 The encoder and its Split-Shuffle-Next-bottleneck module

(1)單幀語義融合

經(jīng)外參標(biāo)定對齊激光雷達(dá)和相機(jī)的坐標(biāo)后，利用語義圖像，將語義標(biāo)簽及其類別概率和3D點(diǎn)云進(jìn)行關(guān)聯(lián)[20]。語義分割網(wǎng)絡(luò)首先在一幀RGB圖像I上對每個(gè)像素(i,j)分別計(jì)算其屬于類別k的置信度Ci,jk，然后使用softmax正則化函數(shù)將置信度轉(zhuǎn)換為歸一化的標(biāo)簽概率

(13)

為了得到每個(gè)像素最可能的標(biāo)簽概率，取所有類別標(biāo)簽概率的最大值

圖4 語義3D地圖構(gòu)建總流程圖Fig.4 The building process of semantic 3D map

Pi,j=

max[p(1|Ii,j),p(2|Ii,j),…,p(k|Ii,j)]

(14)

在得到每個(gè)像素的語義標(biāo)簽和標(biāo)簽概率后，通過外參矩陣將其與對應(yīng)的點(diǎn)云進(jìn)行融合得到語義點(diǎn)云。

(2)體素占有概率更新

當(dāng)新的單幀融合后的語義點(diǎn)云插入到八叉樹存儲(chǔ)結(jié)構(gòu)的地圖時(shí)，八叉樹內(nèi)每個(gè)體素占有概率的更新公式如下[21]

P(n|z1:t)=

(15)

其中，z1:t為第1～t時(shí)刻所有的觀測數(shù)據(jù)；P(n|z1:t)為第n個(gè)節(jié)點(diǎn)基于第1～t時(shí)刻觀測數(shù)據(jù)的后驗(yàn)概率；P(n|zt)為第n個(gè)節(jié)點(diǎn)基于當(dāng)前觀測數(shù)據(jù)的占有概率；P(n)為節(jié)點(diǎn)n的先驗(yàn)概率(取0.5)。

根據(jù) logit 變換，將式(15)簡化成如下形式

L(n|z1:t)=L(n|z1:t-1)+L(n|zt)

(16)

(3)體素語義信息更新

采用八叉樹結(jié)構(gòu)存儲(chǔ)語義地圖，每個(gè)語義點(diǎn)云用立方體柵格表示，其中每一個(gè)體素在多幀圖像中都可能被觀察到并可能被語義分割網(wǎng)絡(luò)預(yù)測一個(gè)不同的語義類別，因此需要通過融合多個(gè)時(shí)刻觀察到的語義來確定一個(gè)體素的最終類別。

為保證實(shí)時(shí)性(尤其當(dāng)語義類別較多時(shí))，采用最大概率更新方法。體素v在t時(shí)刻的語義類別為L(vt)，標(biāo)簽概率P(c|zt)依賴于當(dāng)前體素測量值zt和t-1時(shí)刻的狀態(tài)量P(c|zt-1)，則最大概率融合后的當(dāng)前體素v的語義類別L(vt)的計(jì)算公式如下

(17)

最大概率融合后的當(dāng)前體素v的標(biāo)簽概率P(c|zt)的計(jì)算如下

P(c|zt)=

(18)

其中，ε∈(0,1)表示t時(shí)刻體素v的語義標(biāo)簽概率對t-1時(shí)刻體素v語義標(biāo)簽概率的修正因子，取ε=0.9。

(4)體素語義修正

在更新完語義信息后，由于外參標(biāo)定和語義分割存在誤差，所以導(dǎo)致個(gè)別體素語義類別分配錯(cuò)誤,因此采用空間推理法減少這些體素。首先，檢測體素的K近鄰體素，并計(jì)算這些體素和體素之間的平均距離。隨后，如果平均距離小于某個(gè)閾值，則它對應(yīng)于分割良好的體素，否則它被認(rèn)為是噪聲體素。表達(dá)式如下

(19)

(5)運(yùn)動(dòng)目標(biāo)去除

當(dāng)語義地圖完成時(shí)，許多地圖點(diǎn)是由移動(dòng)的物體產(chǎn)生的，這使得地圖變得混亂，必須消除這些地圖點(diǎn)。采用一種具有低參數(shù)特征的基于密度的噪聲應(yīng)用空間聚類(Density-Based Spatial Clustering of Applications with Noise，DBSCAN)算法，基于密度對體素進(jìn)行分組。然而，其缺點(diǎn)是基于生成的映射的大小，存儲(chǔ)效率顯著降低，因此需要長的計(jì)算時(shí)間。為了解決這個(gè)問題，將三維體素表示轉(zhuǎn)換為二維柵格表示。通過將所有三維體素投影到相同的Z來實(shí)現(xiàn)，定義如下

(20)

(21)

其中，Di為體素?cái)?shù)量；Li為ith聚類的長度；ηd和ηl為相應(yīng)的閾值。

3 基于語義3D地圖的運(yùn)動(dòng)規(guī)劃

如圖5所示，基于以上方法構(gòu)建以無人車為中心的局部語義 3D 地圖后，通過語義和高度投影得到可通行性代價(jià)地圖；基于投影得到2D地圖后，運(yùn)動(dòng)規(guī)劃器能夠感知可通過區(qū)域(道路、草叢等)、不可通過區(qū)域(樹木和其他障礙等)以及高度信息，從而選擇一條可通行路徑。

圖5 基于語義地圖的運(yùn)動(dòng)規(guī)劃Fig.5 Motion planning based on semantic map

3.1 語義和高度投影

如圖5所示，通過將局部語義3D地圖投影成2D代價(jià)地圖的方式，為無人車導(dǎo)航提供運(yùn)動(dòng)規(guī)劃的代價(jià)地圖。地圖中可通過區(qū)域如道路、草叢等通行成本分別設(shè)為1和10，高的綠植的成本設(shè)為50,不可通過的障礙物(巖石等)成本設(shè)為200；另外，為了彌補(bǔ)語義分割的不足(誤分類)，同時(shí)對3D語義柵格的高度也進(jìn)行最大值投影。語義和高度投影如圖6所示，Z0<{z0,z1,…,zn}

圖6 語義和高度投影示意圖Fig.6 Projection of semantic and height information

3.2 單條路徑通行代價(jià)計(jì)算

在投影得到局部 2D 代價(jià)地圖和某一路徑后，先在該地圖上計(jì)算一條采樣路徑中每個(gè)路徑點(diǎn)在語義代價(jià)地圖中對應(yīng)的語義成本,并結(jié)合高度值進(jìn)行修正得到總的通行代價(jià);再將一條中間路徑按照無人車的寬度復(fù)制6條平移軌跡，分別計(jì)算每條平移軌跡的通行代價(jià)，并將7條軌跡的通行代價(jià)累加起來作為該路徑的總通行代價(jià)。單條路徑通行代價(jià)的計(jì)算原理如圖7所示。

圖7 通行代價(jià)計(jì)算Fig.7 Passage cost calculation principle

3.3 基于DWA的運(yùn)動(dòng)規(guī)劃

采用DWA實(shí)現(xiàn)局部路徑規(guī)劃，主要流程如下：

1)在無人車運(yùn)動(dòng)空間進(jìn)行速度離散采樣(Δx,Δy,Δθ)；

2)對每一個(gè)采樣速度執(zhí)行行進(jìn)模擬，檢測使用該采樣速度移動(dòng)一段時(shí)間會(huì)發(fā)生什么；

3)評價(jià)行進(jìn)模擬中每個(gè)前行軌跡，評價(jià)準(zhǔn)則有：靠近障礙物、靠近目標(biāo)、貼近全局路徑、丟棄非法軌跡(如靠近障礙物的軌跡)；

4)挑出得分最高的軌跡(路徑)并發(fā)送對應(yīng)的速度給運(yùn)動(dòng)控制器；

5)重復(fù)上面步驟。

在每個(gè)采樣周期內(nèi)的線速度v和角速度ω受到電機(jī)最大最小速度和加減速度性能的限制，即

(22)

每個(gè)時(shí)刻無人車在全局坐標(biāo)系下的平面位置(x,y) 分別等于上一時(shí)刻的位置加上當(dāng)前Δt時(shí)間內(nèi)的位移。DWA 算法在每個(gè)周期內(nèi)會(huì)根據(jù)運(yùn)動(dòng)學(xué)模型和速度、加速度等限制進(jìn)行路徑采樣。為了在投影的2D代價(jià)地圖中計(jì)算通行成本，修改了 DWA 算法，不同之處主要為：增加了語義代價(jià)函數(shù)；在固定窗口內(nèi)采樣等長度路徑。固定窗口參數(shù)：固定車輛速度為v，角速度范圍為[-ω,ω]，角速度增量為Δyaw，在每次采樣周期內(nèi)采樣n條長度為L的軌跡，如圖8所示。

圖8 局部語義規(guī)劃器采樣空間Fig.8 The sampling space of the local semantic planner

4 試驗(yàn)驗(yàn)證

驗(yàn)證平臺如圖9所示，為四輪驅(qū)動(dòng)的無人地面車輛，自主導(dǎo)航系統(tǒng)的主要傳感器有安裝在車輛頂部的3D激光雷達(dá)(Robosense-16E)、安裝在車輛前方的雙目相機(jī)(ZED2)、安裝在車輛中心的 IMU(Xsens MTi-30)、安裝在驅(qū)動(dòng)電機(jī)后的編碼器，以及GNSS模塊(司南M600 mini GNSS)。

圖9 地面無人平臺SCOUTFig.9 Unmanned ground platform SCOUT

4.1 融合定位試驗(yàn)結(jié)果

圖1融合定位系統(tǒng)中，用INS表示慣性定位，EO為輪式里程計(jì)(編碼器)定位，VO為視覺定位(VINS)，LO為激光雷達(dá)定位(LeGO-LOAM)。

(1)街道場景(校園)

路程D≈1460m，定位結(jié)果如圖10所示。采用INS/EO/VO/LO多傳感器融合定位，其平均誤差(2D/3D)為2.210m/2.916m，相對定位誤差<0.2%D。

(a)實(shí)景圖

(b) 定位軌跡/路徑(平面)

(c) 全局定位誤差曲線圖10 街道場景(校園)定位結(jié)果(參考值：RTK)Fig.10 Positioning results in street scene (campus)

(2)樹林場景(夜晚)

路程D≈242m，定位結(jié)果如圖11所示。采用INS/EO/VO/LO多傳感器融合定位，其平均誤差(2D/3D)為0.289m/0.357m，相對定位誤差<0.2%D。

(3)野外場景

路程D≈175m，定位結(jié)果如圖12所示。采用INS/EO/VO/LO多傳感器融合定位，其平均誤差(2D/3D)為0.304m/0.650m，相對定位誤差<0.4%D。

(a)實(shí)景圖

(b) 定位軌跡/路徑(平面)

4.2 環(huán)境感知試驗(yàn)結(jié)果

(1)語義分割

真實(shí)環(huán)境測試(廈大翔安校區(qū)及荒地)的語義分割結(jié)果示例如圖13所示，第1列為原始圖像，第2列為MiniLedNet語義分割結(jié)果;其中第1行為校園環(huán)境，第2行為未開發(fā)的荒地。MiniLedNet能對道路、人行道、建筑物、路燈/桿類、交通標(biāo)志、行人、汽車、天空、高植和草叢等物體進(jìn)行分割。

(a)實(shí)景圖

(c) 全局定位誤差曲線圖12 野外場景(山區(qū))定位結(jié)果(參考值：RTK)Fig.12 Positioning results in wild scene (mountainous area)

圖13 真實(shí)環(huán)境語義分割測試結(jié)果(道路 人行道 建筑 路燈(桿類) 交通標(biāo)志 天空 高植 草叢 未知)Fig.13 Real environment semantic segmentation results

在Cityscapes數(shù)據(jù)集測試中，MiniLedNet相較于LedNet速度提高了31.2%，達(dá)到了20Hz的處理速度，精度損失只有3.1%；在Freiburg森林?jǐn)?shù)據(jù)集測試中，MiniLedNet模型對12類的平均交并比(Mean Intersection over Union, MIoU)達(dá)到79.32%，高于CNNS-FCN和Dark-Fcn。

(2)地圖構(gòu)建

無人車在校區(qū)街道場景下行駛，建立的局部地圖如圖14和圖15所示。通過點(diǎn)云和語義信息的融合、更新，實(shí)現(xiàn)了路面、草叢、高植、建筑、汽車準(zhǔn)確感知與3D建模，建圖速度達(dá)到15Hz。

無人車在荒地和樹林環(huán)境下行駛，建立的局部地圖如圖16和圖17所示，實(shí)現(xiàn)了路面、草叢、高植、巖石的準(zhǔn)確感知與3D建模，建圖速度達(dá)到15Hz。其中過高的草叢在語義分割中被識別為高植。

圖14 城區(qū)環(huán)境局部語義3D地圖Fig.14 Local semantic 3D map of urban environment I

圖15 城區(qū)環(huán)境局部語義3D地圖Fig.15 Local semantic 3D map of urban environment II

圖16 荒地局部語義3D地圖Fig.16 Local semantic 3D map of wasteland

圖17 樹林局部語義3D地圖Fig.17 Local semantic 3D map of forest

4.3 運(yùn)動(dòng)規(guī)劃試驗(yàn)結(jié)果

(1)路沿避讓測試

本測試中路沿區(qū)域也被誤分類為地面，路徑規(guī)劃對于高度大于指定高度的體素(非草叢或高植)，認(rèn)其為障礙物。本試驗(yàn)高度設(shè)置為0.24m，測試結(jié)果如圖18所示，無人車能夠繞行90°到達(dá)目標(biāo)點(diǎn)，沒有與高的路沿發(fā)生碰撞(如果不考慮高度則將發(fā)生碰撞)。

圖18 路沿避讓測試結(jié)果(軌跡)Fig.18 Roadside avoidance test results (track)

(2)巖石避讓測試

測試中巖石被當(dāng)作未知類別(默認(rèn)為障礙物)，語義地圖中巖石可能是黑色或深綠色。如圖19所示，無人車遇到大的石頭時(shí)，能夠成功繞行。

圖19 巖石避讓路徑規(guī)劃結(jié)果Fig.19 Rock avoidance path planning results

(3)草叢可通行測試

如圖20所示，無人車能夠在有路面和草地的環(huán)境中選擇最優(yōu)路徑行駛達(dá)到目標(biāo)點(diǎn)，草叢盡管有一定高度但不會(huì)造成繞行行為。

(4)高植通行對比測試

對于高度比車高低的綠植，路徑規(guī)劃器會(huì)根據(jù)高度信息對高植的語義代價(jià)進(jìn)行修正，得到行駛軌跡如圖21所示，可以看出高度較低的綠植(主要是在語義分割中誤認(rèn)為高植的草叢)并沒有影響通行性。

圖20 穿越草叢的通行測試(軌跡)Fig.20 Passage test through grass (track)

圖21 高植語義代價(jià)修正后的路徑規(guī)劃結(jié)果Fig.21 Path planning results after high plant semantic cost modification

(5)有無語義代價(jià)的DWA測試

(a)測試場景;(b)語義分割結(jié)果;(c)不使用草叢語義信息的 規(guī)劃路徑(失敗);(d)使用草叢語義信息的規(guī)劃路徑(成功)圖22 有無語義信息下的路徑規(guī)劃對比實(shí)驗(yàn)Fig.22 A comparative experiment of path planning with or without semantic information

為驗(yàn)證語義信息的存在與否對DWA算法產(chǎn)生的作用，如圖22(c)所示，傳統(tǒng)的基于幾何的DWA規(guī)劃不會(huì)進(jìn)入草叢，因?yàn)樗偸前延幸欢ǜ叨鹊狞c(diǎn)云當(dāng)作障礙物；如圖22(d)所示，當(dāng)考慮草叢的語義信息后，因?yàn)椴輩彩强梢酝ㄟ^的(代價(jià)低)，因此會(huì)選擇直接進(jìn)入草叢到達(dá)目標(biāo)點(diǎn)goal，說明增加語義信息可以讓DWA規(guī)劃器在實(shí)際可通行的區(qū)域(草叢)表現(xiàn)出類似人的駕駛行為。

以上實(shí)時(shí)路徑規(guī)劃的頻率(1/周期)達(dá)到10Hz。圖14～圖22中，不同顏色代表的含義為：道路草叢高植建筑車輛天空未知障礙；存在誤分割的情況。

5 結(jié)論

本文針對復(fù)雜環(huán)境下地面無人平臺的連續(xù)導(dǎo)航和智能感知規(guī)劃問題，提出了衛(wèi)星拒止下IMU/磁力計(jì)/輪式里程計(jì)/雙目視覺/3D激光雷達(dá)的融合定位、基于多模態(tài)感知的語義3D地圖構(gòu)建與路徑規(guī)劃方案。算法分析與試驗(yàn)結(jié)果表明：

1)基于序貫式ESEKF的融合定位算法，擴(kuò)展性好、場景適應(yīng)性強(qiáng)，為后續(xù)全源導(dǎo)航的傳感器即插即用及在線配置問題提供了解決思路。

2)改進(jìn)的深度學(xué)習(xí)(圖像)語義分割算法 —— MiniLedNet，在保持精度的同時(shí)具有快速推斷能力；語義與點(diǎn)云的融合為自主導(dǎo)航提供了類人的智能感知規(guī)劃能力，克服了基于幾何地圖方法的不足。

3)在真實(shí)環(huán)境中進(jìn)行了整個(gè)系統(tǒng)的試驗(yàn)驗(yàn)證，表明該系統(tǒng)能夠使無人車實(shí)時(shí)完成導(dǎo)航任務(wù)。為了進(jìn)一步驗(yàn)證和完善所建立的導(dǎo)航定位、感知、規(guī)劃方法，今后將在雨天、灰霧、復(fù)雜地形、電磁干擾環(huán)境下進(jìn)行更充分的測試和完善。