許振權(quán)，徐紅偉

（中國計量大學(xué)機(jī)電工程學(xué)院，浙江 杭州 310000）

0 引 言

近幾年，多數(shù)學(xué)者提出了一種以激光點(diǎn)云數(shù)據(jù)進(jìn)行機(jī)器人自定位的方法，通過機(jī)載激光雷達(dá)與點(diǎn)云地圖進(jìn)行匹配來獲取機(jī)器人精確的定位信息，并且這種方法是可行的[1]。在該項(xiàng)定位技術(shù)中，點(diǎn)云匹配是其關(guān)鍵技術(shù)，目前有的方法包括SVD（奇異值分解）、ICP 以及PCA（主成分分析）[2-5]。其中：對于SVD 和PCA 算法而言，它們的計算效率較高[6-7]，但其對輸入點(diǎn)云的剔除異常值要求較高，并且存在誤差大的缺點(diǎn)；而ICP 算法相對而言，誤差較小，并且較為穩(wěn)定。但是，對于ICP 算法，其又存在計算效率低下，計算量較大，而且對于輸入點(diǎn)云的初始姿態(tài)有著苛刻的要求，如果沒有很好地提供初始姿態(tài)，誤差也會出現(xiàn)大偏差。在3D-NDT 算法中，其采用計算正態(tài)分布的方式作為點(diǎn)云配準(zhǔn)的方法，就不同于ICP 算法，需要大量時間來計算得出配準(zhǔn)結(jié)果，適用于機(jī)器人定位方法中[8-9]。但其在計算過程中，在沒有提供輸入點(diǎn)云的初始姿態(tài)下，匹配結(jié)果還是會存在較大誤差，而且要消耗大量時間。針對該缺陷，本文提出基于ISS 特征點(diǎn)的改進(jìn)3D-NDT 算法，來進(jìn)行機(jī)器人的定位。

1 3D-NDT 算法

3D-NDT 算法是把一個三維體素內(nèi)的點(diǎn)云數(shù)據(jù)集轉(zhuǎn)換成一個連續(xù)可微的概率分布函數(shù)來進(jìn)行點(diǎn)云匹配的算法[8-9]。它將輸入點(diǎn)云劃格分開，形成均勻規(guī)則固定大小的三維單元格，在各個單元格內(nèi)就形成了一個個的點(diǎn)云集，通過對各個單元格內(nèi)的點(diǎn)云集進(jìn)行正態(tài)概率分布，兩幅點(diǎn)云數(shù)據(jù)進(jìn)行相互計算，不斷反復(fù)，最終得到匹配的結(jié)果。在整個算法流程中，網(wǎng)格內(nèi)的正態(tài)分布概率密度只需要計算一次，不用對每個點(diǎn)都進(jìn)行相對應(yīng)的計算和匹配操作，因此，該算法計算量不大，計算效率也較高。

3D-NDT 算法的輸入?yún)?shù)是兩幅點(diǎn)云數(shù)據(jù)S1、S2，輸出是兩幅點(diǎn)云變換的參數(shù)R、T。在機(jī)器人自定位中，兩幅分別為機(jī)器人在當(dāng)前位置掃描出來的點(diǎn)云圖以及機(jī)器人在當(dāng)前大致位置的點(diǎn)云地圖，通過輸出的R、T就可以明確知道機(jī)器人在當(dāng)前點(diǎn)云地圖中的確切位置。

3D-NDT 算法具體步驟為：

1）將輸入點(diǎn)云進(jìn)行空間劃分，分開為大小固定的各個小的空間，也叫體素，其中體素大小決定了兩幅點(diǎn)云配準(zhǔn)之間的精度，體素的尺寸越大，點(diǎn)云配準(zhǔn)的誤差越大，反之，則越小。對于時間來說，體素尺寸越大，時間反而越小。

2）把輸入的參考點(diǎn)云投影到各個體素上。

3）計算每個體素的正態(tài)分布概率密度函數(shù)參數(shù)：

式中：yk,k= 1,2,…,m表示體素內(nèi)的各個點(diǎn)；Σ描述各個體素內(nèi)的協(xié)方差。

4）將第二幅點(diǎn)云按照初始輸入R、T進(jìn)行變換。

5）第二幅點(diǎn)云落于各個體素，計算其相應(yīng)的概率分布函數(shù)PDF。

6）使用最大似然函數(shù)得到最終結(jié)果。

2 改進(jìn)的3D-NDT 算法

2.1 算法原理

改進(jìn)的3D-NDT 算法在于為3D-NDT 算法提供一個初始姿態(tài)估計，先對輸入點(diǎn)云進(jìn)行ISS 特征點(diǎn)提取，然后計算這些特征點(diǎn)的特征直方圖（FPFH），根據(jù)這些FPFH 進(jìn)行對應(yīng)點(diǎn)匹配[10]，用RANSAC 去除錯誤對應(yīng)關(guān)系，得到初始姿態(tài)估計結(jié)果。改進(jìn)后的3D-NDT 算法如圖1 所示。

圖1 改進(jìn)的3D-NDT 流程圖

2.2 算法實(shí)現(xiàn)

2.2.1 ISS 特征點(diǎn)

想要獲取兩幅點(diǎn)云的變換關(guān)系，提取特征點(diǎn)是其中的關(guān)鍵所在[11]。ISS 特征點(diǎn)是一種通過與鄰域信息建立聯(lián)系，并利用特征值之間的關(guān)系來表示點(diǎn)特征程度的方法。

ISS 特征點(diǎn)提取步驟為：

1）對輸入的每個查詢點(diǎn)設(shè)定一個搜索半徑。

2）計算查詢點(diǎn)與鄰域內(nèi)各個點(diǎn)的歐氏距離，并設(shè)定相關(guān)的權(quán)值大小。

3）計算每個查詢點(diǎn)與鄰域內(nèi)所有點(diǎn)的協(xié)方差矩陣。

4）計算協(xié)方差矩陣的所有特征值，并將其排序，采用從大到小的排序方式。

5）設(shè)置閾值，滿足閾值條件的即為ISS 特征點(diǎn)。

ISS 特征點(diǎn)示意圖如圖2 所示。

圖2 ISS 特征點(diǎn)示意圖

2.2.2 FPFH 特征

FPFH 特征可以描述點(diǎn)的特征，是點(diǎn)特征的描述子，是基于PFH 做的，提高了計算效率。使用FPFH 特征提取，可以在一定程度上提高對點(diǎn)云的認(rèn)知，實(shí)現(xiàn)點(diǎn)云的高層次認(rèn)知。

FPFH 特征的計算方法如下：

1）對于點(diǎn)云模型中所有點(diǎn)先以半徑r鄰域計算三個特征算子，在此基礎(chǔ)上，統(tǒng)計鄰域內(nèi)所有區(qū)間的點(diǎn)數(shù)量所占百分比得出簡化點(diǎn)特征直方圖SPFH。

2）對于每個點(diǎn)都需要另外包含2r半徑內(nèi)的其他一些點(diǎn)。對于新包含進(jìn)來的這些點(diǎn)在第一步計算中得到的SPSH 進(jìn)行加權(quán)并與點(diǎn)p本身的SPFH 求和，最終得到點(diǎn)p的FPFH，其中，F(xiàn)PFH 的計算公式如下：

2.2.3 RANSAC 去除錯誤對應(yīng)關(guān)系

得到點(diǎn)云中每個點(diǎn)的FPFH 之后，應(yīng)用直接對應(yīng)估計算法得到兩個點(diǎn)云中的對應(yīng)關(guān)系。而在對應(yīng)匹配的計算中，由于點(diǎn)云中操作異常噪聲點(diǎn)會帶來匹配結(jié)果存在很大的誤差，從而導(dǎo)致配準(zhǔn)失敗，因此，消除錯誤對應(yīng)關(guān)系是關(guān)鍵。

采用RANSAC 的方法剔除錯誤的點(diǎn)對關(guān)系，其主要用于解決樣本中的外點(diǎn)問題，它能有效地剔除一些不符合最優(yōu)參數(shù)模型的點(diǎn)，例如噪聲、無效點(diǎn)等[12]。RANSAC 算法步驟如下：

1）在樣本中隨機(jī)采樣K個點(diǎn)；

2）對K個點(diǎn)進(jìn)行模型擬合；

3）計算其他點(diǎn)到該擬合模型的距離，并設(shè)置閾值，如果大于閾值，則為外點(diǎn)，并刪除，小于閾值則保留；

4）采取內(nèi)點(diǎn)最多的模型重新估計模型并進(jìn)行改進(jìn)。

在剔除錯誤的點(diǎn)對關(guān)系后，就可以根據(jù)剩余的點(diǎn)云關(guān)系進(jìn)行匹配，得到一個比較正確的初始姿態(tài)估計，當(dāng)作3D-NDT 算法的初始姿態(tài)估計。

3 實(shí)驗(yàn)結(jié)果與分析

本文在機(jī)器人上搭建三維掃描平臺進(jìn)行相關(guān)實(shí)驗(yàn)數(shù)據(jù)獲取，水平采用360°方位掃描，垂直采用180°上下掃描，水平垂直的掃描角度都為0.5°，因此，可以得到點(diǎn)云數(shù)為260 642 的點(diǎn)云數(shù)據(jù)。再經(jīng)過體素降采樣與離群點(diǎn)去除來減少計算量以及去除異常點(diǎn)對配準(zhǔn)結(jié)果的影響[13]。室外兩幅場景點(diǎn)云數(shù)量為15 241 和15 452，室內(nèi)兩副場景點(diǎn)云數(shù)量為17 231 和17 624。實(shí)驗(yàn)測試數(shù)據(jù)如圖3 所示。

圖3 室內(nèi)外測試點(diǎn)云圖

對點(diǎn)云先進(jìn)行ISS 特征點(diǎn)提取，如圖4 所示。在經(jīng)過ISS 特征點(diǎn)提取后，室外一幅場景點(diǎn)云ISS 特征點(diǎn)數(shù)據(jù)為841個，室內(nèi)一幅場景點(diǎn)云ISS特征點(diǎn)數(shù)據(jù)為865 個，可以看到，在ISS 特征點(diǎn)提取后在保留源點(diǎn)云一些特征的條件下，大大降低了點(diǎn)云數(shù)量，從而也讓點(diǎn)云的初始匹配速度得到了一定的提高。

圖4 ISS 特征點(diǎn)提取圖

采用均方根誤差（RMSE）和時間作為性能評價指標(biāo)，其中，RMSE 可以描述匹配結(jié)果的好與壞，RMSE 數(shù)值越小，描述兩幅點(diǎn)云匹配之后差距越小，匹配結(jié)果更好，反之，則越差。時間是指在計算機(jī)輸入兩幅需要配準(zhǔn)的源點(diǎn)云后，到計算機(jī)輸出結(jié)果R、T數(shù)值之間所需要的時間長短。實(shí)驗(yàn)對比結(jié)果如表1 所示。

表1 實(shí)驗(yàn)結(jié)果對比

改進(jìn)3D-NDT 算法在精度上得到了一定的提升，在室外場景定位中RMSE 提高了54.651 2 mm，室內(nèi)場景定位中RMSE 提高了31.507 4 mm，改進(jìn)的3D-NDT 算法配準(zhǔn)效果也比較理想。點(diǎn)云配準(zhǔn)實(shí)驗(yàn)結(jié)果如圖5 所示。

圖5 實(shí)驗(yàn)結(jié)果對比圖

4 結(jié) 語

本文提出的改進(jìn)3D-NDT 的配準(zhǔn)算法相比傳統(tǒng)的3D-NDT 算法，對源點(diǎn)云進(jìn)行了基于ISS 特征點(diǎn)提取獲取初始姿態(tài)估計，提高了其3D-NDT 配準(zhǔn)的精度，為機(jī)器人自定位提供了更加準(zhǔn)確的定位結(jié)果。