numerical integration and rigid body dynamics for potential field planners
DESCRIPTION
Numerical Integration and Rigid Body Dynamics for Potential Field Planners. David Johnson. Recall. Define the world as a potential field Towards goal Away from obstacles. Robot Motion. Move down the potential field to minimum Think about contours Which way has zero change ? - PowerPoint PPT PresentationTRANSCRIPT
![Page 1: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/1.jpg)
Numerical Integration and Rigid Body Dynamics for Potential
Field Planners
David Johnson
![Page 2: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/2.jpg)
Recall• Define the world as a potential field
– Towards goal– Away from obstacles
![Page 3: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/3.jpg)
Robot Motion• Move down the potential field to minimum
– Think about contours• Which way has zero change?• Which way has maximum?• The directional derivative gives the rate of change
in a direction: Duf(x,y) = f(x,y) . ∇ u
– Move along the gradient
),,()(1 nq
UqUqU
![Page 4: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/4.jpg)
Gradient Descent
• Evaluate the gradient, move along it some step size, repeat
)(1 nnnn qUqq
![Page 5: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/5.jpg)
Difficulties
• Determining step size can be difficult
• Narrow canyons difficult
![Page 6: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/6.jpg)
Gradient and Force• This step
is the same as
since force is the gradient of a potential field• This is just a non-Newtonian physics
simulation – replace velocity with acceleration
)(1 nnnn qUqq
tqFqq nnn )(1
![Page 7: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/7.jpg)
Potential Field vs Physics Simulation
• Potential field planner can thus also be seen as a physics simulation
• Robot has state (its configuration and velocities)
• Goal and obstacle apply forces• Evolve the state of the robot
![Page 8: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/8.jpg)
Physics of a Point• Based on ordinary differential equation (ODE):
• For physics examples, the f may be constant for x, as for gravity, might also be constant for t
• Initial value problem– Given start state, how does it change over time?– Path of a point over time is the integral curve
),( tf xx
![Page 9: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/9.jpg)
Integral Curve
![Page 10: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/10.jpg)
Common way to solve the ODE
• Euler’s Method
• Many problems– No correction, so errors build– May diverge!– Requires tiny time steps to be at all useful, so
is not efficient
ttttttftt
)()(),()(
xxxxxx
![Page 11: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/11.jpg)
Where does error come from?• Look at Taylor series
• For taking time steps,
• So
nn
axnafaxafaxafafxf )(!)(...)(
!2)())(()()( 2
ttxta
0
0
nn
tntfttfttttftfttf )(!)(...)(
!2)())(()()( 020
00000
![Page 12: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/12.jpg)
Euler error
• Euler uses only first two terms of Taylor series
• Error dominated by missing quadratic
![Page 13: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/13.jpg)
Use higher order
• Runge Kutta integration– Evaluates at multiple places to cancel out
error• Implicit integration
– Has to invert small matrix, but allows big steps
![Page 14: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/14.jpg)
Midpoint methoda) Compute a normal Euler
integration stepb) Move halfway along that
change and evaluate again for a half timestep
c) Move from the original spot in the direction given by b), but for a full timestep.
d) This is called a second order method
![Page 15: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/15.jpg)
Determine Stepsize
• Fixed– Simulation is only as fast as worst case
• Measure change in taking a step vs 2 half steps
![Page 16: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/16.jpg)
Stiff ODEs• Equations with large constants
– Example: keep x at zero
– Euler integration
– Diverges if
kxx
t
ttt
xtktkxxx)1(
1
kt
tk22
![Page 17: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/17.jpg)
Stiff ODEs
• Can add energy into the system• Use friction to dampen the system
– Prevent out of control oscillations
![Page 18: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/18.jpg)
Back to potential field planner• Robot is not a point
– Can we do this in configuration space?• Apply forces to multiple locations on robot
body (n particles on robot body)
– Force
– Torque
n
iiFF
1
n
iii Fr
1
![Page 19: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/19.jpg)
Rigid Body Dynamics
• Integrate force to get new translational velocity, position
• Integrate angular acceleration to get angular velocity, orientation
• In 2D, just (x,y,q) and (x’,y’,q’)• 3D is more complicated
''q I
''mxF
![Page 20: Numerical Integration and Rigid Body Dynamics for Potential Field Planners](https://reader036.vdocuments.mx/reader036/viewer/2022081520/56816401550346895dd5a015/html5/thumbnails/20.jpg)
Back to Our Robot
• Sample points on the robot– Compute distance to goal– Compute distance to obstacles– Compute forces from each
• Apply forces to each point– Accumulate total force, total torque
• Do Euler integration to find new position, orientation