导航函数避开障碍物形成轨迹matlab代码

以下是一个使用曼哈顿距离作为导航函数，并能够避开障碍物形成轨迹的Matlab代码示例：

function [path, cost] = Astar_obstacle_avoidance(start, goal, obstacles)
% 输入参数：
% start - 起点坐标
% goal - 终点坐标
% obstacles - 障碍物二维数组，1表示障碍物，0表示可通过
% 输出参数：
% path - 路径数组，每个元素为一个坐标点
% cost - 路径代价

% 初始化起点和终点节点
start_node = struct('pos', start, 'g', 0, 'h', 0, 'parent', []);
goal_node = struct('pos', goal, 'g', Inf, 'h', 0, 'parent', []);

% 初始化open和closed列表
open_list = [start_node];
closed_list = [];

while ~isempty(open_list)
    % 从open列表中找到f值最小的节点
    [min_f, min_idx] = min([open_list.g] + [open_list.h]);
    current_node = open_list(min_idx);
    
    % 如果当前节点是终点，则返回路径
    if isequal(current_node.pos, goal_node.pos)
        path = backtrack_path(current_node);
        cost = current_node.g;
        return;
    end
    
    % 将当前节点从open列表中移除，并添加到closed列表中
    open_list(min_idx) = [];
    closed_list(end+1) = current_node;
    
    % 扩展当前节点的邻居节点
    neighbors = get_neighbors(current_node, obstacles);
    for i = 1:length(neighbors)
        neighbor = neighbors(i);
        % 如果邻居节点已经在closed列表中，则跳过
        if ismember(neighbor, closed_list)
            continue;
        end
        % 计算邻居节点的g值和h值
        new_g = current_node.g + 1;
        new_h = calculate_h(neighbor.pos, goal_node.pos, obstacles);
        % 如果邻居节点已经在open列表中，则更新g值和parent
        open_idx = find(ismember(open_list, neighbor));
        if ~isempty(open_idx)
            if new_g < open_list(open_idx).g
                open_list(open_idx).g = new_g;
                open_list(open_idx).parent = current_node;
            end
        % 否则添加邻居节点到open列表中
        else
            neighbor.g = new_g;
            neighbor.h = new_h;
            neighbor.parent = current_node;
            open_list(end+1) = neighbor;
        end
    end
end

% 如果open列表已经空了，但是仍然没有找到路径，则返回空
path = [];
cost = Inf;

end

% 获取当前节点的邻居节点，同时考虑避开障碍物
function neighbors = get_neighbors(node, obstacles)
    x = node.pos(1);
    y = node.pos(2);
    neighbors = [];
    for i = -1:1
        for j = -1:1
            if i == 0 && j == 0
                continue;
            end
            if x+i < 1 || x+i > size(obstacles, 1) || y+j < 1 || y+j > size(obstacles, 2)
                continue;
            end
            if obstacles(x+i, y+j)
                continue;
            end
            % 判断是否需要绕过障碍物
            if i ~= 0 && j ~= 0 && (obstacles(x+i, y) || obstacles(x, y+j))
                continue;
            end
            neighbors(end+1) = struct('pos', [x+i y+j], 'g', Inf, 'h', 0, 'parent', []);
        end
    end
end

% 计算导航函数h值，同时考虑避开障碍物
function h = calculate_h(pos, goal, obstacles)
    % 计算曼哈顿距离
    h = abs(pos(1)-goal(1)) + abs(pos(2)-goal(2));
    % 如果当前节点周围存在障碍物，则增加代价
    for i = -1:1
        for j = -1:1
            if i == 0 && j == 0
                continue;
            end
            if pos(1)+i < 1 || pos(1)+i > size(obstacles, 1) || pos(2)+j < 1 || pos(2)+j > size(obstacles, 2)
                continue;
            end
            if obstacles(pos(1)+i, pos(2)+j)
                h = h + 10;
            end
        end
    end
end

% 回溯路径
function path = backtrack_path(node)
    path = [node.pos];
    while ~isempty(node.parent)
        node = node.parent;
        path = [node.pos; path];
    end
end

上述代码在A*算法的基础上实现了避开障碍物形成轨迹的功能。在计算导航函数h值时，考虑了当前节点周围存在障碍物的情况，并给路径增加了代价。同时，在获取邻居节点时，也考虑了绕过障碍物的情况。该代码可以在二维障碍物地图上进行路径规划，并返回一条从起点到终点的最短路径。