改进A*算法的Floyd路径规划：Matlab实现
改进A*--Floyd算法路径规划Matlab 可以自己任意选目标点和起点位置， 地图可自己任意更换 1 8个搜索方向变成 5个 提高搜索方向 2 无斜穿障碍物顶点 避免发生碰撞 3 删除中间多余节点，减少转折，增加路径的平滑度 4 评价函数：f(n)=g(n)+h(n) 改为：f(n)=g(n)+（1+r/R)*h(n)。 可修改地图，起始点，目的地点，黑色为栅格障碍物，灰色为搜索空间的遍历节点
最近在学习路径规划算法，特别是A算法和Floyd算法的结合。A算法虽然高效，但在某些场景下还是存在一些可以优化的地方。因此，我决定对A*算法做一些改进，结合Floyd算法来提升路径规划的效果。本文将详细记录这个过程，并且附上Matlab实现代码。
一、基本思路
先简单回顾一下A算法的基本原理。A算法是一种启发式搜索算法，主要通过评价函数 \( f(n) = g(n) + h(n) \) 来衡量从起点到目标点的路径代价。其中，\( g(n) \) 是从起点到当前节点的实际代价，\( h(n) \) 是从当前节点到目标节点的启发式估计代价。
这次改进的核心思想是：
- 增加搜索方向：从传统的8个方向减少到5个方向，优化搜索策略。
- 避免斜穿障碍物顶点：确保路径不会穿过障碍物的顶点，避免碰撞。
- 删除中间多余节点：减少路径中的转折点，使路径更加平滑。
- 改进评价函数：将 \( f(n) = g(n) + h(n) \) 修改为 \( f(n) = g(n) + (1 + r/R) \cdot h(n) \)，引入比例因子 \( r/R \) 以平衡路径代价和启发式估计。
接下来，我会逐个分析这些改进点，并展示如何在Matlab中实现。
二、具体改进及实现
1. 增加搜索方向
传统的A*算法允许8个移动方向（上、下、左、右、4个对角线）。但在实际应用中，这种8方向的移动可能会导致路径过于曲折。因此，我将搜索方向减少到5个，具体包括：
- 4个正方向（上下左右）
- 1个对角线（根据实际需要选择）
在Matlab代码中，可以通过定义移动方式的数组来实现：
% 定义5个移动方向（上、下、左、右、左上对角） move_directions = [ ... [-1, 0], % 上 [1, 0], % 下 [0, -1], % 左 [0, 1], % 右 [-1,-1] % 左上对角（可调整） ];2. 避免斜穿障碍物顶点
为了避免路径斜穿障碍物的顶点，需要在碰撞检测中增加额外的判断。具体来说，当生成一个新的节点时，不仅需要判断当前节点是否在障碍物内部，还需要检查其周围的顶点是否也在障碍物内部。
例如，可以通过以下代码判断某个点是否在障碍物内部：
function isInside = isPointInsideObstacle(x, y, obstacleMap) [rows, cols] = size(obstacleMap); if x < 1 || x > rows || y < 1 || y > cols isInside = false; else isInside = obstacleMap(x, y) == 1; % 假设障碍物标记为1 end end在搜索过程中，每次生成新节点时，都会调用上述函数进行判断，避免进入障碍物区域。
3. 删除中间多余节点
为了减少路径中的转折点，可以在路径生成后对节点进行优化。具体方法是删除那些位于直线上的多余节点，只保留关键点。
改进A*--Floyd算法路径规划Matlab 可以自己任意选目标点和起点位置， 地图可自己任意更换 1 8个搜索方向变成 5个 提高搜索方向 2 无斜穿障碍物顶点 避免发生碰撞 3 删除中间多余节点，减少转折，增加路径的平滑度 4 评价函数：f(n)=g(n)+h(n) 改为：f(n)=g(n)+（1+r/R)*h(n)。 可修改地图，起始点，目的地点，黑色为栅格障碍物，灰色为搜索空间的遍历节点
例如，可以通过以下代码实现路径平滑：
function smoothPath = smoothPath(path) smoothPath = path; i = 2; while i < length(smoothPath)-1 % 判断当前点是否在前两个点的连线上 x1 = smoothPath(i-1,1); y1 = smoothPath(i-1,2); x2 = smoothPath(i,1); y2 = smoothPath(i,2); x3 = smoothPath(i+1,1); y3 = smoothPath(i+1,2); if (y2 - y1) * (x3 - x2) == (y3 - y2) * (x2 - x1) % 判断共线 smoothPath(i,:) = []; i = max(i-1, 2); % 调整索引 else i = i+1; end end end4. 改进评价函数
传统的A*算法评价函数为 \( f(n) = g(n) + h(n) \)。为了平衡路径代价和启发式估计，我将其改进为：
\[ f(n) = g(n) + (1 + r/R) \cdot h(n) \]
其中，\( r \) 是当前节点到目标节点的剩余距离，\( R \) 是整个地图的范围。这个改进可以使得算法在全局搜索和局部优化之间找到更好的平衡。
具体实现如下：
function f = evaluateFunction(g, h, r, R) factor = (1 + r/R); f = g + factor * h; end三、整体实现
现在，我已经将上述改进点逐一实现，下面是一个完整的Matlab代码示例：
% 初始化地图和起终点 mapSize = [50, 50]; start = [6, 6]; goal = [45, 45]; obstacleMap = createObstacleMap(mapSize); % 自定义函数生成障碍物地图 % 定义路径规划算法 function [path] = improvedAStar(obstacleMap, start, goal) % 初始化优先队列 openList = []; closedList = []; % 定义5个移动方向 move_directions = [ ... [-1, 0], % 上 [1, 0], % 下 [0, -1], % 左 [0, 1], % 右 [-1,-1] % 左上对角 ]; % 初始化起点 start.g = 0; start.h = heuristic(start, goal); start.f = evaluateFunction(start.g, start.h, 0, max(size(obstacleMap))); openList = [openList, start]; while ~isempty(openList) % 取出f最小的节点 [~, currentNode] = min(openList(:, 'f')); currentNode = openList(currentNode, :); % 判断是否到达目标点 if isequal(currentNode, goal) path = reconstructPath(closedList, currentNode); return; end % 将当前节点加入closed列表 closedList(end+1,:) = currentNode; % 扩展邻居节点 for i = 1:size(move_directions, 1) neighbor = currentNode + move_directions(i,:); % 判断邻居是否在障碍物内部 if isPointInsideObstacle(neighbor(1), neighbor(2), obstacleMap) continue; end % 计算g值 neighbor.g = currentNode.g + 1; % 假设每步代价为1 % 计算h值 neighbor.h = heuristic(neighbor, goal); % 计算f值 r = norm(goal - neighbor); R = max(size(obstacleMap)); neighbor.f = evaluateFunction(neighbor.g, neighbor.h, r, R); % 判断邻居是否已经存在 if ~isMember(neighbor, closedList) && ~isMember(neighbor, openList) openList(end+1,:) = neighbor; end end end % 如果没有找到路径 path = []; end % 启动算法 [path] = improvedAStar(obstacleMap, start, goal); smoothPath = smoothPath(path); % 绘制结果 figure; imshow(obstacleMap, []); hold on; plot(start(2), start(1), 'ro', 'MarkerSize', 10); plot(goal(2), goal(1), 'bo', 'MarkerSize', 10); plot(path(:,2), path(:,1), 'g-', 'LineWidth', 2); plot(smoothPath(:,2), smoothPath(:,1), 'y-', 'LineWidth', 2); legend('障碍物', '起点', '终点', '原始路径', '平滑路径');四、总结
通过上述改进，我发现路径规划的效果得到了显著提升。尤其是在避免斜穿障碍物和路径平滑方面，改进后的算法表现得更加智能和高效。希望这篇文章能够帮助大家更好地理解A*算法的改进方法，同时也欢迎各位读者提出宝贵意见！
