路径规划算法分享

路径规划

路径规划算法是人工智能、机器人学、控制理论等领域的重要研究方向之一。它的目的是在给定的起点和终点之间寻找一条最优路径，使得该路径满足一些特定的约束条件。在本文中，我将介绍几种常用的路径规划算法，包括最短路径算法、A*算法、Dijkstra算法等。

最短路径算法

最短路径算法是指在有向或无向图中找到两个节点之间的最短路径的一种算法。该算法可以使用图论、动态规划等技术实现。其中最经典的算法是Dijkstra算法，它通过贪心策略，从起点开始一步步扩展，直到到达终点。在扩展过程中，每个节点的距离值都会被更新，直到到达终点或者无法继续扩展为止。最后，我们可以通过回溯路径，得到最短路径。

import heapq

def dijkstra(graph, start, end):
    heap = [(0, start, [])]
    visited = set()
    while heap:
        (cost, node, path) = heapq.heappop(heap)
        if node not in visited:
            visited.add(node)
            path = path + [node]
            if node == end:
                return (cost, path)
            for neighbor, c in graph[node].items():
                if neighbor not in visited:
                    heapq.heappush(heap, (cost + c, neighbor, path))
    return None

A*算法

A*算法是一种启发式搜索算法，用于寻找最短路径。该算法通过维护一个开放列表和一个关闭列表，利用启发函数估计每个节点到终点的距离，从而确定下一个要扩展的节点。具体来说，该算法维护两个值f(n)和g(n)，其中f(n)表示从起点到终点经过节点n的最短路径长度，g(n)表示从起点到节点n的路径长度。启发函数h(n)用来估计节点n到终点的距离。在扩展过程中，算法优先选择f(n)最小的节点进行扩展。

import heapq
from math import sqrt

def heuristic(a, b):
    return sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2)

def a_star(graph, start, end):
    heap = [(0, start, [])]
    visited = set()
    while heap:
        (cost, node, path) = heapq.heappop(heap)
        if node not in visited:
            visited.add(node)
            path = path + [node]
            if node == end:
                return (cost, path)
            for neighbor, c in graph[node].items():
                if neighbor not in visited:
                    heapq.heappush(heap, (cost + c + heuristic(neighbor, end), neighbor, path))
    return None

RRT算法

RRT（Rapidly-exploring Random Tree）算法是一种随机探索算法，用于构建无人机、机器人等的运动轨迹。该算法通过随机选择点，并以该点为起点向外扩展，最终构建一棵树形结构，从而找到一条可行路径。在扩展过程中，算法通过优先选择离目标点最近的节点来增加成功的概率。

import random
from math import sqrt

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None

class RRT:
    def __init__(self, start, goal, obstacle_list, max_iter=500, expand_distance=1.0):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.obstacle_list = obstacle_list
        self.max_iter = max_iter
        self.expand_distance = expand_distance

    def generate_random_node(self):
        x = random.uniform(0, 10)
        y = random.uniform(0, 10)
        return Node(x, y)

    def nearest_node(self, node, node_list):
        nearest_node = None
        min_distance = float('inf')
        for n in node_list:
            distance = sqrt((n.x - node.x) ** 2 + (n.y - node.y) ** 2)
            if distance < min_distance:
                min_distance = distance
                nearest_node = n
        return nearest_node

    def steer(self, from_node, to_node):
        distance = sqrt((from_node.x - to_node.x) ** 2 + (from_node.y - to_node.y) ** 2)
        if distance > self.expand_distance:
            theta = atan2(to_node.y - from_node.y, to_node.x - from_node.x)
            new_node = Node(from_node.x + self.expand_distance * cos(theta), from_node.y + self.expand_distance * sin(theta))
        else:
            new_node = to_node
        new_node.parent = from_node
        return new_node
        
    def check_collision(self, from_node, to_node):
	    for obs in self.obstacle_list:
	        if (obs[0] <= from_node.x <= obs[2] and obs[1] <= from_node.y <= obs[3]) or (obs[0] <= to_node.x <= obs[2] and obs[1] <= to_node.y <= obs[3]):
	            return True
	        if obs[0] <= from_node.x <= obs[2] and obs[0] <= to_node.x <= obs[2]:
	            if obs[1] <= from_node.y <= obs[3] and obs[1] <= to_node.y <= obs[3]:
	                return True
	    return False

	def find_path(self):
	    node_list = [self.start]
	    for i in range(self.max_iter):
	        rand_node = self.generate_random_node()
	        nearest_node = self.nearest_node(rand_node, node_list)
	        new_node = self.steer(nearest_node, rand_node)
	        if not self.check_collision(nearest_node, new_node):
	            node_list.append(new_node)
	        if sqrt((new_node.x - self.goal.x) ** 2 + (new_node.y - self.goal.y) ** 2) < self.expand_distance:
	            final_node = self.steer(new_node, self.goal)
	            if not self.check_collision(new_node, self.goal):
	                final_node.parent = new_node
	                node_list.append(final_node)
	                path = [[final_node.x, final_node.y]]
	                while final_node.parent:
	                    final_node = final_node.parent
	                    path.append([final_node.x, final_node.y])
	                return path[::-1]
	    return None