張俊豪，潘樹國，高旺，郭芃，王萍，胡鵬

(東南大學(xué)儀器科學(xué)與工程學(xué)院,南京 210096)

0 引言

近年來，得益于電子通信技術(shù)、智能控制技術(shù)和智能計(jì)算技術(shù)等先進(jìn)科技領(lǐng)域研究的突飛猛進(jìn)，無人駕駛技術(shù)在世界各國得到快速發(fā)展.其中，路徑規(guī)劃是實(shí)現(xiàn)無人駕駛的重要技術(shù)之一，也是無人駕駛車輛能否安全并有效地抵達(dá)目的地的關(guān)鍵.

路徑規(guī)劃是指在規(guī)定區(qū)域內(nèi)規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)解路徑，且要保證與障礙物無碰撞，并盡可能保證路徑短、平滑度高、規(guī)劃時(shí)間短[1].目前，路徑規(guī)劃算法可以分為傳統(tǒng)路徑規(guī)劃算法和智能仿生路徑規(guī)劃算法兩類，其中傳統(tǒng)路徑規(guī)劃算法有A*(A star)算法、快速搜索隨機(jī)樹算法(rapidly-exploring random trees，RRT)、人工勢場算法、動態(tài)A*算法(dynamic A*，D*)等；智能仿生路徑規(guī)劃算法有蟻群算法、粒子群算法、遺傳算法等.在這些算法中，RRT算法是一種基于采樣的算法，因其具有概率完備性的特點(diǎn)而被廣泛應(yīng)用于機(jī)器人路徑規(guī)劃中.

近年來，針對RRT 算法收斂速度慢、隨機(jī)性較大的缺點(diǎn)，許多學(xué)者都提出了改進(jìn)RRT 算法的措施.Karaman等[2]提出 RRT*算法，該算法引入了重新布線的思想，通過重新選取路徑的父節(jié)點(diǎn)，使得得到的路徑最短；彭君[3]提出了基于變權(quán)重勢場的改進(jìn)RRT 算法和基于模糊邏輯的改進(jìn)RRT*算法，前者通過采用KD-Tree 算法(k-dimensional tree)和變權(quán)重的人工勢場法的規(guī)劃策略，解決了RRT 算法在路徑規(guī)劃時(shí)收斂速度慢和避障效果較差的問題，后者在RRT*算法的基礎(chǔ)上加入了模糊邏輯策略，提高了路徑質(zhì)量；Zucker等[4]提出了MP-RRT 算法，該算法通過偏置采樣分布并重新使用先前規(guī)劃迭代中的分支來實(shí)現(xiàn)動態(tài)環(huán)境中的實(shí)時(shí)規(guī)劃；Otte等[5]提出RRTX算法，它在整個(gè)導(dǎo)航過程中對相同的搜索圖進(jìn)行了改進(jìn)，以處理不可預(yù)測的移動障礙物.

現(xiàn)有的算法大多聚焦于改進(jìn)各個(gè)算法本身，專注于對算法進(jìn)行優(yōu)化和結(jié)合，但卻忽略了實(shí)際情況中環(huán)境和無人車輛本身的約束.在狹長空間中，可行區(qū)域狹小，隨機(jī)樹很容易陷入局部困境，從而導(dǎo)致規(guī)劃效率和成功率較低，影響無人車輛的正常運(yùn)行.另外，在狹長空間中，無人車輛的尺寸不可忽視，這對于路徑則有著更高的要求.

在已有研究成果的基礎(chǔ)上，結(jié)合RRT 算法本身的缺陷以及無人車輛和環(huán)境的約束，本文提出了一種改進(jìn)RRT 算法.該算法首先通過目標(biāo)動態(tài)概率采樣策略，將終點(diǎn)信息加入了RRT 算法隨機(jī)采樣的過程里，減少了RRT 算法隨機(jī)采樣的盲目性；之后利用人工勢場對RRT 算法中的隨機(jī)樹進(jìn)行引導(dǎo)，使其更容易接近終點(diǎn)；然后算法考慮了傳統(tǒng)RRT 算法中步長固定導(dǎo)致的規(guī)劃效率低的問題，以一種動態(tài)步長代替原有的固定步長，提高了規(guī)劃效率；最后，算法考慮了實(shí)際情況中路徑的轉(zhuǎn)角限制和車輛的碰撞限制，提高了路徑的平滑度和安全性.

1 傳統(tǒng)人工勢場引導(dǎo)下的RRT 算法

傳統(tǒng)的RRT 算法是一種基于采樣的路徑規(guī)劃算法，以一個(gè)初始的根節(jié)點(diǎn)為起點(diǎn)，通過隨機(jī)采樣的方法在空間搜索，然后添加子節(jié)點(diǎn)來不斷擴(kuò)展隨機(jī)樹.當(dāng)目標(biāo)點(diǎn)進(jìn)入隨機(jī)樹后，隨機(jī)樹擴(kuò)展立即停止，此時(shí)能找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑.

人工勢場法(artificial potential field，APF)是一種人為的在地圖中添加一個(gè)特殊的場來引導(dǎo)無人車輛從起點(diǎn)到終點(diǎn)的路徑規(guī)劃算法.

RRT 算法最大的優(yōu)點(diǎn)在于其搜索能力強(qiáng)，能夠在時(shí)間充足的情況下搜索整個(gè)空間，從而得到一條可行路徑，但其搜索的隨機(jī)性也帶來了搜索效率低的缺陷.而APF 則擁有目標(biāo)導(dǎo)向的作用，將兩者結(jié)合，人工勢場能夠在一定程度上引導(dǎo)隨機(jī)樹向目標(biāo)點(diǎn)擴(kuò)展，同時(shí)RRT 算法也能在一定程度上解決APF 容易陷入局部最優(yōu)解的情況.

圖1 為人工勢場引導(dǎo)下的RRT 算法(APF-RRT算法).

圖1 APF-RRT 算法示意圖

其算法流程圖如圖2 所示.

2 算法優(yōu)化及改進(jìn)

2.1 改進(jìn)RRT 算法

2.1.1 自適應(yīng)目標(biāo)概率采樣

在傳統(tǒng)的RRT 算法中，雖然隨機(jī)樹能夠保證在時(shí)間充足的情況下搜索出一條可行路徑，但這樣的搜索具有盲目性，會使得隨機(jī)樹產(chǎn)生許多無用的節(jié)點(diǎn)，另外，當(dāng)隨機(jī)樹擴(kuò)展到目標(biāo)點(diǎn)附近時(shí)，由于搜索的隨機(jī)性，會出現(xiàn)多次擴(kuò)展都無法擴(kuò)展至目標(biāo)點(diǎn)范圍的情況，降低了規(guī)劃效率.為解決這樣的問題，本文采用自適應(yīng)目標(biāo)概率采樣的方法為隨機(jī)樹增加目標(biāo)導(dǎo)向性，使得其能夠快速擴(kuò)展到目標(biāo)點(diǎn)附近.傳統(tǒng)的目標(biāo)概率采樣策略的做法，在采樣階段，以一定的概率Pgoal選用目標(biāo)點(diǎn)Xgoal作為隨機(jī)采樣點(diǎn)Xrand，即

式中：Xrandom為原算法隨機(jī)產(chǎn)生的采樣點(diǎn)；P為0～1的一個(gè)隨機(jī)值.

傳統(tǒng)的目標(biāo)概率采樣策略能夠很好地對隨機(jī)樹產(chǎn)生一定的目標(biāo)引導(dǎo)作用，但是，當(dāng)?shù)貓D中的障礙物較多且較為密集，尤其是狹長路段時(shí)，直接使用傳統(tǒng)的目標(biāo)概率采樣策略不僅不會提高規(guī)劃效率，反而會出現(xiàn)陷入局部困境的問題.為了解決這樣的問題，本文采用目標(biāo)動態(tài)概率采樣的方法來應(yīng)對障礙物密集的情況.具體的做法是將原有的固定的概率Pgoal調(diào)整為自適應(yīng)的值，即當(dāng)隨機(jī)樹擴(kuò)展到障礙物密集的區(qū)域時(shí)則降低Pgoal，使得隨機(jī)樹能夠更好地逃離狹長區(qū)域.即

式中：niter為迭代次數(shù)；nvoid為無效節(jié)點(diǎn)的個(gè)數(shù)(即與障礙物發(fā)生碰撞的節(jié)點(diǎn)個(gè)數(shù))；Pmax為概率采樣閾值的最大值(一般為0.1～0.3).

2.1.2 動態(tài)步長

在RRT 算法中，由于步長的固定性，導(dǎo)致算法不能適應(yīng)地圖中的各種情況，從而使得生成路徑的平滑度和效率較低.當(dāng)步長較大時(shí)，生成路徑的效率較高，當(dāng)產(chǎn)生路徑的平滑度較低，并且當(dāng)遇到狹長區(qū)域時(shí)會陷入局部困境從而提高迭代次數(shù)；當(dāng)步長較小時(shí)，在遇到狹長區(qū)域時(shí)能夠快速逃離障礙物區(qū)域并且生成路徑較為平滑，但規(guī)劃效率較低.因此，為了解決這種矛盾，本文采用一種動態(tài)步長的方法，使得步長隨著障礙物的密集程度進(jìn)行變化，能夠有效解決大步長和小步長之間的矛盾，提高RRT 算法適應(yīng)各種環(huán)境的能力，從而使得其能夠更好地應(yīng)用于狹長區(qū)域.具體的動態(tài)步長 λ為

式中：Sobs為所有障礙物與上述矩形相交部分的面積；Stotal為以Xrand和Xnear為對角線且長和寬平行于坐標(biāo)軸的矩形面積；λori為設(shè)置的初始最大步長.示意圖如圖3 所示.

圖3 動態(tài)步長策略示意圖

其中，以Xrand和Xnear為對角 線的矩 形面積即為Stotal，紅色區(qū)域的面積即為Sobs.通過這樣的動態(tài)步長，能夠讓隨機(jī)樹在狹長空間減少步長，從而快速逃離該區(qū)域；在空曠的區(qū)域，步長會隨之增大，從而提高規(guī)劃效率.

2.2 路徑約束條件

常見的路徑規(guī)劃算法大多關(guān)注于算法的優(yōu)化和結(jié)合，而忽略了實(shí)際情況中的一些動力學(xué)約束條件，這些約束條件是讓無人駕駛車輛能夠正常運(yùn)作的關(guān)鍵條件，因此并不能忽略.例如實(shí)際無人車輛不能作為質(zhì)點(diǎn)進(jìn)行處理，而必須考慮其本身大小，另外實(shí)際無人車輛的底盤有最大轉(zhuǎn)角的限制，過大的路徑轉(zhuǎn)角會使得無人車輛花費(fèi)更多的動力甚至出現(xiàn)無法轉(zhuǎn)向的情況，因此本節(jié)將從碰撞約束和路徑轉(zhuǎn)角約束兩個(gè)方面展開.

2.2.1 碰撞約束

安全性是路徑規(guī)劃的基本要求之一，為了讓生成的路徑能夠使得無人車輛安全地抵達(dá)終點(diǎn)，必須對路徑點(diǎn)進(jìn)行碰撞約束.常用的碰撞約束是判斷以Xrand和Xnear為端點(diǎn)和線段與障礙物是否相交來完成的，但是這樣方法的前提是將無人車輛看作質(zhì)點(diǎn)，然而實(shí)際情況中如狹長空間中，無人車輛的大小無法忽略，常用的碰撞約束方法則不適用.

本文考慮了無人車輛的大小，將其視為一個(gè)矩形，在每產(chǎn)生一個(gè)Xnew節(jié)點(diǎn)之后，對其進(jìn)行碰撞檢測，若以Xnew為中心的矩形與障礙物有相交的情況，則舍棄該節(jié)點(diǎn).但是，當(dāng)步長較大時(shí)，會出現(xiàn)圖4 所示的情況.

圖4 步長較大時(shí)兩節(jié)點(diǎn)連線與障礙物相交

這種情況的Xnew和Xnear節(jié)點(diǎn)均滿足碰撞約束，但是，他們的連線與障礙物發(fā)生的相交.為了減少這種情況，取Xnew和Xnear的中點(diǎn)Xmid，對Xmid同樣進(jìn)行一次碰撞檢測，即當(dāng)Xnew和Xmid均通過碰撞檢測時(shí)才認(rèn)為Xnew是符合碰撞約束的，從而提高路徑的安全性.

2.2.2 路徑轉(zhuǎn)角約束

路徑的轉(zhuǎn)角約束對于無人車輛也是十分有必要的，這樣不僅能夠減少無人車輛轉(zhuǎn)向所消耗的動力，也能提高路徑的平滑度.路徑的轉(zhuǎn)角如圖5 所示.

圖5 路徑轉(zhuǎn)角

其中，Xparent為Xrand的父節(jié)點(diǎn)，θ 為轉(zhuǎn)角，根據(jù)余弦定理，有

式中，lXnear,Xparent、lXnear,Xnew和lXnew,Xparent分別表示三個(gè)節(jié)點(diǎn)之間的距離.當(dāng)產(chǎn)生Xnew時(shí)，若 θ 的值大于所允許的最大轉(zhuǎn)角限制 θmax，則舍棄該節(jié)點(diǎn)，從而保證轉(zhuǎn)角約束.

2.2.3 限制區(qū)域隨機(jī)轉(zhuǎn)向

上述的路徑轉(zhuǎn)角約束對于無人車輛的作用不可忽視，但是，這樣的約束對于規(guī)劃效率帶來了很大的困難，由于RRT 算法的隨機(jī)性，滿足轉(zhuǎn)角約束的節(jié)點(diǎn)較少，這就導(dǎo)致每次規(guī)劃都將花費(fèi)較多的迭代次數(shù)，大大降低了規(guī)劃效率，這在狹長的空間尤為明顯.狹長空間由于可行區(qū)域較窄，轉(zhuǎn)角的限制會使得隨機(jī)樹陷入局部困境.

為了解決這樣的問題，本文提出了限制區(qū)域隨機(jī)轉(zhuǎn)向的策略.即當(dāng)產(chǎn)生的Xnew不滿足轉(zhuǎn)角約束的時(shí)候，在以Xnear為頂點(diǎn)，2θmax為張角的范圍內(nèi)隨機(jī)選擇一個(gè)方向，在這個(gè)方向以步長產(chǎn)生新的Xnew，這樣產(chǎn)生的Xnew滿足轉(zhuǎn)角限制，因此只需要進(jìn)行碰撞檢測即可.示意圖如圖6 所示.

圖6 限制區(qū)域隨機(jī)轉(zhuǎn)向策略示意圖

其中，Xold為不符合轉(zhuǎn)角約束的節(jié)點(diǎn)，θrand為0～θmax隨機(jī)選取的角度.隨機(jī)轉(zhuǎn)向的策略為隨機(jī)樹在障礙物密集或狹長的空間提供了更多擴(kuò)展到可行區(qū)域的機(jī)會.

2.3 算法總流程

圖7 為算法整體流程.

圖7 改進(jìn)RRT 算法流程圖

算法的具體步驟如下：

1)設(shè)置起點(diǎn)Xstart，終點(diǎn)Xgoal，可行區(qū)域O，障礙物rect，初始最大步長 stepori 等參數(shù)；

2)在可行區(qū)域O內(nèi)采用自適應(yīng)目標(biāo)概率采樣得到Xrand；

3)遍歷隨機(jī)樹上的所有節(jié)點(diǎn)，計(jì)算得出距離Xrand最近的節(jié)點(diǎn)Xnear；

4)根據(jù)動態(tài)步長策略得到新的步長step；

5)計(jì)算Xnear所受的引力場和斥力場的合力F；

6)計(jì)算Xnear指向Xrand的單位 向量并和F的單位向量進(jìn)行矢量疊加得到新的向量Fnew；

7)以Fnew方向，step 為步長擴(kuò)展生成新的節(jié)點(diǎn)Xnew；

8)若Xnew不滿足碰撞約束，則舍棄Xnew；若Xnew滿足碰撞約束且滿足轉(zhuǎn)角限制，則將Xnew作為新的樹節(jié)點(diǎn)加入到隨機(jī)樹中；若Xnew滿足碰撞約束但不滿足轉(zhuǎn)角限制，采用限制區(qū)域隨機(jī)轉(zhuǎn)向策略產(chǎn)生新的Xnew，若新的Xnew不滿足碰撞約束，則舍棄Xnew；若新的Xnew滿足碰撞約束則將Xnew作為新的樹節(jié)點(diǎn)加入到隨機(jī)樹中；

9)重復(fù)步驟2)～8)直到新的樹節(jié)點(diǎn)與終點(diǎn)的距離小于設(shè)定的閾值，之后回溯樹節(jié)點(diǎn)得到路徑.

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

本文利用Python 語言，基于Windows10 下的PyCharm Community (Edition 2023.1)編寫程序.分別在簡單環(huán)境、密集障礙物環(huán)境以及狹長空間三種情況下對現(xiàn)有算法和改進(jìn)RRT 算法進(jìn)行仿真對比分析.為了減少結(jié)果的隨機(jī)性，在三種不同環(huán)境下分別運(yùn)行不同算法各100次，取平均迭代次數(shù)、平均路徑長度、平均規(guī)劃時(shí)間、成功率作為路徑規(guī)劃的評價(jià)指標(biāo).另外，選用20×20 的矩形作為仿真地圖，并在仿真地圖中根據(jù)不同環(huán)境設(shè)置不同障礙物進(jìn)行測試.

3.1 參數(shù)設(shè)置

考慮初始設(shè)置參數(shù)對實(shí)驗(yàn)結(jié)果的影響，采用不同的初始最大步長(0.5、1 和1.5)和不同的最大迭代次數(shù)(2 000、5 000 和10 000)對約束條件下的改進(jìn)RRT算法和APF-RRT 算法在狹長空間進(jìn)行測試從而選取適合的初始參數(shù).

首先固定最大迭代次數(shù)為10 000，測試不同的初始最大步長對結(jié)果的影響(為了減少隨機(jī)性，對不同算法分別運(yùn)行100 次取平均值)，結(jié)果如圖8 所示.

圖8 不同初始最大步長下算法的實(shí)驗(yàn)結(jié)果折線圖

結(jié)果表明，隨著步長的增大，兩種算法平均迭代次數(shù)和規(guī)劃時(shí)間呈下降趨勢，并且在步長為0.5 時(shí)成功率有所下降，而平均路徑長度則變化較小.因此，選用1.5 作為初始最大步長能同時(shí)保證算法的成功率和效率，減少其對算法性能的影響.

其次，固定初始最大步長為1.5，測試不同的最大迭代次數(shù)對結(jié)果的影響，結(jié)果如圖9 所示.

圖9 不同最大迭代次數(shù)下算法的實(shí)驗(yàn)結(jié)果折線圖

結(jié)果表明，當(dāng)最大迭代次數(shù)為2 000 和5 000時(shí)，兩種算法的成功率較低，因此選用10 000 作為最大迭代次數(shù)來減少其對其算法性能影響.

3.2 簡單環(huán)境

由圖10 可知，在簡單環(huán)境下，傳統(tǒng)的APF-RRT算法得到路徑質(zhì)量較差，會出現(xiàn)路徑貼合障礙物的情況發(fā)生，這在實(shí)際中是不允許的，而在約束條件下的APF-RRT 算法和改進(jìn)RRT 算法均生成了合理、安全的路徑.由表1 可知，在沒有約束條件時(shí)，改進(jìn)RRT算法相較于APF-RRT 算法在平均迭代次數(shù)、平均路徑長度和平均規(guī)劃時(shí)間方面分別降低了34.99%、4.89%、40.67%；在有約束條件時(shí)，改進(jìn)RRT 算法相較于APF-RRT 算法在在上述指標(biāo)里分別降低了44.51%、2.23%、14.49%.

表1 簡單環(huán)境下不同算法實(shí)驗(yàn)結(jié)果數(shù)據(jù)

3.3 復(fù)雜障礙物環(huán)境

如圖11 所示，在復(fù)雜障礙物的情況下，傳統(tǒng)的APF-RRT 算法依然產(chǎn)生了貼合障礙物的情況.由表2可知，在沒有約束條件時(shí)，改進(jìn)RRT 算法相較于APF-RRT 算法在平均迭代次數(shù)、平均路徑長度和平均規(guī)劃時(shí)間方面分別降低了16.13%、4.05%、2.72%；在有約束條件時(shí)，改進(jìn)RRT 算法相較于APF-RRT 算法在在上述指標(biāo)里分別降低了11.99%、5.09%、0.85%.

表2 復(fù)雜障礙物環(huán)境下不同算法實(shí)驗(yàn)結(jié)果數(shù)據(jù)

圖11 復(fù)雜障礙物環(huán)境下不同算法所得路徑圖

3.4 狹長空間

在狹長空間中，改進(jìn)RRT 算法能夠在自適應(yīng)目標(biāo)概率采樣策略和動態(tài)步長策略下快速通過狹長區(qū)域，提高了規(guī)劃效率.首先由圖12 可知，在增加了約束條件后，路徑質(zhì)量得到明顯提升，并且減少了路徑貼合障礙物的情況.由表3 可知，在沒有約束條件時(shí)，改進(jìn)RRT 算法相較于APF-RRT 算法在平均迭代次數(shù)、平均路徑長度方面分別降低了35.73%、0.1%；在有約束條件時(shí)，改進(jìn)RRT 算法相較于APF-RRT 算法在上述指標(biāo)分別降低了33.09%、0.06%，另外在平均規(guī)劃時(shí)間方面也降低了6.44%.

表3 狹長空間下不同算法實(shí)驗(yàn)結(jié)果數(shù)據(jù)

圖12 狹長空間下不同算法所得路徑圖

當(dāng)空間更加狹窄的時(shí)候，改進(jìn)RRT 算法更能體 現(xiàn)其優(yōu)勢，如圖13 所示.

圖13 狹長空間(極端狹窄)下不同算法所得路徑圖

由表4 可知，在約束條件下，RRT 和傳統(tǒng)APFRRT 算法的規(guī)劃成功率極低，其中RRT 算法甚至在100 次規(guī)劃無一次成功，而改進(jìn)RRT 算法則有著55%的成功率.這說明當(dāng)隨機(jī)樹進(jìn)入狹長空間時(shí)，自適應(yīng)目標(biāo)概率采樣策略降低了采樣概率，同時(shí)，動態(tài)步長策略降低采樣步長，從而使得隨機(jī)樹能夠更好地逃離狹長區(qū)域，完成路徑規(guī)劃.

表4 狹長空間(極端狹窄)下不同算法實(shí)驗(yàn)結(jié)果數(shù)據(jù)

4 結(jié)束語

為了解決RRT 算法在狹長空間路徑規(guī)劃時(shí)可能出現(xiàn)的隨機(jī)性過大、迭代次數(shù)較大的問題，本文提出了改進(jìn)RRT 算法，其中自適應(yīng)目標(biāo)概率采樣策略和動態(tài)步長策略有效改善了在障礙物復(fù)雜或狹長空間中隨機(jī)樹容易陷入局部困境的問題，提高了規(guī)劃效率.尤其在狹長空間的情況下，自適應(yīng)目標(biāo)概率采樣策略和動態(tài)步長策略能夠很好地引導(dǎo)隨機(jī)樹逃離障礙物區(qū)域，使得隨機(jī)樹快速到達(dá)終點(diǎn)附近.

另外，本文還考慮了實(shí)際情況中的路徑約束，對路徑進(jìn)行了碰撞約束和轉(zhuǎn)角約束，并針對轉(zhuǎn)角約束會導(dǎo)致迭代次數(shù)激增的問題提出了一種限制區(qū)域內(nèi)隨機(jī)轉(zhuǎn)向的策略，這種策略能夠有效降低迭代次數(shù)，提高規(guī)劃速度.

最后，通過仿真實(shí)驗(yàn)，驗(yàn)證了改進(jìn)RRT 算法在簡單環(huán)境、復(fù)雜障礙物環(huán)境、狹長空間中應(yīng)用的有效性.在狹長空間中，改進(jìn)RRT 算法在約束條件下相較于傳統(tǒng)APF-RRT 算法迭代次數(shù)降低了33.09%，路徑長度減少了0.06%，規(guī)劃時(shí)間減少了6.44%，另外在極端狹窄的情況下，改進(jìn)RRT 算法相較于傳統(tǒng)APFRRT 算法規(guī)劃成功率從2%提高到了55%，提升效果十分明顯.此外在簡單環(huán)境和復(fù)雜障礙物環(huán)境下該算法均有提升.