A * Алгоритм поиска не дает оптимального пути

Question

Я пытаюсь создать сценарий MATLAB для алгоритма поиска A *. Я создал сценарий. Я попробовал это с 6-узловым графом и вычислил правильный путь. Однако на этот раз я попробовал сценарий для другого графа (с 12 узлами) и на этот раз получил неправильный ответ.

Это файл node.csv с информацией об узлах:

Node number, (x, y) coords, heuristic cost

1,   -0.5,-0.5,         1.4142    
2,   -0.09,-0.4,        1.0762    
3,   -0.285,-0.305,     1.1244    
4,    0.0575,-0.225,    0.8494    
5,   -0.0525,-0.0175,   0.7604    
6,   -0.37,0.3,         0.8927    
7,    0.3525,-0.0525,   0.5719
8,    0.0625,0.255,     0.5014
9,   -0.1,0.3725,       0.6134
10,   0.4275,0.195,     0.3135
11,   0.345,0.3525,     0.214
12,   0.5,0.5,            0

Это информация о ребрах из файла edge.csv (ребра неориентированы):

Node 1, Node 2, Cost

12,     11,   0.214
12,     10,   0.3135
12,     8,    0.5014
11,     10,   0.1778
11,     9,    0.4454
10,     7,    0.2586
9,      8,    0.2005
9,      6,    0.2796
9,      5,    0.5994
8,      4,    0.48
7,      4,    0.3417
5,      2,    0.179
4,      3,    0.3517
4,      2,    0.2289
3,      2,    0.2169
3,      1,    0.2903
2,      1,    0.422
7,      5,    0.4402
5,      4,    0.11

Решение графа: 1 - 3 - 4 - 7 - 10 - 12.

Путь, вычисленный моим сценарием: 1 - 3 - 4 - 2 - 5 - 5 - 7 - 10 - 12.

Обратите внимание, что узел 5 показан дважды, о чем я понятия не имею. Я попытался отладить, но не смог найти, что было неправильной частью.

Вот мой код:

clc
clear
%% Constructing the Nodes with their information:
%  Assigning the adjacent node information to nodes:
nodes = readmatrix('nodes.csv');
edges = readmatrix('edges.csv');

node(1).successors = [2 3];
node(2).successors = [3 4 5];
node(3).successors = [1 2 4];
node(4).successors = [5 2 3 7 8];
node(5).successors = [4 7 2 9];
node(6).successors = 9;
node(7).successors = [5 4 10];
node(8).successors = [4 9 12];
node(9).successors = [5 6 8 11];
node(10).successors = [7 11 12];
node(11).successors = [9 10 12];
node(12).successors = [8 10 11];

%  Assigning heuristic cost values and cost numbers to nodes:
heuristics = nodes(:,4);
for i = 1 : length(nodes)
    node(i).heuristic = heuristics(i);
    node(i).order = i;
end

%  Initializing the pastCost and totalCost of the nodes:
node(1).pastCost = 0;
node(1).totalCost = node(1).pastCost + node(1). heuristic;
for i = 2 : length(nodes)
    node(i).pastCost = Inf;
    node(i).totalCost = node(i).pastCost + node(i). heuristic;
end

nodeStart = nodes(1, 1);
nodeGoal = nodes(12, 1);

%  Node start has no parent node:
node(nodeStart).parent = [];

%  Adding the first node to OPEN List
OPEN = {node(1)};

CLOSED = {};

%% Starting the A* Search Algorithm:
%  If tthe OPEN list is not empty, iterations are going to start:
while(~isempty(OPEN))
  current = OPEN{1};
  OPEN(1) = [];
  
  CLOSED{length(CLOSED) + 1} = current;
 
  %  If the current node is the goal node, algorithm is finished with
  %  success, return the path from the CLOSED List:
  if (current.order == nodeGoal)
      disp('SUCCESS!')
     
      for i = 1 : length(CLOSED)
         
          fprintf(' %d ', CLOSED{i}.order)
         
      end
      return
     
  end
 
  % Creating an array which consists of the node numbers of the closed nodes:
        for i = 1 : length(CLOSED)
        closedNodes(i) = CLOSED{i}.order;
        end
 
  % Starting searching through the adjacents/successors of the current node:      
  for i = 1 : length(current.successors)
      
      %  If the current node is not in the CLOSED List / member of the
      %  closedNodes array, calculations are going to be taking place:
      if (~ismember(current.successors(i), closedNodes))
         
          tentativePastCost = current.pastCost + edgeCost(current.order, current.successors(i), edges);
         
          if (tentativePastCost < node(current.successors(i)).pastCost)
              node(current.successors(i)).pastCost = tentativePastCost;
              node(current.successors(i)).parent = current.order;
             
             
              node(current.successors(i)).totalCost = node(current.successors(i)).pastCost + node(current.successors(i)).heuristic;
              
              %  Adding the current node to the OPEN List and then sorting
              %  the OPEN List with respect to total cost values:
             
              if(length(OPEN) == 0 || node(current.successors(i)).order ~= OPEN{length(OPEN)}.order)
              OPEN{length(OPEN) + 1} = node(current.successors(i));
              end
              [~,I] = sort(cellfun(@(s) getfield(s,"totalCost"), OPEN));
              OPEN = OPEN(I);
             
          end
         
      end
     
  end
 
end
disp('FAILURE!')

По какой причине моя программа перескакивает с узла 4 на 2 вместо узел 7?