/Astar-with-smoothed-path

Astar algorithm with smoothed path using Fast marching method

Primary LanguagePythonApache License 2.0Apache-2.0

Modified A* algorithm with Distance cost

For planning path for indoor robots, the original A* algorithm has a problem; Genelating the path too close to obstacles. Therefore, in this modification, "Distance Cost" is added to cost calculation of A* algorithm.

f = g + h + d

The distance cost is given to the certain node according to the distance of the node and nearing obstacles. High distance cost means that the current node is nearing the obstacles. Therefore, because the original A* algorithm select nodes with minimum cost, the generated path have tendency to be far away from the obstacles.
(For deriving distance cost, Fast marching methond was used.)

Below chart shows the difference by applying distance cost to actual pathplanning.

Without distance cost With distance cost
a a

You can try this pathplanning in 'lobby_obtimization_sample.py.

Dependency

import matplotlib.pyplot as plt
import cv2
import numpy as np
import imutils
import random
import pyfmm #if you want to use fast marching method to derive distance cost
import time #if you want to see calculation speed of the algorithm

How to generate pathplanning

def pathplanning(start, end, image_path, verbose=0):
  return convert2meter(path) 
  # Return the metered path, which is converted from  grid scale to grid scale

To generate the path, offer these informations; start grid position, end grid position, and path of the map image file. This function return the list of meter-scaled path positions.

Main functions

1. img2binList

def img2binList(img, lenWidth, GRID_SIZE=50, verbose=0):
  global DISTANCECOSTMAP
  return maze

Convert RGB image to binary list. In this function, the image file is cropped first, and then converted to binary list. Additionally, global variable DISTANCECOSTMAP is created containing the information about the distance from every grid to nearing obstacles. This variable is calculated by march function of fast marching method(fmm).
Parameters: lenWidth is the actual width of the map in cm scale, and GRID_SIZE is the actual size of the grid in cm scale.

Original Image (SLAM) Cropped Image Binary List DISTANCECOSTMAP
a a a a

2. distcost

def distcost(x, y, safty_value=2):
  return distance_cost 
  # You can manually tune the weight of the distance cost by multiplying to the returning value.

It calculate the distance cost of the specific grid(x, y) using global variable DISTANCECOSTMAP. The node which is closer from the obstacles has the higher distance cost value. In this function, you can make several nodes around the obstacle as 'almostly not allowed nodes' by activate following three lines:

    #if distance_cost > (max_distance_cost/safty_value):
    #    distance_cost = 1000
    #    return distance_cost

Large safty value make more 'almostly not allowed nodes' nearing the obstacles. However, if it is too large, almost every grid will have maximum distance cost(=1000) which leads to eliminate the meaning of distance cost. Therfore, this value should be manually tuned.

3. astar

def astar(maze, start, end):
  return path[::-1] # Return reversed path

It generate the path list in grid scale. The input maze is type of binary list which is the returning form of img2binList function. Return value should be reversed because A* algorithm collect the elements of the path from the end node to start node.

drawing

Original python A* algorithm code is retrieved from https://gist.github.com/Nicholas-Swift/003e1932ef2804bebef2710527008f44#file-astar-py

4. Pseudo code

drawing