Continuous space shortest path

Question

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...

Answer 1

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.

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.