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.

),( qqx

),( uxfx

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’sMotivation

– 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.

),(1 kkK uxfx

PLANNER BASED ON RRT’sNaive 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’sRRT 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 TEXTEND(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’sPath 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 PRMDifferences

– 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.