基于A*算法|灰狼優(yōu)化算法的機(jī)器人路徑規(guī)劃

A*算法(A-star Algorithm)是一種啟發(fā)式搜索算法，廣泛用于路徑規(guī)劃和圖搜索問(wèn)題，如地圖導(dǎo)航、游戲AI、機(jī)器人路徑規(guī)劃等。它結(jié)合了廣度優(yōu)先搜索和貪婪最佳優(yōu)先搜索的優(yōu)點(diǎn)，能夠有效地找到從起點(diǎn)到目標(biāo)點(diǎn)的最短路徑。

NO.1|A*算法詳解，群智能算法小狂人

1、A*算法的核心思想

A*算法使用啟發(fā)式函數(shù)（heuristic function）來(lái)估算當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的代價(jià)，從而決定下一步搜索哪個(gè)節(jié)點(diǎn)。具體來(lái)說(shuō)，A*算法在搜索過(guò)程中會(huì)考慮以下兩個(gè)代價(jià)函數(shù)：

(1)g(n): 從起點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)（路徑長(zhǎng)度）。

(2)h(n): 從節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估算代價(jià)（啟發(fā)式函數(shù)）。

A*算法通過(guò)綜合這兩個(gè)代價(jià)函數(shù)，使用總的代價(jià)函數(shù)f(n) = g(n) + h(n)來(lái)選擇下一步要擴(kuò)展的節(jié)點(diǎn)，其中：

g(n)確保了路徑的實(shí)際代價(jià)被考慮，避免盲目貪心。

h(n)提供了前瞻性，引導(dǎo)搜索方向朝向目標(biāo)。

2、A*算法的步驟

(1)初始化

創(chuàng)建一個(gè)開(kāi)放列表（open list），初始時(shí)只包含起點(diǎn)節(jié)點(diǎn)。

創(chuàng)建一個(gè)關(guān)閉列表（closed list），初始為空。

將起點(diǎn)節(jié)點(diǎn)的 g 值設(shè)為 0，f 值設(shè)為 h(起點(diǎn))。

(2)主循環(huán)

在開(kāi)放列表中選擇 f 值最小的節(jié)點(diǎn)（稱(chēng)為當(dāng)前節(jié)點(diǎn)），將其移出開(kāi)放列表，并加入關(guān)閉列表。

檢查當(dāng)前節(jié)點(diǎn)是否為目標(biāo)節(jié)點(diǎn)。如果是，搜索結(jié)束，已找到最短路徑。否則，對(duì)當(dāng)前節(jié)點(diǎn)的所有鄰居節(jié)點(diǎn)進(jìn)行處理

如果鄰居節(jié)點(diǎn)在關(guān)閉列表中，跳過(guò)該節(jié)點(diǎn)。

如果鄰居節(jié)點(diǎn)不在開(kāi)放列表中，將其添加進(jìn)去，并計(jì)算其 g 值和 f 值。

如果鄰居節(jié)點(diǎn)已經(jīng)在開(kāi)放列表中，并且新的 g 值更小，更新該節(jié)點(diǎn)的 g 值和 f 值，并更新其父節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)。

3. 重復(fù)主循環(huán)，直到找到目標(biāo)節(jié)點(diǎn)或者開(kāi)放列表為空（意味著無(wú)解）。

4. 回溯路徑：從目標(biāo)節(jié)點(diǎn)開(kāi)始，依次通過(guò)各個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)回溯到起點(diǎn)節(jié)點(diǎn)，得到最短路徑。

3、例子

假設(shè)我們有一個(gè)5x5的網(wǎng)格地圖，起點(diǎn)在左上角(0,0)，目標(biāo)在右下角(4,4)，我們使用曼哈頓距離作為 h(n)。A*算法將會(huì)根據(jù) f(n) 值選擇路徑，并最終找到最短路徑。A*算法在圖搜索和路徑規(guī)劃中具有廣泛應(yīng)用，其效率和最優(yōu)性使其成為了許多實(shí)際問(wèn)題的首選算法.

import heapqimport matplotlib.pyplot as plt# A* Algorithm Implementationdef a_star(grid, start, goal):    # Initialize open list and closed list    open_list = []    heapq.heappush(open_list, (0 + heuristic(start, goal), 0, start, []))    closed_set = set()
    while open_list:        # Get the current node with the lowest f(n)????????f,?g,?current,?path?=?heapq.heappop(open_list)        if current in closed_set:????????????continue        path = path + [current]????????closed_set.add(current)        # Check if we have reached the goal        if current == goal:????????????return?path        # Explore neighbors        for neighbor in get_neighbors(grid, current):            if neighbor in closed_set:????????????????continue
????????????tentative_g?=?g?+?1??#?Assuming?uniform?cost?for?all?edges????????????heapq.heappush(open_list,?(tentative_g?+?heuristic(neighbor,?goal),?tentative_g,?neighbor,?path))????return?None??#?No?path?founddef get_neighbors(grid, node):    neighbors = []    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]  # Up, Down, Left, Right    for direction in directions:        neighbor = (node[0] + direction[0], node[1] + direction[1])        if 0 <= neighbor[0] < len(grid) and 0 <= neighbor[1] < len(grid[0]):            neighbors.append(neighbor)????return?neighborsdef heuristic(node, goal):????return?abs(node[0]?-?goal[0])?+?abs(node[1]?-?goal[1])??#?Manhattan?Distance# Grid representation (0: free space, 1: obstacle)grid = [[0, 0, 0, 0, 0],        [0, 1, 1, 1, 0],        [0, 1, 0, 1, 0],        [0, 1, 0, 1, 0],        [0, 0, 0, 0, 0]]
start = (0, 0)goal = (4, 4)# Run A* algorithmpath = a_star(grid, start, goal)# Visualizationdef visualize_grid(grid, path):    fig, ax = plt.subplots()    for i in range(len(grid)):        for j in range(len(grid[i])):            if grid[i][j] == 1:                ax.add_patch(plt.Rectangle((j, len(grid) - 1 - i), 1, 1, color='black'))            else:????????????????ax.add_patch(plt.Rectangle((j,?len(grid)?-?1?-?i),?1,?1,?color='white',?edgecolor='black'))    for (i, j) in path:????????ax.add_patch(plt.Rectangle((j,?len(grid)?-?1?-?i),?1,?1,?color='blue'))    ax.add_patch(plt.Rectangle((start[1], len(grid) - 1 - start[0]), 1, 1, color='green'))????ax.add_patch(plt.Rectangle((goal[1],?len(grid)?-?1?-?goal[0]),?1,?1,?color='red'))    plt.xlim(0, len(grid[0]))    plt.ylim(0, len(grid))    plt.gca().set_aspect('equal', adjustable='box')????plt.show()# Visualize the pathvisualize_grid(grid, path)

代碼說(shuō)明與可視化結(jié)果

1. 啟發(fā)式函數(shù)：我們使用曼哈頓距離作為啟發(fā)式函數(shù)，即??，其中?和?分別是當(dāng)前節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的坐標(biāo)。

這是A*算法在一個(gè)5x5網(wǎng)格上的可視化結(jié)果。在圖中：綠色方塊表示起點(diǎn)位置。紅色方塊表示目標(biāo)位置。黑色方塊表示障礙物。藍(lán)色方塊表示A*算法找到的從起點(diǎn)到目標(biāo)的最短路徑。A*算法成功地繞過(guò)障礙物，找到了一條從起點(diǎn)到目標(biāo)的最短路徑。

NO.2|GWO原理，群智能算法小狂人

灰狼優(yōu)化算法（Grey Wolf Optimizer, GWO）是一種基于灰狼捕獵行為的群體智能優(yōu)化算法，由Mirjalili等人在2014年提出。GWO算法模仿了灰狼在自然界中的社會(huì)等級(jí)結(jié)構(gòu)和圍獵、追蹤獵物的過(guò)程，用于解決各種優(yōu)化問(wèn)題。灰狼在捕獵過(guò)程中通常有一個(gè)明確的社會(huì)等級(jí)結(jié)構(gòu)，其中包含以下幾種角色：

1. α狼（Alpha）：群體的領(lǐng)導(dǎo)者，負(fù)責(zé)決策，通常是最優(yōu)解的代表。

2. β狼（Beta）：第二等級(jí)的狼，協(xié)助α狼決策，代表次優(yōu)解。

3. δ狼（Delta）：第三等級(jí)的狼，服從α狼和β狼的指令，代表第三優(yōu)解。

4. ω狼（Omega）：最低等級(jí)的狼，服從所有其他狼。

在GWO算法中，搜索過(guò)程主要由這些灰狼的角色來(lái)實(shí)現(xiàn)，模擬了捕獵、包圍和攻擊獵物的過(guò)程，其詳細(xì)的更新及原理可以看我之前發(fā)布的文章：引用過(guò)萬(wàn)！灰狼算法(Grey Wolf Optimizer, GWO)原理及實(shí)現(xiàn)|含源碼

NO.3|應(yīng)用于路徑規(guī)劃，群智能算法小狂人

相關(guān)算法的路徑規(guī)劃原理可以查看我之前發(fā)布的文章：

(1)基于粒子群算法（Particle Swarm Optimization, PSO）柵格地圖機(jī)器人路徑規(guī)劃

(2)COA算法：改進(jìn)小龍蝦優(yōu)化算法（Crayfish Optimization Algorithm, COA）機(jī)器人路徑規(guī)劃

(3)基于牛頓-拉夫遜優(yōu)化算法(Newton-Raphson-based optimizer, NBRO)的無(wú)人機(jī)三維路徑規(guī)劃

NO.4|運(yùn)行結(jié)果，群智能算法小狂人