Video 11.1 Vijay Kumar. Property of University of Pennsylvania, Vijay Kumar

Transcription

1 Video 11.1 Vijay Kumar 1

2 Smooth three dimensional trajectories START INT. POSITION INT. POSITION GOAL Applications Trajectory generation in robotics Planning trajectories for quad rotors 2

3 Motion Planning of Quadrotors

4 General Set up Start, goal positions (orientations) Waypoint positions (orientations) Smoothness criterion Generally translates to minimizing rate of change of input Order of the system (n) The input is algebraically related to the nth derivative of position (orientation) Boundary conditions on (n-1) th order and lower derivatives 4

5 Calculus of Variations running cost Examples function Shortest distance path (geometry) cost functional Fermat s principle (optics) Principle of least action (mechanics) 5

6 Calculus of Variations Consider the set of all differentiable curves, x(t), with a given x(0) and x(t). 6

7 Calculus of Variations Euler Lagrange Equation Necessary condition satisfied by the optimal function x(t) Courant, R and Hilbert, D. Methods of Mathematical Physics. Vol. I. Interscience Publishers, New York, Cornelius Lanczos, The Variational Principles of Mechanics, Dover Publications,

8 Smooth trajectories (n=1) input 0 T 8

9 Smooth trajectories (n=1) Euler Lagrange Equation 9

10 Smooth trajectories (n=1) 0 T 10

11 Smooth trajectories (general n) input 11

12 Euler-Lagrange Equation Euler Lagrange Equation Necessary condition satisfied by the optimal function 12

13 Smooth Trajectories n=1, shortest distance n=2, minimum acceleration n=3, minimum jerk n=4, minimum snap velocity n order of system n th derivative is an algebraic function of input 13

14 Smooth Trajectories n=1, shortest distance n=2, minimum acceleration n=3, minimum jerk n=4, minimum snap velocity Why is the minimum velocity curve also the shortest distance curve? 14

15 Video 11.2 Vijay Kumar 15

16 Smooth Trajectories n=1, shortest distance n=2, minimum acceleration n=3, minimum jerk n=4, minimum snap 16

17 Minimum Jerk Trajectory Design a trajectory x(t) such that x(0) = a, x(t) = b Euler-Lagrange: 0 17

18 Solving for Coefficients Boundary conditions: Position Velocity Acceleration Solve: t = 0 a 0 0 t = T b

19 Minimum Jerk Trajectory a=0, b=1, T=50 19

20 Extensions to multiple dimensions (first order system, n=1) Euler Lagrange Equation Necessary condition satisfied by the optimal function 20

21 Minimum Jerk for Planar Motions Minimum-jerk trajectory in (x, y, q) goal position, orientation Human manipulation tasks Rate of change of muscle fiber lengths is critical in relaxed, voluntary motions start position, orientation T. Flash and N. Hogan, The coordination of arm movements: an experimentally confirmed mathematical model, Journal of neuroscience, 1985 G.J. Garvin, M. Žefran, E.A. Henis, V. Kumar, Two-arm trajectory planning in a manipulation task, Biological Cybernetics, January 1997, Volume 76, Issue 1, pp

22 Optimal Trajectories with Constraints Design a trajectory x(t) such that x(0) = a, x(t) = b 22

23 Minimum Time Trajectories Trapezoidal Velocity Profile 0 T 23

24 Video 11.3 Vijay Kumar 24

25 Smooth 1D Trajectories Design a trajectory x(t) such that x(0) = a, x(t) = b x t 25

26 Multi-Segment 1D Trajectories Design a trajectory x(t) such that: x t 26

27 Multi-Segment 1D Trajectories Design a trajectory x(t) such that: Define piecewise continuous trajectory: 27

28 Continuous but not Differentiable Design a trajectory x(t) such that: x What if the system is a 2 nd order system? t 28

29 Minimum Acceleration Curve for 2 nd Order Systems Design a trajectory x(t) such that: x t 29

30 Minimum Acceleration Curve for 2 nd Order Systems Design a trajectory x(t) such that: 4m degrees of freedom Cubic spline 30

31 Cubic Spline Design a trajectory x(t) such that: x t 31

32 Cubic Spline Design a trajectory x(t) such that: x 2m t 32

33 Cubic Spline Design a trajectory x(t) such that: x 2m+2(m-1) 33

34 Cubic Spline Design a trajectory x(t) such that: x 2m+2(m-1)+2 = 4m constraints t 34

35 Spline for nth order system Design a trajectory x(t) such that: x t 35

36 Spline for nth order system Design a trajectory x(t) such that: x t 36

37 Summary Polynomial interpolants Boundary conditions at intermediate points Splines Smooth polynomial functions defined piecewise (degree n) Smooth connections at in between knots (match values of functions and n-1 derivatives) 37

38 Video 11.4 Vijay Kumar 38

39 Minimum Snap Trajectory When working with quadrotors, we want to find a trajectory that minimizes the cost function: From the Euler-Lagrange equations, a necessary condition for the optimal trajectory is: The minimum-snap trajectory is a 7 th order polynomial. 39

40 Trajectory with 3 waypoints Design a trajectory x(t) such that: The trajectory will be a 7 th -order piecewise polynomial with 2 segments: This trajectory has 16 unknowns. 40

41 Trajectory with 3 waypoints Design a trajectory x(t) such that: x t 41

42 Trajectory with 3 waypoints Position constraints in matrix form: 42

43 Trajectory with 3 waypoints Endpoint derivative constraints at t 0 in matrix form: 43