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

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

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

1、A*算法的核心思想

A*算法使用啟發(fā)式函數(shù)（heuristic function）來估算當前節(jié)點到目標節(jié)點的代價，從而決定下一步搜索哪個節(jié)點。具體來說，A*算法在搜索過程中會考慮以下兩個代價函數(shù)：

(1)g(n): 從起點到節(jié)點n的實際代價（路徑長度）。

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

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

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

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

2、A*算法的步驟

(1)初始化

創(chuàng)建一個開放列表（open list），初始時只包含起點節(jié)點。

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

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

(2)主循環(huán)

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

檢查當前節(jié)點是否為目標節(jié)點。如果是，搜索結(jié)束，已找到最短路徑。否則，對當前節(jié)點的所有鄰居節(jié)點進行處理

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

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

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

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

4. 回溯路徑：從目標節(jié)點開始，依次通過各個節(jié)點的父節(jié)點回溯到起點節(jié)點，得到最短路徑。

3、例子

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

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)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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