基于圖優化方法的點云三維環境重構

陳 文，岳驚濤，李 華，韓棟斌，齊 堯

(1.陸軍軍事交通學院 研究生管理大隊，天津 300161； 2.軍事交通運輸研究所，天津 300161； 3.陸軍軍事交通學院 軍用車輛系，天津 300161)

● 基礎科學與技術BasicScience&Technology

基于圖優化方法的點云三維環境重構

陳 文1，岳驚濤2，李 華3，韓棟斌1，齊 堯1

(1.陸軍軍事交通學院 研究生管理大隊，天津 300161； 2.軍事交通運輸研究所，天津 300161； 3.陸軍軍事交通學院 軍用車輛系，天津 300161)

點云的三維重構對無人車感知和高精度地圖的制作具有重要作用。為得到與真實環境一致的點云三維環境，提出以GPS差分狀態良好的點云為參考，將閉環圖優化延伸到非閉環條件下，分別對局部差分不好的路段進行ICP配準和圖優化，在此基礎上建立全局圖結構實施精細優化的算法。通過實驗發現，運用該算法消除了配準的累積誤差和軌跡重合區域的點云誤差，得到全局相對位置一致的點云，達到與真實環境一致的三維重構效果。

配準；圖優化；點云；三維重構；無人車

Abstract: Three-dimensional reconstruction of point cloud plays an important role in perceiving unmanned vehicles and making high precision maps. In order to obtain three-dimensional environment of point cloud which is consistent with real environment, the paper firstly proposes a method that extending the closed-loop graph to non-closed-loop graph with point cloud of good GPS differential state as reference. Then, it conducts ICP registration and graph optimization on partial bad differential section respectively, and establishes global graph structure to implement fine optimization. The experiment shows that this algorithm eliminates cumulative error of the registration and point cloud error in the intersection area, and it obtains the point cloud which has relative position in global and achieves three-dimensional reconstruction effect consistent with real environment.

Keywords: registration; graph optimization; point cloud; three-dimensional reconstruction; unmanned vehicles

三維環境重構對無人車的感知和高精度地圖的制作具有重要作用。目前，三維環境重構的方法主要有基于視覺和激光雷達[1-2]。視覺易受環境因素的影響，比如光線、天氣等，而激光雷達則具有較高的環境適應性和精準的測距精度。因此，本文主要是基于激光雷達點云數據來離線構建無人車三維道路環境。

基于點云的三維重構主要是將相對位置有偏差的點云進行糾正，使所有點云間的相對位置達到全局一致。對于相對位置有偏差的點云常采用配準的方法解決，目前國內外配準算法的研究已經很成熟，主要包括基于幾何特征、RANSAC的初始配準和基于ICP、改進ICP的精細配準以及多種算法融合的配準[3-5]。但是這些配準算法對于兩幀或少數幾幀效果很好，對于連續配準來說，會隨著點云數量的增加，細小的配準誤差就會被累積而放大，從而造成配準的末端與實際有很大的偏離。因此，針對無人車這種相對位置有偏差且連續配準的情況，還需要對配準后的累積誤差進行消除。Dorit Borrmann等[6]提出了一種圖優化的算法，該算法在配準的過程中采用閉環檢測，進行累積誤差的初始分配，構建圖結構實施圖優化可以達到全局一致的效果，但該算法是將所有的點云進行配準且需要閉環，有很多冗余處理和一定的局限性。

本文主要利用GPS差分狀態信息，將閉環圖優化延伸到非閉環條件下，只對差分不好路段進行配準和局部圖優化，最后進行全局圖優化，實現無人車三維道路環境重構。

1 研究的總體思路

GPS在差分狀態下，位置精度非常高，通常在2 cm左右，可以作為參考點云。本文主要依托GPS差分信號的準確性來進行非閉環條件下的點云優化。首先，根據GPS狀態信號，搜索差分丟失路段序列；其次，以差分信號良好的點云為參考，構建圖結構實施局部圖優化，由于點云配準的累積誤差比較大，為避免圖優化陷入局部最優，這里將局部圖優化分為初始調整和精細優化；最后，在全局實施精細優化。

2 點云配準

2.1初始轉換矩陣

雖然ICP(最近點迭代算法)具有很高的配準精度，但它受初始條件的限制，當初始條件不好時很容易陷入局部最優而帶來較大的配準誤差。因此，在進行ICP配準之前需要對點云進行初始位姿調整以適用ICP精細配準。

本文采用的是GPS/IMU所發出的點云位姿信息，經過濾波處理，軌跡比較平滑，相鄰點云的相對位置偏差比較小。因此，在對激光雷達點云配準時可以認為前一幀的轉換矩陣與當前幀的轉換矩陣相近，滿足ICP精細配準的初始條件。本文在對連續點云序列兩兩配準時，將前一幀的轉換矩陣作為下一幀ICP 配準前的初始轉換矩陣。

2.2ICP精細配準

本文以前一幀點云為源點云s(n)，當前幀點云為目標點云T(n)，其對應點對集分別表示為M(m)和D(m)。以目標點云相對于源點云的平移和旋轉為變量，根據最小二乘原理建立誤差目標函數：

(1)

式中：R為3×3的旋轉矩陣；t為3×1的列向量；Mi與Di為一對對應點對；m為對應點對的數目。

以誤差目標函數F(R，t)的值最小為最優，運用四元數法求解R和t，通過不斷迭代，使誤差目標函數值收斂來獲取最優轉換位置。

ICP算法主要流程：①在點云數據降采樣后，對源點云數據構建k-d樹；②利用k-d樹，以最近歐式距離cldist(默認為50 cm)為對應點選取閾值，尋找兩幀點云對應點對；③運用四元數法求解R和t，更新目標點云位置；④重復步驟②和③，直到前后兩次迭代的目標函數差值小于預設閾值(默認為0.000 01 cm)或達到迭代次數(默認為150次)時停止。

3 基于圖結構的優化處理

3.1局部優化處理

3.1.1 初始調整

為將ICP的累計誤差以一種理想的方式分配給圖結構中的各個頂點，用以減少直接精細優化所帶來的時間消耗，避免陷入局部最優，因此在配準后對不好路段的點云進行整體初始調整[7]。

假設不好路段的點云為P0，P1，…,Pn，其位姿用歐拉角表示，則Pi=(xi,yi,zi,θxi，θyi,θzi)，其中P0和Pn為差分狀態良好的點云，P1到Pn-1為差分狀態不好的點云。對P0到Pn-1進行配準，之后以前、后兩幀點云位置為頂點，以它們的點云關系為邊構建圖結構。此時將Pn-1的位置另記為V1，接著將Pn-1的位置轉換到配準前的原始位置并與Pn進行配準，這里認為此時的配準位置才是Pn-1最接近真實的位置，另記為V2，將ΔV=V2-V1作為累計誤差。而后將Pn加入到頂點中，并以Pn-1與Pn相對位置關系為邊，至此圖結構構建完畢。

接下來是計算每個頂點的權重(這里權重為分配誤差與ΔV的比值，用wi表示)。設P0和Pn的權重為0，表示不進行調整；Pn-1的權重為1，表示按照ΔV誤差進行調整。其他的頂點權重按照如下公式進行分配：

(2)

式中：d(vs,vi)為P0到Pi之間所有邊之和；d(vs,ve)為P0到Pn-1之間所有邊之和。

從上述初始調整的思路可以看出，邊的選取直接影響著各點誤差分配的比例，這里邊的選取為馬氏距離協方差矩陣Cij。

3.1.2 精細優化

雖然經過初始調整后，解決了ICP配準的偏離問題，但相鄰點云間的相對位置還是存在一些細小的偏差，特別是路段中間位置的點云。為消除這種細小的誤差，還需要對其實施精細優化。

精細圖優化的實質是將優化相關的變量和約束條件以圖的形式表述的優化問題，即這里將點云的位置作為頂點，點云間的相對位置關系為約束邊，引用馬氏距離建立誤差目標函數，通過求解目標函數的極值來得到最優估計。

假設圖結構中某相鄰頂點Pi=(xi,yi,zi,θxi,θyi，θzi)與Pj=(xj,yj,zj,θxj,θyj，θzj)，則目標函數為

(3)

(4)

(5)

Cij=s2(MTM)-1

(6)

(7)

D=HP

(8)

則馬氏距離函數變為

(9)

通過使W最小化來解算該誤差目標函數，即

(10)

Cp=(HTC-1H)-1

(11)

(12)

式中：Gij為矩陣G的子項；Bi為B的子項。

因此，將求解線性最優估計問題轉化為求解線性等式方程組，即

GP=B

(13)

(14)

此時雖然已經找到了圖結構中位置頂點的最優估計，但從式(4)以及觀測邊和估計邊的求解可以看出，其位置估計值是依賴位置更新前觀測位置的最小配準誤差，圖優化的關鍵思想就是讓所有邊相連點云間的相對位置估計誤差盡可能地接近其最小配準誤差，因此圖結構精細優化也需要類似ICP的迭代，算法流程如下：

(4)根據線性等式方程組GP=B，運用喬里斯基分解求解P。

(6)判斷w和迭代次數，若w小于誤差閾值δg(一般默認為0.5 cm)或達到迭代次數(一般默認為50次)停止迭代，否則重復步驟(1)—(5)。

3.2全局優化處理

全局的優化處理主要是針對軌跡重合區域內點云間的誤差，這里以它們的點云位置為頂點，以3.1.2小節中所描述的相對位置關系為邊，在全局構建圖結構，實施精細優化。

假設無人車采集的點云為x0，x1，…，xn，相鄰兩幀間隔為5 m，以它們的點云位置為頂點，為保證相鄰點云間的相對位置關系準確，這里為相鄰兩幀的點云添加一個相應的邊。為保證不同采集路線間交叉區域中重疊較大的點云相對位置關系比較準確，也需要添加相應的約束，這里以兩幀點云的歐式距離為判斷，差值小于5 m的為其添加一個相應的邊。這樣在全局上保證了相鄰間和交叉區域內偏差比較大的點云間的相對準確的位置關系，從而實現與實際環境一致的點云三維重構。

4 實驗驗證

4.1實驗條件及過程

本實驗驗證的平臺為JJUV-5無人車(如圖1所示)。車上安裝了GPS/IMU組合導航系統，其單點定位精度小于2 m，RTK定位精度可達2 cm+1 ppm(CEP)，姿態精度為0．02°，數據更新率最高為200 Hz，可以為無人車提供精確的位姿信息。車的頂部安裝了velodyne-64HDL激光雷達，其垂直掃描范圍為-24．8°±2°，轉速為600 r/min，每幀采集的點數約為13萬個，可以很清楚地描述無人車周身的三維環境。

圖1 JJUV-5實驗平臺

為驗證算法的有效性，利用JJUV-5無人車實驗平臺，在某校園內連續采集一部分數據，其中包括有交叉區域的路口或道路、GPS差分良好的路段以及受遮擋導致差分丟失的路段。

4.2結果分析

4.2.1 局部圖優化分析

圖2所示是對GPS信號不好路段(序列號為333-353)進行ICP配準前后對比圖，其中圖2(a)是在差分信號丟失的情況下所呈現出的點云環境，從圖中可以明顯看出，重復區域內的特征物位置依次錯開，造成環境視圖比較模糊。而圖2(b)則是ICP配準后所呈現出的點云環境，與圖2(a)形成鮮明的對比，圖2(b)整個點云環境比較清晰，特別是重復區域里可以明顯地辨別出特征物的形態和屬性。但是ICP只是保證了相鄰兩幀相對準確位置關系，它會隨著配準數量的增加而導致誤差的累積。如圖2(c)所示，在進行ICP配準后，可以明顯地看出配準的最后一幀與其下一幀GPS差分狀態良好的點云有很大累積誤差(如圖2(c)兩箭頭之間所示)。

(a)局部差分信號丟失點云環境視圖

(b)配準末端局部放大點云環境視圖

(c)配準后局部點云環境視圖圖2 ICP配準前后對比

圖3所示是對ICP配準后的點云進行圖優化后的對比圖。為消除累積誤差，先進行圖優化的初始調整，如圖3(a)所示，這是進行初始調整后的點云環境視圖，從中可以看出配準時最后一幀的偏離已經消除，點云環境視圖比較清晰，重復區域特征物明顯。但是經過檢查發現軌跡中段的340幀和341幀點云間存在一定的誤差，如圖3(b)中矩形區域所示，仔細觀察可以發現路牌出現重影，道路邊界線不重合等。經過精細優化后，如圖3(c)中矩形區域所示，誤差已經消除且點云環境視圖較之前清晰，特別是路牌重影消失、道路邊界重合。

(a)初始調整后末端點云環境視圖

(b)初始調整后中段環境視圖

(c)精細優化后中段環境視圖圖3 局部圖優化對比

由以上分析可知，差分信號丟失路段經過ICP配準雖然消除相鄰點云間相對位置誤差，但是會帶來較大的累積誤差。經過圖優化的初始調整后可以初步消除累積誤差，但有些點云還是存在一些細小的誤差。通過精細優化后所有相鄰點云間的相對位置準確，有效地解決了配準累積誤差消除的問題。

4.2.2 全局圖優化分析

圖4所示是對全局進行精細圖優化前后點云環境對比圖。由于外參標定、單幀點云位置校準等細小誤差導致重復路線交叉區域內的點云間存在一定的誤差。如圖4(a)所示，可以很明顯地觀察到矩形區域內特征物出現重影、道路邊界不重合。通過為這些區域增加約束邊而重新構建圖結構，實施精細圖優化后，如圖4(b)所示，可以看到圖中矩形區域重影消失、道路邊界重合，與圖4(a)形成鮮明的對比，圖4(b)中點云環境很清晰，可以明顯辨識出特征物的形態和屬性。

(a)精細優化前局部放大點云環境視圖

(b)精細優化后局部放大點云環境視圖圖4 全局精細圖優化前后對比

由以上分析可知，在解決ICP累積誤差消除的基礎上，對重復路線交叉區域內的點云添加約束邊，通過全局的圖優化可以很好地解決此區域內的點云誤差，能夠在全局上得到相對位置比較準確的點云，達到與實際環境一致的點云三維環境重構，證實了整個算法的有效性。

5 結 語

本文主要對點云的三維重構進行了研究。首先，利用無人車GPS/IMU組合導航系統的GPS狀態信息，搜索出點云差分效果不好的路段，分別對其進行ICP配準，配準時間為每幀60～120 ms，配準精度在5 cm以內；其次，以差分良好的點云為參考，將閉環圖優化延伸到非閉環條件下，消除ICP配準帶來的累積誤差，與差分狀態良好點云的相對位置精度在10 cm以內；最后，在此基礎上從全局的角度出發，構建圖結構實施全局精細優化，消除重復采集路線交叉區域內點云誤差，得到了點云間相對位置精度為10 cm左右(個別點云間會達到20 cm)的三維環境，實現了與實際環境基本一致的點云三維重構。但也存在一定的問題：在進行全局精細優化的時候，本文只采用了第一幀作為固定點云，整個點云環境相對于第一幀有0.5°左右的旋轉和20 cm左右的平移，導致整體絕對精度不高。下一步，將在差分狀態良好的點云中選取更多的關鍵點云作為支撐框架，對全局點云實施分區域精細圖優化來提高整個點云環境的絕對精度。

[1] 李競超.基于立體視覺的三維重建[D].北京：北京交通大學,2010.

[2] 劉超.點云數據處理與三維重構研究[D].南京：東南大學,2015.

[3] 劉新.三維點云數據的配準算法研究[D].秦皇島：燕山大學,2015.

[4] 朱新宇,萬劍華,劉善偉,等. 改進的ICP點云配準算法[J]. 海洋測繪,2015,35(2):77-79.

[5] 陸軍,彭仲濤,夏桂華.點云多法向量鄰域特征配準算法[J].光電子激光 ,2015,26(4):780-787.

[6] BORRMANN D, ELSEBERG J, LINGEMANN K, et al. Globally consistent 3D mapping with scan matching[J]. Robotics and Autonomous Systems, 2008, 56(2): 130-142.

[7] SPRICKERHOF J, NU CHTER A, LINGEMANN K, et al. A heuristic loop closing technique for large-scale 6D SLAM[J]. Automatika: Journal for Control, Measurement, Electronics, Computing & Communications, 2011, 52(3)：199-222.

[8] 吳雄華.矩形論[M]．上海：同濟大學出版社，1994：191-193．

(編輯：史海英)

Three-dimensionalReconstructionofPointCloudBasedonGraphOptimizationMethod

CHEN Wen1, YUE Jingtao2, LI Hua3, HAN Dongbin1, QI Yao1
(1.Postgraduate Training Brigade, Army Military Transportation University, Tianjin 300161, China; 2.Military Transportation Institute， Tianjin 300161, China; 3.Military Vehicle Department, Army Military Transportation University, Tianjin 300161, China)

10．16807/j.cnki.12-1372/e.2017.09.020

TP18

A

1674-2192(2017)09- 0085- 06

2017-04-20；

2017-05-10．

國家自然科學基金重大項目(91220301)；國家重大研發計劃(2016YFB0100903)．

陳 文(1989—)，男，碩士研究生；岳驚濤(1971—)，男，博士，高級工程師，碩士研究生導師．