動(dòng)態(tài)環(huán)境下多重A*算法的機(jī)器人路徑規(guī)劃方法

華 洪，張志安，施振穩(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)度更短。

1 改進(jìn)A*算法

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)。

1.1 優(yōu)化全局路徑節(jié)點(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)后，更新路徑。

1.2 局部路徑規(guī)劃

局部路徑規(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é)模型為：

1.3 避障控制策略

移動(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ī)劃路徑后，再按原路徑行駛。

2 實(shí)現(xiàn)流程

2.1 地圖及算法環(huán)境處理

為了便于研究，本文對(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)。

2.2 算法流程

動(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é)束。

3 仿真分析

3.1 多重A*算法與原算法的比較

為了驗(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)境變化折線圖

3.2 動(dòng)態(tài)環(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ì)

4 結(jié)束語(yǔ)

傳統(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)障礙物。