路径规划入门
-
首先查阅了ortools文档
- 在车辆路由问题 (VRP) 中，目标是为访问一组位置的多辆车找到最佳路线。（如果只有一辆车，问题会减少到差旅销售人员的问题）
- 如果没有其他约束，最佳解决方案是仅分配一辆车访问所有地点，并找到该车辆的最短路线 (TSP)
- 定义最佳路线的更好方法是尽可能缩短所有车辆中最长的单个路线的长度。如果目标是尽快完成所有交付，那么此定义是正确的
- 通过对车辆添加约束条件来泛化 TSP 的其他方式
- 容量限制：车辆需要在他们到达的每个地点自提物品，但载客能力上限。
- 时间范围：每个地点都必须在特定时间段内访问。
-
然后让chatgpt写了一个cbs方法
import heapq
import numpy as np
class Node:
def __init__(self, agents):
self.agents = agents
self.cost = 0
self.conflicts = set()
def __lt__(self, other):
return self.cost < other.cost
class PriorityQueue:
def __init__(self):
self.elements = []
def empty(self):
return not self.elements
def put(self, item, priority):
heapq.heappush(self.elements, (priority, item))
def get(self):
return heapq.heappop(self.elements)[1]
class Grid:
def __init__(self, width, height, obstacles):
self.width = width
self.height = height
self.obstacles = obstacles
def is_valid(self, x, y):
return 0 <= x < self.width and 0 <= y < self.height and (x, y) not in self.obstacles
def neighbors(self, x, y):
# Define possible movements (4-connected)
moves = [(1, 0), (-1, 0), (0, 1), (0, -1)]
neighbors = [(x + dx, y + dy) for dx, dy in moves]
return [(nx, ny) for nx, ny in neighbors if self.is_valid(nx, ny)]
def astar_search(grid, start, goal):
open_list = PriorityQueue()
open_list.put(start, 0)
came_from = {}
g_score = {pos: float('inf') for pos in np.ndindex(grid.shape)}
g_score[start] = 0
while not open_list.empty():
current = open_list.get()
if current == goal:
path = reconstruct_path(came_from, current)
return path
for neighbor in grid.neighbors(*current):
tentative_g_score = g_score[current] + 1 # Assuming a cost of 1 for each step
if tentative_g_score < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score = tentative_g_score + heuristic(neighbor, goal)
open_list.put(neighbor, f_score)
return None
def heuristic(a, b):
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def reconstruct_path(came_from, current):
path = [current]
while current in came_from:
current = came_from[current]
path.insert(0, current)
return path
def cbs(grid, agents):
# Initialize the priority queue with the root node
root = Node(agents)
heapq.heappush(open_list, root)
while open_list:
current = heapq.heappop(open_list)
if all(agent[0] == agent[1] for agent in current.agents):
return current.agents
for i, agent in enumerate(current.agents):
for j, other_agent in enumerate(current.agents):
if i != j and agent[0] == other_agent[0]:
current.conflicts.add((i, j))
for conflict in current.conflicts:
new_agents = current.agents.copy()
agent_i, agent_j = conflict
new_agents[agent_i] = a_star_path(grid, new_agents[agent_i])
new_cost = calculate_cost(new_agents)
new_node = Node(new_agents)
new_node.cost = new_cost
heapq.heappush(open_list, new_node)
return None # No solution found
def a_star_path(grid, agent):
start, goal = agent
path = astar_search(grid, start, goal)
return path
def calculate_cost(agents):
return sum(len(agent) for agent in agents)
# Example grid and agent positions
obstacles = [(1, 1), (1, 3)]
grid = Grid(5, 5, obstacles)
agents = [((0, 0), (4, 0)), ((0, 2), (4, 2)), ((0, 4), (4, 4))]
open_list = []
result = cbs(grid, agents)
if result:
print("Solution found:")
for i, agent in enumerate(result):
print(f"Agent {i}: {agent}")
else:
print("No solution found.")