Numerical Integration and Rigid Body Dynamics for Potential Field Planners

1. Numerical Integration and Rigid Body Dynamics for Potential Field Planners David Johnson

2. Recall • Define the world as a potential field • Towards goal • Away from obstacles

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

4. Gradient Descent • Evaluate the gradient, move along it some step size, repeat

5. Difficulties • Determining step size can be difficult • Narrow canyons difficult

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

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

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

9. Integral Curve

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

11. Where does error come from? • Look at Taylor series • For taking time steps, • So

12. Euler error • Euler uses only first two terms of Taylor series • Error dominated by missing quadratic

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

14. Midpoint method • Compute a normal Euler integration step • Move halfway along that change and evaluate again for a half timestep • Move from the original spot in the direction given by b), but for a full timestep. • This is called a second order method

15. Determine Stepsize • Fixed • Simulation is only as fast as worst case • Measure change in taking a step vs 2 half steps

16. Stiff ODEs • Equations with large constants • Example: keep x at zero • Euler integration • Diverges if

17. Stiff ODEs • Can add energy into the system • Use friction to dampen the system • Prevent out of control oscillations

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

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

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