请用中文注释下面A算法路径规划代码的每一行 Q=[source 0 heuristic(source,goal) 0+heuristic(source,goal) -1]; closed=ones(size(map)); % the closed list taken as a hash map. 1=not visited, 0=visited closedList=[]; % the closed list taken as a list pathFound=false; tic; counter=0; size(Q) while size(Q,1)>0 [A, I]=min(Q,[],1); n=Q(I(5),:); Q=[Q(1:I(5)-1,:);Q(I(5)+1:end,:)]; if n(1)==goal(1) && n(2)==goal(2) pathFound=true;break; end [rx,ry,rv]=find(conn==2); % robot position at the connection matrix [mx,my,mv]=find(conn==1); for mxi=1:size(mx,1) %iterate through all moves newPos=[n(1)+mx(mxi)-rx n(2)+my(mxi)-ry]; % possible new node if checkPath(n(1:2),newPos,map) %if path from n to newPos is collission-free if closed(newPos(1),newPos(2))~=0 historicCost=n(3)+historic(n(1:2),newPos); heuristicCost=heuristic(newPos,goal); totalCost=historicCost+heuristicCost; add=true; % not already in queue with better cost if length(find((Q(:,1)==newPos(1)) . (Q(:,2)==newPos(2))))>=1 I=find((Q(:,1)==newPos(1)) . (Q(:,2)==newPos(2))); if Q(I,5)<totalCost, add=false; else Q=[Q(1:I-1,:);Q(I+1:end,:);];add=true; end end if add Q=[Q;newPos historicCost heuristicCost totalCost size(closedList,1)+1]; % add new nodes in queue end end end end closed(n(1),n(2))=0;closedList=[closedList;n]; % update closed lists i0 = counter; i1 = 40; counter=counter+1; if display_process == true && (rem(i0,i1) == 0) temp_img = (map==0).0 + ((closed==0).(map==1)).125 + ((closed==1).(map==1)).*255 + (mapResized - map).*100 ; % plot goal and source temp_img(goal(1), goal(2) ) = 110; temp_img(source(1), source(2) ) = 110; image(temp_img); M(counter)=getframe; end size(Q) end

好的，以下是用中文注释的代码：

Q=[source 0 heuristic(source,goal) 0+heuristic(source,goal) -1];  // 初始化队列Q，source为起点，0为历史代价，heuristic(source,goal)为启发函数值，0+heuristic(source,goal)为总代价，-1为当前节点的父节点
closed=ones(size(map)); // 初始化closed列表为1，表示所有节点均未被访问
closedList=[]; // 初始化closed列表为空
pathFound=false; // 初始化路径未找到
tic; // 记录时间
counter=0; // 计数器初始化为0

// 当队列Q不为空时循环
while size(Q,1)>0
    [A, I]=min(Q,[],1); // 找到队列中总代价最小的节点
    n=Q(I(5),:); // 取出该节点
    Q=[Q(1:I(5)-1,:);Q(I(5)+1:end,:)]; // 从队列中删除该节点
    
    if n(1)==goal(1) && n(2)==goal(2) // 如果该节点为终点
        pathFound=true; // 标记路径已找到
        break; // 结束循环
    end
    
    [rx,ry,rv]=find(conn==2); // 找到机器人在连接矩阵上的位置
    [mx,my,mv]=find(conn==1); // 找到机器人可以移动的方向
    
    // 遍历所有可移动的方向
    for mxi=1:size(mx,1)
        newPos=[n(1)+mx(mxi)-rx n(2)+my(mxi)-ry]; // 计算新的位置
        if checkPath(n(1:2),newPos,map) // 如果新位置可以到达
            if closed(newPos(1),newPos(2))~=0 // 如果该位置还未被访问
                historicCost=n(3)+historic(n(1:2),newPos); // 计算历史代价
                heuristicCost=heuristic(newPos,goal); // 计算启发函数值
                totalCost=historicCost+heuristicCost; // 计算总代价
                add=true; // 假设新节点未在队列中
                
                // 如果新节点已经在队列中，并且总代价更小，则更新队列中的节点信息
                if length(find((Q(:,1)==newPos(1)) . (Q(:,2)==newPos(2))))>=1
                    I=find((Q(:,1)==newPos(1)) . (Q(:,2)==newPos(2)));
                    if Q(I,5)<totalCost
                        add=false;
                    else
                        Q=[Q(1:I-1,:);Q(I+1:end,:);];
                        add=true;
                    end
                end
                
                // 如果新节点未在队列中，则加入队列
                if add
                    Q=[Q;newPos historicCost heuristicCost totalCost size(closedList,1)+1];
                end
            end
        end
    end
    
    closed(n(1),n(2))=0; // 标记该节点已被访问
    closedList=[closedList;n]; // 把该节点加入closed列表
    
    // 每40次迭代绘制一次地图
    i0 = counter;
    i1 = 40;
    counter=counter+1;
    if display_process == true && (rem(i0,i1) == 0)
        temp_img = (map==0).0 + ((closed==0).(map==1)).125 + ((closed==1).(map==1)).*255 + (mapResized - map).*100 ;
        temp_img(goal(1), goal(2) ) = 110;
        temp_img(source(1), source(2) ) = 110;
        image(temp_img);
        M(counter)=getframe;
    end
    
    size(Q) // 打印队列长度
end