余 恒 孫志剛 肖 力

（華中科技大學自動化學院 武漢 430074）

1 引言

近些年來，自主導航技術(shù)雖然取得了極大的進步，然而井下環(huán)境非常復雜，傳統(tǒng)自主導航方式難以正常運行，無法滿足井下自主導航的需求。三維激光掃描技術(shù)由于其非接觸性、高精度、實時性強等優(yōu)點［1］，與傳統(tǒng)的導航技術(shù)相比，具有對周圍環(huán)境不敏感，信息量大，數(shù)據(jù)準確等優(yōu)點。即使處在環(huán)境復雜的井下，也能夠發(fā)揮預期的性能，使用三維激光掃描儀進行井下自主導航具有巨大的研究價值。

井下導航在國內(nèi)外得到了很長一段時間的研究，并在實際環(huán)境中得到了應用。文獻［2］提出了一種利用自然信標實現(xiàn)的鏟運機絕對導航方法。該方法使用慣性導航進行定位，利用最小結(jié)構(gòu)算法和卡爾曼濾波算法判定車輛姿態(tài)，并通過迭代最近點算法判定所處位置，這種方式能精確地獲取系統(tǒng)的位置信息，根據(jù)系統(tǒng)搭載的地圖生成軌跡，但對慣性導航的依賴較高，且慣性導航的累積誤差無法消除，定位誤差會隨著時間的累加不斷增加?？ㄌ乇死?，CSIRO和澳大利亞高校聯(lián)合開發(fā)的MINE?GEM半自動導航系統(tǒng)，在巷道壁支架安裝反光帶，利用反光帶生成導航軌跡，目前已在澳大利亞以及瑞典的礦山得到應用［3］。國內(nèi)楊超等借助條形碼原理和帶有標度信息的環(huán)境拓撲圖設(shè)計了一種基于“沿壁法”的反應式導航方法，利用二維碼信息生成導航軌跡，在導航時利用測程法定位，使用激光雷達掃描前方和巷道側(cè)壁，在實現(xiàn)沿壁導航的同時保證一定的前瞻性［4］。上述兩種方法都是在井下安裝相應的標識物，系統(tǒng)通過識別標識物來進行導航，但這種方法需要事先設(shè)置，且維護這些標識物也需要人力和物力成本。

本文提出了基于點云骨線的導航軌跡生成方法，該方法使用激光掃描儀采集井下環(huán)境的三維場景信息，系統(tǒng)根據(jù)采集的三維點云數(shù)據(jù)進行判斷，不需要事先設(shè)置標識物。

2 導航軌跡生成

整體流程如圖1所示。

首先通過平面提取算法提取出系統(tǒng)的行駛面，接著將行駛面作為輸入數(shù)據(jù)，基于L1中值理論進行骨線提取，提取出的骨線便可作為系統(tǒng)的導航軌跡。

2.1 點云平面提取

三維激光掃描儀采集的點云是無序和散亂的，在散亂點之間建立合適的拓撲關(guān)系，一般通過鄰域分析實現(xiàn)，用一個點鄰域范圍內(nèi)所有點的分布與空間特征來描述該點的特征［5］。點云平面提取是點云分割的一個具體表現(xiàn)。點云數(shù)據(jù)的分割是將雜亂無章的點云數(shù)據(jù)根據(jù)屬性特征分割成若干個互不相交的子集，每一個子集中的數(shù)據(jù)具有相同的屬性特征或一定的語義信息［6］。目前的平面提取算法主要有以下三種：基于霍夫變換方法［7］，基于區(qū)域生長或聚類方法［8］和基于隨機一致采樣（Ran?dom Sample Consensus，RANSAC）［9］方法，本文使用的平面提取算法是隨機一致采樣（Random Sam?ple Consensus，RANSAC）方法。這種方式能夠剔除局外點的影響，使得集合中都是具有相同的特征的點。作為一種有效的穩(wěn)健估計算法，RANSAC在基礎(chǔ)矩陣估計、特征匹配、運動模型選擇等計算機視覺領(lǐng)域內(nèi)有著廣泛應用［10］。

RANSAC算法在進行參數(shù)選取時，不會不加區(qū)分地將所有的輸入數(shù)據(jù)全部選取，具體來講，先根據(jù)實際應用問題提出一個模型，例如，判斷某些點是否是圓柱面上的點，就可以先構(gòu)造出一個圓柱面的方程。接著找出一個可以判斷此模型的規(guī)則，不符合該規(guī)則的數(shù)據(jù)舍棄掉，符合的數(shù)據(jù)保留，最后使用選取好的數(shù)據(jù)去估計具體的模型參數(shù)。在進行種子點的選取時，還可以根據(jù)實際需求進行調(diào)整。

如圖2所示，該算法需要3個輸入?yún)?shù)：迭代次數(shù)K、誤差t、閾值大小N。一般選取迭代次數(shù)K的時候依據(jù)的表達式：

z代表隨機選取的數(shù)據(jù)點是有效點的概率；w代表數(shù)據(jù)點符合有效點集D的概率；n為確定M的最少點數(shù)，本文中M是平面，n取3。

圖2 RANSAC算法

2.2 L1中值理論

目前已有的骨線提取算法有以下幾種：基于測地線的多邊形網(wǎng)格模型骨線提?。?1］、基于拉普拉斯算子收縮的點云模型骨線提?。?2］和基于旋轉(zhuǎn)對稱軸（ROSA）的骨架曲線提?。?3］。 L1中值是統(tǒng)計學里面長期研究的一個重要概念［14］，在應對噪聲干擾有著獨特的優(yōu)勢。

假設(shè)有一個輸入點集Q={qi}，在空間中求出一點x，使得該點到點集內(nèi)所有的點的距離加在一。當p=2時，求解的是L2平均值，該問題的解為集Q中點的個數(shù)。當p=1時，求解的就是L1中值，上面的敘述可知，當點集Q中存在野點時，L2平均值就會產(chǎn)生較大的偏移，因此L2平均值無法解決點云數(shù)據(jù)的野點和噪聲點。L1中值會賦予每個點不同的權(quán)重，當某個點的距離比較遠時，此時賦予的權(quán)重比較小，對結(jié)果產(chǎn)生的影響也會減小，因此L1中值能很好的降低點云中野點和噪聲點所產(chǎn)生的影響，圖3便是兩者之間具體的差別。

圖3 L1中值（左圖）和L2平均值

整個點集的L1中值是一個點，而點云的骨線是由線組成的，因此僅僅求出L1中值是無法提取出一副點云的骨線的，但是可以通過求解出局部點集的中值點，再將這些中值點連接在一起，這樣求解的線就可以稱作是點云的骨線，因為求解的骨線上的點都是各自鄰域內(nèi)的中值點，這些中值點能夠反映各自鄰域的特征，這些中值點組合成的點云骨線也就可以最大限度的反映三維場景?？梢詫⑦@里的中值點稱為局部L1中值。

2.3 骨線提取算法流程

基于L1中值的骨線提取算法流程圖如圖4所示。

圖4 骨線提取算法流程圖

由于骨線提取比較復雜，直接將原始數(shù)據(jù)點集作為后續(xù)迭代計算的起始點會導致迭代計算時間過長。為了節(jié)約后續(xù)的計算時間，本文采用隨機下采樣的方式對點云進行稀疏化處理，采樣點數(shù)量越少，計算時間越短，同時算法穩(wěn)定性也就會有所下降。

點云迭代收縮是算法的核心部分，具體分為六個步驟：

1）首先對三維激光掃描儀采集到的原始的三維點云數(shù)據(jù)集合J進行精簡處理，之后再進行隨機下采樣得到新的集合I；

2）確定一個較小的鄰域半徑h，求解出I中每個點xi在兩個集合中的鄰域集合，在J中的鄰域集合設(shè)為O，在I中的鄰域集合設(shè)為S；

4）接下來需要求出xi的分布度量值σi，分布式度量值可用于骨線點的選取，基于步驟2求解出的集合I中的每個點的鄰域集合S，對于每個點xi所對應的S，遍歷S中的每個點xi，求出集合S的平均值 xˉi，再次遍歷集合 S，計算偏移量 dif，按照構(gòu)建協(xié)方差矩陣，求出該矩陣的特征值λi，從小到大依次為

6）根據(jù)上述步驟求解出的Vi和Wi，則集合I中每個點 xi的新坐標：xi=Vi+σi·μ·Wi，其中 μ 可以根據(jù)需求自行設(shè)定，當?shù)螖?shù)過少或者任意兩次迭代的結(jié)果差別大于設(shè)定的閾值，返回步驟2）。

2.4 系統(tǒng)軌跡修正

當提取出系統(tǒng)行駛面的骨線時，便可以通過利用骨線對系統(tǒng)的軌跡進行修正。假設(shè)提取出的骨線點集為Q，系統(tǒng)在當前時刻t1的位置坐標w1(x1，y1，z1)，系統(tǒng)有記錄的與t1最接近時刻的系統(tǒng)軌跡點坐標w2(x2，y2，z2)，接著遍歷提取的骨線點集Q，找出與w1最接近的點通過近鄰搜索找出點集Q中與點v1最近的點

圖5 軌跡調(diào)整變量示意圖

1）當偏航距離小于設(shè)定的閾值且偏航角β小于設(shè)定的閾值時，即可以認為系統(tǒng)正沿著骨線行進；

2）當偏航距離小于設(shè)定的閾值且偏航角β大于設(shè)定的閾值時，即可以認為系統(tǒng)正準備偏離骨線，可以使系統(tǒng)向著向量的方向進行小角度偏轉(zhuǎn)；

3）當偏航距離大于設(shè)定的閾值時，即可以認為系統(tǒng)已經(jīng)偏離骨線，可以使系統(tǒng)向著向量的方向進行大角度偏轉(zhuǎn)。

提取的系統(tǒng)行駛面骨線是理想的導航軌跡，但在實際運行中，系統(tǒng)需要對軌跡上的障礙物進行避障處理，這就會導致系統(tǒng)偏離理想的軌跡，這時就需要采用上述的方式對系統(tǒng)的軌跡進行修正。

3 算法實例與實驗分析

為了驗證本文提出的方法的可行性和有效性，本文以真實的井下三維場景進行實驗。上位機軟件在Qt的開源框架下編程開發(fā)的［15］。圖6是原始井下點云數(shù)據(jù)，該點云數(shù)據(jù)采用開源點云庫Point Cloud Library（PCL）［16］中定義的 PCD 格式進行存儲，。圖7是對井下點云數(shù)據(jù)直接進行骨線提取的結(jié)果，對比圖4和圖5可以看出，由于井下環(huán)境復雜，無法直接提取骨線。

圖6 井下點云圖像

圖7 井下點云圖像骨線提取

為了使提取的骨線能夠適用于井下的導航要求，本文對圖6進行平面提取，具體效果如圖8所示。接著對提取出的平面進行骨線提取，結(jié)果如圖9所示。

圖8 井下點云圖像平面提取

圖9 提取出的骨線

為了更加直觀地展示實驗結(jié)果，本文將提取的骨線和原始點云數(shù)據(jù)組合在一起，圖10和圖11分別從不同視角展示。

圖11 骨線提取側(cè)視圖

提取的骨線能真實的反映井下行駛面的走向，從而為井下導航提供導航軌跡。圖12是提取的骨線的具體點云數(shù)據(jù)信息。文件由文件頭和數(shù)據(jù)區(qū)組成，文件頭各字段所代表的具體信息可以查閱相關(guān)文獻，數(shù)據(jù)區(qū)便是提取的骨線的真實坐標，在系統(tǒng)軌跡已知的情況下，可以使用2.4的方法進行軌跡修正，在未知環(huán)境進行自主導航。

圖12 骨線點云PCD文件信息

4 結(jié)語

本文針對傳統(tǒng)導航方式無法在井下正常工作以及目前井下導航方式無法實現(xiàn)“邊探索邊導航”這一理想的導航方式，提出了基于點云骨線的導航軌跡生成的方法。首先對原始點云進行平面提取以消除井下復雜的環(huán)境對于導航軌跡生成的影響，再基于中值理論對提取的平面進行骨線提取，針對實際應用對算法進行了調(diào)整，最后結(jié)合提取的骨線信息和系統(tǒng)的行駛軌跡，在不同的情況下對系統(tǒng)未來的行駛方向給出不同的決策。實驗結(jié)果表明，本文提出的方法既能不受井下復雜環(huán)境的干擾，又能提供具體的導航軌跡，為井下理想的“邊探索邊導航”導航方式提供基礎(chǔ)。