机器人路径规划- A* (星号)

Question

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

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

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

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

// Open List是检查std：：向量位置的当前节点；

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存在=假；

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;

        }

    }

这是我的问题。每次尝试，它总是找到想要的位置，但从来没有最短的路径。参见下面的图1。

​

​

图一。黄色的标记是想要的位置，红色的飞镖是通往我想要的位置的“路径”，最后，“蓝色”标记是A星开始的地方。

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

Answer 1

概括这些评论，有两个重要的问题

• 曼哈顿距离是不允许你的移动成本，因为实际最短的路径可以采取对角线的捷径，曼哈顿的距离不会考虑。
• 在将新节点添加到Open之前，不仅需要检查它是否在关闭列表中，而且还需要检查它是否已经在Open中。如果它已经在开放列表中，则必须对G进行比较，并且必须选择最小的(与相应的父指针一起).1

由于您有10/14成本的八进制移动，您的启发式函数可以是(来自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。当然，您也可以使用任何其他允许的方法，但是这个公式已经反映了开阔平原上的实际距离，因此很难改进。

在Open中查找和更新节点是A*的一个恼人的部分，我相信许多人都会假装这是不必要的，因为这意味着您不能合理地使用任何预定义的优先级队列(他们缺乏有效的查找)。可以通过手动实现二进制堆和哈希表将坐标映射到堆中的相应索引来完成。每当移动节点时，哈希表必须由堆更新。

1:来自维基百科文章的伪代码的相关片段是：

    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)