机器人路径规划

A*算法是一种启发式搜索算法，通过评估函数选择最优路径。 它结合了Dijkstra算法的完备性和贪心算法的效率。

import numpy as np
from heapq import heappush, heappop

class AStar:
    def __init__(self, grid):
        self.grid = grid
        self.rows, self.cols = grid.shape
        
    def heuristic(self, a, b):
        """计算启发式函数值（曼哈顿距离）"""
        return abs(a[0] - b[0]) + abs(a[1] - b[1])
        
    def get_neighbors(self, node):
        """获取相邻节点"""
        x, y = node
        neighbors = []
        for dx, dy in [(0, 1), (1, 0), (0, -1), (-1, 0)]:
            nx, ny = x + dx, y + dy
            if 0 <= nx < self.rows and 0 <= ny < self.cols and self.grid[nx, ny] == 0:
                neighbors.append((nx, ny))
        return neighbors
        
    def find_path(self, start, goal):
        """寻找路径"""
        frontier = []
        heappush(frontier, (0, start))
        came_from = {start: None}
        cost_so_far = {start: 0}
        
        while frontier:
            current = heappop(frontier)[1]
            
            if current == goal:
                break
                
            for next_node in self.get_neighbors(current):
                new_cost = cost_so_far[current] + 1
                if next_node not in cost_so_far or new_cost < cost_so_far[next_node]:
                    cost_so_far[next_node] = new_cost
                    priority = new_cost + self.heuristic(goal, next_node)
                    heappush(frontier, (priority, next_node))
                    came_from[next_node] = current
                    
        # 重建路径
        path = []
        current = goal
        while current is not None:
            path.append(current)
            current = came_from[current]
        path.reverse()
        
        return path

快速随机树(RRT)算法是一种基于采样的路径规划算法， 适用于高维空间和复杂环境。

import numpy as np
from scipy.spatial import KDTree

class RRT:
    def __init__(self, start, goal, bounds, obstacles, max_iter=1000):
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.bounds = bounds
        self.obstacles = obstacles
        self.max_iter = max_iter
        self.vertices = [self.start]
        self.parents = {tuple(self.start): None}
        
    def random_state(self):
        """生成随机状态"""
        return np.random.uniform(self.bounds[:, 0], self.bounds[:, 1])
        
    def nearest_vertex(self, state):
        """找到最近的顶点"""
        vertices = np.array(self.vertices)
        tree = KDTree(vertices)
        return self.vertices[tree.query(state)[1]]
        
    def new_state(self, nearest, random_state, step_size):
        """生成新状态"""
        direction = random_state - nearest
        norm = np.linalg.norm(direction)
        if norm > step_size:
            direction = direction / norm * step_size
        return nearest + direction
        
    def is_collision_free(self, state):
        """检查是否发生碰撞"""
        for obstacle in self.obstacles:
            if np.linalg.norm(state - obstacle) < obstacle.radius:
                return False
        return True
        
    def plan(self):
        """规划路径"""
        for _ in range(self.max_iter):
            # 随机采样
            if np.random.random() < 0.1:
                random_state = self.goal
            else:
                random_state = self.random_state()
                
            # 找到最近顶点
            nearest = self.nearest_vertex(random_state)
            
            # 生成新状态
            new_state = self.new_state(nearest, random_state, 0.5)
            
            # 检查是否发生碰撞
            if not self.is_collision_free(new_state):
                continue
                
            # 添加新顶点
            self.vertices.append(new_state)
            self.parents[tuple(new_state)] = tuple(nearest)
            
            # 检查是否到达目标
            if np.linalg.norm(new_state - self.goal) < 0.5:
                return self.reconstruct_path(new_state)
                
        return None
        
    def reconstruct_path(self, state):
        """重建路径"""
        path = [state]
        while state is not None:
            state = self.parents[tuple(state)]
            if state is not None:
                path.append(state)
        return path[::-1]