【无人机三维路径规划】基于鱼鹰算法OOA实现复杂城市地形下无人机避障三维航迹规划附Matlab代码

% 定义地图
map = zeros(10, 10); % 10x10的地图
map(3:7, 4) = 1; % 障碍物
map(3:7, 7) = 1; % 障碍物

% 定义起点和终点
start = [1, 1];
goal = [10, 10];

% 进行A*路径规划
path = astar_path_planning(map, start, goal);

% 绘制地图和路径
figure;
hold on;
grid on;
axis equal;
colormap([1 1 1; 0 0 0]);
image(1-map’);
plot(start(2), start(1), ‘go’, ‘MarkerSize’, 10, ‘LineWidth’, 2);
plot(goal(2), goal(1), ‘ro’, ‘MarkerSize’, 10, ‘LineWidth’, 2);
plot(path(:, 2), path(:, 1), ‘b-’, ‘LineWidth’, 2);
legend(‘起点’, ‘终点’, ‘路径’);
title(‘路径规划’);

% A*路径规划函数
function path = astar_path_planning(map, start, goal)
% 定义启发式函数（曼哈顿距离）
heuristic = @(x, y) abs(x(1)-y(1)) + abs(x(2)-y(2));

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

% 初始化开放列表和关闭列表
openList = startNode;
closeList = [];

% 进行A*搜索
while ~isempty(openList)
    % 选择f值最小的节点
    [~, idx] = min([openList.f]);
    currentNode = openList(idx);
    
    % 将当前节点从开放列表移除，并加入关闭列表
    openList(idx) = [];
    closeList = [closeList, currentNode];
    
    % 判断是否达到终点
    if isequal(currentNode.pos, goalNode.pos)
        path = backtrack_path(currentNode);
        return;
    end
    
    % 遍历当前节点的邻居
    neighbors = get_neighbors(currentNode.pos, map);
    for i = 1:length(neighbors)
        neighborNode = neighbors(i);