2024mathorcup妈妈杯数学建模B题思路模型

 %% 预处理

    
 % 初始化closeList
    
 closeList = start_node;
    
 closeList_path = {start_node,start_node};
    
 closeList_cost = 0;
    
 child_nodes = child_nodes_cal(start_node,  m, n, obs, closeList); %子节点搜索函数 
    
  
    
 % 初始化openList
    
 openList = child_nodes;
    
 for i = 1:size(openList,1)
    
     openList_path{i,1} = openList(i,:);
    
     openList_path{i,2} = [start_node;openList(i,:)];%从初始点到第i个子节点
    
 end
    
  
    
 for i = 1:size(openList, 1)
    
     g = norm(start_node - openList(i,1:2));%norm求范数，返回最大奇异值；abs求绝对值
    
     h = abs(target_node(1) - openList(i,1)) + abs(target_node(2) - openList(i,2));
    
     %终点横坐标距离加纵坐标距离
    
     f = g + h;
    
     openList_cost(i,:) = [g, h, f];
    
 end
    
  
    
 %% 开始搜索
    
 % 从openList开始搜索移动代价最小的节点
    
 [~, min_idx] = min(openList_cost(:,3));%输出openlist_cost表中最小值的位置
    
 parent_node = openList(min_idx,:);%父节点为代价最小节点
    
  
    
  
    
 %% 进入循环
    
 flag = 1;
    
 while flag   
    
     
    
     % 找出父节点的忽略closeList的子节点
    
     child_nodes = child_nodes_cal(parent_node,  m, n, obs, closeList); 
    
     
    
     % 判断这些子节点是否在openList中，若在，则比较更新；没在则追加到openList中
    
     for i = 1:size(child_nodes,1)
    
     child_node = child_nodes(i,:);
    
     [in_flag,openList_idx] = ismember(child_node, openList, 'rows');%ismember函数表示子节点在open表中则返回1，判断flag,输出此子节点在openlist表中的位置
    
     g = openList_cost(min_idx, 1) + norm(parent_node - child_node);%按照新父节点计算此子节点的g,h值
    
     h = abs(child_node(1) - target_node(1)) + abs(child_node(2) - target_node(2));
    
     f = g+h;
    
     
    
     if in_flag   % 若在，比较更新g和f        
    
         if g < openList_cost(openList_idx,1)
    
             openList_cost(openList_idx, 1) = g;%将openlist_cost表中第id个位置的第一个数更新为以新父节点计算的g值
    
             openList_cost(openList_idx, 3) = f;
    
             openList_path{openList_idx,2} = [openList_path{min_idx,2}; child_node];
    
         end
    
     else         % 若不在，追加到openList
    
         openList(end+1,:) = child_node;
    
         openList_cost(end+1, :) = [g, h, f];
    
         openList_path{end+1, 1} = child_node;
    
         openList_path{end, 2} = [openList_path{min_idx,2}; child_node];
    
     end
    
     end
    
    
    
     
    
     % 从openList移除移动代价最小的节点到 closeList
    
     closeList(end+1,: ) =  openList(min_idx,:);
    
     closeList_cost(end+1,1) =   openList_cost(min_idx,3);
    
     closeList_path(end+1,:) = openList_path(min_idx,:);
    
     openList(min_idx,:) = [];%openlist表中已跳出的最小值位置设为空
    
     openList_cost(min_idx,:) = [];
    
     openList_path(min_idx,:) = [];
    
  
    
     % 重新搜索：从openList搜索移动代价最小的节点（重复步骤）
    
     [~, min_idx] = min(openList_cost(:,3));
    
     parent_node = openList(min_idx,:);
    
     
    
     % 判断是否搜索到终点
    
     if parent_node == target_node
    
     closeList(end+1,: ) =  openList(min_idx,:);
    
     closeList_cost(end+1,1) =   openList_cost(min_idx,1);
    
     closeList_path(end+1,:) = openList_path(min_idx,:);
    
     flag = 0;
    
     end
    
 end