華 洪,張志安,施振穩(wěn),陳冠星
南京理工大學(xué) 機(jī)械工程學(xué)院,南京210094
路徑規(guī)劃技術(shù)是移動(dòng)機(jī)器人技術(shù)的核心內(nèi)容之一,近年來(lái)在許多領(lǐng)域都有著廣泛的應(yīng)用,例如在制造領(lǐng)域中應(yīng)用于生產(chǎn)線上下料搬運(yùn)的自主引導(dǎo)運(yùn)輸車,在電商工廠中應(yīng)用廣泛的無(wú)人送貨小車,在家中負(fù)責(zé)清掃地板的掃地機(jī)器人等。它們都是在不同的環(huán)境下,按照指定要求,以感知、建模、規(guī)劃、執(zhí)行的標(biāo)準(zhǔn)路徑規(guī)劃方法執(zhí)行[1-2]。根據(jù)路徑規(guī)劃環(huán)境是否已知,可將路徑規(guī)劃算法分為全局路徑規(guī)劃與局部路徑規(guī)劃。根據(jù)路徑規(guī)劃環(huán)境中是否存在動(dòng)態(tài)障礙物,可將路徑規(guī)劃分為靜態(tài)路徑規(guī)劃與動(dòng)態(tài)路徑規(guī)劃[3-4]。
A*算法作為一種全局的靜態(tài)路徑規(guī)劃搜索算法,具有魯棒性好、反應(yīng)快等優(yōu)點(diǎn),但受算法原理限制,規(guī)劃出來(lái)的路徑折線較多,且隨著環(huán)境變大搜索代價(jià)呈指數(shù)增長(zhǎng)。文獻(xiàn)[5]提出對(duì)A*算法中的估價(jià)函數(shù)進(jìn)行指數(shù)加權(quán)處理,并且對(duì)路徑進(jìn)行5 次多項(xiàng)式平滑處理,有效解決了搜索效率低和路徑不平滑的問(wèn)題,但機(jī)器人并不具備局部規(guī)劃能力與動(dòng)態(tài)避障能力,在密集區(qū)域內(nèi)容易陷入極小值。文獻(xiàn)[6]提出在A*全局規(guī)劃的基礎(chǔ)上,采用自適應(yīng)步長(zhǎng)調(diào)節(jié)方法的人工勢(shì)場(chǎng)法進(jìn)行局部路徑優(yōu)化,且在局部路徑規(guī)劃的同時(shí)增設(shè)虛擬目標(biāo)點(diǎn),讓機(jī)器人能夠逃離陷進(jìn)點(diǎn),但機(jī)器人并不具備動(dòng)態(tài)避障能力,且規(guī)劃出來(lái)的路徑可能非最優(yōu)。文獻(xiàn)[7]針對(duì)機(jī)器人在類似超市環(huán)境等密集區(qū)域下的路徑規(guī)劃問(wèn)題,在A*算法基礎(chǔ)上提出了一種動(dòng)態(tài)環(huán)境下機(jī)器人多狀態(tài)自主導(dǎo)航方法,在狹窄通道與動(dòng)態(tài)障礙物的環(huán)境下,機(jī)器人都有良好的避障策略與局部規(guī)劃能力,但其考慮參數(shù)過(guò)多,容易導(dǎo)致全局路徑非最優(yōu)。
針對(duì)以上方法的特點(diǎn)與不足,本文提出一種基于A*算法的改進(jìn)的路徑規(guī)劃方法。首先在傳統(tǒng)的全局A*算法之上將路徑節(jié)點(diǎn)進(jìn)行優(yōu)化,按照刪除冗余點(diǎn)準(zhǔn)則與新增節(jié)點(diǎn)準(zhǔn)則,減小全局規(guī)劃任務(wù),提高效率;再結(jié)合滾動(dòng)窗口法的思想,在既定的滾動(dòng)周期內(nèi),結(jié)合全局規(guī)劃的節(jié)點(diǎn)信息,按照一定準(zhǔn)則在局部環(huán)境中生成子目標(biāo);在該環(huán)境內(nèi)加入動(dòng)態(tài)避障控制策略的同時(shí)再進(jìn)行“全局路徑規(guī)劃”,根據(jù)傳感器探測(cè)范圍,如果環(huán)境范圍過(guò)大再生成子局部環(huán)境,依次遞歸,進(jìn)行多重A*算法,直至機(jī)器人能夠到達(dá)子目標(biāo)點(diǎn)。在Μatlab 軟件中建立柵格圖進(jìn)行仿真,仿真結(jié)果表明,相比于傳統(tǒng)的A*算法,改進(jìn)算法下機(jī)器人能在部分地圖信息未知的情況下實(shí)現(xiàn)局部路徑規(guī)劃,并擁有良好的避障功能,且消耗的時(shí)間更少,路徑的長(zhǎng)度更短。
A*算法是在Dijkstra 和BFS 兩種算法的基礎(chǔ)上提出的靜態(tài)網(wǎng)絡(luò)最優(yōu)路徑搜索方法,它是一種啟發(fā)式的搜索算法[8]。傳統(tǒng)A*算法僅適用于全局的靜態(tài)環(huán)境,同時(shí)由于啟發(fā)式搜索的原理限制,并不會(huì)考慮到路徑節(jié)點(diǎn)連線的平滑度。針對(duì)傳統(tǒng)A*算法的局限性與不足,進(jìn)行以下改進(jìn):
(1)優(yōu)化路徑節(jié)點(diǎn),平滑路徑軌跡;
(2)結(jié)合滾動(dòng)窗口法,綜合節(jié)點(diǎn)信息確定局部路徑規(guī)劃區(qū)域;
(3)加入避障控制策略,在局部路徑規(guī)劃過(guò)程中保證機(jī)器人在較短時(shí)間內(nèi)到達(dá)子目標(biāo)點(diǎn)。
傳統(tǒng)A*算法是以最短路線優(yōu)化目標(biāo),規(guī)劃出來(lái)的全局路徑在復(fù)雜的環(huán)境下通常會(huì)出現(xiàn)折線較多的情況,原因是其包含部分冗余點(diǎn)以及一些不必要的轉(zhuǎn)折點(diǎn),易導(dǎo)致機(jī)器人運(yùn)動(dòng)的角速度及向心加速度發(fā)生變化,現(xiàn)采取以下方法準(zhǔn)則對(duì)路徑節(jié)點(diǎn)進(jìn)行優(yōu)化:
刪除節(jié)點(diǎn)準(zhǔn)則:若當(dāng)前點(diǎn)與其父節(jié)點(diǎn)、下一節(jié)點(diǎn)在同一直線上,則判定該節(jié)點(diǎn)為可刪除節(jié)點(diǎn);若當(dāng)前點(diǎn)與其父節(jié)點(diǎn)、下一節(jié)點(diǎn)連線所成角大于α 小于180°,且父節(jié)點(diǎn)和下一節(jié)點(diǎn)的連線與不可通行區(qū)域無(wú)交點(diǎn),則判定該節(jié)點(diǎn)為可刪除節(jié)點(diǎn)。找出可刪除節(jié)點(diǎn)剔除后,重新遍歷新節(jié)點(diǎn),更新路徑。
新增節(jié)點(diǎn)準(zhǔn)則:若當(dāng)前節(jié)點(diǎn)和軌跡所成夾角與下一節(jié)點(diǎn)和軌跡所成夾角差值大于β,在兩節(jié)點(diǎn)中點(diǎn)處新增一個(gè)節(jié)點(diǎn),并將該節(jié)點(diǎn)沿軌跡法線方向以減小β 角的方向平移γ 個(gè)柵格單位,重新遍歷新節(jié)點(diǎn),確認(rèn)路徑與不可通行區(qū)域無(wú)交點(diǎn)后,更新路徑。
局部路徑規(guī)劃中移動(dòng)機(jī)器人需要知曉環(huán)境信息與自身位置信息,利用外界傳感器構(gòu)造滾動(dòng)窗口探測(cè)環(huán)境信息。本文結(jié)合滾動(dòng)窗口法與A*算法進(jìn)行局部路徑規(guī)劃的思想為:在構(gòu)造的柵格地圖環(huán)境中,設(shè)定起始點(diǎn)S、目標(biāo)點(diǎn)E、傳感器環(huán)境探測(cè)距離R、滾動(dòng)窗口Vision,在規(guī)劃完全局路徑后,結(jié)合節(jié)點(diǎn)信息與窗口范圍,選取若干局部路徑規(guī)劃區(qū)域與子目標(biāo)點(diǎn),在當(dāng)前窗口內(nèi)基于原規(guī)劃路徑實(shí)時(shí)進(jìn)行局部路徑規(guī)劃,到達(dá)子目標(biāo)點(diǎn)后更新局部路徑區(qū)域,重復(fù)以上過(guò)程,直到最后一個(gè)滾動(dòng)窗口刷新,到達(dá)目標(biāo)點(diǎn)E,完成路徑規(guī)劃。
下面在構(gòu)造的柵格地圖環(huán)境中,對(duì)某個(gè)局部路徑規(guī)劃窗口進(jìn)行數(shù)學(xué)建模。設(shè)柵格地圖的環(huán)境大小為m×n,柵格尺寸為δ,工作空間為柵格集合J ,機(jī)器人當(dāng)前位置柵格為sEn,目標(biāo)柵格為sEn+1,機(jī)器人傳感器探測(cè)距離R 視作矩形距離RV,如式(1)所示;滾動(dòng)窗口Jn 為以柵格sEn中心點(diǎn)為圓心,RV為半徑的圓形區(qū)域,d 為歐幾里德距離,滾動(dòng)窗口Vision 為滾動(dòng)窗口Jn內(nèi)柵格x 的集合,如式(2)所示。
由式(1)可知,本文研究的移動(dòng)機(jī)器人的探測(cè)柵格的規(guī)模t=RV/δ ,機(jī)器人的有效視野窗口Vision 的矩陣模型如下所示:
如圖1所示,機(jī)器人處在一個(gè)10×10的柵格環(huán)境中,設(shè)定柵格尺寸δ=1,RV=3,當(dāng)前機(jī)器人處于sEn點(diǎn),也是坐標(biāo)為(2,1)的柵格的中心,可計(jì)算得t=3,m=n=10。由式(2)和式(3)可推得機(jī)器人在該環(huán)境下的有效滾動(dòng)窗口視野柵格包括:Vision={(1,1);(2,1);(3,1);(4,1);(1,2);(2,2);(3,2);(4,2);(1,3);(2,3);(3,3)}。
圖1 滾動(dòng)窗口環(huán)境下的柵格環(huán)境模型
假設(shè)整個(gè)路徑規(guī)劃過(guò)程完成了n 次局部路徑規(guī)劃,第m 次局部路徑規(guī)劃的路徑為D(m),則全程的路徑可以表示為:
由式(4)可知,局部子目標(biāo)點(diǎn)sE 的選取對(duì)于路徑通往終點(diǎn)E 具有導(dǎo)向作用,需要建立起始點(diǎn)、局部子目標(biāo)點(diǎn)sE 與終點(diǎn)E 的聯(lián)系。因此,局部路徑規(guī)劃過(guò)程的關(guān)鍵點(diǎn)在于局部子目標(biāo)點(diǎn)的選取。結(jié)合全局路徑節(jié)點(diǎn),本文提出將局部子目標(biāo)點(diǎn)所處位置分為多種情況討論。
(1)局部子目標(biāo)點(diǎn)與節(jié)點(diǎn)重合但處于可行域內(nèi)
若當(dāng)前環(huán)境過(guò)小,將會(huì)出現(xiàn)目標(biāo)點(diǎn)E 在第一個(gè)滾動(dòng)窗口中的情況,此時(shí)目標(biāo)點(diǎn)E 與局部子目標(biāo)點(diǎn)重合,這種情況即為全局路徑規(guī)劃。若目標(biāo)點(diǎn)E 并未出現(xiàn)在第一個(gè)滾動(dòng)窗口中,則從起點(diǎn)開(kāi)始,選取在每個(gè)滾動(dòng)窗口中出現(xiàn)的序列號(hào)最大的節(jié)點(diǎn)作為第一個(gè)子目標(biāo)點(diǎn)。
(2)局部子目標(biāo)點(diǎn)與節(jié)點(diǎn)不重合但處于可行域內(nèi)
若在某個(gè)滾動(dòng)窗口中,因環(huán)境過(guò)大或在優(yōu)化全局路徑時(shí)刪除的序列點(diǎn)過(guò)多,導(dǎo)致序列點(diǎn)過(guò)于稀疏,傳感器檢測(cè)不到下一節(jié)點(diǎn)信息,此時(shí)新增的局部子目標(biāo)點(diǎn)設(shè)定于機(jī)器人滾動(dòng)窗口范圍Vision 與原規(guī)劃路徑的交點(diǎn)處。
(3)局部子目標(biāo)點(diǎn)處于不可行區(qū)域內(nèi)
若在某個(gè)滾動(dòng)窗口中,檢測(cè)到在全局路徑規(guī)劃之前并未記錄到的障礙物,且障礙物恰好與本應(yīng)設(shè)定的局部子目標(biāo)點(diǎn)重合,這種情況出現(xiàn)時(shí)需要加入新的障礙物信息,更新地圖,重新進(jìn)行全局路徑規(guī)劃,直至本次滾動(dòng)窗口的局部子目標(biāo)點(diǎn)設(shè)定在可行域內(nèi)。
下面給出局部子目標(biāo)選取的數(shù)學(xué)模型:
滾動(dòng)窗口Vision 的柵格集合可由式(2)和(3)推得:
其中,s ∈{1,2,…,n},Vision包含柵格總數(shù)為t2=(RV/δ)2。
由式(5)可以推出滾動(dòng)窗口視野中柵格點(diǎn)的坐標(biāo)集合為:
式中,X、Y 為柵格環(huán)境中所有柵格點(diǎn)的橫縱坐標(biāo)值所組成的集合。
滾動(dòng)窗口中任意柵格點(diǎn)到目標(biāo)點(diǎn)的距離函數(shù)為:
為了研究方便,移動(dòng)機(jī)器人在柵格環(huán)境中的移動(dòng)方向可看成是前、后、左、右、左前、右前、左后、右后8個(gè)方向,即從當(dāng)前柵格向鄰近柵格行徑有8 個(gè)方向,不妨設(shè)左前方是不可通行點(diǎn),則機(jī)器人可運(yùn)動(dòng)方向共有7 個(gè),可表示為:
當(dāng)前點(diǎn)柵格到相鄰柵格的距離可表示為:
由式(5)~(9)可推得,局部子目標(biāo)sE 選取規(guī)則的數(shù)學(xué)模型為:
移動(dòng)機(jī)器人如何在未知環(huán)境下自主躲避動(dòng)態(tài)障礙物一直是路徑規(guī)劃問(wèn)題的難點(diǎn)。本文為了研究方便,將動(dòng)態(tài)障礙物的運(yùn)動(dòng)做勻速直線運(yùn)動(dòng)處理,同時(shí)對(duì)于障礙物的體積和形狀做點(diǎn)化處理,即不考慮障礙物的具體尺寸和大小。
自主移動(dòng)機(jī)器人避障技術(shù)發(fā)展以來(lái),可將其避障控制結(jié)構(gòu)分為兩大類:反應(yīng)型控制結(jié)構(gòu)和慎思型控制結(jié)構(gòu)[9-10]。反應(yīng)型控制結(jié)構(gòu)是指機(jī)器人的感應(yīng)器在接收到障礙物信息后,執(zhí)行器直接執(zhí)行設(shè)定好的指令,即“感知-執(zhí)行”的過(guò)程,這種控制結(jié)構(gòu)優(yōu)點(diǎn)是反應(yīng)迅速,缺點(diǎn)是缺少全局觀。如文獻(xiàn)[11]與文獻(xiàn)[12]中采用的避障策略,在復(fù)雜環(huán)境下容易無(wú)法到達(dá)最優(yōu)點(diǎn)。慎思型控制結(jié)構(gòu)為“感知-規(guī)劃-執(zhí)行”,這種控制結(jié)構(gòu)的機(jī)器人一定要具備分布式的嵌入式控制結(jié)構(gòu),感應(yīng)器的信息首先傳入上層決策層,經(jīng)過(guò)決策層分析處理后才傳入執(zhí)行機(jī)構(gòu)做出動(dòng)作,這種控制結(jié)構(gòu)的智能化程度雖然提升了,但是控制結(jié)構(gòu)過(guò)于臃腫,往往在設(shè)備性能不高的情況下難以實(shí)現(xiàn)。如文獻(xiàn)[13]與文獻(xiàn)[14]就是這種控制結(jié)構(gòu),它將避障環(huán)境分為兩種情形,每種情形下障礙物不同的運(yùn)動(dòng)方向避障策略也截然不同,這在真實(shí)環(huán)境下是難以實(shí)現(xiàn)的。
結(jié)合前文中的路徑規(guī)劃方法,本文綜合反應(yīng)型控制結(jié)構(gòu)和慎思型控制結(jié)構(gòu)的優(yōu)點(diǎn),在局部路徑規(guī)劃過(guò)程中,基于云的協(xié)作定位系統(tǒng),使用卷積層和ConvLSTΜ層的組合算法確定動(dòng)態(tài)障礙物與局部路徑軌跡的關(guān)系[15],混合使用上述兩種避障控制結(jié)構(gòu)。
(1)動(dòng)態(tài)障礙物軌跡與路徑軌跡重合,如圖2和圖3所示。
圖2 障礙物軌跡與路徑方向相反
圖3 障礙物軌跡與路徑方向相同
圖中紅線表示機(jī)器人原定的路徑軌跡,紅色方塊(R)表示移動(dòng)機(jī)器人,藍(lán)色方塊(M)表示膨化處理后的障礙物,褐色物塊(X) 表示膨化處理后的碰撞位置。圖2表示障礙物的行進(jìn)方向與機(jī)器人的行進(jìn)方向相反,此時(shí)采用反應(yīng)型控制策略,機(jī)器人向路徑法線方向平移一個(gè)柵格單位的距離,在與障礙物距離開(kāi)始遞增并且大于一定閾值時(shí),機(jī)器人返回原軌跡行駛。圖3表示障礙物的行進(jìn)方向與機(jī)器人的行進(jìn)方向相同,當(dāng)障礙物的速度大于機(jī)器人的行駛速度,易知機(jī)器人可按原軌跡行進(jìn),當(dāng)障礙物速度小于機(jī)器人的行駛速度,機(jī)器人采用慎思型控制結(jié)構(gòu),計(jì)算出碰撞點(diǎn)位置后,將碰撞點(diǎn)膨化處理為不可行點(diǎn),重新進(jìn)行路徑規(guī)劃,更新路徑。
(2)動(dòng)態(tài)障礙物與路徑交為一個(gè)點(diǎn)或多個(gè)點(diǎn),如圖4和圖5所示。
圖4 障礙物速度小于機(jī)器人行進(jìn)速度
圖5 障礙物速度大于機(jī)器人行進(jìn)速度
當(dāng)動(dòng)態(tài)障礙物行徑與路徑軌跡的交點(diǎn)非碰撞點(diǎn)時(shí),易知機(jī)器人可按原路徑行駛,不予討論。當(dāng)障礙物的行徑與機(jī)器人有一個(gè)碰撞點(diǎn)時(shí),若動(dòng)態(tài)障礙物速度遠(yuǎn)小于機(jī)器人行進(jìn)速度,采用慎思型控制結(jié)構(gòu),如圖4所示,可按照障礙物的速度大小將碰撞點(diǎn)做相應(yīng)的膨化處理,將膨化區(qū)域視作不可行區(qū)域,重新進(jìn)行局部路徑規(guī)劃,更新路徑。若障礙物速度遠(yuǎn)大于機(jī)器人速度,將導(dǎo)致膨化區(qū)域過(guò)大,應(yīng)采用反應(yīng)型控制策略,如圖5所示,機(jī)器人在碰撞點(diǎn)前等待,待障礙物穿過(guò)機(jī)器人規(guī)劃路徑后,再按原路徑行駛。
為了便于研究,本文對(duì)移動(dòng)機(jī)器人及環(huán)境地圖做以下設(shè)定:
(1)采用柵格法對(duì)環(huán)境地圖進(jìn)行數(shù)學(xué)建模,可行進(jìn)區(qū)域與不可行進(jìn)區(qū)域分別用白色柵格與黑色柵格表示。
(2)地圖環(huán)境的起點(diǎn)、終點(diǎn)已知,地圖中部分障礙物信息未知。
(3)將移動(dòng)機(jī)器人與動(dòng)態(tài)障礙物點(diǎn)化后膨化處理。
(4)地圖環(huán)境中的動(dòng)態(tài)障礙物做勻速直線運(yùn)動(dòng),但障礙物速度方向、出現(xiàn)規(guī)律未知。
(5)移動(dòng)機(jī)器人帶有傳感器,能夠感知有限范圍內(nèi)地圖的信息,如障礙物的方位、速度等。
(6)移動(dòng)機(jī)器人的運(yùn)動(dòng)為勻速運(yùn)動(dòng),且可全向運(yùn)動(dòng)。
動(dòng)態(tài)環(huán)境下的多重A*算法流程如下:
步驟1 算法的初始化。初始化系統(tǒng)參數(shù):路徑規(guī)劃的起始位置S,目標(biāo)位置E,柵格尺寸δ,柵格數(shù)量n,生成工作空間柵格集合J ;初始化算法參數(shù):機(jī)器人傳感器探測(cè)距離R,路徑優(yōu)化參數(shù)α、β、γ。
步驟2 基于已知的環(huán)境信息,生成全局路徑并優(yōu)化全局路徑。以傳統(tǒng)算法生成路徑后,經(jīng)刪除節(jié)點(diǎn)、新增節(jié)點(diǎn)優(yōu)化路徑,生成可參照節(jié)點(diǎn)信息,基于該部分的節(jié)點(diǎn)信息進(jìn)行初步局部路徑規(guī)劃。
步驟3 選取當(dāng)前環(huán)境下滾動(dòng)窗口的子目標(biāo)點(diǎn),判斷當(dāng)前子目標(biāo)點(diǎn)信息,若當(dāng)前子目標(biāo)點(diǎn)處恰好為不可通行柵格,則轉(zhuǎn)到步驟2,若子目標(biāo)點(diǎn)處為可通行柵格區(qū)域,則轉(zhuǎn)到步驟4。
步驟4 在當(dāng)前局部路徑規(guī)劃區(qū)域內(nèi)采用起始點(diǎn)為sEn、目標(biāo)點(diǎn)為sEn+1 的多重A*算法,同時(shí)引入避障策略,根據(jù)動(dòng)態(tài)障礙物信息的不同,混合使用反應(yīng)型控制結(jié)構(gòu)和慎思型控制結(jié)構(gòu)。
步驟5 在到達(dá)局部子目標(biāo)點(diǎn)后,判斷當(dāng)前位置是否與目標(biāo)終點(diǎn)E 重合,若不重合,則跳到步驟3;若重合,說(shuō)明機(jī)器人已到達(dá)目標(biāo)位置,算法結(jié)束。
為了驗(yàn)證多重A*算法有較好的全局路徑規(guī)劃能力與局部路徑規(guī)劃能力,本文在不同環(huán)境下將多重A*算法與A*算法進(jìn)行實(shí)驗(yàn)仿真比較。實(shí)驗(yàn)在CPU為I7-8700,RAΜ 為16 GB 的計(jì)算機(jī)上運(yùn)行,算法通過(guò)Μatlab 編程仿真實(shí)現(xiàn)。
3.1.1 路徑軌跡的比較
本文算法相比于傳統(tǒng)A*算法,在靜態(tài)全局路徑規(guī)劃問(wèn)題上主要是對(duì)規(guī)劃的路徑軌跡進(jìn)行優(yōu)化處理。在20×20 的柵格環(huán)境下,以(1,1)為起點(diǎn),(20,20)為終點(diǎn)進(jìn)行傳統(tǒng)A*算法的全局路徑規(guī)劃后,用本文算法優(yōu)化路徑節(jié)點(diǎn)。
優(yōu)化路徑節(jié)點(diǎn)的關(guān)鍵在于參數(shù)α、β、γ 的選取,α、β 越小理論上優(yōu)化效果越好,但α 過(guò)小容易導(dǎo)致節(jié)點(diǎn)少,路徑不平滑,β 過(guò)小規(guī)劃軌跡容易與不可行域相交,同時(shí)α、β、γ 的選取也與環(huán)境大小、障礙物覆蓋率相關(guān)。在20×20 的柵格環(huán)境下,經(jīng)過(guò)多組數(shù)據(jù)仿真比較,選取參數(shù)α=126°,β=58°,γ=2 下路徑的優(yōu)化效果最好,優(yōu)化軌跡的參考標(biāo)準(zhǔn)有累積轉(zhuǎn)角、平均轉(zhuǎn)角、路徑長(zhǎng)度。圖6為路徑節(jié)點(diǎn)的優(yōu)化過(guò)程,紅線表示生成的全局路徑。圖6(a)為傳統(tǒng)A*算法生成的路徑軌跡,圖6(b)為刪除部分冗余點(diǎn)后生成的路徑軌跡,圖6(c)為新增節(jié)點(diǎn)后生成的路徑軌跡,即為本文算法生成的路徑。
由表1可知,經(jīng)過(guò)本文的路徑優(yōu)化策略,對(duì)比傳統(tǒng)A*算法累積轉(zhuǎn)角降低了81.23%,平均轉(zhuǎn)角降低了37.94%,路徑長(zhǎng)度降低了9.58%。文獻(xiàn)[13]與文獻(xiàn)[14]對(duì)于軌跡僅做了本文路徑優(yōu)化策略的第一步,即只對(duì)路徑算法做了刪除冗余點(diǎn)處理。由表1可知,雖然本文算法在路徑長(zhǎng)度上相比于文獻(xiàn)[13]與文獻(xiàn)[14]有少許增加,但是累積均轉(zhuǎn)角與平均轉(zhuǎn)角相比其他文獻(xiàn)降低了23.64%和15.82%,使得路徑軌跡更加平滑,更符合移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)規(guī)律。除此之外,新增的節(jié)點(diǎn)縮短了節(jié)點(diǎn)間的平均距離,更有利于后續(xù)局部路徑規(guī)劃范圍的選取。
表1 不同算法路徑的優(yōu)化性能比較
圖6 優(yōu)化路徑過(guò)程
3.1.2 算法效率的比較
Dynamic A*是由Stentz在1994年提出的一種在傳統(tǒng)A*算法的基礎(chǔ)上發(fā)展而來(lái)的動(dòng)態(tài)路徑搜索算法,一種比較典型的動(dòng)態(tài)路徑規(guī)劃,擁有局部路徑規(guī)劃能力[16-17]。Dynamic A*算法是一種動(dòng)態(tài)環(huán)境下的廣義A*算法,將其與本文算法比較時(shí)間效率與空間效率。
為了減少環(huán)境的變化給實(shí)驗(yàn)帶來(lái)的影響,進(jìn)一步提高實(shí)驗(yàn)的客觀性,本文采用不同數(shù)據(jù)在同一臺(tái)計(jì)算機(jī)上進(jìn)行算法仿真模擬,并建立3個(gè)不同復(fù)雜程度的環(huán)境將本文算法與Dynamic A*算法進(jìn)行比對(duì)。環(huán)境的復(fù)雜程度設(shè)定為環(huán)境的大小與障礙物的覆蓋率,如圖7 及表2所示。圖7(a)表示環(huán)境面積小且障礙物覆蓋率低的環(huán)境,圖7(b)表示環(huán)境面積適中、障礙物覆蓋率大的環(huán)境,圖7(c)表示環(huán)境面積大、障礙物覆蓋率高的環(huán)境。
表2 不同復(fù)雜程度下的環(huán)境參數(shù)
如表3 所示,本文從路徑規(guī)劃時(shí)間(單位為程序執(zhí)行的周期次數(shù)T)與規(guī)劃的路徑長(zhǎng)度兩個(gè)指標(biāo)對(duì)比Dynamic A*算法與本文算法,可知在20×20的柵格環(huán)境下本文算法路徑規(guī)劃時(shí)間與路徑長(zhǎng)度分別減少了12.3%和11.0%,在50×50的柵格環(huán)境下本文算法路徑規(guī)劃時(shí)間與路徑長(zhǎng)度分別減少了18.6%和16.4%,在100×100的柵格環(huán)境下本文算法路徑規(guī)劃時(shí)間與路徑長(zhǎng)度分別減少了50.4%和49.1%。
實(shí)驗(yàn)數(shù)據(jù)表明,隨著柵格環(huán)境的增大,環(huán)境越復(fù)雜,本文算法的優(yōu)勢(shì)越加明顯。如圖8所示,隨著環(huán)境面積的增加,在柵格數(shù)目超過(guò)140 左右,Dynamic A*算法所花費(fèi)的時(shí)間與空間將呈指數(shù)型增長(zhǎng),而本文算法則在柵格數(shù)目超過(guò)180后斜率才慢慢增加,理論上也可證實(shí)隨著環(huán)境增大,本文算法效率越來(lái)越優(yōu)于Dynamic A*算法。這是因?yàn)锳*算法是通過(guò)等勢(shì)線逐級(jí)擴(kuò)展的方式進(jìn)行搜索,搜索空間較大,這是一種長(zhǎng)度優(yōu)先算法,生成的路徑容易在小范圍區(qū)域內(nèi)出現(xiàn)多次轉(zhuǎn)彎的現(xiàn)象,環(huán)境越復(fù)雜,這種現(xiàn)象越加明顯[18-19]。而本文算法的路徑更新僅發(fā)生在每個(gè)滾動(dòng)窗口內(nèi),隨著空間環(huán)境變化,耗費(fèi)值的增加僅來(lái)源于滾動(dòng)窗口內(nèi)部路徑的變化以及滾動(dòng)窗口數(shù)量的增加,大大降低了路徑規(guī)劃的時(shí)間和空間成本。
圖7 不同復(fù)雜程度環(huán)境下的柵格地圖
表3 不同環(huán)境下算法性能對(duì)比
圖8 規(guī)劃時(shí)間與路徑長(zhǎng)度隨環(huán)境變化折線圖
為了驗(yàn)證本文算法的動(dòng)態(tài)規(guī)劃能力,在算法仿真中引入未先驗(yàn)的動(dòng)態(tài)障礙物,建立簡(jiǎn)單環(huán)境進(jìn)行仿真驗(yàn)證。在該環(huán)境下首先根據(jù)先驗(yàn)的地圖信息,以S(1,1)為起始點(diǎn)、E(20,20)為終點(diǎn),進(jìn)行全局路徑規(guī)劃,優(yōu)化路徑后得到如圖9所示的路徑,用紅線表示。在該路徑下選取合適的局部子目標(biāo)點(diǎn),增加的局部子目標(biāo)點(diǎn)數(shù)目為2個(gè),設(shè)為A、B。
圖9 先驗(yàn)環(huán)境下的路徑
在該環(huán)境下路徑被分割成3個(gè)子路徑,分別是SA、AB、BE。機(jī)器人在起始位置時(shí),在第一個(gè)滾動(dòng)窗口內(nèi)未檢測(cè)到地圖信息有變化,機(jī)器人沿先驗(yàn)環(huán)境下的路徑由S 點(diǎn)行進(jìn)到第一個(gè)局部子目標(biāo)點(diǎn)A。此時(shí),機(jī)器人在第二個(gè)滾動(dòng)窗口內(nèi)檢測(cè)地圖信息,此時(shí)檢測(cè)到動(dòng)態(tài)障礙物M(藍(lán)色方塊),根據(jù)傳感器的信息反饋,機(jī)器人將在X 位置(紅色柵格)與障礙物相撞,如圖10(a)所示。
根據(jù)傳感器的檢測(cè)信息,該動(dòng)態(tài)障礙物M 的行進(jìn)速度小于機(jī)器人的行駛速度,M 的行徑軌跡與路徑軌跡有一個(gè)交點(diǎn),采用慎思型控制結(jié)構(gòu),在局部區(qū)域中重新進(jìn)行路徑規(guī)劃,更新子路徑AB。如圖10(b)所示,原路徑用虛線表示,機(jī)器人的行駛路徑用實(shí)線表示。當(dāng)?shù)竭_(dá)第二個(gè)子目標(biāo)點(diǎn)B 時(shí),障礙物位于機(jī)器人左側(cè)位置,完成動(dòng)態(tài)避障過(guò)程。
表4 為在圖9 所示的20×20 的柵格環(huán)境下,引入未先驗(yàn)的動(dòng)態(tài)障礙物時(shí)不同算法的定量實(shí)驗(yàn)比對(duì)。傳統(tǒng)A*算法不具備避障能力,在遇到未先驗(yàn)的障礙物后路徑規(guī)劃失敗,不可到達(dá)目標(biāo)位置。Dynamic A*算法與本文算法在遇到未先驗(yàn)障礙物時(shí),都有重新規(guī)劃路徑的能力,可到達(dá)目標(biāo)位置,且本文算法的規(guī)劃時(shí)間與路徑長(zhǎng)度相對(duì)于Dynamic A*在該實(shí)驗(yàn)環(huán)境下減少了12.9%與8.1%。
圖10 動(dòng)態(tài)避障
表4 動(dòng)態(tài)環(huán)境下不同算法比對(duì)
傳統(tǒng)的A*算法是一種全局的靜態(tài)路徑規(guī)劃算法,存在路徑不平滑、搜索效率低等缺點(diǎn),本文針對(duì)A*算法的不足,提出了一種適用于局部動(dòng)態(tài)環(huán)境的路徑規(guī)劃的多重A*算法。多重A*算法首先在原有全局路徑之上優(yōu)化了全局路徑軌跡并提取了局部子目標(biāo)點(diǎn),在每個(gè)滾動(dòng)窗口內(nèi)進(jìn)行局部路徑規(guī)劃,并結(jié)合反應(yīng)型和慎思型避障控制策略的優(yōu)點(diǎn)進(jìn)行實(shí)時(shí)避障。本文將改進(jìn)后的算法與原算法進(jìn)行了比對(duì),并在動(dòng)態(tài)環(huán)境下進(jìn)行了仿真分析,在20×20的柵格環(huán)境下改進(jìn)后算法相比于原算法累積轉(zhuǎn)角降低了81.23%,平均轉(zhuǎn)角降低了37.94%,路徑長(zhǎng)度降低了9.58%,規(guī)劃時(shí)間降低了12.3%,且在未知環(huán)境中能有效地避開(kāi)動(dòng)態(tài)障礙物。