A星算法函数

%% A星算法函数
% 输入参数：start起始位置 goal目标位置 mapSize地图尺寸 map地图二维数组 show_flag: 显示过程标志位,0:直接显示  1:显示搜索过程
%          take是否取货物  save是否存货物 0代表不是 1代表是
% 输出参数：road最短路径列表  turnTime转弯次数 travelTime行驶时间
function [road, turnTimes, travelTime] = Astar(AGV, goal, mapSize, map, show_flag, take, save, travelTime)
    global UnitTime TurningTime
    global PickUpTime
    global DrapTime
    UnitTime = 1;  TurningTime = 1;
    PickUpTime = 1;  DrapTime = 1;
    % 取出坐标值
    startx = AGV(1);
    starty = AGV(2);
    endx = goal(1);
    endy = goal(2);

    %-----已探索边界列表-----%
    f = 0 + abs(startx-endx)+abs(starty-endy);  %开始的当前代价为0
    already_frontier = {f,startx,starty,0,[startx,starty]};

    %-----待探索边界列表-----%
    % (第一行是总代价，第二行是横坐标，第三行是纵坐标，第四行是当前代价，第五行是路径)
    frontier = cell(0,5);
    % 寻找邻居点
    % N的第一列和第二列是坐标 第三列是对应的转弯代价
    N = find_frontier(startx, starty, mapSize.up, mapSize.up+mapSize.left, mapSize.down, mapSize.down+mapSize.right, map, already_frontier);
    % 循环当前边界邻居
    for i = 1:size(N,1)
        % 1.传统代价函数 欧式距离 不会出现局部绕行，但可能会出现转弯较多的情况
        % g = sqrt((N(1, i) - startx)^2 + (N(2, i) - starty)^2); 
        % h = sqrt((N(1, i) - endx)^2 + (N(2, i) - endy)^2); 
        % f = g + h ; 

        % 2.曼哈顿距离 转弯较少，但可能会出现局部绕行
        g = norm([startx,starty]-[N(i,1),N(i,2)]);   % 计算当前代价
        h = abs(N(i,1)-endx)+abs(N(i,2)-endy);
        f = g + h ; 
        frontier{i,1} = f;
        frontier{i,2} = N(i,1);
        frontier{i,3} = N(i,2);
        frontier{i,4} = g;
        frontier{i,5} = [[startx,starty];[N(i,1),N(i,2)]];     % 将当前节点设置为父亲
    end
    frontier = sortrows(frontier,1);

    %-----待插入待边界列表的列表-----%
    temp_frontier = cell(0,5);
    
    % 更新待探索列表
    while isempty(frontier) == 0
        % 1.将当前节点移入already_frontier
        current = frontier(1,:);
        already_frontier(end+1,:) = frontier(1,:);    %当前节点加入already_frontier
        frontier(1,:) = [];     %删除当前节点（frontier的第一行）

        % 当前节点坐标
        mx = current{1,2};
        my = current{1,3};

        if show_flag == 2
            plot(current{1,2},current{1,3},'g*');   %当前节点标绿色*号
        end

        % 2.查找当前节点边界(忽略already_frontier中的)
        N = find_frontier(mx,my,mapSize.up,mapSize.up+mapSize.left,mapSize.down,mapSize.down+mapSize.right,map,already_frontier);
        % 循环当前节点边界
        for i = 1:size(N,1)
            % 3.判断边界节点是否在frontier中
            b = find([frontier{:,2}]==N(i,1));
            a = b([frontier{b,3}]==N(i,2));

            % 曼哈顿距离 转弯较少，但可能会出现局部绕行
            g = current{1,4} + norm([mx, my]-[N(i,1),N(i,2)]) + N(i, 3);   % 计算当前代价+转弯代价
            h = abs(N(i,1)-endx)+abs(N(i,2)-endy);
            f = g + h; % 加上转弯代价

            % 3.1 如果边界节点已经在frontier中，检查其当前代价是否更小
            if a > 0
                if frontier{a,4} > g   %如果经过边界的当前代价更小，则重新计算g、f，并更换路径
                    frontier{a,5} = [current{1,5};[N(i,1),N(i,2)]];
                    frontier{a,4} = g;  %重新计算当前代价
                    frontier{a,1} = f;  %重新计算总代价
                end
            % 3.2 如果边界节点没有加入frontiner则加入，成为待探测节点
            else
                temp{1,1} = f;
                temp{1,2} = N(i,1);
                temp{1,3} = N(i,2);
                temp{1,4} = g;
                temp{1,5} = [current{1,5};[N(i,1),N(i,2)]];     %将当前节点设置为父亲
                %frontier = [temp;frontier];
                temp_frontier = [temp;temp_frontier];   %合法边界插入到frontier的第二列(因为第一列之后要移动到already_frontier)
            end
        end

        % 4.如果新加入already_frontier的节点是终点，则跳出循环
        if already_frontier{end,2}==endx && already_frontier{end,3}==endy
            % plot(already_frontier{end,5}(:,1),already_frontier{end,5}(:,2),'r-','LineWidth',2);   %画出最终路径
            % drawnow;
            break; 
        end

        % 5.插入排序
        frontier = [frontier; temp_frontier];
        frontier = sortrows(frontier,1);
        temp_frontier = cell(0,5);    %清空临时列表
        

        if show_flag == 1
            plot(already_frontier{end,5}(:,1),already_frontier{end,5}(:,2),'r-','LineWidth',2);
            drawnow; % 更新图窗并处理回调
            pause(0.05)
        end
    end

    road = already_frontier{end,5};    %最短路径列表

    % 为了计算后续的时间窗算法，需要得出最短路径的实际转弯次数
    turnTimes = 0;
    road(:,3) = zeros(size(road,1) ,1);
    for i = 2:size(road,1)-1
        if isTurn(road(i-1,:), road(i,:), road(i+1,:))
            turnTimes = turnTimes + 1;
            road(i,3) = 1; % 转弯标记
        end
    end

    % 求出路径对应的时间窗信息
    % 定义：小车移动一个格子的时间为1s，转向花费的时间为1s
    %       小车到达取货点时取货时间1s 卸货时间1s
    road(:,4) = zeros(size(road,1) ,1); % 总时间
    road(:,5) = zeros(size(road,1) ,1); % 是否负载

    % 将刚卸货的点的负载优先级设置为3 表示正在卸货 优先级最高
    if (road(1,1) == 4 && road(1,2) == 16) || (road(1,1) == 27 && road(1,2) == 16)...
            || (road(1,1) == 16 && road(1,2) == 4) || (road(1,1) == 16 && road(1,2) == 27)
        road(1,5) = 3;
    end

    tmp = road(:,3); % 是否发生了转向
    % 1.如果是去取货物 即路程是空载运行
    if take == 1
        for i = 1:size(road, 1)
            road(i, 4) = travelTime;
            if i == size(road, 1)
                break
            end
            if tmp(i) == 0 % 如果没有转向 则表明正常行驶
                travelTime = travelTime + UnitTime; 
            else           % 如果有转向 则正常行驶时间+转弯时间
                travelTime = travelTime + TurningTime;
            end
        end
    end

    % 1.如果是去卸货物 即路程是带载运行
    if save == 1
        for i = 1:size(road, 1)
            road(i, 4) = travelTime;
            % 四个入库点    入库A [4,16] [27,16] 入库B [16,4] [16,27] 
            if isequal(goal, [4,16]) || isequal(goal, [27,16]) % 代表是去优先级高的卸货点
                road(i, 5) = 2;
            else
                road(i, 5) = 1; % 代表是去优先级低的卸货点
            end
            if i == size(road, 1)
                break
            end
            if tmp(i) == 0
                travelTime = travelTime + UnitTime;
            else
                travelTime = travelTime + TurningTime;
            end
        end
    end

end