"""
Created on Fri May 24 09:04:23 2024""" import os
import sys
import math
import heapq
import matplotlib. pyplot as plt
import time'''
传统A*算法
''' class Astar : '''AStar set the cost + heuristics as the priorityAStar将成本+启发式设置为优先级''' def __init__ ( self, s_start, s_goal, heuristic_type, xI, xG) : self. s_start = s_startself. s_goal = s_goalself. heuristic_type = heuristic_typeself. u_set = [ ( - 1 , 0 ) , ( 0 , 1 ) , ( 1 , 0 ) , ( 0 , - 1 ) ] self. Open = [ ] self. Closed = [ ] self. parent = dict ( ) self. g = dict ( ) self. x_range = 51 self. y_range = 51 self. xI, self. xG = xI, xGself. obs = self. obs_map( ) def animation ( self, path_l, visited_l, name, path_color= 'g' ) : obs_x = [ x[ 0 ] for x in self. obs] obs_y = [ x[ 1 ] for x in self. obs] plt. plot( self. xI[ 0 ] , self. xI[ 1 ] , "bs" ) plt. plot( self. xG[ 0 ] , self. xG[ 1 ] , "gs" ) plt. plot( obs_x, obs_y, "sk" ) plt. title( name) plt. axis( "equal" ) visited_l = [ node for node in visited_l if node != self. xI and node != self. xG] for x in visited_l: plt. plot( x[ 0 ] , x[ 1 ] , color= 'gray' , marker= 'o' ) path_x = [ point[ 0 ] for point in path_l] path_y = [ point[ 1 ] for point in path_l] plt. plot( path_x, path_y, linewidth= 3 , color= path_color) plt. show( block= True ) def obs_map ( self) : """Initialize obstacles' positions:return: map of obstacles初始化障碍物位置返回：障碍物""" x = 51 y = 31 self. obs = set ( ) self. obs. update( ( i, 0 ) for i in range ( x) ) self. obs. update( ( i, y - 1 ) for i in range ( x) ) self. obs. update( ( 0 , i) for i in range ( y) ) self. obs. update( ( x - 1 , i) for i in range ( y) ) self. obs. update( ( i, 15 ) for i in range ( 10 , 21 ) ) self. obs. update( ( 20 , i) for i in range ( 15 ) ) self. obs. update( ( 30 , i) for i in range ( 15 , 30 ) ) self. obs. update( ( 40 , i) for i in range ( 16 ) ) return self. obsdef searching ( self) : """A_star Searching.:return: path, visited orderAstart搜索，返回路径、访问集，""" self. parent[ self. s_start] = self. s_start self. g[ self. s_start] = 0 self. g[ self. s_goal] = math. inf heapq. heappush( self. Open, ( self. f_value( self. s_start) , self. s_start) ) while self. Open: _, s_current = heapq. heappop( self. Open) self. Closed. append( s_current) if s_current == self. s_goal: break for s_next in self. get_neighbor( s_current) : new_cost = self. g[ s_current] + self. cost( s_current, s_next) if s_next not in self. g: self. g[ s_next] = math. infif new_cost < self. g[ s_next] : self. g[ s_next] = new_costself. parent[ s_next] = s_currentheapq. heappush( self. Open, ( self. f_value( s_next) , s_next) ) return self. extract_path( self. parent) , self. Closeddef get_neighbor ( self, s_current) : """:param s_current::return: 相邻点集合""" return [ ( s_current[ 0 ] + u[ 0 ] , s_current[ 1 ] + u[ 1 ] ) for u in self. u_set] def cost ( self, s_current, s_next) : """:param s_current 表示当前点:param s_next 表示相邻点:return 若与障碍物无冲突，则范围欧式距离成本，否则为无穷大成本计算当前点与相邻点的距离成本""" if self. is_collision( s_current, s_next) : return math. infreturn math. hypot( s_next[ 0 ] - s_current[ 0 ] , s_next[ 1 ] - s_current[ 1 ] ) def is_collision ( self, s_current, s_next) : """check if the line segment (s_start, s_end) is collision.:param s_current: start node:param s_next: end node:return: True: is collision / False: not collision检查起终点线段与障碍物是否冲突如果线段的起点或终点之一位于障碍物集合 self.obs 内，则直接判定为碰撞，返回 True。若线段不垂直也不水平（即斜线段），则分为两种情况检查：若线段为45度线（斜率为1:1或-1），则检查线段的端点形成的矩形框内是否有障碍物。否则检查线段端点形成的另一矩形框内是否有障碍物。若上述任一矩形框内有障碍，则判定为碰撞，返回 True若无碰撞情况，则返回 False""" if s_current in self. obs or s_next in self. obs: return True '''''' if s_current[ 0 ] != s_next[ 0 ] and s_current[ 1 ] != s_next[ 1 ] : if s_next[ 0 ] - s_current[ 0 ] == s_current[ 1 ] - s_next[ 1 ] : s1 = ( min ( s_current[ 0 ] , s_next[ 0 ] ) , min ( s_current[ 1 ] , s_next[ 1 ] ) ) s2 = ( max ( s_current[ 0 ] , s_next[ 0 ] ) , max ( s_current[ 1 ] , s_next[ 1 ] ) ) else : s1 = ( min ( s_current[ 0 ] , s_next[ 0 ] ) , max ( s_current[ 1 ] , s_next[ 1 ] ) ) s2 = ( max ( s_current[ 0 ] , s_next[ 0 ] ) , min ( s_current[ 1 ] , s_next[ 1 ] ) ) if s1 in self. obs or s2 in self. obs: return True return False def f_value ( self, s_currrent) : """f = g + h. (g: Cost to come, h: heuristic value):param s: current state:return: f""" return self. g[ s_currrent] + self. heuristic( s_currrent) def extract_path ( self, parent) : path = [ self. s_goal] s = self. s_goalwhile True : s = parent[ s] path. append( s) if s == self. s_start: break return list ( path) def heuristic ( self, s_current) : heuristic_type = self. heuristic_type goal = self. s_goal if heuristic_type == "manhattan" : return abs ( goal[ 0 ] - s_current[ 0 ] ) + abs ( goal[ 1 ] - s_current[ 1 ] ) else : return math. hypot( goal[ 0 ] - s_current[ 0 ] , goal[ 1 ] - s_current[ 1 ] ) if __name__ == '__main__' : time_start = time. time( ) s_start = ( 5 , 5 ) s_goal = ( 45 , 26 ) star_m = Astar( s_start, s_goal, "ee" , s_start, s_goal) path, visited = star_m. searching( ) star_m. animation( path, visited, "A*" ) time_end = time. time( ) print ( "程序运行时间：" , time_end - time_start)
求√n)
)
