激光SLAM 自主導(dǎo)航的線段特征提取混合算法研究

桑 迪，周 軍，皇攀凌，李 蕾

（1.山東大學(xué)機(jī)械工程學(xué)院，山東 濟(jì)南 250061；2.山東大學(xué)高效潔凈機(jī)械制造教育部重點(diǎn)實(shí)驗(yàn)室，山東 濟(jì)南 250061；3.齊魯工業(yè)大學(xué)（山東省科學(xué)院），機(jī)械與汽車工程學(xué)院，山東 濟(jì)南 250353）

1 引言

自主導(dǎo)航作為一種新的導(dǎo)航技術(shù)已經(jīng)廣泛應(yīng)用于移動(dòng)機(jī)器人行業(yè)，移動(dòng)機(jī)器人通過所攜帶的激光、視覺、里程計(jì)等傳感器，在移動(dòng)過程中完成構(gòu)建地圖與定位。SLAM（simultaneous localization and mapping）為自主導(dǎo)航的關(guān)鍵技術(shù)，也稱為CML（Concurrent Mapping and Localization），即時(shí)定位與地圖構(gòu)建，或并發(fā)建圖與定位[1-2]。SLAM 描述的是機(jī)器人在陌生的環(huán)境中通過自身攜帶的傳感器，根據(jù)前一時(shí)刻的位姿以及里程計(jì)數(shù)據(jù)估計(jì)現(xiàn)在時(shí)刻的位姿，再通過激光視覺傳感器所獲得的數(shù)據(jù)進(jìn)行修正，不斷構(gòu)建增量式地圖，同時(shí)對(duì)原始的地圖進(jìn)行更新[3]。基于激光雷達(dá)SLAM自主導(dǎo)航中，環(huán)境對(duì)激光雷達(dá)的影響較小，具有較高的定位導(dǎo)航精度，缺點(diǎn)是傳感器所獲得的環(huán)境的信息少[4-8]。對(duì)原始數(shù)據(jù)的特征提取處理對(duì)建圖的精度以及機(jī)器人在環(huán)境中的定位有著重要的影響[9-10]。

國(guó)內(nèi)外學(xué)者針對(duì)線段特征提取的方法進(jìn)行了深入的研究：文獻(xiàn)[11]針對(duì)室內(nèi)環(huán)境相對(duì)簡(jiǎn)單的結(jié)構(gòu)特征，基于Hough 變換進(jìn)行特征提取，充分利用有限的測(cè)量數(shù)據(jù)，獲得較好的線段特征。但是霍夫變換特征提取算法的最大難點(diǎn)就在于對(duì)線性參數(shù)的量化確定。文獻(xiàn)[12]針對(duì)D&C 算法在明顯呈凹形分布的區(qū)域中，通過檢測(cè)兩條直線的斜率差值是否小于某一閾值，有效解決了凹形區(qū)域?qū)す饫走_(dá)D&C 特征提取算法失效的情況，但是沒有考慮到噪聲的影響，在直線擬合的時(shí)候會(huì)造成擬合誤差大。文獻(xiàn)[13]針對(duì)直接從激光雷達(dá)掃描數(shù)據(jù)不精確、不穩(wěn)定的提取特征點(diǎn)的問題，提出一種基于分割-合并的線段特征提取方法，其先采用IEPF 的方法判斷是不是分割點(diǎn)，然后通過分割的兩個(gè)線段之間的角度來判斷這個(gè)分割點(diǎn)是不是分割，這樣減少了線段的過度分割的問題。文獻(xiàn)[14]在構(gòu)建占用柵格地圖時(shí)，基于的Hough 進(jìn)行局部雷達(dá)進(jìn)行特征提取，構(gòu)建新的障礙物邊界或?qū)⑾惹疤崛〉倪吔绲难由欤摲椒ㄟm用于由線段組成的辦公室環(huán)境地圖的構(gòu)建。

針對(duì)特征地圖的構(gòu)建問題：首先建立激光雷達(dá)的數(shù)學(xué)模型，然后分析PDBS、IEPF 分割算法的優(yōu)缺點(diǎn)，提出了一種混合方法將PDBS 與IEPF 方法相結(jié)合，PDBS 和IEPF 的閾值設(shè)定通過激光雷達(dá)的系統(tǒng)誤差等進(jìn)行選擇，提高區(qū)域分割的性能，最后通過對(duì)激光雷達(dá)的一組數(shù)據(jù)進(jìn)行處理，比較特征提取與實(shí)際的環(huán)境特征的相似程度驗(yàn)證本論文所提混合方法的有效性。

2 激光雷達(dá)數(shù)據(jù)模型

激光雷達(dá)是SLAM 自主導(dǎo)航時(shí)構(gòu)建地圖及定位獲取數(shù)據(jù)的來源，為了便于數(shù)據(jù)的處理首先建立激光雷達(dá)的數(shù)據(jù)模型，通過坐標(biāo)的變換將傳感器所獲得的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)換為直角坐標(biāo)系，為了在區(qū)域分割時(shí)提取線段特征，采用線性最小二乘法進(jìn)行擬合。

2.1 激光雷達(dá)

激光雷達(dá)的原理是一種基于時(shí)間飛行原理的技術(shù)（Time of Flight，TOF），通過激光發(fā)射器發(fā)出脈沖波發(fā)出和接收時(shí)間計(jì)算出激光雷達(dá)到被測(cè)物體之間的距離s：

2.2 傳感器數(shù)據(jù)的模型

選用單線激光雷達(dá)LMS111-10100，激光雷達(dá)的具體參數(shù)，如表1 所示。

表1 LMS111-10100 參數(shù)Tab.1 LMS111-10100 Parameters

激光雷達(dá)的輸出是一系列的測(cè)得的距離的信息，每?jī)蓚€(gè)相鄰的光束之間的角度是角度分辨率，如圖1 所示。激光雷達(dá)的數(shù)據(jù)是按照逆時(shí)針排序從起始角度到終止角度，其中α 表示兩個(gè)相鄰激光束之間的角度，即激光雷達(dá)的角度分辨率，用αs表示測(cè)量開始的角度。在此研究中的角度選用激光雷達(dá)的分辨率選用0.5°，測(cè)量范圍為270°，測(cè)量起始角度為-45°，終止角度為225°，共采集到541 組數(shù)據(jù)。

圖1 傳感器測(cè)距模型Fig.1 Sensor Ranging Model

為了便于數(shù)據(jù)處理，每一個(gè)激光束的極坐標(biāo)系轉(zhuǎn)換為直角坐標(biāo)系，所測(cè)得的距離相對(duì)于傳感器的直角坐標(biāo)系分別用表示xi，yi求解方法為：

3 特征提取算法對(duì)比與改進(jìn)

對(duì)傳感器建立模型之后對(duì)數(shù)據(jù)進(jìn)行處理后，為了將掃描的線段形狀特征提取出來，首先將之前的數(shù)據(jù)進(jìn)行區(qū)域分割，區(qū)域分割的目的是找出角點(diǎn)和斷點(diǎn)，將測(cè)得的點(diǎn)分割為不同的區(qū)域，再擬合出特征線段，本節(jié)主要是通過分析PDBS、IEPF 這兩種常用的區(qū)域分割方法優(yōu)缺點(diǎn)，提出一種混合的算法。

3.1 PDBS 算法

PDBS（point distance based methods）線段分割的方法，是根據(jù)激光雷達(dá)兩個(gè)相鄰點(diǎn)之間在直角坐標(biāo)下的距離di與設(shè)定的閾值ε相比，如果兩個(gè)相鄰測(cè)量點(diǎn)之間的距離di大于閾值ε，則認(rèn)為兩個(gè)點(diǎn)屬于不同的區(qū)域。將激光雷達(dá)獲得的一組數(shù)據(jù)從第一個(gè)點(diǎn)依次計(jì)算兩個(gè)相鄰點(diǎn)之間的距離，通過與設(shè)定的閾值ε 相比較，將所測(cè)得的點(diǎn)分割為不同的區(qū)域后進(jìn)行最小二乘法擬合提取線段。

PDBS 方法中閾值ε 的設(shè)定對(duì)線段的分割具有重要的影響，閾值設(shè)定太小的時(shí)容易出現(xiàn)過分割的現(xiàn)象，閾值設(shè)定過大的時(shí)候又容易出現(xiàn)分割不足的現(xiàn)象，從激光雷達(dá)中獲得的連續(xù)的幾個(gè)測(cè)量值，如圖2（a）所示。設(shè)定的不同閾值分割相同的區(qū)域時(shí)的影響，如圖2（b）、圖2（c）所示。Pi與Pj之間距離的值大于閾值ε，區(qū)域被分為兩個(gè)區(qū)域，在擬合時(shí)擬合出兩個(gè)線段，如圖2（b）所示；由于距離的值小于閾值，區(qū)域未被分割，在擬合時(shí)，線段擬合為一條線段，如圖2（c）所示。這對(duì)于在構(gòu)建地圖時(shí)線段特征的數(shù)量有較大的影響。PDBS 算法在角點(diǎn)處的會(huì)出現(xiàn)分割失敗的情況。從現(xiàn)實(shí)環(huán)境墻角處所得出的數(shù)據(jù)，如圖3（a）所示；提取出的正確的線段，如圖3（b）所示；但是在利用PDBS 方法對(duì)這一區(qū)域進(jìn)行閾值分割時(shí)，由于并沒有提取出角點(diǎn)，會(huì)將此處的所有點(diǎn)劃分在一個(gè)區(qū)域內(nèi)，在對(duì)此區(qū)域進(jìn)行最小二乘法進(jìn)行數(shù)據(jù)擬合時(shí)，擬合出現(xiàn)錯(cuò)誤，如圖3（c）所示。

圖2 PDBS 過分割與欠分割問題Fig.2 PDBS Over Segmentation and Under Segmentation

圖3 PDBS 在角點(diǎn)處擬合失敗Fig.3 PDBS Fitting Failure at Corner

3.2 IEPF 方法

IEPF（iterative end point fit）的原理步驟如下：

（1）從傳感器提取出的數(shù)據(jù)，原始數(shù)據(jù)轉(zhuǎn)化為直角坐標(biāo)系，用黑色點(diǎn)從P1到Pn的坐標(biāo)點(diǎn)，如圖4（a）所示。

（2）將數(shù)據(jù)中的第一個(gè)點(diǎn)P1和最后一個(gè)點(diǎn)Pn連接形成一條直線L1，然后計(jì)算從P2點(diǎn)到倒數(shù)Pn-1點(diǎn)之間所有點(diǎn)到L1的距離，并找出距離最遠(yuǎn)的Pj點(diǎn)，與設(shè)定的閾值相比如果超過設(shè)定的閾值ε 相比，如果最遠(yuǎn)距離小于設(shè)定的閾值則認(rèn)為是一條直線，不再劃分；如果最遠(yuǎn)距離大于ε 再將所有的點(diǎn)分為兩部分從P1到Pj點(diǎn)及Pj+1點(diǎn)到Pn點(diǎn)兩部分，再按上面的步驟對(duì)以步驟操作，直到不能再劃分為止，如圖4（b）、圖4（c）所示。

圖4 IPEF 分割過程Fig.4 IPEF Algorithm Segmentation Process

IEPF 算法能夠在線段的交點(diǎn)處將線段的角點(diǎn)提取出來，避免PDBS 在兩個(gè)線段的交點(diǎn)處的特征提取錯(cuò)誤的問題。但是IEPF 方法無法判斷一個(gè)點(diǎn)是否是斷點(diǎn)，如圖5 所示。Pi與Pj是斷點(diǎn)，如圖5（a）所示；正確分割的情況，如圖5（b）所示。將區(qū)域分為兩個(gè)部分，在擬合時(shí)擬合出兩條線段；但是實(shí)際上IEPF 無法根據(jù)兩個(gè)相鄰點(diǎn)之間的距離判斷Pi與Pj是否是斷點(diǎn)，在擬合時(shí)仍被認(rèn)為是同一直線上的點(diǎn)，如圖5（c）所示。擬合出的是一條直線。

圖5 IEPF 分割問題Fig.5 IEPF Segmentation Problem

3.3 算法改進(jìn)

根據(jù)PDBS 方法及IEPF 方法的優(yōu)缺點(diǎn)提出了一種改進(jìn)的區(qū)域分割的方法：首先用PDBS 方法對(duì)區(qū)域進(jìn)行分割，然后利用IEPF 算出角點(diǎn)，用角點(diǎn)對(duì)數(shù)據(jù)再次分割，這樣就避免了PDBS 方法在擬合的時(shí)角點(diǎn)處擬合失敗的問題。研究選用SICK-LMS111-10100 激光雷達(dá)對(duì)空間進(jìn)行數(shù)據(jù)的采集。閾值的選擇，查閱機(jī)關(guān)傳感器的誤差的相關(guān)數(shù)據(jù)，如表2 所示。根據(jù)系統(tǒng)誤差和統(tǒng)計(jì)的誤差，同時(shí)在本實(shí)驗(yàn)中，選用的測(cè)量場(chǎng)景為10m 以內(nèi)的測(cè)量范圍，在這里選用PDBS 的閾值選用閾值為35mm。同時(shí)IEPF 的閾值設(shè)定為35mm。為了便于數(shù)據(jù)處理，首先定義重要的數(shù)據(jù)，如表3 所示。

表2 傳感器的誤差Tab.2 Sensor Errors

表3 參數(shù)定義Tab.3 Definition of Parameters

具體步驟如下：

（1）將原始數(shù)據(jù)的極坐標(biāo)轉(zhuǎn)變?yōu)橹苯亲鴺?biāo)系下，極坐標(biāo)中的第i 個(gè)測(cè)量點(diǎn)的極坐標(biāo)的值為：

（3）兩個(gè)點(diǎn)之間的最小的距離與thd_pdbs 之間的關(guān)系，dis_pdbs 大于thd_pdbs 時(shí)，將斷點(diǎn)提取出來，存放到數(shù)組con 中；如果dis_pdbs 小于thd_pdbs 時(shí)，繼續(xù)2、3 步，直到所有的斷點(diǎn)都提取出來；

（4）如果con 數(shù)組中有數(shù)據(jù)與前后數(shù)據(jù)在的下標(biāo)相差一個(gè)時(shí)，認(rèn)為這是一個(gè)噪聲點(diǎn)，刪除；

（5）將擬合直線：將x_axis、y_axis 相鄰下標(biāo)分別為div_sub（i_con）和div_sub（i_con+1）的兩個(gè)點(diǎn)擬合成直線，在此處，在步驟3 處的噪音點(diǎn)所在區(qū)域不做直線擬合，計(jì)算出k、b；

（6）依次計(jì)算點(diǎn)到直線之間的距離，求出最大的值dis_max_iepf，并記錄他的下標(biāo)值i_con；

（7）計(jì)算dis_max_iepf 與thd_iepf 的關(guān)系：當(dāng)dis_max_iepf的值大于thd_iepf 時(shí)，將dis_max_iepf 的下標(biāo)i_con 存入數(shù)組div_sub，并將數(shù)組重新排序；跳到步驟5；當(dāng)dis_max_iepf 的值小于thd_iepf 時(shí)，記錄下這個(gè)區(qū)間，后期檢測(cè)直接跳過這個(gè)區(qū)間；

（8）當(dāng)div_sub（i_con）的值超過541 時(shí)停止計(jì)算。

4 仿真實(shí)驗(yàn)

在實(shí)際環(huán)境中搭建一個(gè)由線段的組成的環(huán)境，并通過激光雷達(dá)的一組數(shù)據(jù)保存為文本文檔，通過MATLAB 對(duì)原始的數(shù)據(jù)進(jìn)行分析。

現(xiàn)實(shí)環(huán)境，在走廊的放置桌子，通過激光雷達(dá)收集現(xiàn)場(chǎng)的數(shù)據(jù)，如圖6 所示。

激光雷達(dá)的一次掃描541 個(gè)點(diǎn)，通過串口助手將數(shù)據(jù)讀出，并將其存為文本文件，以便于后期的分析，將原始的數(shù)據(jù)轉(zhuǎn)換為直角坐標(biāo)系并在MATLAB 中的圖像，如圖7 所示。對(duì)原始的數(shù)據(jù)利用PDBS 方法處理，在使用PDBS 方法擬合的時(shí)候在兩個(gè)線段的交點(diǎn)處，算法中無法將兩個(gè)線段分開擬合，造成擬合出的直線與實(shí)際的環(huán)境相差很大，如圖8 所示。對(duì)原始圖像的數(shù)據(jù)處理利用IEPF 方法提取，通過對(duì)兩個(gè)線段之間的交點(diǎn)進(jìn)行處理，線段的擬合效果提高，但是IEPF 在擬合中，存在噪音或者出現(xiàn)斷點(diǎn)時(shí)，會(huì)出現(xiàn)擬合的直線與實(shí)際環(huán)境相差較大，如圖9 所示。

圖7 原始數(shù)據(jù)圖像Fig.7 Original Data Image

圖8 PDBS 方法特征提取Fig.8 PDBS Algorithm Feature Extraction

圖9 IEPF 方法特征提取Fig.9 IEPF Algorithm Feature Extraction

對(duì)原始的數(shù)據(jù)進(jìn)行數(shù)據(jù)處理利用PDBS 與IEPF 方法進(jìn)行處理，在數(shù)據(jù)的處理過程中，首先通過PDBS 提取出斷點(diǎn)，然后再通過IEPF 提取出角點(diǎn)，擬合出的線段較單獨(dú)使用PDBS、IEPF 線段擬效果明顯，驗(yàn)證了算法的有效性，如圖10 所示。

圖10 PDBS 與IEPF 結(jié)合特征提取Fig.10 PDBS Algorithm Combined with IEPF Algorithm for Feature Extraction

5 結(jié)論

針對(duì)基于激光SLAM 自主導(dǎo)航建圖時(shí)特征線段提取方法進(jìn)行研究，通過對(duì)PDBS 和IEPF 線段特征提取的方法進(jìn)行比較，對(duì)PDBS 對(duì)兩個(gè)線段的交點(diǎn)處的擬合失效的問題進(jìn)行分析，同是結(jié)合IEPF 的方法在斷點(diǎn)處的擬合失敗問題，將兩個(gè)算法進(jìn)行互補(bǔ)，首先利用PDBS 方法找出斷點(diǎn)，然后再利用IEPF 方法找出角點(diǎn)，最后將數(shù)據(jù)擬合，對(duì)比單獨(dú)使用PDBS 和IEPF 的方法，混合特征線段的提取與實(shí)際環(huán)境更接進(jìn)，驗(yàn)證了混合算法的有效性。