語素關聯約束的動態環境視覺定位優化

邵曉航，吳杭彬，劉 春，陳 晨，蔡天池，程帆瑾

同濟大學測繪與地理信息學院，上海 200092

高精度定位是自動駕駛的核心技術[1]。傳統的GNSS(Global Navigation Satellite System)定位技術易受樹木、建筑和隧道等障礙物遮蔽的影響。這時，同時定位與地圖構建(simultaneous location and mapping，SLAM)成為自動駕駛技術的定位方案之一。視覺SLAM不僅成本低、易維護，還可以獲得豐富的色彩、紋理信息，逐漸成為該領域的重要補充[2-3]。

PTAM[4]是其中第1個采用并行方式實現跟蹤和建圖的SLAM方案，它通過非線性的BA(bundle adjustment)優化，能夠實時獲得圖像關鍵幀的軌跡和姿態。ORB-SLAM[5-7]在此基礎上新增了回環檢測線程，并利用改進的圖像特征，實現了基于單、雙目相機和RGB-D相機的定位和跟蹤，它也是當前基于特征點法中最成熟的開源視覺SLAM方案。除此之外還有基于像素灰度變化的直接法[8]和半直接法[9]等方案。

這些方法在定位過程中普遍存在“靜態世界”的假設，即假設像素或特征點的空間位置是不變的，而造成圖像灰度和視角變化的唯一原因是相機的位姿變化。然而自動駕駛場景是動態的，行人、汽車等移動物體會影響視覺特征關聯的準確性。雖然這種影響可以通過RANSAC[10]或者核函數[11]過濾，但這是一種被動機制，并沒有從根本上改變這一假設。因此，如何在動態場景中尋找更穩健的特征是視覺定位技術應用于自動駕駛的關鍵之一。

解決上述問題有兩種思路[12]。一種是把動態目標的運動囊括進SLAM的估計過程，例如采用粒子濾波方法克服錯誤估計的影響[13]，但是粒子的數量選擇和維持實時計算較困難。另一種更普遍，是讓SLAM方法結合深度學習技術[14-15]，分割圖像中的動態區域，從而把它轉換為靜態環境的SLAM問題。例如，DS-SLAM[16]算法使用SegNet[17]識別環境中的物體，并將人體去除；DynaSLAM[18]算法采用Mask-RCNN[19]檢測動態物體，過濾目標的同時補全被遮擋部分；MaskFusion[20]實現了一種實例級別的語義分割，以更加完全地過濾動態特征。DetectionFusion(arXiv:1707.09127,2019.)在實例分割的基礎上識別了未知運動目標，從而實現更為完整的動態過濾。還有學者結合道路結構化特征，設計了帶有改進金字塔池化模塊的語義分割神經網絡[21]。這些方法通過設計精巧的神經網絡完成動態環境中的目標識別，但它們依賴于動態目標的分類規則，忽略了物體可能有“動態”和“靜態”雙重屬性的事實。例如行駛的汽車是動態的，但是停靠的汽車是靜態的；并且靜態的物體仍能為視覺SLAM過程提供豐富的特征。因此有學者提出Detect-SLAM的方法[22]，考慮了物體上特征點的運動特性，在識別出語義目標的基礎上為其分配運動概率，每一幀位姿在估計時都更新概率值并去除高于閾值的運動點，但這方面研究較少。

綜上所述，目前視覺定位SLAM技術存在受動態目標影響大，且在復雜環境中不易準確分辨動態目標。為此，筆者提出了基于語素關聯的方法，并且基于經典的ORB-SLAM方法建立了動態環境中的視覺定位優化流程。

1 語素關聯約束的定位優化方法

傳統視覺定位的原理是：通過幀間匹配計算相同關鍵點之間的像素差異，從而估計產生這種差異的位置和姿態(簡稱位姿)變化，同時輔以閉環優化以減少誤差，其流程如圖1(a)所示。然而提取的特征點可能位于動態目標上，但基于“靜態世界”假設，這些特征點不利于位姿估計，具體說明如圖2所示。因此，本文提出的視覺定位優化方法在傳統流程基礎上，通過語義關聯識別環境中的動態語義要素(簡稱動態語素)，并且形成特征掩膜用于“特征匹配”過程，從而幫助過濾動態特征點以獲得“穩定的特征”，進而優化后續位姿估計，如圖1(b)所示。

圖1 基于語義要素關聯優化的動態環境定位流程Fig.1 Flowcharts of location optimization in dynamic scenes by semantic association

1.1 動態環境的影響

一般SLAM算法認為環境中的地標點(圖像中關鍵點所對應的物體)是固定的，空間中同一個地標在前后兩幀圖像中的像素差異，僅源于相機的位置和姿態改變。因此，當相機內參已知時，它的位姿和地標點之間存在如式(1)的關系

ξ(P,R)=Γ(K,M)

(1)

式中，K是用像素坐標表達的關鍵點；M是相機的內參數；ξ為相機的位姿，是一個與位置P和旋轉R有關的矩陣；Γ(·)是一個抽象函數，具體的表達可參考文獻[23—24]。

圖2 場景的動態物體對于視覺定位的影響Fig.2 Negative effects of moving objects in dynamic scenes

1.2 語義實體識別

獲得動態語素的第1步是識別出視覺圖像中的語義實體。假設環境中的語義實體G由位置L、大小S和類別M表達，即G={L,S,M}，根據相機的投影模型，圖像中的像素點(x,y)和g∈G存在如下的映射關系

g=f(x,y)

(2)

語義實體識別就是確定映射f(·)的過程。本文利用了以YOLO(arXiv:2004.10934,2020)為代表的目標識別算法和以SegNet[17]為代表的語義分割算法。在YOLO算法中，M為識別出的目標類別，L和S分別是目標最小外包矩形(bounding box)的邊界和大小。在語義分割方法中，M為分割出的目標類別，L和S分別是圖像中的屬于指定類別的像素坐標和數量。

1.3 語義要素抽象和關聯

語義要素約束是通過語素抽象和關聯實現的，其形式是動態特征掩膜。本文中，語素抽象就是提取語義實體識別出的元素，例如圖1(b)中列出的人、道路、汽車、自行車等元素。語素關聯就是通過建立不同元素之間的關系，判斷關聯后的語義是動態或靜態。例如“汽車”和“道路”關聯的結果是“行駛的車”這種動態語素，而“汽車”和“車位”關聯的結果是“停放的車”這種靜態語素。語素關聯能體現“汽車”具有“動”“靜”雙重屬性的事實。

在視覺圖像中，關聯性通過像素區域的位置關系和面積大小計算得到。設有兩個語義實體g1,g2∈G，它們在圖像中的關系可以按照式(3)計算

(3)

式中，h(·)表示計算實體在圖像上的像素區域和面積；∩表示計算重疊像素區域的面積；∪表示計算合并后像素區域的面積；L表示像素位置；S表示像素大小；r1、r2為兩種指標計算方法；r1是相交區域面積比其中一種實體區域面積的指標，它側重體現動態部分與其中一種語義實體的關系；r2是相交區域面積比兩種實體合并后區域面積的指標，它側重體現動態部分與兩種實體之間的關系。當關聯的兩者僅有一個具有動態性時，例如“人和道路”中“人”具有動態性，更加適用于r1指標，此時g1代表“人”；而當關聯的兩者都具有動態性的時，例如“人和自行車”，更加適用于r2指標。

2 試驗與分析

2.1 試驗環境和數據采集

為了驗證提出的方法，本文基于自主研發的移動視覺機器人平臺在校園道路場景開展了試驗。該平臺為小型輪式移動機器人(圖3(a))，搭載了雙目RGB相機，相片分辨率為640×400像素，采樣頻率30 Hz。

如圖3(b)所示，試驗行駛路線全程約2.4 km，途經食堂、教學樓和校門，全程48 min，共得到約8萬對圖像；其中，除校門前的路段為機動車道路外，其他路段均為非機動車道路。如圖3(c)所示，SOI-1至SOI-6是典型動態場景的圖像興趣序列(sequence of interest)。由于“汽車”“自行車”“行人”是該環境中影響視覺定位的主要語義實體，因此本文利用YOLO目標識別方法識別這3種目標，利用SegNet語義分割方法分割與之相關的“道路”“停車位”。

圖3 試驗數據采集平臺和過程Fig.3 Equipment and data collection

表1以6個興趣序列為例，不分實例地統計了每段中各個實體出現的總頻次。從表1中可以看出，SOI-1和SOI-2所對應的食堂、教學樓附近人、自行車較密集，機動車道路段中的SOI-4序列中有汽車駛過。環境中的停車區讓汽車或自行車具備了“動”“靜”雙重屬性，這給動態要素的準確識別帶來困難。

表1 局部場景圖像興趣序列信息

2.2 語素關聯和定位約束

根據場景的語義特點，參考圖1(b)擬提取3種動態語素，其含義見表2。同時為了確定動態語素關聯的閾值，從興趣序列中選擇樣本，根據式(3)計算了3種動態語素的r指標的分布，并把它們從大到小排序，結果如圖4所示。其中，根據指標特性在動態語素1和動態語素3上統計r1指標，而動態語素2上統計r2指標。需要說明的是，由于環境中車輛少、機動車路段集中，動態語素3的樣本不夠，因此相似場景下計算得到的r值較為接近。

圖4 r指標統計結果Fig.4 Statistics for r value

表2 語素關聯過程和閾值

考慮到閾值應滿足篩選出大部分動態語素的要求，試驗中以成功篩選出90%樣本為目標確定了r的閾值，得到取值區間見表4。對于本試驗的圖像序列，如果計算出的r落入當前指定區間，則認為該語素對應的像素區域屬于動態語素區域，則構建特征掩膜以過濾相應特征點；反之則不構建掩膜。需要說明的是，在以樣本估計總體閾值的過程中，本文方法基于兩個前提：不考慮靜態語素的關聯性和動態語素關聯性越強r值越大。

動態語素關聯通過特征掩膜參與定位過程，如圖5所示。在圖5(a)中，通過語義實體識別可以得到“自行車”和“人”兩種語義實體，由最小外包矩形標注；通過語義分割方法可以得到“道路”實體，由多邊形標注。根據人與自行車的關聯性可以判斷出圖像存在“騎行目標”語素，如圖5(b)中紅色區域； 根據人與道路的交集可以判斷圖像中存在“行人”語素，如圖5(b)中藍色區域。它們在視覺定位中都屬于動態語素，因此根據其范圍形成特征掩膜，如圖5(c)所示。在SOI-4中， 通過語義實體識別可以得到“汽車”“停車位”“道路”3種語義實體。通過“汽車”與“停車位”的交集判斷出“停放的汽車”語素，通過“汽車”與“道路”的交集判斷出“車行目標”語素，如圖5(e)中黃色區域。后者在視覺定位過程中屬于動態語素，因此根據其范圍形成特征掩膜，如圖5(f)所示。

圖5 語素關聯優化過程Fig.5 Semantic elements association

視覺定位過程中，高響應值的特征點優先參與位姿解算過程。然而它們中有一部分會落在動態語素目標上，這些點并不利于位姿計算。語素關聯約束過程會過濾掉動態語素上的關鍵點，如圖5(b)、(e)所示，綠色的關鍵點被保留并參與位姿估計，而紅色的關鍵點被特征掩膜過濾。

2.3 試驗結果和分析

2.3.1 局部優化結果分析

本文以SOI-1至SOI-6 6個場景的興趣序列為例，詳細說明在局部場景中語素關聯優化對視覺定位結果的提升。圖6展示了序列中代表性的圖像，標注了主要動態對象的移動方向和優化前后的定位軌跡。其中，本文方法指經過語素關聯約束得到的定位軌跡，對比方法指過濾所有人、車、自行車目標后的定位軌跡，原始軌跡指不過濾任何目標的定位軌跡。機器人的實際運動狀態為向前行駛(SOI-1、SOI-2和SOI-4)和轉彎(SOI-3、SOI-5和SOI-6)兩種狀態。

圖6 動態場景和語素關聯優化前后視覺定位軌跡對比Fig.6 Performance of semantic elements filtering in dynamic scenes

從試驗對比中可看出，動態目標通過特征點影響視覺定位結果，識別和消除其影響可以獲得更穩定的視覺定位結果。在SOI-1中，有運動的自行車和行人。其中一輛自行車從右后方駛入視野，為了避讓行人而左拐進入步行道。自行車相對于機器人移動速度快，剛開始占據相機大部分視野，隨后逐漸消失。由于車身尾部顏色對比明顯，部分關鍵點落在自行車上。另一輛自行車由遠處迎面駛來，但目標較小，其上的關鍵點不多。視野中還有很多停放在路邊的自行車，它們提供了一些可用于定位的關鍵點。從軌跡對比結果可以看出，雖然機器人實際保持向前行駛，但是原始軌跡表現為先后退、再右轉。造成這種現象的原因是：相對于機器人，主要運動自行車上的關鍵點發生了向左前方的快速移動，按照視覺定位算法“靜態世界”的假設，如果認為關鍵點是靜態的，那么相機就應該發生向右后方向的移動。相對比地，無論是本文方法還是對比方法得到的軌跡都一直保持向前，比較符合機器人的行進路線。

這種情況在其他場景序列中也表現明顯。SOI-2序列中有行人從右向左橫穿視野，其書包和衣服紋理豐富，提供了大量視覺關鍵點。但是在運動目標的影響下，原始軌跡發生了向右的移動；經過特征掩膜過濾后，優化后定位軌跡不再存在這種情況。SOI-3序列中機器人緩慢向左拐彎，有人騎車從右向左穿過視野，經過優化后的軌跡更加平滑。SOI-5序列中采集車緩慢左拐彎，有人在視野中先向右前方移動，然后轉向左前方；由于行人運動的影響，算法認為機器人提前開始左轉，而在優化后的軌跡中，機器人先保持向前行駛一段距離后再左轉。

然而雖然本文方法和對比方法都能減少運動目標的影響，但是本文提出的語素關聯方法能夠在定位時保留更多具備高響應值的特征點。例如SOI-4序列中只有1輛汽車從視野中駛過，由于其上特征點少而且僅4 s就駛出視野，原始軌跡與本文方法軌跡差別不大。但由于對比方法丟失了一些靜止汽車上的高響應特征，其定位軌跡在尺度上與前兩者具有明顯差別。需要特殊說明的是，如果動態語素區域的關鍵點少，那么三者的定位軌跡區別也不大，例如SOI-6序列中特征點集中于植被和樹枝，而不在騎行人身上。

表3記錄了上述動態場景序列的時長，并且從本文方法和原始軌跡中采樣了受影響的定位點，計算相應兩個采樣點的差距。由表3可以看出，采樣點的最大差距在0.1～2.5 m之間；最大平均差距出現在SOI-3序列中，為1.454 m，該序列中機器人正在轉彎，并且有行駛快的自行車橫向穿過。最小平均差距在SOI-6中，該場景的關鍵點集中在非動態區域，優化作用不大。

表3 局部區域語素優化前后定位軌跡差統計

分析表明，語素關聯約束對動態場景的視覺定位結果有一定的提升作用，尤其在相機轉彎時或者動態目標在視野中產生橫向移動時優化效果明顯。然而，由于動態目標是通過關鍵點影響視覺定位的，如果運動目標本身視覺特征不明顯、出現時間不長或者運動幅度不大，優化前后定位軌跡沒有明顯差異。

2.3.2 整體優化結果和分析

因激光傳感器具有精度高、不易受動態環境影響的優勢，定位結果更穩定可靠[25]，故以激光雷達數據計算得到的軌跡為真值，采用了EVO(https:∥github.com/Michae(Grupp/evo))軌跡評價工具，對比了原始軌跡(不處理動態目標)、對比方法(剔除所有“人”“車”目標)和本文方法的二維軌跡結果，結果如圖7所示。圖7(a)說明視覺軌跡與真值之間存在仿射變換差異，X軸是初始狀態的左右方向，Y軸是初始狀態的前后方向。經過旋轉、平移和縮放變換后，本文方法更加貼近真值。圖7(b)的“小提琴圖”展示了絕對位姿誤差(absolute pose error,APE)中位移分量的分布，本文方法平均誤差更小，而且分布更加集中，這說明軌跡差更加穩定。表4為視覺優化前后軌跡與真值誤差對比。

由表4和圖7(b)的誤差柱狀圖可以看出，本文方法定位軌跡在均方根誤差、偏差均值、偏差中位數、偏差標準差和最大偏差多個指標都小于對比方法，遠小于原始軌跡。相對于原始軌跡和對比方法，均方根誤差分別降低了38%和17%。

圖7 語素優化前后視覺定位整體軌跡對比Fig.7 Full trajectory before and after semantic elements filtering

表4 視覺優化前后軌跡與真值誤差對比

2.3.3 精度和效率分析

以6段興趣序列為基礎，觀察和統計了語素關聯結果的正確性，得到預測與觀察結果的混淆矩陣見表5。同時選擇了精確率P、召回率R和綜合指標F1來反映方法的精度，計算過程如式(4)—式(6)所示

(4)

(5)

(6)

經計算，P約為91%，R約84%，平均精度F1約為87%。

為了評價計算效率，以6個興趣序列為例，統計了其中語素關聯步驟(計算和判斷r值)的耗時，由表3可以看出，該步驟的效率與場景中語義的復雜性有關，序列要判斷的動態語素種類越多耗時越高。同時對比序列的時長可以看出，語素關聯步驟能夠與數據采集同步。但是，語素關聯計算的前提是語義實體識別，即目標識別和語義分割過程。在單核2.2 GHz CPU的計算平臺上進行，處理每張圖片的耗時約0.8 s。因此，由于語義實體識別耗時過大，當前方法仍不能達到實時計算的效果。

表5 預測和觀察結果的混淆矩陣

3 結論和展望

本文提出的動態環境視覺定位優化方法，能夠解決現有的特征點法受動態目標影響大、視覺定位結果準確度下降的問題。在保留更多高響應值特征點的基礎上，能夠較好地抑制由于動態實體橫向穿越相機視場而導致的定位誤差。

視覺定位是自動駕駛場景定位方案的重要補充，但環境中的動態目標不是影響其的唯一因素，圖像匹配誤差、相機標定誤差和深度估計誤差都會影響視覺定位結果[26]。另外，由于語義識別耗時，當前方法無法達到實時優化效果，并且受平臺限制本文未能討論其在城市級道路的定位提升能力。在更復雜的無人駕駛場景中，如何更好地優化視覺定位結果仍有待深入研究。

致謝：感謝武漢大學郭遲教授團隊在結果分析的對比方法中提供幫助。