导航函数matlab代码

以下是使用曼哈顿距离作为导航函数的Matlab代码示例：

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

% 计算曼哈顿距离
h = @(x,y) abs(x-goal(1)) + abs(y-goal(2));

% 初始化起点和终点节点
start_node = struct('pos', start, 'g', 0, 'h', h(start(1), start(2)), '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值
        new_g = current_node.g + 1;
        % 如果邻居节点已经在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 = h(neighbor.pos(1), neighbor.pos(2));
            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 = [];
    if x > 1 && ~obstacles(x-1, y)
        neighbors(end+1) = struct('pos', [x-1 y], 'g', Inf, 'h', 0, 'parent', []);
    end
    if x < size(obstacles, 1) && ~obstacles(x+1, y)
        neighbors(end+1) = struct('pos', [x+1 y], 'g', Inf, 'h', 0, 'parent', []);
    end
    if y > 1 && ~obstacles(x, y-1)
        neighbors(end+1) = struct('pos', [x y-1], 'g', Inf, 'h', 0, 'parent', []);
    end
    if y < size(obstacles, 2) && ~obstacles(x, y+1)
        neighbors(end+1) = struct('pos', [x y+1], 'g', Inf, 'h', 0, 'parent', []);
    end
end

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

上述代码实现了一个基于A*算法的路径规划器，其中使用曼哈顿距离作为导航函数。该代码可以在二维障碍物地图上进行路径规划，返回一条从起点到终点的最短路径。