Trajectory Planning and Collision Detection for Robotics Applications

Transcription

1 Trajectory Planning and Collision Detection for Robotics Applications joint work with Rene Henrion, Dietmar Homberg, Chantal Landry (WIAS) Institute of Mathematics and Applied Computing Department of Aerospace Engineering University of the Federal Armed Forces at Munich 2nd SADCO Industrial February 2-3, 2012 Fotos: Magnus Manske (Panorama), Luidger (Theatinerkirche), Kurmis (Chin. Turm), Arad Mojtahedi (Olympiapark), Max-k (Deutsches Museum), Oliver Raupach (Friedensengel), Andreas Praefcke (Nationaltheater)

2 Motivation Purpose: virtual factory design (digital factory) automatic re-configuration of robots in production line Control tasks: basic task: single robot moves load from A to B no collision constraints many robots moving in limited space anti-collision constraints for robots one or more robots working on workpiece with complicated geometry anti-collision constraints for workpiece and robot (courtesy of WIAS, Prof. D. Hömberg)

3 Contents Introduction Optimal control problem Direct discretization method Collision avoidance Examples

4 Mechanical Multibody Robot Model Kinetic energy: T (q, q ) = 1 2 i=1 Equations of motion: (u = (u 1, u 2, u 3 ) : joint torques) with mass matrix 4 ) (m i R i ω i J i ω i M(q)q = G(q, q ) + F (q, u) M(q) := 2 q,q T (q, q ) generalized Coriolis forces G(q, q ) := qt (q, q ) ( ) 2 q,q T (q, q ) q and generalized applied forces F (q, u).

5 Robot Control Basic task: Minimize t f subject to q (t) = v(t) M(q(t))v (t) = f (q(t), v(t), u(t)) and boundary conditions q(0) = q 0, v(0) = v 0, q(t f ) = q f, v(t f ) = v f and state or control constraints, e.g. q(t) q max, v(t) v max, u(t) u max

6 State Constrained Optimal Control Problem Trajectory Planning and Collision Detection for Robotics Applications OCP Minimize Φ(x(0), x(1), p) s.t. x (t) f (x(t), u(t), p) = 0 a.e. in [0, 1] c(x(t), u(t), p) 0 a.e. in [0, 1] s(x(t), p) 0 in [0, 1] ψ(x(0), x(1), p) = 0 Notation state: x W 1, ([0, 1], IR nx ) control: u L ([0, 1], IR nu ) parameter: p, no optimization variable!

7 Solution Approaches Indirect Approach: OCP minimum principle BVP multiple shooting

8 Solution Approaches Indirect Approach: OCP minimum principle BVP multiple shooting Direct Approach: Version 1: Discretization Approach OCP discretization NLP SQP

9 Solution Approaches Indirect Approach: OCP minimum principle BVP multiple shooting Direct Approach: Version 1: Discretization Approach OCP discretization NLP SQP Version 2: Function Space Approach OCP SQP/Newton BVP discretization

10 Solution Approaches Indirect Approach: OCP minimum principle BVP multiple shooting Direct Approach: Version 1: Discretization Approach OCP discretization NLP SQP Version 2: Function Space Approach Postoptimality Analysis: sufficient conditions OCP SQP/Newton BVP discretization sensitivity analysis real-time optimization

11 Trajectory Planning and Collision Detection for Robotics Applications Direct Shooting x h (BDF, RK) x 1 x M u h (B-Splines) u 1 u N control grid t 0 t N state grid t 0 t M

12 Direct Shooting DOCP Minimize Φ(x h (t 0 ; z), x h (t N ; z), p) s.t. c(t i, x h (t i ; z), u h (t i ; z), p) 0, i, s(t i, x h (t i ; z), p) 0, i, ψ(x h (t 0 ; z), x h (t N ; z), p) = 0, x h ( t j ; z) x j = 0, j Structure M = 1 (single shooting) : small & dense; z = (x 1, u 1,..., u N ) M > 1 (multiple shooting) : large-scale & sparse; z = (x 1,..., x M 1, u 1,..., u N )

13 Direct Shooting: Solution Process optimal/feasible? initial guess z [0] iteration i = 0 i = i + 1 consistent IVs DAE & SDAE new SQP iterate z [i+1] = z [i] + λ i d [i] numerical solution DAE & SDAE Evaluation obj./constr./deriv.

14 Numerical Solution Software OCPID-DAE1 [G.]: solves DAE optimal control and parameter identification problems direct multiple shooting discretization SQP method (filter method, non-monotone linesearch, BFGS update, primal active-set QP solver) various integrators (Runge-Kutta, BDF methods, linearized Runge-Kutta methods) various control approximations (B-splines of order k) gradients by sensitivity differential equation sensitivity analysis and adjoint estimation extensions to adjoint gradient computation and mixed-integer optimal control problems

15 Example I 1 Control 1 vs time 1 Control 2 vs time 1 Control 3 vs time control control control t t t state 1 state 4 State 1 vs time State 4 vs t time t state 2 state 5 State 2 vs time State 5 vs t time t state 3 state State 3 vs time State 6 vs t time t

16 Example II adjoint 1 adjoint 4 Adjoint 1 vs time t Adjoint 4 vs time t adjoint 2 adjoint 5 Adjoint 2 vs time t Adjoint 5 vs time t adjoint 3 adjoint 6 Adjoint 3 vs time t Adjoint 6 vs time t

17 Robot Control Why anti collision constraints? without constraints with constraints

18 Anti Collision Constraints for Single Robot Goal: avoid collision and penetration of bodies Trajectory Planning and Collision Detection for Robotics Applications Approaches: simple angle restrictions for first arm define a cone second arm tip above cone

19 Anti Collision Constraints for Robot Arms Trajectory Planning and Collision Detection for Robotics Applications Goal: avoid collision and penetration of bodies Approaches: discretize robot bodies and impose mutual restrictions for every discrete point large number of state constraints restrict distance of bodies z x y

20 Minimum Distance of Bodies Centerline representation: (i = 1, 2) -1 0 R i (t) = r i M 1 + ts i, s i = r i M 2 r i M 1 Minimum distance: ( Solution: (A = min t 1,t 2 [0,1] R 1 (t 1 ) R 2 (t 2 ) 2 2 s 1 s 2 ), b = r 2 M 1 r 1 M 1 ) 1 z M x -1 s y M2 ( ( ) 1 t = Π [0,1] A A) A b Constraint: (d=diameter) z R 1 (t 1 ) R 2 (t 2 ) 2 2 d(t1, t 2 ) 2 x y

21 Robot Control Cooperative robots: top view front view

22 Collision Detection Q 3 P 3 P 4 P 5 Q 2 P 2 P 1 Q 1 Robot Obstacle Robot: union of convex polyhedra n p P = P (i) with P (i) = {x R 3 A (i) x b (i) } i=1 Obstacle: union of convex polyhedra n q Q = Q (j) with Q (j) = {x R 3 C (j) x d (j) } j=1

23 Collision Detection The Static Case No collision of robot P and obstacle Q, if and only if P (i) Q (j) = i, j Equivalent: No solution exists of the linear system ( ) ( ) A (i) b (i) x C (j) d (j) i, j Farkas Lemma The linear system ( A (i) ) ( b (i) ) C (j) x d (j) i, j is not solvable, if and only if there exists w (i,j) with ( A (i) ) T w (i,j) = 0 and ( ) T b (i) w (i,j) < 0. w (i,j) 0, C (j) d (j)

24 Collision Detection Motion in Time Configuration at time t: n p P(t) = P (i) (t) with P (i) (t) = {x R 3 A (i) (t)x b (i) (t)} i=1 Translation and rotation: (S (i) (t): orthogonal rotation matrix, r (i) (t): translational vector) P (i) (t) = S (i) (t)p (i) (0) + r (i) (t) Property A (i) (t) = A (i) (0)S (i) (t) T and b (i) (t) = b (i) (0) + A (i) (t)r (i) (t) Collision Criteria Robot P(t) and (fixed) obstacle Q do not collide if and only if for each i, j there exists w (i,j) (t) 0 with ( A (i) (t) ) T w (i,j) (t) = 0 and ( ) T b (i) (t) w (i,j) (t) < 0 C (j) d (j)

25 Optimal Control with Collision Avoidance Trajectory Planning and Collision Detection for Robotics Applications DAE Optimal Control Problem (OCP) Minimize tf c 1 t f + c 2 u(t) 2 dt 0 subject to M(q(t))q (t) = f (q(t), v(t), u(t)) 0 = ψ(q(0), v(0), q(t f ), v(t f )) and the mixed control-state constraints (with ε > 0) ( A (i) (q(t)) ) T w (i,j) (t) = 0 C (j) ( b (i) (q(t)) ) T w (i,j) (t) ε d (j) and the control constraints u(t) U, w (i,j) (t) 0

26 Shooting Discretization Discretization by direct shooting technique: Nonlinear Optimization Problem Minimize J(z) with respect to z = (u 0,..., u N ) R (N+1)nu w = (w 1,0,..., w 1,N,..., w M,0,..., w M,N ) R (N+1)nw subject to the constraints h(z) = 0, w I,k 0, I = 1,..., M, k = 0,..., N, G I,k (z) w I,k = 0, I = 1,..., M, k = 0,..., N, g I,k (z) w I,k ε, I = 1,..., M, k = 0,..., N, u k U k = 0,..., N

27 Numerical Approaches elimination of equality constraints: w J I,k = GJ I,k (z) G Jc I,k (x) w Jc I,k 0 backface culling active set strategy: eliminate invisible anti-collision constraints from SQP iteration elimination of artificial control variables: Replace anti-collision constraints by non-smooth constraints d I,k (z) ε, I = 1,..., M, k = 0,..., N, where d I,k (z) is the value function of the parametric linear program: LP I,k (z) : Minimize g I,k (z) w w.r.t. w subject to G I,k (z) w = 0, w 0 exploitation of sparsity: regularization of zero block in Hessian and Schur complement technique

28 Partially Sparse Structure KKT matrix structure in active set SQP: L zz ((γ 1,0 ) z ) ((γ M,N ) z ) h (z) r 1,0 (z) r M,N (z) s 1,0 (z) s M,N (z) (γ 1,0 ) z G 1,0 (z) g 1,0 (z) (γ M,N ) z G M,N (z) g M,N (z) h (z) r 1,0 (z) G 1,0 (z) r M,N (z) G M,N (z) s 1,0 (z) g 1,0 (z) s M,N (z) g M,N (z)

29 Linear Algebra Interior-point methods and active set methods require to solve linear equations with saddlepoint structure: Q A B A 0 0 B 0 Λ 1 S Semismooth Newton methods yield unsymmetric systems: Q A B A 0 0 ΛB 0 S S, Λ: diagonal matrices, positiv (semi-)definite

30 Direct Factorization of Sparse Matrices sparse matrix dense LU re-ordering sparse LU Trajectory Planning and Collision Detection for Robotics Applications

31 WORHP (developed by C. Büskens, M. Gerdts) WORHP (We Optimize Really Huge Problems) SQP method globalization by filter method or linesearch method Trajectory Planning and Collision Detection for Robotics Applications QP by primal-dual interior-point-method (with warm start) or semi-smooth Newton designed for large scale and sparse problems automatic Hessian approximations by sparse finite difference approximations or by BFGS and sparse BFGS update strategies iterative equation solvers (cgne, cgs, bicgstab, cgnr) and direct solvers (interfaces to MA57, MA86, MA97, PARDISO, SuperLU, MUMPS, WSMP) interfaces (Fortran 90/95, C++, Matlab, ASTOS, AMPL, reverse communication, traditional) Sponsors and project partners:

32 WORHP Results (920 CUTEr + 68 COPS Net-mod) Tabelle: Summary of test-results for the CUTEr in view of robustness. Tabelle: Summary of test-results for the COPS and Net-mod test-sets in view of robustness.

33 Trajectory Planning and Collision Detection for Robotics Applications Results left face bottom face right face

34 Trajectory Planning and Collision Detection for Robotics Applications Some more robots...

35 Trajectory Planning and Collision Detection for Robotics Applications Automatic Drive along a Test-course Task: Minimize Time + Steering effort! Why? I provide simulation tools useable in development process I automatic/autonomous driving (fix influence of driver, standardized environment for set-up of cars) I future: driving assistance system

36 Outlook Mechanical systems with contacts Realtime Optimal Control parametric sensitivity analysis model predictive control Sparsity and tailored methods Robustness of numerical methods Simultaneous optimization of schedules and robot motions

37 Trajectory Planning and Collision Detection for Robotics Applications Thanks for your attention! Questions? Further Information: Fotos: Magnus Manske (Panorama), Luidger (Theatinerkirche), Kurmis (Chin. Turm), Arad Mojtahedi (Olympiapark), Max-k (Deutsches Museum), Oliver Raupach (Friedensengel), Andreas Praefcke (Nationaltheater)