Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr
description
Transcript of Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr
![Page 1: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/1.jpg)
Randomized Kinodynamics Planning
Steven M. LaVelle and James J. Kuffner, Jr
By – Yohanand Gopal
![Page 2: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/2.jpg)
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.
![Page 3: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/3.jpg)
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)
![Page 4: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/4.jpg)
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
![Page 5: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/5.jpg)
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.
![Page 6: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/6.jpg)
PATH PLANNING IN THE STATE SPACE
![Page 7: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/7.jpg)
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.
![Page 8: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/8.jpg)
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
![Page 9: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/9.jpg)
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.
![Page 10: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/10.jpg)
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.
![Page 11: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/11.jpg)
PLANNER BASED ON RRT’s
![Page 12: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/12.jpg)
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;
![Page 13: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/13.jpg)
PLANNER BASED ON RRT’s
EXTEND operation
![Page 14: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/14.jpg)
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
![Page 15: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/15.jpg)
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.
![Page 16: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/16.jpg)
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.
![Page 17: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/17.jpg)
EXPERIMENTS CONDUCTED
![Page 18: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/18.jpg)
EXPERIMENTS CONDUCTED
![Page 19: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/19.jpg)
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.
![Page 20: Randomized Kinodynamics Planning Steven M. LaVelle and James J. Kuffner, Jr](https://reader034.fdocuments.in/reader034/viewer/2022051518/5681642f550346895dd5f9ea/html5/thumbnails/20.jpg)
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.