机器人路径规划-A *(星级) [英] Robotic Path Planning - A* (Star)

问题描述

我正在针对C ++中的主要机器人探索行为实现A *路径规划算法.随着机器人的移动，它会将周围的环境映射为2D图形.在此图中，我设置了一个Vector2D元组{x, y}，它保存了这个航路点的位置，我也希望机器人也可以在其中导航.

I'm implementing A* path planning algorithm for my main robots exploration behavior in C++. As the robot moves, it maps the environment around itself as a 2D graph. From this graph, I have set a Vector2D Tuple {x, y} which holds the location of this waypoint, where I want the robot to navigate too.

我对A *所做的第一件事是拥有一个Node类，该类保存有关当前节点的信息；

The first thing I do with A* is to have a Node class, which holds information about the current node;

double f; //  F, final score
double g; // Movement cost
double h; // Hueristic cost (Manhatten)
Node* parent;
Vector2d position;

在A *开始时，我将我的开始节点作为我的机器人的开始位置(我也将此位置保持为Vector以便于访问).然后，我进入一个while循环，直到找到最终目标.在此循环中，我要做的第一件事是生成八个相邻节点(左，下，右，上，左上，右上，左下，右下)，然后在OpenList向量中将其返回.

As A* starts, I have my starting node as my Robots starting position (I also hold this position as a Vector for easy access). Then, I enter a while loop until the end goal is found. The first thing I do in this loop is to generate eight adjacent Nodes (Left, Bottom, Right, Top, Top-left, Top-Right, Bottom-Left, Bottom Right), I then return this in a OpenList vector.

//打开列表是当前要检查的节点 std :: vector位置；

// Open List is current nodes to check std::vector positions;

positions.push_back(Vector2d(current->position.getX() - gridSize, current->position.getY())); // Left of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() + gridSize, current->position.getY())); // right of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX(), current->position.getY() + gridSize)); // Top of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX(), current->position.getY() - gridSize)); // Bottom of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() + gridSize,current->position.getY() + gridSize)); // Top Right of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() - gridSize,current->position.getY() + gridSize)); // Top Left of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() + gridSize,current->position.getY() - gridSize)); // Bottom Right of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() - gridSize,current->position.getY() - gridSize)); // Bottom Left of my current grid space (parent node)

// moving diagnolly has a bigger cost
int movementCost[8] = { 10, 10, 10, 10, 14, 14, 14, 14 };

// loop through all my positions and calculate their g, h and finally, f score.
for (int i = 0; i < positions.size(); i++)
{
    Node* node = new Node(positions[i]);

    node->parent = current;
    node->movementCost = movementCost[i];
    if (!presentInClosedList(node))
    {
        // if the probability value of the current node is less then 0.5 (not an obstacle) then add to the open list, else skip it as an obstacle
        // Set astar grid occupancy
        if (grid->getProbabilityValue(node->position) < 0.51)
        {
            node->g = current->g + movementCost[i];
            node->h = (abs(positions[i].getX() - wantedLocation.getX())) + (abs(positions[i].getY() - wantedLocation.getY()));
            node->f = node->g + node->h;

            openList.push_back(node);
        }
    }
}

这是查看当前节点是否存在于我的closeList中的代码

This is the code to see if the current node is present in my closedList

布尔存在=假;

for (int i = 0; i < closedList.size(); i++)
{
    if (closedList[i]->position == currentNode->position)
    {
        closedList[i]->f = currentNode->f;
        closedList[i]->g = currentNode->g;
        closedList[i]->h = currentNode->h;
        closedList[i]->parent = currentNode->parent;

        exists = true;
        break;
    }
}

return exists;

这将返回可能路径的openlist.接下来，我选择得分最低的F，并将其添加到我的closedList.我会继续执行此例程，直到找到最终目标.最后，一旦找到，我就使用parent对象返回列表.这是其余的代码

This returns an openlist of possible routes. Next, I select the one with the smallest F score, and add this to my closedList. I keep doing this routine until the end goal has been found. Finally, once found I go back down the list using the parent objects. Here is the rest of the code

    // If my parents location is the same as my wanted location, then we've found our position.
    if (locationFound(current, wantedLocation))
    {
        // Now we need to backtrack from our wantedNode looking at the parents of each node to reconstruct the AStar path
        Node* p = current->parent;
        rollingDist = p->g;

        while (!wantedFound)
        {
            if (p->position == startLocation)
            {
                wantedFound = true;
                wantedNodeFound = true;
                break;
            }

            path.push_back(p);
            p = p->parent;

        }

    }

现在这是我的问题.在每次尝试时，它始终会找到所需的位置，但永远不会找到最短的路径.请参见下面的图一.

Now this is my issue. On every attempt it always finds the wanted location, but never the shortest path. See figure one below.

图一.黄色标记是想要的位置，红色飞镖是指向我想要的位置的路径"，最后，蓝色标记是A星开始的位置.

这是我的问题.我似乎无法重建这条路.

This is my issue. I can't seem to reconstruct this path.

推荐答案

要重述评论，有两个重要问题

To recap the comments, there are two important problems

• 曼哈顿距离不适合您的移动费用，因为实际的最短路径可能会采用对角线捷径，而曼哈顿距离不会考虑在内.
• 在将新节点添加到打开"列表之前，不仅需要检查它是否在关闭"列表中，还需要检查它是否已经在打开"列表中.如果已经在打开"列表中，则必须对G进行比较，并且必须选择最小的G(以及相应的父指针).[1]

由于您有10/14的费用进行波动运动，您的启发式功能可能是> http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html )

Since you have octile movement with 10/14 costs, your heuristic function could be (from http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html)

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

在D = 10时，D2 =14.当然，您也可以使用其他允许的任何值，但是该公式已经反映了开阔平原上的实际距离，因此无法轻松进行改进.

With D = 10, D2 = 14. Of course you can also use anything else admissible but this formula already reflects the actual distance on an open plain so it can't easily be improved.

在打开"列表中查找和更新节点是A *中令人讨厌的一部分，我敢肯定很多人都不想假装，因为这意味着您不能合理地使用任何预定义的优先级队列(他们缺乏有效的查询).可以通过手动实施二进制堆和将坐标映射到堆中其对应索引的哈希表来完成.每当移动节点时，哈希表都必须由堆更新.

Finding and updating nodes in the Open list is an annoying part of A* that I'm sure many people would like to pretend isn't necessary, since it means you can't reasonably use any pre-defined priority queue (they lack efficient lookup). It can be done by having a manually implemented binary heap and a hash table that maps coordinates to their corresponding indexes in the heap. The hash table has to be updated by the heap whenever a node is moved.

[1]:维基百科文章中的伪代码相关片段为:

[1]: the relevant snippet of pseudo code from the wikipedia article is:

    tentative_gScore := gScore[current] + dist_between(current, neighbor)
    if neighbor not in openSet  // Discover a new node
        openSet.Add(neighbor)
    else if tentative_gScore >= gScore[neighbor]
        continue        // This is not a better path.

    // This path is the best until now. Record it!
    cameFrom[neighbor] := current
    gScore[neighbor] := tentative_gScore
    fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)

这篇关于机器人路径规划-A *(星级)的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！