混合視覺系統(tǒng)的運動物體檢測和靜態(tài)地圖重建

胡譽生，何炳蔚，鄧清康

（福州大學(xué)機械工程及自動化學(xué)院，福州 350108）

0 引言

近年來機器人技術(shù)蓬勃發(fā)展，自主機器人［1］是目前最熱門的研究領(lǐng)域之一，機器人可以完成人類不能夠完成的任務(wù)，如危險性較高的工作。想要在特定環(huán)境下自由移動，機器人必須具備定位［2］和導(dǎo)航［3］等功能，而這些功能依賴于環(huán)境地圖，因此環(huán)境地圖的構(gòu)建至關(guān)重要。目前，通常使用相機或激光雷達(dá)等視覺傳感器來獲取室內(nèi)或室外環(huán)境下的三維地圖［4-5］，但當(dāng)環(huán)境中存在運動物體時，三維地圖重建仍然是一個難點，運動物體會在地圖上留下一系列“痕跡”，將會形成錯誤的特征從而影響機器人對自身位置的判斷，同時增加導(dǎo)航的難度。

動態(tài)背景下進(jìn)行運動物體檢測時，傳感器和目標(biāo)都處于運動狀態(tài)，難以區(qū)分出背景和前景的運動，因此背景運動補償估計是動態(tài)背景下運動物體檢測的主要方法?；趫D像的常見背景運動補償方法包括特征點匹配法和塊匹配法，文獻(xiàn)［6］提出了一種基于改進(jìn)尺度不變特征變換（Scale Invariant Feature Transform，SIFT）特征匹配算法估計背景運動，并利用圖像差分法來檢測出運動目標(biāo)。文獻(xiàn)［7］提出了一種基于子塊運動補償背景估計方法和圖像差分法相結(jié)合來實現(xiàn)運動物體檢測，避免了錯誤的特征點匹配結(jié)果對背景運動補償?shù)挠绊?。上述兩種方法均未解決運動檢測不完整問題?；邳c云的運動物體檢測方法主要借助于柵格思想，通過點云配準(zhǔn)算法［8-9］將當(dāng)前幀點云投到上一幀的坐標(biāo)系下，然后分析柵格狀態(tài)前后變化來提取運動物體，但是該方法對小位移運動物體檢測效果不佳。文獻(xiàn)［10］提出了一種基于圖像與點云數(shù)據(jù)融合的運動物體檢測方法，從點云數(shù)據(jù)中獲得小車自我運動參數(shù)來進(jìn)行背景運動補償，并結(jié)合光流算法完成運動特征的檢測，但是該方法在非剛性物體提取完整度上仍效果不佳。

針對以上問題，同時考慮圖像可以提供豐富的色彩紋理信息，點云可以提供高精度的距離信息，二者融合可以重建出信息更加豐富的三維彩色地圖，因此在文獻(xiàn)［10］研究的基礎(chǔ)上，本文提出了一種點云分割輔助下運動物體提取的方法，解決了檢測不完整問題，能較好地處理非剛性物體運動，同時基于八叉樹地圖（Octomap）算法［11］提高了靜態(tài)柵格地圖重建的質(zhì)量和效率。

1 本文方法及其原理

本章將詳細(xì)介紹點云分割輔助下的運動物體檢測和靜態(tài)地圖重建的方法。該方法的主要框架流程如圖1 所示，具體步驟如下：

圖1 本文方法主要框架流程Fig.1 Flowchart of main framework of proposed method

1）輸入數(shù)據(jù)為相機所獲得的圖像數(shù)據(jù)和激光雷達(dá)所獲得的點云數(shù)據(jù)。首先對混合視覺系統(tǒng)進(jìn)行外參標(biāo)定，獲取相機與激光雷達(dá)兩傳感器之間的坐標(biāo)變換參數(shù)，為后續(xù)數(shù)據(jù)融合提供基礎(chǔ)。

2）數(shù)據(jù)作地面移除預(yù)處理后，將其作為特征點投影到對應(yīng)的圖像上，利用光流算法獲取特征點的像素運動向量，并估算特征點因小車自我運動而引起的人工運動向量來進(jìn)行背景運動補償，之后通過分析比較兩運動向量的差異來獲得點云數(shù)據(jù)中的動態(tài)點。

3）完整地提取出運動物體，利用改進(jìn)的歐氏聚類算法對當(dāng)前點云數(shù)據(jù)進(jìn)行簇分割；之后利用點云數(shù)據(jù)索引值的唯一特性來融合動態(tài)點檢測結(jié)果與點云聚類分割結(jié)果，從而實現(xiàn)運動物體完整的提取。

4）通過Octomap 工具與當(dāng)前點云數(shù)據(jù)所對應(yīng)的激光雷達(dá)里程計，實現(xiàn)靜態(tài)地圖的重建。

1.1 全向相機與三維激光雷達(dá)的坐標(biāo)變換

相機和激光雷達(dá)標(biāo)定的目的在于獲取從激光雷達(dá)坐標(biāo)系到相機坐標(biāo)系的投影變換，其變換公式為：

其中：表示t時刻激光掃描下的第i個點表示在對應(yīng)圖像相機坐標(biāo)系下的值；K表示相機的內(nèi)參數(shù)；R和T分別表示投影變換陣中的旋轉(zhuǎn)矩陣和平移向量。

本文中使用的相機為全向相機，該相機系統(tǒng)通過每個相機的內(nèi)參數(shù)將圖像投影到統(tǒng)一的特定虛擬相機系統(tǒng)坐標(biāo)系中并以球坐標(biāo)系方式展示，因此可以忽略內(nèi)參數(shù)K。令，該點對應(yīng)的像素坐標(biāo)設(shè)為(u，v)T，則從激光雷達(dá)坐標(biāo)系轉(zhuǎn)換到像素坐標(biāo)系的計算式為：

其 中：int(?) 表示取整函數(shù)；arctan2(?) 表示值域為[-π，+π]的反正切函數(shù)；H和W分別表示圖像的高度與寬度。本文采用了文獻(xiàn)［12］中基于點云強度信息輔助下棋盤格角點自動提取的高精度標(biāo)定方法進(jìn)行系統(tǒng)傳感器間外參數(shù)的標(biāo)定。

1.2 點云數(shù)據(jù)中動態(tài)點的檢測

光流算法是運動檢測領(lǐng)域中常用的算法之一，在靜態(tài)背景環(huán)境下通過光流算法對圖像中像素點的運動估計，可以實現(xiàn)運動物體的檢測；但在建圖過程中由于相機跟隨小車一起移動，因此在圖像背景區(qū)域也會出現(xiàn)相應(yīng)的像素運動，同時在全景圖像中每個相機視角有著各自的運動方向，僅靠光流法不能很好地區(qū)分靜態(tài)背景和動態(tài)前景。

通過分析光流法原理，可以知道由光流法所估計的像素運動向量在一定程度上反映了三維世界中物體的運動場，因此只要獲知小車的自我運動位姿，就可反映背景運動模型，從而通過背景運動補償來實現(xiàn)前景運動物體的檢測。借鑒文獻(xiàn)［10］提出的背景運動補償方法，本文的運動檢測方法在此基礎(chǔ)上進(jìn)一步提升，以完整地提取出運動物體。

本文的動態(tài)點檢測方法可以分為三部分，分別是點云地面識別，特征點光流運動向量，以及人工運動向量的估計、點云數(shù)據(jù)動態(tài)點的提取。

1.2.1 點云地面識別

點云數(shù)據(jù)中包括了大量的地面點，其不僅增大了計算的復(fù)雜度，而且對后續(xù)動態(tài)點的檢測帶來干擾，因此點云地面識別是重要的。本文中使用了基于隨機采樣一致性（RANdom SAmple Consensus，RANSAC）算法來識別點云地面，其過程［13］如下：

1）從點云數(shù)據(jù)中任意選取三個不共線的點，計算其相應(yīng)的平面方程ax+by+cz=d。

2）之后計算點云中每個點到該平面的距離（di=axi+byi+czi-d），并與設(shè)定的閾值t比較，若di≤t，則將該點視為局部內(nèi)點并統(tǒng)計有效點數(shù)n，反之將其剔除。

3）重復(fù)1）～2）步驟，迭代m次之后選取有效點數(shù)n最大的平面并采用最小二乘法估算平面參數(shù)。

通過上述過程可以知道隨機采樣一致性算法可以提取到局部內(nèi)點數(shù)量最多的平面，但是當(dāng)點云中地面點數(shù)量不是最大時將會出現(xiàn)錯誤的結(jié)果，如圖2 所示，可以發(fā)現(xiàn)墻壁被誤作為最終的識別結(jié)果。

圖2 基于RANSAC地面識別錯誤例子Fig.2 Example of ground recognition error based on RANSAC

為了解決這個問題，本文考慮到較平坦的地面環(huán)境與傳感器的安裝位置，先對點云數(shù)據(jù)進(jìn)行一次直通濾波（PassThrough）來過濾掉大部分位于墻體上的點云，保證在點云平面集合中位于地面的數(shù)據(jù)點最多；之后再通過隨機一致性算法對剩下的點云數(shù)據(jù)進(jìn)行平面檢測以完成地面點云的提取。具體過程如圖3所示。

圖3 基于PassThrough+RANSAC 地面識別例子Fig.3 Example of ground recognition based on PassThrough+RANSAC

1.2.2 特征點運動向量的估計

經(jīng)過1.2.1 節(jié)的點云地面點識別之后，通過激光雷達(dá)坐標(biāo)系與相機像素坐標(biāo)系的變換，將非地面點數(shù)據(jù)作為特征點投影到對應(yīng)的圖像上，通過光流法估計特征點的像素運動，本文使用文獻(xiàn)［14］中提出的一種由粗到精的光流估計方法，可以在一定程度上減小光度變化所引起的光流估計誤差，特征點的光流運動向量vo為：

其中：pt表示t時間下點云數(shù)據(jù)中非地面點集合；siftflow(?)表示光流估計函數(shù)。

獲取特征點的光流運動向量之后，進(jìn)行特征點的人工運動向量估計，該值反映了特征點因相機跟隨小車自我運動而引起的背景運動，其完全忽略了圖像中任何可能的運動對象。為了獲得人工運動向量，首先通過LeGO-LOAM（Lightweight and Ground-Optimized Lidar Odometry And Mapping）算 法［5］從點云數(shù)據(jù)中估計出小車的移動位姿，并利用式（6）轉(zhuǎn)換后得到相鄰間里程計Ot：t+1；再由式（7）將點云數(shù)據(jù)pt通過Ot：t+1轉(zhuǎn)換，獲得其虛擬下一時刻出現(xiàn)的位置集合；最后將pt與分別投影到圖像上得到相應(yīng)的像素坐標(biāo)值，兩者相減的結(jié)果作為人工運動向量va的值，如式（8）所示。

其中：Ot：1和Ot+1：1分別表示第t時刻到第1時刻的里程計和第t+1 時刻到第1 時刻的里程計；Tl2c(?)函數(shù)表示激光雷達(dá)坐標(biāo)系到像素坐標(biāo)系的映射關(guān)系。

1.2.3 點云數(shù)據(jù)動態(tài)點的提取

特征點的光流運動向量表示三維世界物體真實運動場在圖像上的投影，其人工運動向量則表示僅因傳感器跟隨小車自我運動而引起的運動場在圖像上的投影，如圖4 所示，其中綠色箭頭（淺灰）和紅色箭頭（深灰）分別表示了特征點的光流運動向量vo和人工運動向量va，可以明顯地觀察到兩者在圖像背景上運動狀態(tài)相似，但是其在前景上具有較大的差異。其中點云為無序點，并以150 個點為間隔投影到圖像上便于觀察。

圖4 特征點光流運動向量和人工運動向量例子Fig.4 Example of optical flow motion vectors and artificial motion vectors of feature points

一般來說，通過特征點的光流劃分為移動的或者靜止的，可以轉(zhuǎn)換為每對運動向量都要考慮它們的角度和模長的過程：

1）對運動向量在像素坐標(biāo)系下的值，之后利用平面向量內(nèi)積公式計算出運動向量間的夾角θ。

2）向量間夾角θ與設(shè)定的臨界閾值Thθ（取20°）比較，如果θ≥Thθ，則對應(yīng)的特征點為動態(tài)點。

3）比較兩運動向量差的模長，利用高斯核函數(shù)來表示一點的運動狀態(tài)Pm，如式（9）所示：

其中，σ2表示θ

經(jīng)過上述處理之后獲取到一組散亂的動態(tài)點，其中可能出現(xiàn)誤檢和檢測不完整問題，需要進(jìn)一步處理以完整地提取出運動物體；同時，考慮到出現(xiàn)運動物體與小車移動狀態(tài)完全一致的情況可能性很低，因此在動態(tài)點檢測過程中忽略了光流運動向量為0的特征點，以減少干擾因素。

1.3 基于點云分割輔助的運動物體提取

點云數(shù)據(jù)動態(tài)點檢測過程中，不可避免地出現(xiàn)檢測不完整問題，可以將其分為以下兩種類型：

1）位于運動前景的特征點未被檢測出來；

2）當(dāng)非剛性物體的部分位置發(fā)生運動時，位于其靜態(tài)部分的特征點無法被檢測出來。

為了解決上述兩種檢測不完整類型，本文提出了一種基于點云分割輔助的運動物體提取方法，對點云數(shù)據(jù)進(jìn)行聚類分割后，通過每個點索引值的唯一特性來融合點云聚類分割結(jié)果與動態(tài)點檢測結(jié)果，以提取出完整的運動物體。

1.3.1 劃區(qū)域點云距離聚類分割

由于點云數(shù)據(jù)量的龐大，綜合考慮時間復(fù)雜度和分割效果兩個因素，本文提出了一種劃區(qū)域歐氏聚類分割方法，防止在單一距離閾值條件下出現(xiàn)的分割不足和過度分割現(xiàn)象，并保證最終的分割效果。

傳統(tǒng)的歐氏距離分割算法［15］的主要思想是將距離在設(shè)定閾值內(nèi)的點歸為同一類，其過程如下：

1）為輸入的點云數(shù)據(jù)pt建立一個KD-Tree 數(shù)據(jù)結(jié)構(gòu)，并創(chuàng)建一個空隊列q；

2）從pt中選取一個未處理點放入q中；

3）利用KD-Tree 數(shù)據(jù)結(jié)構(gòu)獲得該點的鄰近點，計算其到鄰近點的距離，并將距離小于設(shè)定閾值r的點索引加入到隊列q中；

4）在隊列q中選擇一個未處理的點，重復(fù)步驟3）；

5）當(dāng)隊列q不再有新點加入時則完成一個實例分割，備份該實例索引集合，并清空隊列q；

6）重復(fù)步驟2）～5），直到點云數(shù)據(jù)中所有點都被處理過。

從上述過程可以知道，傳統(tǒng)的歐氏距離分割算法其關(guān)鍵在于對距離閾值r的選取，當(dāng)r值過小時會出現(xiàn)過度分割現(xiàn)象，反之則會出現(xiàn)分割不足現(xiàn)象。本文使用的激光雷達(dá)設(shè)備為Velodyne HDL-64E S3，其最大角分辨率?（垂直角分辨率）約為0.5°，根據(jù)式（10）可知隨著目標(biāo)點距離激光雷達(dá)中心的位置越遠(yuǎn)，其反射回來點與點之間距離越大。為了達(dá)到更好的聚類效果，本文采用動態(tài)閾值的方法，即在不同距離區(qū)域內(nèi)使用不同的距離閾值r策略，如圖5所示，r的計算式如下：

圖5 不同區(qū)域?qū)?yīng)不同距離閾值示意圖Fig.5 Schematic diagram of different distance thresholds corresponding to different areas

其中，d表示激光雷達(dá)在XY平面上的掃描線距離。

以激光雷達(dá)為中心劃分其射線距離為：D1=15 m，D2=30 m，D3=45 m，D4=60 m，其對應(yīng)的距離閾值通過將該區(qū)域內(nèi)最大射線距離值代入式（10）求得，分別為r1=0.13 m，r2=0.26 m，r3=0.39 m，r4=0.52 m，之后點云將在各自的區(qū)域內(nèi)使用相應(yīng)的距離閾值進(jìn)行聚類分割，當(dāng)點的距離大于80 m 時則忽略該點。

1.3.2 運動物體的提取

完成點云數(shù)據(jù)動態(tài)點檢測和聚類分割后，描述如何融合兩者以提取出更完整的運動物體。如圖6 所示，左邊圓形區(qū)域表示點云數(shù)據(jù)中動態(tài)點集合，右邊方形區(qū)域中C表示點云聚類分割結(jié)果，根據(jù)點云數(shù)據(jù)結(jié)構(gòu)中索引值的唯一特性對動態(tài)點進(jìn)行歸簇，即尋找每個動態(tài)點所位于的點云簇位置；之后對每個簇中動靜態(tài)點的比值進(jìn)行判斷，如式（11）所示，當(dāng)該值超過給定的閾值ThR（取0.4）時則斷定其為運動物體。

圖6 動態(tài)點歸簇示意圖Fig.6 Schematic diagram of finding clusters of dynamic points

其中：numd表示點云簇的動態(tài)點數(shù)量；numa表示點云簇的總點數(shù)量。

1.4 基于Octomap的靜態(tài)地圖重建

點云數(shù)據(jù)中運動物體提取之后，通過與之對應(yīng)的里程計（1.2.2 節(jié)）將其轉(zhuǎn)換到世界坐標(biāo)系下以完成點云地圖的重建，如式（12）所示，通常默認(rèn)將地圖的第1時刻激光雷達(dá)所在的位置設(shè)為世界坐標(biāo)系。

隨著建圖的進(jìn)行，點云地圖的容量將會越來越大，并且會產(chǎn)生很多冗余數(shù)據(jù)，不僅提高了對計算機性能的要求，而且影響了點云地圖的精度，同時無法直接運用于路徑規(guī)劃等機器人任務(wù)。為了解決這個問題，本文使用了基于八叉樹數(shù)據(jù)結(jié)構(gòu)的Octomap 地圖表示方法，其將三維空間劃分為樹狀空間網(wǎng)格，利用概率模型方式對每個葉子的狀態(tài)（空閑、占據(jù)、位置）進(jìn)行刻畫，設(shè)t=1，2，…，T時刻，對應(yīng)觀測到的數(shù)據(jù)為z1，z2，…，zT，則第n個葉子點到T時刻記錄的全部信息P(n|z1：T)為：

其中：P(n|z1：T-1)表示第n個葉子點到T-1 時刻記錄的全部信息，P(n|zT)表示第n個葉子在T時刻記錄的信息；P(n)表示一個固定的先驗概率，通常取0.5。

2 實驗與結(jié)果分析

本文所使用的混合視覺系統(tǒng)由全向相機Ladybug5 和三維激光雷達(dá)Velodyne HDL-64E S3組成，如圖7（a）所示將其搭載在Guardian 移動小車上。其中，全向相機系統(tǒng)所采集圖像最高分辨率為8192 × 4096，本實驗中所用的圖像像素大小為2048× 1024；三維激光雷達(dá)由64 個激光發(fā)射器和64 個激光接收器組成，其垂直方向視野為+2°到-24.8°；在數(shù)據(jù)采集過程中全向相機和三維激光雷達(dá)的頻率均為10 Hz，圖7（b）為實驗環(huán)境示意圖。

圖7 實驗平臺和實驗環(huán)境示意圖Fig.7 Schematic diagram of experimental platform and experimental environment

2.1 運動物體提取結(jié)果

為了更好地展示所提出的基于點云分割輔助下運動目標(biāo)檢測算法在解決完整性問題方面上的提升，本文對運動物體常規(guī)移動狀態(tài)進(jìn)行分類分析，具體如圖8 所示，其中，圖（a）表示實驗環(huán)境的實際場景圖，圖（b）表示對應(yīng)的三維點云數(shù)據(jù)，圖（c）表示對應(yīng)的無點云分割輔助下運動點檢測結(jié)果，圖（d）表示對應(yīng)的有點云分割輔助下的運動物體完整提取結(jié)果。相較于改進(jìn)前和改進(jìn)后的檢測效果，如圖（c）、圖（d）中圓圈標(biāo)記部分所示，運動物體檢測不完整問題得到了有效的改善，特別地，第六行中物體在上下運動過程中膝蓋以下部位幾乎處于靜止?fàn)顟B(tài)，本文方法也可以完整地將其提取出來；同時，部分檢測錯誤動態(tài)點因無法滿足運動物體提取條件（1.3.2 節(jié)）而被過濾掉，在一定程度上抑制了錯誤檢測的發(fā)生，最終準(zhǔn)確地提取出運動物體。

圖8 運動物體提取結(jié)果Fig.8 Result of moving object extraction

2.2 靜態(tài)地圖重建結(jié)果

點云運動物體提取后即可進(jìn)行靜態(tài)地圖的創(chuàng)建，根據(jù)相應(yīng)的里程計信息將其轉(zhuǎn)換到世界坐標(biāo)系下之后，經(jīng)過八叉樹數(shù)據(jù)結(jié)構(gòu)存儲后得到Octomap 三維柵格地圖，使用octovis 工具（http：//wiki.ros.org/octovis）可以查看最終的地圖結(jié)果。本文實驗中柵格地圖的最高分辨率為0.05，其結(jié)果如圖9所示，圖9（a）表示未經(jīng)運動物體提取的重建結(jié)果，圖9（b）表示經(jīng)運動物體提取之后的重建結(jié)果，圖9（c）為地圖所對應(yīng)的部分實景圖。從圖9 可以發(fā)現(xiàn)，靜態(tài)地圖更好地反映了實際環(huán)境，實驗中存在30個運動物體，檢測到28個。

圖9 基于Octomap的三維重建結(jié)果Fig.9 3D reconstruction result based on Octomap

本文分別對點云地圖和Octomap 地圖從點數(shù)與存儲大小兩個方面進(jìn)行統(tǒng)計，其結(jié)果如表1 所示?？梢园l(fā)現(xiàn)，較于點云地圖，Octomap 地圖在點數(shù)與存儲空間上分別降低了92%與95%，其可以構(gòu)建一個更高效、緊湊的環(huán)境地圖。

表1 地圖的點數(shù)和存儲大小Tab.1 Number of points and storage of maps

3 結(jié)語

為了重建靜態(tài)地圖，本文首先運用了一種基于相機與激光雷達(dá)相結(jié)合的方法，將點云數(shù)據(jù)投影到圖像后通過估計其兩運動向量來區(qū)分背景和前景；同時為了改善檢測不完整問題，提出了一種點云分割輔助下的運動物體提取方法，實驗結(jié)果表明該方法不僅可以提升檢測完整度，而且在一定程度上可以去除誤檢點；最后，基于Octomap 算法在低內(nèi)存損耗下完成靜態(tài)地圖的重建，為后續(xù)移動小車的自主導(dǎo)航提供了基礎(chǔ)，具有較好的實用價值。