Question

我正在为C ++中的主要机器人探索行为实施A *路径规划算法。当机器人移动时，它将环境周围的环境映射为2D图形。从这张图中，我设置了一个Vector2D元组{x, y}，它保存了这个航点的位置，我希望机器人也能在这里导航。

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

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

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

// Open List是要检查的当前节点 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);
        }
    }
}

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

bool exists = false;

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对象返回列表。这是代码的其余部分

    // 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;

        }

    }

现在这是我的问题。在每次尝试时，它总是找到想要的位置，但绝不是最短的路径。见下图。

图一。黄色标记是想要的位置，红色飞镖是＆＃34;路径＆＃34;到我想要的位置，最后，＆＃34;蓝＆＃34;标记是A星开始的地方。

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

Answer 1

To recap the comments, there are two important problems

Manhattan distance is not admissible for your movement costs, since the actual shortest path can take a diagonal shortcut that Manhattan distance wouldn't take into account.
Before adding a new node to the Open list, it not only necessary to check whether it is in the Closed list, but also whether it is already in the Open list. If it is already in the Open list, the G's have to be compared and the smallest must be chosen (together with the corresponding parent pointer).[1]

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)

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.

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]: 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 *（星级）

1 个答案: