全局路徑規劃

一、什麼是全局路徑規劃

全局路徑規劃是指在給定的地圖中，通過使用搜索算法和路徑規划算法，尋找從起點到目標點的最佳路徑，並將其轉換為機械人可以遵循的指令序列，使得機械人能夠在給定地圖上避開障礙物，沿最優路徑到達目標點。

全局路徑規劃通常用於自動駕駛車輛、無人機及移動機械人等領域，在實際應用中起到了重要的作用。

二、全局路徑規劃的算法

1. Dijkstra算法

Dijkstra算法是一種常見的最短路徑搜索算法，適用於無權圖或者權值都為正數的圖。它以起點為中心向外層層擴展，直到擴展到目標點位置。

def dijkstra(graph, start, end):
    #初始化
    inf = float('inf')
    distance = {vertex: inf for vertex in graph}
    path = {vertex: [] for vertex in graph}
    visited = []
    queue = [start]
    distance[start] = 0

    #搜索
    while queue:
        current = queue.pop(0)
        visited.append(current)
        
        if current == end:
            break
        
        for neighbor in graph[current]:
            if neighbor not in visited:
                new_distance = distance[current] + graph[current][neighbor]
                if new_distance < distance[neighbor]:
                    distance[neighbor] = new_distance
                    path[neighbor] = path[current] + [current]
                queue.append(neighbor)

    return path[end] + [end]

2. A*算法

A*算法結合了Dijkstra算法和啟發式函數（Heuristic Function）的思想，在搜索過程中通過評估函數評估某個節點是否為最優解。它適用於連續空間的最短路徑規劃，具有搜索速度快、路徑優秀等優點。

def heuristic(node1, node2):
    #曼哈頓距離
    return abs(node1.x - node2.x) + abs(node1.y - node2.y)

def a_star(start, end):
    #初始化
    open_list = [start]
    closed_list = []
    start.g = 0
    start.f = start.h = heuristic(start, end)
    
    #搜索
    while open_list:
        current = min(open_list, key = lambda x:x.f)
        
        if current == end:
            path = []
            while current.parent != None:
                path.append(current)
                current = current.parent
            path.append(start)
            path.reverse()
            return path

        open_list.remove(current)
        closed_list.append(current)
        
        for neighbor in current.adjacent:
            if neighbor in closed_list:
                continue
            
            if neighbor not in open_list:
                neighbor.g = current.g + heuristic(current, neighbor)
                neighbor.h = heuristic(neighbor, end)
                neighbor.f = neighbor.g + neighbor.h
                neighbor.parent = current
                open_list.append(neighbor)
                
            else:
                new_g = current.g + heuristic(current, neighbor)
                if neighbor.g > new_g:
                    neighbor.g = new_g
                    neighbor.f = neighbor.g + neighbor.h
                    neighbor.parent = current
    return None

三、實例

下面是一個簡單的例子，給定一個地圖和起點終點，使用A*算法尋找最優路徑。

class Node:
    def __init__(self, value, x, y):
        self.value = value
        self.x = x
        self.y = y
        self.adjacent = []
        self.parent = None
        self.f = 0
        self.g = 0
        self.h = 0

def create_graph(grid):
    height = len(grid)
    width = len(grid[0])
    graph = {}
    for i in range(height):
        for j in range(width):
            node = Node(grid[i][j], i, j)
            if i+1 < height and grid[i+1][j] != "#":
                node.adjacent.append(Node(grid[i+1][j], i+1, j))
            if j+1 < width and grid[i][j+1] != "#":
                node.adjacent.append(Node(grid[i][j+1], i, j+1))
            graph[(i, j)] = node
    return graph

grid = [
  ["S", 0, 0, "#"],
  [0, "#", 0, "#"],
  ["#", 0, 0, 0],
  ["#", "#", 0, "E"]
]
graph = create_graph(grid)
start = graph[(0, 0)]
end = graph[(3, 3)]

print(a_star(start, end))

四、總結

全局路徑規劃是自動駕駛車輛、無人機等移動機械人領域必備的技術之一，Dijkstra算法、A*算法是其中常用的兩種算法，它們都可以用來尋找起點到目標點的最短路徑。對於不同場景可以根據需要選擇不同的算法進行路徑規劃。