Constrained Motion Strategies for Robotic Systems

Transcription

1 Constrained Motion Strategies for Robotic Systems Jaeheung Park Ph.D. Candidate in Aero/Astro Department Advisor: Professor Khatib Artificial Intelligence Laboratory Computer Science Department Stanford University Ph.D. Defense p.1/42

2 Contributions Robust force control for multi-body robotic systems High fidelity haptically augmented teleoperation Development of generalized motion/force control structure for multi-link multi-contact Contact force consistent motion/force control strategy for humanoid systems Ph.D. Defense p.2/42

3 Motion Control The operational space formulation (Khatib 87) Equations of motion in joint space F A q + b(q, q) + g(q) = Γ x Ph.D. Defense p.3/42

4 Motion Control The operational space formulation (Khatib 87) Equations of motion in joint space F A q + b(q, q) + g(q) = Γ x Dynamics in the operational space Λẍ + µ(q, q) + p(q) = F Ph.D. Defense p.3/42

5 Motion Control The operational space formulation (Khatib 87) Equations of motion in joint space F A q + b(q, q) + g(q) = Γ x Dynamics in the operational space Control force Λẍ + µ(q, q) + p(q) = F F = ˆΛf + ˆµ(q, q) + ˆp(q) Ph.D. Defense p.3/42

6 Motion Control The operational space formulation (Khatib 87) Equations of motion in joint space F A q + b(q, q) + g(q) = Γ x Dynamics in the operational space Control force Λẍ + µ(q, q) + p(q) = F F = ˆΛf + ˆµ(q, q) + ˆp(q) The operational space formulation was extended to handle multiple points (Russakow, Khatib, and Rock 1995) x = [x 1, x 2,..., x m ] T Ph.D. Defense p.3/42

7 Motion/Force Control Control force Force Control F = ˆΛf + ˆµ(q, q) + ˆp(q) + ˆf c Motion Control Compliant frame selection matrix Ω f and Ω m ( Raibert and Craig 81, Khatib 87 ) Ph.D. Defense p.4/42

8 Motion/Force Control Control force Force Control F = ˆΛf + ˆµ(q, q) + ˆp(q) + ˆf c Motion Control Compliant frame selection matrix Ω f and Ω m ( Raibert and Craig 81, Khatib 87 ) f = Ω f f f + Ω m f m Ph.D. Defense p.4/42

9 Motion/Force Control Control force Force Control F = ˆΛf + ˆµ(q, q) + ˆp(q) + ˆf c Motion Control Compliant frame selection matrix Ω f and Ω m ( Raibert and Craig 81, Khatib 87 ) f = Ω f f f + Ω m f m With environmental model (stiffness k s ), ẍ m = f m 1 k s fc = f f Ph.D. Defense p.4/42

10 Motion/Force Control Framework Force Sensor ˆf c f d x d Force Control MotionControl f f f m Ω f Ω m Σ f ˆΛ ˆµ + ˆp Σ J T Σ Γ Robot In contact f c q, x V d Null Space Control Γ 0 N T { ẍ m = f m 1 k s fc = f f Ph.D. Defense p.5/42

11 Motion/Force Control Framework Force Sensor f d x d Force Control (Lag Compensator) MotionControl f f f m Ω f Ω m Σ f ˆΛ ˆf c ˆµ + ˆp Σ J T Σ Γ Robot In contact f c q, x V d Null Space Control Γ 0 N T { ẍ m = f m 1 k s fc = f f Ph.D. Defense p.5/42

12 Force Control Result One point contact with hard surface Force in vertical direction[n] Desired Contact Force Measured Contact Force Time [sec] Lag compensator Ph.D. Defense p.6/42

13 Motion/Force Control Framework Force Sensor f d x d Force Control (Lag Compensator) MotionControl f f f m Ω f Ω m Σ f ˆΛ ˆf c ˆµ + ˆp Σ J T Σ Γ Robot In contact f c q, x V d Null Space Control Γ 0 N T { ẍ m = f m 1 k s fc = f f Ph.D. Defense p.7/42

14 Motion/Force Control Framework Observer Force Sensor f d x d Force Control (Full State Feedback) f f MotionControl f m Ω f Ω m Σ f ˆΛ ˆf c ˆµ + ˆp Σ J T Σ Γ Robot In contact f c q, x V d Null Space Control Γ 0 N T { ẍ m = f m 1 k s fc = f f Ph.D. Defense p.7/42

15 Control with Estimator f c,desired r k L 1 Σ Σ f f System f c Force Sensor ˆp k L r ˆx k Observer ˆx k ˆp k L r L 1 f c f c,d r k state estimate - ˆf c and ˆ f c input disturbance estimate a full state feedback gain obtained by Pole Placement Method a scaling factor to compute reference input contact force desired contact force reference input Ph.D. Defense p.8/42

16 Force Control Result One point contact with hard surface Force in vertical direction[n] Desired Contact Force Measured Contact Force Time [sec] Full state feedback control Ph.D. Defense p.9/42

17 Force Control Result One point contact with hard surface Force in vertical direction[n] Desired Contact Force Lag Compensator Full state feedback Time [sec] Comparison Ph.D. Defense p.10/42

18 Teleoperation f contact x master Ph.D. Defense p.11/42

19 Teleoperation Scheme Observer Force Sensor Haptic Device x haptics K vir Σ x robot f d x d V d Force Control (Full State Feedback) Motion Control Null Space Control f f f m Γ 0 Ω f Ω m Σ f ˆΛ ˆf c ˆµ + ˆp Σ N T J T Σ Γ Robot In contact f c q, x Ph.D. Defense p.12/42

20 Teleoperation Scheme Stiffness Estimator(k s ) f d, fm, ˆfc Observer Force Sensor Haptic Device x haptics K vir Σ x robot f d x d V d Force Control (Full State Feedback) Motion Control Null Space Control f f f m Γ 0 Ω f Ω m Σ f ˆΛ ˆf c ˆµ + ˆp Σ N T J T Σ Γ Robot In contact f c q, x Ph.D. Defense p.12/42

21 System Setup Master Haptic Device: PHANTOM 1.0 SensAble Stanford Mobile Platform: PUMA560 on XR4000 Ph.D. Defense p.13/42

22 Experimental Results End-effector controlled by teleoperation Base motion is controlled in the null-space Ph.D. Defense p.14/42

23 Motion/Force Tracking Position in vertical direction[m] Haptic Device Time [sec] Ph.D. Defense p.15/42

24 Motion/Force Tracking Position in vertical direction[m] Haptic Device End-Effector Time [sec] Ph.D. Defense p.16/42

25 Motion/Force Tracking Position in vertical direction[m] Haptic Device End-Effector Time [sec] Desired Contact Force Force in vertical direction[n] Time [sec] Ph.D. Defense p.17/42

26 Motion/Force Tracking Position in vertical direction[m] Haptic Device End-Effector Time [sec] Force in vertical direction[n] Desired Contact Force Measured Contact Force Time [sec] Ph.D. Defense p.18/42

27 Motion/Force Tracking Estimated Environment Stiffness [N/m] Time [sec] Force in vertical direction[n] Desired Contact Force Measured Contact Force Time [sec] Ph.D. Defense p.19/42

28 Juliet Setting a Dinner Table Ph.D. Defense p.20/42

29 Multiple Contacts Compliant frame selection matrices is not general enough to describe multi-contact Ph.D. Defense p.21/42

30 Multiple Contacts Compliant frame selection matrices is not general enough to describe multi-contact Special selection matrix Ω f (n) and Ω m (n) Featherstone, Sonck, and Khatib (1997) Ph.D. Defense p.21/42

31 Multiple Contacts Compliant frame selection matrices is not general enough to describe multi-contact Special selection matrix Ω f (n) and Ω m (n) Featherstone, Sonck, and Khatib (1997) Bruyninckx and Schutter (1998) Ph.D. Defense p.21/42

32 Multi-link Multi-contact 0 B B B B B Λ 11 Λ Λ 1m Λ 21 Λ Λ 2m Λ m1 Λ m2... Λ mm 1 C C C C C C A 0 B B B B B ẍ 1 ẍ 2... ẍ m 1 C C C C C C A + µ + p + f c = 0 B B B B B F 1 F 2... F m 1 C C C C C C A Ph.D. Defense p.22/42

33 Multi-link Multi-contact The spacial selection matrices will be defined by the contact normal directions of all the operational points : Ω f (n 1, n 2,..., n l ) and Ω m (n 1, n 2,..., n l ) F = ˆΛf + ˆµ + ˆp + ˆf c f = Ω f f f + Ω mf m Ph.D. Defense p.23/42

34 Motion/Force Control Framework Observer Force Sensor f d x d Force Control (Full State Feedback) f f MotionControl f m Ω f Ω m Σ f ˆΛ ˆf c ˆµ + ˆp Σ J T Σ Γ Robot In contact f c q, x V d Null Space Control Γ 0 N T { ẍ m = f m 1 k s fc = f f Ph.D. Defense p.24/42

35 Three contact experiment Two contact at the end-effector and one contact at the third link 0 Desired Force Force[N] Time [sec] Contact force measurement Ph.D. Defense p.25/42

36 Three contact experiment Two contact at the end-effector and one contact at the third link 0 Desired Force 1st Contact Force Force[N] Time [sec] Contact force measurement Ph.D. Defense p.25/42

37 Three contact experiment Two contact at the end-effector and one contact at the third link 0 Desired Force 1st Contact Force 2nd Contact Force Force[N] Time [sec] Contact force measurement Ph.D. Defense p.25/42

38 Three contact experiment Two contact at the end-effector and one contact at the third link 0 Desired Force 1st Contact Force 2nd Contact Force 3rd Contact Force Force[N] Time [sec] Contact force measurement Ph.D. Defense p.25/42

39 Three contact experiment with motion Ph.D. Defense p.26/42

40 Humanoid robot Many humanoid systems have been built. Honda ASIMO, Sony SDR, HRP, and KHR Controls are still limited mostly focused on walking walking and manipulation are treated as separate problems Ph.D. Defense p.27/42

41 Humanoid Research in the group Whole-body control framework Whole-body dynamic behavior and control of human-like robots ( Khatib, Sentis, Park, Warren 2004 ) SAI simulation environment Dynamic simulation -Efficient recursive algorithm for the operational space inertia matrix of branching mechanisms (Chang, Khatib 2001) -A framework for multi-contact multi-body dynamic simulation and Haptic Display (Ruspini, Khatib 2000) SAI graphics engine and programming environment - Conti, Pashchenko Learning from human motion Simulating the Task-level Control of Human Motion: A Methodology and Framework for Implementation (De Sapio, Warren, Khatib, and Delp 2005 ) Control implementation on ASIMO - Thaulad Ph.D. Defense p.28/42

42 Humanoid robot characteristics Under-actuated system in free space Non fixed base Contacts are necessary to support the system Ph.D. Defense p.29/42

43 Constrained dynamics of robot A(q) q + b(q, q) + g(q) + J T c f c = Γ f c Ph.D. Defense p.30/42

44 Constrained dynamics of robot A(q) q + b(q, q) + g(q) + J T c f c = Γ By treating the contacts as constraints, ẍ c = 0 and ẋ c = 0, f c = J T c (q)γ µ c (q, q) p c (q) constrained Ph.D. Defense p.30/42

45 Constrained dynamics of robot A(q) q + b(q, q) + g(q) + J T c f c = Γ By treating the contacts as constraints, ẍ c = 0 and ẋ c = 0, f c = J T c (q)γ µ c (q, q) p c (q) A(q) q + b(q, q) + g(q) J T c (µ c (q, q) + p c (q)) constrained = (I J T c J T c )Γ Ph.D. Defense p.30/42

46 Operational Space Dynamics Constrained Joint Space Dynamics A q + b + g J T c (µ c + p c ) = (I J T c J T c )Γ F constrained Ph.D. Defense p.31/42

47 Operational Space Dynamics Constrained Joint Space Dynamics A q + b + g J T c (µ c + p c ) = (I J T c J T c )Γ F Constrained Operational space dynamics Λẍ + µ(q, q) + p(q) = F constrained Ph.D. Defense p.31/42

48 Operational Space Dynamics Constrained Joint Space Dynamics A q + b + g J T c (µ c + p c ) = (I J T c J T c )Γ F Constrained Operational space dynamics Λẍ + µ(q, q) + p(q) = F Control Force constrained F = ˆΛf + ˆµ + ˆp Ph.D. Defense p.31/42

49 Control Torque The relation between joint torque and force Γ = J T F F constrained Ph.D. Defense p.32/42

50 Control Torque The relation between joint torque and force F = J T Γ F constrained Ph.D. Defense p.32/42

51 Control Torque The relation between joint torque and force F = J T Γ F Torques at virtual joints are zero. F = J T S T Γ actuated constrained Ph.D. Defense p.32/42

52 Control Torque The relation between joint torque and force F = J T Γ F Torques at virtual joints are zero. F = J T S T Γ actuated The Control torque is constrained (Γ actuated ) task = J T S T F Ph.D. Defense p.32/42

53 Constrained dynamics of robot A q + b + g J T c (µ c + p c ) = (I J T c J T c )Γ f c = J T c Γ µ c (q, q) p c (q) constrained Ph.D. Defense p.33/42

54 Constrained dynamics of robot A q + b + g J T c (µ c + p c ) = (I J T c J T c )Γ f c = J T c Γ µ c (q, q) p c (q) f c,i min < f c,i < f c,i max f c Ph.D. Defense p.33/42

55 Reaction Force Control Γ actuated = (Γ actuated ) task Reaction forces are given by f c = J T c S T (Γ actuated ) task µ c (q, q) p c (q) Ph.D. Defense p.34/42

56 Reaction Force Control Γ actuated = (Γ actuated ) task +(Γ actuated ) contact Reaction forces are given by f c = J T c S T (Γ actuated ) task µ c (q, q) p c (q) Modify control torque if they exceed the boundaries, f c fc (Γ actuated ) contact = J T c S T f c Ph.D. Defense p.34/42

57 Standing on two feet Gravity compensation with reaction force control Ph.D. Defense p.35/42

58 Balancing Controlled operational space coordinates Position of Center of Mass AND Orientation of head, chest, and hip Ph.D. Defense p.36/42

59 Walking Controlled operational space coordinates Position of Center of Mass AND Orientation of head, chest, and hip Left or right foot in one foot supporting Ph.D. Defense p.37/42

60 Jumping Controlled operational space coordinates Position of Center of Mass AND Orientation of head, chest, and hip Both feet at no contact Ph.D. Defense p.38/42

61 Climbing Ladder Controlled operational space coordinates Position of Center of Mass AND Orientation of head, chest, and hip Hands or feet in climbing Ph.D. Defense p.39/42

62 Manipulation combined with walking Controlled operational space coordinates Position of Center of Mass AND Orientation of head, chest, and hip Position and Force control of right hand Ph.D. Defense p.40/42

63 Conclusion Robust force control was developed for multi-body robotic systems. High performance was demonstrated using full state feedback with Kalman estimator (improvement over lag compensator). Haptic teleoperation was implemented. High fidelity transparent teleoperation was achieved by using force control with stiffness estimation. Motion/force control structure was generalized. Composition of decoupled motion/force control was achieved by constructing selection matrices over multiple control points. Contact force consistent control strategy was developed for humanoid systems This enables implementation of motion/force control to non fixed base robots. Ph.D. Defense p.41/42

64 Acknowledgement Advisor Prof. Khatib Committee Prof. Gerdes, Prof. Rock, Prof. Thrun, Prof. Tomlin Research Group (Past & Present) Oliver Brock, Francois Conti, Rui Cortesao, Kyong-Sok Chang, Vince De Sapio, Bob Holmberg, Kyoobin Lee, Tine Lefebvre, Irena Pashchenko, Anna Petrovskaya, Luis Sentis, Dongjun Shin, Jinsung Kwon, Peter Thaulad, James Warren, Mike Zinn Family Yunhyung, Kevin, Bryan, My family and In-laws in Korea KAASA members & friends Ph.D. Defense p.42/42