头哥 educoder 人工智能 A*（A start）算法答案

题目：

地图中存在一些凸多边形障碍物，设计算法寻求机器人从Start点到Goal点的最短路径。

 因为没有障碍时，机器人走直线，所以
1： 机器人的路径由一系列的折线构成
2： 路径将是（start,  某障碍点1， 某障碍点2，…,  goal）
3： 因为不能穿过障碍，一定要转弯，因此路径上的障碍点，一定在障碍物的顶点上，如：

 

所以建图模型：
V =（start,  所有障碍物顶点，goal)
E =  (所有的v之间的连线  减去 穿过障碍物的线)
应用A星算法最短路算法。

相关知识
建立图模型时，判断线段是否是否穿过障碍物，这是计算几何的问题，我们直接可实现一个Tools类，能够定义点、线段、多边形，能够计算线段的长度，判断线段和线段之间、线段和多边形之间是否相交。在本项目中，我们给出了所有顶点的坐标，提供了Tools的实现，并且建立好了图模型。

其次，A星算法针对一个问题模型建立搜索树结点完成，因此我们提供了问题模型的类实现problem类，可以给出一个状态下的可选择行为，以及一个行为下的状态变化结果。实际上它是图模型换一种表述。我们还给出node类，负责结点生成，计算结点的启发信息，建立结点间的树结构。

你的任务是基于上面两个类。在右侧的代码中完成A星算法函数 Astar(problem)。

测试说明
平台会对你编写的代码进行测试，Goal的坐标已经固定，程序会输入Start点的坐标，两个实数用空格分隔：

测试输入：34.1  113
预期输出：481.7586

实际上因为实数运算，你的输出跟预期有小的误差也会接受。

示例实现代码：

import math
from queue import Queue,PriorityQueue
import Tools
import numpy as np
import matplotlib.pyplot as plt


class Problem():
    def __init__(self,points,Adj,start, goal):
        self.Adj=Adj                 #`记录邻接矩阵备用`
        self.Points=points           #`记录每个顶点坐标备用`
        self.InitialState=start      #`起始状态`
        self.GoalState=goal          #`目标状态`
    def GoalTest(self,state):        #`测试是否到达目标`
        return state==self.GoalState
    def Action(self,state):          #`获取某状态下的行为集合（邻点）`
        n=len(self.Adj)
        res=[i for i in range(n)
             if Adj[i][state]<MAX and Adj[i][state]>0]
        return res
    def Result(self,state, action):  #`在某状态下某行为后的新状态`
        return action                #`邻点既表示行为，也表示新状态`
    def StepCost(self,state,action): #`在某状态下某行为需要的费用`
        return self.Adj[state][action]


class Node():
    def __init__(self,problem, parent=None,action=None):
        #`定义由父亲结点parent通过行为action生成的子结点`
        if parent == None:                #`起始结点`
            self.State=problem.InitialState
            self.Parent=None
            self.Action=None
            self.PathCost=0
        else:
            self.State=problem.Result(parent.State,action)

            self.Parent=parent   #`记录此结点的父亲结点`
            self.Action=action   #`记录生成此结点的行为`
            self.PathCost=parent.PathCost+problem.StepCost(\
                parent.State,action)#`到此结点路径总费用`
                                 
        self.g=self.PathCost     #`g信息`
        self.h=Tools.distance(   #`h信息`
                   problem.Points[self.State],
                   problem.Points[problem.GoalState])
        self.f=self.g+self.h     #`f信息`
    def __lt__(self, other):
        return other.f > self.f  #`符号重载，用于优先队列对结点排序`

def Solution(node):
    s=[node.State]
    while (node.Parent!=None):
        s.insert(0, node.Parent.State)
        node=node.Parent
    return s
        
############################################3###
#请实现A*算法，并返回最短路径长度，

def Astar(problem):
    # 创建一个优先队列，用于存储待探索的节点
    frontier = PriorityQueue()
    # 创建一个集合，用于存储已经探索过的节点，避免重复探索
    explored = set()
    
    # 初始化起始节点
    start_node = Node(problem)
    frontier.put(start_node)
    
    # 当优先队列不为空时继续搜索
    while not frontier.empty():
        # 从优先队列中取出f值最小的节点
        current_node = frontier.get()
        
        # 如果已经探索过这个节点，则跳过
        if current_node.State in explored:
            continue
        
        # 将当前节点标记为已探索
        explored.add(current_node.State)
        
        # 如果到达目标状态，则返回路径
        if problem.GoalTest(current_node.State):
            return Solution(current_node), current_node.PathCost
        
        # 获取当前节点的所有可能行为（邻点）
        actions = problem.Action(current_node.State)
        
        # 对每个行为生成新的节点，并将其加入优先队列
        for action in actions:
            child_node = Node(problem, current_node, action)
            
            # 如果新节点未被探索过，则加入优先队列
            if child_node.State not in explored:
                frontier.put(child_node)
    
    # 如果没有找到路径，则返回None和无穷大
    return None, float('inf')




################################################

if __name__ == '__main__':
    MAX=100000
    n=35
    x, y = map(float, input().strip().split())
    s = Tools.Point(x, y, 's')
    a1 = Tools.Point(51.5, 180.5, "a1")
    a2 = Tools.Point(51.5,240.5, "a2")
    a3 = Tools.Point(242,240.5, "a3")
    a4 = Tools.Point(242,180.5, "a4")
    

    b1 = Tools.Point(101,9.8, "b1")
    b2 = Tools.Point(31.8,72.4, "b2")
    b3 = Tools.Point(43,134.8, "b3")
    b4 = Tools.Point(109.2,150, "b4")
    b5 = Tools.Point(145.4,70.6, "b5")

    c1 = Tools.Point(172.9,59.8, "c1")
    c2 = Tools.Point(147.1,155.4, "c2")
    c3 = Tools.Point(200.4,155.4, "c3")

    d1 = Tools.Point(204.8,15, "d1")
    d2 = Tools.Point(204.8,91.4, "d2")
    d3 = Tools.Point(284.6,44.4, "d3")
    d4 = Tools.Point(251.9,9.8, "d4")
  

    e1 = Tools.Point(251.7,124, "e1")
    e2 = Tools.Point(270,211, "e2")
    e3 = Tools.Point(309.9,173.4, "e3")

    f1 = Tools.Point(295.6,16.4, "f1")
    f2 = Tools.Point(375.5,16.4, "f2")
    f3 = Tools.Point(375.5,142.5, "f3")
    f4 = Tools.Point(295.6,142.5, "f4")
    

    h1 = Tools.Point(415,14.4, "h1")
    h2 = Tools.Point(385.8,33.3, "h2")
    h3 = Tools.Point(425.5,156.9, "h3")
    h4 = Tools.Point(437.3,37.9, "h4")
 

    i1 = Tools.Point(384,148.7, "i1")
    i2 = Tools.Point(340.7,178, "i2")
    i3 = Tools.Point(340.7,221.7, "i3")
    i4 = Tools.Point(377.2,244.3, "i4")
    i5 = Tools.Point(416.3,224.7, "i5")
    i6 = Tools.Point(416.3,177, "i6")

    g = Tools.Point(448.4,19, "g")


    shapeA = [a1,a2,a3,a4]
    shapeB = [b1,b2,b3,b4,b5]
    shapeC = [c1,c2,c3]
    shapeD = [d1,d2,d3,d4]
    shapeE = [e1,e2,e3]
    shapeF = [f1,f2,f3,f4]
    shapeH = [h1,h2,h3,h4]
    shapeI = [i1,i2,i3,i4,i5,i6]
    shapeAll = [shapeA,shapeB,shapeC,shapeD,shapeE,shapeF,shapeH,shapeI]

    points = [s,
              a1,a2,a3,a4,
              b1,b2,b3,b4,b5,
              c1,c2,c3,
              d1,d2,d3,d4,
              e1,e2,e3,
              f1,f2,f3,f4,
              h1,h2,h3,h4,
              i1,i2,i3,i4,i5,i6,
              g]
    def getArrary():
        n=len(points)
        a = np.zeros([n,n])
        for i in range(0,n):
            for j in range(i,n):
                l = Tools.Line(points[i],points[j])
                a[i,j] = Tools.distance(points[i],points[j])
                for shape in shapeAll:
                    if Tools.shapeInteresect(l,shape) is True:
                        a[i,j] = MAX
                a[j,i]=a[i,j]
        return a
    

    Adj=getArrary()
    p=Problem(points,Adj, 0,34)
    
    L=Astar(p)
    print(L[1])