Trajectory Planning from Multibody System Dynamics

Transcription

1 Trajectory Planning from Multibody System Dynamics Pierangelo Masarati Politecnico di Milano Dipartimento di Ingegneria Aerospaziale

2 Manipulators 2 Manipulator: chain of links commanded by motors purpose: place end effector in specified configuration along specified trajectory (pos., vel., acc.) carrying given payload end effector Canadarm (from NASA)

3 Manipulators 3 Canadarm (from NASA)

4 4 Manipulators Industrial robots: wafer-handling manipulators R-Theta 2-dof Selectively Compliant Articulated Robot Arm (SCARA) 3-dof, limited footprint, no workspace limitation (from Innovative Robotics)

5 Examples of manipulator multibody modeling with MBDyn Delta robot inverse dynamics for computed torque control 5

6 Examples of manipulator multibody modeling with MBDyn Robotics: PA-10 inverse kinematics with path optimization of cooperating robots 6

7 Examples of manipulator multibody modeling with MBDyn Robotics: biomimetic robot real-time motion planning by inverse kinematics with fault detection 7

8 Manipulators: classification 8 Manipulators: end effector prescribed degrees of freedom: n manipulator number of degrees of freedom: f number of motors: c Classification: When n = c = f the problem is purely kinematic When n < c = f the problem is redundant When n = c < f the problem is underactuated

9 Multibody dynamics 9 Basic equations: M (x) x = f (x, x, t) mechanics of unconstrained system of bodies subjected to configuration-dependent loads Can be obtained from many (equivalent!) approaches: Newton-Euler: linear/angular equilibrium of each body d'alembert-lagrange: virtual work of active forces/moments Gauss, Hertz, Hamilton,...: variational principles

10 10 Multibody dynamics Constrained system: kinematic constraints holonomic ϕ( x, t)=0 x =a+m 1 f c non-holonomic (not integrable to holonomic) ψ( x, x, t)=0 usually A(x, t) x =b( x, t) a=m 1 f algebraic relationship between kinematic variables explicitly dependent on time: rheonomic scleronomous otherwise M 1 f c =M 1 ϕt/ x λ

11 11 Multibody dynamics Redundant coordinate set: M (x) x = f (x, x, t) ϕt/ x λ ϕ( x)=0 Minimal coordinate set: (q) q = f (q, q, t) M requires capability to write easier in differential form: q x=θ(q) ϕ( x)=ϕ(θ(q)) 0 x =θ / q q q =θ +/ q x ϕ / x x =ϕ / x θ /q q 0 ϕ / x θ / q 0 Lagrange multipliers intrinsically eliminated

12 12 Multibody dynamics MBDyn uses redundant coordinate set at first order M ( x) x =β T β = f ( x, x, t) ϕ / x λ ϕ( x)=0 One can easily show how formulations in the following can be generalized to redundant coordinate set at first order For the sake of clarity, minimal coordinate set is used in the following

13 13 Motion Prescription Manipulator problem: prescribed motion (at position level): (q) q = f (q, q, t)+ B T c M φ(q)=α(t) assume by now number of motor torques c equal to number of prescribed degrees of freedom n problem structure similar to that of passive constraints BUT: M (x) x = f (x, x, t) ϕt/ x λ ϕ( x)=0 (q) q = f (q, q, t)+ B T c M φ(q)=α(t)

14 14 Motion Prescription Lagrange multipliers: Motor torques: M ( x) x = f (x, x, t) ϕt/ x λ ϕ( x)=0 ϕ/ x x =b' 1 T x =M ( f ϕ / x λ) 1 1 T ϕ / x M f b '=ϕ / x M ϕ / x λ 1 T 1 /x 1 λ=(ϕ / x M ϕ ) (ϕ / x M f b') invertible under broad assumptions (ϕ / x x )/ x x,(φ /q q )/ q q omitted for clarity (q) q = f (q, q, t)+ B T c M φ(q)=α(t) φ /q q =α 1 ( f + B T c) q = M 1 f +φ / q M 1 B T c=α φ /q M 1 T 1 1 f ) c=(φ / q M B ) ( α φ / q M invertible? (related to the concept of differential flatness)

15 Motion Prescription: Fully Determined 15 When n. prescribed degrees of freedom = n. motors, rotor torques: B T I 1 f ) c=(φ / q M 1 B T ) 1 ( α φ / q M = M φ 1 α f /q invertible? Boils down to purely kinematic problem: φ / q q =α q =φ 1 /q α which formally implies (the last problem may need iterative solution) 1 φ / q q =α q =φ 1 α, q=φ (α) /q

16 Examples of manipulator multibody modeling with MBDyn Delta robot: 3 dof, 3 prescribed motion eqs. inverse dynamics for computed torque control 16

17 Motion Prescription: Underdetermined 17 When n. prescribed dof < n. motors, problem is underdetermined: B T I 1 f ) c=(φ / q M 1 B T )+ ( α φ / q M φ+ α f =M /q Pseudo-invertible (when full rank)! NOTE: we are considering a LOCAL optimization GLOBAL optimization is a totally different problem multibody dynamics can be a tool in support of optimization local optimization can be used in real time

18 Motion Prescription: Underdetermined 18 When n. prescribed dof < n. motors, problem is underdetermined; Which pseudo-inverse? Moore-Penrose Generalized Inverse: φ / q q =α q =φ+/q α =φt/ q (φ /q φt/ q) 1 α Inertia-weighted GI (often better, heavier parts move less): q =φt/q μ q = M 1 φt/ q μ φ /q M 1 φt/q μ=α φ / q q =α M 1 φt/q ) 1 α q = M 1 φt/ q (φ /q M 1 φt/q ) 1 α μ=(φ /q M in any case, minimum (weighted) norm solutions; then one can add + arbitrary (position /) velocity (/ acceleration) in the nullspace of φ / q I φ+/q φ / q)ω q =φ+/ q α +ω, ω=(

19 Motion Prescription: Underdetermined 19 When n. prescribed dof < n. motors, problem is underdetermined; Problem can be split in staggered sequence of: configuration (nonlinear) φ(q)=α, φ /q Δ q=α φ(q), Δ q=φ+/q (α φ(q)) velocity (linear) φ / q q =α, q =φ+/ q α acceleration (linear) φ / q q =α, q =φ+/ q α All problems share same matrix (only needs be updated during config.) Different pseudo-inverses can be used in different phases: ergonomy, minimum kinetic energy change, minimum torque,...

20 Examples of manipulator multibody modeling with MBDyn Robotics: PA-10 7 dof, up to 6 prescribed motion eqs. 20

21 Examples of manipulator multibody modeling with MBDyn biomimetic robot 11 dof, up to 6 prescribed motion eqs. 21

22 Examples of manipulator multibody modeling with MBDyn 22 Human arm 7 dof, up to 6 prescribed hand motion eqs. inverse kinematics with ergonomy cost functions inverse dynamics to compute joint torques optimization to compute muscular activation

23 Examples of manipulator multibody modeling with MBDyn shoulder abduction prono-supination shoulder flexion wrist flexion 23 elbow flexion wrist deviation

24 Examples of manipulator multibody modeling with MBDyn 24 helicopter pilot's left arm holding collective control inceptor and performing a vertical repositioning maneuver

25 Examples of manipulator multibody modeling with MBDyn PA 10 robot doing corner smoothing trajectory 7 dofs, 5 prescribed motion eqs. 25

26 Examples of manipulator multibody modeling with MBDyn 26 PA 10 robot doing corner smoothing trajectory and obstacle avoidance 7 dofs, 5 prescribed motion eqs.

27 Motion Prescription: Underactuated 27 When n. prescribed dof = n. motors < n. dofs, problem is underactuated: B T φt/q 1 f ) c=(φ / q M 1 B T ) 1 ( α φ / q M 1 f ) = P 1 ( α φ / q M invertible?

28 28 Motion Prescription: Underactuated When n. prescribed dof = n. motors < n. dofs, problem is underactuated: /q M 1 B T P=φ Consider a QR decomposition then φt/ q=q R=[Q1 Q 2 ] /q M 1 B T =RT1 QT1 M 1 B T P=φ consider now the equality [] R1 =Q1 R1 0 parallel to constraint manifold [] T Q Q Q M B = M [Q1 Q 2 ] 1 M 1 B T = M (Q1 QT1 +Q 2 QT2 ) M 1 B T = B T + B T B = M T Q2 T then T 1 T /q M 1 B T =RT1 QT1 M 1 B T =R T1 QT1 M 1 B T P=φ normal to constraint manifold

29 29 Motion Prescription: Underactuated When n. prescribed dof = n. motors < n. dofs, problem is underactuated: If /q M 1 B T P=φ is singular, tangent realization of control is needed. Several techniques have been proposed, all essentially based on differential flatness (staggered differentiation and substitution to affect constraint equation via control forces through other than inertia forces) clever approach: when elastic forces are present, q = B T c K q M numerical solution using implicit scheme: q = r ( M+(h b0 )2 K)Δ now 1 B T P =φ / q ( M+(h b0 )2 K) Δ q=(h b0)2 Δ q non-singular when matrix pencil is not!

30 Examples of manipulator multibody modeling with MBDyn 30 Inspired from Betsch et al., 2008 & 2010 three torque motors links can slide through motors prescribed lemniscate ( eight -shaped) trajectory of smaller triangle 10th order polynomial Actuator #1 used to prescribe motion (problem actually modeled in 3D) Actuator #2 < > Actuator #3

31 Examples of manipulator multibody modeling with MBDyn 31 Feedforward verification with predicted motor rotations: trajectories of triangles P. Masarati, M. Morandini, A. Fumagalli, "Control Constraint Realization Applied to Underactuated Aerospace Systems", ASME 2011 IDETC/CIE August 28-31, 2011, Washington DC (DETC ).

32 Examples of manipulator multibody modeling with MBDyn 32 Feedforward verification with predicted motor rotations: motor rotations and torques P. Masarati, M. Morandini, A. Fumagalli, "Control Constraint Realization Applied to Underactuated Aerospace Systems", ASME 2011 IDETC/CIE August 28-31, 2011, Washington DC (DETC ).

33 Examples of manipulator multibody modeling with MBDyn 33 Feedforward verification with predicted motor rotations: animation P. Masarati, M. Morandini, A. Fumagalli, "Control Constraint Realization Applied to Underactuated Aerospace Systems", ASME 2011 IDETC/CIE August 28-31, 2011, Washington DC (DETC ).

34 Feedforward/feedback 34 Motion planning: determine joint motion from end effector motion planned joint motion can be prescribed through localized control feedforward can improve quality of tracking Torque demand as a function of acceleration: q f c= M when acceleration for torque demand is desired acceleration: q d f c ff = M when acceleration for torque demand is torque becomes and dynamics become q = q d +K D ( q d q )+K P (q d q) q d +K D ( q d q )+K P (q d q)) f c fb= M( (( q d q )+K D ( q d q )+K P (q d q))=0 M appropriate choice of coefficients yields asymptotic error cancellation

35 Feedforward/feedback 35 Biomimetic manipulator: 11 dof, 5 prescribed motion eqs.

36 Feedforward/feedback Biomimetic manipulator: verification with and without feedforward (same gains) 36

37 Feedforward/feedback 37 Biomimetic manipulator: verification with and without feedforward angles torques

38 38 Questions?