棋谱篇

路线规划

November 18, 2021

Grid-Based Route (Re-)Planning

路线规划用于自主代理的导航，例如自动驾驶车辆、机器人和人类（考虑地图服务）基于网格的路由规划研究在二维空间中，将一个初始单元到一个目标单元的路由划分为阻塞或空的网格单元的问题.图1a显示了一个由10行10列组成的示例网格，第0行和第0列的初始单元格由I表示（是的，我们从零开始计算），第9行和第9列的目标单元格由G表示。绿色箭头显示了从I到G的计划移动，阻塞路段显示为橙色正方形的块。

路线规划图1

随着环境的变化，即块被移除或放置在一些新的单元上，计划的路由可能变得无效如图所示。例如一个机器人，按照计划的路线从图1a到达图1b网格中的单元格I和单元格g，它将被阻塞，故需要并重新规划绕过障碍物，如图1c所示。

Input Data

输入数据从文件中获取，如图2所示，第1行为表格大小，第2行为起始坐标,第3行为目的坐标。第三行开始为路线障碍节点坐标直到读取有“$”。之后行指示的是路线。

Reading and Analyzing Input Data

解决问题的第一步，读取文件数据并分析。将各节点关键信息如下方式显示:

[localhost]>ass2-soln<test0.txt ==STAGE 0======================================= The grid has 10 rows and 10 columns. The grid has 9 block(s). The initial cell in the grid is [0,0]. The goal cell in the grid is [9,9]. The proposed route in the grid is: [0,0]->[0,1]->[0,2]->[0,3]->[0,4]-> [1,4]->[2,4]->[3,4]->[4,4]->[5,4]-> [6,4]->[7,4]->[8,4]->[9,4]->[9,5]-> [9,6]->[9,7]->[9,8]->[9,9]. The route is valid!

Drawing and Replanning

进入第二阶段，当路线中遇到障碍时需要重新规划路线。首先打印路线如图3所示: 重新规划的路线的整体思路是：如何图1f所示，将遇到障碍点为原点扩散，离原点的距离。我们通过上下左右的方式遍历可访问的每个节点放入唯一队列当中，并判别放入的节点是否等于阻塞点的下个节点。如果相等则遍历结束，并获取了重新规划的路线。将队列的课访问路线添加到之前的路线当中。

coding

下列是我遍历路线的代码：

static int travers_queue(LinkList *queue, char **grid, int srow, int sclum,int drow,int dclum,
        int gridRow, int gridClum) {
    int ret;
    int gRow,gClum;

    int tra = 0,row,clum,count = 0;
    gRow = srow + 1;
    gClum = sclum + 1;
    row = srow;
    clum = sclum;

    //原点
    if (row >=0 && clum >= 0){
        //printf("begin:[%d,%d]%c \n",row,clum,grid[gRow][gClum]);
        RouteNode *initPair = malloc(sizeof(RouteNode));
        initPair->clum = clum;
        initPair->row = row;
        initPair->value = count; 
        LinkList_Insert(queue, (LinkListNodeData*) initPair,
                            initPair->value);
    }
    RouteNode *node =(RouteNode *) LinkList_Get(queue,0);
    while (node != NULL) {
        row = node->row;
        clum = node->clum;

        gRow = row + 1;
        gClum = clum + 1;
        count = node->value;
        //上
        if (row - 1 >= 0 && grid[gRow - 1][gClum] != '#') {
            //printf("above[%d,%d]%c \n",row-1,clum,grid[gRow - 1][gClum]);
            RouteNode *above = malloc(sizeof(RouteNode));
            above->clum = clum;
            above->row = row - 1;
            above->value = count + 1;
            append_unique(queue,above); 
            if (drow == row -1 && dclum == clum)
                return 0;
        }
        //下
        if (row + 1 < gridRow && grid[gRow + 1][gClum] != '#') {
            //printf("below:[%d,%d] %c \n",row+1,clum,grid[gRow+ 1][gClum]);
            RouteNode *below = malloc(sizeof(RouteNode));
            below->clum = clum;
            below->row = row + 1;
            below->value = count + 1;
            append_unique(queue,below); 
            if (drow == row + 1 && dclum == clum)
                return 0;

        }
        //左
        if (clum - 1  >= 0 && grid[gRow][gClum - 1] != '#') {
            //printf("left:[%d,%d]%c \n",row,clum- 1,grid[gRow][gClum - 1]);
            RouteNode *left= malloc(sizeof(RouteNode));
            left->clum = clum -1;
            left->row = row ;
            left->value = count + 1;
            append_unique(queue,left); 
            if (drow == row && dclum == clum - 1)
                return 0;

        }
        //右
        if (clum + 1  < gridClum && grid[gRow][gClum + 1] != '#') {
            //printf("right:[%d,%d]%c \n",row,clum+ 1,grid[gRow][gClum + 1]);
            RouteNode *right = malloc(sizeof(RouteNode));
            right->clum = clum + 1;
            right->row = row;
            right->value = count + 1;
            append_unique(queue,right); 
            if (drow == row && dclum == clum + 1)
                return 0;

        }
        node = LinkList_Get(queue, ++tra);

    }
    return 1;
}

如需全部代码，扫码关注，回复"规划"即可获得链接。

◀ 返回