Robotic path planning - A * (star)
I am implementing an A * path planning algorithm for my main robot research behavior in C ++. As the robot moves, it displays the environment around it as a 2D graph. From this graph, I installed a Vector2D Tuple {x, y}
which contains the location of this waypoint where I want the robot to move too.
The first thing I do with A * is to have a class Node
that contains information about the current node;
double f; // F, final score
double g; // Movement cost
double h; // Hueristic cost (Manhatten)
Node* parent;
Vector2d position;
When A * starts, I have the start node as the start position of my robot (I also take this position as a vector for easy access). Then I go into a while loop until the final target is found. The first thing I do in this loop is create eight neighboring nodes (Left, Bottom, Right, Top, Top-left, Top-Right, Bottom-Left, Bottom Right), then put that back into the vector OpenList
.
// The public list is the current nodes to check std :: vector position;
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);
}
}
}
This is the code to see if the current node is present in my private list
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;
This returns OpenList
possible routes. Then I select the one with the smallest metric F
and add it to mine closedList
. I keep doing this procedure until the end goal is found. Finally, once found, I go back down the list using objects parent
. 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 problem. With every attempt, she always finds the right place, but never the shortest path. See Figure below.
Figure 1. Where the yellow marker is in the right place, and the red arrows are the "Path" to my desired location, and finally the "Blue" marker is the start of the star.
It is my problem. I cannot recover this path.
source to share
To reproduce the comments, there are two important issues:
- The Manhattan distance is unacceptable for your travel costs, as the shortest path can take on a diagonal label that does not account for the Manhattan distance.
- Before adding a new list node to the Open list, you need to not only check if it is in the closed list, but also if it is in the Open list. If it is already in the Open list, you must compare G and select the smallest one (along with the corresponding parent pointer). [1]
Since you have a 10/14 cost circular motion 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 valid, but this formula already reflects the actual distance in the open plain, so it cannot be easily improved.
Finding and updating nodes in the Open list is an annoying part of A * that I'm sure a lot of people would like to pretend it's not necessary as it means you can't intelligently use any predefined priority queue (they're missing effective search). This can be done with a manually built-in binary heap and hash table that maps coordinates to corresponding indices on the heap. The hash table must be updated by the heap whenever a node is moved.
[1]: Relevant pseudocode snippet from wikipedia article :
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)
source to share