任家豪 ，岳云霄

（長安大學(xué)工程機械學(xué)院，陜西 西安 710064）

視覺應(yīng)用是農(nóng)用機器人自動化中的關(guān)鍵技術(shù)[1-4]。深度相機是指能測量目標(biāo)到相機之間距離（深度）的相機。一般的相機僅僅能完成影像的拍攝及2D圖像的采集，而深度相機增加了深度測量的功能。將多深度相機點云利用GICP算法進(jìn)行配準(zhǔn)融合，該算法的誤差比標(biāo)準(zhǔn)ICP算法的誤差小一個數(shù)量級[5]，可擴展相機的范圍，提高點云密度，這在機器人自主運動領(lǐng)域有著十分重要的作用。

1 點云數(shù)據(jù)采集

1.1 硬件介紹

本研究使用英特爾公司的RealSense D435i深度相機。2014年，英特爾公司推出了RealSense深度攝像頭，幀率可達(dá)Kinect的兩倍[6]。該深度相機集成了兩個紅外傳感器（IR Stereo Camera）、一個紅外激光發(fā)射器（IR Projector）和一個彩色相機（Color Camera）[7]。立體深度相機系統(tǒng)主要包括視覺處理器D4和深度模塊兩部分[8]。D435i相機中每一幀原始點云的點數(shù)量為307 200，可以滿足配準(zhǔn)融合要求并且有所冗余。RealSense D435i深度相機如圖1所示。

圖1 RealSense D435i深度相機

1.2 點云采集

啟動相機節(jié)點roslaunch realsense2_camera rs_rgbd.launch，保存/camera/depth-registered/points話題中點云數(shù)據(jù)為pcd格式，在不同位置對同一目標(biāo)位置進(jìn)行點云采集作為點云融合的數(shù)據(jù)集。圖2為點云掃描的目標(biāo)位置，圖3為采集到的點云數(shù)據(jù)。其中，圖3（a）的相機位置沿x軸平移30 cm后得到圖3（b）的相機位置，由于地面反光，因此會存在部分無效點。

圖2 目標(biāo)位置

圖3 點云數(shù)據(jù)

2 GICP算法及優(yōu)化

2.1 GICP算法原理

ICP算法最早由Arun等[9]于1987年提出，這種點集與點集坐標(biāo)系匹配的算法被證明是解決復(fù)雜配準(zhǔn)問題的關(guān)鍵方法[10]。GICP點云融合算法與ICP算法目標(biāo)一致，但實現(xiàn)有所區(qū)別。ICP的理論推導(dǎo)嚴(yán)謹(jǐn)，但對點云要求比較嚴(yán)格，在實驗中可能無法做到兩個點集一一對應(yīng)；GICP統(tǒng)一了各種點云的對應(yīng)情況，故ICP算法可以被視為GICP算法的一種情況。當(dāng)然，在特殊情況下，GICP也會“退化”為ICP。

GICP算法使用高斯分布概率模型對點云各個點的位置進(jìn)行描述，目標(biāo)點云T={t1，t2，…，tn}與源點云S={S1，S2，…，Sn}中，各個點均服從高斯分布，則：

式中，^t、^s表示各個點云的理想位置。若點云中各個點均處于理想位置，理論上就存在Tr，使得源點云和目標(biāo)點云完全匹配。事實上，由于測量或傳感器自身的原因，點對之間存在著協(xié)方差。

定義殘差di=ti-TrSi，則殘差也應(yīng)服從高斯分布，即：

在上述問題中，已知數(shù)個殘差di，則變?yōu)橐阎c云結(jié)果的情況下求使得這種結(jié)果出現(xiàn)的最大概率的轉(zhuǎn)換矩陣T*的問題，將其表示如下：

取其負(fù)對數(shù)，將最大值問題轉(zhuǎn)化為最小值，即：

2.2 GICP算法優(yōu)化

針對RealSense D435i深度相機的點云采集特點，本研究對GICP算法進(jìn)行部分改進(jìn)，以提高算法計算效率及配準(zhǔn)的準(zhǔn)確性。

在進(jìn)行點云采集時，深度相機每幀可采集307 200個點云數(shù)據(jù)，點云之中不乏部分冗余數(shù)據(jù)及部分無效點數(shù)據(jù)，因此對源點云和目標(biāo)點云使用體素下采樣（體素濾波）方法。在不改變目標(biāo)點云特征的前提下，大大減少點云數(shù)量，提高GICP算法計算效率。與此同時，在使用RealSense D435i深度相機進(jìn)行點云采集時，所得到的點云數(shù)據(jù)可能存在測量誤差，造成部分點云坐標(biāo)存在NaN值，故本研究在配準(zhǔn)數(shù)據(jù)前對源點云與目標(biāo)點云中的無效點都進(jìn)行了去除。體素下采樣及去除無效點如圖4所示。

圖4 體素下采樣及去除無效點

在對深度點云進(jìn)行處理后，開始進(jìn)行算法優(yōu)化后的相機點云融合，深度相機點云融合流程如圖5所示。

圖5 深度相機點云融合流程

3 實驗驗證

本實驗基于開源點云庫 PCL1.9.1，使用C++語言進(jìn)行驗證?？衫肞CL庫中的Vtk模塊顯示結(jié)果。本研究使用試湊法，對濾波器處理時采用的體素大小的參數(shù)進(jìn)行實驗，當(dāng)使用（0.05, 0.05, 0.05）作為網(wǎng)格大小時，既可保證目標(biāo)特征又可提高融合速率。點云融合結(jié)果如圖6所示。

圖6 點云融合結(jié)果

4 小結(jié)

由圖6可知，使用RealSense D435i深度相機從不同方位拍攝點云圖，通過基于去除無效點及體素下采樣改進(jìn)的GICP算法成功進(jìn)行了融合，融合效果良好，且擴展了單深度相機的視角范圍。該方法可應(yīng)用于機器人導(dǎo)航等多個方向。