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

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.

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)

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.

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.

PATH PLANNING IN THE STATE SPACE

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.

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.

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.

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.

PLANNER BASED ON RRT’s

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;

PLANNER BASED ON RRT’s EXTEND operation

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

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.

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.

EXPERIMENTS CONDUCTED

EXPERIMENTS CONDUCTED

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.

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.