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

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

0 概述

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

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

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

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

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

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

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

1 STIRRT＊算法

STIRRT*算法相較于RRT 算法有以下4 點(diǎn)改進(jìn)：

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

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

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

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

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

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

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

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

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

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

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

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

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

2）對(duì)矩陣M中的4 個(gè)元素進(jìn)行高斯濾波，從而消除一些冗余的點(diǎn)和凸起，得到新的矩陣M。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 節(jié)點(diǎn)擴(kuò)充策略

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

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

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

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

圖6 拓?fù)涔?jié)點(diǎn)擴(kuò)充Fig.6 Topology node expansion

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

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

2.3 橢圓采樣策略

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

表1 角點(diǎn)和拓?fù)涔?jié)點(diǎn)消耗的時(shí)間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

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

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

圖10、圖11、圖12 為隨機(jī)抽取地圖1、地圖2、地圖3 中一組對(duì)比實(shí)驗(yàn)結(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

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

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

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

將分段優(yōu)化結(jié)果與RRT*、Informed RRT*和STIRRT*算法進(jìn)行比較。為了盡可能地在較短的時(shí)間內(nèi)得到較優(yōu)的規(guī)劃路徑，令每次優(yōu)化的迭代次數(shù)為2 000 次。由于RRT*和Informed RRT*兩種算法都是基于RRT 算法在尋找初始路徑的同時(shí)進(jìn)行優(yōu)化，區(qū)別在于兩種算法找到初始路徑后，其隨機(jī)擴(kuò)展的采樣空間不同。因此，根據(jù)RRT 算法得到初始路徑的平均值再加上分布優(yōu)化的迭代次數(shù)，這樣就保證了其探索次數(shù)相同，其結(jié)果如圖14、圖15、圖16 所示。同時(shí)，本文將提出的算法和文獻(xiàn)［7］深度強(qiáng)化學(xué)習(xí)（DQN）算法進(jìn)行比較。為了減少深度強(qiáng)化學(xué)習(xí)算法的訓(xùn)練時(shí)間，加快算法的收斂。本文根據(jù)實(shí)驗(yàn)場景中的柵格地圖，對(duì)其按相應(yīng)的比例進(jìn)行縮放以建立訓(xùn)練場景。為此，根據(jù)障礙物在柵格地圖上的占位信息建立如圖17 所示的仿真模型。DQN 算法訓(xùn)練和測試時(shí)使用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＊算法的對(duì)比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＊算法對(duì)比結(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＊算法對(duì)比結(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版本進(jìn)行編程實(shí)現(xiàn)。其中圖17（b）、圖17（d）、圖17（f）分別對(duì)應(yīng)圖17（a）、圖17（c）、圖17（e）建立的仿真訓(xùn)練環(huán)境。本文中對(duì)比的DQN算法中的Q值維數(shù)為4，即可執(zhí)行動(dòng)作的個(gè)數(shù)為4個(gè)，分別是上、下、左、右。由于柵格地圖在向訓(xùn)練環(huán)境轉(zhuǎn)換時(shí)，柵格地圖中的圓形障礙物并不能占滿整個(gè)柵格。但由于可執(zhí)行動(dòng)作的僅有上下左右，為了盡可能地讓規(guī)劃的路徑不經(jīng)過障礙物，只要柵格地圖中的障礙物映射到訓(xùn)練環(huán)境時(shí)占據(jù)訓(xùn)練環(huán)境中的部分柵格，在訓(xùn)練時(shí)該柵格會(huì)被標(biāo)記為障礙物。

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

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

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

表3 5 種不同算法下規(guī)劃路徑的長度和時(shí)間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

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

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

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

4 結(jié)束語

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