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? - PowerPoint PPT Presentation
Randomized Kinodynamics PlanningSteven M. LaVelle and James J. Kuffner, JrBy Yohanand GopalINTRODUCTION 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 PLANNINGRandomized 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 efficientConnecting configurations is a difficult task, particularly for complicated nonholonomic dynamical systems. (non-linear control problem)
PATH PLANNING IN THE STATE SPACEProblem Formulation
Path planning in a state space that has first order derivativesLet 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:
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 SPACEObstacles
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/CobstPlanning in X consists of finding a continuous path in Xfree = X/XobstDefinition: x Xobst if and only if q Cobst forPlanning in X can also consist of finding a continuous path in Xfree = X/XricA 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 SPACESolution 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 RRTsMotivation
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 robots 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 RRTsNaive 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 RRTsRRT Construction
Insert the initial state as a vertexRepeatedly 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 pointFind the output vertex using the control law (xk+1)Insert and edge between xk and xk+1.
PLANNER BASED ON RRTs
PLANNER BASED ON RRTsBasic rapidly exploring random tree construction algorithm
BUILD_RRT(xinit )1 T.init(xinit );2 for k = 1 to K do3 xrand RANDOM_STATE();4 EXTEND(T, xrand);5 Return TEXTEND(T, x)1 xnear NEAREST_NEIGHBOR(x, T );2 if NEW_STATE(x, xnear, xnew, unew) then3 T.add_vertex(xnew);4 T.add_edge(xnear, xnew, unew);5 if xnew = x then6 Return Reached;7 else8 Return Advanced;9 Return Trapped; PLANNER BASED ON RRTs
PLANNER BASED ON RRTs A bidirectional rapidly exploring random treesbased planner
RRT_BIDIRECTIONAL(xinit, xgoal );1 Ta.init(xinit ); Tb.init(xgoal );2 for k = 1 to K do3 xrand RANDOM_STATE();4 if not (EXTEND(Ta, xrand) = Trapped) then5 if (EXTEND(Tb, xnew) = Reached) then6 Return PATH(Ta, Tb);7 SWAP(Ta, Tb);8 Return FailurePLANNER BASED ON RRTsPath 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.
Sample the space with a biased toward a region close to the goal.
RRT and PRMDifferences
RRTs drive forwardVertexes are inserted as needed until a solution is reachedEach new vertex is only connected to its nearest neighbourNearest neighbour criteria depends on reachable states. Similarities
Both based on the idea of sampling random nodes in free space and connecting them.
FUTURE WORKBetter 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.
CONCLUSIONPath 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.