shortest path between 2 points
This spec goes into the cost function design.
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.
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.
prefer straight lines
This spec goes into the cost function design.
limit turn radius (> X cm) limit
This spec goes into the motion model design.
maximum change in turn radius (steering angle must be continuous)
This spec goes into the motion model design.
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
Expand the obstacles by the minimum distance value
Compute dist2wall values for 2D environment
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.