基于改进A星与改进人工势场APF的路径规划算法。 A星算法生成全局参考路径，APF实时避开动态障碍物和静态障碍物并到达目标 改进A星： 1.采用5*5邻域搜索 2.动态加权 3.冗余点删除 改进APF:通过只改进斥力函数来解决局部最小和目标不可达 的matlab代码，代码简洁，可扩展性强，可提供。

首先，A星算法，这个老牌选手，以其高效的全局搜索能力著称。但传统的A星算法在搜索邻域和路径优化上还有提升空间。我们采用了5*5的邻域搜索，这意味着在每一步，算法都会考虑更多可能的路径，而不仅仅是上下左右四个方向。这样，即使在复杂的地形中，也能找到更优的路径。

function [path] = improvedAStar(start, goal, map)
    % 初始化open和close列表
    openList = start;
    closeList = [];
    
    while ~isempty(openList)
        % 从openList中选择代价最小的节点
        [currentNode, openList] = selectMinCostNode(openList);
        
        % 如果当前节点是目标节点，则返回路径
        if isGoal(currentNode, goal)
            path = reconstructPath(currentNode);
            return;
        end
        
        % 将当前节点加入closeList
        closeList = [closeList; currentNode];
        
        % 扩展当前节点的5*5邻域
        neighbors = expandNeighbors(currentNode, map, 5);
        
        for i = 1:size(neighbors, 1)
            neighbor = neighbors(i, :);
            
            % 如果邻居节点在closeList中，则跳过
            if isInList(neighbor, closeList)
                continue;
            end
            
            % 计算从起点到邻居节点的代价
            tentativeCost = currentNode.gCost + distance(currentNode, neighbor);
            
            % 如果邻居节点不在openList中，或者新的代价更小
            if ~isInList(neighbor, openList) || tentativeCost < neighbor.gCost
                % 更新邻居节点的代价和父节点
                neighbor.gCost = tentativeCost;
                neighbor.hCost = distance(neighbor, goal);
                neighbor.parent = currentNode;
                
                % 如果邻居节点不在openList中，则加入
                if ~isInList(neighbor, openList)
                    openList = [openList; neighbor];
                end
            end
        end
    end
    
    % 如果没有找到路径，返回空
    path = [];
end

接下来，我们引入了动态加权机制。这个机制允许算法在搜索过程中根据实际情况调整启发式函数的权重。比如，在接近目标时，可以减少对启发式函数的依赖，更多地考虑实际代价，这样可以避免算法在最后阶段“走弯路”。

function [weight] = dynamicWeight(currentNode, goal)
    % 计算当前节点到目标的距离
    dist = distance(currentNode, goal);
    
    % 根据距离动态调整权重
    if dist < 10
        weight = 0.5;
    elseif dist < 20
        weight = 0.7;
    else
        weight = 1.0;
    end
end

最后，我们还加入了冗余点删除的步骤。在生成路径后，算法会检查路径中的每一个点，如果发现某个点可以被绕过而不影响整体路径的可行性，就将其删除。这样，最终的路径会更加简洁，减少不必要的拐弯。

function [path] = removeRedundantPoints(path)
    i = 1;
    while i < length(path) - 1
        % 检查当前点是否可以绕过
        if canSkip(path(i), path(i+2))
            % 删除冗余点
            path(i+1) = [];
        else
            i = i + 1;
        end
    end
end

现在，让我们转向人工势场法。APF算法在实时避障方面表现出色，但传统的APF算法存在局部最小和目标不可达的问题。我们通过改进斥力函数来解决这些问题。新的斥力函数不仅考虑了障碍物的距离，还考虑了障碍物的速度和方向，这样即使在动态环境中，也能有效避开障碍物。

function [repulsiveForce] = improvedRepulsion(robot, obstacle)
    % 计算机器人到障碍物的距离
    dist = distance(robot, obstacle);
    
    % 计算斥力
    if dist < obstacle.radius
        repulsiveForce = (1 / dist - 1 / obstacle.radius) * (robot.position - obstacle.position) / dist^3;
    else
        repulsiveForce = [0; 0];
    end
end

通过结合改进后的A星算法和APF算法，我们得到了一个既能在全局范围内找到最优路径，又能在实时避障的路径规划系统。这个系统不仅代码简洁，而且可扩展性强，可以根据需要进一步优化和调整。

function [finalPath] = combinedPathPlanning(start, goal, map, obstacles)
    % 使用改进的A星算法生成全局路径
    globalPath = improvedAStar(start, goal, map);
    
    % 使用改进的APF算法进行实时避障
    finalPath = [];
    for i = 1:length(globalPath)
        currentPos = globalPath(i);
        repulsiveForce = [0; 0];
        for j = 1:length(obstacles)
            repulsiveForce = repulsiveForce + improvedRepulsion(currentPos, obstacles(j));
        end
        % 更新当前位置
        currentPos = currentPos + repulsiveForce;
        finalPath = [finalPath; currentPos];
    end
end

这就是我们今天要分享的内容。希望这些改进能帮助你在路径规划的道路上走得更远。记住，代码只是工具，真正的艺术在于如何巧妙地运用它们。