移动机器人进入无限循环的路径查找

Question

import java.util.*;
public class path1 {
public static void main(String[] args) {
    //Maze design 1 indicate a cell with obstacles
    int[][] grid = new int[][]{{0, 0, 1, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 1, 1, 1, 0}, {0, 0, 0, 0, 1, 0}};
    //Initial position of the Robot
    int[] init = new int[]{0, 0};
    //Goal position of the Robot
    int[] goal = new int[]{grid.length - 1, grid[0].length - 1};
    //The cost function which is initially defined as 1
    int cost = 1;
    //Movement of a robot(-1,0)->up,(0,-1)->left,(1,0)->down,(0,1)->right
    int[][] delta = new int[][]{{-1, 0}, {0, -1}, {1, 0}, {0, 1}};
    path1 a =new path1();
    a.search(init,goal,grid,delta,cost);
}
    public static int[] search(int[]init,int [] goal,int[][] grid,int[][] delta,int cost) {

       int[][] closed = new int[5][6];
        closed[init[0]] [init[1]]=1;

        int x=init[0];
        int  y = init[1];
        int g=0;
        ArrayList <Integer>open= new ArrayList<Integer>();
        open.add(g);
        open.add(x);
        open.add(y);

        boolean found=false;
        boolean resign=false;
        while (!found && !resign) {
           if (open.size() == 0) {
                resign = true;
                System.out.println("Fail");
            } else {

               Collections.sort(open);
               Collections.reverse(open);
               Collection.pop(open);//I think error in this line as java arraylist cannot allow pop function but I have to pop the last element from the list and remove it from list. So how can I do that?
               x = open.get(1);
               y = open.get(2);
               g = open.get(0);

           }

            if(x==goal[0]&& y==goal[1]){
                found=true;
                System.out.println(x);
                System.out.println(y);
                System.out.println(g);
                System.out.println("Search Successful");
            }
            else{

                for (int i=0;i<delta.length;i++){
                    int x2=x+delta[i][0];
                    int y2=y+delta[i][1];
                    if(x2>=0 && x2<grid.length && y2>=0 && y2<grid[0].length){
                         if(closed[x2][y2]==0 && grid[x2][y2]==0){
                           int g2= g+cost;
                             open.add(g2);
                             open.add(x2);
                             open.add(y2);
                             System.out.println("Now g2 is"+g2);
                             System.out.println("Now x2 is"+x2);
                             System.out.println("Now y2 is"+y2);
                            closed[x2][y2]=1;

                        }

                    }
                }
            }
        }
        return goal;
}

}

这是一种机器人运动的寻路算法，其中机器人声明位置为 (0,0)，位置为 (4,5)。
每个单元格的成本是 1。我在这里使用网格矩阵创建一个网格。其中 1 是障碍单元格。它有7个障碍物，即（0,1），（1,2），（2,4），（3,2）（3,3），（3,4）和（4,4）位置。我有一个封闭矩阵，其列和行与网格矩阵相同。当机器人移动并占据一个单元格时，它将标记为 1 作为单元格被占用。
通过手动计算，我的目标位置 (4,5) 单元格中的成本为 11
但是每当我运行这个程序时，它显示成本在位置 (4,5) 是 5，这是不可能的。我调试了这段代码，并在 3 次迭代后看到它考虑了被阻塞的单元格并通过了这个。
我认为 open.sort();和 open.reverse();制造问题。

Answer 1

一种简单的方法来找到目标路径的成本可以如下完成；

public static int searching(int[]init, int[] goal, int[][] grid, int[][] delta, int cost) {
    int r = init[0];
    int c = init[1];

    int[][] closed = new int[5][6];
    closed[r][c] = 1;

    Stack<int[]> st = new Stack<>();
    st.add(new int[] {r, c});

    int costsum = 0;

    int[] temp = new int[] {r, c};
    while(r!=goal[0] || c!=goal[1]) {
        temp = get_next_rc(r, c, grid, delta, closed);
        if(temp != null) {
            r = temp[0];
            c = temp[1];
            costsum = costsum + cost;
            closed[r][c] = 1;
            st.add(new int[] {r, c});
        }
        else {
            // hit a blockade, backtrack steps
            boolean foundnextopen = false;
            while(foundnextopen != true && st.size() > 0) {
                temp = st.pop();
                r = temp[0];
                c = temp[1];
                costsum = costsum - cost;
                temp = get_next_rc(r, c, grid, delta, closed);
                if(temp != null) {
                    st.add(new int[] {r, c}); // add the stack entry back, as it is in path...
                    costsum = costsum + cost; // also add back the cost, as it is in path
                    foundnextopen = true;
                    r = temp[0];
                    c = temp[1];
                    costsum = costsum + cost;
                    closed[r][c] = 1;
                    st.add(new int[] {r, c});
                }
            }
            if(st.size() == 0) {
                // backtracked to initial, no path possible
                return -1;
            }
        }           
    }
    // print nodes
    System.out.println("nodes visited in reverse order:");
    while(st.size() > 0) {
        int[] rc = st.pop();
        System.out.println("visit " + String.valueOf(rc[0]) + ", "+ String.valueOf(rc[1]));
    }
    return costsum;
}

public static int[] get_next_rc(int r, int c, int[][] grid, int[][] delta, int[][] closed) {
    int nextr = r;
    int nextc = c;

    for(int i=0; i<delta.length; i++) {
        nextr = r + delta[i][0];
        nextc = c + delta[i][1];
        if(nextr >=0 && nextc >=0 && nextr < grid.length && nextc < grid[0].length) {
            // not out of bounds
            if(grid[nextr][nextc] != 1) {
                // not a obstacle
                if(closed[nextr][nextc] != 1) {
                    // not visited already
                    return new int[] {nextr, nextc};
                }
            }
        }
    }
    return null;
}

要运行调用在主函数最后一行搜索的函数为：

System.out.println(searching(init,goal,grid,delta,cost));

这会将成本输出为 17 及其路径。这既不是最佳的，也不是非常彻底的测试。为了找到最佳路径，您需要研究 A* 搜索或类似的方法，它使用某种启发式方法。