420 likes | 583 Views
CS 326 A: Motion Planning. http://robotics.stanford.edu/~latombe/cs326/2004 Kinodynamic Planning. Underactuated Robots. Fewer controls than dimensions in configuration space What is a degree of freedom: number of dimensions of C-space (global) or number of controls (local)?.
E N D
CS 326 A: Motion Planning http://robotics.stanford.edu/~latombe/cs326/2004 Kinodynamic Planning
Underactuated Robots • Fewer controls than dimensions in configuration space • What is a degree of freedom: number of dimensions of C-space (global) or number of controls (local)?
How can m controls generate span a C-space with n>m dimensions? By exploiting mechanics properties: - Rolling-with-no-sliding contact (friction), e.g.,: car, bicycle, roller skate- Conservation of angular momentum: satellite robot, under-actuated robot, cat • Others: submarine, plane, object pushingWhy is it useful?- Fewer actuators (less weight)- Design simplicity • - Convenience (think about driving a car with 3 controls!)
dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Example: Car-Like Robot f L q f y x Configuration space is 3-dimensional: q = (x, y,q) But control space is 2-dimensional: (v, f) with |v| = sqrt[(dx/dt)2+(dy/dt)2]
dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Example: Car-Like Robot f L q f y x q = (x,y,q) q’= dq/dt = (dx/dt,dy/dt,dq/dt)dx sinq – dy cosq = 0is a particular form off(q,q’)=0 A robot is nonholonomic if its motion is constrained by a non-integrable equation of the form f(q,q’) = 0
dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Example: Car-Like Robot f L q f y x Lower-bounded turning radius
dx sinq – dy cosq = 0 f q dx/dt = v cos q dy/dt = v sin q (x,y,q) L (dx,dy,dq) q dq/dt = (v/L) tan f f y |f| <F x y (dx,dy) x q How Can This Work?Tangent Space/Velocity Space
dx sinq – dy cosq = 0 f q dx/dt = v cos q dy/dt = v sin q (x,y,q) L (dx,dy,dq) q dq/dt = (v/L) tan f f y |f| <F x y (dx,dy) x q How Can This Work?Tangent Space/Velocity Space
Nonholonomic Path Planning Approaches • Two-phase planning: (first paper) • Compute collision-free path ignoring nonholonomic constraints • Transform this path into a nonholonomic one • Efficient, but possible only if robot is “controllable” • Plus need to have “good” set of maneuvers • Direct planning: (second paper) • Build a tree of milestones until one is close enough to the goal (deterministic or randomized) • Robot need not be controllable • Works in high-dimensional c-spaces
Holonomic path Nonholonomic path Path Transform
q + q’ Coverage of a Path by Cylinders q y x
Type 1 Maneuver CYL(x,y,dq,h) h • = 2r/cosdq d = 2r(1/cosdq - 1) dq dq Allows sidewise motion r
Type 2 Maneuver Allows pure rotation
q + q’ Coverage of a Path by Cylinders q y x
Drawbacks ofTwo-phase planning • Final path can be far from optimal • Not applicable to robots that are not locally controllable (e.g., car that can only move forward)
Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions -X -Y Y X (dt)
dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f X: Going straight Y: Turning, angle f |f| <F Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions For example:
X: Going straight Y: Turning, angle f -X -Y Y X (dt) [X,Y] (dt2 ) Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions For example:
Control-Based Sampling • Previous sampling technique: Pick each milestone in some region • Control-based sampling: • Pick control vector (at random or not) • Integrate equation of motion over short duration (picked at random or not) • The endpoint is the new milestone • Tree-structured roadmaps • Need for endgame regions
endgame region mg mb Control-Based Sampling
dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Example • 1. Select a milestone m • 2. Pick v, f, and dt • 3. Integrate motion from m • new configuration
Computed Paths Tractor-trailer Car That Can Only Turn Left jmax=45o, jmin=22.5o jmax=45o
Cooperative load carrying (Grasp Lab - U. Penn) Another Example
dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Nonholonomic vs. Dynamic Constraints • Nonholonomic constraint:q’ = f(q,u)where u is the control input (function of time), with dim(u) < dim(q) u = (v, f)
Nonholonomic vs. Dynamic Constraints • Nonholonomic constraint:q’ = f(q,u)where u is the control input (function of time), with dim(u) < dim(q) • Dynamic constraint: • s = (q,q’), the state of the system • s’ = f(s,u)where u is the control input
General Dynamic Equation For an arbitrary mechanical linkage:u = M(q)q” + C(q,q’) + G(q) + F(q,q’)where: - M is the inertia matrix - C is the vector of centrifugal and Coriolis terms - G is the vector of gravity terms - F is the vector of friction terms+ constraints on u
Nonholonomic vs. Dynamic Constraints • Nonholonomic constraint:q’ = f(q,u)where u is the control input (function of time), with dim(u) < dim(q) • Dynamic constraint: • s = (q,q’), the state of the system • s’ = f(s,u)where u is the control input Similar techniques to handle nonholonomic and dynamic constraints (kinodynamic planning)
“Space” Robot (ARL Lab) robot obstacles air thrusters gas tank air bearing
f a y x Modeling of Robot q = (x,y) s = (q,q’) u = (f,a) x” = (f/m) cosa y” = (f/m) sina f fmax s’ = F(s,u)
endgame region mg mb PRM in State (x Time) Space The roadmap is a tree oriented along the time axis
Computed Path Mean planning time: .002 s Mean number of milestones: 22
Another Path Mean planning time: .27s Mean number of milestones: 1946
1 2 3 5 4 6 7 Example with Replanning
Another Example with Replanning Total duration : 40 sec
R(mb) Rw(M) Rw(m) mb lookout Endgame region m lookout point Expansive Space
Expansive Space Visibility Reachability Visibility Reachability S
Optimality of a Trajectory Often one seeks a trajectory that optimizes a given criterion, e.g.: • smallest number of backup maneuvers, • minimal execution time, • minimal energy consumption Bobrow’s paper + variational techniques
Variational Path/Trajectory Optimization • Steepest descent technique. • Parameterize the geometry of a trajectory, e.g., by defining control points through which cubic spines are fitted. • Vary the parameters. For the new values re-compute the optimal control. If better value of criterion, vary further. No performance guarantee regarding optimality of computed trajectory