Natural and artificial constraints

Transcription

1 FORCE CONTROL Manipulator interaction with environment Compliance control Impedance control Force control Constrained motion Natural and artificial constraints Hybrid force/motion control

2 MANIPULATOR INTERACTION WITH ENVIRONMENT Constrained motion Motion control (?) positioning accuracy environment uncertainty Unstable contact Force measurements Control strategies indirect direct

3 COMPLIANCE CONTROL Effects of contact force on operational space motion control schemes B(q) q + C(q, q) q + F q + g(q) = u J T (q)h e at equilibrium J T A(q)K P x = J T (q)h e x = K 1 P T T A (x)h e = K 1 P h A K 1 P control active compliance linear with respect to f torsional with respect to µ If h e N(J T ) = x = 0 h e 0

4 Passive compliance elementary displacement between two rigid bodies R and S elastically coupled (with respect to either reference frame) dx r,s = [ ] dor,s ω r,s dt = v r,s dt h s = [ fs µ s ] = [ Kf K c K T c K µ ] [ ] dor,s ω r,s dt = Kdx r,s h r = h s Stiffness matrix K (symmetric and positive semi-definite) K f translational stiffness K µ torsional stiffness K c coupling stiffness Compliance matrix C dx r,s = Ch s Remote Center of Compliance (RCC)

5 Active compliance end-effector position and orientation with respect to desired frame T d e = (T d) 1 T e = [ R d e o d d,e 0 T 1 ] R d e = R T d R e o d d,e = R T d (o e o d ) x = [ o d d,e φ d,e ] x = J Ad (q, x) q J Ad (q, x) = T 1 A (φ d,e) [ R T d O O R T d ] J(q) PD control with gravity compensation u = g(q) + J T A d (q, x)k P x J T A d K D J Ad (q, x) q

6 Stability analysis V = 1 2 qt B(q) q xt K P x > 0 q, x 0, at equilibrium J T A d (q)k P x = J T (q)h e h d e = T T A (φ d,e)k P x in terms of elementary displacements d x = x 1 dt = TA x=0 (0)(vd d vd 1 e )dt = TA (0)dx e,d h e = K P dx e,d dx e,d = K 1 P h e Elastically compliant environment model h e = Kdx r,e K environment stiffness positive semi-definite (only for the constrained motion directions)

7 Active vs. passive compliance... at equilibrium h e = ( I 6 + KK 1 ) 1Kdxr,d P dx e,d = K 1 P ( I6 + KK 1 ) 1Kdxr,d P manipulator ± compliant environment ± compliant

8 Example K = K f = diag{k x, 0} K P = diag{k Px, k Py } At equilibrium k Px k x f e = (x d x r ) k Px + k x o e = 0 [ kpx x d + k x x r k Px + k x y d ] if k Px /k x 1 if k Px /k x 1 x e x d f x k x (x d x r ) x e x r f x k Px (x d x r )

9 Joint space active compliance at equilibrium K P q = J T (q)h e q = K 1 P JT (q)h e d x = J(q)K 1 P JT (q)h e

10 IMPEDANCE CONTROL Operational space inverse dynamics control u = B(q)y + n(q, q) q = y B 1 (q)j T (q)h e choice of new control input y = J 1 A ( 1 (q)md Md ẍ d + K D x + KP x M d JA (q, q) q ) M d x + KD x + KP x = M d B 1 A (q)h A B A (q) = J T A 1 (q)b(q)ja (q) Mechanical impedance mass M d damping K D stiffness K P

11 Integration of contact force measuerements u = B(q)y + n(q, q) + J T (q)h e y = J 1 A ( ) 1 (q)md Md ẍ d +K D x+kp x M d JA (q, q) q h A M d x + KD x + KP x = h A active linear impeadance

12 To avoid representation singularity problems... time varying desired frame x = J Ad (q, x) q + b( x, R d, ȯ d, ω d ) [ R T b( x, R d, ȯ d, ω d ) = d ȯ d + S(ωd d ] )od d,e T 1 (φ d,e )ωd d x = J Ad q J Ad q + ḃ new control input y = J 1 A d M 1 ( d KD x + KP x M d JAd q + M d ḃ h d e) M d x + KD x + KP x = h d e

13 Passive impedance mechanical devices

14 Example Impedance control with force measurements M d = diag{m dx, m dy } K D = diag{k Dx, k Dy } K P = diag{k Px, k Py } manipulator environment systems dynamics along the two operational space directions (constant x d ) m dx ẍ e + k Dx ẋ e + (k Px + k x )x e = k x x r + k Px x d m dy ÿ e + k Dy ẏ e + k Py y e = k Py y d

15 Environment (x r = 1) more compliant k x = 10 3 N/m less compliant k x = 10 4 N/m Control m dx = m dy = 100 k Dx = k Dy = 500 k Px = k Py = 2500 Manipulator x e = [ 1 0 ] T x d = [ ] T tip y pos tip x force [m] [N] [s] [s]

16 Admittance control Mechanical impedance between desired frame and compliant frame t M t z + KDt z + KPt z = h d e compliant behaviour during interaction with environment good robustness of inner motion control feedback loop (larger bandwidth)

17 FORCE CONTROL Compliance or impedance control = indirect force control Direct control PD control action on force (force measurements corrupted by noise) closure of outer force regulation feedback loop = reference input to inverse dynamics motion control scheme Hyp: only position variables elastic environment model f e = K(x e x r )

18 Force control with inner position loop u = B(q)y + n(q, q) + J T (q)h e y = J 1 (q)m 1 ( ) d KD ẋ e + K P (x F x e ) M d J(q, q) q M d ẍ e + K D ẋ e + K P x e = K P x F Choice of control x F = C F (f d f e ) M d ẍ e + K D ẋ e + K P (I 3 + C F K)x e = K P C F (Kx r + f d )

19 PI action C F = K F + K I t ( ) dς choice of K D, K P, K F, K I = stability margins + bandwidth At (stable) equilibrium f e = f d Kx e = Kx r + f d

20 Force control with inner velocity loop y = J 1 (q)m 1 ( ) d KD ẋ e + K P x F M d J(q, q) q x F = K F (f d f e ) M d ẍ e + K D ẋ e + K P K F Kx e = K P K F (Kx r + f d ) At (stable) equilibrium f e = f d Kx e = Kx r + f d sensitive to unmodelled dynamics (lack of integral action)

21 Parallel force/position control if f d / R(K) = drift of x e if f d R(K) = x e (o ẋ e ) 0 Addition of x d y = J 1 (q)m 1 ( d KD ẋ e + K P ( x + x F ) M d JA (q, q) q ) At equilibrium f e = f d x e = x d + C F ( K(xr x e ) + f d )

22 Example Impedance (M d, K D, K P ) like above References f d = [ 10 0 ] T x d = [ ] T Force control C F = diag{c Fx, 0} 1. control with inner position loop k Fx = k Ix = control with inner velocity loop k Fx = parallel control k Fx = k Ix =

23 1.02 tip x pos 50 tip x force [m] [s] [N] [s] 1.02 tip x pos 50 tip x force [m] [s] [N] [s] 1.02 tip x pos 50 tip x force [m] [s] [N] [s]

24 CONSTRAINED MOTION Real contact situation kinematic constraints imposed by the environment on the end-effector motion during constrained motion, the end effector may exert dynamic forces and moments the contact force and moment may depend on the structural compliance of robot and force sensor local deformations of parts in contact (distributed contact areas), static and dynamic friction Simplifying assumptions for control perfectly rigid manipulator + environment, purely kinematic constraints perfectly rigid manipulator + compliance localized in environment

25 Rigid environment Constraint equations (holonomic constraints) ϕ(q) = 0 J ϕ (q) q = 0 J ϕ (q) = ϕ/ q: constraint Jacobian (m 6) In absence of friction J ϕ (q)δq = 0 principle of virtual works τ = J T ϕ (q)λ (contact kept with environment) h e = J T (q)τ = S f (q)λ λ = S f (q)h e S f = (ST f WS f) 1 Sf T W Problem of invariance of solution

26 J ϕ (q) q = 0 S f = J T (q)jϕ T (q) J ϕ (q)j 1 (q)j(q) q = Sf T v e = 0 Reciprocity h T e v e = 0 R(S f ) force control subspace R(S v ) velocity control subspace v e = S v (q)ν S T f (q)s v(q) = O ν = S v (q)v e Selection matrices: S f, S v, S f, S v projector in R(S f ): P f = S f S f projector in R(S v ): P v = S v S v

27 Compliant environment Partially constrained motion environment modelled as pair of rigid bodies S and R h e = S f λ dx r,s = Ch e elementary displacement with respect to equilibrium pose dx r,e = dx v + dx f dx v = P v dx r,e dx f = (I 6 P v )dx r,e = (I 6 P v )dx r,s Sf T dx r,e = Sf T dx r,s = Sf T CS fλ h e = K dx r,e K = S f (Sf T CS f ) 1 Sf T dx f = C h e C = (I 6 P v )C possibility of rigid contact along certain directions

28 NATURAL AND ARTIFICIAL CONSTRAINTS Force and velocity references chosen consistently with the environment it is not possible to impose both a force and a velocity along the same direction Kineto-static analysis of interaction natural constraints imposed by the environment along each task degree of freedom (velocity or force) artifical constraints subject to control along each task degree of freedom (force or velocity) Constraint frame O c x c y c z c simplified task description in terms of natural and artificial constraints

29 Analysis of tasks Sliding on a planar surface Natural Constraints ȯ c z ωx c ωy c fx c fy c Artificial Constraints fz c µ c x µ c y ȯ c x ȯ c y µ c z ω c z S f = S v =

30 in case of elastically compliant plane (along z c, about x c, y c ) K m = c 3,3 c 3,4 c 3,5 c 4,3 c 4,4 c 4,5 c 5,3 c 5,4 c 5,5 1

31 Peg-in-hole Natural Constraints ȯ c x ȯ c y ωx c ωy c fz c Artificial Constraints fx c fy c µ c x µ c y ȯ c z µ c z ω c z S f = S v =

32 Turning a crank Natural Constraints ȯ c x ȯ c z ωx c ωy c fy c Artificial Constraints fx c fz c µ c x µ c y ȯ c y µ c z ω c z S f = S v =

33 HYBRID FORCE/MOTION CONTROL Operational space dynamic model v e = J(q) q + J(q) q B e (q) v e + n e (q, q) = γ e h e B e = J T BJ 1 n e = J T (C q + g) B e J q Compliant environment dx r,e = P v dx r,e + C S f λ. (fixed r frame) v e = S v ν + C S f λ ν = S v(q)v e C = (I 6 P v )C common frame base frame, Ṡv = O, Ṡf = O, Ċ = O v e = S v ν + C S f λ inverse dynamics control γ e = B e (q)α + n e (q, q) + h e

34 v e = α Hybrid force/velocity control α = S v α ν + C S f f λ ν = α ν λ = f λ velocity control t α ν = ν d + K Pν (ν d ν) + K Iν (ν d (ς) ν(ς))dς 0 force control f λ = λ d + K Dλ ( λ d λ) + K Pλ (λ d λ) λ = S fḣe... λ = S f K J(q) q

35 works also in case of available stiffness Ĉ

36 Example ] ] [ Sf c 0 = 1 [ ] Pf c 0 0 = 0 1 [ Sv c 1 = 0 [ ] Pv c 1 0 = 0 0 [ ] [ K c 0 0 = 0 c 1 C c 0 0 = 2,2 0 c 2,2 [ ] 1/ 2 1/ 2 R c = 1/ 2 1/ 2 v c e = Sc v ν + C c S c f λ ]

37 if f c e = [ f c x f c y ] T, v c e = [ ȯ c x ȯ c y ] T ν = ȯ c x, λ = fc y in base frame S f = R c S c f = [ 1/ 2 1/ 2 ] [ ] S v = R c Sv c 1/ 2 = 1/ 2 C = R c C c R T c = c 2,2 hybrid force/velocity control [ 1/2 1/2 1/2 1/2 ] ν = ö c x = α ν λ = f c y = f λ

38 Rigid environment B e (q) v e + n e (q, q) = γ e h e h e = S f λ ( ) λ = B f (q) Sf T Be 1 (q)(γ e n e (q, q)) + ṠT f v e B f = (Sf T B 1 e S f) 1 B e (q) v e + S f B f (q)ṡt f v e = P(q)(γ e n e (q, q)) P = I 6 S f B f S T f B 1 e reduced order dynamic model v e = S v ν + Ṡvν 6 m independent equations B v (q) ν = S T v ( ) γ e n e (q, q) B e (q)ṡvν ( ) λ = B f (q)sf T Be 1 (q) γ e n e (q, q) B e (q)ṡvν

39 Hybrid force/velocity control γ e = B e (q)s v α v + S f f λ + n e (q, q) + B e (q)ṡvν ν = α ν λ = f λ force control f λ = λ d + K Pλ (λ d λ) position control t f λ = λ d + K Iλ (λ d (ς) λ(ς))dς 0 α ν = r d + K Dr (ṙ d ν) + K Pr (r d r)