自主迷宫解决机器人和故障路径排序

简介

我正在建造一个迷宫跑步机器人，其目标是能够在从给定起点到给定终点的迷宫中导航 . 我自己设计了算法 - 在查看以前的工作之前，我坚持让自己的实现工作 .

问题

算法的一部分涉及对方向进行排序（使用插入排序，因为数组非常小） - 前进，后退，上升和下降就它们的前景而言 .

我在排序方面遇到了一些问题，它根据三个因素对方向进行排序

• Status: 如果方向d的路径已经探索过了？

• Distance: 路径p末端的方形到 endpoints 的距离 - x和y的最小值不同

• Length: 路径实际有多长 .

排序时，我要比较因子1.如果它们相等，我比较因子2.如果因子2相等，我继续3.如果任何两个因子小于或大于，我返回 . For some reason, sometimes a path with a lower status gets pushed to the back. Something is going wrong with my sorting of the paths.

enter code here

有任何想法吗？

代码

/*------------( Getting the best direction to go in )----------------    */
    /*
     * 0: North
     * 1: East
     * 2: South
     * 3: West
     */
    int  bestDirection(Point _curr_pos){
    Vec4 statuses/*1 for unexplored(better), 2 for explored(worse)*/,
        distances ,
        lengths = collectDistancesToWalls(),
        availablities = {POINT_REACHABLE, POINT_REACHABLE, POINT_REACHABLE, POINT_REACHABLE},
        directions = {0,1,2,3};

    //Give directions a length rating and distance rating and then sorts the directions, leaving the best direction in the front.

    //If we discover that we have already been in a location, we should block off the passage behind us leading to that location because it must be a loop.
    //Collecting the distance and length data. 
      for (int i=0; i < 4; i++) {
        Point direction_translated = translateOnAxisInDirection(_curr_pos, i, 1);

     //Converts the point to a reachable square - unnecessary to describe here. There is a domain specific problem which necessitates this function to be used. 
        Point heading_direction = pointToReachableSquare(direction_translated , i);

        //Distance from end of path to "headinglocation", the goal
        Point vec_to_end = subVec(heading_direction, headingLocation);
        distances[i]    =  min(absv(vec_to_end.x), absv(vec_to_end.y));
        statuses[i]     =  history[heading_direction.x][heading_direction.y].status;

        //If path is unreachable because of wall or something, then mark it as unreachable. 
        if (cmpVec(heading_direction, _curr_pos) || history[heading_direction.x][heading_direction.y].classification == WALL || !pointInIndBounds(direction_translated)) {
            availablities[i] = POINT_UNREACHABLE;
        }
    }

    //Insertion sort the distances.
    for (int i = 1; i < 4; i++) {
        int j = i - 1;
        while (
            comparePathOptions(
            statuses[i],
            distances[i],
            lengths[i],
            statuses[j],
            distances[j],
            lengths[j]
               ) == LESS_THAN && (j >= 0)) {
                int temp = directions[i];
                directions[i] = directions[j];
                directions[j] = temp;
                j--;
        }
    }

    //Return the first reachable direction. 
    int ind = 0;

    int dir = directions[ind];

    while (availablities[ directions[ind] ] == POINT_UNREACHABLE && (ind<4)) {
        dir = directions[ind+1];
        ind++;
    }

    return dir;
}

比较功能：

int relationship(int a, int b){
    if (a < b) return LESS_THAN;
    if (a > b) return MORE_THAN;
    return EQUAL;
}

//Edit this function
//TODO: Edit comparePathOptions.
//NOTE: Something
int comparePathOptions(int n_explored, int n_closeness, int n_length,
                 int b_explored, int b_closeness, int b_length){

    int objs[][3] = {
        {n_explored, n_closeness, n_length},
        {b_explored, b_closeness, b_length}
    };

    for (int i = 0; i < 3; i++){
        int rel = relationship(objs[1][i],objs[0][i]);
        if (rel!= EQUAL ) return rel;

    }
    return EQUAL;
}

解决了

感谢@Kittsil我已经让算法运行了！相反访问 statuses ， lengths ，并通过 distances 和 j 的 i ，您可以通过 directions[i or j] 这样做，因为 i 和 j 停止参考电流方向时的方向排列改变 .

The edited code:

while ( (j >= 0) &&
        comparePathOptions(
        statuses[  directions[i] ],
        distances[ directions[i] ],
        lengths[   directions[i] ],
        statuses[  directions[j] ],
        distances[ directions[j] ],
        lengths[   directions[j] ]
           ) == MORE_THAN ) {
            int temp = directions[i];
            directions[i] = directions[j];
            directions[j] = temp;
            j--;
    }

And the solved maze:

x: 0, y: 0
H: 5, W:5, Ss:1
4|#####|
3|#####|
2|#####|
1|#####|
0|*::::|
  01234 
4|#####|
3|#####|
2|#####|
1|#####|
0| *:::|
  01234 
4|#####|
3|#####|
2|#####|
1|#####|
0|  *::|
  01234 
4|#####|
3|#####|
2|#####|
1|#####|
0|   *:|
  01234 
4|#####|
3|#####|
2|####:|
1|####:|
0|    *|
  01234 
4|#####|
3|#####|
2|####:|
1|####*|
0|     |
  01234 
4|#####|
3|#####|
2|::::*|
1|#### |
0|     |
  01234 
4|#####|
3|#####|
2|:::* |
1|#### |
0|     |
  01234 
4|#####|
3|#####|
2|::*  |
1|#### |
0|     |
  01234 
4|#####|
3|#####|
2|:*   |
1|#### |
0|     |
  01234 
4|:####|
3|:####|
2|*    |
1|#### |
0|     |
  01234 
4|:####|
3|*####|
2|     |
1|#### |
0|     |
  01234 
4|*####|
3| ####|
2|     |
1|#### |
0|     |
  01234