室內(nèi)場景下應(yīng)用拓?fù)浣Y(jié)構(gòu)的高效路徑規(guī)劃算法

李冠達，金 兢，王 凡，夏營威，楊學(xué)志

（1.合肥工業(yè)大學(xué)計算機與信息學(xué)院，合肥 230601；2.合肥工業(yè)大學(xué)工業(yè)安全與應(yīng)急技術(shù)安徽省重點實驗室，合肥 230601；3.中國科學(xué)院合肥物質(zhì)科學(xué)研究院安徽光學(xué)精密機械研究所，合肥 230031；4.中國科學(xué)技術(shù)大學(xué) 研究生院科學(xué)島分院，合肥 230026；5.合肥工業(yè)大學(xué) 軟件學(xué)院，合肥 230601）

0 概述

隨著人工智能領(lǐng)域的發(fā)展，移動機器人路徑規(guī)劃技術(shù)相對地也獲得了較大的提升。目前，移動機器人的需求量正逐步擴大，如家庭中的掃地機器人、物流工廠中的送貨機器人等。移動機器人作為一種重要的生產(chǎn)生活工具，自主路徑導(dǎo)航是其核心，所以如何使移動機器人在較短的時間內(nèi)規(guī)劃出一條有效的路徑成為該領(lǐng)域的一項熱門研究。

常見的路徑規(guī)劃算法包括蟻群算法［1］、粒子群優(yōu)化算法［2］、遺傳算法［3］、人工勢場法［4］和A*算法［5］。近年來，基于深度學(xué)習(xí)和深度強化學(xué)習(xí)的方法來解決路徑規(guī)劃的問題備受研究人員的關(guān)注。文獻［6］采用卷積神經(jīng)網(wǎng)絡(luò)和強化學(xué)習(xí)相結(jié)合的方式來解決路徑規(guī)劃問題。文獻［7］提出一種改進的深度Q學(xué)習(xí)算法，減少過估計對機器人在選擇動作時的影響，達到所選策略最優(yōu)。文獻［8-9］介紹了基于采樣的路徑規(guī)劃算法，通過對狀態(tài)空間的采樣點進行碰撞檢測，避免了對空間的建模，并且可以直接添加運動約束。其優(yōu)勢主要在于無需對搜索區(qū)域進行幾何劃分，在搜索空間的覆蓋率高，搜索的范圍廣以及盡可能地探索未知區(qū)域。

1998 年，LAVALLE 等［10］提出一種基于采樣的路徑規(guī)劃工具——快速搜索隨機樹（Rapidly-exploring Random Tree，RRT）。由于其具有收斂速度快、可探索未知障礙物的空間等優(yōu)點，被廣泛應(yīng)用于路徑規(guī)劃領(lǐng)域。文獻［11-12］提出了相應(yīng)的改進算法，這些改進算法都以初始點為根節(jié)點，并通過迭代探索未知區(qū)域來構(gòu)建一組軌跡生成隨機樹。文獻［13］指出由于隨機采樣的性質(zhì)，RRT 及其改進的相關(guān)算法并沒有考慮路徑成本，也不能保證解的最優(yōu)性。

RRT-Connect 算法是在RRT 算法的基礎(chǔ)上采用兩棵樹雙向搜索的引導(dǎo)策略。同時，該算法在擴展方式上利用貪婪策略加快了搜索速度，減少了非障礙空間的無用搜索，節(jié)省了搜索時間。文獻［14-15］提出由于RRT-Connect 算法缺乏優(yōu)化過程，其無法保證解的最優(yōu)性。

為了解決解的最優(yōu)性問題，文獻［16］提出一種漸進最優(yōu)的路徑規(guī)劃算法RRT*，其通過重新布線原則并為新節(jié)點選擇代價最小的父節(jié)點來保證解的最優(yōu)性。文獻［17］介紹了RRT*是漸近最優(yōu)的，當(dāng)采樣次數(shù)達到無窮大時可以找到最優(yōu)解，但其探索過程會消耗大量的時間。文獻［18］提出一種基于簡化地圖的區(qū)域采樣RRT*算法，其通過簡化地圖并在地圖中隨機采樣N個節(jié)點來引導(dǎo)生成初始路徑。

文獻［19］提出一個RRT*的修改版本，稱為知情RRT*（Informed-RRT*，IRRT*）算法。在IRRT*算法中，通過RRT 算法找到初始路徑后，根據(jù)初始路徑構(gòu)建一個會逐漸收斂的超橢球體，其采樣子集被限制在超橢球體中。文獻［15］介紹了IRRT*算法雖然在完整性和最優(yōu)性方面具有與RRT*算法相同的性能，但相比于RRT*算法在優(yōu)化路徑時對整個空間進行探索，IRRT*算法在超橢球體內(nèi)進行采樣可以提高探索效率。文獻［20-21］提出一種改進的IRRT*算法，稱為STIRRT*算法，其首先對柵格地圖進行骨架提取，然后通過角點檢測算法對有用的特征點進行提取，這樣可以加快IRRT*尋找到初始路徑，并對其進行優(yōu)化。但其通過角點檢測算法在不規(guī)則的地方所保留的節(jié)點通常有過多的冗余節(jié)點，這樣會使初始路徑代價變大。在起點和終點距離很遠(yuǎn)時，其算法所構(gòu)成的橢圓仍會包含整個探索空間，無法有效地提高算法的效率。由于采樣優(yōu)化半徑為單調(diào)遞減函數(shù)，因此在距離較遠(yuǎn)的相鄰特征點很難得到有效的優(yōu)化。

針對STIRRT*算法中特征點的冗余造成初始路徑代價較大和得到初始路徑后其優(yōu)化效果不明顯的問題，本文提出一種應(yīng)用拓?fù)浣Y(jié)構(gòu)的高效路徑規(guī)劃算法ATIRRT*。該算法通過引入拓?fù)涔?jié)點代替STIRRT*算法中利用Harris角點檢測算法得到的特征點進行采樣，以減少尋找初始路徑的代價，同時給出非單一父節(jié)點的連接方式提高算法的魯棒性。通過節(jié)點擴充策略在相鄰的兩個拓?fù)涔?jié)點間增加節(jié)點的數(shù)量，加快優(yōu)化算法的收斂。最終根據(jù)柵格地圖的大小和障礙物在地圖中的位置和數(shù)量定義相關(guān)約束條件來約束拓?fù)涔?jié)點之間的距離，并將初始路徑分段并進行逐段優(yōu)化。

1 STIRRT＊算法

STIRRT*算法相較于RRT 算法有以下4 點改進：

1）對柵格地圖進行骨架提取并通過Harris 角點檢測算法得到相應(yīng)的特征點以引導(dǎo)隨機樹擴展，加快得到初始路徑。

2）為新節(jié)點選擇代價最小的父節(jié)點。如圖1所示，首先根據(jù)設(shè)定的起點nodestart(nodes)和終點nodegoal(nodeg)通過RRT 算法在柵格地圖中進行采樣。然后尋找目前隨機樹中距離采樣點noderand(noder)最近的節(jié)點nodenear(noden)，通過RRT 算法中的擴展方式得到了一個新節(jié)點nodenew(nodene)。最后以nodene為圓心，一定半徑畫一個圓。逐個計算起點到圓內(nèi)節(jié)點的距離與圓內(nèi)節(jié)點到新節(jié)點nodene的距離和，其中最小的距離和就是起點到新節(jié)點nodene的路徑代價，相應(yīng)的節(jié)點為nodene的父節(jié)點。

圖1 新節(jié)點選擇代價最小的父節(jié)點過程Fig.1 Parent node process with lowest cost of new node

3）重布線原則，即為新節(jié)點尋找子節(jié)點的過程。在由新節(jié)點nodene所構(gòu)成的圓中，對圓中的任意一個節(jié)點，計算以新節(jié)點為圓中節(jié)點的父節(jié)點時，其距離代價是否會降低。如果降低，那么將該節(jié)點的父節(jié)點賦值為新節(jié)點，對應(yīng)的代價更新為新的代價。從圖2 可以看出，節(jié)點e原來的代價為23，如果以節(jié)點c為父節(jié)點，那么新的代價為20，小于原來的代價。因此，將節(jié)點e的父節(jié)點改為節(jié)點c，結(jié)果如圖2 所示。

圖2 新節(jié)點尋找子節(jié)點的過程Fig.2 Process of finding child nodes for new node

4）根據(jù)得到的初始路徑，將采樣空間限制在由起點、終點和初始路徑所構(gòu)成的橢圓中，并且隨著路徑長度的不斷縮短，逐漸縮小橢圓區(qū)域。

2 拓?fù)浣Y(jié)構(gòu)的高效路徑規(guī)劃算法

拓?fù)浣Y(jié)構(gòu)的高效路徑規(guī)劃（STIRRT*）算法可以快速得到初始路徑，加快進入到優(yōu)化過程，同時在優(yōu)化過程中把采樣點限制在橢圓中以避免不必要的探索。但在起點和終點距離較遠(yuǎn)的情況下，其由初始路徑所生成的橢圓往往會包含整個場景；在優(yōu)化過程中算法的收斂速度較慢，在距離較遠(yuǎn)的相鄰特征點很難得到有效的優(yōu)化?；谏鲜鲈?，本文提出ATIRRT*算法，其與STIRRT*算法相同，主要分為兩個部分：尋找初始路徑和基于橢圓采樣。但是本文算法做出以下3 點改進：1）為了消除通過Harris 算法生成的冗余特征點，本文根據(jù)柵格地圖的大小提出了一種閾值的自適應(yīng)選擇方法，同時給出非單一父節(jié)點的連接方式，提高了算法的魯棒性；2）為了加快優(yōu)化算法的收斂，在相鄰的兩個拓?fù)涔?jié)點之間通過節(jié)點擴充策略，增加節(jié)點的數(shù)量；3）ATIRRT*算法在STIRRT*算法的基礎(chǔ)上加入相關(guān)約束條件，該條件可以自適應(yīng)調(diào)整采樣范圍，減少無用的空間擴展從而減少迭代次數(shù)。

2.1 拓?fù)浣Y(jié)構(gòu)

文獻［22-23］介紹了骨架提取算法。該算法可以將連通區(qū)域細(xì)化成一個像素的寬度，主要用于特征提取和目標(biāo)拓?fù)浔硎?。圖3 所示為對仿真的室內(nèi)場景通過骨架提取算法處理后的結(jié)果。

圖3 仿真室內(nèi)場景的骨架提取Fig.3 Skeleton extraction of simulated indoor scene

經(jīng)過骨架提取算法處理后，其連通區(qū)域會被細(xì)化成一條細(xì)線，仿真室內(nèi)場景的骨架主要由一些重要的點連接而成。這些點被稱為特征點，本文采用角點檢測算法檢測這些特征點。目前，角點檢測算法主要分為基于圖像邊緣的方法和基于圖像灰度的方法兩大類。前者嚴(yán)重依賴于圖像邊緣檢測結(jié)果，后者算法簡單，結(jié)果穩(wěn)定可靠。文獻［24］介紹了在基于圖像灰度的典型角點檢測算法中，Harris算法提取的角點較為理想。因此本文采用Harris 角點檢測算法來確定這些特征點。文獻［25］介紹了Harris 角點檢測算法應(yīng)用鄰近像素點灰度差值概念，從而進行判斷是否為角點、邊緣、平滑區(qū)域。Harris 角點檢測過程的主要步驟如下：

1）對圖像中的逐個像素點通過水平和豎直兩種差分算子進行濾波求得Ix、Iy，從而得到矩陣M中的4 個元素的值：

2）對矩陣M中的4 個元素進行高斯濾波，從而消除一些冗余的點和凸起，得到新的矩陣M。

3）利用矩陣M計算對應(yīng)每個像素的角點響應(yīng)函數(shù)R，這里令R為：

其中：det(M)為矩陣M的行列式；trace(M)為矩陣M的跡。文獻［26］給出了經(jīng)驗值k的取值范圍為0.04～0.06，在本文中取值為0.04。

4）在矩陣R中，滿足點R(i，j)大于一定閾值，在本文中閾值取0.01Rmax(i，j)并且R(i，j)是其鄰域內(nèi)的局部極大值，則該點被認(rèn)為是角點。

通過上述方式在得到所有特征點（角點）之后，由于獲得的角點在柵格地圖分布并不均勻，在骨架不均勻的地方生成的特征點較為密集，為了消除冗余的特征點，ATIRRT*算法根據(jù)柵格地圖的大小，采用式（3）對所獲得的特征點進行自適應(yīng)選?。?/p>

其中：a為仿真柵格地圖的高；b為仿真柵格地圖的寬；ε為地圖大小的數(shù)量級。以當(dāng)前特征點為圓心、Threshold為半徑，在其所組成的圓內(nèi)剔除圓心以外的特征點。圖4 為對特征點根據(jù)式（3）進行閾值處理后的效果。

圖4 室內(nèi)場景仿真的拓?fù)浣Y(jié)構(gòu)Fig.4 Topological structure of indoor scene simulation

其中，圖4（a）為直接利用Harris 角點檢測算法所得到的特征點建立的結(jié)構(gòu)，圖4（b）為在對所得特征點進行閾值處理后建立的拓?fù)浣Y(jié)構(gòu)。從圖4（b）可以看出，通過自適應(yīng)閾值式（3）所保留的拓?fù)涔?jié)點nodetopology(nodet)可以有效體現(xiàn)所有可行路徑。

在ATIRRT*算法中，對于初始路徑的尋找是通過RRT 算法與拓?fù)浣Y(jié)構(gòu)相結(jié)合的方式來進行路徑搜索，相比STIRRT*算法通過RRT 算法與Harris 角點檢測算法所得到的特征點相結(jié)合的方式來進行初始路徑的探索，ATIRRT*算法探索的初始路徑長度較短。首先將拓?fù)涔?jié)點添加到本文的初始樹中，其初始樹按如下步驟生成：

步驟1所有閉合環(huán)路生成為一顆初始樹；

步驟2在交叉支路上定義相關(guān)拓?fù)涔?jié)點的父節(jié)點為parent1，parent2，…，parentn，以這樣的方式生成一顆初始樹；

步驟3統(tǒng)計由拓?fù)涔?jié)點所組成的所有可行路徑，并對其進行比較，選取長度最短的作為初始路徑；

步驟4當(dāng)節(jié)點和最近的拓?fù)涔?jié)點的子節(jié)點連線穿過障礙物時，令起點為最近的拓?fù)涔?jié)點的父節(jié)點，然后把起點插入到初始樹中；

步驟5當(dāng)節(jié)點和最近拓?fù)涔?jié)點的子節(jié)點的連線沒有穿過障礙物時，令起點為最近拓?fù)涔?jié)點的子節(jié)點的父節(jié)點，同樣把起點插入到初始樹中。

在RRT 的相關(guān)算法中，隨機樹中的每個節(jié)點只有一個父節(jié)點，這樣節(jié)點不能和拓?fù)浣Y(jié)構(gòu)相結(jié)合。本文提出的算法讓拓?fù)涔?jié)點可以同時具有多個父節(jié)點，這樣可以有效地將拓?fù)浣Y(jié)構(gòu)以樹的形式表示出來。ATIRRT*算法尋找初始路徑的主要思想如下：首先在隨機樹中找到距離起點和終點拓?fù)涔?jié)點；然后通過所設(shè)定的父子關(guān)系將起點和終點分別替代最近的拓?fù)涔?jié)點；經(jīng)過步驟3 和步驟4 的判斷，可以有效地將起點和終點加入到拓?fù)浣Y(jié)構(gòu)中。ATIRRT*算法尋找初始路徑的偽代碼如下：

在算法1 中，TreeTopology 代表拓?fù)浣Y(jié)構(gòu)地圖生成的初始樹，通過Nearest（）函數(shù)找到初始樹中距離起點和目標(biāo)點最近，然后將起點和終點插入到初始樹中，并結(jié)合拓?fù)浣Y(jié)構(gòu)加快找到初始路徑。Collision（）函數(shù)為碰撞檢測函數(shù)，如果擴展的新節(jié)點和與其最近的節(jié)點的連線穿過障礙物，則重新進行擴展。

2.2 節(jié)點擴充策略

在尋找到初始路徑后，拓?fù)浣Y(jié)構(gòu)圖中節(jié)點的信息過少。由于STIRRT*算法在對初始路徑進行優(yōu)化時，其仍采用文獻［19］IRRT*算法中的采樣優(yōu)化半徑，通過對當(dāng)前節(jié)點并以一定半徑進行采樣優(yōu)化。其采樣優(yōu)化半徑r的一般形式如式（4）所示：

其中：k為自適應(yīng)參數(shù)；n代表隨機樹含有的節(jié)點數(shù)量，由于隨機樹在初始化時就已經(jīng)包含起止點，因此n≥2。由于隨著節(jié)點數(shù)量的增加，會增加比較的次數(shù)，其收斂速度會變慢，本文為加快采樣半徑的收斂，取d=2。如圖5 所示，函數(shù)特性在出現(xiàn)第2 個節(jié)點后就單調(diào)下降。

圖5 采樣優(yōu)化半徑Fig.5 Sampling optimization radius

隨機樹在進行采樣時會根據(jù)現(xiàn)有的節(jié)點向外進行擴展。隨著節(jié)點數(shù)量的增加，節(jié)點的采樣優(yōu)化半徑的長度是單調(diào)遞減的，通過拓?fù)涔?jié)點可以快速有效地找到初始路徑。為了加快優(yōu)化算法的收斂，本文通過節(jié)點擴充策略，并以半個步長對其進行節(jié)點擴充。但是相較于RRT 算法在柵格地圖中隨機生成節(jié)點，本文是在生成的初始路徑相鄰的兩個拓?fù)涔?jié)點之間進行擴充，如圖6 所示。

圖6 拓?fù)涔?jié)點擴充Fig.6 Topology node expansion

在圖6 中，nodet和nodetopologynear(nodetn)為拓?fù)浣Y(jié)構(gòu)地圖中的相鄰?fù)負(fù)涔?jié)點，新節(jié)點的增加方式是沿著兩節(jié)點之間的直線距離和角度并以1/2 的擴展步長進行添加。其擴展公式如式（5）～式（8）所示：

其中：(x1，y1)和(x2，y2)分別代表nodet和nodetn的橫縱坐標(biāo)；DDis和θ代表它們之間的距離和角度；(nodeadd.x，nodeadd.y)代表生成新節(jié)點的坐標(biāo)。通過節(jié)點擴充的方式可以使節(jié)點之間建立有效的聯(lián)系，加快算法的收斂。

2.3 橢圓采樣策略

在對初始路徑進行節(jié)點擴充后，當(dāng)對擴充后的初始路徑進行優(yōu)化時，采用橢圓采樣的方式代替整個地圖采樣，這樣可以有效地減少搜索路徑的次數(shù)。采樣點均勻分布在橢圓內(nèi)部，如果通過采樣點生成的路徑長度小于原先規(guī)劃的長度，則替代原來的長度并根據(jù)新的路徑長度逐漸調(diào)整橢圓的大小。當(dāng)兩點之間沒有障礙物時，其會漸漸收斂為一條線段。其中二維空間中橢圓最基本的形式如式（9）所示：

橢圓的兩軸為別是x軸及y軸，軸長分別是2a和2b，將式（9）寫成矩陣的形式，其結(jié)果如式（10）所示：

由此可知矩陣B∈Rn×n為對稱正定矩陣，對其進行Cholesky 分解，其可分解為一個下三角矩陣L和其轉(zhuǎn)置的乘積：

經(jīng)過Cholesky 分解，根據(jù)文獻［27］對橢圓方程進行如式（12）～式（14）的變換：

其中：Xcircle為平面內(nèi)圓的方程；diag{.}代表對角矩陣；cmin代表起點和終點的直線距離；cbest為設(shè)定的長度，其值要大于cmin。

逐次對cbest減小一定的數(shù)值直到減小到cmin停止采樣，同時根據(jù)文獻［28］進行如式（15）的旋轉(zhuǎn)變換：

其中：det(·)代表矩陣的行列式；U∈Rn×n和V∈Rn×n是酉矩陣；定義xcenter=(xa+xb)/2 為橢圓的中心，xa、xb為橢圓的兩個焦點。通過式(14)、式(15)，可以在橢圓中根據(jù)式（16）進行采樣：

圖7 為在起點(0，0)和終點(100，100)所構(gòu)成的橢圓空間中進行采樣。從圖7 可以看到，當(dāng)起點和終點之間沒有障礙物時，其逐漸收斂為由起點和終點所構(gòu)成的一條線段。

圖7 橢圓的采樣過程Fig.7 Sampling process of ellipse

基于橢圓采樣的偽代碼如下：

在算法2 中，SampleUnitBall()函數(shù)是在單位球中進行取點，SampleFreeSpace()函數(shù)代表在非障礙物所在的空間進行采樣。

在STIRRT*算法中，cbest為規(guī)劃路徑的長度，規(guī)劃路徑由采樣點所組成，cmin為起點和終點的直線距離。雖然采用橢圓采樣的方式可以有效地減小采樣點探索的空間，避免了大量的無效搜索，但是當(dāng)起點和終點距離較遠(yuǎn)時，其生成的橢圓仍會覆蓋整個地圖空間。因此，根據(jù)柵格地圖的大小和障礙物在地圖中的位置和數(shù)量，本文提出的算法定義相關(guān)約束條件來約束拓?fù)涔?jié)點之間的距離，通過該約束條件把初始路徑進行分段并對每一段進行優(yōu)化。通過將不同的障礙物設(shè)定不同的權(quán)重，障礙物在室內(nèi)場景中主要抽象成3 種：矩形障礙物α，圓形障礙物β和類似過道或門一樣的障礙物γ，其滿足關(guān)系如式（17）所示，約束條件如式（18）所示：

其中：a為柵格地圖的寬；b為柵格地圖中的長；m、n、p為3 種不同障礙物的數(shù)量；μ和θ是根據(jù)障礙物在整個地圖中的具體位置可調(diào)整的自適應(yīng)參數(shù)，其取值范圍為0～1，障礙物在地圖中的位置影響程度越大其值越接近1；［］為取整符，將所得到的值進行取整。從式（18）中可以得到，在同種地圖中，當(dāng)障礙物的數(shù)量越多影響程度越大時，約束條件的值越小。通過該約束條件可以將初始路徑進行分段，提高了算法的優(yōu)化效率，ATIRRT*算法分段優(yōu)化的偽代碼如下：

在算法3 中，初始路徑(initialpath)是由拓?fù)涔?jié)點所組成的，nodetopologydistance(nodetd)表示拓?fù)涔?jié)點之間的距離，nodetopology(nodeti)表示構(gòu)成initial_path 中的第i個拓?fù)涔?jié)點。Nearest()函數(shù)返回隨機樹中離采樣點最近的節(jié)點nodenearest(nodenr)，steer()函數(shù)通過隨機采樣點和最近的節(jié)點來生成新節(jié)點。

3 仿真實驗結(jié)果與分析

為了對ATIRRT*算法進行性能比較，本文實驗對如圖8 所示的地圖進行仿真，地圖包含3 種情況，分別為常規(guī)環(huán)境（地圖1）、狹長空間（地圖2）和仿真的室內(nèi)環(huán)境（地圖3）。其中地圖中空白區(qū)域代表無障礙物區(qū)域，灰色區(qū)域代表障礙物區(qū)域。仿真所使用的硬件平臺為Intel?CoreTMi5-5200U 2.2 GHz CPU，RAM 8 GB 和Intel?Xeon?Silver 4114 2.2 GHz CPU 2.19 GHz CPU（2 處理器），RAM 128 GB，算法通過Python 編程實現(xiàn)。在實驗中，分別以RRT、RRT-Connect、RRT*、Informed-RRT*、STIRRT* 和DQN 作為改進算法的比較算法，通過比較證明ATIRRT*尋找路徑的優(yōu)越性。圖9 為對特征點進行閾值處理后所構(gòu)成的拓?fù)浣Y(jié)構(gòu)地圖，表1 為3 種仿真地圖下得到角點和拓?fù)涔?jié)點所需的時間。

表1 角點和拓?fù)涔?jié)點消耗的時間Table 1 Time consumed by corners and topology nodes s

圖8 2D 柵格地圖仿真Fig.8 2D grid map simulated

圖9 3 種地圖的拓?fù)浣Y(jié)構(gòu)Fig.9 Topology of three kinds maps

本文在相同的情況下，對ATIRRT*算法與RRT 和RRT-Connect 算法所尋找的路徑進行了10 次對比實驗。其中根據(jù)拓?fù)涔?jié)點的閾值式（3）來設(shè)定擴展步長，其擴展步長和閾值相等。為了盡可能地找到規(guī)劃路徑，設(shè)定最大的迭代次數(shù)為50 000 次。其在3 種地圖上的起始位置和目標(biāo)位置如表2 所示。

表2 3 種地圖下的機器人起止位置Table 2 Start and stop position of robot under three kinds maps

圖10、圖11、圖12 為隨機抽取地圖1、地圖2、地圖3 中一組對比實驗結(jié)果。

圖10 常規(guī)環(huán)境中3 種算法的生成路徑結(jié)果Fig.10 Generated path results of three algorithms in conventional environment

圖11 狹長空間中3 種算法的生成路徑結(jié)果Fig.11 Generation path results of three algorithms in long and narrow space

圖12 仿真的室內(nèi)環(huán)境中3 種算法生成路徑結(jié)果Fig.12 Generated path results for three algorithms in a simulated indoor environment

從上述的實驗結(jié)果可知，傳統(tǒng)算法在仿真地圖中，其無用的迭代次數(shù)過多會極大地占用計算機的內(nèi)存，這樣不僅增加了尋找路徑時間，而且降低了路徑的平滑性。通過對實驗結(jié)果進行比對，可以發(fā)現(xiàn)ATIRRT*算法在規(guī)劃路徑的迭代次數(shù)和路徑的平滑性上都有較大的提升。為了更直觀地體現(xiàn)所提出算法的優(yōu)越性，分別在3 種地圖上進行了10 次實驗，圖13 通過在3 種地圖上比較規(guī)劃時間、迭代次數(shù)和路徑長度來驗證本文算法的性能。

圖13 3 種仿真地圖上的性能比較Fig.13 Performance comparison on three simulation maps

從圖13可以看出，地圖2（狹長空間）RRT算法和RRTConnect算法在最大迭代次數(shù)為50 000次的10次實驗中，其成功率分別為40%和60%。而本文所提出的算法其成功率可以達到100%。相比RRT 和RRT-Connect算法，在其余兩種地圖中，本文所提出的算法在規(guī)劃時間、迭代次數(shù)和路徑長度上均有不同程度的提升，同時提高了規(guī)劃路徑的穩(wěn)定性。在得到初始路徑后，利用式（17）、式（18），根據(jù)柵格地圖的大小和障礙物在地圖中的數(shù)量和位置，得到本文中3 種仿真地圖中的Constraint。通過計算，其值分別為Constraint1=250，Constraint2=259，Constraint3=300。通過約束條件對初始路徑分段并進行逐段優(yōu)化，每一段優(yōu)化都限制在橢圓中，防止其進行無用的探索。

將分段優(yōu)化結(jié)果與RRT*、Informed RRT*和STIRRT*算法進行比較。為了盡可能地在較短的時間內(nèi)得到較優(yōu)的規(guī)劃路徑，令每次優(yōu)化的迭代次數(shù)為2 000 次。由于RRT*和Informed RRT*兩種算法都是基于RRT 算法在尋找初始路徑的同時進行優(yōu)化，區(qū)別在于兩種算法找到初始路徑后，其隨機擴展的采樣空間不同。因此，根據(jù)RRT 算法得到初始路徑的平均值再加上分布優(yōu)化的迭代次數(shù)，這樣就保證了其探索次數(shù)相同，其結(jié)果如圖14、圖15、圖16 所示。同時，本文將提出的算法和文獻［7］深度強化學(xué)習(xí)（DQN）算法進行比較。為了減少深度強化學(xué)習(xí)算法的訓(xùn)練時間，加快算法的收斂。本文根據(jù)實驗場景中的柵格地圖，對其按相應(yīng)的比例進行縮放以建立訓(xùn)練場景。為此，根據(jù)障礙物在柵格地圖上的占位信息建立如圖17 所示的仿真模型。DQN 算法訓(xùn)練和測試時使用Intel?Xeon?Silver 4114 2.2 GHz CPU 2.19 GHz CPU（2處理器），RAM 128 GB 服務(wù)器。

圖14 地圖1 分步優(yōu)化結(jié)果與RRT＊、Informed-RRT＊、STIRRT＊算法的對比Fig.14 Comparison of map one step-by-step optimization results with RRT＊，Informed-RRT＊，STIRRT＊ algorithms

圖15 地圖2 分步優(yōu)化結(jié)果與RRT＊、Informed-RRT＊、STIRRT＊算法對比結(jié)果Fig.15 Comparison of map two step-by-step optimization results with RRT＊，Informed-RRT＊，STIRRT＊ algorithms

圖16 地圖3 分步優(yōu)化結(jié)果與RRT＊、Informed-RRT＊、STIRRT＊算法對比結(jié)果Fig.16 Comaprison map three step-by-step optimization results with RRT＊，Informed-RRT＊，STIRRT＊ algorithms

網(wǎng)絡(luò)框架為Pytorch 1.4.0，選用python 3.6.13版本進行編程實現(xiàn)。其中圖17（b）、圖17（d）、圖17（f）分別對應(yīng)圖17（a）、圖17（c）、圖17（e）建立的仿真訓(xùn)練環(huán)境。本文中對比的DQN算法中的Q值維數(shù)為4，即可執(zhí)行動作的個數(shù)為4個，分別是上、下、左、右。由于柵格地圖在向訓(xùn)練環(huán)境轉(zhuǎn)換時，柵格地圖中的圓形障礙物并不能占滿整個柵格。但由于可執(zhí)行動作的僅有上下左右，為了盡可能地讓規(guī)劃的路徑不經(jīng)過障礙物，只要柵格地圖中的障礙物映射到訓(xùn)練環(huán)境時占據(jù)訓(xùn)練環(huán)境中的部分柵格，在訓(xùn)練時該柵格會被標(biāo)記為障礙物。

圖17 3 種仿真訓(xùn)練環(huán)境Fig.17 Three simulation training environments

為了加快算法的收斂，本文中DQN 算法的獎懲函數(shù)公式如式（19）所示：

其中：dis 代表當(dāng)前狀態(tài)距離終點的距離。為了能讓算法收斂，在3 種仿真訓(xùn)練環(huán)境中分別訓(xùn)練了100 萬步、50 萬步、150 萬步。對訓(xùn)練得到的模型進行測試，其結(jié)果如圖18 所示。同時根據(jù)相應(yīng)的縮放比例還原規(guī)劃路徑的長度，具體數(shù)據(jù)如表3 所示，其中，DQN 算法中的訓(xùn)練和測試的硬件平臺為Intel?Xeon?Silver 4114 2.2 GHz CPU 2.19 GHz CPU（2 處理器），RAM 128 GB 服務(wù)器。其余算法的硬件平臺為Intel?CoreTMi5-5200U 2.2 GHz CPU，RAM 8 GB筆記本。

表3 5 種不同算法下規(guī)劃路徑的長度和時間Table 3 The length and time of planning path under five different algorithms

圖18 3 種環(huán)境的測試結(jié)果Fig.18 Test results of three environments

從上述的實驗結(jié)果可知，ATIRRT*算法可以有效地減少路徑的生成時間和路徑長度。相較于Informed-RRT*算法，ATIRRT*算法在3 種地圖中所規(guī)劃的路徑長度分別縮短了4%、7%、11%，在時間上分別減少了63%、52%、20%。相較于STIRRT*算法，ATIRRT*算法在3 種地圖中所規(guī)劃的路徑長度分別縮短了6%、13%、16%，在時間上分別減少了16%、8%、6%。相較于DQN 算法在3 種地圖中所規(guī)劃的路徑長度分別縮短了12%、20%、16%。

同時DQN 算法在不同的場景進行路徑規(guī)劃時，需要重新訓(xùn)練，訓(xùn)練時間較長。因此，針對環(huán)境地圖變化的場景其往往很難快速地找到有效的路徑。而針對同一場景，其通過訓(xùn)練后，可以做到更換不同的起止點快速地規(guī)劃出路徑，但是其訓(xùn)練的時間會大幅增加。

綜上所述，本文所提出的算法在不同的仿真地圖環(huán)境中均取得了良好的效果，同時在更換地圖場景后可以快速地適應(yīng)新的場景，找到規(guī)劃路徑。

4 結(jié)束語

本文提出一種應(yīng)用拓?fù)浣Y(jié)構(gòu)的高效路徑規(guī)劃算法，通過自適應(yīng)閾值消除路徑骨架上提取的冗余特征點，并給出非單一父節(jié)點的連接方式加強交叉支路上的拓?fù)涔?jié)點間的聯(lián)系。根據(jù)節(jié)點擴充策略增加相鄰?fù)負(fù)涔?jié)點間的節(jié)點數(shù)量以加快優(yōu)化算法的收斂，定義相關(guān)約束條件將初始路徑進行分段并進行逐段優(yōu)化。仿真實驗結(jié)果表明，該算法具有一定的可擴展性，相較于傳統(tǒng)算法在路徑平滑性及規(guī)劃路徑的效率上都有不同程度的提升。本文算法將初始路徑分段并進行逐段優(yōu)化，可以縮短時間，但是對于路徑長度的優(yōu)化只是在局部進行，下一步將障礙物的邊緣輪廓添加相關(guān)約束條件，以使規(guī)劃出的路徑達到全局最優(yōu)。