规划算法实践(2)

您所在的位置：网站首页 › 地图缩放算法 › 规划算法实践(2)

规划算法实践(2)

2023-04-02 18:47| 来源: 网络整理| 查看: 265

本文是对A算法的实现。关于A算法的描述，可见

文中使用的UI，可见

1、具体实现

引入库：

import numpy as np

定义算法类：

class AStar: def __init__(self): self.info = None # 地图信息，记录障碍信息 self.cost = np.zeros((20, 20)) # 记录从起点到当前节点的实际路径长度 self.road_map = [] # 记录到达各节点的最短路径 self.road_to_goal_list = [] # 到达终点的最短路径，形式是其坐标 self.explore_list = [] # 按顺序记录开辟的新节点，形式是其坐标 self.open_set = [] # 开集 self.close_set = [(0, 0)] # 闭集 self.get_res = False # 结果标签，若找到结果，则为True self.explore_sign = True # 探索标签，若仍可继续探索，则为True。否则为False

对算法必需的数据进行初始化：

def initialize(self, info): # 初始化方法 self.info = info for i in range(20): # 为最短路径记录开辟空间 self.road_map.append([]) for j in range(20): self.road_map[i].append([]) self.road_map[0][0].append((0, 0)) if self.info[1][0] != 0 and self.info[1][0] != 0: self.explore_sign = False else: self.explore_round(0, 0, 1, 0) self.explore_round(0, 0, 0, 1)

其中用到的self.explore_round是对某一个格子周围四个格子探索的方法，具体实现：

def explore_round(self, index_x, index_y, dir_x, dir_y): # 在一个点四周开辟新的点到开集 if self.info[index_x + dir_x][index_y + dir_y] != 1: # 判断周边是否是障碍 if not self.is_in_close_set(index_x + dir_x, index_y + dir_y): if not self.is_in_open_set(index_x + dir_x, index_y + dir_y): self.open_set.append((index_x + dir_x, index_y + dir_y)) self.cost[index_x + dir_x][index_y + dir_y] = self.cost[index_x][index_y] + 1 self.road_map[index_x+dir_x][index_y+dir_y].clear() for item in self.road_map[index_x][index_y]: self.road_map[index_x+dir_x][index_y+dir_y].append(item) self.road_map[index_x+dir_x][index_y+dir_y].append((index_x+dir_x, index_y+dir_y))

在A*算法，将节点移入开集前，必须对这个节点是否已经在开集或闭集做判断。具体实现为：

def is_in_close_set(self, point_x, point_y): # 判断一个点是否在闭集 return (point_x, point_y) in self.close_set def is_in_open_set(self, point_x, point_y): # 判断一个点是否在开集 if not self.open_set: return False return (point_x, point_y) in self.open_set

进而可进行为闭集增加新节点的具体步骤：

def explore_one_point(self): # 开辟一个新的节点到闭集 min_length = 10000 min_index_x = 0 min_index_y = 0 if not self.open_set: # 开集是否为空为继续探索的条件 self.explore_sign = False else: for point in self.open_set: point_x, point_y = point heuristic = (19 - point_x) + (19 - point_y) # 启发项 point_val = self.cost[point_x][point_y] + heuristic if point_val < min_length: min_length = point_val min_index_x, min_index_y = point_x, point_y self.open_set.remove((min_index_x, min_index_y)) self.close_set.append((min_index_x, min_index_y)) if min_index_x == 19 and min_index_y == 19: self.get_res = True if min_index_x < 19: self.explore_round(min_index_x, min_index_y, 1, 0) if min_index_x > 0: self.explore_round(min_index_x, min_index_y, -1, 0) if min_index_y < 19: self.explore_round(min_index_x, min_index_y, 0, 1) if min_index_y > 0: self.explore_round(min_index_x, min_index_y, 0, -1) if not self.open_set: self.explore_sign = False

将探索的结果进行输出并转化为UI要求的形式，有：

def run(self): while self.explore_sign and (not self.get_res): self.explore_one_point() for point in self.close_set: x, y = point self.explore_list.append((x, 19 - y)) self.explore_list.pop(0) def road_to_goal(self): for point in self.road_map[19][19]: x, y = point self.road_to_goal_list.append((x, 19 - y))

贴个完整的：

import numpy as np class AStar: def __init__(self): self.info = None # 地图信息，记录障碍信息 self.cost = np.zeros((20, 20)) # 记录从起点到当前节点的实际路径长度 self.road_map = [] # 记录到达各节点的最短路径 self.road_to_goal_list = [] # 到达终点的最短路径，形式是其坐标 self.explore_list = [] # 按顺序记录开辟的新节点，形式是其坐标 self.open_set = [] # 开集 self.close_set = [(0, 0)] # 闭集 self.get_res = False # 结果标签，若找到结果，则为True self.explore_sign = True # 探索标签，若仍可继续探索，则为True。否则为False def initialize(self, info): # 初始化方法 self.info = info for i in range(20): # 为最短路径记录开辟空间 self.road_map.append([]) for j in range(20): self.road_map[i].append([]) self.road_map[0][0].append((0, 0)) if self.info[1][0] != 0 and self.info[1][0] != 0: self.explore_sign = False else: self.explore_round(0, 0, 1, 0) self.explore_round(0, 0, 0, 1) def is_in_close_set(self, point_x, point_y): # 判断一个点是否在闭集 return (point_x, point_y) in self.close_set def is_in_open_set(self, point_x, point_y): # 判断一个点是否在开集 if not self.open_set: return False return (point_x, point_y) in self.open_set def explore_round(self, index_x, index_y, dir_x, dir_y): # 在一个点四周开辟新的点到开集 if self.info[index_x + dir_x][index_y + dir_y] != 1: # 判断周边是否是障碍 if not self.is_in_close_set(index_x + dir_x, index_y + dir_y): if not self.is_in_open_set(index_x + dir_x, index_y + dir_y): self.open_set.append((index_x + dir_x, index_y + dir_y)) self.cost[index_x + dir_x][index_y + dir_y] = self.cost[index_x][index_y] + 1 self.road_map[index_x+dir_x][index_y+dir_y].clear() for item in self.road_map[index_x][index_y]: self.road_map[index_x+dir_x][index_y+dir_y].append(item) self.road_map[index_x+dir_x][index_y+dir_y].append((index_x+dir_x, index_y+dir_y)) def explore_one_point(self): # 开辟一个新的节点到闭集 min_length = 10000 min_index_x = 0 min_index_y = 0 if not self.open_set: # 开集是否为空为继续探索的条件 self.explore_sign = False else: for point in self.open_set: point_x, point_y = point heuristic = (19 - point_x) + (19 - point_y) # 启发项 point_val = self.cost[point_x][point_y] + heuristic if point_val

【本文地址】

规划算法实践(2)

规划算法实践(2)

今日新闻

推荐新闻