Поиск пути для мобильного робота, идущего в бесконечный цикл

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, как ячейка занята. Путем вычисления вручную моя стоимость равна 11 в ячейке целевой позиции (4,5). Но когда я запускаю эту программу, она показывает, что стоимость в позиции 5 (4,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* поиске или аналогичном, который использует какую-то эвристику.