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

实验结果：

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

源代码：

python"># -*- 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 mathclass Point:def __init__(self, x: float, y: float, name: str):self.x = xself.y = yself.name = nameclass Line:def __init__(self, p1: Point, p2: Point):self.u = p1self.v = p2def 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 Falsereturn 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 Truereturn Falseclass 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.GoalStatedef 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 resdef 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.InitialStateself.Parent = Noneself.Action = Noneself.PathCost = 0else: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.Parentreturn sdef 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 = 100000n = 35s = 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] = MAXa[j, i] = a[i, j]return adef 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)