周飛龍，甘 屹

（上海理工大學(xué)機(jī)械工程學(xué)院，上海 200093）

0 引言

相對(duì)于其他算法，RRT 算法因其在多約束、高維路徑規(guī)劃問題中具有概率完備性和漸近最優(yōu)性［13］而越來(lái)越受歡迎。針對(duì)RRT 算法收斂速度較慢的問題，Urmson 等［14］更改節(jié)點(diǎn)采樣方式，提出具有節(jié)點(diǎn)偏向目標(biāo)概率的hRRT（Heuristically RRT）算法；Wang［15］從多隨機(jī)樹方面提出雙向隨機(jī)樹；劉成菊等［16］提出基于RRT 的變步長(zhǎng)搜索以提高搜索效率。

路徑規(guī)劃算法的目的是找到可達(dá)路徑，然而一些基于RRT 的算法在狹窄復(fù)雜環(huán)境中并不能求解出路徑。這是由于在狹窄空間中選取節(jié)點(diǎn)是困難的，狹窄通道體積小，任何基于容量的抽樣分布都可能失敗［17］。Rodriguez 等［18］從多向隨機(jī)樹角度提出了在OB-RRT（Obstacle Based RRT）中通過狹窄通道區(qū)域建立根節(jié)點(diǎn)，讓隨機(jī)樹更快地尋找到可行路徑，但需要預(yù)先確定狹窄空間位置；Informed-RRT*與RABIT*通過局部?jī)?yōu)化器，提高路徑中的狹窄區(qū)域采樣概率，但是其效率有待提高［19-20］；文獻(xiàn)［13］提供一種橋梁檢測(cè)算法提高對(duì)狹窄空間的采樣概率，但會(huì)在有凹三角形輪廓障礙物附近產(chǎn)生多余采樣節(jié)點(diǎn)，增加了算法時(shí)間。

本文提出一種融合切線段的hRRT 狹窄空間路徑規(guī)劃算法（An hRRT Narrow Space Path Planning Algorithm Fused with Tangent Segment，T-hRRT）。先使用hRRT 算法搜索到障礙物附近，再利用切線段尋找狹窄空間入口并構(gòu)建障礙物輪廓的可通過狹窄空間，由于最短路徑的解一定是由切線及其輪廓組成［8］，因此，T-hRRT 算法在狹窄空間環(huán)境中可以得到相對(duì)較短的路徑，并且算法效率大幅提高。

1 狹窄空間定義

全局通行度是在地圖上的所有可通行空間內(nèi)，除去機(jī)器人或車輛所占空間外，自由空間面積的最小值與該點(diǎn)自由空間總面積的百分比［21］。在二維的管道與洞穴環(huán)境中，將自由空間面積轉(zhuǎn)化為水平截面寬度化簡(jiǎn)計(jì)算，如式（1）所示。

式（1）中，p為全局通行度，wmin為全局地圖可通行的自由空間的最小水平截面寬度，wrobot為移動(dòng)機(jī)器人寬度。當(dāng)p>0 時(shí)，機(jī)器人能從狹窄通道通過，當(dāng)p≤0 時(shí)，該路段不可通行。本文將通行度為50% 以下的空間定義為狹窄空間，50%以上的稱為普通空間。本文主要以狹窄空間為基礎(chǔ)對(duì)hRRT 路徑規(guī)劃算法進(jìn)行研究。

2 hRRT 算法原理

hRRT 算法優(yōu)先向目標(biāo)方向擴(kuò)展低代價(jià)路徑，同時(shí)保持對(duì)探索的合理偏向。hRRT 算法以起始點(diǎn)qstɑrt作為隨機(jī)樹RRTree 的根節(jié)點(diǎn)。按照特定概率函數(shù)F 從自由空間Sfree中選擇隨機(jī)點(diǎn)qrɑnd或者目標(biāo)點(diǎn)qgoɑl作為采樣點(diǎn)qsɑmple。在隨機(jī)樹RRTree 中尋找距離采樣點(diǎn)qsɑmple最近的節(jié)點(diǎn)qneɑr，以節(jié)點(diǎn)qneɑr為原點(diǎn)向采樣點(diǎn)qsɑmple方向延長(zhǎng)步長(zhǎng)ε獲得新節(jié)點(diǎn)qnew。若新節(jié)點(diǎn)qnew路徑未經(jīng)過障礙物空間Sobs，則將新節(jié)點(diǎn)qnew加入隨機(jī)樹RRTree 中，若經(jīng)過障礙物空間Sobs，則搜索失敗，重新選取采樣點(diǎn)qsɑmple。當(dāng)樹中的節(jié)點(diǎn)擴(kuò)散到目標(biāo)點(diǎn)qgoɑl時(shí)，完成整個(gè)搜索，返回起始點(diǎn)qstɑrt到目標(biāo)點(diǎn)qgoɑl的路徑節(jié)點(diǎn)，hRRT 算法原理如圖1 所示。

我們來(lái)梳理一下這三個(gè)步驟，第一步通過寫作建立人物形象，這是“以寫促讀”；第二步對(duì)照原文修改寫作片段，這是“以讀促寫”；第三步學(xué)習(xí)聯(lián)想，用聯(lián)想的方法再次修改寫作片段，這又是“以讀促寫”。在不斷修改的過程中，加深學(xué)生對(duì)“全神貫注”一詞及文章主旨的認(rèn)識(shí)。

hRRT 算法采用啟發(fā)式成本低的目標(biāo)偏置節(jié)點(diǎn)進(jìn)行擴(kuò)展，縮短計(jì)算時(shí)間，但是當(dāng)環(huán)境中存在狹窄通道時(shí)，其性能會(huì)出現(xiàn)大幅度下降。其原因主要是由于hRRT 算法采用基于概率函數(shù)的隨機(jī)采樣策略，相對(duì)于整個(gè)環(huán)境空間而言，窄道所占面積比例很小，導(dǎo)致窄道內(nèi)采樣概率很低，隨機(jī)樹難以在狹窄通道中擴(kuò)展，從而狹窄通道兩端無(wú)法連通，最終導(dǎo)致路徑無(wú)解。

Fig.1 Schematic diagram of hRRT algorithm principle圖1 hRRT 算法原理示意圖

3 融合切線段的hRRT 路徑規(guī)劃算法

切線算法基于障礙物邊緣間的切線構(gòu)成最短無(wú)碰撞路徑原理，應(yīng)用Dijkstra 算法遍歷所有障礙物切點(diǎn)，在二維地圖中能得到最短路徑，并且其環(huán)境適應(yīng)性好，在狹窄通道環(huán)境中也能搜索到最短路徑。缺點(diǎn)是時(shí)間復(fù)雜度與空間復(fù)雜度較高，隨著地圖中障礙物切點(diǎn)數(shù)目的增多，算法搜索效率逐漸下降。

本文采用hRRT 算法與切線算法相結(jié)合的方法，利用hRRT 算法避免切線算法的全局搜索，利用切線算法通過狹窄通道，加快路徑搜索速度，兩者相輔相成。

3.1 地圖柵格化

任意地圖環(huán)境都可以被量化成具有一定分辨率的柵格，而柵格大小直接影響信息存儲(chǔ)量的大小和建模精度兩個(gè)問題［21］。由于原始地圖信息過大，切線算法時(shí)間和空間復(fù)雜度較高，為了提高效率、降低資源占用，本文使用柵格法進(jìn)行環(huán)境建模，簡(jiǎn)化地圖。

對(duì)于狹窄空間，較大的障礙柵格尺寸會(huì)使通路變得更窄，甚至使通路封閉而影響結(jié)果，而較小的柵格尺寸會(huì)增加算法計(jì)算量。因此設(shè)計(jì)合理的柵格大小很重要，本文定義柵格化尺度為移動(dòng)機(jī)器人體積的一半，保證地圖狹窄通道的原有信息，又減少了計(jì)算量，如式（2）所示。

式（3）中，Gxy為柵格取值，1 為障礙柵格，0 為自由柵格；glimit為柵格劃分閾值，當(dāng)附近的障礙面積大于此值，代表此柵格為障礙物點(diǎn)。式（4）中，g(x,y)為原始地圖(x,y)處的值，障礙空間數(shù)值為1，自由空間數(shù)值為0；∑gɑround為柵格點(diǎn)附近原始地圖中的取值之和，代表了附近柵格的障礙面積。

3.2 障礙物凸點(diǎn)檢測(cè)

地圖中任意障礙物經(jīng)過柵格化處理可以形成多邊形模型，自由空間Sfree內(nèi)任意一點(diǎn)與障礙物多邊形模型的切點(diǎn)一定在其凸點(diǎn)［8］。如圖2 所示，黑色區(qū)域?yàn)檎系K物柵格化后的多邊形模型，紅色點(diǎn)為障礙物多邊形模型的凸點(diǎn)（彩圖掃OSID 碼可見）。

Fig.2 Rasterized maps圖2 柵格化地圖

因?yàn)槁窂揭?guī)劃時(shí)將移動(dòng)機(jī)器人設(shè)為質(zhì)點(diǎn)，但機(jī)器人體積不可忽略，故需要在障礙物周圍設(shè)置安全距離dsɑfe。本文設(shè)置的安全距離為障礙物向外一個(gè)柵格的距離，即dsɑfe=，如圖2 所示的灰色區(qū)域內(nèi)為移動(dòng)機(jī)器人碰撞區(qū)域，在移動(dòng)機(jī)器人運(yùn)動(dòng)過程中需要避開該區(qū)域。

針對(duì)安全距離問題，如圖2 所示，將灰色包圍區(qū)域作為最終障礙物模型，使用藍(lán)色凸點(diǎn)作為本文規(guī)劃算法中所使用的凸點(diǎn)，從而保證算法能夠規(guī)劃出一條無(wú)碰撞路徑。搜索地圖環(huán)境中所有凸點(diǎn)，將凸點(diǎn)位置信息放入凸點(diǎn)矩陣bumps{[xi yi]}。凸點(diǎn)檢測(cè)的偽代碼如下：

3.3 切點(diǎn)搜索

在搜索切點(diǎn)時(shí)，本文通過有限長(zhǎng)度的切線段搜索切點(diǎn)。連接自由空間任意一點(diǎn)xfree與所有凸點(diǎn)，并且兩端延長(zhǎng)e=wrobot，如果生成的線段中沒有經(jīng)過障礙物空間，則該生成線段的凸點(diǎn)為xfree對(duì)障礙物模型的切點(diǎn)。如圖3 所示，A點(diǎn)為xfree對(duì)障礙物模型的一個(gè)切點(diǎn)，用來(lái)尋找狹窄空間入口處的節(jié)點(diǎn)。

根據(jù)上述方法，還可以規(guī)劃障礙物模型輪廓路徑，如圖3 所示，以障礙物模型上的凸點(diǎn)A 為原點(diǎn)搜索切點(diǎn)，點(diǎn)B滿足要求。A、B 兩點(diǎn)連接構(gòu)成障礙物模型的一段輪廓路徑，確保隨機(jī)樹在狹窄空間中有效擴(kuò)展，切點(diǎn)搜索偽代碼如下：

對(duì)于有公切線的障礙物，切線段會(huì)多次往復(fù)搜索，陷入局部收斂。本文將重復(fù)的切點(diǎn)只在RRTree 中記錄一次，減少隨機(jī)樹節(jié)點(diǎn)數(shù)，并且使得算法快速跳出局部收斂。

Fig.3 Tangent section of point to obstacle圖3 點(diǎn)對(duì)障礙物切線段

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

算法實(shí)現(xiàn)過程如下：

Step1：輸入起始點(diǎn)qstɑrt、目標(biāo)點(diǎn)qgoɑl、地圖環(huán)境的信息。

Step2：柵格化地圖，并查找地圖中障礙物凸點(diǎn)bumps。

Step3：設(shè)置概率函數(shù)，本文采用簡(jiǎn)單概率函數(shù)F，利用函數(shù)在0 到1 區(qū)間隨機(jī)取值，當(dāng)大于0.5 時(shí)，進(jìn)入隨機(jī)采樣，當(dāng)小于等于0.5 時(shí)，偏向目標(biāo)采樣。

Step4：尋找隨機(jī)樹RRTree 中距離采樣點(diǎn)qsɑmple最近的點(diǎn)qneɑr，按照采樣點(diǎn)qsɑmple方向定步長(zhǎng)ε擴(kuò)展生成新節(jié)點(diǎn)qnew，判斷該路徑間是否有障礙，若存在障礙，則進(jìn)入Step5，若無(wú)障礙，將新節(jié)點(diǎn)qnew加入到隨機(jī)樹中RRTree。

Step5：以當(dāng)前距離采樣點(diǎn)qsɑmple最近的點(diǎn)qneɑr做障礙物切線段，將切點(diǎn)加入隨機(jī)樹RRTree 中，刪除重復(fù)切點(diǎn)。

Step6：當(dāng)隨機(jī)樹中的節(jié)點(diǎn)與目標(biāo)點(diǎn)qgoɑl沒有障礙時(shí)，路徑搜索完成。

4 實(shí)驗(yàn)分析

為了驗(yàn)證T-hRRT 路徑規(guī)劃算法性能，本文在3 種地圖環(huán)境中對(duì)hRRT 算法、切線算法以及T-hRRT 算法進(jìn)行仿真，地圖中黑色幾何圖形代表障礙物，空白區(qū)域?yàn)樽杂煽臻g，藍(lán)色細(xì)短線表示分支，黑色粗線表示最終生成的路徑。算法運(yùn)行環(huán)境為：64bit Windows10 操作系統(tǒng)；MATLAB 2019a；處理Intel（R）Core（TM）i5-9300H；主頻2.4GHz；內(nèi)存8GB。

4.1 普通空間環(huán)境

地圖一為普通空間環(huán)境，大小為500*500 分辨率，全局通行度大于50%，起始點(diǎn)與目標(biāo)點(diǎn)位置分布為（10，10）與（490，490）。該地圖用于驗(yàn)證3 種算法在起點(diǎn)與終點(diǎn)之間包含多障礙物時(shí)的搜索能力，如圖4（a）、4（b）、4（c）分別為切線算法、hRRT 算法、T-hRRT 算法在地圖一環(huán)境下的一次實(shí)驗(yàn)結(jié)果，其中hRRT 算法的步長(zhǎng)為30。

Fig.4 Results of map 1 experiment圖4 地圖一中實(shí)驗(yàn)結(jié)果

在地圖一普通空間環(huán)境中對(duì)3 種算法經(jīng)過20 次試驗(yàn)，其規(guī)劃時(shí)間、路徑長(zhǎng)度與節(jié)點(diǎn)數(shù)如表1 所示。

Table 1 Simulation results of three algorithms in ordinary space environment表1 普通空間環(huán)境下3 種算法仿真結(jié)果

由表1 可知，在地圖一環(huán)境中T-hRRT 算法獲得相對(duì)于hRRT 算法的較短路徑，平均節(jié)點(diǎn)數(shù)減少了87%，搜索時(shí)間減少了90%；相對(duì)于切線算法減少了99%的平均規(guī)劃時(shí)間。結(jié)果表明，T-hRRT 算法在普通空間環(huán)境中表現(xiàn)良好。

4.2 狹窄空間環(huán)境

地圖二與地圖三為狹窄空間環(huán)境，大小為500*500 分辨率，全局通行度小于50%，其中地圖二只為直線型狹窄空間環(huán)境，地圖三為不規(guī)則輪廓型狹窄空間環(huán)境。起始點(diǎn)與目標(biāo)點(diǎn)位置分布為（10，10）與（490，490）。用于驗(yàn)證3 種算法在起點(diǎn)與終點(diǎn)之間包含不同狹窄空間時(shí)的搜索能力，圖5（a）、5（b）、5（c）、6（a）、6（b）、6（c）為兩種狹窄空間環(huán)境中切線算法、hRRT 算法、T-hRRT 算法的一次實(shí)驗(yàn)結(jié)果。

在地圖二直線型狹窄空間環(huán)境與地圖三不規(guī)則輪廓型狹窄空間環(huán)境中對(duì)3 種算法經(jīng)過20 次試驗(yàn)，其規(guī)劃時(shí)間、路徑長(zhǎng)度與節(jié)點(diǎn)數(shù)如表2 和表3 所示。

Fig.5 Results of map 2 experiment圖5 地圖二中實(shí)驗(yàn)結(jié)果

Fig.6 Results of map 3 experiment圖6 地圖三中實(shí)驗(yàn)結(jié)果

Table 2 Simulation results of three algorithms under linear narrow space environment表2 直線型狹窄空間環(huán)境下3 種算法仿真結(jié)果

表2 與表3 中的值為無(wú)窮，表示該算法在此環(huán)境中失效，而對(duì)于切線算法無(wú)節(jié)點(diǎn)數(shù)，步長(zhǎng)為30 的hRRT 算法在地圖二與地圖三狹窄空間環(huán)境中失效，顯然狹窄環(huán)境影響了隨機(jī)樹的有效擴(kuò)展，達(dá)到節(jié)點(diǎn)最大搜索次數(shù)時(shí)無(wú)解。切線算法與T-hRRT 算法都能通過狹窄通道，但是T-hRRT 算法在地圖二直線型狹窄空間環(huán)境與地圖三不規(guī)則輪廓型狹窄空間環(huán)境中平均規(guī)劃時(shí)間相比于切線算法減少了89%，而平均路徑長(zhǎng)度不超過切線算法規(guī)劃最短路徑長(zhǎng)度的5%。

Table 3 Simulation results of three algorithms in narrow space with irregular contour表3 不規(guī)則輪廓型狹窄空間環(huán)境下3 種算法仿真結(jié)果

5 結(jié)語(yǔ)

針對(duì)狹窄空間下hRRT 算法路徑無(wú)解問題，本文結(jié)合切線算法與hRRT 算法，提出T-hRRT 算法，利用切線算法加快hRRT 算法節(jié)點(diǎn)擴(kuò)展與通過狹窄空間，利用hRRT 算法隨機(jī)擴(kuò)展節(jié)點(diǎn)避免切線算法的全局計(jì)算，相互彌補(bǔ)缺點(diǎn)從而獲得規(guī)劃時(shí)間較少并且長(zhǎng)度較短的路徑。T-hRRT 算法完成了hRRT 算法不能解決的狹窄通道問題，并且其路徑長(zhǎng)度與切線算法規(guī)劃的最短路徑長(zhǎng)度相差不到5%，而相對(duì)于切線算法的規(guī)劃時(shí)間減少了約89%。在后續(xù)研究中，將進(jìn)一步提升算法在三維空間與未知、動(dòng)態(tài)環(huán)境中的規(guī)劃能力。