基于多模態感知與融合的無人車韌性導航系統

2021-12-02 04:57:58仲訓昱武東杰陳登龍莊明溪吳汶鴻彭俠夫

導航定位與授時 2021年6期

仲訓昱，武東杰，陳登龍，莊明溪，吳汶鴻，彭俠夫

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

0 引言

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

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

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

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

1 基于濾波的融合定位

1.1 融合框架與子系統設計

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

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

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

1.2 ESEKF多模態信息融合位姿估計器

設計的ESEKF融合算法實現流程如圖2所示，主要包括全狀態預測、誤差狀態EKF預測、誤差狀態EKF更新和誤差補償與重置等模塊。

圖2 基于ESEKF的多模態融合定位架構Fig.2 Multi-modal fusion positioning architecture based on ESEKF

全狀態x∈R16定義為

(1)

(2)

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

ESEKF多模態信息融合位姿估計器的思路是[17]：1)利用高頻IMU數據預測當前時刻的全狀態和誤差值；2)基于EKF算法，利用多模態子系統的測量值獲得系統誤差的最優估計值；3)將誤差估計值和全狀態預測值合成，獲得系統全狀態的最優估計。

(1)全狀態預測估計過程

設am和ωm分別為加速度計和陀螺儀的測量值，利用數值積分知識可得到全狀態的預測方程，設為

(3)

(4)

其中，R∈SO(3)表示姿態的旋轉矩陣；g為重力加速度矢量；q{}表示將旋轉向量轉換為相應的四元數；?表示四元數乘法。

(2)誤差狀態EKF估計過程

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

(5)

其中

Fk=

wF,k～N(0,QF)

其中，I為單位矩陣；0為零矩陣；[]×表示將向量轉換為對應的反對稱矩陣；wF,k為過程噪聲，其協方差設為QF,k。

當前時刻的全狀態測量模型設為

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

(6)

其中，函數h的具體形式由子系統的輸出性質決定；wh,k+1表示測量噪聲，其協方差記為Rh,k+1。

根據EKF框架，誤差狀態最優估計過程為[16]

(7)

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

(8)

其中

位置測量方程為

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

(9)

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

姿態測量方程為

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

(10)

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

(3)全狀態更新估計過程

1)誤差補償

(11)

2)誤差重置

當完成對全狀態預測值的補償后，需要對EKF誤差狀態估計器中的變量進行重置，過程如下[16]

(12)

其中

2 環境感知與語義3D地圖構建

2.1 基于深度語義分割的環境感知

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

2.2 融合激光點云的語義3D地圖

如圖4所示，雙目相機RGB圖像經MiniLedNet得到語義圖像，與 3D 激光雷達的點云融合得到語義點云，再結合融合定位信息，通過八叉樹存儲結構的占有更新、語義更新建立局部和全局語義3D地圖[19]。此外，為了獲得更高精度的語義3D地圖，通過語義標簽分布修正和運動目標去除算法對地圖進行糾錯和修正，為地面無人車平臺執行復雜任務提供準確的導航特征地圖。

(a) MiniLedNet

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

(1)單幀語義融合

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

(13)

為了得到每個像素最可能的標簽概率，取所有類別標簽概率的最大值

圖4 語義3D地圖構建總流程圖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)

在得到每個像素的語義標簽和標簽概率后，通過外參矩陣將其與對應的點云進行融合得到語義點云。

(2)體素占有概率更新

當新的單幀融合后的語義點云插入到八叉樹存儲結構的地圖時，八叉樹內每個體素占有概率的更新公式如下[21]

P(n|z1:t)=

(15)

其中，z1:t為第1～t時刻所有的觀測數據；P(n|z1:t)為第n個節點基于第1～t時刻觀測數據的后驗概率；P(n|zt)為第n個節點基于當前觀測數據的占有概率；P(n)為節點n的先驗概率(取0.5)。

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

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

(16)

(3)體素語義信息更新

采用八叉樹結構存儲語義地圖，每個語義點云用立方體柵格表示，其中每一個體素在多幀圖像中都可能被觀察到并可能被語義分割網絡預測一個不同的語義類別，因此需要通過融合多個時刻觀察到的語義來確定一個體素的最終類別。

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

(17)

最大概率融合后的當前體素v的標簽概率P(c|zt)的計算如下

P(c|zt)=

(18)

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

(4)體素語義修正

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

(19)

(5)運動目標去除

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

(20)

(21)

其中，Di為體素數量；Li為ith聚類的長度；ηd和ηl為相應的閾值。

3 基于語義3D地圖的運動規劃

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

圖5 基于語義地圖的運動規劃Fig.5 Motion planning based on semantic map

3.1 語義和高度投影

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

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

3.2 單條路徑通行代價計算

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

圖7 通行代價計算Fig.7 Passage cost calculation principle

3.3 基于DWA的運動規劃

采用DWA實現局部路徑規劃，主要流程如下：

1)在無人車運動空間進行速度離散采樣(Δx,Δy,Δθ)；

2)對每一個采樣速度執行行進模擬，檢測使用該采樣速度移動一段時間會發生什么；

3)評價行進模擬中每個前行軌跡，評價準則有：靠近障礙物、靠近目標、貼近全局路徑、丟棄非法軌跡(如靠近障礙物的軌跡)；

4)挑出得分最高的軌跡(路徑)并發送對應的速度給運動控制器；

5)重復上面步驟。

在每個采樣周期內的線速度v和角速度ω受到電機最大最小速度和加減速度性能的限制，即

(22)

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

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

4 試驗驗證

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

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

4.1 融合定位試驗結果

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

(1)街道場景(校園)

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

(a)實景圖

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

(2)樹林場景(夜晚)

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

(3)野外場景

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

(a)實景圖

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

4.2 環境感知試驗結果

(1)語義分割

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

(a)實景圖

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

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

(2)地圖構建

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

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

圖14 城區環境局部語義3D地圖Fig.14 Local semantic 3D map of urban environment I

圖15 城區環境局部語義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 運動規劃試驗結果

(1)路沿避讓測試

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

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

(2)巖石避讓測試

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

圖19 巖石避讓路徑規劃結果Fig.19 Rock avoidance path planning results

(3)草叢可通行測試

如圖20所示，無人車能夠在有路面和草地的環境中選擇最優路徑行駛達到目標點，草叢盡管有一定高度但不會造成繞行行為。

(4)高植通行對比測試

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

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

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

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

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

為驗證語義信息的存在與否對DWA算法產生的作用，如圖22(c)所示，傳統的基于幾何的DWA規劃不會進入草叢，因為它總是把有一定高度的點云當作障礙物；如圖22(d)所示，當考慮草叢的語義信息后，因為草叢是可以通過的(代價低)，因此會選擇直接進入草叢到達目標點goal，說明增加語義信息可以讓DWA規劃器在實際可通行的區域(草叢)表現出類似人的駕駛行為。

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

5 結論

本文針對復雜環境下地面無人平臺的連續導航和智能感知規劃問題，提出了衛星拒止下IMU/磁力計/輪式里程計/雙目視覺/3D激光雷達的融合定位、基于多模態感知的語義3D地圖構建與路徑規劃方案。算法分析與試驗結果表明：

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

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

3)在真實環境中進行了整個系統的試驗驗證，表明該系統能夠使無人車實時完成導航任務。為了進一步驗證和完善所建立的導航定位、感知、規劃方法，今后將在雨天、灰霧、復雜地形、電磁干擾環境下進行更充分的測試和完善。