1. Randomized Kinodynamics Planning Steven M. LaVelle and James J
Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr
By – Yohanand Gopal

2. INTRODUCTION What is Kinodynamic planning?
Is a class of path planning algorithms that take into consideration both robot's kinematics and dynamics.
What is the purpose of this research?
Extend classic kinodynamic path planning techniques to suit systems with high- dimensional state spaces and complicated dynamics.
What is the approach to solve the problem?
The use of rapidly exploring random trees which offer similar benefits as randomized holonomic planning methods but apply to a broader class of problems.

3. RELATED WORK IN RANDOMIZED HOLONOMIC PLANNING
Randomized Potential Fields
A heuristic function is defined on the configuration space to steer robot towards goal through gradient descent.
Random walks are used to escape local minimum traps.
Efficient for holonomic planning but depends on the choice of a good heuristic function.
Choosing a good heuristic function is difficult when obstacles and differential constraints are added to the problem.
Probabilistic Roadmaps
A graph is constructed on configuration space by generating random configurations and attempting to connect pairs of nearby configurations with a local planner.
Local planning is efficient
Connecting configurations is a difficult task, particularly for complicated nonholonomic dynamical systems. (non-linear control problem)

4. PATH PLANNING IN THE STATE SPACE
Problem Formulation
Path planning in a state space that has first order derivatives
Let C be the configuration space and q each configuration in C.
Let X be the state space and x each state in X defined as:
Differential Constraints
C-Planning: rolling contacts between rigid bodies or limited control sets.
X-Planning: conservation laws (e.g., angular momentum conservation).
Constraints may be expressed as follows, where u represents the control inputs.

5. PATH PLANNING IN THE STATE SPACE
Obstacles
Assume environment only contains static obstacles.
Assume we can determine efficiently if a configuration is in collision.
Planning in C consists of finding a continuous path in Cfree = C/Cobst
Planning in X consists of finding a continuous path in Xfree = X/Xobst
Definition: x ∈ Xobst if and only if q ∈ Cobst for
Planning in X can also consist of finding a continuous path in Xfree = X/Xric
A state lies in Xric (region of inevitable collision) if there exists no input that can be applied over any time interval to avoid collision.

6. PATH PLANNING IN THE STATE SPACE

7. PATH PLANNING IN THE STATE SPACE
Solution Trajectory
The trajectory is a time parameterized continuous path that satisfies the nonholonomic constraints.
The objective is to find an input function u :[0,T ] → U that results in a collision free trajectory from xinit to xgoal.

8. PLANNER BASED ON RRT’s Motivation Illustration of concepts
Develop a planning method that easily drives forward (like potential fields)
Explore the space quickly and uniformly (like PRM)
Illustration of concepts
Consider a simple case of planning for a point robot in 2-D space.
To extend to kinodynamic planning assume the robot’s motion is governed by the following discretized control law.
The set of control inputs U correspond to directions in which the robot can be moved a fixed, small distance in S1.

9. PLANNER BASED ON RRT’s Naive Random Tree Construction
Choose a random vertex from the tree (xk)
Choose a random input from the control input set U (uk)
Find the output vertex using the control law (xk+1)
Insert and edge between xk and xk+1.

10. PLANNER BASED ON RRT’s RRT Construction
Insert the initial state as a vertex
Repeatedly select a random point in the space and find its nearest neighbour in the tree (xk).
Choose an input (uk) in U that pulls the vertex (xk) toward the random point
Find the output vertex using the control law (xk+1)
Insert and edge between xk and xk+1.

11. PLANNER BASED ON RRT’s

12. PLANNER BASED ON RRT’s
Basic rapidly exploring random tree construction algorithm
BUILD_RRT(xinit )
1 T.init(xinit );
2 for k = 1 to K do
3 xrand ←RANDOM_STATE();
4 EXTEND(T, xrand);
5 Return T
EXTEND(T, x)
1 xnear ←NEAREST_NEIGHBOR(x, T );
2 if NEW_STATE(x, xnear, xnew, unew) then
3 T.add_vertex(xnew);
4 T.add_edge(xnear, xnew, unew);
5 if xnew = x then
6 Return Reached;
7 else
8 Return Advanced;
9 Return Trapped;

13. PLANNER BASED ON RRT’s EXTEND operation

14. PLANNER BASED ON RRT’s
A bidirectional rapidly exploring random trees–based planner
RRT_BIDIRECTIONAL(xinit, xgoal );
1 Ta.init(xinit ); Tb.init(xgoal );
2 for k = 1 to K do
3 xrand ←RANDOM_STATE();
4 if not (EXTEND(Ta, xrand) = Trapped) then
5 if (EXTEND(Tb, xnew) = Reached) then
6 Return PATH(Ta, Tb);
7 SWAP(Ta, Tb);
8 Return Failure

15. PLANNER BASED ON RRT’s Path Planning Queries
Classical bidirectional search:
Two RRT are built, one rooted at xinit and the other one at xgoal
Perform a search for states that are common to both trees to find a path.
Only one of the trees is extended in each iteration to enhance performance.
One drawback is the presence of discontinuities where trees meet.
Single RRT:
Sample the space with a biased toward a region close to the goal.

16. RRT and PRM Differences Similarities RRTs drive forward
Vertexes are inserted as needed until a solution is reached
Each new vertex is only connected to its nearest neighbour
Nearest neighbour criteria depends on reachable states.
Similarities
Both based on the idea of sampling random nodes in free space and connecting them.

17. EXPERIMENTS CONDUCTED

18. EXPERIMENTS CONDUCTED

19. FUTURE WORK
Better techniques to calculate the nearest neighbor must be implemented to increase performance. (naive techniques were used, all nodes were explored)
Finding a better metric for the cost to transition between states.
Methods such as variational optimization could be implemented to optimize trajectories.

20. CONCLUSION
Path planner generates good paths but not necessarily optimal.
Problems with up to 12 DOF were successfully implemented using RRT techniques.
Performance of RRTs is not as dependant of the cost function as techniques like potential fields.