何佳澤 張壽明

（昆明理工大學(xué)信息工程與自動(dòng)化學(xué)院）

移動(dòng)機(jī)器人同步定位與建圖（SLAM）中，需要規(guī)劃一條從起始點(diǎn)到終點(diǎn)的路徑，這個(gè)過(guò)程就被定義為全局路徑規(guī)劃［1，2］。傳統(tǒng)的A*算法是一種啟發(fā)式搜索算法， 它將整個(gè)地圖分成很多個(gè)節(jié)點(diǎn)，以評(píng)估節(jié)點(diǎn)的代價(jià)函數(shù)值來(lái)作為節(jié)點(diǎn)的綜合優(yōu)先級(jí)［3］，通過(guò)遍歷周邊的節(jié)點(diǎn)，選取最高優(yōu)先級(jí)的節(jié)點(diǎn)， 逐漸找到一條從起點(diǎn)到終點(diǎn)的最優(yōu)路徑。 傳統(tǒng)A*算法在二維柵格地圖中的路徑規(guī)劃效果比較好， 它能快速找到代價(jià)最小的函數(shù)值，從而得到線路最短的路徑。 但是，因?yàn)樗枰闅v周邊節(jié)點(diǎn)， 所以在尺寸較小的地圖中比較實(shí)用。隨著地圖尺寸的不斷擴(kuò)大，它將會(huì)搜索大量的無(wú)用節(jié)點(diǎn)，使得搜索的節(jié)點(diǎn)數(shù)量以指數(shù)級(jí)增長(zhǎng)［4］。

為了使A*算法在尺寸較大的地圖中也能使用，筆者提出將它與啟發(fā)神經(jīng)網(wǎng)絡(luò)（GBNN）模型結(jié)合，并使用跳點(diǎn)搜索（JPS）算法跳過(guò)下一個(gè)無(wú)用節(jié)點(diǎn)，減少整體計(jì)算量，從而提高算法的性能。

1 算法原理

1.1 A*算法柵格地圖全局路徑規(guī)劃

A*算法采用代價(jià)函數(shù)，通過(guò)不斷尋找周圍的節(jié)點(diǎn)，分析其代價(jià)規(guī)劃出一條最優(yōu)路徑［5］，代價(jià)函數(shù)模型如下：

式中 F（n）——從起始狀態(tài)經(jīng)由狀態(tài)n到目標(biāo)狀態(tài)的代價(jià)；

G（n）——在狀態(tài)空間從起始狀態(tài)到狀態(tài)n的實(shí)際代價(jià)；

H（n）——從狀態(tài)n到目標(biāo)狀態(tài)規(guī)劃的最佳路徑的代價(jià)。

A*算法是一種二維柵格地圖， 首先建立A*代價(jià)函數(shù)柵格地圖（圖1）。

圖1 A*代價(jià)函數(shù)柵格地圖

圖1建立的是5×5的柵格地圖，A*算法的基本原理是從起點(diǎn)開(kāi)始遍歷周邊8個(gè)節(jié)點(diǎn)的代價(jià)函數(shù)值。 每一個(gè)柵格的長(zhǎng)、寬都為10，根據(jù)對(duì)角線計(jì)算方法可知，柵格對(duì)角線距離為，近似為14；G（n）的值為起點(diǎn)到某一個(gè)柵格的最短距離［6，7］。筆者采用曼哈頓距離展開(kāi)計(jì)算求得H（n）：

其中10為柵格的寬度。

求出起點(diǎn)周圍8個(gè)相鄰節(jié)點(diǎn)的代價(jià)函數(shù)值F（n），并且方向是朝著終點(diǎn)，（3，3）節(jié)點(diǎn)的代價(jià)函數(shù)值最小［8］，所以全局路徑規(guī)劃到（3，3），并且以此點(diǎn)為中心點(diǎn)， 計(jì)算周圍8個(gè)相鄰節(jié)點(diǎn)的代價(jià)函數(shù)值，選取下一個(gè)規(guī)劃路徑點(diǎn)，直到到達(dá)目標(biāo)位置點(diǎn)為止［9］，如圖2所示。

圖2 A*算法代價(jià)函數(shù)規(guī)劃路徑

圖2中，起點(diǎn)到第2個(gè)節(jié)點(diǎn)最小代價(jià)函數(shù)值為54，然后是48，再到終點(diǎn)是42，由此得到一條最優(yōu)全局路徑規(guī)劃。

1.2 A*算法的優(yōu)缺點(diǎn)

A*算法由于原理簡(jiǎn)單，在實(shí)際生產(chǎn)中得到了廣泛的應(yīng)用， 但是它的缺點(diǎn)也同樣非常明顯，如果地圖比較大的話，它將會(huì)遍歷很多點(diǎn)，計(jì)算量特別大，并且每一個(gè)點(diǎn)的計(jì)算量也非常大，所以對(duì)系統(tǒng)的運(yùn)算能力要求較高［10］。 為了彌補(bǔ)這兩方面的不足，筆者使用JPS算法與GBNN模型結(jié)合的方法來(lái)克服這兩個(gè)方面的缺點(diǎn)。

2 跳點(diǎn)搜索法

為減少中間無(wú)效節(jié)點(diǎn)和一些效果比較差的節(jié)點(diǎn)，筆者采用跳點(diǎn)搜索規(guī)則對(duì)每個(gè)節(jié)點(diǎn)周圍的可拓展節(jié)點(diǎn)進(jìn)行篩選，跳過(guò)不需要拓展的節(jié)點(diǎn)［11］。 這樣就可以減少大量的節(jié)點(diǎn)運(yùn)算，提高整體性能，進(jìn)而提高搜索效率［12］。

水平方向和對(duì)角線方向的篩選規(guī)則如圖3、4所示。

圖3 水平方向篩選規(guī)則

圖3、4中灰色點(diǎn)代表不適合經(jīng)過(guò)X節(jié)點(diǎn)建立全局路徑規(guī)劃的點(diǎn)， 白色點(diǎn)代表適合經(jīng)過(guò)X節(jié)點(diǎn)建立全局路徑規(guī)劃的點(diǎn)。 圖3中路徑從X22節(jié)點(diǎn)朝著X點(diǎn)向地圖的右邊運(yùn)動(dòng)， 可以直觀地看出從X22到Y(jié)最短的路線是走直線：X22→X→X42→Y，直接經(jīng)過(guò)X的路徑是最短的，如果繞過(guò)X，那么將不是最優(yōu)選擇；如果終點(diǎn)Y是節(jié)點(diǎn)X51或者X53，那么經(jīng)過(guò)X節(jié)點(diǎn)就不是最優(yōu)路徑選擇， 此時(shí)的規(guī)劃路徑將不經(jīng)過(guò)X節(jié)點(diǎn)。 同理在圖4中X22節(jié)點(diǎn)朝著X方向向Y節(jié)點(diǎn)運(yùn)行， 搜索過(guò)程則可以直接跳過(guò)X節(jié)點(diǎn)。所以筆者根據(jù)圖3所示的水平方向篩選規(guī)則分成a、b兩種情況來(lái)篩選拓?fù)潼c(diǎn)（圖4所用對(duì)角線方向篩選規(guī)則同理）：

圖4 對(duì)角線方向篩選規(guī)則

a. 從X22節(jié)點(diǎn)向右側(cè)移動(dòng)不經(jīng)過(guò)X節(jié)點(diǎn)比經(jīng)過(guò)X節(jié)點(diǎn)的路徑差的情況，如終點(diǎn)為X42或者X52；

b. 從X22節(jié)點(diǎn)向右側(cè)移動(dòng)不經(jīng)過(guò)X節(jié)點(diǎn)比經(jīng)過(guò)X節(jié)點(diǎn)的路徑優(yōu)的情況，如終點(diǎn)為X51或者X53。

情況a中X節(jié)點(diǎn)會(huì)加入到點(diǎn)集合中，情況b中X節(jié)點(diǎn)就會(huì)被去掉，這樣就可以去掉無(wú)效或者低效節(jié)點(diǎn)，減少節(jié)點(diǎn)處理量。

在沒(méi)有障礙物節(jié)點(diǎn)的環(huán)境中， 沿著X、Y方向的節(jié)點(diǎn)篩選規(guī)則為：

其中，m代表X可以拓展的相鄰節(jié)點(diǎn)，len（）代表路徑行進(jìn)長(zhǎng)度；（X22，…，m|X）代表不通過(guò)X而直接到m的最短路徑；（X22，X，m）代表X節(jié)點(diǎn)到達(dá)m的最優(yōu)路徑。

同理，可以得到在沒(méi)有障礙物節(jié)點(diǎn)的環(huán)境中對(duì)角線方向的篩選規(guī)則：

3 A＊算法與GBNN模型優(yōu)化

3.1 GBNN模型

GBNN模型是一種離散的生物啟發(fā)神經(jīng)網(wǎng)絡(luò)，它不需要機(jī)器學(xué)習(xí)和深度學(xué)習(xí)。GBNN模型是三維立體神經(jīng)網(wǎng)絡(luò)，主要用在水下機(jī)器人的路徑規(guī)劃中。 筆者通過(guò)簡(jiǎn)化，構(gòu)建了如圖5所示的二維神經(jīng)網(wǎng)絡(luò)［13］，與二維柵格地圖一一對(duì)應(yīng)。

圖5 簡(jiǎn)化的二維GBNN模型

該算法的主體思想是不斷向四周傳遞激勵(lì)，障礙物對(duì)發(fā)散出的激勵(lì)有抑制作用，通過(guò)迭代算法計(jì)算出每個(gè)位置的活性數(shù)值。 該算法有記憶特性，地圖上的每一個(gè)點(diǎn)都可以通過(guò)不斷迭代到達(dá)激勵(lì)的發(fā)出點(diǎn)，這個(gè)激勵(lì)的發(fā)出點(diǎn)也就是路徑規(guī)劃中的終點(diǎn)［14］。

GBNN數(shù)學(xué)模型如下：

其中，Wij是兩個(gè)神經(jīng)元i、j之間的連接權(quán)值，xi是i神經(jīng)元的活性輸出值，Ii是i神經(jīng)元的外部激勵(lì)值，ωij是i、j兩個(gè)神經(jīng)元之間的連接系數(shù)，函數(shù)g是模型變換函數(shù)，γ、r為常數(shù)，根據(jù)情況設(shè)置。筆者采用的是二維柵格地圖，所以式（7）使用的是GBNN二維模型的歐拉公式，xix、xiy分別表示第i個(gè)神經(jīng)元的x、y坐標(biāo)值，通過(guò)式（7）即可求出i、j神經(jīng)元之間的距離。

3.2 A*算法與GBNN模型結(jié)合

GBNN的二維神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)可以與路徑規(guī)劃中的柵格地圖一一對(duì)應(yīng)起來(lái)，每一個(gè)神經(jīng)元和一個(gè)平面二維柵格對(duì)應(yīng)，它依據(jù)二維柵格地圖中各個(gè)柵格的狀態(tài)來(lái)決定神經(jīng)元的外部輸入。 通過(guò)直接計(jì)算神經(jīng)元的活性值來(lái)規(guī)劃路徑，這樣就可以避免大量代價(jià)函數(shù)計(jì)算， 減少節(jié)點(diǎn)之間的運(yùn)算量。 GBNN模型不需要神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)， 不需要訓(xùn)練，可以滿足路徑規(guī)劃對(duì)實(shí)時(shí)性的要求。

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

筆者提出在A*算法的基礎(chǔ)上使用跳點(diǎn)搜索，這樣去掉不需要的低效和無(wú)效節(jié)點(diǎn)，然后在此基礎(chǔ)上加入GBNN模型。 模型構(gòu)建以后，通過(guò)MATLAB仿真方法比較算法優(yōu)化前后的效果。

實(shí)驗(yàn)配置如下：

硬件 CPU i7-8750H

內(nèi)存 16GB

軟件 MATLAB2016a

首先建立50×50的柵格地圖， 規(guī)定起點(diǎn)和終點(diǎn)的位置，隨機(jī)生成障礙物坐標(biāo)，通過(guò)比較算法優(yōu)化前后的路徑規(guī)劃時(shí)間、路徑遍尋節(jié)點(diǎn)數(shù)量和全局規(guī)劃路徑長(zhǎng)度這3個(gè)指標(biāo)來(lái)比較。

圖6為優(yōu)化算法流程。

圖6 優(yōu)化算法流程

圖7、8分別顯示了50×50的二維柵格地圖未使用和使用筆者所提算法的規(guī)劃路徑，圖中綠色圈為起點(diǎn)，藍(lán)色圈為終點(diǎn)，規(guī)劃路徑為藍(lán)色曲線。圖9、10分別對(duì)應(yīng)圖7、8的運(yùn)行時(shí)間、遍歷節(jié)點(diǎn)數(shù)和路徑總長(zhǎng)度。

圖7 未使用優(yōu)化算法規(guī)劃路徑

圖8 使用優(yōu)化算法規(guī)劃路徑

圖9 未使用優(yōu)化算法路徑規(guī)劃數(shù)據(jù)

圖10 使用優(yōu)化算法路徑規(guī)劃數(shù)據(jù)

因?yàn)樗惴ň哂须S機(jī)性，為了使結(jié)果更具真實(shí)性，所以通過(guò)多次仿真實(shí)驗(yàn)，比較算法優(yōu)化前后的路徑規(guī)劃時(shí)間、路徑遍尋節(jié)點(diǎn)數(shù)量和全局規(guī)劃路徑長(zhǎng)度的平均值，詳見(jiàn)表1。

表1 算法優(yōu)化前后性能對(duì)比

通過(guò)表1可以看出優(yōu)化后的算法對(duì)二維柵格地圖全局路徑規(guī)劃效果較好。

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

對(duì)傳統(tǒng)的A*算法進(jìn)行優(yōu)化， 加入JPS 和GBNN，將3種算法進(jìn)行結(jié)合。 采用路徑規(guī)劃時(shí)間、路徑遍尋節(jié)點(diǎn)數(shù)量和全局規(guī)劃路徑長(zhǎng)度這3個(gè)指標(biāo)對(duì)算法的優(yōu)劣性進(jìn)行對(duì)比， 可以看出使用3種算法結(jié)合后的優(yōu)化算法效果較好。