基于改進(jìn)3D-NDT 機(jī)器人自定位算法

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

（中國計(jì)量大學(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]。其中：對(duì)于SVD 和PCA 算法而言，它們的計(jì)算效率較高[6-7]，但其對(duì)輸入點(diǎn)云的剔除異常值要求較高，并且存在誤差大的缺點(diǎn)；而ICP 算法相對(duì)而言，誤差較小，并且較為穩(wěn)定。但是，對(duì)于ICP 算法，其又存在計(jì)算效率低下，計(jì)算量較大，而且對(duì)于輸入點(diǎn)云的初始姿態(tài)有著苛刻的要求，如果沒有很好地提供初始姿態(tài)，誤差也會(huì)出現(xiàn)大偏差。在3D-NDT 算法中，其采用計(jì)算正態(tài)分布的方式作為點(diǎn)云配準(zhǔn)的方法，就不同于ICP 算法，需要大量時(shí)間來計(jì)算得出配準(zhǔn)結(jié)果，適用于機(jī)器人定位方法中[8-9]。但其在計(jì)算過程中，在沒有提供輸入點(diǎn)云的初始姿態(tài)下，匹配結(jié)果還是會(huì)存在較大誤差，而且要消耗大量時(shí)間。針對(duì)該缺陷，本文提出基于ISS 特征點(diǎn)的改進(jìn)3D-NDT 算法，來進(jìn)行機(jī)器人的定位。

1 3D-NDT 算法

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

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)行空間劃分，分開為大小固定的各個(gè)小的空間，也叫體素，其中體素大小決定了兩幅點(diǎn)云配準(zhǔn)之間的精度，體素的尺寸越大，點(diǎn)云配準(zhǔn)的誤差越大，反之，則越小。對(duì)于時(shí)間來說，體素尺寸越大，時(shí)間反而越小。

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

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

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

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

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

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

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

2.1 算法原理

改進(jìn)的3D-NDT 算法在于為3D-NDT 算法提供一個(gè)初始姿態(tài)估計(jì)，先對(duì)輸入點(diǎn)云進(jìn)行ISS 特征點(diǎn)提取，然后計(jì)算這些特征點(diǎn)的特征直方圖（FPFH），根據(jù)這些FPFH 進(jìn)行對(duì)應(yīng)點(diǎn)匹配[10]，用RANSAC 去除錯(cuò)誤對(duì)應(yīng)關(guān)系，得到初始姿態(tài)估計(jì)結(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）對(duì)輸入的每個(gè)查詢點(diǎn)設(shè)定一個(gè)搜索半徑。

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

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

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

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

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

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

2.2.2 FPFH 特征

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

FPFH 特征的計(jì)算方法如下：

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

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

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

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

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

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

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

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

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

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

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

本文在機(jī)器人上搭建三維掃描平臺(tái)進(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)去除來減少計(jì)算量以及去除異常點(diǎn)對(duì)配準(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)云圖

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

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

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

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

改進(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é)果對(duì)比圖

4 結(jié) 語

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