启发式搜索:A*算法《人工智能案例与实验》

声明：著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

实验结果：

1.1 教学目标
(1)让学生能够应用基于搜索的技术来实现问题求解智能体。能够进行问题建模、数据结构设计及A*算法设计。
(2)能够对 A*算法和 Dijkstra 算法进行分析和比较。
(3)能够使用matplotlib 完成计算及结果展示。
1.2 实验内容与任务
图 1.1是机器人导航问题的地图。机器人需要从起点 Start 出发,搜索目标点 Goal。
图中存在一些凸多边形障碍物，设计算法寻求从Start 点到 Goal 点的最短路径。

源代码：

# -*- coding: utf-8 -*-
"""
Created on Wed Mar 19 10:52:11 2025

@author: 破无差
"""
from queue import PriorityQueue
import numpy as np
import matplotlib.pyplot as plt
import math

class Point:
    def __init__(self, x: float, y: float, name: str):
        self.x = x
        self.y = y
        self.name = name

class Line:
    def __init__(self, p1: Point, p2: Point):
        self.u = p1
        self.v = p2

def distance(p1: Point, p2: Point):
    return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2)

def in_line(A, B, C):
    return (C.y - A.y) * (B.x - A.x) == (B.y - A.y) * (C.x - A.x)

def ccw(A, B, C):
    return (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x)

def intersect(A, B, C, D):
    if in_line(A, C, D) or in_line(B, C, D) or in_line(C, A, B) or in_line(D, A, B):
        return False
    return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)

def shapeInteresect(l1: Line, points):
    for i in range(0, len(points)):
        for j in range(i, len(points)):
            l = Line(points[i], points[j])
            if intersect(l1.u, l1.v, points[i], points[j]):
                return True
    return False

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 self.Adj[i][state] < MAX and self.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 is 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 = distance(problem.Points[self.State], problem.Points[problem.GoalState])  # h 信息
        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 is not None:
        s.insert(0, node.Parent.State)
        node = node.Parent
    return s

def Astar(problem):
    node = Node(problem)  # 起始结点
    if problem.GoalTest(node.State):
        return Solution(node)
    frontier = PriorityQueue()  # 前沿是 f 值最小优先的队列
    frontier.put(node)  # 起始结点进入前沿
    explored = set()  # 已探索状态记录区
    while frontier.qsize() > 0:
        node = frontier.get()  # 取出一个前沿结点
        if problem.GoalTest(node.State):
            print(node.PathCost, len(explored))
            return Solution(node)
        explored.add(node.State)  # 状态已探索
        for action in problem.Action(node.State):  # 遍历行为
            child = Node(problem, node, action)  # 生成子结点
            if child.State not in explored:
                frontier.put(child)  # 子结点进入前沿

if __name__ == '__main__':
    MAX = 100000
    n = 35

    s = Point(160,260, 's')
    a1 = Point(51.5, 180.5, "a1")
    a2 = Point(51.5, 240.5, "a2")
    a3 = Point(242, 180.5, "a3")
    a4 = Point(242, 240.5, "a4")

    b1 = Point(101.9, 8, "b1")
    b2 = Point(31.8, 72.4, "b2")
    b3 = Point(43, 134.8, "b3")
    b4 = Point(109.2, 150, "b4")
    b5 = Point(145.4, 70.6, "b5")

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

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

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

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

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

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

    g = 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 getArray():
        n = len(points)
        a = np.zeros([n, n])
        for i in range(0, n):
            for j in range(i, n):
                l = Line(points[i], points[j])
                a[i, j] = distance(points[i], points[j])
                for shape in shapeAll:
                    if shapeInteresect(l, shape):
                        a[i, j] = MAX
                a[j, i] = a[i, j]
        return a

    def Draw(shapes, points, Adj, solution):
        font = {'family': 'SimHei',
                'weight': 'bold',
                'size': '12'}
        plt.rc('font', **font)
        fig = plt.figure()
        n = len(points)
        ax = fig.add_subplot(1, 1, 1)
        ax.tick_params(labelbottom=False, labeltop=True)
        ax.set_ylim(300, 0)
        ax.set_xlim(0, 500)

        for s in shapes:
            for p in s:
                for q in s:
                    plt.plot([p.x, q.x], [p.y, q.y], color="black")
        for i in range(len(solution) - 1):
            plt.plot([points[solution[i]].x, points[solution[i + 1]].x],
                     [points[solution[i]].y, points[solution[i + 1]].y], color="red")

        plt.text(points[0].x, points[0].y, "S", color="black")
        plt.text(points[-1].x, points[-1].y, "G", color="black")
        plt.show()

    Adj = getArray()
    p = Problem(points, Adj, 0, 34)

    s = Astar(p)
    print(s)
    Draw(shapeAll, points, Adj, s)