特征點提取下的AGV柵格法建模與分析

趙 江，孟晨陽，王曉博，郝崇清，李 冉，劉慧賢，王昭雷

1.河北科技大學(xué) 電氣工程學(xué)院，石家莊 050018

2.國網(wǎng)河北省電力有限公司，石家莊 050051

自動引導(dǎo)車（AGV）是一種具有安全防護(hù)和多種裝載功能的運輸小車。它有多個自動引導(dǎo)裝置，可以按規(guī)定的路徑行駛[1]。由于AGV既可以應(yīng)用于碼頭的自動裝卸，又可以提供家庭服務(wù)[2]，近年來受到越來越多的關(guān)注。由于路徑規(guī)劃是AGV實現(xiàn)功能的基礎(chǔ)，所以該問題也成為機(jī)器人領(lǐng)域研究的熱點。路徑規(guī)劃是指在具有障礙物的環(huán)境約束下，根據(jù)給定的起點和終點，應(yīng)用算法準(zhǔn)則，為AGV找到一條最優(yōu)或接近最優(yōu)的、安全、無障礙路徑[3-4]。AGV路徑規(guī)劃可分為靜態(tài)規(guī)劃和動態(tài)規(guī)劃兩種。其中靜態(tài)路徑規(guī)劃也稱為全局或離線路徑規(guī)劃，所有的環(huán)境信息均為已知；動態(tài)路徑規(guī)劃問題則是基于傳感器對周圍環(huán)境的感知，由于其環(huán)境信息為全部未知或部分未知，也被稱為局部或在線路徑規(guī)劃問題。本文研究的是已知環(huán)境下的全局靜態(tài)路徑規(guī)劃問題，通過選擇柵格法來解決這類問題。

解決靜態(tài)路徑規(guī)劃的主要方法有粒子群優(yōu)化算法[5]、蟻群算法[6]、A*算法[7]、人工勢場法[8]等，這些算法通常在柵格圖環(huán)境模型中對路徑進(jìn)行搜索。其中蟻群算法是一種概率仿生算法，隨著種群的迭代得到最優(yōu)路徑[9]；A*算法是一種啟發(fā)式路徑規(guī)劃算法，它通過判斷從起點到當(dāng)前點的距離和從當(dāng)前點到終點的距離的啟發(fā)式信息之和來尋找最優(yōu)路徑[10]；人工勢場法是一種虛擬力法[11]，它通過引力和斥力的相互作用將AGV引向終點。研究人員對上述算法進(jìn)行了大量的研究，并提出了許多優(yōu)化方案。

傳統(tǒng)的蟻群算法存在迭代次數(shù)多、搜索效率低等缺點。為了解決這些問題，Lee[12]提出了一種基于異構(gòu)蟻群的路徑規(guī)劃方法，該方法改進(jìn)了蟻群的轉(zhuǎn)移概率函數(shù)和信息素更新規(guī)則，在不需要后續(xù)平滑處理的情況下可以直接找到節(jié)點數(shù)較少的最優(yōu)路徑；Zhang等[13]提出了一種智能蟻群路徑規(guī)劃算法，該方法通過消除蟻群算法中禁忌表的約束，引入臨時權(quán)值矩陣，避免了小權(quán)值路徑的重復(fù)選擇，提高了算法的效率；Lee等[14]改變了蟻群算法生成初始種群的方式，縮短了尋找最優(yōu)解的時間，加快了算法的收斂速度；Saidi-Mehrabad[15]將蟻群算法分為兩個階段進(jìn)行路徑搜索，有效地縮短了路徑規(guī)劃的完成時間。

傳統(tǒng)的A*算法搜索過程盲目，規(guī)劃出的路徑轉(zhuǎn)彎次數(shù)過多，導(dǎo)致AGV的運行損耗過大。為了解決這些問題，Yu等[16]提出了一種蟻群優(yōu)化算法和A*算法相結(jié)合的雙層算法，其中蟻群算法負(fù)責(zé)確定目標(biāo)的移動順序，A*算法負(fù)責(zé)快速建立目標(biāo)之間的移動成本圖，以便于蟻群算法的計算；Song等[17]發(fā)現(xiàn)A*算法規(guī)劃的路徑包含大量的拐點，在實踐中會影響AGV的運行，因此提出了一種改進(jìn)的A*算法，并在規(guī)劃后對路徑進(jìn)行了平滑處理。

為解決傳統(tǒng)人工勢場法局部最優(yōu)問題，Sudhakara等[18]在人工勢場法的基礎(chǔ)上，提出了一種不考慮排斥力和吸引力影響的優(yōu)化算法，通過構(gòu)建一種排斥函數(shù)產(chǎn)生排斥勢場，并將其應(yīng)用于任意形狀的障礙物上，通過該方法可以為移動機(jī)器人提供更為準(zhǔn)確的路徑；Orozco-Rosas等[19]提出將遺傳算法與人工勢場方法相結(jié)合，找到一條可行的安全路徑；Zhou等[20]提出用粒子群算法改進(jìn)人工勢場法，引入切向量對規(guī)劃出的路徑進(jìn)行優(yōu)化，并在人工勢場模型中加入基于障礙物信息的矢量來輔助完成避障過程，大大改進(jìn)了人工勢場法的局部最優(yōu)問題。

利用上述改進(jìn)算法在柵格圖中進(jìn)行路徑規(guī)劃的本質(zhì)仍是比較所有相鄰無障礙柵格的估計函數(shù)，即計算柵格圖中每個相鄰可行柵格的估計函數(shù)，然后通過比較估計值得到下一個路徑點。這種逐點規(guī)劃的方法存在著大量的無效計算，導(dǎo)致算法的效率大幅降低。為了解決這一問題，卜新蘋等[21]采用四叉樹方法對傳統(tǒng)柵格圖進(jìn)行分割，減少柵格的數(shù)量，并對分割后柵格的連接關(guān)系進(jìn)行定義，結(jié)果表明，減少柵格數(shù)可以在不影響路徑質(zhì)量的情況下縮小搜索范圍，提高算法的計算效率，并減少路徑中的節(jié)點數(shù)。Han[22]提出了一種關(guān)鍵障礙物及其周圍點的提取方法，該方法根據(jù)障礙物的不同重要性，在環(huán)境中找到相對重要的障礙物，并找到與這些障礙物相關(guān)的柵格點，從而降低了三維路徑規(guī)劃的復(fù)雜度，大量仿真實驗表明，該方法可以提高三維路徑規(guī)劃的效率，并減少路徑中拐點的數(shù)量，從而提高移動機(jī)器人的運行效率。

在使用四叉樹方法對柵格圖進(jìn)行分割時，對于不同的環(huán)境和不同的路徑規(guī)劃要求會有不同的分割原則，并且在重新建立柵格間的連接關(guān)系時，頻繁的入棧和出棧操作可能會導(dǎo)致計算量過大。而提取關(guān)鍵障礙物及其周圍柵格點的方法僅適用于對一條路徑進(jìn)行微調(diào)，在尋找不同的路徑時，柵格間的重要性關(guān)系會發(fā)生改變，從而影響路徑規(guī)劃的過程。為了解決上述問題，本文提出了一種基于特征點提取的環(huán)境建模方法，該方法通過提取每個障礙物的頂點柵格作為特征點對環(huán)境進(jìn)行建模，以期得到一個更為簡化且適用面更廣的環(huán)境模型，從而有效地縮小路徑規(guī)劃算法的搜索范圍，提高算法的搜索效率，得到一條更為優(yōu)化的路徑。

1 傳統(tǒng)柵格法建模及其改進(jìn)

1.1 傳統(tǒng)柵格模型的建立

在進(jìn)行路徑規(guī)劃前，需要對AGV的運行環(huán)境進(jìn)行建模。選擇的建模方法通常為柵格法，柵格法建模的方法如下。

對機(jī)器人運行環(huán)境進(jìn)行量化處理，形成一些以AGV投影尺寸為單位的小單元，作為一個環(huán)境的網(wǎng)格映射。根據(jù)環(huán)境的大小設(shè)置相應(yīng)的網(wǎng)格映射為m×m。網(wǎng)格映射模型如圖1所示。圖中紅色為起點，綠色為終點，黑色網(wǎng)格表示量化后的障礙物。該圖所示起點坐標(biāo)表示為S=( 4 .5,4.5)，終點坐標(biāo)表示為E=(1 5.5,15.5)。

圖1 柵格法建模圖示Fig.1 Grid model diagram

柵格圖內(nèi)的每個格都可以設(shè)定為一個小的單元，數(shù)學(xué)表達(dá)為cell(x,y)，其中x、y表示每個小單元的位置。根據(jù)每個網(wǎng)格是否為障礙物，給每個單元設(shè)置為：

當(dāng)機(jī)器人遇到障礙物的時候?qū)?dāng)前單元格記錄為1，表示障礙物柵格。相對于障礙物柵格其他的柵格為0，表示可行柵格。柵格還可以儲存機(jī)器人路徑信息，表示為相關(guān)屬性和訪問屬性，同樣?xùn)鸥褚部梢杂涗涀陨硖厥庑畔ⅰ?/p>

1.2 傳統(tǒng)柵格模型的路徑規(guī)劃

建立環(huán)境模型后，需要確定AGV的起點和終點。如圖2所示，S為起點，E為終點，找到一條從S到E的有效路徑，這個過程稱為路徑規(guī)劃。

圖2 傳統(tǒng)柵格圖中的路徑規(guī)劃Fig.2 Path planning in traditional grid graph

傳統(tǒng)的AGV路徑通常根據(jù)特定的性能指標(biāo)（最短路徑、最短時間、最小損耗等）進(jìn)行規(guī)劃。但在實際應(yīng)用中，不僅要考慮路徑長度和運行時間，還要考慮AGV運行時的磨損、與障礙物之間的安全距離和算法的計算效率等。而在AGV的實際運行中，由于規(guī)劃的路徑通常是分段線性路徑，AGV必須在每個換向節(jié)點處停止，根據(jù)下一個路徑改變其方向，然后重新啟動?；趥鹘y(tǒng)柵格圖的路徑規(guī)劃方法在尋找最短路徑時往往忽略了節(jié)點過多而造成的影響。因此，減少換向節(jié)點對提高AGV的運行效率和減少AGV的磨損具有重要的作用。

另外，基于傳統(tǒng)柵格法建模的路徑規(guī)劃算法在搜索路徑時，往往通過判斷當(dāng)前節(jié)點周圍的節(jié)點信息來選擇下一個節(jié)點。要計算的柵格數(shù)隨著柵格圖分辨率的增加而增加，從而降低了搜索速度。

為了使算法在不影響解的質(zhì)量的情況下提高對路徑搜索的目的性，本文提出了一種基于特征點提取的柵格圖模型，以減少算法的計算量，使AGV的路徑更加平滑。

1.3 基于特征柵格提取的柵格模型

（1）特征提取規(guī)則

在路徑規(guī)劃問題中，只要起點和終點互不可見，基于柵格法規(guī)劃出的最短路徑的有效拐點往往是障礙物柵格的頂點。然而，由于逐層規(guī)劃的弊端，有效拐點之間往往存在大量無效拐點。當(dāng)兩個可相互到達(dá)的有效拐點之間的路徑是一條線段時，該路徑為兩個有效拐點之間的最佳路徑，因此將特征柵格提取規(guī)則定義如下：

規(guī)則1特征柵格可以是與單個黑色柵格的四個頂點處相對應(yīng)的可行柵格。

規(guī)則2特征柵格也可以是與組合黑色柵格的所有凹角處相對應(yīng)的可行柵格。

規(guī)則3特征柵格數(shù)量應(yīng)與障礙物柵格數(shù)量相關(guān)，障礙物數(shù)量越多特征柵格數(shù)量越多。

（2）障礙物網(wǎng)絡(luò)模塊化處理

結(jié)合特征柵格提取規(guī)則，本文利用蒙特卡羅搜索的方法來分析處理環(huán)境柵格模型，利用各種網(wǎng)絡(luò)模型來合理區(qū)分各種類型的障礙物柵格，由蒙特卡洛搜索分解出的基本障礙物柵格如圖3所示。

圖3 網(wǎng)絡(luò)模型化處理下的障礙物示意圖Fig.3 Obstacle diagram based on network modeling

通過蒙特卡洛模擬可以得出上述十二種基本障礙物形狀，其可以組合出柵格法建模中任意障礙物形狀。通過特征柵格的提取規(guī)則可以得出各種障礙物形狀對應(yīng)的特征柵格。結(jié)合特征提取規(guī)則，任意環(huán)境中都可以提取出相應(yīng)的特征柵格，特征柵格可以代表柵格法路徑規(guī)劃的全部環(huán)境信息。

柵格法建模是將AGV運行的工作環(huán)境單元分割為大小相等的方塊，形成一組網(wǎng)格，網(wǎng)格圖中每一個柵格都可以作為一個二值信息網(wǎng)格單元來存儲環(huán)境信息，這些信息可以包含人為指定信息，也可包含算法規(guī)劃信息，每個網(wǎng)格被稱為環(huán)境單元。柵格法建模將整個環(huán)境進(jìn)行抽象建模，路徑規(guī)劃時采用相應(yīng)的路徑規(guī)劃搜索算法在該空間內(nèi)進(jìn)行搜索。通過特征提取規(guī)則選擇特征柵格，其周圍都會有白色可行柵格，特征柵格可以表示周圍白色可行柵格的環(huán)境信息。基于柵格法規(guī)劃出的最短路徑中的有效拐點往往是黑色柵格對應(yīng)的可行頂點柵格，而其他柵格只是作為可行柵格進(jìn)行規(guī)劃，特征柵格可以很好地表達(dá)周圍白色可行柵格的環(huán)境信息，在簡化環(huán)境模型的同時，保證了環(huán)境信息的完整性。

（3）新型柵格法建模結(jié)果

基于上述規(guī)則和柵格建模方法，可以得出在柵格建模環(huán)境下，膨脹化的障礙物是由多個正方形組成的不規(guī)則圖形，由提取規(guī)則1、2，特征柵格的數(shù)量應(yīng)與障礙物柵格數(shù)量相關(guān)，即障礙物柵格數(shù)量越多特征柵格數(shù)量越多。根據(jù)規(guī)則1、規(guī)則2和規(guī)則3提取特征網(wǎng)格，提取結(jié)果如圖3所示。

由圖4可知，從6×6網(wǎng)格模型中提取了10個頂點，搜索范圍從25個柵格縮小為10個柵格，減少了柵格法建模產(chǎn)生的60%無效柵格，新型柵格法建模減少了算法的搜索范圍，局部路徑規(guī)劃算法只需要對特征柵格進(jìn)行路徑規(guī)劃，大大提高了算法的搜索效率。

圖4 特征柵格提取Fig.4 Feature grid extraction

在提取頂點后，大部分提取的柵格不再是連續(xù)柵格，因此需要通過柵格可見性判斷的方法重建每個柵格的鄰接矩陣。柵格之間的連接可以分為三種類型，如圖5所示。

圖5 柵格可見性判斷Fig.5 Grid visibility judgment

類型一：圖中的柵格a和b之間沒有障礙物，皆為白色柵格，因此，定義柵格a和b是可見的，線段l1為可行路徑。

類型二：圖中的柵格a和c之間有障礙物，存在黑色柵格，因此，定義柵格a和c是不可見的，線段l2為不可行路徑。

類型三：圖中的柵格a和d之間雖未直接經(jīng)過障礙物柵格，但線段l3卻與黑色障礙物柵格交于點（1，2），由于AGV自身存在體積，為了AGV運行時的安全性，該類路徑應(yīng)被定義為不可行路徑，然而，在傳統(tǒng)的基于柵格法的路徑規(guī)劃算法中，并沒有將該類不可行路徑排除，使AGV的運行路徑存在隱患，所以本文提出的將該類路徑定義為不可行路徑的方法，一定程度上提升了AGV運行時的安全性。

1.4 新型柵格法建模性能分析

柵格環(huán)境建模方法是將機(jī)器人周圍空間分解為互相連接且不重疊的空間單元，空間單位可以表示為柵格，柵格法建模分為精細(xì)化柵格法和近似化柵格法兩種方式。新型柵格法建模屬于近似柵格法建模，將障礙物做膨脹化處理，柵格不僅可以完整地保存環(huán)境信息還可以保證AGV在環(huán)境中的運行安全。新型柵格法建模提取的是黑色柵格頂角位置的白色柵格，由于柵格法對障礙物膨脹化處理，通過特征提取到的白色柵格保證了原有柵格法建模對環(huán)境信息的完整性，并且保證了AGV在環(huán)境中的運行安全。

相對于傳統(tǒng)柵格法建模中將全局環(huán)境作為有效信息輸入到AGV的路徑規(guī)劃中，新型柵格法建模大量減少了傳統(tǒng)柵格法中需要運算搜索的柵格數(shù)量，使用30%～40%的特征柵格就可以表示全局環(huán)境信息。圖6中，數(shù)字標(biāo)出的是特征點提取下的有效柵格，障礙物邊緣是路徑規(guī)劃算法運算的重要位置，將重要位置提取出來，不僅可以保證環(huán)境信息的完整性還可以保證AGV路徑規(guī)劃的搜索速度。同時使得AGV的搜索更加具有目的性，并結(jié)合傳統(tǒng)柵格法建模的優(yōu)點，保證了AGV運行的安全。

圖6 新舊柵格法對比分析Fig.6 Comparative analysis of new and old grid method

針對局部路徑規(guī)劃算法，如蟻群算法、A*算法和人工勢場法等需要對每個白色柵格進(jìn)行算法估計，而黑色柵格頂點處白色柵格上的局部路徑規(guī)劃算法估計值變化巨大，新型柵格法建模選擇提取單個黑色柵格的四個頂點處相對應(yīng)的可行柵格和組合黑色柵格的所有凹角處相對應(yīng)的可行柵格作為特征柵格，可以保證特征點具有代表性。

1.5 新舊路徑對比分析

基于特征點提取的思想，將傳統(tǒng)柵格圖中的障礙物頂點選為特征點提取出來，在進(jìn)行了頂點可視性判斷、確定頂點間的連接關(guān)系之后，重新在該環(huán)境下進(jìn)行路徑規(guī)劃，得到的結(jié)果如圖7所示。

在圖7中，黑色路徑是傳統(tǒng)柵格模型中規(guī)劃的路徑，紅色路徑是特征柵格提取的環(huán)境模型中規(guī)劃的路徑。

圖7 新舊路徑對比分析Fig.7 Comparative analysis of old and new paths

兩條路徑之間的主要數(shù)據(jù)比較如表1所示。從路徑長度上看，相比于傳統(tǒng)柵格圖下的路徑規(guī)劃算法，基于特征柵格提取的柵格法規(guī)劃出的路徑由于搜索范圍被嚴(yán)格約束在了障礙物的頂點上，在進(jìn)行路徑規(guī)劃時會大概率選擇路徑ab，而不選擇路徑acb。由三角形法則可得，在三角形abc中，ac+cb＞ab，最終使得路徑長度變短；從換向節(jié)點數(shù)和路徑總轉(zhuǎn)角上看，由于特征柵格提取的柵格圖中，路徑從柵格a到b沒有經(jīng)過c，減少了拐點c之后，由d到g減少了三個90°的拐角，轉(zhuǎn)化成了兩個45°的角，使得路徑總轉(zhuǎn)角大幅減少，從而減少了AGV運行時的損耗；從平均安全距離上看，基于特征點提取的柵格圖中加大了路徑的平均安全距離，有利于AGV的安全運行。

表1 新舊路徑的主要數(shù)據(jù)對比Table 1 Comparison of main data between old and new paths

2 新環(huán)境模型在典型路徑規(guī)劃算法中的應(yīng)用

為了驗證所提出的環(huán)境建模方法的有效性，將其應(yīng)用于多種不同類型的路徑規(guī)劃算法中，包括虛擬力法、直接搜索法和進(jìn)化算法，并解決算法中存在的運行時間長、局部最優(yōu)等問題。柵格法為全局搜索算法，新型柵格法適用于基于先驗完全信息的局部路徑規(guī)劃方法，例如A*算法（直接搜索法）、人工勢場算法（虛擬力法）和蟻群算法（進(jìn)化算法）等。

2.1 新柵格模型在人工勢場法中的應(yīng)用

人工勢場法是Khatib提出的一種虛擬力法，其思想是將AGV、目標(biāo)點和障礙物視為質(zhì)點，將AGV運動環(huán)境抽象為一個具有吸引力和排斥力的力場。在這個力場中，障礙物提供排斥力，目標(biāo)點提供吸引力，根據(jù)AGV的位置產(chǎn)生合力決定AGV的運動方向，其實質(zhì)是根據(jù)環(huán)境建立一個模擬力場。

人工勢場法的數(shù)學(xué)模型如式（1）所示：

式（1）中，U為小車的勢能之和，Uatt為目標(biāo)點對AGV的吸引力，Urep為障礙物對AGV的排斥力，再將引力場對AGV的引力梯度和斥力場對AGV的斥力梯度通過拉格朗日變換得出力的關(guān)系為：

式（2）中，F(xiàn)att為目標(biāo)點對AGV的吸引力，F(xiàn)rep為障礙物對AGV的排斥力，AGV所受的合力如圖8所示。

圖8 柵格圖中AGV所受合力Fig.8 Resultant force of AGV in grid graph

設(shè)AGV在工作空間中的位置為p=(x,y)T，則其引力勢函數(shù)為：

k為大于零的引力勢場常量，p為AGV位置向量，pe為AGV在勢場中的目標(biāo)位置，則引力勢函數(shù)的負(fù)梯度為：

該引力隨著AGV趨近于目標(biāo)點而趨近于零。根據(jù)Khatib提出的一種FIARS函數(shù)得出斥力勢場公式如下：

式（5）中，η為當(dāng)前障礙物對AGV有影響時的斥力勢場常量，ρ為AGV所在位置與障礙物的最短距離，ρ0為單個障礙物所能影響的最大距離范圍，當(dāng)AGV與障礙物的距離大于ρ0時斥力場對AGV的影響為零。斥力場產(chǎn)生的斥力為：

根據(jù)上述理論依據(jù)可得人工勢場法的基本流程如下：

步驟1參數(shù)初始化：設(shè)置引力常量、斥力常量、起點以及目標(biāo)點。

步驟2獲取當(dāng)前AGV狀態(tài)，根據(jù)公式（2）判斷此時機(jī)器人的受力情況，確定接下來AGV的移動方向。

步驟3判斷AGV是否到達(dá)目標(biāo)點，如果到達(dá)，則結(jié)束程序，否則返回步驟2。

雖然人工勢場法能有效地解決AGV與障礙物的碰撞問題，但傳統(tǒng)的人工勢場法路徑規(guī)劃方法仍存在以下問題：（1）當(dāng)AGV移動到某一位置時，吸引力和排斥力之和為零，AGV會錯誤地認(rèn)為已經(jīng)到達(dá)了目標(biāo)位置并停止，這是一個典型的局部最優(yōu)問題；（2）傳統(tǒng)人工勢場法產(chǎn)生的路徑會頻繁經(jīng)過障礙物的邊緣，無法保持有效的安全距離，從而影響AGV的安全運行。

為了解決上述問題，將基于特征點提取的柵格模型應(yīng)用到人工勢場法中。由于該柵格模型的引入，由AGV所受到的合力確定AGV運動方向的方法不再適用，所以不再計算每個柵格的各個方向的合力，而是計算其勢能，然后通過勢能下降的原理確定AGV的運行方向。

在柵格模型中利用人工勢場法規(guī)劃路徑時，通常對每個柵格賦值，將導(dǎo)致AGV的運行在環(huán)境中缺乏有效的信息導(dǎo)向。因此，根據(jù)本文提出的特征點提取的柵格模型，在計算勢能時，只需計算提取的特征柵格的勢能，這樣可以為AGV的運行提供更有效的信息指導(dǎo)，從而解決人工勢場法路徑規(guī)劃中的局部最優(yōu)問題，提高AGV與障礙物之間的安全距離，保障AGV運行時的安全。

通過對人工勢場法在新型柵格圖中的應(yīng)用，可以看出新型柵格圖可以很好地解決應(yīng)用虛擬力法時移動機(jī)器人的路徑規(guī)劃問題，對于以模擬退火算法和人工勢場法為代表的虛擬力法具有很好的適用性。

2.2 新柵格模型在A＊算法中的應(yīng)用

A*算法是一種根據(jù)給定的代價函數(shù)，在靜態(tài)環(huán)境中尋找從起點到目標(biāo)點最優(yōu)路徑的一種直接搜索方法。并通過在Dijkstra算法中加入啟發(fā)式函數(shù)，提高算法的計算效率，其數(shù)學(xué)模型為：

式（6）中，f(x)是從起點S通過x位置到達(dá)終點E的最優(yōu)成本，g(x)表示從位置S到目標(biāo)點x的實際成本，h(x)表示從目標(biāo)點x到終點E的估計成本。如圖9所示，所以估計成本中min(f(x))被選擇的概率較高。因此，A*算法在規(guī)劃路徑時總是選擇min(f(x))的節(jié)點。

圖9 利用A*算法進(jìn)行路徑規(guī)劃Fig.9 Path planning using A*algorithm

每個柵格左上角的數(shù)字表示最優(yōu)成本f(x)，左下角的數(shù)字表示從起點S到位置x的實際成本g(x)，右下角的數(shù)字表示從位置x到目標(biāo)點E的估計成本h(x)。

根據(jù)上述理論依據(jù)可得A*算法的基本流程如下：

步驟1初始化參數(shù)，設(shè)置開發(fā)列表Q，設(shè)置起點和終點。

步驟2根據(jù)公式（6），選擇Q中的最小f(x)的節(jié)點x作為下一個路徑點。

步驟3確定搜索是否已到達(dá)終點，如果是則停止算法，如果不是則轉(zhuǎn)到步驟4。

步驟4搜索擴(kuò)展，對圖中每兩個節(jié)點m、n進(jìn)行比較。如果g(m)＞g(n)+cost(n,m)則g(m)=g(n)+cost(n,m),f(m)=g(m)+h(m)，并將m點加入到開放列表Q中，否則，m點不加入到Q中，返回步驟2。

A*算法在Dijkstra算法中增加了一個啟發(fā)式函數(shù)。盡管A*算法有效地減少了搜索范圍和計算復(fù)雜度，但它仍然需要估計柵格圖中每個節(jié)點的距離，用h(x)表示當(dāng)前節(jié)點和目標(biāo)節(jié)點之間的成本，并通過選擇最小的f(x)來選擇最優(yōu)路徑。當(dāng)環(huán)境較大時，柵格圖中大量的柵格將延長A*算法的搜索時間。因此，采用新柵格法來解決這個問題。

首先提取障礙物的每個頂點柵格作為特征柵格，只計算特征柵格的估計值。A*算法在搜索柵格時，只搜索這些特征柵格，從而減少A*算法在柵格模型中搜索的柵格數(shù)，避免A*算法搜索時間過長和范圍過大。另外，由于新的柵格建模方法提取的特征柵格不再是簡單的水平或垂直關(guān)系，因此使用歐幾里德距離代替曼哈頓距離來計算估計值，該方法還可以有效地減少規(guī)劃出的路徑的轉(zhuǎn)彎次數(shù)和角度。

將新型柵格法應(yīng)用于A*算法中可以有效減少A*算法的搜索面積，提高搜索速度，對于以A*算法和Dijkstra算法為代表的搜索算法有很好的適用性。新型柵格法建模有效提高了算法的性能。

2.3 新柵格模型在蟻群算法中的應(yīng)用

蟻群算法是一種通過模擬自然界螞蟻尋找最短覓食路徑的仿生算法。每只螞蟻在覓食時都會在路徑上留下一定濃度的信息素，并且由于信息素會揮發(fā)，所以路徑越長，信息素濃度越低。隨后的螞蟻更傾向于沿著信息素濃度較高的路徑行走，并繼續(xù)在路徑上留下新的信息素。通過上述正反饋效應(yīng)，最終螞蟻們行走的路線將收斂到信息素濃度最高的最短路徑上。蟻群算法的數(shù)學(xué)模型包括節(jié)點選擇策略、信息素更新和鄰接矩陣三個部分。

節(jié)點選擇策略是指當(dāng)螞蟻位于柵格中時，為了從備選柵格中選擇一個柵格進(jìn)行轉(zhuǎn)移，需要使用狀態(tài)轉(zhuǎn)移概率公式來選擇下一個柵格，螞蟻k從柵格i到柵格j的轉(zhuǎn)移概率為：

其中，allowed k={c~-tabuk}代表每只螞蟻下一步中的所有可選柵格，c~為所有柵格的集合，tabuk為第k只螞蟻已經(jīng)走過的所有柵格的集合，τij(t)為當(dāng)前時刻i,j兩個柵格之間在t時刻的信息素濃度，ηij(t)為啟發(fā)函數(shù)，通常取ηij(t)=1/d ij，α為信息啟發(fā)因子，表示信息素的重要程度，α越大螞蟻之間的協(xié)作也就越強(qiáng)，β為啟發(fā)式因子，表示啟發(fā)函數(shù)的重要程度。

在路徑搜索過程中，為了避免信息素濃度過高導(dǎo)致啟發(fā)式信息失效，通常在搜索路徑后需要對信息素進(jìn)行更新。更新規(guī)則如式（8）和（9）所示：

其中，ρ∈(0,1)為信息素?fù)]發(fā)因子，其大小關(guān)系到算法的全局收斂能力和收斂速度表示第k只螞蟻在柵格i和j之間留下的信息素，有三種不同的蟻群算法模型，“蟻周系統(tǒng)”（Ant-Cycle）模型，“蟻量系統(tǒng)”（Ant-Quantity）模型和“蟻密系統(tǒng)”（Ant-Density）模型，其中蟻周系統(tǒng)算法性能優(yōu)于其余兩種算法，其公式如下：

蟻群算法通過鄰接矩陣進(jìn)行判斷兩點之間是否可達(dá)，在n×n的柵格圖中，鄰接矩陣D是一個大小為n2×n2矩陣，當(dāng)i,j兩個柵格之間不可形成路徑時，D(i,j)=0，當(dāng)i,j兩個柵格之間可以形成路徑時，D(i,j)=d ij。

根據(jù)上述理論依據(jù)可得蟻群算法的基本流程如下：

步驟1初始化α,β,ρ,M,N等參數(shù)。

步驟2將螞蟻置于起點，并準(zhǔn)備開始搜索路徑。

步驟3通過式（7），在鄰接矩陣D(i,j)≠0的柵格中選擇下一個節(jié)點。

步驟4更新禁忌表tabuk。

步驟5判斷所有螞蟻是否都完成了搜索，如果完成搜索進(jìn)入下一步，否則返回步驟3。

步驟6根據(jù)式（8）～（10）更新信息素。

步驟7判斷是否達(dá)到最大迭代次數(shù)，如果是則輸出最優(yōu)路徑，否則返回步驟2，開始新一輪的搜索。

蟻群算法作為一種生物進(jìn)化算法，其收斂速度是評價算法性能的重要指標(biāo)。然而，傳統(tǒng)的蟻群算法存在收斂速度慢的問題。這是由于螞蟻在搜索路徑時需要考慮的路徑數(shù)量龐大，大大影響了算法的計算效率。應(yīng)用本文提出的基于特征點提取的柵格模型，可以在不影響求解質(zhì)量的前提下，減少對差解的搜索次數(shù)，從而提高蟻群算法的收斂速度。

在蟻群算法中，柵格間的連接關(guān)系是由鄰接矩陣D確定的。傳統(tǒng)蟻群算法的鄰接矩陣是通過判斷該柵格周圍的8個柵格是否為障礙物柵格求得的，設(shè)起點柵格為i，判斷柵格i+1與其的連接關(guān)系時，只需判斷柵格i+1是否為障礙物柵格或非相鄰柵格，若是則鄰接矩陣D(i,i+1)=0，若不是，則鄰接矩陣D(i,i+1)=d i+1。而當(dāng)將障礙物頂點作為特征點提取出來后，判斷的不再是i與其相鄰柵格的連接關(guān)系，而是所有的障礙物頂點柵格與柵格i的連接關(guān)系。通過第1章中提到的可視性判斷，當(dāng)柵格i與頂點柵格j可見時，鄰接矩陣D(i,j)=d ij,否則D(i,j)=0。

將新型柵格法建模應(yīng)用于蟻群算法時有效減少了算法的搜索次數(shù)，大大提高了算法的收斂速度，說明新型柵格法建模適用于以蟻群算法、粒子群算法為代表的概率型算法。

通過以上的理論分析，可以得出局部路徑規(guī)劃算法使用二值信息的網(wǎng)格單元來儲存信息，都可以選擇用柵格法為工作環(huán)境進(jìn)行建模。例如柵格可以存儲A*算法中每一個位置的估計值，也可以存儲蟻群算法每一個位置的信息素濃度。當(dāng)移動機(jī)器人選擇可以使用柵格來存儲算法信息的路徑規(guī)劃算法時，可以選擇新型柵格法建模來完成環(huán)境建模。

3 仿真結(jié)果

為了驗證新的柵格建模方法的適用性，將其應(yīng)用于三種不同的路徑規(guī)劃算法：人工勢場法、A*算法和蟻群算法中，并與傳統(tǒng)柵格法的路徑規(guī)劃進(jìn)行比較。

為了保證實驗的客觀性，仿真中只改變路徑規(guī)劃中各算法的環(huán)境模型，而不改變其具體的算法參數(shù)。柵格的邊長設(shè)置為1，障礙物的密度和分布是隨機(jī)產(chǎn)生的，在10×10、20×20和30×30的柵格地圖上仿真并顯示三組不同算法的結(jié)果。

3.1 新柵格法模型應(yīng)用于人工勢場法的結(jié)果

在三個柵格地圖中，起點S的坐標(biāo)分別為（0.5，0.5），（1.5，18.5）和（2.5，27.5），終點E的坐標(biāo)分別為（9.5，9.5），（19.5，3.5）和（24.5，1.5）。設(shè)k為1.72，圖10（a）、（b）和（c）上圖顯示了人工勢場法在傳統(tǒng)柵格法規(guī)劃出的路徑，下圖顯示了人工勢場法在新型柵格法規(guī)劃出的路徑，其中黑色實線為規(guī)劃路徑。

從圖10可以看出，將基于特征點提取的柵格模型應(yīng)用于人工勢場法中時，可以明顯減少規(guī)劃路徑的轉(zhuǎn)向次數(shù)。另外，傳統(tǒng)柵格圖中利用人工勢場法規(guī)劃出的路徑會頻繁經(jīng)過障礙物柵格的頂點，嚴(yán)重影響AGV的安全運行，這也是人工勢場法很少采用柵格法的主要原因，將基于特征點提取的柵格圖應(yīng)用于人工勢場法路徑規(guī)劃增加了規(guī)劃出的路徑與障礙物柵格之間的安全距離，提高了AGV運行的安全性，使柵格法更適用于人工勢場法的路徑規(guī)劃中。

同時，由圖10（b）可以看出，從柵格圖中提取出特征柵格，可以有效減少柵格圖中局部最優(yōu)點的個數(shù)，從而解決人工勢場法中的局部最優(yōu)問題。在兩種柵格模型中人工勢場法路徑規(guī)劃的主要數(shù)據(jù)對比，如表2～4所示。從表中可以看出，新柵格模型中規(guī)劃出的路徑不再頻繁經(jīng)過障礙物的頂點，提高了車輛行駛時的安全性。此外，新的網(wǎng)格模型中規(guī)劃出的路徑在轉(zhuǎn)彎次數(shù)和總轉(zhuǎn)彎角度上都小于傳統(tǒng)柵格模型中規(guī)劃出的路徑，大大降低了AGV運行時的損耗。

表2 人工勢場法在10×10環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 2 Data comparison of artificial potential field method in 10×10 environmental model

表3 人工勢場法在20×20環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 3 Data comparison of artificial potential field method in 20×20 environmental model

從柵格的計算次數(shù)上可以看出，在人工勢場法應(yīng)用于新的柵格模型中進(jìn)行路徑規(guī)劃后，計算量大大減少。此外，可以得出，新柵格圖的計算量不再依賴于柵格圖的大小，而是取決于環(huán)境中障礙物的形狀和分布。

表4 人工勢場法在30×30環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 4 Data comparison of artificial potential field method in 30×30 environmental model

3.2 新柵格法模型應(yīng)用于A＊算法的結(jié)果

在三個柵格地圖中，起點S的坐標(biāo)分別為（7.5，8.5），（0.5，0.5）和（16.5，23.5），終點E的坐標(biāo)分別為（8.5，0.5），（14.5，19.5）和（13.5，8.5）。圖10（a）、（b）和（c）上圖顯示了A*算法在傳統(tǒng)柵格法規(guī)劃出的路徑，下圖顯示了A*算法在新型柵格法規(guī)劃出的路徑，其中黑色實線為規(guī)劃出的路徑，黑色圓圈所在的柵格為路徑中包含的柵格，灰色柵格為A*算法搜索的全部柵格。

從圖11可以看出，將A*算法應(yīng)用于特征點提取的柵格建模中，可以減少規(guī)劃路徑上換向節(jié)點和非換向節(jié)點的數(shù)量。此外，還可以減少搜索的柵格數(shù)量，從而提高了A*算法的計算效率。

圖11 新舊柵格模型中A*算法路徑規(guī)劃的比較Fig.11 Comparison of A*algorithm path planning in new and old grid models

表5～7給出了A*算法在傳統(tǒng)柵格模型和新柵格模型中進(jìn)行路徑規(guī)劃的主要數(shù)據(jù)比較。從表中可以看出，與A*算法在傳統(tǒng)柵格模型下規(guī)劃的路徑相比，新柵格圖的A*算法規(guī)劃的路徑更短，轉(zhuǎn)彎次數(shù)更少，總轉(zhuǎn)彎角度更小，這樣提高了AGV的運行效率，并減少AGV的運行損耗。更重要的是，A*算法的柵格計算數(shù)量大大減少，由此加快了A*算法的計算速度。

表5 A*算法在10×10環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 5 Data comparison of A*algorithm planning path in 10×10 environment model

表6 A*算法在20×20環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 6 Data comparison of A*algorithm planning path in 20×20 environment model

3.3 新柵格法模型應(yīng)用于蟻群算法的結(jié)果

在三個柵格地圖中，起點S有坐標(biāo)（3.5，9.5），（0.5，19.5）和（0.5，8.5），終點E有坐標(biāo)（2.5，0.5），（18.5，0.5）和（25.5，28.5）。將信息素啟發(fā)因子α設(shè)為1，β設(shè)為12。η設(shè)為1.55。圖12（a）、（b）和（c）上圖顯示了蟻群算法在傳統(tǒng)柵格法規(guī)劃出的路徑，下圖顯示了蟻群算法在新型柵格法規(guī)劃出的路徑，其中黑色實線為規(guī)劃路徑，黑色圓圈為路徑中包含的節(jié)點。從圖12可以看出，蟻群算法在特征點提取的柵格模型中進(jìn)行路徑規(guī)劃，減少了規(guī)劃路徑的拐點數(shù)量，從而降低了AGV的運行損耗。

圖12 新舊柵格模型中蟻群算法路徑規(guī)劃的比較Fig.12 Comparison of ant colony algorithm path planning in new and old grid models

表7 A*算法在30×30環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 7 Data comparison of A*algorithm planning path in 30×30 environment model

蟻群算法的迭代曲線如圖13（a）、（b）和（c）所示，其中實線為蟻群算法尋找到的當(dāng)前最短路徑，虛線為蟻群算法每一代尋找到的最短路徑。

圖13 新舊柵格模型中蟻群算法迭代曲線的比較Fig.13 Comparison of iterative curves of ant colony algorithm in new and old grid models

由圖可以驗證，當(dāng)蟻群算法在特征點提取的柵格模型中路徑規(guī)劃時，算法的效率顯著提高，迭代次數(shù)明顯減少。算法主要數(shù)據(jù)的比較如表8～10所示。

表8 蟻群算法在10×10環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 8 Data comparison of ant colony algorithm in 10×10 environment model

從表中可以看出，基于特征點提取的蟻群算法規(guī)劃出的路徑具有較少的轉(zhuǎn)彎點和總轉(zhuǎn)彎角度的優(yōu)點，這樣可以減少AGV運行時的損耗，此外，還提高了路徑與障礙物之間的平均安全距離。從迭代次數(shù)和節(jié)點計算次數(shù)來看，基于特征點提取的柵格模型排除了原柵格模型中的一些較差解，使得蟻群算法在路徑規(guī)劃時不必考慮這些較差解，從而使路徑選擇更有目的性，大大縮短了算法的計算時間。

表9 蟻群算法在20×20環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 9 Data comparison of ant colony algorithm in 20×20 environment model

表10 蟻群算法在30×30環(huán)境模型中規(guī)劃路徑的數(shù)據(jù)比較Table 10 Data comparison of ant colony algorithm in 30×30 environment model

3.4 實驗結(jié)果分析

通過仿真結(jié)果可以看出，新型柵格法建模對于局部路徑規(guī)劃算法有很好的優(yōu)化效果。局部路徑規(guī)劃算法只需要由傳感器采集環(huán)境信息，新型柵格法建模結(jié)合傳統(tǒng)柵格法中的環(huán)境信息表達(dá)方式保證了環(huán)境信息的完整性，同時新型柵格法建模通過特征提取的方式排除了算法中的較差解。局部路徑規(guī)劃算法中特征柵格節(jié)點與周圍節(jié)點的環(huán)境信息變化不大，過多的信息相似解反而會增加算法的運算時間，排除這些解使得算法選擇路徑更加有目的性。可以看出局部路徑規(guī)劃算法更加適合應(yīng)用于新型柵格法建模。

4 結(jié)語

路徑規(guī)劃算法改進(jìn)的思想通常是對算法本身進(jìn)行優(yōu)化和改進(jìn)，有時這種改進(jìn)用于其他路徑規(guī)劃算法時存在局限性。因此，本文提出了一種優(yōu)化環(huán)境建模的新方法，通過提取障礙物的頂點柵格作為特征點來減小算法的搜索范圍。該方法可以應(yīng)用于多種路徑規(guī)劃算法，同時保證一定的優(yōu)化效果。為了驗證這種新柵格模型的適用性，將其應(yīng)用于人工勢場法、A*和蟻群三種不同類型的路徑規(guī)劃算法，并對新方法的性能提升進(jìn)行了評價。結(jié)果表明，新柵格模型適用于不同類型的路徑規(guī)劃算法，并且可以解決這些算法存在的局部最優(yōu)、搜索速度慢和迭代次數(shù)多等問題，從而提高了這些算法的性能。特征點提取下的柵格模型具有很強(qiáng)的適用性，并且可以對不同的算法做出優(yōu)化，為優(yōu)化環(huán)境建模方法提供了一種新的思路。