改进A*算法路径规划：Matlab 实现

一、基本思路

先简单回顾一下 A算法的基本原理。A算法是一种启发式搜索算法，主要通过评价函数 ( f(n) = g(n) + h(n) ) 来衡量从起点到目标点的路径代价。其中，( g(n) ) 是从起点到当前节点的实际代价，( h(n) ) 是从当前节点到目标节点的启发式估计代价。

这次改进的核心思想是：

1. 增加搜索方向：从传统的 8 个方向减少到 5 个方向，优化搜索策略。
2. 避免斜穿障碍物顶点：确保路径不会穿过障碍物的顶点，避免碰撞。
3. 删除中间多余节点：减少路径中的转折点，使路径更加平滑。
4. 改进评价函数：将 ( 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. 删除中间多余节点

为了减少路径中的转折点，可以在路径生成后对节点进行优化。具体方法是删除那些位于直线上的多余节点，只保留关键点。

例如，可以通过以下代码实现路径平滑：

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
end

4. 改进评价函数

传统的 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('障碍物', '起点', '终点', '原始路径', '平滑路径');

四、总结

通过上述改进，路径规划的效果得到了显著提升。尤其是在避免斜穿障碍物和路径平滑方面，改进后的算法表现得更加智能和高效。