Path Planning algorithm

Question

The broad question is this; given an occupancy grid and the robots current pose (x,y,theta,theta'(first derivative)) how should I plan motion for my robot with the following constraints:

• shortest path between 2 points
• maintain a minimum distance to obstacles
• prefer to stay away from walls - use the center of open spaces
• prefer straight lines
• limit turn radius (> X cm)
• limit maximum change in turn radius (steering angle must be continuous)
• limit maximum acceleration

Here is how my current implementation works...

A* for the entire map (only once for a given target location)

the following then loops 5 times a second

1. Run A* on the local area (1 x 1 meter) overlaying the latest lidar scan and depth camera point cloud data for transient obstacles
2. given the robots current location (x,y - taken from particle filter), use the A* plan to determine 2 waypoints at 30cm intervals from the current location.
3. Use gradient descent to plan a path from the current pose (x,y,theta,theta') through the way points (with a bunch of constraints).
4. Use the first step of the plan from gradient descent for the next action the robot takes.

The problem with this is the gradient descent step, I haven't yet implemented all of the constraints in my cost function and already for certain pose/waypoint combinations it can't solve.

I can get a solve using simulated annealing but this is 100 times slower and can't possibly run 5 times a second.

I've considered using A* taking into consideration the full pose (x,y,theta,theta') which would take it to 4 dimensions and considering the current 2 dimensional A* is just barely fast enough, that doesn't seem like a solution.

so the actual question(s)...

is there something other than GD(gradient descent)/SA(simulated annealing) that I should be using?

have I gone about it all wrong - what is the right way?

Answer 1

1. shortest path between 2 points

   This spec goes into the cost function design.

2. maintain a minimum distance to obstacles

   Given 2D occupancy grid, threshold probability values to get occupied/free cell representation of the environment. Then, expand each obstacle cell by given minimum distance value. Once done, this spec goes into the cost function design.

3. prefer to stay away from walls - use the center of open spaces

Implement a distance to wall function that computes the minimum distance to the nearest wall for a given (x,y) cell. This distance to wall function can be pre-computed via dynamic programming before invoking the path planner. Once done, this spec goes into the cost function design.

4. prefer straight lines

   This spec goes into the cost function design.

5. limit turn radius (> X cm) limit

   This spec goes into the motion model design.

6. maximum change in turn radius (steering angle must be continuous)

   This spec goes into the motion model design.

7. limit maximum acceleration

   This spec goes into the motion model design. Do you mean linear or angular acceleration?

A vehicle motion model can be chosen as follows:

dx_dt = v*cos(\theta);

dy_dt = v*sin(\theta);

d\theta_dt = v*\kappa;

d\kappa_dt = \alpha;

dv_dt = a;

State X := (x, y, \theta, \kappa, v)

(x,y) : position [m], x_min <= x <= x_max, y_min <= y <= y_max

\theta : heading [rad], -pi <= \theta <= pi

\kappa : curvature [m^-1], \kappa_min <= \kappa <= \kappa_max

v : linear speed in [m/s], v_min <= v <= v_max

Control U := (\alpha, a)

\alpha : curvature rate [(m*s)^-1], \alpha_min <= \alpha <= \alpha_max

a : linear acceleration [m/s^2], a_min <= a <= a_max

You can implement a graph search algorithm on discretized 5D (x, y, \theta, \kappa, v) state space and 2D control space (\alpha, a).

Your cost function can have the following inputs:

• current state: x_cur, y_cur, \theta_cur, v_cur, \kappa_cur

• next state: x_next, y_next, \theta_next, v_next, \kappa_next

• current control action: \alpha_cur, a_cur

spec 1 (minimizing path length): cost_1 := norm (x_next - x_cur, y_next - y_cur);

spec 2 (maintaining a minimum distance to obstacles): cost_2 := 'inf' (x_cur, y_cur) on an obstacle in the expanded environment, 0 otherwise;

spec 3 (distance to nearest wall): cost_3 := dist2wall(x_cur, y_cur);

spec 4 (preferring straight lines): cost_4 = abs(\kappa_cur);

You need to blend these cost terms in your cost function by using some coefficients.

In summary, in order to solve your motion planning problem in a principled way with some optimality guarantees, the steps of your algorithm should be

1. Expand the obstacles by the minimum distance value

2. Compute dist2wall values for 2D environment

3. Run the graph search algorithm on discretized 5D state, 2D control space

The bad news is that searching on 5D space is not practical!

A good roboticist should use a multi-step approach for practical applications. That is, we can first solve the motion planning model using a low-order motion model with state X = (x,y,\theta) and control U =(\kappa) to ensure minimum turn radius constraint (bounded curvature).

dx_ds = cos(\theta);

dy_ds = sin(\theta);

d\theta_ds = \kappa;

State X := (x, y, \theta)

Control U := (\kappa)

Here the derivatives are defined wrt the arclength parameter s not time! Using this low order motion model requires searching only in 3D state space which can be done quickly a graph search algorithm. Once the graph search algorithm computes a geometric path by using the low order motion model, we can smooth out curvature and speed profile by considering curvature rate and acceleration constraints.

The catch is that we cannot guarantee feasibility guarantee anymore. Bad news from a theoretical point! That is, the motion planning problem has a solution per se; and we can compute the optimal solution if we search in 5D state space. However, in the 2-step approach, the initial geometric path may be too close to obstacles; and it cannot be smoothed out to generate a path wrt the given curvature rate and acceleration constraint. To remedy this drawback, in practice, we usually relax the path length cost term little bit, so that the initial path can be computed far from obstacles. Then, the post smoothing method usually works well once the initial path has enough clearance from obstacles.

Another practical advice, for the search algorithm, keep it simple, stupid (KISS)! Try the Dijkstra algorithm first, if it can get the job done, implement the Dijkstra algorithm. Always keep track the invariant properties of the search algorithm implementation for detecting bugs. When implementing a path planner, most of the time is spent on the cost function design, developing a good low-order motion model, and field tests. Just don't waste your time in implementing a complicated search algorithm from papers. A good path planner is the one that you can easily interpret its output by using your intuition.