梅 強，吳懷宇

（武漢科技大學 信息科學與工程學院，湖北 武漢 430081）

在移動機器人領域，環(huán)境建模與運動規(guī)劃都是機器人實現(xiàn)智能性的具體表現(xiàn)。其中環(huán)境建模是一個非常復雜和耗時的問題。因為這項研究涉及一些基本的科學問題。而本文將介紹自主移動機器人的控制和二維激光測距儀結合電機云臺生成三維點云，然后通過多方位的三維點云在同一個坐標系統(tǒng)中建立一個大致的場景。

激光測距儀雷達以其精度高、測距速度、快獲取信息直觀等優(yōu)點在軍事、民用和航空等領域得到了越來越多的應用。有些團隊用二維激光測距儀建立三維環(huán)境。如zhao[1]等人使用了兩個二維測距儀來獲取三維數(shù)據(jù)。一個激光掃描儀水平安裝，另一個垂直安裝。通過當前的機器人機器人位姿將其轉換為一個三維點。還有一些團隊跟本文采取方法一樣。Unchter和Surmann[2]，蔡自興和鄒小兵[3]采用精度轉動云臺與2-D掃描的激激光測距傳感器，設計并實現(xiàn)了非結構化環(huán)境下移動機器人識別感知系統(tǒng)，該系統(tǒng)可分析地面的平坦性和判斷移動機器人的障礙區(qū)域和可行區(qū)域，為移動機器人導航過程中的路徑規(guī)劃與環(huán)境建模提供支持。

在三維重建過程中，數(shù)據(jù)配準自動化程度決定了三維重建過程的自動化程度，數(shù)據(jù)配準結果也在很大程度上決定了三維模型重建精度。數(shù)據(jù)配準過程分為：粗配準、精度配準和全局配準[4]。最常見的粗配準方法是基于物體的球諧函數(shù)、高階矩不變量?；陔S機采樣的幾何哈希技術的配準方法也十分常見。將這種算法應用于三維數(shù)據(jù)配準中，并考慮到了誤差的對速度影響。利用了仿射不變量來約束候選樣本數(shù)量，極大地加快了配準的速度。

1 三維點云信息獲取

在測量距離時，通過測量激光從發(fā)出到接受所經(jīng)歷的時間來計算出物體到激光測距儀的距離。本文采用二維激光配合旋轉電機的方法獲取三維空間點云數(shù)據(jù)，實現(xiàn)三維場景構建。通過10 s左右的時間內(nèi)完成一次從下到上，從右到左的完整掃描，獲取豎直方向100°區(qū)域、平面方向180°區(qū)域內(nèi)的一組大約361×500的三維激光點云數(shù)據(jù)。

如圖1所示，為三維激光測距系統(tǒng)的參數(shù)坐標關系圖，ρ作為激光發(fā)射點到被測物體的距離；θ為掃描方向的水平偏向角度；φ為云臺旋轉角度，從而得到以云臺旋轉軸為坐標中心的三維激光參數(shù)（ρ，θ，φ）。再通過參數(shù)標定獲取平臺參數(shù)（a，b，z）然后由式（1）計算機器人當前位姿場景數(shù)據(jù)（x，y，z）通過里程計獲取當前位姿Px，Py，Pθ，由式（2）計算得到三維場景的全（X，Y，Z）[5]。如圖2所示，為二維激光和云臺結合的三維點陣效果圖。

圖1 三維激光測距系統(tǒng)參數(shù)坐標關系圖Fig.1 Coordinate diagram of 3D laser ranging system

圖2 三維點陣Fig.2 3D point cloud array

2 多平面特征提取

機抽樣一致性RANSAC算法并不是用所有的初始值去估計模型參數(shù)的算法，而是滿足可行條件的盡量少的初始數(shù)據(jù)。并使用一致數(shù)據(jù)集去擴大它，這是一種尋找模型去擬合數(shù)據(jù)的思想。

2.1 擬合直線段端點的定權算法

本文提出的算法直接擬合平面所屬的擬合直線段集合中每個擬合直線段的端點數(shù)據(jù)進行擬合平面參數(shù)?？蓪M合直線段端點的定權問題轉化到二維空間中進行討論。并認為同一條掃描線中共面的掃描點的精度相同且相互獨立，根據(jù)間接平差法的原理并考慮本文所采用的直線擬合公式，可知擬合直線方程公式中兩系數(shù)的協(xié)因數(shù)陣為

擬合直線段端點的坐標為擬合直線段系數(shù)的參數(shù)，寫成矩陣形式如式（4）：

其中，（xe，xb）（xb，yb）分別為擬合直線段的兩段點。根據(jù)誤差傳播律，向量（yb，ye）T的協(xié)因數(shù)陣為：

就擬合直線段集合而言，各條擬合直線段可認為相互獨立。因此，整個擬合直線段集合集中所有擬合直線段點的協(xié)因數(shù)陣（此矩陣對角線上元素不為零，其余元素為零）為：

式中：Qiep為擬合直線段集合中第i條擬合段兩端點的協(xié)因數(shù)陣；n為擬合直線段集合中擬合直線段的數(shù)量。采用分塊矩陣逆算法對Q陣求逆，即可得到所需的權陣。

2.2 利用擬合直線段端點計算擬合平面

一個三維平面通過兩種方式就可模擬出來。第一種可以用不共線的3個三維點（p1，p2，p3∈R3）計算出穿過這3個點的平面ax+by+cz+d=0的參數(shù)；第2種是通過一個三維點和通過這個點的平面法向量（p1，n且‖n‖=1，p1，n∈R3）也可以計算出這個平面的參數(shù)。通過以下步驟來提取平面[8]：

1）在初始點云中隨機選擇3個點，直接計算平面ax+by+cz=0，然后再計算點云至生成平面的距離di=|axi+byi+czi+d|。

2）選擇閾值t，若di≤ε則認為是內(nèi)點，統(tǒng)計得出此平面內(nèi)點的個數(shù)。

3）如果這個平面的內(nèi)點數(shù)超過了一個作者設計的極致數(shù)（比如50個點），那么就確定這個平面是存在的且記錄下來。再用同樣的方法迭代其他的平面。如果這個平面內(nèi)點不能超過極致數(shù)，這個平面就不存在且不記錄下來。同樣再重復第一步驟迭代提取平面。

閾值的選擇很重要，在判斷內(nèi)點的時候如果選擇過小的閾值會放棄選擇的內(nèi)點，而選擇過大的閾值則可能將異常點誤判為內(nèi)點。

3 ICP算法配準

Besl和Mikay所介紹的迭代最近點法ICP是從測量點集中確認對應的最近點點集后，運用和所提出的嚴密解算過程重新計算最近點點集；迭代計算直到目標函數(shù)值不變化，停止迭代過程。Besl和Mikay用以下7個參數(shù)向量作為旋轉和平移的表示方法：

[q0，qx，qy，qz]表示旋轉參數(shù)，tx，ty，tz」表示平移參數(shù)。其中選擇參數(shù)滿足約束條件為：根據(jù)迭代初值X0，計算新點集Pi

其中Pi表示原始未修改的點集，Pi的下標表示迭代次數(shù)，參數(shù)向量X的初始值為：

ICP處理過程如下：

1）由點集Pk中的點，搜索曲面S上相應最近點點集Prk；

2）計算兩個點集的重心位置坐標，并進行點集中心化生成新的點集，并確定正定矩陣N。

3）根據(jù)[16]計算最大特征向量等價于殘差平方和最小時旋轉矩陣R。其中旋轉矩陣R：

4）在旋轉矩陣R被確定后，由平移向量t僅僅是兩個點集的重心差異，可以通過兩個坐標中的重心點和旋轉矩陣確定。

4）通過迭代的思想用參數(shù)向量Xk+1、R、t生成一個新的點集Rk+1；

5）當距離平方和的變化小于預設的閾值τ時就停止迭代，停止迭代的判斷準則為fk－fk+1＜τ。

4 四點算法配準

在ICP算法的基礎之上加入限制約束條件。對ICP算法計算前隨機提取有規(guī)律的四點進行迭代。使全局配準具有較好的魯棒性，在原始數(shù)據(jù)有外點，噪聲較大時也能獲得較好的初值。對應點搜索步驟如下：

1）點集中隨機選擇一個平面上的4個點a，b，c，d計算交點e，以及比例和距離d1＝‖a－b‖；d2＝‖c－d‖，保證魯棒性a，b，c，d 4點兩兩的距離大于一定值，如圖3所示。

圖3 獲取四點距離及其比例Fig.3 The proportions and distances of four points

2）在Q點集中。找出所有可能的點對，使距離分別等于d1和d2，并將這對集合分別保存在集合A和B中，其中qa，qb，qc，qd分別是a，b，c，d點可能的對應點，如圖4所示。

圖4 可能存在的對應點Fig.4 The corresponding possible point

3）分別計算集合和中的每一對點映射相交點e1＝qa+r1（qa－qd），e2＝qc+r2（qc－qd），如圖5所示。

圖5 配準點集合及其可能交點Fig.5 Registering collection and possible intersections

4）如果e1≈e2，則qa，qb，qc，qd對應點就確定了。其中需找相同交點可以采用KD－Tree來加速。

5 實驗結果與分析

本文開發(fā)了配準軟件的圖形界面，基于OpenGL技術進行三維點云數(shù)據(jù)的人機交互顯示，使用C++開發(fā)語言。選擇使用的VCGlibrary開源函數(shù)庫，提供了大量的三維數(shù)據(jù)處理的函數(shù)模塊，給算法實現(xiàn)減少了很多編碼工作量，使得研究工作能更多地專注于算法思想上。

本文將四點算法和傳統(tǒng)進行比較分析。如圖6（a）（c），隨著高斯噪音和異常點比例的加強，評估誤差（馬爾科夫誤差）會有明顯不一樣。在低高斯噪音和低異常點比例的良好條件下二者算法的配準精確度相差不多。而在高噪音和高異常點比例的不利條件下時四點算法精確度明顯好于RANSAC。如圖6（b）隨著重疊區(qū)域的比例變小，四點算法的精確度也優(yōu)于傳統(tǒng)RANSAC。同時在計算的速度上面四點算法也優(yōu)于傳統(tǒng)RANSAC，如圖6（d）。

圖6 （a，b，c，d）四點算法和傳統(tǒng)算法指標參數(shù)比較Fig.6 Parameters comparsion of four points algoritnm and traditional RANSAC algorithm

下面是經(jīng)過展現(xiàn)的效果圖。圖7是通過提取平面特征后再經(jīng)過四點算法配準后得到的三維配準圖。

圖7（a）為兩個激光掃描圖初始配準圖；7（b）圖為經(jīng)過5次迭代后的配準圖；圖7（c）為經(jīng)過25次配準迭代后的配準圖。

圖7 配準實驗圖Fig.7 The registration rendering

6 結束語

通過三維激光和轉動云臺得到三維點陣，并用四點算法進行粗配準。經(jīng)實驗驗證，該算法計算量小且能正確的配準三維模型。完全滿足移動機器人實時性和準確性的要求。

本文只考慮了兩視角間三維點云數(shù)據(jù)的配準問題，并沒有考慮多視角間的全局數(shù)據(jù)配準。而后續(xù)工作將可以基于這個方向發(fā)展?？梢约尤隝CP算法精確配準加以完善?？梢愿鶕?jù)文獻[6－7]實現(xiàn)根據(jù)不同的數(shù)據(jù)集，自動選擇采樣方法，以增加配準的自動化程度和魯棒性。

[1]Zhao H，Shibasaki R.Reconstructing textured CAD model of urban environment using vechile-borne laser range scanners and line cameras[J].Lecture Notes in Computer Science，2001，34（5）：284－295.

[2]Nuchter A，Surman H，Hertzberg J.Automatic model refinement for 3D reconstruction with mobile robots[C]//Proceedings of the Fourth International Conference on 3－D Digital Imaging and Modeling，2003：394－401.

[3]鄒小兵，蔡自興，于金霞.基于激光測距的移動機器人3－D環(huán)境感知系統(tǒng)設計[J].高技術通訊，2005，15（9）：38－43.ZOU Xiao-bing，CAI Zi-xing，YU Jin-xia.Mobile robot based on laser ranging 3－D context-aware system design[J].Chinese High Technology Letters，2005，15（9）：38－43.

[4]Cecilia.C and Chen.R.Segmentation and registration for 3D modeling of large-scale urban scenes[D].City University of New York：Chen.R，2007.

[5]陳杭.移動機器人三維環(huán)境建模與自主運動規(guī)劃[D].大連：大連理工大學，2009.

[6]Gelfand N.，Ikemoto L.，Rusinkiewicz S，et al.Geometrically stable sampling for the ICP algorithm[C].Proceedings of the Fourth International Conference on 3D Digital Imaging and Modeling，2003：260－267.

[7]Lee B U，Kim C M，Park R H.An orientation reliability matrix for The iterative closest point algorithm[J].IEEE Tansactions on PAMI，2000，22（10）：1205－1208.

[8]趙錄剛，吳成柯.基于隨機抽樣一致性的多平面區(qū)域檢測算法[J].計算機應用，2008，28（r2）：154－157.ZHAO Lu-gang，WU Cheng-ke.Multi－plane region detection algorithm based on random sampling consistency[J].Journal of Computer Applications，2008，28（2）：154－157.