使用matlab应用动态规划算法走迷宫

使用matlab应用动态规划算法走迷宫，并回溯出最短路径。
生成迷宫的方法见：使用matlab生成迷宫.

clc
clear
close
% 读取迷宫数据
load('puzzle.mat');
% 设置起点
start_p = [2,1];
queue = start_p;
range = 0;
result = nan(size(puzz));
result(puzz == 0) = -1;
direction = nan(size(puzz));
% 遍历迷宫的所有点
while size(queue ,1) > 0
    new_queue = [];
    for i = 1: size(queue ,1)
       p_now = queue(i,:);
       result(p_now(1), p_now(2)) = range;
       % 判断上下左右的有效点是否没有经过
       % 上1下2左3右4
       try
           if isnan(result(p_now(1)-1, p_now(2))) && ~max(ismember(queue(:,1:2), [p_now(1)-1, p_now(2)], 'rows'))
               new_queue = [new_queue; p_now(1)-1, p_now(2), 1];
               direction(p_now(1)-1, p_now(2)) = 1;
           end
       end
       try
           if isnan(result(p_now(1)+1, p_now(2))) && ~max(ismember(queue(:,1:2), [p_now(1)+1, p_now(2)], 'rows'))
               new_queue = [new_queue; p_now(1)+1, p_now(2), 2];
               direction(p_now(1)+1, p_now(2)) = 2;
           end
       end
       try
           if isnan(result(p_now(1), p_now(2)-1)) && ~max(ismember(queue(:,1:2), [p_now(1), p_now(2)-1], 'rows'))
               new_queue = [new_queue; p_now(1), p_now(2)-1, 3];
               direction(p_now(1), p_now(2)-1) = 3;
           end
       end
       try
           if isnan(result(p_now(1), p_now(2)+1)) && ~max(ismember(queue(:,1:2), [p_now(1), p_now(2)+1], 'rows'))
               new_queue = [new_queue; p_now(1), p_now(2)+1, 4];
               direction(p_now(1), p_now(2)+1) = 4;
           end
       end
    end
    queue = new_queue;
    range = range + 1;
    show = puzz;
    show(~isnan(result) & result>=0) = 0.5;
    imshow(imresize(show, 4, 'nearest'));
    drawnow;
end

route = zeros(size(puzz));
% 设置终点
end_p = [size(puzz, 1) - 1, size(puzz, 2)];
p_now = end_p;
% 通过动态规划中记录的方向信息回溯出最短路径
while p_now(1)~= start_p(1) || p_now(2)~= start_p(2)
    route(p_now(1), p_now(2)) = 1;
    switch direction(p_now(1), p_now(2))
        case 1
            p_now(1) = p_now(1) + 1;
        case 2
            p_now(1) = p_now(1) - 1;
        case 3
            p_now(2) = p_now(2) + 1;
        case 4
            p_now(2) = p_now(2) - 1;
    end
end
route(start_p(1), start_p(2)) = 1;
% 输出最短路径路线
figure;
show = puzz;
show(route>0) = 0.5;
imshow(imresize(show, 4, 'nearest'));

输入的迷宫：

遍历结果：

最短路径：