基于因子圖優化的眾包高精度地圖云端融合方法

谷 崢， 隗寒冰， LIU Zheng，2， 婁 路， 鄭國峰

（1.重慶交通大學，重慶 400074；2.University of British Columbia Okanagan，Kelowna，BC，V6T1Z4，Canada）

高精度地圖（High-Definition Map，HD-MAP）定位精度可達厘米級，能提供比傳統導航地圖更加準確和詳細的地圖數據，是實現自動駕駛汽車自主定位的關鍵［1］。高精度地圖作為自動駕駛的關鍵核心技術之一，在定位［2-5］、感知［6］、決策［7］方面都有廣泛應用。

傳統高精度地圖的基本制作流程是，配備高性能位置傳感器的車輛沿著目標路線行駛以獲取測繪數據，然后基于測繪車輛上傳的道路地圖數據，對地圖信息進行特征分類，最后根據地圖中的特征信息進行手動細化和數據確認。該方法存在以下缺點：

1）地圖測繪車輛需要配備高性能傳感器，地圖制作成本高昂；

2）需要對采集到的特征信息進行人工修正和標注，耗費大量人力資源；

3）道路狀況發生變化時需要重新采集數據上傳并驗證，地圖更新速度較慢。

為了克服傳統高精度地圖生成方法的不足，學術界開始致力于研究眾包地圖（Crowd-Sourced Mapping，CSM），即通過在車端布置低成本傳感器，如相機、GPS、GNSS/IMU 來采集道路數據，在云端服務器中進行地圖軌跡融合處理后再下發局部地圖到車端以進行定位和導航。目前，視覺V-SLAM 算法趨近成熟，如基于稀疏特征的ORBSLAM3［8］相比前兩代ORB-SLAM［9-10］精度提升了2～5 倍，且實現了VO 丟失后的地圖再融合功能。DSM［11］是首個完全基于直接法實現回環檢測和地圖重用的SLAM 系統。VINS-Fusion［12］方案利用基于視覺結合IMU的緊耦合滑窗優化算法，實現了很高的定位精度，并且其對計算資源的消耗較小，能在普通CPU上實時在線運行。因此，不少眾包地圖研究都以V-SLAM作為車端建圖基礎。HERB 等［13］利用ORB-SLAM 作為車端局部地圖來重建算法，利用卷積神經網絡（Convolutional Neural Networks，CNN）對關鍵幀進行語義分割并上傳特征信息，然后在云端對各關鍵幀進行特征匹配并對齊，最后利用上傳的特征信息對道路邊沿進行合并重建。但利用特征匹配并對齊的方式存在誤匹配問題。KIM等［14］利用Graph- SLAM［15］方法在車端局部地圖中構建新的特征層并上傳，然后在云端對多車上傳的特征層進行融合，在增加地圖信息豐富度的同時極大地加快了地圖的更新。然而，該方法極度依賴車端傳感器對所需特征的檢測，實用性較低。

針對傳統高精度地圖云端融合方法生成的地圖存在置信度較低、誤差較大等問題，本文開發了一種基于因子圖優化的服務器云端多地圖融合算法。首先，利用絕對全局位姿信息進行多個地圖片段之間的匹配，并采用一致性篩選方法保證匹配對的準確率；然后，在此基礎上構建了圖優化模型以求得地圖間的最優變換矩陣；最后，結合最優變換矩陣對原始地圖進行映射以完成地圖片段間的融合。此外，還提出了一種高精地圖融合評價指標，并以此為標準對試驗結果進行對比評估。

1 眾包高精度地圖基礎框架

借鑒QIN Tong 等［16］提出的眾包建圖思想，本文采用的高精度地圖生產方法主要有兩部分，即車載前端和服務器云端，整體基礎框架如圖1所示。

圖1 眾包高精度地圖框架

1.1 車載前端

1.1.1 圖像分割

利用基于CNN 的方法［17-18］，對車輛采集的圖像進行所需特征如車道線、馬路邊沿線、停止線、道路標識等的語義分割。然后通過逆透視變換（Inverse Perspective Mapping，IPM）將提取的語義特征從圖像平面反向投影到車輛坐標系下的地面。假設地面為水平面，像素［u，v］投影到水平地面的坐標變換為：

式中：s為尺度；T為車輛后軸中心到相機的變換矩陣；C為相機投影模型。

1.1.2 局部地圖構建

采用基于位姿圖優化理論的里程計構建方法［12］，對車輛位姿進行全局優化，再利用優化后的車輛位姿將上述提取的語義特征從車輛坐標系轉換到全局坐標系，如式（2）所示。

式中：R(qi)表示將i時刻車輛位姿qi轉化為旋轉矩陣；pi為i時刻車輛在全局坐標系下的位置。

1.2 服務器云端

服務器云端的主要工作是完成多地圖間的融合，主要分為3 個步驟：多車地圖矢量片段匹配、矢量片段一致性篩選和基于因子圖優化的地圖合并。

步驟一：多車地圖矢量片段匹配。

通過建立全局位姿圖模型，利用上傳至云端的RTK-GPS數據對相應局部地圖進行全局化處理，再根據處理后地圖間位姿的相對關系完成地圖片段間的粗略匹配。

步驟二：矢量片段一致性篩選。

針對步驟（1）可能會得到錯誤的匹配關系，本文利用里程計的全局一致性，構建地圖匹配對之間的誤差模型，將誤差優化問題轉化為求圖論中的最大團問題，最后得到符合全局一致性的地圖間匹配關系。

步驟三：基于因子圖優化的地圖合并。

將地圖間目標匹配對轉化為約束地圖間變換關系的因子節點，構建因子圖優化模型，最終求得地圖間全局最優的變換矩陣。

本文的主要研究內容在服務器云端上，所以不對車載前端建圖部分做過多描述。

2 基于圖優化的云端地圖融合

2.1 多車地圖矢量片段匹配

在SLAM 位姿因子圖中，通常用xi∈SE(3)表示機器人在某時刻的位姿。因子圖中的因子是由車輛的觀測值鏈接而成，即馬爾可夫鏈。通常用zij表示與變量xi和xj相關的測量值，如果i和j在時刻上是連續的，則稱其為里程計測量值；如果i和j在時刻上不連續，則稱其為閉環測量值。位姿圖的目標就是在給定測量值zij的情況下估計每個位姿變量xi最可能的值。

對單車SLAM問題的描述如式（3）所示。

式中：χ為所有位姿變量xi的集合；Z為所有觀測值zij的集合。

對于多車SLAM 地圖問題，還需要估計各車輛局部坐標系之間的相對位置變換關系。借鑒KIM等［19］與QIN Tong 等［12］的做法，本文引入GPS 測量值作為位姿圖的全局測量因子，從而得到統一的全局坐標系。全局位姿結構如圖2所示。

圖2 全局位姿結構

圖中，藍色節點為車輛在某一時刻的位姿xi，包含了車輛的位置p 和朝向q；紫色邊為GNSS 約束，其僅僅約束節點的位置；黃色邊為里程計約束，其約束相鄰兩節點的位姿。全局位姿圖優化問題可表示為：

式中：xi為位姿（位置和朝向）；ro為里程計殘差；為里程計測量值，即相鄰兩時刻的位置變換Δpi-1，i以及朝向變換Δqi-1，i；rg為GNSS 殘差；zig為GNSS測量值，即全局坐標系中的絕對位置pi。

殘差ro與rg的定義如式（5）所示。

式中：R(q)為將四元數q轉化為旋轉矩陣；[q-i1·qi-1· Δqoi-1，i]xyz為四元數的前3個元素。

為了提高運算效率，將以上步驟得到的地圖片段進行網格化處理。本文將地圖片段分割為多個5 m的地圖矢量片段，并且以地圖矢量片段中里程計的起點位姿xs代表該地圖矢量片段，最后利用角度和距離關系對地圖矢量片段進行匹配。

2.2 矢量片段一致性篩選

里程計誤差的存在可能導致匹配關系不符合全局一致性，而直接判斷某個地圖矢量片段匹配對是否為異常值較為困難。采用MANGELSON 等［20］提出的一致性篩選方法，對獲得的地圖矢量片段匹配對進行篩選，從而得到一個符合全局一致性的匹配對子集合最大值，算法結構如圖3 所示。圖中，步驟1 為待篩選的兩車地圖間的匹配關系示意圖。其中，藍色節點和綠色節點分別表示a、b兩車在某一時刻的位姿；黃色邊表示單車軌跡間的里程計約束；虛線邊表示兩車位姿間的匹配關系，其中包含了待剔除的匹配關系。步驟2 表示一致性矩陣，其將符合一致性度量的匹配關系對進行二值化處理并儲存，即符合一致性度量的匹配關系對之間的顏色塊更深。步驟3 為根據一致性矩陣建立的一致性圖，其中黃色陰影部分表示一致性圖中的最大團，即符合一致性度量的所有匹配關系。步驟4 為一致性篩選后兩地圖間的匹配關系，其剔除了不符合一致性度量的匹配關系。

圖3 一致性篩選算法結構

2.2.1 成對一致性

定義：對于一個測量值集合Z?，如果其滿足成對連續一致性，如式（6）所示。

式中：C為度量函數；zi，zj為集合?中不同的匹配測量值；γ為給定的閾值條件。

度量函數C的定義如式（7）所示。

式中：為車輛a 在i時刻與車輛b 在k時刻的匹配關系測量值；為車輛a 在i，j時刻間的里程計測量值；||·||∑為馬氏距離。

2.2.2 一致性圖模型建立與求解

根據成對一致性的定義，對得到的匹配集合Z?進行篩選，并將篩選結果進行二值化處理，即滿足閾值條件的匹配對置1，否則置為0，并將結果存儲在如圖3（步驟2）所示的二值化矩陣中。

根據以上步驟得到的矩陣，可以明確得到任意一對匹配測量值的一致性關系，以此關系建立一致性圖模型，如圖3（步驟3）所示。圖中，節點表示某一匹配關系測量值，邊表示兩匹配關系測量值之間的一致性關系。滿足成對一致性閾值的兩節點相連，否則不相連。

圖論中的團被定義為頂點的子集，此子集中任意兩頂點之間必有一條邊。最大團問題（Maximum Clique Problem，MCP）是指在圖中尋找包含頂點個數最多的一個團。因此，尋找集合Z?中的最大一致性子集問題就是尋找一致性圖中的最大團問題。本文采用PATTABIRAMAN 等［21］提出的搜索方法作為最大團問題解決法，最終得到符合全局一致性的匹配關系，如圖3（步驟4）所示。

2.3 多約束因子圖

得到最佳地圖矢量片段匹配對之后，需要對矢量片段中車道線以及標志牌等目標進行匹配，并進行位姿化建模處理，然后建立多約束因子圖模型，其流程如圖4 所示。圖中，步驟1 表示對匹配上的兩地圖片段中的目標進行匹配；步驟2 為車道線位姿化建模過程中的示意圖；步驟3 表示根據匹配對建立的因子圖模型。具體含義以及構建過程將在下文介紹。

圖4 多約束因子圖建立流程

本文采用貪心算法分別從角度和距離兩方面對地圖矢量片段匹配對中的目標進行匹配，匹配結果如圖4（步驟1）所示。

為了構建圖優化模型，需要對車道線進行位姿化處理。為便于理解，以二維平面坐標系為例，如圖4（步驟2）所示。圖中，紫色線段表示實線車道線片段；綠色線段表示虛線車道線片段；pl為實線段中點坐標；pb為虛線段起點坐標；θ為虛線段與坐標軸間的夾角。因此，設旋轉矩陣R=平移矢量t=(x2，y2)T。則虛線段的李群表示為：

實線段的李群表示與式（8）類似，僅僅在平移矢量的選擇上有區別。

通過上述步驟，對于兩地圖矢量片段而言，就得到了一組匹配好的n個李群的集合T和T′：

接下來建立如圖4（步驟3）所示的多約束因子圖。圖中，節點為待估計的地圖矢量片段的變換矩陣T，紫色塊為實線對節點的約束，綠色塊為虛線對節點的約束，虛線塊為其他目標對節點的約束。

與視覺SLAM 問題類似，該圖優化問題的本質也是最大似然估計問題，由多軌跡間變換關系的聯合概率分布組成。假設所有測量概率獨立，該問題可以表示為：

式中：T為所有待估計變量即地圖片段的變換矩陣集合；S為所有觀測值的集合，包括虛線因子、實線因子。

2.3.1 實線因子

由于目標遮擋、路面破損等原因，車端對于實線的檢測往往存在漏檢。然而，根據車端試驗數據驗證，僅有極小概率會出現車道線類型誤檢，如實線車道線檢測為虛線車道線，因此，本文不考慮此類型誤檢造成的微小誤差?？紤]式（9）中的一對實線匹配對Tl1與T'l1，分別屬于第t-1 個和第t個地圖片段，其約束兩實線段中點以及朝向相同。

式中：Tt-1和Tt分別為待估計的第t-1個和t個地圖片段的變換矩陣。

2.3.2 虛線因子

由于車端大部分時候對于虛線的檢測都是完整而準確的，所以考慮式（9）中的一對虛線匹配對Tb1與T'b1，分別屬于第t-1 個和第t個地圖片段，其約束兩虛線段起點以及朝向相同。

式中：Tt-1和Tt分別為待估計的第t-1個和第t個地圖片段的變換矩陣。

值得注意的是，除了實線因子、虛線因子，本文算法框架還可以增加其他全局殘差因子，如標識牌、紅綠燈等。

2.4 因子圖優化

參考文獻［22］，考慮一含有n條邊的因子圖，其目標函數為：

式中：ei(·)為誤差，作為優化變量xi和觀測值zi符合程度的一個度量；Ωi為信息矩陣，是協方差矩陣的逆；xi為因子圖中的一個頂點，在傳統SLAM 問題中表示位姿。

進一步，將總體優化問題改寫為誤差和的形式，又因為觀測值zi本身已知，所以為了形式上的簡潔，將ei(xi，zi)改寫為ei(xi)，如式（15）所示。

為了求解優化問題，需要知道初值和迭代方向，考慮初始值x?i與增量Δx，對于第i條邊的目標函數，有：

式中：Ji為e i關于xi的導數；Ci為與Δx無關的常數項，實則為Fi變化前的值；2bi為一次項系數；二次項系數Hi為Hessian矩陣。

則增量Δx令目標函數發生改變的量為：

由于目標是找到增量Δx，使增量變為極小值，所以令目標函數的增量對于增量Δx的導數為0。最終優化為：

以上為因子圖優化的基本原理，出于算法復雜度的考慮，本文選用g2o 優化庫［23］完成因子圖優化的迭代過程，最終尋找到極小值增量Δx，使優化結果收斂。

3 試驗結果及分析

為驗證提出的基于因子圖優化的眾包地圖云端融合方法的可行性與有效性，本文使用安裝有前置攝像頭、IMU、RTK-GPS的車輛在真實城市道路上進行數據采集建圖并上傳云端服務器，再將本文算法結果與迭代最近鄰算法（Iterative Closest Point，ICP）結果進行對比。ICP算法是目前最經典的配準算法，其不依賴于環境特征，具有較高的配準精度。車端采集并上傳至云端的試驗數據如圖5 所示，全長約為20 km。圖5a 為上傳的車端數據總覽，可以看出試驗場景較復雜，滿足評價算法的試驗標準，其包含了直道、彎道、十字路口等；圖5b為總覽數據的局部放大圖。由圖5b 可知，車端上傳的車道地圖數據包含馬路邊沿（標號1，紫色線段）、虛線車道線（標號2）、實線車道線（標號3）、地面箭頭標識（標號4）、停止線（標號5）、人行橫道（標號6）和標識牌（標號7）。不難看出，車端上傳的數據中，實線車道線聚集度較好，并且與谷歌衛星底圖較符合；而虛線車道線的聚集度稍差，與衛星底圖有些許差距。

圖5 谷歌影像下車端上傳數據

3.1 聚集度評價指標

為了更好地評價本算法的聚集結果，根據車道地圖的特點，提出以下3 個評價標準，如圖6所示。

圖6 聚集度評價標準

3.1.1 點距離指標

點的距離指標是指空間中兩點p1和p2的絕對距離d12。定義點的距離指標得分如式（19）所示。

式中：γ1為點的距離評價指標。

3.1.2 線段距離指標

線段的距離指標是指空間中兩線段l1和l2在xy平面的投影線段l'1和l'2重疊部分的距離d12。定義線的距離指標得分如式（20）所示。

式中：γ2為線段的距離評價指標。

3.1.3 線段角度指標

線段的角度指標是指空間中兩線段l1和l2在xy平面的投影線段l'1和l'2的夾角θ12。定義線段的角度指標得分如式（21）所示。

式中：γ3為線段的角度評價指標。

3.1.4 虛線、實線匹配對得分計算公式

根據實際需要，分別對點的距離評價指標γ1取值為1 m、線段的距離評價指標γ2取值為1.5 m、線段的角度評價指標γ3取值為10°。對于虛線車道線匹配對，分別從點的距離以及線段的角度兩方面進行打分，具體得分計算如式（22）所示；對于實線車道線匹配對，分別從線段的距離以及線段的角度兩方面進行打分，具體得分計算如式（23）所示。

結合實線、虛線的位姿化標準，設有一虛線匹配對l1、l2，其端點在xy平面的坐標分別為ps1、pe1、ps2、pe2，現定義虛線車道線匹配對聚集度得分如式（22）所示。

式中：s1( · )為兩點間距離指標得分；s3( · )為兩線段間角度指標得分；θ12為虛線車道線匹配對在xy平面投影的夾角。

設有一實線匹配對l1、l2，其在xy平面的投影線段l'1、l'2，現定義實線車道線匹配對聚集度得分如式（23）所示。

式中：s2( · )為兩線段間距離指標得分；s3( · )為兩線段間角度指標得分；θ12為實線車道線匹配對在xy平面投影的夾角。

3.2 試驗結果對比

本文算法和ICP 算法對復雜路口的地圖優化聚合結果，如圖7 所示。從局部放大圖可以看出，兩算法對于實線的優化聚合結果差別不大，但本文算法在虛線的表現上明顯優于ICP算法。

圖7 復雜路口聚合結果對比

為了得到聚集度得分評價對比結果，首先對真實道路數據進行如圖4（步驟1）所示的目標間的匹配，即得到實線車道線以及虛線車道線的匹配對。然后利用3.1 節提出的聚集度評價指標對原始車道線匹配對進行聚集度打分計算，再對多地圖進行聚合優化，并對優化后的地圖進行聚集度打分計算。最終，聚集度得分對比柱狀圖結果如圖8所示。

圖8 聚集度得分柱狀分布對比

圖8a為虛線車道線匹配對聚集度得分柱狀對比圖，圖8b 為實線車道線匹配對聚集度得分柱狀對比圖。圖中，紫色表示原始數據聚集度得分的匹配對數目；紅色表示ICP 算法優化結果聚集度得分的匹配對數目；綠色表示本文算法優化結果聚集度得分的匹配對數目。由圖8a可知，原始虛線車道線匹配對聚集度較差，體現為超過50%的虛線車道線匹配對聚集度得分分布為0～30 分；本文算法虛線得分結果基本符合正態分布，得分中位數約為40～50分；而ICP 算法得分結果為右偏分布，高分相對較少，得分中位數約為30～40 分。兩種算法對于低分段（0～20 分）的優化結果差異較大，可以看出ICP算法對于低分段虛線匹配對的減少量小于50%，且遠小于本文算法，這也說明了ICP 算法對于優化初始值的依賴性。由圖8b 可知，兩種算法優化結果的聚集度得分柱狀分布圖都呈現左偏分布，并且得分差異性較小，表明兩算法對于實線車道線匹配對聚集度的優化效果較好。

為了更精確地衡量本文算法，現對兩算法優化結果的聚集度得分取平均值，見表1。由表可知，兩算法對于實線車道線匹配對的聚集度得分都提高了10 分以上，差距相對較??；而本文算法在虛線車道線匹配對的聚集度得分結果上高出ICP 算法10分左右。綜合車道線聚集度平均得分結果，本文算法相較于原始車道線聚集度得分提升了44.7%。結合原始虛線車道線匹配對聚集度平均得分結果，以及虛線車道線匹配對聚集度得分柱狀分布圖可知，車端上傳的虛線車道線聚集度較差，即原始虛線車道線聚集度平均得分較低。又由于傳統ICP 算法對于虛線車道線匹配對優化結果的平均聚集度得分的提升相較于本文算法有較大差距，結合兩算法對于聚集度較好的實線車道線優化結果的差異性較小這一事實，以及兩算法對于低分段虛線車道線匹配對減少量的較大差異性，來進一步證實本文算法對于優化初值的較低依賴性。

表1 虛、實線車道線匹配對聚集度平均得分結果對比

此外，還利用RTK-GPS 對一條總長約為0.6 km的路口車道線進行真值采集，如圖9紅色線段所示。通過計算比較聚合后車道線與真值車道線在東方向以及北方向的絕對值誤差來評估本文算法，對比結果見表2。由表2 可知，本文算法聚合后地圖相對于真值的誤差均小于1 m，尤其在虛線車道線的誤差上，相較于傳統ICP 算法具有明顯優勢，進一步體現了本文算法的有效性。

表2 東、北方向車道線絕對誤差對比

圖9 真值采集

4 結論

1）本文提出了一種基于因子圖的眾包高精度地圖云端融合方法，以車端上傳的局部語義地圖為基礎，進行地圖片段間的匹配，利用一致性篩選方法增加了匹配精度；利用因子圖模型，將車道線匹配對轉化為約束地圖間變換關系的因子節點，使聚合后的地圖具有全局最優性。

2）提出了一種地圖間聚集度的評價指標并對試驗結果進行衡量。聚集度平均得分結果表明，本文算法對地圖間的聚集度提升了44.7%，極大地優于傳統ICP 算法。采用眾包方法能極大地減少高精度地圖的制作時間，提高地圖的更新頻率。