A星算法Matlab版本详细讲解 A星算法Matlab版本详细讲解算法总览map_astar.mA星算法的主函数%% Define a small map map = false(10); % Add an obstacle % 添加障碍物 map (1:5, 6) = true; map (6,8:10) = true; map (3:7,2) = true; %% 设置起始点与目标点 start_coords = [1,3]; dest_coords = [1,9]; % 传入参数，进行A星算法运行 [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3); %% start_coords = [10,1]; dest_coords = [2,9]; [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3); %% start_coords = [6,3]; dest_coords = [8,9]; [route, numExpanded ,cost_matrix] = AStarGrid (map, start_coords, dest_coords,true); pause(3); %% start_coords = [6,1]; dest_coords = [10,10]; [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3); %% start_coords = [6,1]; dest_coords = [1,10]; [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3);AStarGrid.mA星算法的具体实现过程function [route,numExpanded,cost_matrix] = AStarGrid (input_map, start_coords, dest_coords, drawMap) % Run A* algorithm on a grid. % Inputs : % input_map : a logical array where the freespace cells are false or 0 and % the obstacles are true or 1 % start_coords and dest_coords : Coordinates of the start and end cell % respectively, the first entry is the row and the second the column. % Output : % route : An array containing the linear indices of the cells along the % shortest route from start to dest or an empty array if there is no % route. This is a single dimensional vector % numExpanded: Remember to also return the total number of nodes % expanded during your search. Do not count the goal node as an expanded node. % set up color map for display % 1 - white - clear cell % 2 - black - obstacle % 3 - red = visited % 4 - blue - on list % 5 - green - start % 6 - yellow - destination cmap = [1 1 1; ... 0 0 0; ... 1 0 0; ... 0 0 1; ... 0 1 0; ... 1 1 0; ... 0.5 0.5 0.5]; colormap(cmap); % variable to control if the map is being visualized on every % iteration % 通过传入变量对每次迭代是否显示进行控制 drawMapEveryTime = drawMap; % 获取传入地图的行数和列数 [nrows, ncols] = size(input_map); % map - a table that keeps track of the state of each grid cell % 建一个大小为nrows × ncols的二维数组map，并将数组中的所有元素初始化为0。 % 0表示网格未被标记 map = zeros(nrows, ncols); % 将空闲网格标为1 占用网格标为2 map(~input_map) = 1; % Mark free cells map(input_map) = 2; % Mark obstacle cells % Generate linear indices of start and dest nodes %使用sub2ind函数将起始节点的行列坐标start_coords转换为地图map中对应的线性索引。 %size(map)获取地图的大小，start_coords(1)和start_coords(2)分别表示起始节点的行和列坐标。 start_node = sub2ind(size(map), start_coords(1), start_coords(2)); dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2)); map(start_node) = 5; map(dest_node) = 6; % meshgrid will `replicate grid vectors' nrows and ncols to produce % a full grid % type `help meshgrid' in the Matlab command prompt for more information % 创建一个大小为nrows × ncols的二维数组parent，用于存储每个节点的父节点索引。 % 将起始节点的索引值赋给parent数组对应位置，表示起始节点本身没有父节点。 parent = zeros(nrows, ncols); parent(start_node)=start_node; % 用于初始化启发式函数值、代价数组以及其他相关变量 [X, Y] = meshgrid (1: ncols, 1: nrows); % 获取目标节点的坐标，将X坐标赋给变量xd，Y坐标赋给变量yd xd = dest_coords(1); yd = dest_coords(2); % Evaluate Heuristic function, H, for each grid cell % Manhattan distance % 计算启发式函数H，即曼哈顿距离，用于估计从每个网格单元到目标节点的代价 H = abs(X - xd) + abs(Y - yd); H = H'; % Initialize cost arrays % 两个数组用于记录每个节点的代价函数值 F(X)+G(X)构成代价函数 % 代价数组用于记录每个节点的代价函数值，其中f表示总代价（启发式函数值加上代价函数值），g表示从起始节点到当前节点的代价。 f = Inf(nrows, ncols);% 初始化为无穷 g = Inf(nrows, ncols); % 起始节点的代价函数值g设置为0，将起始节点的启发式函数值H设置为起始节点在启发式函数中的值 g(start_node) = 0; f(start_node) = H(start_node); % 初始化一个代价矩阵cost_matrix，用于记录每个节点到起始节点的代价 cost_matrix = Inf(nrows, ncols);%这个数组用于记录每个节点到起始节点的代价值 cost_matrix(start_node) = 0;%起始节点的代价值设置为0 % keep track of the number of nodes that are expanded % 初始化一个计数器numExpanded，用于跟踪已扩展的节点数目 numExpanded = 0; % Main Loop while true % Draw current map map(start_node) = 5;%特定标记 起始节点标记为5 map(dest_node) = 6;%特定标记 目标节点标记为6 % make drawMapEveryTime = true if you want to see how the % nodes are expanded on the grid. if (drawMapEveryTime) image(1.5, 1.5, map); % 使用image函数将地图绘制在图形窗口中 grid on; % 用于显示网格线 axis image; % 设置坐标轴比例一致 drawnow; % 实时更新图形窗口 end % Find the node with the minimum f value % 找到代价数组f中的最小代价值min_f以及对应的节点索引current % 表示选择具有最小代价的节点作为当前节点进行扩展 [min_f, current] = min(f(:)); %如果当前节点等于目标节点或者最小代价为正无穷，则跳出循环 % 表示已经找到路径或者无法到达目标节点。 if ((current == dest_node) || isinf(min_f)) break end % Update input_map map(current) = 3;%特定标记 将当前节点在地图中标记为3，表示该节点已经被扩展 %将当前节点在代价数组f中的值设为正无穷，表示不再考虑该节点，避免重复扩展 f(current) = Inf; % remove this node from further consideration % Compute row, column coordinates of current node % 根据当前节点的线性索引current计算其在代价数组f中的行列坐标。 % size(f)获取代价数组的大小。然后使用ind2sub函数将线性索引转换为行列坐标 [i, j] = ind2sub(size(f), current); % ********************************************************************* if ((i>=1)&&(i<10)&&(j>=1)&&(j<=10)&&(parent(i+1,j) == 0)&& input_map(i+1,j) ~=1) g(i+1,j) = g(i,j)+1; f(i+1,j) = g(i+1,j)+H(i+1,j); cost_matrix(i+1,j) = g(i+1,j)+H(i+1,j); parent(i+1,j)= current; numExpanded = numExpanded+1; end if ((i>1)&&(i<=10)&&(j>=1)&&(j<=10)&&(parent(i-1,j)==0)&& input_map(i-1,j) ~=1) g(i-1,j)= g(i,j)+1; f(i-1,j)= g(i-1,j)+H(i-1,j); cost_matrix(i-1,j)= g(i-1,j)+H(i-1,j); parent(i-1,j)= current; numExpanded = numExpanded+1; end if ((i>=1)&&(i<=10)&&(j>1)&&(j<=10)&&(parent(i,j-1)==0)&& input_map(i,j-1) ~=1) g(i,j-1)= g(i,j)+1; f(i,j-1)= g(i,j-1)+H(i,j-1); cost_matrix(i,j-1)= g(i,j-1)+H(i,j-1); parent(i,j-1)=current; numExpanded = numExpanded+1; end if ((i>=1)&&(i<=10)&&(j>=1)&&(j<10)&&(parent(i,j+1)==0)&& input_map(i,j+1)~=1) g(i,j+1)= g(i,j)+1; f(i,j+1)= g(i,j+1)+H(i,j+1); cost_matrix(i,j+1)= g(i,j+1)+H(i,j+1); parent(i,j+1)=current; numExpanded = numExpanded+1; end %********************************************************************* end %% Construct route from start to dest by following the parent links % 代码检查目标节点的代价函数值f(dest_node)是否为正无穷 % 如果是，则表示无法到达目标节点，路径为空。否则，将目标节点作为路径的起点。 if (isinf(f(dest_node))) route = []; else route = dest_node; % 使用一个循环来构建路径，从目标节点开始，通过查找父节点来逐步向前回溯，直到回溯到起始节点为止 % 在每次循环中，将当前节点的父节点加入路径中，以便构建完整的路径。 while (parent(route(1)) ~= start_node) route = [parent(route(1)), route]; end % Snippet of code used to visualize the map and the path % 代码使用循环遍历路径中的节点 for k = 1:length(route) - 1 map(route(k)) = 7;% 将这些节点在地图中标记为特定的值 7 pause(0.1); image(1.5, 1.5, map); grid on; axis image; end end end算法详细讲解代码段1cmap = [1 1 1; ... 0 0 0; ... 1 0 0; ... 0 0 1; ... 0 1 0; ... 1 1 0; ... 0.5 0.5 0.5]; colormap(cmap);定义了一个7×3的矩阵cmap，每行包含RGB三个分量的值。这些分量定义了不同颜色的组合，每个行表示一个颜色。使用colormap函数将定义好的颜色映射应用到当前图形窗口或坐标轴上。通过指定cmap作为参数，将颜色映射设置为cmap中定义的颜色。代码段2map = zeros(nrows, ncols); map(~input_map) = 1; % Mark free cells map(input_map) = 2; % Mark obstacle cells在这行代码中，~input_map表示对input_map中的元素进行逻辑非运算，即将空闲区域的值变为true（逻辑值为1）而将障碍物区域的值变为false（逻辑值为0）。然后，将逻辑值为true的元素的对应位置在map中标记为1，表示该位置是空闲的。这里假设input_map是一个逻辑数组，与map具有相同的大小。input_map表示障碍物区域的逻辑值为true。将逻辑值为true的元素的对应位置在map中标记为2，表示该位置是障碍物区域。code 3start_node = sub2ind(size(map), start_coords(1), start_coords(2)); dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));sub2ind函数的作用是将给定的行索引和列索引转换为对应的线性索引。它在处理多维数组时非常有用。让我解释一下它的用法和作用：语法：idx = sub2ind(size(A), sub1, sub2, ..., subN)参数：size(A): 数组A的大小，返回的线性索引将在该大小范围内。sub1, sub2, ..., subN: 要转换为线性索引的各维度的子索引值。这些子索引值可以是标量、向量或矩阵。返回值：idx: 转换后的线性索引值，用于在多维数组中定位元素。作用：sub2ind函数的作用是根据给定的子索引值和数组的大小，将多维数组中的元素位置转换为对应的线性索引值。线性索引是一个单一的整数值，用于唯一地标识数组中的元素。这在处理多维数组时非常有用，可以将多维索引转换为线性索引进行高效的元素访问和操作。在给定大小为[M, N]的二维数组时，sub2ind函数将二维坐标(i, j)转换为线性索引idx的公式为：idx = (j-1) * M + isub2ind函数可以用于索引多维数组中的元素，例如在地图或图像处理中将二维坐标转换为线性索引以快速访问数组中的元素。4matlabCopy code [X, Y] = meshgrid(1:ncols, 1:nrows);使用meshgrid函数生成网格坐标矩阵。meshgrid函数用于在指定的一维向量范围内生成二维矩阵，其中矩阵的行表示Y坐标，列表示X坐标。在这里，1:ncols生成X坐标向量，1:nrows生成Y坐标向量，然后使用meshgrid函数生成网格坐标矩阵。code 5if ((i>=1)&&(i<10)&&(j>=1)&&(j<=10)&&(parent(i+1,j) == 0)&& input_map(i+1,j) ~=1) g(i+1,j) = g(i,j)+1; f(i+1,j) = g(i+1,j)+H(i+1,j); cost_matrix(i+1,j) = g(i+1,j)+H(i+1,j); parent(i+1,j)= current; numExpanded = numExpanded+1; end这段代码用于判断当前节点的下方相邻节点是否可扩展。判断条件包括：下方相邻节点在地图范围内，其父节点未被设置（parent(i+1,j) == 0），下方相邻节点不是障碍物（input_map(i+1,j) ~=1）。如果满足条件，则更新下方相邻节点的代价和其他信息，包括更新到起始节点的代价g，计算代价函数值f，更新代价矩阵cost_matrix，设置父节点为当前节点parent(i+1,j) = current，并增加已扩展节点的计数器numExpanded。类似地，下面的代码块分别用于判断当前节点的上方、左侧和右侧相邻节点是否可扩展，并进行相应的代价和信息更新。父节点数组有什么用处父节点数组在路径规划算法中用于记录每个节点的父节点索引。每个节点的父节点是在路径搜索过程中，到达当前节点的最佳路径上的前一个节点。在路径规划算法中，从起始节点开始，通过不断扩展节点和更新代价的过程，寻找到达目标节点的最佳路径。父节点数组可以用于回溯路径，从目标节点开始，通过查找父节点索引，依次找到到达起始节点的最佳路径上的所有节点。通过父节点数组，我们可以重构最佳路径，了解从起始节点到目标节点的具体路径。这在可视化路径、导航指引、路径评估等应用中非常有用。父节点数组还可以用于其他相关操作，例如判断两个节点是否相邻，判断节点是否已经被访问等。通过访问父节点数组，我们可以获取与当前节点相关的信息，辅助路径规划算法的执行和路径搜索的优化。H = H'有什么用在这段代码中，计算启发式函数H时，我们使用了H = H'来对计算结果进行转置操作。这是因为在网格地图中，行表示Y坐标，列表示X坐标。而在计算启发式函数时，我们需要每个网格单元与目标节点的曼哈顿距离，这就要求我们的计算结果与地图的行列顺序保持一致。当我们使用abs(X - xd) + abs(Y - yd)计算每个网格单元与目标节点的曼哈顿距离时，X和Y都是从meshgrid函数生成的网格坐标矩阵。默认情况下，X和Y的行表示Y坐标，列表示X坐标。然而，在地图中，我们通常使用行列表示Y坐标，列表示X坐标。因此，为了使计算结果与地图的行列顺序一致，我们需要对计算结果进行转置操作。通过H = H'，我们将计算结果H的行列顺序进行了交换，使得H的行表示X坐标，列表示Y坐标，与地图的行列顺序一致。这样，在后续的路径规划算法中，我们可以方便地通过行和列索引来访问和处理启发式函数值H。

路径规划与轨迹规划辨析 路径规划与轨迹规划路径规划是什么路径规划是在给定环境中确定从起点到终点的最佳路径的过程。在机器人导航、自动驾驶和物流等领域中，路径规划是实现自主移动和导航的重要任务。路径规划的目标是找到一条能够满足特定条件和约束的路径，使机器人或车辆能够从起始位置安全、高效地到达目标位置。这些条件和约束可能包括避开障碍物、考虑运动约束（如最大速度、最大加速度）、最小化时间或能量消耗等。路径规划通常是在静态环境中进行的，它可以是离线计算的（预先规划好的路径），也可以是在线计算的（实时计算适应动态环境）。路径规划算法的选择取决于环境特点、机器人能力以及具体需求。常用的路径规划算法包括：Dijkstra算法：用于计算无权图中的最短路径，适用于环境没有障碍物的情况。A*算法：结合了Dijkstra算法和启发式函数，用于在带有权重的图中计算最短路径，适用于静态环境中的路径规划。RRT（Rapidly-Exploring Random Trees）算法：基于随机采样的快速探索树算法，适用于具有复杂环境和障碍物的路径规划。PRM（Probabilistic Roadmap）算法：基于随机采样和连接的概率图搜索算法，适用于高维状态空间和复杂障碍物的路径规划。路径规划的应用广泛，例如无人机的航迹规划、自动驾驶车辆的路线规划、仓储机器人的路径规划等。通过有效的路径规划，机器人能够在复杂环境中快速、安全地移动，并实现各种任务和应用。轨迹规划是什么轨迹规划（Trajectory Planning）是指在已经确定的路径上，生成机器人或车辆在连续时间上的运动轨迹的过程。它关注的是在动态环境中，考虑机器人的运动约束、动力学限制和避障需求，生成平滑、连续的运动轨迹。轨迹规划的目标是生成适应当前环境和机器人能力的运动轨迹，以确保机器人能够安全、稳定地执行移动任务。在轨迹规划中，会考虑以下因素：运动约束：考虑机器人或车辆的运动约束，例如最大速度、最大加速度、最大转角等，以确保生成的轨迹在机器人的可操作范围内。动力学限制：考虑机器人的动力学特性，例如惯性、摩擦力等，以生成平滑且符合物理特性的轨迹。环境感知：根据环境中的障碍物、地形信息和其他运动物体等，进行碰撞避免和路径调整，以生成与环境相容的轨迹。轨迹优化：使用优化算法，例如最小化能量消耗、最小化时间、最小化运动曲率等，以生成经过优化的轨迹。常见的轨迹规划方法包括速度规划、加速度规划、优化算法（如最小时间优化、样条曲线优化）等。这些方法旨在生成平滑、连续的运动轨迹，以实现机器人或车辆的高效、安全移动。轨迹规划在许多领域都有应用，例如机器人导航、自动驾驶、无人机飞行路径规划、工业机器人的轨迹控制等。通过合适的轨迹规划，机器人能够在复杂的动态环境中灵活、高效地移动，并完成各种任务和动作要求。轨迹规划与路径规划的异同轨迹规划（Trajectory Planning）和路径规划（Path Planning）是机器人导航和移动中两个相关但不同的概念，它们在实现机器人自主移动和避障方面起着不同的作用。异同之处如下：定义和目标：路径规划：路径规划是确定从起始位置到目标位置的最佳路径的过程。它关注在给定的环境中选择路径，以使机器人能够安全、高效地到达目标位置。路径规划是在全局层面进行，考虑整个环境和完整的路径。轨迹规划：轨迹规划是在已确定的路径上生成机器人在连续时间上的运动轨迹的过程。它关注在动态环境中生成平滑、连续的运动轨迹，以使机器人能够在当前环境中安全、稳定地移动。粒度和连续性：路径规划：路径规划通常以离散的方式表示，将路径抽象为一系列路径点或路径段。路径规划不涉及具体的时间和连续的运动。轨迹规划：轨迹规划通常以连续的方式表示，生成机器人在连续时间上的运动轨迹。轨迹规划考虑机器人的动力学限制和运动连续性，以生成平滑、连续的运动轨迹。考虑因素：路径规划：路径规划主要考虑地图、障碍物、运动约束等因素，以找到一条满足约束条件的最佳路径。轨迹规划：轨迹规划除了考虑路径规划中的因素外，还考虑机器人的动力学约束、环境感知和实时性需求，以生成适应当前环境和机器人能力的平滑轨迹。应用领域：路径规划：路径规划广泛应用于机器人导航、自动驾驶、无人机航迹规划等领域，以实现从起始位置到目标位置的路径规划。轨迹规划：轨迹规划主要应用于机器人导航、自动驾驶、工业机器人的轨迹控制等领域，以生成连续时间上的平滑运动轨迹。尽管路径规划和轨迹规划在某些方面有重叠，但它们在粒度、连续性和考虑因素等方面存在明显的区别。在实际应用中，路径规划和轨迹规划通常需要结合使用，以实现机器人的自主移动和导航。

轨迹规划总览 轨迹规划（Trajectory Planning）是指在已经确定的路径上，生成机器人或车辆在连续时间上的运动轨迹的过程。它关注的是在动态环境中，考虑机器人的运动约束、动力学限制和避障需求，生成平滑、连续的运动轨迹。轨迹规划的目标是生成适应当前环境和机器人能力的运动轨迹，以确保机器人能够安全、稳定地执行移动任务。在轨迹规划中，会考虑以下因素：运动约束：考虑机器人或车辆的运动约束，例如最大速度、最大加速度、最大转角等，以确保生成的轨迹在机器人的可操作范围内。动力学限制：考虑机器人的动力学特性，例如惯性、摩擦力等，以生成平滑且符合物理特性的轨迹。环境感知：根据环境中的障碍物、地形信息和其他运动物体等，进行碰撞避免和路径调整，以生成与环境相容的轨迹。轨迹优化：使用优化算法，例如最小化能量消耗、最小化时间、最小化运动曲率等，以生成经过优化的轨迹。常见的轨迹规划方法包括速度规划、加速度规划、优化算法（如最小时间优化、样条曲线优化）等。这些方法旨在生成平滑、连续的运动轨迹，以实现机器人或车辆的高效、安全移动。轨迹规划在许多领域都有应用，例如机器人导航、自动驾驶、无人机飞行路径规划、工业机器人的轨迹控制等。通过合适的轨迹规划，机器人能够在复杂的动态环境中灵活、高效地移动，并完成各种任务和动作要求。常见的轨迹规划算法第三次多项式插值（Third-Order Polynomial Interpolation）：该方法使用多项式插值来生成平滑的轨迹。通过指定起点、终点和约束条件（如起点和终点速度、加速度限制），可以计算出一个连续的三次多项式曲线，即生成的轨迹。样条插值（Spline Interpolation）：样条插值是一种平滑的插值方法，通过连接多个局部的多项式函数来生成轨迹。常见的样条插值方法包括自然样条插值和Hermite样条插值。样条插值可以生成平滑且连续可微的轨迹。最小时间优化（Minimum-Time Optimization）：该方法通过优化算法，如动态规划或二次规划，来寻找最短时间下的最优轨迹。最小时间优化方法考虑了机器人的动力学约束和运动能量，以在规定时间内完成任务。动态窗口方法（Dynamic Window Approach）：动态窗口方法是一种基于机器人动力学约束和环境感知的实时轨迹规划方法。它通过计算机器人在当前时间窗口内的运动状态和轨迹，从一系列可能的轨迹中选择一个最优的轨迹。采样优化方法（Sampling-Based Optimization）：这类方法使用随机采样和优化算法，如遗传算法或优化梯度下降，来搜索最优的轨迹。通过在状态空间中进行采样，并根据某种目标函数评估轨迹质量，优化算法可以找到一个合适的轨迹。这些是常见的轨迹规划算法，每种算法都有其适用的场景和特点。具体选择哪种算法取决于问题的要求、机器人的特性和环境的约束。在实际应用中，通常需要综合考虑机器人的运动约束、环境感知和实时性等因素，选择合适的轨迹规划算法。

A星算法（含python实现 | c++实现） A*算法（含python实现 | c++实现）参考资料Introduction to the A* Algorithm路径规划与轨迹跟踪系列算法Robotic Motion Planning LecturesA星算法详解A星寻路算法路径规划之 A* 算法1. 算法简介[link][item name="深度优先搜索（DFS）和广度优先搜索（BFS）" link="https://xinin.top/index.php/archives/Depth-First-Search-and-Breadth-First-Search.html" desc="" /][item name="A星算法原理讲解" link="https://xinin.top/index.php/archives/Astar-Explanation.html" desc="" /][/link]A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法，广泛应用于室内机器人路径搜索、游戏动画路径搜索等。它是图搜索算法的一种。A*算法是一种启发式的搜索算法，它是基于深度优先算法(Depth First Search, DFS)和广度优先算法(Breadth First Search, BFS)的一种融合算法，按照一定原则确定如何选取下一个结点。启发式搜索算法指的是，从起点出发，先寻找起点相邻的栅格，判断它是否是最好的位置，基于这个最好的栅格再往外向其相邻的栅格扩展，找到一个此时最好的位置，通过这样一步一步逼近目标点，减少盲目的搜索，提高了可行性和搜索效率。深度优先搜索算法的思想是，搜索算法从起点开始进行搜索(初始状态下待搜索区域内所有结点均未被访问)，与周围所有邻点进行比较，选择其中距离终点最近的点进行存储，然后再以该邻点为基础对比其周围未被访问的所有邻点，仍然选择距离终点最近的邻点存储。若访问结点到达终点或访问完所有结点仍未到达终点，则视为搜索失败。成功搜索所存储的结点连接而成的路径即为起点到终点的路径。广度优先搜索的原理是，从初始点出发依次访问与它相连接的邻点，访问完毕后再从这些邻点出发访问邻点的邻点,但是要保证先被访问的邻点的邻点要比后被访问的邻点的邻点先访问，直至图中所有已被访问的结点的邻点都被访问到。如果此时图中尚有未被访问的结点，则需要选取一个尚未被访问的结点作为个新的初始点,继续搜索访问，直到图中所有的结点都被访问一遍为止。因此深度优先算法与广度优先搜索算法从过程上存在较大差异。深度优先算法优先选择离目标点最近的结点，而广度优先搜索算法优先选择离初始点最近的点。基于深度优先算法,能以最快的速度找到一条连接初始点到目标点的路径，但不能保证路径的最优性(例如以路径最短为标准)；广度优先搜索算法则必然能找到最短的路径，但由于需要遍历所有的结点，其计算复杂程度更大。基于这两种算法的优缺点，A*算法基于启发函数构建了代价函数，既考虑了新结点距离初始点的代价，又考虑了新结点与目标点距离的代价。A*算法使用一个路径优劣评价公式为：$ f(n)=g(n)+h(n)$f(n) 是从初始状态经由状态n到目标状态的代价估计，g(n) 是从初始状态到状态n的实际代价，h(n) 是从状态n到目标状态的最佳路径的估计代价。A*算法需要维护两个状态表，分别称为openList表和closeList表。openList表由待考察的节点组成， closeList表由已经考察过的节点组成。2. 算法精讲2.1 预处理如图，假设我们需要从A点到目标点，这两点之间有一堵墙。首先，我们把地图栅格化，把每一个方格的中心称为节点；这个特殊的方法把我们的搜索区域简化为了 2 维数组。数组的每一项代表一个格子，它的状态就是可走 (walkalbe) 和不可走 (unwalkable) 。通过计算出从 A 到 目标点需要走过哪些方格，就找到了路径。一旦路径找到了，便从一个方格的中心移动到另一个方格的中心，直至到达目的地。2.2 开始搜索一旦我们把搜寻区域简化为一组可以量化的节点后，我们下一步要做的便是查找最短路径。在 A* 中，我们从起点开始，检查其相邻的方格，然后向四周扩展，直至找到目标。从起点 A 开始，定义A为父节点，并把它加入到 openList中。现在 openList 里只有起点 A ，后面会慢慢加入更多的项。如上图所示，父节点A周围共有8个节点，定义为子节点。将子节点中可达的（reachable）或者可走的（walkable）放入openList中，成为待考察对象。若某个节点既未在openList，也没在closeList中，则表明还未搜索到该节点。初始时， 节点A离自身距离为0，路径完全确定，将其移入closeList中； closeList 中的每个方格都是现在不需要再关注的。路径优劣判断依据是移动代价，单步移动代价采取Manhattan 计算方式（即 $d=|x_1-x_2|+|y_1-y_2|$)，即把横向和纵向移动一个节点的代价定义为10。斜向移动代价为14（即 $\sqrt2$ 倍的横向或纵向移动代价）。现在openList = {B,C,D,E,F,G,H,I}, closeList = {A}下面我们需要去选择节点A相邻的子节点中移动代价 f ff最小的节点，下面以节点I的计算为例。首先考察 g，由于从A到该格子是斜向移动，单步移动距离为14，故 g = 14.再考察估计代价 h。估计的含义是指忽略剩下的路径是否包含有障碍物（不可走）， 完全按照Manhattan计算方式，计算只做横向或纵向移动的累积代价：横向向右移动3步，纵向向上移动1步，总共4步，故为 h = 40.因此从A节点移动至I节点的总移动代价为：f=g+h=54以此类推，分别计算当前openList中余下的7个子节点的移动代价 f ，从中挑选最小代价节点F，移到closeList中。现在openList = {B,C,D,E,G,H,I}, closeList = {A,F}2.3 继续搜索从openList中选择 f ff值最小的 ( 方格 ) 节点I*(D节点的 f ff值跟I相同，任选其一即可，不过从速度上考虑，选择最后加入 openList 的方格更快。这导致了在寻路过程中，当靠近目标时，优先使用新找到的方格的偏好。 对相同数据的不同对待，只会导致两种版本的 A ∗ A^*A∗ 找到等长的不同路径 )*，从 openList里取出，放到 closeList 中。检查所有与它相邻的子节点，忽略不可走 (unwalkable) 的节点、以及忽略已经存在于closeList的节点；如果方格不在openList中，则把它们加入到 openList中，并把它们作为节点I的子节点 。如果某个相邻的节点(假设为X)已经在 opeLlist 中，则检查这条路径是否更优，也就是说经由当前节点( 我们选中的节点)到达节点X是否具有更小的 g gg 值。如果没有，不做任何操作。否则，如果 g gg 值更小，则把X的父节点设为当前方格 ，然后重新计算X的 f ff 值和 g gg 值。判断完所有子节点后，现在openList = {B,C,D,E,G,H,J,K,L}, closeList = {A,F,I}依次类推，不断重复。一旦搜索到目标节点T，完成路径搜索，结束算法。2.4 确定实际路径完成路径搜索后，从终点开始，向父节点移动，这样就被带回到了起点，这就是搜索后的路径。如下图所示。从起点 A 移动到终点 T就是简单从路径上的一个方格的中心移动到另一个方格的中心，直至目标。3. 算法总结3.1 算法步骤首先把起点加入 openList 。重复如下过程：遍历 openList ，查找 F 值最小的节点，把它作为当前要处理的节点。把这个节点移到 closeList 。对当前方格的 8 个相邻方格：◆ 如果它是不可抵达的或者它在 closeList 中，忽略它；◆ 如果它不在 openList 中，则把它加入 openList ，并且把当前方格设置为它的父节点，记录该方格的 f , g , h f,g,hf,g,h值。◆ 如果它已经在 openList 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 g gg 值作参考，更小的 g gg 值表示这是更好的路径。如果 g gg值更小，把该节点的父节点设置为当前方格，并重新计算它的 g gg 和 h hh 值。停止搜索的情况有两种：把终点加入到了openList 中，此时路径已经找到了查找终点失败，并且 openList 是空的，此时没有路径。保存路径。使用回溯的方法，从终点开始，每个方格沿着父节点移动直至起点，这就是最终搜索到的路径。3.2 伪代码初始化open_list和close_list； * 将起点加入open_list中，并设其移动代价为0； * 如果open_list不为空，则从open_list中选取移动代价最小的节点n： * 如果节点n为终点，则： * 从终点开始逐步追踪parent节点，一直达到起点； * 返回找到的结果路径，算法结束； * 如果节点n不是终点，则： * 将节点n从open_list中移除，并加入close_list中； * 遍历节点n所有的邻近节点： * 如果邻近节点m在close_list中，则： * 跳过，选取下一个邻近节点 * 如果邻近节点m在open_list中，则： * 计算起点经n到m的g值 * 如果此时计算出来的g值小于原来起点经m的原父节点到m的g值： * 更新m的父节点为n，重新计算m的移动代价f=g+h * 否则，跳过 * 如果邻近节点m不在open_list也不在close_list中，则： * 设置节点m的parent为节点n * 计算节点m的移动代价f=g+h * 将节点m加入open_list中 4. python实现代码实现来自PythonRobotics。""" A* grid planning author: Atsushi Sakai(@Atsushi_twi) Nikos Kanargias (nkana@tee.gr) See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) """ import math import matplotlib.pyplot as plt show_animation = True class AStarPlanner: def __init__(self, ox, oy, resolution, rr): """ Initialize grid map for a star planning ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] resolution: grid resolution [m],地图的像素 rr: robot radius[m] """ self.resolution = resolution self.rr = rr self.min_x, self.min_y = 0, 0 self.max_x, self.max_y = 0, 0 self.obstacle_map = None self.x_width, self.y_width = 0, 0 self.motion = self.get_motion_model() self.calc_obstacle_map(ox, oy) class Node: """定义搜索区域节点类,每个Node都包含坐标x和y, 移动代价cost和父节点索引。 """ def __init__(self, x, y, cost, parent_index): self.x = x # index of grid self.y = y # index of grid self.cost = cost self.parent_index = parent_index def __str__(self): return str(self.x) + "," + str(self.y) + "," + str( self.cost) + "," + str(self.parent_index) def planning(self, sx, sy, gx, gy): """ A star path search 输入起始点和目标点的坐标(sx,sy)和(gx,gy)， 最终输出的结果是路径包含的点的坐标集合rx和ry。 input: s_x: start x position [m] s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] output: rx: x position list of the final path ry: y position list of the final path """ start_node = self.Node(self.calc_xy_index(sx, self.min_x), self.calc_xy_index(sy, self.min_y), 0.0, -1) goal_node = self.Node(self.calc_xy_index(gx, self.min_x), self.calc_xy_index(gy, self.min_y), 0.0, -1) open_set, closed_set = dict(), dict() open_set[self.calc_grid_index(start_node)] = start_node while 1: if len(open_set) == 0: print("Open set is empty..") break c_id = min( open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, open_set[o])) current = open_set[c_id] # show graph if show_animation: # pragma: no cover plt.plot(self.calc_grid_position(current.x, self.min_x), self.calc_grid_position(current.y, self.min_y), "xc") # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if len(closed_set.keys()) % 10 == 0: plt.pause(0.001) # 通过追踪当前位置current.x和current.y来动态展示路径寻找 if current.x == goal_node.x and current.y == goal_node.y: print("Find goal") goal_node.parent_index = current.parent_index goal_node.cost = current.cost break # Remove the item from the open set del open_set[c_id] # Add it to the closed set closed_set[c_id] = current # expand_grid search grid based on motion model for i, _ in enumerate(self.motion): node = self.Node(current.x + self.motion[i][0], current.y + self.motion[i][1], current.cost + self.motion[i][2], c_id) n_id = self.calc_grid_index(node) # If the node is not safe, do nothing if not self.verify_node(node): continue if n_id in closed_set: continue if n_id not in open_set: open_set[n_id] = node # discovered a new node else: if open_set[n_id].cost > node.cost: # This path is the best until now. record it open_set[n_id] = node rx, ry = self.calc_final_path(goal_node, closed_set) return rx, ry def calc_final_path(self, goal_node, closed_set): # generate final course rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ self.calc_grid_position(goal_node.y, self.min_y)] parent_index = goal_node.parent_index while parent_index != -1: n = closed_set[parent_index] rx.append(self.calc_grid_position(n.x, self.min_x)) ry.append(self.calc_grid_position(n.y, self.min_y)) parent_index = n.parent_index return rx, ry @staticmethod def calc_heuristic(n1, n2): """计算启发函数 Args: n1 (_type_): _description_ n2 (_type_): _description_ Returns: _type_: _description_ """ w = 1.0 # weight of heuristic d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) return d def calc_grid_position(self, index, min_position): """ calc grid position :param index: :param min_position: :return: """ pos = index * self.resolution + min_position return pos def calc_xy_index(self, position, min_pos): return round((position - min_pos) / self.resolution) def calc_grid_index(self, node): return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) def verify_node(self, node): px = self.calc_grid_position(node.x, self.min_x) py = self.calc_grid_position(node.y, self.min_y) if px < self.min_x: return False elif py < self.min_y: return False elif px >= self.max_x: return False elif py >= self.max_y: return False # collision check if self.obstacle_map[node.x][node.y]: return False return True def calc_obstacle_map(self, ox, oy): self.min_x = round(min(ox)) self.min_y = round(min(oy)) self.max_x = round(max(ox)) self.max_y = round(max(oy)) print("min_x:", self.min_x) print("min_y:", self.min_y) print("max_x:", self.max_x) print("max_y:", self.max_y) self.x_width = round((self.max_x - self.min_x) / self.resolution) self.y_width = round((self.max_y - self.min_y) / self.resolution) print("x_width:", self.x_width) print("y_width:", self.y_width) # obstacle map generation self.obstacle_map = [[False for _ in range(self.y_width)] for _ in range(self.x_width)] for ix in range(self.x_width): x = self.calc_grid_position(ix, self.min_x) for iy in range(self.y_width): y = self.calc_grid_position(iy, self.min_y) for iox, ioy in zip(ox, oy): d = math.hypot(iox - x, ioy - y) if d <= self.rr: self.obstacle_map[ix][iy] = True break @staticmethod def get_motion_model(): # dx, dy, cost motion = [[1, 0, 1], [0, 1, 1], [-1, 0, 1], [0, -1, 1], [-1, -1, math.sqrt(2)], [-1, 1, math.sqrt(2)], [1, -1, math.sqrt(2)], [1, 1, math.sqrt(2)]] return motion def main(): print(__file__ + " start!!") # start and goal position sx = 10.0 # [m] sy = 10.0 # [m] gx = 50.0 # [m] gy = 50.0 # [m] grid_size = 2.0 # [m] robot_radius = 1.0 # [m] # set obstacle positions ox, oy = [], [] for i in range(-10, 60): ox.append(i) oy.append(-10.0) for i in range(-10, 60): ox.append(60.0) oy.append(i) for i in range(-10, 61): ox.append(i) oy.append(60.0) for i in range(-10, 61): ox.append(-10.0) oy.append(i) for i in range(-10, 40): ox.append(20.0) oy.append(i) for i in range(0, 40): ox.append(40.0) oy.append(60.0 - i) if show_animation: # pragma: no cover plt.plot(ox, oy, ".k") plt.plot(sx, sy, "og") plt.plot(gx, gy, "xb") plt.grid(True) plt.axis("equal") a_star = AStarPlanner(ox, oy, grid_size, robot_radius) rx, ry = a_star.planning(sx, sy, gx, gy) if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") plt.pause(0.001) plt.show() if __name__ == '__main__': main()5. c++实现由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。来自 全局路径规划算法——A*算法（含python实现 | c++实现

A星算法原理讲解 A* 算法原理讲解参考链接原文地址https://github.com/paulQuei/a-star-algorithm?spm=a2c6h.12873639.0.0.21834662LgAvMP算法介绍A*（念做：A Star）算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度。本文在讲解算法的同时也会提供Python语言的代码实现，并会借助matplotlib库动态的展示算法的运算过程。A*算法最初发表于1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael发表。它可以被认为是Dijkstra算法的扩展。由于借助启发函数的引导，A*算法通常拥有更好的性能。广度优先搜索为了更好的理解A*算法，我们首先从广度优先（Breadth First）算法讲起。正如其名称所示，广度优先搜以广度做为优先级进行搜索。从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。这种算法就像洪水（Flood fill）一样向外扩张，算法的过程如下图所示：在上面这幅动态图中，算法遍历了图中所有的点，这通常没有必要。对于有明确终点的问题来说，一旦到达终点便可以提前终止算法，下面这幅图对比了这种情况： 在这里插入图片描述在执行算法的过程中，每个点需要记录达到该点的前一个点的位置 – 可以称之为父节点。这样做之后，一旦到达终点，便可以从终点开始，反过来顺着父节点的顺序找到起点，由此就构成了一条路径。Dijkstra算法Dijkstra算法是由计算机科学家Edsger W. Dijkstra在1956年提出的。Dijkstra算法用来寻找图形中节点之间的最短路径。考虑这样一种场景，在一些情况下，图形中相邻节点之间的移动代价并不相等。例如，游戏中的一幅图，既有平地也有山脉，那么游戏中的角色在平地和山脉中移动的速度通常是不相等的。在Dijkstra算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点。直到到达终点为止。下面对比了不考虑节点移动代价差异的广度优先搜索与考虑移动代价的Dijkstra算法的运算结果： 请添加图片描述当图形为网格图，并且每个节点之间的移动代价是相等的，那么Dijkstra算法将和广度优先算法变得一样。最佳优先搜索在一些情况下，如果我们可以预先计算出每个节点到终点的距离，则我们可以利用这个信息更快的到达终点。其原理也很简单。与Dijkstra算法类似，我们也使用一个优先队列，但此时以每个节点到达终点的距离作为优先级，每次始终选取到终点移动代价最小（离终点最近）的节点作为下一个遍历的节点。这种算法称之为最佳优先（Best First）算法。这样做可以大大加快路径的搜索速度，如下图所示： 请添加图片描述但这种算法会不会有什么缺点呢？答案是肯定的。因为，如果起点和终点之间存在障碍物，则最佳优先算法找到的很可能不是最短路径，下图描述了这种情况。A*算法对比了上面几种算法，最后终于可以讲解本文的重点：A*算法了。下面的描述我们将看到，A*算法实际上是综合上面这些算法的特点于一身的。A*算法通过下面这个函数来计算每个节点的优先级。$ f(n) = g(n)+h(n)$ 其中：f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。g(n)是节点n距离起点的代价。h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。关于启发函数我们在下面详细讲解。A* 算法在运算过程中，每次从优先队列中选取 f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。另外，A*算法使用两个集合来表示待遍历的节点，已经遍历过的节点，这通常称之为open_set和close_set。完整的A*算法描述如下：* 初始化open_set和close_set； * 将起点加入open_set中，并设置优先级为0（优先级最高）； * 如果open_set不为空，则从open_set中选取优先级最高的节点n： * 如果节点n为终点，则： * 从终点开始逐步追踪parent节点，一直达到起点； * 返回找到的结果路径，算法结束； * 如果节点n不是终点，则： * 将节点n从open_set中删除，并加入close_set中； * 遍历节点n所有的邻近节点： * 如果邻近节点m在close_set中，则： * 跳过，选取下一个邻近节点 * 如果邻近节点m也不在open_set中，则： * 设置节点m的parent为节点n * 计算节点m的优先级 * 将节点m加入open_set中启发函数上面已经提到，启发函数会影响A*算法的行为。在极端情况下，当启发函数 h(n)始终为0，则将由g(n)决定节点的优先级，此时算法就退化成了Dijkstra算法。如果 h(n)始终小于等于节点n到终点的代价，则A* 算法保证一定能够找到最短路径。但是当 h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。如果 h(n)完全等于节点n到终点的代价，则A* 算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。如果 h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。 在另外一个极端情况下，如果 h(n)相较于 g(n)大很多，则此时只有 h(n)产生效果，这也就变成了最佳优先搜索。由上面这些信息我们可以知道，通过调节启发函数我们可以控制算法的速度和精确度。因为在一些情况，我们可能未必需要最短路径，而是希望能够尽快找到一个路径即可。这也是A* 算法比较灵活的地方。对于网格形式的图，有以下这些启发函数可以使用：如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。如果图形中允许朝八个方向移动，则可以使用对角距离。如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。关于距离曼哈顿距离如果图形中只允许朝上下左右四个方向移动，则启发函数可以使用曼哈顿距离，它的计算方法如下图所示：计算曼哈顿距离的函数如下，这里的D是指两个相邻节点之间的移动代价，通常是一个固定的常数。function heuristic(node) = dx = abs(node.x - goal.x) dy = abs(node.y - goal.y) return D * (dx + dy)对角距离如果图形中允许斜着朝邻近的节点移动，则启发函数可以使用对角距离。它的计算方法如下：计算对角距离的函数如下，这里的D2指的是两个斜着相邻节点之间的移动代价。如果所有节点都正方形，则其值就是 $ \sqrt{2} * D$ function heuristic(node) = dx = abs(node.x - goal.x) dy = abs(node.y - goal.y) return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)欧几里得距离如果图形中允许朝任意方向移动，则可以使用欧几里得距离。欧几里得距离是指两个节点之间的直线距离，因此其计算方法也是我们比较熟悉的： $ \sqrt{(p2.x-p1.x)^2 + (p2.y-p1.y)^2}$ 。其函数表示如下：function heuristic(node) = dx = abs(node.x - goal.x) dy = abs(node.y - goal.y) return D * sqrt(dx * dx + dy * dy)算法实现算法的源码可以到github上下载：paulQuei/a-star-algorithm。我们的算法演示的是在一个二维的网格图形上从起点找寻终点的求解过程。坐标点与地图首先，我们创建一个非常简单的类来描述图中的点，相关代码如下：# point.py import sys class Point: def __init__(self, x, y): self.x = x self.y = y self.cost = sys.maxsize接着，我们实现一个描述地图结构的类。为了简化算法的描述:我们选定左下角坐标[0, 0]的点是算法起点，右上角坐标[size - 1, size - 1]的点为要找的终点。为了让算法更有趣，我们在地图的中间设置了一个障碍，并且地图中还会包含一些随机的障碍。该类的代码如下：# random_map.py import numpy as np import point class RandomMap: def __init__(self, size=50): # 构造函数，地图的默认大小是50x50； self.size = size self.obstacle = size//8 # 设置障碍物的数量为地图大小除以8； self.GenerateObstacle() # 调用GenerateObstacle生成随机障碍物； def GenerateObstacle(self): self.obstacle_point = [] self.obstacle_point.append(point.Point(self.size//2, self.size//2)) self.obstacle_point.append(point.Point(self.size//2, self.size//2-1)) # Generate an obstacle in the middle for i in range(self.size//2-4, self.size//2): # 在地图的中间生成一个斜着的障碍物； self.obstacle_point.append(point.Point(i, self.size-i)) self.obstacle_point.append(point.Point(i, self.size-i-1)) self.obstacle_point.append(point.Point(self.size-i, i)) self.obstacle_point.append(point.Point(self.size-i, i-1)) for i in range(self.obstacle-1): # 随机生成其他几个障碍物； x = np.random.randint(0, self.size) y = np.random.randint(0, self.size) self.obstacle_point.append(point.Point(x, y)) if (np.random.rand() > 0.5): # Random boolean 障碍物的方向也是随机的； for l in range(self.size//4): self.obstacle_point.append(point.Point(x, y+l)) pass else: for l in range(self.size//4): self.obstacle_point.append(point.Point(x+l, y)) pass def IsObstacle(self, i ,j): # 定义一个方法来判断某个节点是否是障碍物； for p in self.obstacle_point: if i==p.x and j==p.y: return True return False算法主体有了基本的数据结构之后，我们就可以开始实现算法主体了。这里我们通过一个类来封装我们的算法。首先实现一些算法需要的基本函数，它们如下：# a_star.py import sys import time import numpy as np from matplotlib.patches import Rectangle import point import random_map class AStar: def __init__(self, map): # __init__：类的构造函数。 self.map=map self.open_set = [] self.close_set = [] def BaseCost(self, p): # BaseCost：节点到起点的移动代价，对应了上文的 g(n)。 x_dis = p.x y_dis = p.y # Distance to start point return x_dis + y_dis + (np.sqrt(2) - 2) * min(x_dis, y_dis) def HeuristicCost(self, p): # HeuristicCost：节点到终点的启发函数，对应上文的$h(n)$。由于我们是基于网格的图形，所以这个函数和上一个函数用的是对角距离。 x_dis = self.map.size - 1 - p.x y_dis = self.map.size - 1 - p.y # Distance to end point return x_dis + y_dis + (np.sqrt(2) - 2) * min(x_dis, y_dis) def TotalCost(self, p): # TotalCost：代价总和，即对应上面提到的 f(n)。 return self.BaseCost(p) + self.HeuristicCost(p) def IsValidPoint(self, x, y): # IsValidPoint：判断点是否有效，不在地图内部或者障碍物所在点都是无效的。 if x < 0 or y < 0: return False if x >= self.map.size or y >= self.map.size: return False return not self.map.IsObstacle(x, y) def IsInPointList(self, p, point_list): # 判断点是否在某个集合中。 for point in point_list: if point.x == p.x and point.y == p.y: return True return False def IsInOpenList(self, p): # 判断点是否在open_set中。 return self.IsInPointList(p, self.open_set) def IsInCloseList(self, p): # 判断点是否在close_set中。 return self.IsInPointList(p, self.close_set) def IsStartPoint(self, p): # 判断点是否是起点。 return p.x == 0 and p.y ==0 def IsEndPoint(self, p): # 判断点是否是终点。 return p.x == self.map.size-1 and p.y == self.map.size-1有了上面这些辅助函数，就可以开始实现算法主逻辑了，相关代码如下：# a_star.py def RunAndSaveImage(self, ax, plt): start_time = time.time() start_point = point.Point(0, 0) start_point.cost = 0 self.open_set.append(start_point) while True: index = self.SelectPointInOpenList() if index < 0: print('No path found, algorithm failed!!!') return p = self.open_set[index] rec = Rectangle((p.x, p.y), 1, 1, color='c') ax.add_patch(rec) self.SaveImage(plt) if self.IsEndPoint(p): return self.BuildPath(p, ax, plt, start_time) del self.open_set[index] self.close_set.append(p) # Process all neighbors x = p.x y = p.y self.ProcessPoint(x-1, y+1, p) self.ProcessPoint(x-1, y, p) self.ProcessPoint(x-1, y-1, p) self.ProcessPoint(x, y-1, p) self.ProcessPoint(x+1, y-1, p) self.ProcessPoint(x+1, y, p) self.ProcessPoint(x+1, y+1, p) self.ProcessPoint(x, y+1, p)这段代码应该不需要太多解释了，它就是根据前面的算法逻辑进行实现。为了将结果展示出来，我们在算法进行的每一步，都会借助于matplotlib库将状态保存成图片。上面这个函数调用了其他几个函数代码如下:# a_star.py def SaveImage(self, plt): millis = int(round(time.time() * 1000)) filename = './' + str(millis) + '.png' plt.savefig(filename) def ProcessPoint(self, x, y, parent): if not self.IsValidPoint(x, y): return # Do nothing for invalid point p = point.Point(x, y) if self.IsInCloseList(p): return # Do nothing for visited point print('Process Point [', p.x, ',', p.y, ']', ', cost: ', p.cost) if not self.IsInOpenList(p): p.parent = parent p.cost = self.TotalCost(p) self.open_set.append(p) def SelectPointInOpenList(self): index = 0 selected_index = -1 min_cost = sys.maxsize for p in self.open_set: cost = self.TotalCost(p) if cost < min_cost: min_cost = cost selected_index = index index += 1 return selected_index def BuildPath(self, p, ax, plt, start_time): path = [] while True: path.insert(0, p) # Insert first if self.IsStartPoint(p): break else: p = p.parent for p in path: rec = Rectangle((p.x, p.y), 1, 1, color='g') ax.add_patch(rec) plt.draw() self.SaveImage(plt) end_time = time.time() print('===== Algorithm finish in', int(end_time-start_time), ' seconds')这三个函数应该是比较容易理解的：SaveImage：将当前状态保存到图片中，图片以当前时间命名。ProcessPoint：针对每一个节点进行处理：如果是没有处理过的节点，则计算优先级设置父节点，并且添加到open_set中。SelectPointInOpenList：从open_set中找到优先级最高的节点，返回其索引。BuildPath：从终点往回沿着parent构造结果路径。然后从起点开始绘制结果，结果使用绿色方块，每次绘制一步便保存一个图片。测试入口最后是程序的入口逻辑，使用上面写的类来查找路径：# main.py import numpy as np import matplotlib.pyplot as plt from matplotlib.patches import Rectangle import random_map import a_star plt.figure(figsize=(5, 5)) map = random_map.RandomMap() ① ax = plt.gca() ax.set_xlim([0, map.size]) ② ax.set_ylim([0, map.size]) for i in range(map.size): ③ for j in range(map.size): if map.IsObstacle(i,j): rec = Rectangle((i, j), width=1, height=1, color='gray') ax.add_patch(rec) else: rec = Rectangle((i, j), width=1, height=1, edgecolor='gray', facecolor='w') ax.add_patch(rec) rec = Rectangle((0, 0), width = 1, height = 1, facecolor='b') ax.add_patch(rec) ④ rec = Rectangle((map.size-1, map.size-1), width = 1, height = 1, facecolor='r') ax.add_patch(rec) ⑤ plt.axis('equal') ⑥ plt.axis('off') plt.tight_layout() #plt.show() a_star = a_star.AStar(map) a_star.RunAndSaveImage(ax, plt) ⑦`这段代码说明如下：创建一个随机地图；设置图像的内容与地图大小一致；绘制地图：对于障碍物绘制一个灰色的方块，其他区域绘制一个白色的的方块；绘制起点为蓝色方块；绘制终点为红色方块；设置图像的坐标轴比例相等并且隐藏坐标轴；调用算法来查找路径；由于我们的地图是随机的，所以每次运行的结果可能会不一样，下面是我的电脑上某次运行的结果：路径规划之 A* 算法 - 知乎 (zhihu.com)