FAformulas and algorithmPCplanning and control一、A * 算法核心概念A * 是一种启发式搜索算法结合了 Dijkstra 算法保证最优和贪心算法搜索高效的优点核心思想是对每个待探索的节点计算一个评估函数 f(n)以此决定搜索优先级 优先搜索f(n)最小的节点最终以最小代价找到从起点到终点的最优路径。核心评估函数f(n)g(n)h(n)g(n)从起点到当前节点n的实际代价已走路径的真实长度h(n)从当前节点n到终点的估计代价启发函数关键f(n)从起点经过节点n到终点的总估计代价。启发函数h (n)的选择启发函数的设计直接决定 A * 的性能表格启发函数类型 公式二维网格 特点曼哈顿距离 $h(n) x_n - x_{goal} y_n - y_{goal} $ 适用于只能上下左右移动的网格无对角线欧几里得距离 h(n)(xn​−xgoal​)2(yn​−ygoal​)2​ 适用于可任意方向移动的场景如机器人路径规划切比雪夫距离h(n)max(xn−xgoal,yn−ygoal)h(n) max( x_n - x_{goal} , y_n - y_{goal} )h(n)max(xn​−xgoal​,yn​−ygoal​)适用于可对角线移动的网格关键规则h(n)必须是可采纳的admissible即h(n)≤ 节点n到终点的真实代价这样才能保证 A * 找到最优路径若h(n)0A * 退化为 Dijkstra 算法搜索全图效率低若h(n)过大超过真实代价A * 可能找不到最优路径但搜索速度更快。核心数据结构开放列表Open List存储待探索的节点按f(n)从小到大排序关闭列表Closed List存储已探索完成的节点避免重复搜索。二、A * 算法的使用场景A * 因 “最优 高效” 的特性是路径规划领域的首选算法主要应用场景游戏开发游戏角色 / NPC 的路径寻路如《王者荣耀》《我的世界》的角色移动 移动机器人AGV、扫地机器人、无人机的全局路径规划已知环境地图 自动驾驶高精度地图下的全局路径规划如从 A 点到 B 点的最优道路选择 物流 / 仓储无人叉车、分拣机器人的路径规划 地图导航高德 / 百度地图的步行 / 驾车路径规划底层核心算法之一。优势保证找到最优路径当h(n)可采纳时 搜索效率远高于 Dijkstra 算法启发函数减少无效搜索 逻辑清晰易于实现和扩展。局限性依赖完整的环境地图无法处理未知环境 内存消耗较大需存储开放 / 关闭列表 对动态障碍物适配性差需重新规划。三、A * 算法的具体操作步骤以二维平面连续空间的机器人路径规划为例核心步骤如下步骤 1初始化定义起点S、终点G、障碍物区域 初始化开放列表仅加入起点S计算f(S)g(S)h(S)其中g(S)0 初始化关闭列表空 为每个节点记录父节点用于回溯路径。步骤 2循环搜索从开放列表中取出f(n)最小的节点n当前节点 将节点n从开放列表移至关闭列表 若节点n是终点G回溯父节点得到完整路径结束算法 生成节点n的相邻节点如上下左右 / 周围采样点 对每个相邻节点m 若m在障碍物区域 / 关闭列表中跳过 计算$g(m) g(n) 节点 n 到 m$的实际距离 若m不在开放列表中 计算h(m)如欧几里得距离f(m)g(m)h(m) 将m加入开放列表记录父节点为n 若m已在开放列表中 若新的g(m)更小说明有更优路径到达m更新g(m)、f(m)并将m的父节点改为n 重复步骤 2直到开放列表为空无路径或找到终点。步骤 3路径回溯从终点G开始依次回溯父节点直到起点S反转后得到完整路径。四、A * 算法代码实现二维连续空间以下是适配机器人路径规划的 A * 代码连续空间非网格包含可视化功能可直接运行importnumpy as npimportmatplotlib.pyplot as plt from matplotlib.patchesimportCircleimportmath# 定义节点类存储位置、代价、父节点class Node: def __init__(self, x, y): self.xx# 节点x坐标self.yy# 节点y坐标self.g0.0# 起点到当前节点的实际代价self.h0.0# 当前节点到终点的估计代价self.f0.0# 总代价 f g hself.parentNone# 父节点用于回溯路径def __eq__(self, other):# 重载相等判断用于检查节点是否已存在ifisinstance(other, Node):return(self.xother.x)and(self.yother.y)returnFalse# 定义A*规划器类class AStarPlanner: def __init__(self): self.step_size0.5# 相邻节点的步长控制搜索精度self.obstacle_radius1.0# 障碍物影响半径self.max_iter5000# 最大迭代次数防止死循环# 启发函数欧几里得距离可采纳保证最优def heuristic(self, node, goal_node):returnmath.hypot(node.x - goal_node.x, node.y - goal_node.y)# 检查节点是否在障碍物区域内def is_collision(self, node, obstacles):for(ox, oy)inobstacles:ifmath.hypot(node.x - ox, node.y - oy)self.obstacle_radius:returnTruereturnFalse# 生成当前节点的相邻节点8方向采样def generate_neighbors(self, current_node): neighbors[]# 8个方向的偏移量上下左右对角线directions[(self.step_size,0),(-self.step_size,0),(0, self.step_size),(0, -self.step_size),(self.step_size, self.step_size),(-self.step_size, -self.step_size),(self.step_size, -self.step_size),(-self.step_size, self.step_size)]fordx, dyindirections: xcurrent_node.x dx ycurrent_node.y dy# 限制坐标范围可选防止节点超出地图if0x10and0y10: neighbors.append(Node(x, y))returnneighbors# 从开放列表中找到f值最小的节点def find_min_f_node(self, open_list): min_nodeopen_list[0]fornodeinopen_list:ifnode.fmin_node.f: min_nodenodereturnmin_node# 规划路径def plan(self, start_pos, goal_pos, obstacles):# 初始化起点和终点节点start_nodeNode(start_pos[0], start_pos[1])goal_nodeNode(goal_pos[0], goal_pos[1])# 初始化开放列表和关闭列表open_list[start_node]closed_list[]iter_count0whileopen_list and iter_countself.max_iter: iter_count1# 步骤1取出f值最小的节点current_nodeself.find_min_f_node(open_list)open_list.remove(current_node)closed_list.append(current_node)# 步骤2检查是否到达终点ifcurrent_nodegoal_node:# 回溯路径path[]whilecurrent_node is not None: path.append((current_node.x,current_node.y))current_nodecurrent_node.parent# 反转路径从起点到终点returnpath[::-1]# 步骤3生成相邻节点neighborsself.generate_neighbors(current_node)forneighborinneighbors:# 跳过碰撞节点和已探索节点ifself.is_collision(neighbor, obstacles)or neighborinclosed_list:continue# 计算g值当前节点到相邻节点的实际距离tentative_gcurrent_node.g math.hypot(neighbor.x - current_node.x, neighbor.y - current_node.y)# 情况1相邻节点不在开放列表中ifneighbor notinopen_list: neighbor.gtentative_g neighbor.hself.heuristic(neighbor, goal_node)neighbor.fneighbor.g neighbor.h neighbor.parentcurrent_node open_list.append(neighbor)# 情况2相邻节点已在开放列表中检查是否有更优路径else:# 找到开放列表中的对应节点foropen_nodeinopen_list:ifopen_nodeneighbor:iftentative_gopen_node.g: open_node.gtentative_g open_node.fopen_node.g open_node.h open_node.parentcurrent_nodebreak# 若迭代结束仍未找到路径print(警告未找到有效路径)return[]# -------------------------- 测试代码 --------------------------if__name____main__:# 1. 初始化A*规划器a_starAStarPlanner()# 2. 设置起点、终点、障碍物start(0.0,0.0)# 起点goal(8.0,8.0)# 终点obstacles[# 障碍物列表(3.0,3.0),(5.0,5.0),(7.0,3.0)]# 3. 规划路径patha_star.plan(start, goal, obstacles)# 4. 可视化结果plt.figure(figsize(8,8))# 绘制障碍物for(ox, oy)inobstacles: circleCircle((ox, oy), a_star.obstacle_radius,colorr,alpha0.3,labelObstacleif(ox, oy)obstacles[0]else)plt.gca().add_patch(circle)# 绘制路径ifpath: path_x[p[0]forpinpath]path_y[p[1]forpinpath]plt.plot(path_x, path_y,b-,linewidth2,labelA* Path)# 绘制起点和终点plt.scatter(start[0], start[1],colorgreen,s100,labelStart)plt.scatter(goal[0], goal[1],colorred,s100,labelGoal)# 图表设置plt.xlabel(X (m))plt.ylabel(Y (m))plt.title(A* Path Planning)plt.legend(locupper left)# 图例放左上角plt.grid(True)plt.axis(equal)plt.xlim(0,10)plt.ylim(0,10)plt.show()五、代码关键部分解释1. 节点类Node存储节点的核心信息坐标x,y、代价g/h/f、父节点用于路径回溯重载__eq__方法方便判断节点是否重复。2. 核心方法heuristic计算欧几里得距离作为启发函数可采纳保证最优is_collision检查节点是否在障碍物范围内避免碰撞generate_neighbors生成当前节点的 8 方向相邻节点控制搜索范围find_min_f_node从开放列表中找到f(n)最小的节点核心搜索逻辑plan主规划函数实现 A * 的完整搜索流程。3. 运行前置条件安装依赖pipinstallnumpy matplotlib4. 运行效果代码会绘制出从(0,0)到(8,8)的最优路径绕过(3,3)、(5,5)、(7,3)三个障碍物路径是连续的坐标点可直接用于机器人的运动控制图例默认放在左上角符合你之前的要求。六、A* vs APF人工势场法表格特性A * 算法APF 算法路径最优性保证最优h (n) 可采纳时不一定最优易陷入局部最小值环境要求需完整地图全局规划可局部感知局部规划实时性中等需搜索高实时计算受力障碍物适配静态障碍物最优动态障碍物适配性好实现复杂度中等需管理开放 / 关闭列表简单仅计算受力七、结束语核心原理A * 通过评估函数f(n)g(n)h(n)优先搜索最优路径启发函数h(n)是关键需可采纳以保证最优关键步骤初始化→循环搜索取最小f节点→生成邻居→更新代价→路径回溯使用要点启发函数优先选欧几里得 / 曼哈顿距离保证可采纳步长step_size越小路径越精准但搜索效率越低适用于已知静态环境的全局最优路径规划核心优势相比 APFA * 不会陷入局部最小值能保证找到最优路径。