一種對地圖特征定向采樣的改進A*算法

趙梓辛，肖世德，靳天石，楊明亮，2

（1.西南交通大學機械工程學院，四川 成都 610031；2.先進驅(qū)動節(jié)能技術(shù)教育部工程研究中心，四川 成都 610031）

1 引言

路徑規(guī)劃技術(shù)在日常生活、高新技術(shù)、決策管理等諸多領(lǐng)域具有廣泛的應(yīng)用，主要解決成本、效率、安全等問題［1-2］。

移動機器人路徑規(guī)劃是指在一定的環(huán)境模型基礎(chǔ)上，給定移動機器人起始點和目標點后，按照算法規(guī)劃出一條無碰撞、能安全到達目標點的有效路徑［3-4］。近年來新的路徑規(guī)劃算法不斷出現(xiàn)和發(fā)展，在移動機器人路徑規(guī)劃領(lǐng)域具有代表性的是基于采樣的算法和基于搜索的算法［5］。基于采樣的算法是通過均勻隨機采樣，將環(huán)境空間離散的點構(gòu)建成連通圖，效率高，但所得的路徑代價高?；谒阉鞯乃惴ㄊ菍h(huán)境空間離散成柵格的形式，然后利用代價函數(shù)最小搜索可行解甚至最優(yōu)解，但在大量的搜索計算下實時性很難滿足要求。路徑規(guī)劃效果的關(guān)鍵在于規(guī)劃算法的性能，而規(guī)劃算法評價指標有兩個：一個是路徑代價，描述路徑長度和冗余拐點；另一個是算法實時性，描述算法所需的時間。

國外對于相關(guān)研究較為深入，例如Weighted A*［6］通過增加啟發(fā)式函數(shù)的權(quán)重，進一步加速向目標節(jié)點的搜索，但容易陷入局部最優(yōu)，所得解大概率為次優(yōu)解；ARA*［7］是在規(guī)定時間內(nèi)多次執(zhí)行Weighted A*，并且每次執(zhí)行時將啟發(fā)式函數(shù)的權(quán)重逐漸減小，使次優(yōu)解不斷逼近最優(yōu)解，但是要多次執(zhí)行Weighted A*，運行時間也會成倍增長，所以在允許時間內(nèi)一般為次優(yōu)解；還有Sandip Aline等提出了MHA*［8］，引入多個啟發(fā)式函數(shù)，保證其中有一個啟發(fā)式函數(shù)在單獨使用時可以找到最優(yōu)解，從而通過協(xié)調(diào)不同啟發(fā)式函數(shù)生成的路徑代價，可以兼顧算法的效率和最優(yōu)性，但是確保一個最優(yōu)啟發(fā)式的過程和協(xié)調(diào)的過程是一個耗時的前處理。國內(nèi)研究也在跟進，例如利用雙向A*算法［9］雙向同時使用A*算法，從而提高了算法的執(zhí)行效率，但是路徑并未得到優(yōu)化。

上述搜索路徑規(guī)劃方法多數(shù)從啟發(fā)式函數(shù)角度優(yōu)化，其根本目的就是減少無關(guān)節(jié)點的搜索，并保證搜索到關(guān)鍵節(jié)點，從而保證路徑最短的同時避免處理大量節(jié)點。但是在減少節(jié)點的過程中，節(jié)點減少過少時，勢必還是有很多無關(guān)節(jié)點計算；節(jié)點減少較多時，可能忽略關(guān)鍵節(jié)點，致使路徑偏離最優(yōu)解。再者，不同的環(huán)境地圖，所具有的特征是不相同的，所匹配的最優(yōu)啟發(fā)函數(shù)也是不一樣的。因此，與其優(yōu)化啟發(fā)函數(shù)甚至多啟發(fā)函數(shù)混合，不如從環(huán)境地圖特征入手，用這些特征點代替原地圖，從而降低搜索節(jié)點的數(shù)量。故源于采樣的路徑規(guī)劃思想，對環(huán)境地圖對地圖特征做定向采樣處理。進而在地圖特征點構(gòu)建的無向路徑網(wǎng)絡(luò)圖上，使用基于搜索的算法找出最優(yōu)解。這樣既避免了基于采樣算法的隨機性帶來的路徑成本過高問題，又避免了基于搜索算法在處理大量節(jié)點時效率低下的問題，從而達到降低路徑成本并提高算法實時性的目的。

2 改進A*算法的實現(xiàn)

2.1 環(huán)境模型描述

對移動機器人周圍環(huán)境進行數(shù)學模型搭建是使用算法規(guī)劃路徑的前提。均勻柵格地圖［3］是移動機器人規(guī)劃中最常用的表現(xiàn)形式。最早是由Morave和Elfes提出的，其基本思想是將有限地圖空間離散分解為多個大小相同并以數(shù)值表示狀態(tài)的柵格單元［10］。由此方法將環(huán)境空間分解為大小統(tǒng)一的M*N個柵格，它們均勻分布組合成二維地圖信息U，其中每個柵格的狀態(tài)信息由Mapij表示：

其中，每個柵格單元的狀態(tài)信息Mapij取值如下：

式中：Mapij=0—移動機器人可通行的自由區(qū)域；Mapij=1—移動機器人不可通行的障礙區(qū)域；Mapij=2—移動機器人的起始位置；Mapij=3—移動機器人的目的位置。

為簡化研究，做出以下假設(shè)：

假設(shè)1 以均勻柵格地圖表示實際環(huán)境，只考慮二維空間信息，忽略高度信息。

假設(shè)2 移動機器需考慮占據(jù)的二維空間，忽略高度信息。

假設(shè)3 移動機器每次移動，其幾何中心占據(jù)一個柵格。

假設(shè)4 此全局規(guī)劃算法，是為移動機器人運行做方向指引，移動機器人的實時路徑應(yīng)由局部規(guī)劃實現(xiàn)。因此假設(shè)移動機器人前進和轉(zhuǎn)向行為分開單獨進行。

2.2 地圖特征提取

最優(yōu)的路徑一般出現(xiàn)在障礙物的邊界處。若不存在障礙物，起始點與目的點可以通過一條直線達到，也正是障礙物的出現(xiàn)，才出現(xiàn)了路徑規(guī)劃的問題，因此障礙物外部輪廓可作為地圖特征。邊緣角點是障礙物輪廓的連接點，并可通過角點的連通關(guān)來表示障礙物的輪廓。因此可以將角點和角點的通斷信息來作為地圖特征。

邊緣角點本來是一種圖像特征。邊緣角點檢測是圖像處理中一種方法，是對二維數(shù)組的數(shù)字圖像進行的處理方式。在2.1節(jié)中，環(huán)境模型也是由一個二維數(shù)組描述的，圖像中的每個元素稱為像素，用于存儲灰度值，而環(huán)境地圖中每一個元素稱為柵格單元，用于存放狀態(tài)值。均勻柵格地圖和數(shù)字圖像的本質(zhì)相同，是將角點檢測方法應(yīng)用到路徑規(guī)劃的必要前提。

此處角點檢測并不是完全應(yīng)用圖像處理的方法，狀態(tài)空間的地圖的處理與圖像的處理還是有一定區(qū)別，狀態(tài)空間地圖只有一個通道，因此不需要灰度處理，本身通道可理解為灰度通道，而且通道只需處理0或者1值，相比于彩色RGB通道圖片的角點處理計算量非常小。具體實現(xiàn)步驟如下所示。

（1）分別計算地圖的水平、垂直的狀態(tài)值變化量矩陣Ii、Ij。采用一階橫向梯度濾波算子di、一階縱向梯度濾波算子dj對原地圖矩陣U濾波得矩陣Ii、Ij。

（4）計算地圖矩陣U各個像素點的角點量，構(gòu)建角點量矩陣R，此處k取0.04。

式中：R—角點量矩陣。

當R為正值時，此柵格單元為角點；當R為負時，此柵格單元為邊；當R很小時，此柵格單元為平緩區(qū)域。

（5）關(guān)鍵角點的篩選。

①為了降低無關(guān)角點的數(shù)量和相鄰角點數(shù)量，同時找出最關(guān)鍵的角點。以全局最大角點量值Rmax設(shè)置閾值thresh，式th中取0.3。

并求以5*5窗口遍歷并求窗口局部最大角點量值，并通過閾值抑制的局部角點即為全局角點。

②在此路徑規(guī)劃中，角點作為路徑途徑的候選點，如果其出現(xiàn)在障礙物內(nèi)部，會導(dǎo)致碰撞的嚴重后果。但是圖像處理中，邊緣角點必然出現(xiàn)在障礙物內(nèi)部邊緣。對于這種問題，采用膨脹障礙物的方式，讓角點從障礙物區(qū)域移至障礙物膨脹區(qū)域，從而避免事故的發(fā)生。

③為了剔除地圖邊界的無關(guān)角點，以及出現(xiàn)在非障礙區(qū)域的錯誤角點選出的角點再進行一次是否在障礙膨脹區(qū)域的判定，在膨脹區(qū)域的才為全局角點。

為了方便直觀描述，膨脹區(qū)域不顯示，如圖1所示。左下角紅色圓點為目的點，右上角紅色圓點為初始點，障礙物周圍的紅色星點為篩選出的角點。

圖1 地圖特征提取結(jié)果Fig.1 Map Feature Extraction Results

2.3 障礙物膨脹處理

大多數(shù)路徑規(guī)劃的研究中，常常將移動機器人假設(shè)為點進行研究，忽略其占據(jù)的空間，可能出現(xiàn)移動機器人邊緣與障礙物發(fā)生碰撞的危險行為。因此為了避免發(fā)生上述情況，將移動機器人在水平面上占據(jù)空間的大小考慮到規(guī)劃算法中。移動機器人在搜索中是動態(tài)的角色，如果時刻考慮質(zhì)心和障礙物的距離，再和移動機器人的長寬做判斷是否發(fā)生碰撞。每一次擴張都會計算并判斷一次是否碰撞，加劇了算法的空間復(fù)雜度。以移動機器人質(zhì)心到邊緣最大距離為尺度在障礙物上做一次性的膨脹處理。這樣相比與實時計算是否碰撞，降低了很大的計算量。

障礙物的膨脹處理的偽代碼如下：

在上述偽代碼中，Restricted_area（Pn，τ+1）表示膨脹當前點Pn的τ+1層鄰近柵格，τ由移動機器人質(zhì)心到邊緣的最大距離決定，τ+1表示添加一層角點檢測的膨脹區(qū)域。在膨脹的過程中，將位于自由區(qū)域的柵格，設(shè)置為障礙物膨脹區(qū)域。如下圖所示，數(shù)字5的區(qū)域是障礙物區(qū)域，并以1層鄰近柵格作為膨脹，將1、2、3、4、6、7、8、9自由區(qū)域膨脹為障礙物膨脹區(qū)域。

圖2 障礙物膨脹處理原理Fig.2 Principle of Barrier Expansion Treatment

2.4 構(gòu)建無向路徑網(wǎng)絡(luò)圖

將得到特征角點與始末點兩兩相互連接，摒棄與障礙物或膨脹區(qū)域有交集的連線，保留無碰撞連線，構(gòu)建出無向路徑網(wǎng)絡(luò)圖。如下圖所示，此圖是初始點v1，目的點v8與正方形障礙物的4個角點，三角形障礙物的3個角點構(gòu)成了基于地圖特征的無碰撞無向路徑網(wǎng)絡(luò)圖，從而將100*100的地圖轉(zhuǎn)化為在9點之間連通網(wǎng)絡(luò)圖，降低搜索成本，提高搜索效率。

圖3 無向路徑網(wǎng)絡(luò)圖Fig.3 Path Network Diagram without Direction

但是無向路徑圖不方便存儲也不方便搜索，先將其以鄰接矩陣的形式存儲。無向路徑圖中的信息分為兩種，一是各個頂點的信息，二是各個頂點的通斷關(guān)系。用鄰接矩陣E存放頂點的關(guān)系信息。但各個頂點的信息部分無法存儲在E中。因此，再添加數(shù)組V存放圖中所有頂點數(shù)據(jù)。兩個數(shù)組共同存放無向路徑圖所有信息，如式（11）、式（12）所示。

式中：矩陣E—描述頂點關(guān)系的矩陣。元素E（i，j）—V（i）和V（j）頂點的關(guān)系。頂點信息V與頂點關(guān)系E具有以下性質(zhì)。

（1）順序存儲對應(yīng)性。對于頂點在V數(shù)組的存儲順序與二維數(shù)組E相對，即E（i，j）表示頂點V（i）與頂點V（j）的關(guān)系。

（2）主對角線元素都為0。主對角線表示V（i）與V（i）的關(guān)系，此時，E（i，j）取0。

（3）其余任意元素的意義，如下式所示。

（4）對稱性。鄰接矩陣是關(guān)于主對角線對稱。對于E（i，j）與E（j，i）分別表示V（i）到V（j），V（j）到V（i）連通性，因為連通圖沒有方向性，因此E（i，j）與E（j，i）是相等。

2.5 基于鄰接矩陣的改進A＊算法

基于搜索的路徑算法中，A*是在經(jīng)典Dijkstra算法的框架下引入估價函數(shù)，優(yōu)先搜索代價低的柵格。估價函數(shù)表示式為：

式中：F（v）—起始節(jié)點經(jīng)由當前區(qū)域到目的位置的總代價值；G（v）—起始位置到當前節(jié)點v的實際代價；H（v）當前節(jié)點v到目的位置的代價估算值；H（v）—般取歐幾里得距離，定義如下：

式中：（xv，yv）—當前節(jié)點v坐標；（xgoal，ygoal）—目標節(jié)點坐標。

傳統(tǒng)A*算法維護兩個集合：OPEN和CLOSED。CLOSED存儲已經(jīng)搜索過的節(jié)點。OPEN 存儲已搜索過節(jié)點的子節(jié)點優(yōu)先隊列，根據(jù)路徑總代價值F（v）由小到大排序。算法每次從OPEN中取出總代價最小的節(jié)點，作為新的搜索節(jié)點插入到CLOSED中，再將該節(jié)點進行擴展，最后將不在CLOSED集合中的擴展節(jié)點，插入到OPEN中。這樣一次一次循環(huán)下去，直到擴展到目標節(jié)點。其中擴展的意義是在柵格地圖的前提下，對當前節(jié)點周圍節(jié)點進行遍歷。最常見的擴展方式如圖4（a）、圖4（b）兩種擴展方式。當前點以“米”字向周圍柵格進行擴展，一共8個方向的最近柵格進行遍歷，因此也稱作“八連接”擴展方式，如圖4（a）所示。當前點以“十”字向周圍柵格進行擴展，一共4個方向的最近柵格進行遍歷，因此也稱作“四連接”擴展方式，如圖4（b）所示。

圖4 “八連接”“、四連接”擴展方式示意圖Fig.4 Schematic Diagram of“Eight-Connection”and“Four-Connection”Expansion Modes

其中對于每一步擴展的代價式（16）進行計算，式中：v’—當前節(jié)點v的擴展節(jié)點。

改進A*算法的實現(xiàn)如下偽代碼所示，加粗字體區(qū)域為在A*算法基礎(chǔ)上所作的改進。偽代碼分為兩個部分，初始化部分和路徑搜索部分。1-2行為初始化部分，和傳統(tǒng)A*一樣的初始方法。除去G（vstart）取0外，所有其他點的實際代價值G 都取無窮大。OPEN中只存放vstar節(jié)點，并且CLOSED為空。

3-17行為路徑搜索部分，3行偽代碼SearchPath（），開始運行路徑搜索程序。

4行將傳統(tǒng)while循環(huán)條件goal is not expanded改進為OPEN≠?。傳統(tǒng)A*算法中由于估計函數(shù)H（v）非最優(yōu)，在擴展到目標點時，算法就會結(jié)束，此時的路徑長度可能會大于最優(yōu)解，從而得到次優(yōu)解。若OPEN為空時算法結(jié)束，此時算法擴展到目標點搜索到可行路徑后，算法會繼續(xù)搜索，直到OPEN為空，在這期間會搜索到多個可行路徑，從中找出最優(yōu)路徑。

6行、8行、11行出現(xiàn)的solution是用來存放次優(yōu)路徑。在搜索算法執(zhí)行到OPEN為空的過程中，為了降低可行路徑的存儲空間。在第二個可行路徑出現(xiàn)時，與前一個存放在solution中可行路徑做對比，將較優(yōu)路徑在11行中再次存放在solution中。同時為了減少可行路徑的數(shù)量，6行當前在OPEN中的代價最小的節(jié)點必須滿足F（v）＜F（solution），8行在當前節(jié)點的擴展子節(jié)點代價總和滿足G（v）+C（v，v’）+H（v’）＜F（solution），從此避免計算過多節(jié)點，避免生成過多的次于當前路徑的可行路徑，浪費計算資源，降低實時性。

8行中StateExpand（）表示執(zhí)行狀態(tài)擴展。傳統(tǒng)狀態(tài)擴展采用“八連接”擴展方式，這種方式讓路徑只能一格一格前進，導(dǎo)致所得路徑并非最優(yōu)。針對這一問題，基于鄰接矩陣的擴展方式，去除大量中間點，讓關(guān)鍵點連接，在其上的擴展。結(jié)合式（11）、式（12）對鄰接矩陣的定義，StateExpand（）程序的偽代碼如下所示。

3 實驗過程及結(jié)果評價

為了驗證改進的A*算法的性能和適用范圍，在Matlab R2019a仿真平臺中，構(gòu)建多障礙物的柵格環(huán)境下進行了仿真。

首先，構(gòu)建如圖5（a）所示的柵格地圖，尺寸為100×100。其中，黑色柵格表示障礙物區(qū)域；白色柵格表示無障礙物區(qū)域；左上方和右下方的兩處黑色圓分別表示坐標為（5，5）的起始點、坐標為（95，80）的目標點。并在此柵格地圖上，運行傳統(tǒng)A*路徑規(guī)劃算法。由圖5（b）可得傳統(tǒng)A*算法規(guī)劃的路徑有幾處明顯與障礙物相交。

圖5 柵格圖構(gòu)建、傳統(tǒng)A*仿真結(jié)果示意圖Fig.5 Raster Diagram Construction and Traditional A* Simulation Results

再對傳統(tǒng)A*算法添加障礙物膨脹處理，其仿真結(jié)果，如圖6（a）所示。規(guī)劃的路徑與障礙無一處相交。算法對比是在安全規(guī)劃的前提下進行，因此后序?qū)Ρ葦?shù)據(jù)都在傳統(tǒng)A*添加障礙物膨脹處理后進行。如圖6（b）所示，改進A*算法的仿真結(jié)果。改進A*算法相對于A*算法少了很多拐點，路徑更加流暢。

圖6 帶膨脹A*仿真結(jié)果、改進A*算法仿真結(jié)果示意圖Fig.6 Schematic Diagram of Simulation Results with Expansion A* and Improved A* Algorithm

接著將圖5（a）柵格圖的障礙物不變，分別調(diào)整初始點和目標點，初始點為坐標為（5，95），目的點坐標為（95，5）。在新調(diào)整的柵格圖中運行傳統(tǒng)A*和改進A*算法，其中因為地圖障礙物未發(fā)生變化，其特征也應(yīng)相同，此次改進A*算法運行時直接調(diào)用上次角點檢測結(jié)果。如圖7所示，最后仿真結(jié)果中改進A*算法相對于A*算法規(guī)劃的路徑少了很多拐點，更加流暢。

圖7 帶膨脹A*仿真結(jié)果、改進A*算法仿真結(jié)果示意圖Fig.7 Schematic Diagram of Simulation Results with Expansion A* and Improved A* Algorithm

對兩種柵格圖重復(fù)實驗50次，得到表1數(shù)據(jù)。表1中一次規(guī)劃是指在全新地圖上的規(guī)劃，再次規(guī)劃是指在相同地圖不同始末點上進行的規(guī)劃。路徑代價評價指標分為路徑長度和路徑總轉(zhuǎn)角。

表1 兩種實驗下的算法路徑代價與實時性Tab.1 Path Cost and Search Time of Algorithm in Two Experiments

可以看到兩種實驗中改進A*算法路徑長度傳統(tǒng)A*優(yōu)化率為（2.5～3.6）%，路徑總轉(zhuǎn)角降低（62.57～65.34）%；并且在尋路時間上改進A*算法也有優(yōu)勢，特別是在二次規(guī)劃的實驗下提升高達43.93%。這是因為二次規(guī)劃就是在相同地圖下的再次規(guī)劃，改進A*算法可以直接提取上次規(guī)劃中特征點，加速路徑規(guī)劃。

4 結(jié)論

針對傳統(tǒng)路徑規(guī)劃算法中基于采樣的方法無法得到最優(yōu)解，且基于搜索的方法計算量大、耗時長的情況，以傳統(tǒng)A*算法為基礎(chǔ)結(jié)合基于采樣路徑規(guī)劃的思想，提出了對地圖特征定向采樣的改進A*路徑規(guī)劃算法，其目的是在確保安全的前提下，兼顧算法的效率和最優(yōu)性。此算法具有更好的安全性前提。路徑搜索前進行了障礙物膨脹處理，在有無障礙物膨脹的對比實驗中，障礙物膨脹處理能有效地保證后續(xù)算法規(guī)劃路徑安全無碰撞。此算法所得路徑最優(yōu)性更高。在兩次改進A*與傳統(tǒng)A*實驗的對比中，路徑長度縮短（2.5～3.6）%；路徑總轉(zhuǎn)角降低（62.57～65.34）%。此算法在特定情景下算法效率有顯著提升。在一次規(guī)劃對比實驗中，改進A*算法規(guī)劃效率有略微提升，為6.51%。但是在同地圖不同始末點的再次規(guī)劃實驗中，改進A*算法規(guī)劃效率提升了高達49.93%。