4

I need a shortest path algorithm for controlling a real life robot.

Lets say i have map of the environment in the form of a matrix where 1 is an obstacle and 0 is free space. If i use a conventional shortest path algorithm, such as A* then that would give me a Manhattan distance shortest path. So nowhere near a the actual shortest path. This problem arises since i cannot think of a way to penalize movement in such a way that a diagonal line is better than two straight lines. I can make a heuristic that will make A* try euclidean shortest path between two points first, but not actually make euclidean shortest path a better path.

Does anyone know of a method to get the continuous space shortest path? It does not have to be the actual optimal path, but a better one than straight lines and 90 degree angles.

I have one idea: From the start point make a circle. Increase the radius of the circle until the one point on the circle is next to a wall, or at the goal. All the points on the edge of the circle are set as children nodes with the penalty of the radius of the circle. All points inside the circle, that are not open, will be closed since there is no reason to test them. Repeat this in an A* way with euclidean shortest path as heuristic, until the goal state is reached.Make the robot go from one point to the next in a straight line.

This should give something closer to what i am looking for. A set of straight lines with various angles. It would of course be better with a continuous curvy line...

Jim Dennis
  • 17,054
  • 13
  • 68
  • 116
Kartoffel
  • 69
  • 1
  • 5

1 Answers1

6

I have implemented a continuous space path planning algorithm and blogged about it here. It uses A* to get an initial estimate and finalizes it (and pennalizes for sharp turns and robot's orientation at the destination) by using the simple gradient descent algorithm.

Continuous path on a discrete grid

Let's say the discrete path from A* has n "waypoints" (coordinates on the grid). First and last ones cannot be moved but others can, as long as the path doesn't go through blocked grid cells. The function to be minimized is parametrized by n - 2 parameters which move waypoints perpendicular to its current direction.

Shortest path (blue) and more optimal path (red)

NikoNyrh
  • 3,578
  • 2
  • 18
  • 32
  • 1
    Very nice, thank you :) I have a few questions. Did your A* search give you a set of straight lines and 90 (or 45) degree turns, which you used as a base point for your optimization? Or did you get something closer to the optimal path, with a variety of angles? How did you penalize curvature? Did you minimize the deviation from a straight line made by the two previous points, or did you have something smarter? – Kartoffel May 16 '16 at 12:10
  • A bit hard to remember but I think it generated 45 degree turns. The blue line on 2nd figure seems to have some jitter, maybe I added small random displacements before running the optimization. Curvature is measured by `(1 - a \dot b)^2` (`a` and `b` being unit vectors pointing from a way point to the next), and the sum of curvature (multiplied by some constant) is added to the cost function. Way points were moved orthogonal to the current direction of the path, note that this keeps changing as the optimization proceeds. – NikoNyrh May 16 '16 at 13:30