融合波前邊緣檢測(cè)與快速搜索隨機(jī)樹的自主探索方法

張淑珍，馬玉祥，侯致遠(yuǎn)，查富生，何 鎮(zhèn)

（1.蘭州理工大學(xué) 機(jī)電工程學(xué)院，蘭州 730000；2.哈爾濱工業(yè)大學(xué) 機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，哈爾濱 150001）

移動(dòng)機(jī)器人已廣泛應(yīng)用于工業(yè)、農(nóng)業(yè)和軍事等領(lǐng)域。從搜索救援到行星探測(cè)[1]，未知環(huán)境下移動(dòng)機(jī)器人自主探索成為機(jī)器人領(lǐng)域的研究熱點(diǎn)之一[2]。如何在時(shí)間短及路程少的條件下通過感知環(huán)境決定最佳探索目標(biāo)點(diǎn)并移動(dòng)至該點(diǎn)，進(jìn)而完成移動(dòng)機(jī)器人所在未知環(huán)境的遍歷，得到足夠的信息用于全局地圖創(chuàng)建[3]，是自主探索及地圖創(chuàng)建問題的核心，也是機(jī)器人實(shí)現(xiàn)真正自主，智能化的基礎(chǔ)性技術(shù)。

Umari H.[4]提出將快速搜索隨機(jī)樹（Rapid Random Tree,RRT）用于邊界檢測(cè)，并設(shè)計(jì)出局部RRT 前沿點(diǎn)檢測(cè)器與全局RRT 前沿點(diǎn)檢測(cè)器混合的搜索策略。金毅康[5]在傳統(tǒng)RRT 快速搜索隨機(jī)樹算法上，以其生長(zhǎng)的端點(diǎn)作為廣度優(yōu)先搜索（Breadth First Search,BFS）的起點(diǎn)搜索邊界點(diǎn)，從而提高邊界的獲取效率。Sun Xuehao[6]提出一種利用RRT 算法搜索邊界點(diǎn)，以距離代價(jià)、障礙物代價(jià)為判斷依據(jù)的性能指標(biāo)函數(shù)，完成室外地面環(huán)境探索。RRT 算法偏向于覆蓋未知區(qū)域，雖然能夠?qū)崿F(xiàn)探索邊界檢測(cè)的功能，但由于其生長(zhǎng)具有隨機(jī)性，狹窄區(qū)域通過性較差，存在著邊界點(diǎn)提取不全面的問題，同時(shí)在提取邊界點(diǎn)或是樹枝生長(zhǎng)過程中，需不斷檢查樹枝與障礙物及邊界之間的關(guān)系，一定程度上降低了邊界點(diǎn)提取的效率。Keidar M.[7]提出了兩種快速檢測(cè)邊界的方法，分別為快速前邊緣檢測(cè)（Fast Frontier Detector,FFD）與波前邊緣檢測(cè)（Wavefront Frontier Detector,WFD）算法，其中FFD方法只處理實(shí)時(shí)接收到的新激光數(shù)據(jù)，進(jìn)行探索邊界的提取，同樣Sun Y.X.[8]等將柵格地圖與激光數(shù)據(jù)結(jié)合起來探索前沿區(qū)域，這種依賴于激光雷達(dá)數(shù)據(jù)的算法對(duì)傳感器硬件的精度有較高的要求，這在一定程度上限制了應(yīng)用范圍，并且由于激光點(diǎn)在極端范圍內(nèi)發(fā)散，此類算法提取激光數(shù)據(jù)構(gòu)成的邊界輪廓時(shí)將跨越未知區(qū)域，難以覆蓋應(yīng)被檢測(cè)為邊界的所有單元格。雖然Phillip[9]針對(duì)上述問題提出邊界跟蹤式邊界檢測(cè)（Frontier-Tracing Frontier Detection,FTFD）算法使用前一時(shí)刻邊界和傳感器包絡(luò)的端點(diǎn)作為當(dāng)前時(shí)刻尋找新邊界的起點(diǎn)，提高了檢測(cè)邊界的準(zhǔn)確性及完整性，但適用范圍還是受限于激光雷達(dá)的精度。WFD 方法則是只處理已經(jīng)探索過的柵格地圖進(jìn)行邊界的提取，在建圖前期具有較快的收斂速度，但隨著地圖的完備，需要遍歷的柵格數(shù)量變得龐大，使得邊界提取速度不斷上升，針對(duì)該問題Phillip[9]提出的擴(kuò)展波前邊緣檢測(cè)（Expanding-Wavefront Frontier Detector,EWFD）算法在WFD 算法的基礎(chǔ)上對(duì)提取的邊界加入時(shí)間標(biāo)簽，使用前一個(gè)時(shí)間步長(zhǎng)的邊界作為起點(diǎn)，在未知環(huán)境中搜索邊界，減少了遍歷地圖柵格的數(shù)量，但其運(yùn)行時(shí)間仍與已知區(qū)域大小成正比，且因時(shí)間標(biāo)簽的加入而耗費(fèi)大量的內(nèi)存空間。

本文針對(duì)已有探索候選點(diǎn)算法提取耗時(shí)久、不全面，探索目標(biāo)點(diǎn)引導(dǎo)機(jī)器人大角度轉(zhuǎn)向等問題，對(duì)傳統(tǒng)WFD 算法進(jìn)行改進(jìn)并與快速搜索隨機(jī)樹方法結(jié)合，分別作為局部候選點(diǎn)與全局候選點(diǎn)的提取方法。同時(shí)將螢火蟲算法輕簡(jiǎn)化后，用于提取眾多探索候選點(diǎn)中的關(guān)鍵點(diǎn)，并在傳統(tǒng)評(píng)價(jià)函數(shù)中引入角度代價(jià)，提出一種基于改進(jìn)的WFD 與RRT 融合的自主探索方法。

1 自主探索及地圖構(gòu)建系統(tǒng)框架

未知環(huán)境下自主探索及地圖構(gòu)建主要包括探索目標(biāo)點(diǎn)的提取和地圖實(shí)時(shí)構(gòu)建。自主探索算法提取探索目標(biāo)點(diǎn)傳遞給定位導(dǎo)航算法，確定運(yùn)動(dòng)路線，采用SLAM 算法獲取感知傳感器的數(shù)據(jù)構(gòu)建局部柵格地圖，隨著探索目標(biāo)點(diǎn)的不斷生成，引導(dǎo)機(jī)器人遍歷環(huán)境，最終實(shí)現(xiàn)未知環(huán)境的全局地圖構(gòu)建。自主探索及地圖構(gòu)建系統(tǒng)如圖1 所示。

圖1 自主探索及地圖構(gòu)建系統(tǒng)框圖Fig.1 System diagram of autonomous exploration and mapping

本系統(tǒng)利用ROS 平臺(tái)[10]編寫提取探索目標(biāo)點(diǎn)模塊中的相關(guān)節(jié)點(diǎn)，即改進(jìn)的WFD 局部候選點(diǎn)提取、RRT 全局候選點(diǎn)提取、輕量化螢火蟲聚類、改進(jìn)的評(píng)價(jià)函數(shù)等?；贕Mapping 算法接收激光雷達(dá)數(shù)據(jù)，用以構(gòu)建二維柵格地圖，探索過程中利用ROS 集成的move_base 功能包獲取里程計(jì)信息和探索目標(biāo)點(diǎn)，繼而進(jìn)行路徑規(guī)劃[11]，不斷迭代直至探索完成。

2 探索候選點(diǎn)提取

2.1 基于改進(jìn)波前邊緣檢測(cè)算法的探索候選點(diǎn)提取

Keidar M.[7]提出的傳統(tǒng)WFD（Wavefront Frontier Detector）算法只遍歷已探索的地圖，以機(jī)器人當(dāng)前的位置為初始柵格利用廣度優(yōu)先理論提取邊界點(diǎn)，通過檢測(cè)邊界點(diǎn)周邊相鄰單元格，快速提取邊界。該算法中存在三個(gè)隊(duì)列與一個(gè)集合，分別為待檢測(cè)隊(duì)列Qc、邊界點(diǎn)隊(duì)列Qp、邊界隊(duì)列Qf、邊界集合Sf{Qf1,Qf2,…,Qfn}，算法運(yùn)行時(shí)將初始柵格Xr放入Qc中，搜索Qc中柵格的四鄰域，若不存在未知柵格，表示該柵格不是邊界點(diǎn)，將其四鄰域放入Qc中，不斷迭代，直至發(fā)現(xiàn)四鄰域存在未知柵格的邊界點(diǎn)，并將其放入Qp中，進(jìn)而搜索Qp中邊界點(diǎn)的鄰域，將連續(xù)的邊界點(diǎn)存入Qf中，構(gòu)成一條邊界，添加至Sf中。在建圖初始階段該算法確有較高的提取效率，但隨已探索區(qū)域擴(kuò)大，Qc中儲(chǔ)存的元素增多，算法收斂時(shí)間不斷增加，且WFD 算法提取邊界時(shí)，需要為邊界點(diǎn)附上標(biāo)簽以避免重復(fù)搜索同一邊界點(diǎn)，因此加劇了內(nèi)存占用量，對(duì)于未知不確定區(qū)域范圍的搜索空間，如何在搜索區(qū)域擴(kuò)大的情況下，保持較高的邊界點(diǎn)提取效率，是提高探索效率的前提。針對(duì)該問題，本文對(duì)傳統(tǒng)WFD 算法進(jìn)行改進(jìn)，通過限制搜索范圍并對(duì)邊界點(diǎn)歸類進(jìn)行簡(jiǎn)化操作以提高算法提取效率，將改進(jìn)方法稱為WFD_L（Wavefront Frontier Detector Limit），改進(jìn)思想如下：

(1)在Qc隊(duì)列元素篩選條件中加入距離條件。當(dāng)搜索Qc中柵格的四鄰域不存在未知柵格，且滿足與初始柵格的歐式距離小于搜索閾值時(shí)，再將四鄰域柵格加入Qc中，以減少儲(chǔ)存元素的數(shù)量。圖2 為距離限制條件示意圖，圖中F、B 為WFD 算法搜索邊界點(diǎn)時(shí)所要遍歷的柵格，R 為當(dāng)前機(jī)器人位置即初始柵格，ε為搜索閾值，可以看出B 的四鄰域柵格B1、B2、B3、B4的柵格狀態(tài)皆為空閑，且，可將B1、B2、B3、B4存入Qc中，而F 的四鄰域柵格雖然都不是未知柵格，但與初始柵格的距離大于ε，因此將F1、F2、F3、F4不存入cQ中。該策略使得WFD_L 算法在已探索區(qū)域增大的情況下仍然保持較低的收斂時(shí)間，提高邊界點(diǎn)提取效率。

(2)去除搜索Qp中元素鄰域，將連續(xù)的邊界點(diǎn)存入Qfn中，并添加至Sf集合的步驟，減少算法的計(jì)算量，降低主控單元計(jì)算負(fù)荷。

基于WFD_L 算法的候選點(diǎn)提取步驟如下：

步驟1：以機(jī)器人當(dāng)前位置為初始柵格，利用四鄰域法判斷該柵格周圍柵格是否存在未知柵格，若不存在未知柵格則將其四鄰域放入待處理隊(duì)列中。

步驟2：若待處理隊(duì)列中存在數(shù)據(jù)，則檢測(cè)其中每一數(shù)據(jù)的四鄰域是否存在未知柵格，若存在則將其對(duì)應(yīng)的數(shù)據(jù)移動(dòng)到邊界點(diǎn)隊(duì)列，若不存在未知柵格，進(jìn)行步驟3。

步驟3：判斷四鄰域柵格與機(jī)器人當(dāng)前位置的距離，當(dāng)小于一定閾值時(shí)，放入待處理隊(duì)列中。

步驟4：重復(fù)步驟2 與步驟3 直至待處理隊(duì)列中無元素存在，則表明機(jī)器人當(dāng)前位置處的邊界點(diǎn)提取結(jié)束，返回邊界點(diǎn)隊(duì)列中的所有邊界點(diǎn)。

如圖3 所示為仿真環(huán)境下WFD 算法與WFD_L算法在探索地圖時(shí)提取邊界點(diǎn)所耗費(fèi)時(shí)間對(duì)比，從圖中可以看出，WFD 算法迭代一次的時(shí)間隨著已知地圖的不斷擴(kuò)大而持續(xù)增加，探索環(huán)境越大，則整體搜索效率隨著WFD 算法提取時(shí)間的上升而降低。而WFD_L 在提取邊界點(diǎn)時(shí)，通過距離條件改變柵格進(jìn)入待處理隊(duì)列的規(guī)則使得遍歷范圍確定，將時(shí)間固定在2 ms 左右，不隨已知柵格的增多而上升。

圖3 WFD 與WFD_L 提取邊界點(diǎn)消耗時(shí)間Fig.3 Time consuming of extracting frontier points by WFD with WFD_L

2.2 引入全局視角的探索候選點(diǎn)提取

基于WFD_L 算法提取局部候選點(diǎn)具有較高的效率，但未考慮全局最優(yōu)，因此需融合一種具有全局視角的邊界點(diǎn)提取算法，避免機(jī)器人陷入局部陷阱。本文將搜索全局性、隨機(jī)性較好的隨機(jī)生成樹數(shù)據(jù)結(jié)構(gòu)（RRT）[12]用于全局候選點(diǎn)提取。

融合算法提取候選點(diǎn)的步驟如下：

步驟1：初始化RRT 樹的根節(jié)點(diǎn)即機(jī)器人初始位置，使RRT 樹枝生長(zhǎng)，同時(shí)啟用WFD_L 算法獲取局部候選點(diǎn)，最大化維持探索候選點(diǎn)提取的效率。

步驟2：若局部候選點(diǎn)數(shù)量大于某一閾值時(shí)，將其數(shù)據(jù)信息傳遞給聚類模塊進(jìn)行處理，反之將RRT 生成的全局候選點(diǎn)信息傳遞給聚類模塊。

步驟3：若全局候選點(diǎn)與局部候選點(diǎn)不存在或小于某一閾值時(shí)表示地圖探索完成，停止自主探索并保存地圖。

3 最佳探索目標(biāo)點(diǎn)選取

傳統(tǒng)最佳探索目標(biāo)點(diǎn)提取，對(duì)所有邊界點(diǎn)進(jìn)行評(píng)價(jià)函數(shù)處理，得到指標(biāo)最優(yōu)的邊界點(diǎn)作為探索目標(biāo)點(diǎn)，增加了主控單元的計(jì)算負(fù)荷與算法收斂時(shí)間。為減少計(jì)算量，本文先對(duì)探索候選點(diǎn)進(jìn)行聚類，再輸入評(píng)價(jià)模塊中，選出最佳探索目標(biāo)點(diǎn)。

3.1 基于輕簡(jiǎn)化螢火蟲算法的候選點(diǎn)聚類

常用的聚類算法有K 均值聚類、均值聚類及螢火蟲算法等。K 均值聚類需要提前知道類別數(shù)量，均值聚類則是尋找每一部分圓形滑動(dòng)窗口中密度最大的中心點(diǎn)，無法適用于線性分布居多且無種類之分的邊界點(diǎn)。螢火蟲算法[13]通過自定的目標(biāo)函數(shù)決定自身亮度，在邊界點(diǎn)聚類中可以將信息增益與路徑代價(jià)作為評(píng)價(jià)標(biāo)準(zhǔn)，更好地剔除每一區(qū)域較差的探索候選點(diǎn)，適用于線性分布較多的探索候選點(diǎn)聚類。但該算法無法保證自主探索的效率，針對(duì)該問題本文將螢火蟲算法嵌入自主探索系統(tǒng)，對(duì)大量探索候選點(diǎn)進(jìn)行實(shí)時(shí)聚類時(shí)，建立目標(biāo)函數(shù)并進(jìn)行輕簡(jiǎn)化處理，輕簡(jiǎn)化思想如下：

(1)將邊界點(diǎn)的信息增益與路徑代價(jià)綜合考慮，構(gòu)建螢火蟲算法的目標(biāo)函數(shù)即自身光強(qiáng)I0如式(1)，其中0r表示當(dāng)前候選點(diǎn)與機(jī)器人的距離，作為指數(shù)能夠避免路徑代價(jià)無限制地增大，γ為吸收系數(shù)，S 表示當(dāng)前候選點(diǎn)的信息增益，其計(jì)算時(shí)以邊界點(diǎn)為圓心，傳感器測(cè)量距離為半徑，統(tǒng)計(jì)圓內(nèi)的所有未知柵格的數(shù)量。

(2)將輕簡(jiǎn)化螢火蟲算法的迭代次數(shù)設(shè)定為1，并保持感知范圍內(nèi)最亮螢火蟲的位置不變，避免多次迭代導(dǎo)致探索效率下降，得到局部最優(yōu)個(gè)體后，將剩余邊界點(diǎn)剔除，達(dá)到釋放緩存的同時(shí)，降低評(píng)價(jià)模塊的計(jì)算負(fù)荷。

輕簡(jiǎn)化螢火蟲算法的運(yùn)行步驟如下：

步驟1：計(jì)算群體中每一螢火蟲（候選點(diǎn)）的自身光強(qiáng)I0如式(1)。

步驟2：計(jì)算影響范圍內(nèi)相比較的兩個(gè)螢火蟲之間的相對(duì)光強(qiáng)Ir，如式(2)，通過判斷Ir的大小殺死二者中相對(duì)光強(qiáng)較小的螢火蟲。

其中r表示相比較螢火蟲之間的距離。

步驟3：重復(fù)步驟2 遍歷搜索螢火蟲，最后返回存活的螢火蟲，作為聚類后的局部最優(yōu)解。

如圖4(a)所示紅色邊界點(diǎn)是未經(jīng)過輕量化螢火蟲算法聚類的邊界點(diǎn)，數(shù)量密集龐大，對(duì)評(píng)價(jià)函數(shù)的計(jì)算造成較大的負(fù)荷，而圖4(b)中黃色的邊界點(diǎn)是經(jīng)過輕量化螢火蟲算法處理后的結(jié)果，在每一局部區(qū)域剔除影響因子較小的邊界點(diǎn)，保留相對(duì)最優(yōu)的邊界點(diǎn)，在保證邊界信息完整性前提下，減少了控制單元不必要的計(jì)算量，提升最佳目標(biāo)點(diǎn)的選取效率。

圖4 邊界點(diǎn)聚類前后對(duì)比Fig.4 Comparison of before and after frontier point clustering

3.2 具有角度代價(jià)的評(píng)價(jià)函數(shù)

評(píng)價(jià)函數(shù)通過計(jì)算每一探索候選點(diǎn)的探索收益，選擇最優(yōu)值，確定最佳探索目標(biāo)點(diǎn)。已有評(píng)價(jià)函數(shù)只包含信息增益與路徑代價(jià)，可以選出未知鄰域大且需行程少的探索點(diǎn)，但該點(diǎn)引導(dǎo)的運(yùn)動(dòng)趨勢(shì)可能與機(jī)器人當(dāng)前姿態(tài)呈現(xiàn)較大夾角，致使機(jī)器人導(dǎo)航過程中頻繁旋轉(zhuǎn)，加劇導(dǎo)航成本，因此本文引入角度代價(jià)，即以機(jī)器人坐標(biāo)系為基準(zhǔn)計(jì)算最佳目標(biāo)點(diǎn)與機(jī)器人位置連線的傾角，并與機(jī)器人當(dāng)前姿態(tài)做差值，解決機(jī)器人因大曲率轉(zhuǎn)向?qū)е绿剿鞯托У膯栴}，本文稱之為評(píng)價(jià)函數(shù)_IDA（Information Distance Angular）。計(jì)算步驟如下：

步驟1：獲取機(jī)器人當(dāng)前世界坐標(biāo)系下的姿態(tài)，并計(jì)算機(jī)器人當(dāng)前位置點(diǎn)與被評(píng)價(jià)候選點(diǎn)連線的傾斜角。

步驟2：分類討論傾斜角與機(jī)器人姿態(tài)之間的關(guān)系，確定出候選點(diǎn)與機(jī)器人姿態(tài)的角度差值。

如圖5 所示，直角坐標(biāo)系O-xOy為機(jī)器人坐標(biāo)系，P1,P2,P3,P4為各個(gè)象限中的任意候選點(diǎn)。處于機(jī)器人坐標(biāo)系中第二、三象限的P2、P3點(diǎn)不能只通過與原點(diǎn)連線的斜率來計(jì)算相對(duì)機(jī)器人的偏轉(zhuǎn)角，需采用其補(bǔ)角作為相對(duì)機(jī)器人的偏轉(zhuǎn)角，因此將機(jī)器人姿態(tài)與候選點(diǎn)間的偏轉(zhuǎn)角關(guān)系進(jìn)行分類討論。角度代價(jià)的計(jì)算公式為：

圖5 目標(biāo)點(diǎn)相對(duì)機(jī)器人坐標(biāo)系位置關(guān)系Fig.5 Position relationship of target points relative to the robot coordinate system

(a)當(dāng)候選點(diǎn)處于坐標(biāo)系的第一與第四象限時(shí)δ=0 ；(b)當(dāng)候選點(diǎn)處于第二象限時(shí)δ=1 ；(c)當(dāng)候選點(diǎn)處于第三象限時(shí)δ=-1，其中(xi,yi)為候選點(diǎn)坐標(biāo)，(xr,yr)為機(jī)器人坐標(biāo)，yaw為只考慮機(jī)器人繞z軸的姿態(tài)信息，其角度范圍為(0，π)與(0，-π)。

步驟3：對(duì)上述三種因素采用適宜的權(quán)重進(jìn)行線性組合完成評(píng)價(jià)函數(shù)_IDA 的設(shè)計(jì)，評(píng)價(jià)函數(shù)計(jì)算公式為：

其中R為探索收益，μ,ε,τ為權(quán)重值，I為信息增益，d為距離代價(jià)，θ為角度代價(jià)。

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

4.1 最佳目標(biāo)點(diǎn)選取實(shí)驗(yàn)

為驗(yàn)證角度代價(jià)對(duì)提高機(jī)器人探索效率的效果，在40 m×40 m 大小的仿真環(huán)境下進(jìn)行了兩組實(shí)驗(yàn)，其中候選點(diǎn)提取方法統(tǒng)一為傳統(tǒng)WFD 算法，目標(biāo)點(diǎn)選取函數(shù)分別為傳統(tǒng)評(píng)價(jià)函數(shù)與評(píng)價(jià)函數(shù)_IDA。

雖然傳統(tǒng)評(píng)價(jià)函數(shù)能夠在探索過程中不斷產(chǎn)生目標(biāo)點(diǎn)，直至結(jié)束，但由于該函數(shù)基于貪婪策略，選取每一目標(biāo)點(diǎn)都是朝向未知區(qū)域最多，相對(duì)距離較近的邊界點(diǎn)，而沒有考慮機(jī)器人當(dāng)前姿態(tài)，所以導(dǎo)致機(jī)器人在探索過程中多次大角度轉(zhuǎn)向，即機(jī)器人轉(zhuǎn)向目標(biāo)點(diǎn)方位的角度大于π/2，從而降低探索效率，如圖6(b)黃色框線內(nèi)就是機(jī)器人大角度轉(zhuǎn)向的運(yùn)動(dòng)行為。而評(píng)價(jià)函數(shù)_IDA 在考慮信息增益與路徑代價(jià)的基礎(chǔ)上選取目標(biāo)點(diǎn)，控制機(jī)器人轉(zhuǎn)向角度，減少機(jī)器人大角度轉(zhuǎn)向，如圖6(a)顯示機(jī)器人的運(yùn)動(dòng)軌跡較為平滑，基本沒有大角度轉(zhuǎn)向運(yùn)動(dòng)行為，從而提高機(jī)器人在未知環(huán)境的探索效率。

圖6 仿真實(shí)驗(yàn)Fig.6 Simulation experiments

從圖7 可以看出，經(jīng)過傳統(tǒng)評(píng)價(jià)函數(shù)提取后，有50%的目標(biāo)點(diǎn)引導(dǎo)機(jī)器人進(jìn)行大角度轉(zhuǎn)向；而經(jīng)過評(píng)價(jià)函數(shù)_IDA 提取后，機(jī)器人大角度轉(zhuǎn)向的概率為10.00%，相比于傳統(tǒng)評(píng)價(jià)函數(shù)降低了38.89%。

圖7 機(jī)器人轉(zhuǎn)向角分布圖Fig.7 Distribution of robot steering angle

為了消除實(shí)驗(yàn)結(jié)果的偶然性，本文基于兩種評(píng)價(jià)函數(shù)在上述仿真環(huán)境下分別進(jìn)行10 次目標(biāo)點(diǎn)選取實(shí)驗(yàn)，從轉(zhuǎn)向角、時(shí)間、路徑三方面比對(duì)評(píng)價(jià)函數(shù)_IDA與傳統(tǒng)評(píng)價(jià)函數(shù)的優(yōu)劣，并計(jì)算對(duì)應(yīng)平均值。如圖8(a)(b)為機(jī)器人大角度轉(zhuǎn)向占比與轉(zhuǎn)向角數(shù)據(jù)對(duì)比圖，圖8(c)(d)為自主探索過程中耗費(fèi)時(shí)間與行進(jìn)路程對(duì)比圖。

圖8 仿真結(jié)果對(duì)比Fig.8 Comparison of simulation results

從圖8(a)(b)可看出，傳統(tǒng)評(píng)價(jià)函數(shù)選取的目標(biāo)點(diǎn)引導(dǎo)機(jī)器人轉(zhuǎn)向的平均角度為2.142 rad，大角度轉(zhuǎn)向占比為56.18%，評(píng)價(jià)函數(shù)_IDA 選取的目標(biāo)點(diǎn)引導(dǎo)機(jī)器人轉(zhuǎn)向平均角度為1.044 rad，相比于傳統(tǒng)評(píng)價(jià)函數(shù)減少了1.098 rad，大角度轉(zhuǎn)向占比為19.29%，相比于傳統(tǒng)評(píng)價(jià)函數(shù)降低了36.89%。從圖8(c)(d)可看出，機(jī)器人考慮角度代價(jià)后，探索時(shí)間節(jié)省了42.30%，路徑長(zhǎng)度縮減了9.95%。綜上所述，證明了加入角度代價(jià)的評(píng)價(jià)函數(shù)（評(píng)價(jià)函數(shù)_IDA）對(duì)于提升地圖創(chuàng)建效率的有效性。

4.2 自主探索實(shí)驗(yàn)

為驗(yàn)證本文提出的自主探索算法的整體性能，且剔除結(jié)果的偶然性，在如圖9(a)面積約為50 m2的真實(shí)環(huán)境下，使用如圖9(b)控制單元為Jetson nano，搭配有鐳神N10 激光雷達(dá)的樣機(jī)進(jìn)行了5 次實(shí)驗(yàn)。該實(shí)驗(yàn)將Keidar M.[7]提出的WFD 算法、金毅康[5]提出的RRT-BFS 算法與本文提出的WFD-RRT 算法從探索時(shí)間、路徑長(zhǎng)度兩方面進(jìn)行比對(duì)分析。

圖10(a)為機(jī)器人在初始位置探索到的柵格地圖，圖10(b)為探索過程中生成的柵格地圖，圖10(c)為最終建圖的結(jié)果，可以看出該算法能夠引導(dǎo)機(jī)器人在未知環(huán)境中建立完整的地圖，為防止劇烈碰撞，將機(jī)器人最大速度限制為0.1 m/s。從表1 可看出，WFD-RRT算法相較于WFD 算法，探索路徑長(zhǎng)度增加約2.95%，探索消耗時(shí)間降低約38.24%，這表明WFD-RRT 算法沒有陷入局部陷阱而往復(fù)徘徊，反而通過降低候選點(diǎn)提取的計(jì)算量，減少地圖創(chuàng)建時(shí)間，提高了自主探索效率；WFD-RRT 算法相較于RRT-BFS 算法，探索路徑長(zhǎng)度減少約32.13%，探索消耗時(shí)間降低約22.19%，總體來看，WFD-RRT 算法能夠較明顯地提升未知環(huán)境自主探索效率，在探索候選點(diǎn)提取、最佳目標(biāo)點(diǎn)選擇方面都有一定的優(yōu)勢(shì)，驗(yàn)證了WFD-RRT 算法的可行性與有效性。

表1 樣機(jī)實(shí)驗(yàn)結(jié)果對(duì)比Tab.1 Comparison of prototype experimental results

圖10 自主探索過程Fig.10 Process of autonomous exploration

5 結(jié)論

本文以提高自主探索效率為目標(biāo)，針對(duì)候選點(diǎn)提取與聚類、目標(biāo)點(diǎn)選取，提出一種基于WFD_L 與RRT融合的自主探索方法。通過仿真與實(shí)驗(yàn)測(cè)試結(jié)果，得出以下結(jié)論：

(1)提出一種考慮方向的評(píng)價(jià)函數(shù)，仿真實(shí)驗(yàn)得出評(píng)價(jià)函數(shù)_IDA 相比于傳統(tǒng)評(píng)價(jià)函數(shù)選取的探索目標(biāo)點(diǎn)，引導(dǎo)機(jī)器人大角度轉(zhuǎn)向的概率下降36.89%，探索路徑與探索耗時(shí)分別減少9.95%、42.30%。表明引入角度代價(jià)的評(píng)價(jià)函數(shù)_IDA 相比于傳統(tǒng)評(píng)價(jià)函數(shù)能夠提取出更高效的探索目標(biāo)點(diǎn)。

(2)提出的WFD-RRT 算法，通過限制WFD 算法的搜索范圍，并在傳統(tǒng)評(píng)價(jià)函數(shù)中引入角度代價(jià)，使得探索候選點(diǎn)提取時(shí)間縮短、目標(biāo)點(diǎn)選取更有效。在實(shí)驗(yàn)環(huán)境中，該算法的探索路徑及探索耗時(shí)相比于RRT-BFS 算法分別降低了32.13%、22.19%，表明WFD-RRT 算法具有更加高效的探索效果。