基于改進(jìn)A＊算法室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃

劉子豪，趙 津，劉 暢，賴?yán)こ?，王璽喬

貴州大學(xué) 機(jī)械工程學(xué)院，貴陽550025

在機(jī)器人領(lǐng)域中，路徑規(guī)劃一直是國(guó)內(nèi)外研究的熱點(diǎn)話題。路徑規(guī)劃是指移動(dòng)機(jī)器人在通過傳感器獲得所處環(huán)境的全局信息之后，通過路徑規(guī)劃算法來規(guī)劃出一條最優(yōu)路徑，可以使得移動(dòng)機(jī)器人安全且快速地由起始點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)[1]。

A*算法作為一種智能啟發(fā)式算法，由于自身的可移植性和可塑性較強(qiáng)，應(yīng)用領(lǐng)域廣泛[2]，例如航空航天[3]、農(nóng)業(yè)[4]、制造業(yè)[5]、倉(cāng)儲(chǔ)物流[6-7]等。但傳統(tǒng)A*算法有著先天的不足之處。在前期進(jìn)行路徑點(diǎn)搜索時(shí)，需要維護(hù)Openlist 和Closedlist 兩個(gè)列表。在進(jìn)行子節(jié)點(diǎn)計(jì)算時(shí)，需將父節(jié)點(diǎn)周邊八個(gè)點(diǎn)全部加入到Openlist 進(jìn)行計(jì)算，尋找啟發(fā)代價(jià)最小的點(diǎn)作為下一個(gè)父節(jié)點(diǎn)，迭代運(yùn)行[8]。因此當(dāng)環(huán)境信息復(fù)雜，場(chǎng)景比較大時(shí)，許多不必要的節(jié)點(diǎn)都會(huì)被計(jì)算，造成了內(nèi)存開銷過大、尋路時(shí)間變長(zhǎng)的問題；在由父節(jié)點(diǎn)搜尋下一個(gè)子節(jié)點(diǎn)的過程中，子節(jié)點(diǎn)只能在父節(jié)點(diǎn)周邊八個(gè)相鄰的節(jié)點(diǎn)中產(chǎn)生，因此，父節(jié)點(diǎn)與子節(jié)點(diǎn)之間存在著π/4的角度限制[9]，會(huì)造成一些不必要的轉(zhuǎn)折，使得路徑變長(zhǎng)；傳統(tǒng)A*算法中，沒有對(duì)路徑進(jìn)行平滑處理，因此，規(guī)劃好的路徑的轉(zhuǎn)折比較劇烈，不利于移動(dòng)機(jī)器人的路徑跟蹤。針對(duì)上述問題，國(guó)內(nèi)外研究人員提出了一些改進(jìn)方法。例如，文獻(xiàn)[10-11]提高了運(yùn)算速度，文獻(xiàn)[12]提高了路徑的平滑程度，文獻(xiàn)[13]提高了轉(zhuǎn)彎的曲折程度，但沒有對(duì)上述三個(gè)問題一起改進(jìn)的研究成果。

因此針對(duì)傳統(tǒng)A*算法在尋路過程中所需時(shí)間過長(zhǎng)、路徑轉(zhuǎn)折多以及所規(guī)劃的路徑不平滑的問題，本文首先結(jié)合跳躍點(diǎn)搜索理論，剔除傳統(tǒng)A*算法中所不需要計(jì)算的節(jié)點(diǎn)，減小了計(jì)算量，提高了運(yùn)算速度，其次對(duì)所得到的路徑進(jìn)行二次規(guī)劃，刪除不必要的轉(zhuǎn)折點(diǎn)，降低路徑長(zhǎng)度。接著將二次規(guī)劃得到的路徑，在拐點(diǎn)處進(jìn)行優(yōu)化處理，使所得到的路徑更加平滑。最后，將本文提出的算法應(yīng)用于Turtlebot3 移動(dòng)機(jī)器人中，在室內(nèi)進(jìn)行全局路徑規(guī)劃實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明，本文提出的算法在運(yùn)行時(shí)間、路徑長(zhǎng)度、平滑程度上相比于傳統(tǒng)的A*算法都有很大的提高。

1 傳統(tǒng)A＊算法

機(jī)器人進(jìn)行路徑規(guī)劃前，需建立自身所用地圖，根據(jù)地圖種類可分為柵格、拓?fù)涞貓D等，柵格地圖由于自身模型簡(jiǎn)單且對(duì)環(huán)境有很好的表達(dá)，在路徑規(guī)劃時(shí)被廣泛采用，因此本文將采用柵格法來構(gòu)建地圖。柵格地圖是將機(jī)器人得到的2D環(huán)境信息地圖進(jìn)行相同尺寸的柵格劃分。根據(jù)柵格中是否有障礙物將柵格分為白色可通行柵格和黑色不可通過柵格。

柵格地圖中路徑規(guī)劃實(shí)例如圖1所示，周邊環(huán)境固定，綠色的點(diǎn)為起點(diǎn)，紅色的點(diǎn)為終點(diǎn)，黃色的線為規(guī)劃的路徑。

圖1 規(guī)劃路徑

A*算法是一種在靜態(tài)環(huán)境下獲得最優(yōu)路徑的啟發(fā)式算法。該算法通過計(jì)算估價(jià)函數(shù)，來確定接下來的搜尋和運(yùn)動(dòng)方向。首先，起點(diǎn)必須放入Openlist列表，將起點(diǎn)作為父節(jié)點(diǎn)，放入Closelist列表，向周邊擴(kuò)展計(jì)算，得到周邊八個(gè)節(jié)點(diǎn)的代價(jià)值，選擇最小的節(jié)點(diǎn)作為下一個(gè)父節(jié)點(diǎn)，再以此節(jié)點(diǎn)向周邊進(jìn)行擴(kuò)展計(jì)算，得到代價(jià)值最小的節(jié)點(diǎn)作為下一個(gè)父節(jié)點(diǎn)，依次迭代，直到終點(diǎn)。由于在路徑搜尋的過程中，每一個(gè)節(jié)點(diǎn)的代價(jià)值均是最小。因此，得到的路徑一定為代價(jià)最小的最優(yōu)路徑。A*算法估價(jià)函數(shù)為：

Fn表示起始點(diǎn)到任意節(jié)點(diǎn)的估價(jià)值，Gn表示起始點(diǎn)到某一個(gè)節(jié)點(diǎn)的實(shí)際代價(jià)，Hn表示起始該點(diǎn)到終點(diǎn)的估計(jì)代價(jià)值。本文采用歐式距離作為啟發(fā)式函數(shù)：

起始點(diǎn)橫坐標(biāo)為xi，起始點(diǎn)縱坐標(biāo)yi。終點(diǎn)橫坐標(biāo)xn，終點(diǎn)縱坐標(biāo)yn。

圖2為A*算法的實(shí)例。

圖2 傳統(tǒng)A*算法尋路

由上述傳統(tǒng)A*算法步驟可知，在運(yùn)行過程中，需要對(duì)Openlist和Closelist兩個(gè)列表進(jìn)行維護(hù)。圖2中，綠色為選出的路徑點(diǎn)，粉色區(qū)域?yàn)锳*算法需要計(jì)算的點(diǎn)，黑色的線條為規(guī)劃好的路徑。由圖2得出，粉色的區(qū)域?yàn)锳*在尋路過程中需要計(jì)算的節(jié)點(diǎn)，但大部分節(jié)點(diǎn)都與生成的路徑無關(guān)，造成傳統(tǒng)的A*算法在大范圍尋路過程中存在計(jì)算量過大、內(nèi)存消耗嚴(yán)重的問題。在規(guī)劃好的路徑中存在著不必要的轉(zhuǎn)折點(diǎn)，例如點(diǎn)S 可以直接到點(diǎn)C。并且，傳統(tǒng)A*算法規(guī)劃好的路徑轉(zhuǎn)折劇烈，不利于機(jī)器人進(jìn)行路徑跟蹤。

2 改進(jìn)A＊算法

本文針對(duì)傳統(tǒng)A*算法計(jì)算量大、轉(zhuǎn)折點(diǎn)過多、路徑不平滑的問題，提出了一種改進(jìn)算法。首先基于關(guān)鍵點(diǎn)選取的策略來代替?zhèn)鹘y(tǒng)A*算法中的Openlist和Closelist兩個(gè)列表，之后將得到的路徑進(jìn)行二次規(guī)劃，刪除冗余點(diǎn)，最后對(duì)轉(zhuǎn)折處進(jìn)行路徑平滑處理，得到一條最優(yōu)路徑。

2.1 關(guān)鍵點(diǎn)選取

本文受跳躍點(diǎn)算法的啟發(fā)，提出了一種關(guān)鍵點(diǎn)搜索的策略。如圖3 所示，起始點(diǎn)為S，終點(diǎn)為G 。關(guān)鍵點(diǎn)搜索策略的本質(zhì)就是給A*尋路算法一個(gè)先驗(yàn)信息，利用選取所得到的關(guān)鍵點(diǎn)來代替?zhèn)鹘y(tǒng)A*中Openlist 的點(diǎn)進(jìn)行計(jì)算，如本例中，S 為起點(diǎn)，G 為終點(diǎn)。按照傳統(tǒng)的A*算法，應(yīng)首先計(jì)算（1，2），（2，1），（2，2）這三個(gè)點(diǎn)的代價(jià)值，選擇（2，2）點(diǎn)作為下一個(gè)父節(jié)點(diǎn)，并計(jì)算其周邊的八個(gè)點(diǎn)的代價(jià)值，再選擇代價(jià)最小的路徑，即圖3 中所有的點(diǎn)都需要進(jìn)行計(jì)算。但在圖中，可以明顯得出，S →(2,2)→G 為一條最優(yōu)路徑，與之無關(guān)的點(diǎn)均不需要計(jì)算。在下文提出了具體策略，使得在尋路過程中，路徑僅僅經(jīng)過（2，2）點(diǎn)。這樣就解決了傳統(tǒng)A*算法在尋路上存在的內(nèi)存開銷過大的問題。在文獻(xiàn)[14]中提出了一種策略，本文基于此，做出了調(diào)整，提出了自己的關(guān)鍵點(diǎn)策略。通過對(duì)A*算法和實(shí)際路徑規(guī)劃情況分析，可知，當(dāng)路徑點(diǎn)周邊存在障礙物和不存在障礙物時(shí)搜尋路徑的關(guān)鍵點(diǎn)不同，因此提出了下列兩種情況關(guān)鍵點(diǎn)的選取策略。

圖3 關(guān)鍵點(diǎn)

2.1.1 兩點(diǎn)共線，且周邊不存在障礙物

在圖4 中J 和G 是A*算法中的任意節(jié)點(diǎn)，J 為父節(jié)點(diǎn)。根據(jù)圖可以得出，在J 到達(dá)G 時(shí)，一共存在著兩類路徑。一種是直線路徑，由J 經(jīng)由x 直接到G 點(diǎn)。另一種為折線路徑，不經(jīng)過x，由J 折線到達(dá)G 點(diǎn)。對(duì)比在這兩種情況中，沿直線運(yùn)動(dòng)的方式花費(fèi)的時(shí)間最短。在傳統(tǒng)的A*算法中，途中所有的柵格點(diǎn)都需要進(jìn)行計(jì)算，但圖中的灰色部分計(jì)算無意義。因此為了刪減這些不必要的點(diǎn)，采用下面策略：

jx 與jg 均為向量。因此，當(dāng)共線時(shí)，僅選取該線段的端點(diǎn)，比如圖中的G 點(diǎn)，作為關(guān)鍵點(diǎn)。

圖4 選取關(guān)鍵點(diǎn)

2.1.2 兩點(diǎn)不共線，但周邊存在障礙物

在進(jìn)行A*算法時(shí)，需要越過障礙物進(jìn)行尋路，如圖5 所示，因此當(dāng)周邊存在障礙物時(shí)，上述所給出的策略就需要做出相應(yīng)的調(diào)整，提出了強(qiáng)制關(guān)鍵點(diǎn)策略：

jx、jf、jn、nf 均為向量。n 為除x 點(diǎn)外任意一點(diǎn)。當(dāng)F 點(diǎn)滿足上述關(guān)系式時(shí)，將其定義為強(qiáng)制關(guān)鍵點(diǎn)，在進(jìn)行路徑規(guī)劃時(shí)，這些強(qiáng)制關(guān)鍵點(diǎn)必須進(jìn)行計(jì)算。

圖5 強(qiáng)制關(guān)鍵點(diǎn)

在應(yīng)用改進(jìn)的A*進(jìn)行路徑規(guī)劃時(shí)，由起始點(diǎn)開始檢測(cè)，先搜尋水平方向，當(dāng)水平方向上有障礙物或者碰觸到邊界，且不能搜尋到強(qiáng)制關(guān)鍵點(diǎn)時(shí)，搜尋停止，改為搜尋對(duì)角線方向，當(dāng)對(duì)角線到一個(gè)新節(jié)點(diǎn)時(shí)，按照之前的策略再進(jìn)行搜尋，尋找到關(guān)鍵點(diǎn)后，將關(guān)鍵點(diǎn)作為下一個(gè)搜尋的起始點(diǎn)，進(jìn)行搜尋，一直搜尋到終點(diǎn)為止。尋路過程如圖6所示。

圖6 關(guān)鍵點(diǎn)尋路實(shí)例

2.2 二次路徑規(guī)劃

在傳統(tǒng)的A*算法中，路徑只進(jìn)行一次規(guī)劃，得到一條在A*算法約束條件下的最優(yōu)路徑。由于自身在搜尋過程中只能向周邊八個(gè)點(diǎn)進(jìn)行擴(kuò)展搜索，因此存在π/4的角度限制，使得路徑中依然存在著冗余轉(zhuǎn)折點(diǎn)。在這種情況下，進(jìn)行二次路徑規(guī)劃，將得到的原始路徑，進(jìn)行冗余點(diǎn)刪除的處理，使得路徑長(zhǎng)度變短。在二次路徑規(guī)劃時(shí)，本文提出了一種反向動(dòng)態(tài)搜索的策略。將路徑的終點(diǎn)作為二次路徑規(guī)劃的起始點(diǎn)，同理，路徑的起始點(diǎn)作為二次規(guī)劃搜尋的終點(diǎn)。由起始點(diǎn)向下一個(gè)頂點(diǎn)連接，假如沒有遇到障礙物，迭代進(jìn)行。直到遇到障礙物時(shí)，將其路徑中心點(diǎn)相連，檢查是否穿過障礙物，如果沒有，連接兩點(diǎn)，如果有障礙物，則連接中點(diǎn)與靠近終點(diǎn)端四分之一處。一直取原來的二分之一長(zhǎng)度作為路徑的搜尋點(diǎn)，直到兩個(gè)路徑的長(zhǎng)度之間沒有較大的變化為止（視地圖尺寸為定，本文地圖大小為7.5 m×7.5 m，經(jīng)多次實(shí)驗(yàn)驗(yàn)證，選擇長(zhǎng)度比為0.9）。將最終得到的點(diǎn)連接，得到二次規(guī)劃的路徑。

2.3 路徑平滑

在經(jīng)過2.1和2.2節(jié)處理后，得到了一條基于本文搜索策略的最優(yōu)的路徑，但在圖6 中可以看出，二次規(guī)劃的路徑轉(zhuǎn)折十分劇烈，室內(nèi)機(jī)器人在進(jìn)行路徑跟蹤時(shí)，沒有好的跟蹤效果。因此，需要對(duì)得到的路徑進(jìn)行平滑處理。Hybird A*算法[15]對(duì)路徑平滑有很好的處理效果，但時(shí)間花費(fèi)高，計(jì)算開銷大，并且容易陷入局部最優(yōu)。在本文中，提出了一種動(dòng)態(tài)相切圓策略。在2.2 節(jié)的基礎(chǔ)上，選取與障礙物不相交的拐點(diǎn)和路徑與障礙物的交點(diǎn)作為優(yōu)化點(diǎn)。對(duì)比優(yōu)化點(diǎn)兩側(cè)的路徑，選取小的路徑的二分之一長(zhǎng)度，分別做垂直，選取兩條垂線的交點(diǎn)作為圓心，畫一條圓弧來代替原來的轉(zhuǎn)折點(diǎn)，達(dá)到路徑平滑的作用。實(shí)驗(yàn)可以得知，雖然平滑效果不如Hybird A*，但運(yùn)算效率高，并且完全可滿足室內(nèi)移動(dòng)機(jī)器人的路徑跟蹤。

3 實(shí)驗(yàn)

3.1 仿真實(shí)驗(yàn)

經(jīng)本文算法得出的路徑與傳統(tǒng)A*算法的路徑如圖7所示。

圖7 A*與Improved A*

由圖7可以看出，本文提出的改進(jìn)算法在路徑長(zhǎng)度和彎折角度較傳統(tǒng)A*算法有著明顯的提高，在運(yùn)算時(shí)，傳統(tǒng)A*算法所需的時(shí)間為3.02 ms，改進(jìn)的A*算法為0.994 ms，運(yùn)算效率提高了67%；傳統(tǒng)A*算法路徑長(zhǎng)度為301.5 cm，本文算法為293.4 cm，減小了2.7%。為了驗(yàn)證該算法的通用性，再選取柵格邊長(zhǎng)為10 cm，地圖大小分別為15×15、30×30、50×50 的環(huán)境中進(jìn)行仿真實(shí)驗(yàn)。實(shí)驗(yàn)選擇固定起點(diǎn)與終點(diǎn)，隨機(jī)環(huán)境進(jìn)行實(shí)驗(yàn)，每一組地圖均進(jìn)行15組隨機(jī)實(shí)驗(yàn)。時(shí)間單位為1×10-6s，路徑單位為厘米。實(shí)驗(yàn)結(jié)果如圖8、9所示。

通過45 組仿真實(shí)驗(yàn)數(shù)據(jù)可以得出，本文提出的算法在時(shí)間上平均為A*算法運(yùn)算時(shí)間的56.09%，路徑長(zhǎng)度降低為原來的97.94%。并且，根據(jù)地圖大小和障礙物數(shù)量的不同，可以得出本文所提出的算法，在大尺寸、多障礙物的環(huán)境下，運(yùn)算速度、路徑長(zhǎng)度明顯優(yōu)于傳統(tǒng)A*算法。

3.2 真實(shí)環(huán)境實(shí)驗(yàn)

圖8 三種環(huán)境時(shí)間

圖9 三種環(huán)境路徑長(zhǎng)度

實(shí)驗(yàn)所采用的機(jī)器人為Turtlebot3buger。該機(jī)器人搭載Raspberry Pi 3樹莓派3開發(fā)板、OpenCR底層控制器，重量為0.995 kg，大小為138 mm×178 mm×192 mm。機(jī)器人運(yùn)動(dòng)時(shí)通過OpenCR 主板控制底部的兩個(gè)帶編碼器的減速電機(jī)，進(jìn)行速度控制，使其轉(zhuǎn)彎或者直行。機(jī)器人如圖10所示。

圖10 Turtlebot3buger

實(shí)驗(yàn)環(huán)境如下，采用邊長(zhǎng)15 cm、大小為12×12的柵格模型進(jìn)行建圖，得到的柵格環(huán)境地圖及規(guī)劃好的路徑如圖11所示。設(shè)定起點(diǎn)坐標(biāo)為（2，2），終點(diǎn)坐標(biāo)為（7，11）。將規(guī)劃好的路徑移植到機(jī)器人的開發(fā)板中，使機(jī)器人自主進(jìn)行路徑跟蹤。實(shí)驗(yàn)效果表明，在運(yùn)算速度，路徑長(zhǎng)度、平滑程度上，較傳統(tǒng)A*算法均有明顯提高，如表1所示。

圖11 實(shí)驗(yàn)環(huán)境（左）柵格地圖路徑規(guī)劃結(jié)果（右）

表1 A*與Improved A*對(duì)比

4 結(jié)論

本文以提出的關(guān)鍵點(diǎn)策略、二次路徑規(guī)劃以及路徑平滑策略，對(duì)傳統(tǒng)A*算法進(jìn)行改進(jìn)。關(guān)鍵點(diǎn)策略減小了傳統(tǒng)A*算法中的內(nèi)存消耗，提高了機(jī)器人的尋路效率。二次路徑規(guī)劃及路徑平滑降低了傳統(tǒng)A*算法的路徑長(zhǎng)度以及轉(zhuǎn)折角度，使機(jī)器人在路徑跟蹤時(shí)效率和跟蹤效果提高。并且，本文提出的算法在場(chǎng)景大且環(huán)境較為復(fù)雜時(shí)，提升的效果尤為顯著。將本文算法應(yīng)用于turtlebot3buger機(jī)器人中，實(shí)驗(yàn)結(jié)果表明，改進(jìn)的A*算法較傳統(tǒng)A*算法優(yōu)勢(shì)明顯。