混合視覺系統的運動物體檢測和靜態地圖重建

胡譽生，何炳蔚，鄧清康

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

0 引言

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

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

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

1 本文方法及其原理

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

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

1）輸入數據為相機所獲得的圖像數據和激光雷達所獲得的點云數據。首先對混合視覺系統進行外參標定，獲取相機與激光雷達兩傳感器之間的坐標變換參數，為后續數據融合提供基礎。

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

3）完整地提取出運動物體，利用改進的歐氏聚類算法對當前點云數據進行簇分割；之后利用點云數據索引值的唯一特性來融合動態點檢測結果與點云聚類分割結果，從而實現運動物體完整的提取。

4）通過Octomap 工具與當前點云數據所對應的激光雷達里程計，實現靜態地圖的重建。

1.1 全向相機與三維激光雷達的坐標變換

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

其中：表示t時刻激光掃描下的第i個點表示在對應圖像相機坐標系下的值；K表示相機的內參數；R和T分別表示投影變換陣中的旋轉矩陣和平移向量。

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

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

1.2 點云數據中動態點的檢測

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

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

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

1.2.1 點云地面識別

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

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

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

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

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

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

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

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

1.2.2 特征點運動向量的估計

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

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

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

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

1.2.3 點云數據動態點的提取

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

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

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

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

2）向量間夾角θ與設定的臨界閾值Thθ（取20°）比較，如果θ≥Thθ，則對應的特征點為動態點。

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

其中，σ2表示θ

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

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

點云數據動態點檢測過程中，不可避免地出現檢測不完整問題，可以將其分為以下兩種類型：

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

2）當非剛性物體的部分位置發生運動時，位于其靜態部分的特征點無法被檢測出來。

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

1.3.1 劃區域點云距離聚類分割

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

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

1）為輸入的點云數據pt建立一個KD-Tree 數據結構，并創建一個空隊列q；

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

3）利用KD-Tree 數據結構獲得該點的鄰近點，計算其到鄰近點的距離，并將距離小于設定閾值r的點索引加入到隊列q中；

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

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

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

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

圖5 不同區域對應不同距離閾值示意圖Fig.5 Schematic diagram of different distance thresholds corresponding to different areas

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

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

1.3.2 運動物體的提取

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

圖6 動態點歸簇示意圖Fig.6 Schematic diagram of finding clusters of dynamic points

其中：numd表示點云簇的動態點數量；numa表示點云簇的總點數量。

1.4 基于Octomap的靜態地圖重建

點云數據中運動物體提取之后，通過與之對應的里程計（1.2.2 節）將其轉換到世界坐標系下以完成點云地圖的重建，如式（12）所示，通常默認將地圖的第1時刻激光雷達所在的位置設為世界坐標系。

隨著建圖的進行，點云地圖的容量將會越來越大，并且會產生很多冗余數據，不僅提高了對計算機性能的要求，而且影響了點云地圖的精度，同時無法直接運用于路徑規劃等機器人任務。為了解決這個問題，本文使用了基于八叉樹數據結構的Octomap 地圖表示方法，其將三維空間劃分為樹狀空間網格，利用概率模型方式對每個葉子的狀態（空閑、占據、位置）進行刻畫，設t=1，2，…，T時刻，對應觀測到的數據為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 實驗與結果分析

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

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

2.1 運動物體提取結果

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

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

2.2 靜態地圖重建結果

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

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

本文分別對點云地圖和Octomap 地圖從點數與存儲大小兩個方面進行統計，其結果如表1 所示?？梢园l現，較于點云地圖，Octomap 地圖在點數與存儲空間上分別降低了92%與95%，其可以構建一個更高效、緊湊的環境地圖。

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

3 結語

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