User:Siyangtao

Navigation by A-Star Path Planning Algorithm

Once a robot detects the frontier to move to, it attempts to find a shortest obstacle-free path to that specific position. This task is to be undertaken by a path planning algorithm. Originally, the navigation is under the control of vfh driver which combines the obstacle avoidance algorithm with local navigation algorithm. It works great as a local navigation system, for example in a maze-like environment, however, it is poor for global navigation when the robot maps a world with many rooms or built-in places which have only one entrance and exit way, the robot, in most cases, go around the current position instead of finding a way out of it (See Figure 6). To solve this problem, I lay the A-star path planning on the top of vfh. That means A-star is responsible to find the path to the goal and vfh is used to follow such a path, in which way the global navigation can be realized.

Figure 6 Robot go around its current place and cannot find a way out

A star [18] is one of most useful heuristic graph search algorithm that builds a path from a given staring node to a given goal node. The cost needed to move from starting node to the goal node is defined as following formula:

f(n) = g(n) + h(n); where g(n) = the cost function to move from the starting point to a given node n. h(n) = the estimated cost function to move from node n to the final destination.

Calculated on a grid, many heuristic functions can be used. I introduce two methods, Manhattan distance and Diagonal distance.

Manhattan distance The standard method to estimate the heuristic is Manhattan distance. If D is the minimum cost to move from one node to its adjacent node, the heuristic can be D multiplied by the Manhattan distance: h(n) = D * (abs(n.x-goal.x) + abs(n.y-goal.y))

where (n.x, n.y) and (goal.x, goal.y) are the coordinates of the starting and goal nodes.

Diagonal distance If diagonal movement is allowed, a different heuristic called diagonal heuristic [18] can be applied. Consider the cost moving along the diagonal between two nodes is shorter than the cost moving along two orthogonal path, the diagonal distance method is developed. h2(n) = min(abs(n.x-goal.x), abs(n.y-goal.y)); h1(n) = (abs(n.x-goal.x) + abs(n.y-goal.y)); h(n) = D2 * h2(n) + D * (h1(n) - 2*h2(n))). In the formulas above, the h2(n) is the number of nodes needed to traverse along the diagonal, h1(n) is the Manhattan distance. The cost of moving diagonally is now D2 = sqrt(2)*D, and the straightly moving cost is still D. One problem by using A-star for the path planning is a map should be given or known in advance before the algorithm can calculate out the path for the robot to follow to the goal. However, SLAM requires no map available but builds the map during localizing the robot itself. In my case, the robot maintains its temporary map in the real time until there is no frontier or places having not been visited. This temporary map can be used to provide the map for A-star algorithm to find out a path or way for robot to follow. Since there must be a path between frontier and the robot’s current location, such algorithm is feasible in this case. Another problem in this algorithm is it always finds the shortest path between two nodes. This always produces the path very close to the walls or obstacles. If the map is grid map like what is used in this experiment, some parts of the path are selected from the neighbouring nodes (cells or girds) of the obstacles. Since vfh has a free-space-cutoff parameter which determines how close a robot will get to an obstacle in the direction of motion before turning to avoid, this can cause that the vfh cannot follow the path smoothly to the end node but perform similarly to the case stated above in Figure 6 trying to reach a particular node. Therefore, the A-star algorithm should be modified to suitable for this specific case. I modified this algorithm by allocating larger costs to the nodes which closer to the obstacles or walls and less cost to those farther. Thus, the closer the path is to the wall, the more expensive it will cost. As a result, the robot can choose a shortest path which considers the cost of each node it will go through. Such a path is often in between two obstacles instead of close to either of them.