Véronique PERDEREAU ISIR UPMC 6 mars 2013

Transcription

1 Véronique PERDEREAU ISIR UPMC mars 2013

2 Conventional methods applied to rehabilitation robotics Véronique Perdereau 2

3 Reference Robot force control by Bruno Siciliano & Luigi Villani Kluwer Academic Publishers ISBN

4 Modeling Indirect force control Direct force control Hybrid force/position control 4

5 Rehabilitation robots are machines that physically interact with a patient, somehow replacing physical therapists. These robots shall Accompany patient s movement Apply corrective forces Conventional methods are robot oriented. They need adaptation to: Human trajectories Human dynamics 5

6 Modeling

7 Actuators Actuator commands Mechanical structure Sensors Sensor readings ESA Matsushita Electric

8 Mechanical structure Open or closed kinematic chain Base Joints Revolute Prismatic Sequence of n+1 links connected by means of n joints n DOF Endeffector In accordance to the task Rigid links and joints High precision in robot positioning 8

9 Sensors Proprioceptive Position, orientation, speed Shaft absolute or incremental encoders θ Joint position readings At each joint Motor positions Tachometers Motor velocities θ Joint velocity readings 9

10 Exteroceptive Vision, tactile, force Wrist force sensor F Force and torque readings Interaction between robot and environment F = f µ External end-effector force 3 1 vector External end-effector moment 3 1 vector 10

11 Complete system Generalized force n 1 vector τ Robotic system θ θ F Joint positions n 1 vector Joint velocities n 1 vector Operational force 1 vector 11

12 Kinematic model Relationship between the joint variables and the operational variables at the end-effector G( ) X = apple p Joint positions n 1 vector X = G( ) Operational position 1 pseudo vector 12

13 Differential kinematic model Relationship between velocities Joint velocities n 1 vector J( ) V End-effector velocity 1 vector n Jacobian matrix V = J( ) 13

14 Dynamic model Relationship between the forces exerted on the structure and the joint positions, velocities and accelerations M( ) = P M( ) Inertia n n symmetric and positive definite matrix 14

15 Driving torques at the joints Torques caused by the external force and moment exerted by the end-effector when in contact with the environment (Principle of virtual work) Gravity torques M( ) = C(, ) F v J T ( )F g( ) n 1 vectors Coriolis and centrifugal torques Negligible at slow motion (rehabilitation case) Torques due to joint friction, only viscous friction in a simplified model, F v is a positive definite (diagonal) matrix of viscous friction coefficients 15

16 When contact J T F g( ) Disturbance inputs M 1 ( ) 1 s 1 s F v G( ) X 1

17 The open-loop robot can be used to assess patient s motor performance Then the robot should be transparent Transparency is the capacity for a robot to follow human movements without human perceptible resistive force. The friction prevents the robot from being transparent => Design a low friction mechanism Compensate for the friction effects in the control law 1

18 ĝ( ) g( ) M 1 ( ) - s s F v ˆF v M 1 ( ) 1 s 1 s 18

19 Reference setpoints + - Real-time controller DAC Open loop system ADC Variables to be controlled 19

20 Motion control 20

21 Find the joint torques which ensure that the end-effector attains a desired position and orientation X d Desired position and orientation Position and orientation control Actual position and orientation Open loop system X Closed loop G( ) No interaction with the environment = M( ) 21

22 2 solutions to make the errors converge to zero Find the joint torque commands X d Position and orientation errors J 1 Controller Open loop system X Direct task space feedback G( ) 22

23 Example: Proportional-derivative (PD) control = K D + KP J 1 (X d X) X d J 1 K M 1 P ( ) s s X K D G( ) 23

24 Find the operational force commands Mechanical intuition suggests that task space regulation can be achieved by designing a suitable control action which realizes an equivalent force and moment aimed at driving the end-effector toward the desired position and orientation X d Position and orientation errors Controller J T Open loop system X Direct task space feedback G( ) 24

25 Example: Proportional-derivative (PD) control = K D + J T K P (X d X) X d K M 1 P J T ( ) s s X K D G( ) 25

26 In practical tasks, the robot end-effector often has to Manipulate an object Peg into hole, screwing, assembly Perform some operation on a surface Polishing, deburring, machining Needs High precision Control of the interaction between a robot manipulator and the environment 2

27 The environment sets constraints on the geometric paths that can be followed by the end-effector Constrained motion If the task is accurately planned = Accurate model of the robot manipulator (kinematics and dynamics) + Accurate model of the environment (geometry and mechanical features) Motion control OK 2

28 But planning errors Give rise to contact forces causing a deviation of the end-effector from the desired trajectory The control system reacts to reduce such deviation Build up of the contact force Saturation of the joint actuators Breakage of the parts in contact Ensure compliant behavior during interaction 28

29 The peg-in-hole insertion example Pure position control Unpredictable forces Passive compliance Deformation of elastic bodies Active compliance Proper control 29

30 Passive control : The remote center of compliance Interpose a suitable compliant mechanical device between the manipulator end-effector and the environment Example: in a peg in hole insertion task, the gripper can be provided with a device ensuring high stiffness along the insertion direction and high compliance along the other directions (remote center of compliance) 30

31 Active control Devise a suitable interaction control strategy Indirect force control Direct force control 31

32 The patient s motor capabilities are limited in magnitude and timing and may show some pathological movement synergies The motion controlled robot attached to the limbs may then cause severe injury in not respecting patient kinematics and possible trajectories 32

33 Indirect force control 33

34 Conceive a force control strategy to manage interaction with a more or less compliant environment without requiring an accurate model Simplified environment model Frictionless and elastically compliant environment F = K e (X X e ) X + - X e K e F K e is the 3 3 contact stiffness matrix of the robot +environment 34

35 Two solutions to control interaction Achieve force control via motion control without explicit closure of a force feedback loop Implicit force feedback Impedance control Achieve force control via motion control with explicit closure of a force feedback loop Impedance control with inner motion control loop 35

36 Interaction with the environment M( ) + J T F = Open loop system + - M 1 ( ) 1 s 1 s J T F K e X e + - X G( ) In task space Ẋ = J( ) Ẍ = J( ) + J( ) J( ) M( )J 1 ( )Ẍ + J T F = 3

37 X d Position and orientation errors J 1 Controller Open loop system F X G( ) The manipulator allows some deviation due to external force 3

38 Example: Proportional-derivative (PD) control = K D + KP J 1 (X d X) X d + - J 1 K P Open loop system F G( ) X The velocity loop is not represented M( )J 1 ( )Ẍ + J T F = = K D J 1 ( )Ẋ + J T ( )K P (X d X) X =[s 2 I + JM 1 K D J 1 s + JM 1 K P J 1 ] [JM 1 K P J 1 X d JM 1 J T F ] 1 38

39 If no interaction with the environment F =0 No steady state error in task space X + JM 1 K D J 1 X + JM 1 K P J 1 X =0 independent DOF when K P = 2 nm K D =2 n M Gain tuning For each axis X i X id = 2 n s 2 +2 n s + 2 n 39

40 In case of interaction (non null contact force and moment), the control scheme no longer ensures that the end-effector reaches its desired position and orientation. X + JM 1 K D J 1 X + JM 1 K P J 1 X = JM 1 J T F The steady-state position depends on the environment rest position as well as the desired position imposed by the control system of the end-effector and on the mutual weight of the environment and end-effector compliance. X = JK 1 P J T F = JK 1 P J T K e (X d X e ) 40

41 At steady state, the manipulator under proportional action on the position and orientation error behaves as a generalized spring with respect to the force and moment, allowing small displacements in reaction to contact forces. F Equivalent closed loop system X 41

42 K 1 P The matrix gain plays the role of an active compliance, meaning that it is possible to act on the elements of the matrix so as to ensure a compliant behavior during the interaction. It is possible to decrease the active compliance so that the end-effector dominates the environment and vice versa. 42

43 It is also possible to tune the end-effector compliance with the environment compliance according to the prescribed interaction task: Compliant end-effector along a direction with high contact stiffness Steady state position coincides with the environment undeformed position End-effector sustains elastic force Stiff end-effector for high environment compliance (constrained task directions) Steady-state position very close to the desired position Environment sustains the elastic force 43

44 Impose the dynamic relationship between force and motion A robot manipulator under impedance control is described by a equivalent mass-spring-damper system with the contact force as input. F = apple f µ 1 Z(s) X = apple p 44

45 Compliance adjustment Choose the parameters K, B and J considering the task to be achieved Stiffness matrix K Precision or compliance Damping matrix B Energy dissipation Inertial matrix J Smoothness 45

46 Example X + JM 1 K D J 1 X + JM 1 K P J 1 X = JM 1 J T F F = J T MJ 1 X + J T K D J 1 X + J T K P J 1 X Equivalent mechanical system Mass matrix Damping matrix Stiffness matrix M Z = J T ( )M( )J 1 ( ) B Z = J T ( )K D J 1 ( ) K Z = J T ( )K P J 1 ( ) F = M Z X + B Z X + K Z X 4

47 In the general case, the generalized active impedance is the relationship between the contact force and moment and the end-effector position and orientation error X = apple p Z(s) =M Z s 2 + B Z s + K Z F = apple f µ The actual compliance for the end-effector orientation does not depend only on the choice of the parameters but also on the choice of the particular description used for the computation of the orientation error 4

48 Desired position and orientation X d Position and orientation errors X X Z(s) Actual position and orientation Open loop system G( ) F High stiffness leads to instability 48

49 If a force/torque sensor is available, the force measurements can be used in the control law so as to achieve a linear and decoupled impedance. The position error is related to the contact force through a mechanical stiffness or impedance of adjustable parameters The resulting impedance in the various task space directions is typically non linear and coupled. 49

50 Force/torque sensor mounted on a robot manipulator between the wrist and the endeffector + suitable interface Contact force Quantity describing the state of interaction More precise information on interaction than position due to the high gain between position variation wrt environment and force variation 50

51 X d + + Desired position and orientation Combined position set-points 1 Z(s) Position and orientation control X Actual position and orientation Open loop system G( ) F 51

52 Stiffness control J. K. Salisbury, Active stiffness control of a manipulator in Cartesian coordinates, Proceedings of the 19th IEEE Conference on Decision and Control, December 1980 pp X d + + K Position and orientation control X Actual position and orientation Open loop system G( ) F 52

53 Accommodation D. E. Withney, Force feedback control of manipulator fine motions, Journal of dynamic systems, measurement and control, June 19, pp.91-9 V d + + K 1 s Position and orientation control X Actual position and orientation Open loop system G( ) F 53

54 Pros and cons of impedance control Ensure limited values of the contact force for a given rough estimate of the environment stiffness Inadequate to ensure accurate tracking of the desired position and orientation trajectory when the end-effector moves in free space 54

55 References N. Hogan, Impedance control: a approach to manipulation. Part I: Theory, Part II: Implementation, Part III: Applications, Journal of Dynamic Systems, Measurement and Control, March 1985, vol. 10, pp.1-24 D. A. Lawrence, Impedance control stability properties in common implementations, Proceedings of the IEEE International Conference on Robotics and Automation, Philadelphia, USA, April 1988, pp

56 Adaptation of the robot impedance to the human impedance is difficult: Human impedance is modulated by CNS Human impedance is anisotropic (has properties that differ according to the direction of measurement) Loss of voluntary control creates high viscosity (resistance to motion proportional to velocity) 5

57 Direct force control 5

58 Guarded move Move in X until force exceeds F Twist about Z until torque exceeds T Explicit control 58

59 Constrained actions Peg in hole insertion, screwing, scraping Precise positioning High gain between displacement and force measure Contour following, initials engraving Day-to-day taks Opening a door, turning a crank 59

60 Controlling the contact force to a desired value thanks to the closure of a force feedback loop Regulation of the contact force to a constant desired value Fulfillment of a precise value of the contact force Operate on a force error between the desired and the measured values F d = apple fd µ d Force and moment control Open loop system F 0

61 Example: Proportional-derivative (PD) control + - K P Open loop system F The velocity loop is not represented 1

62 Steady state error due to the contact force Can be reduced by the controller gain Stability depends on Sampling frequency Manipulator and environment stiffness Force controller gain 2

63 Closure of an outer force control loop generating the reference input to the motion control scheme the robot manipulator is usually endowed with Use integral action so that force control dominates over position control Force X and moment control X d Position and orientation control Actual position and orientation Open loop system G( ) F 3

64 Example: Proportional-derivative (PD) control = K D + KP J 1 (X d X) X d + - The velocity loop is not represented + - J 1 K P Open loop system F G( ) X 4

65 Often used in industrial applications Reference V. Perdereau and M. Drouin, A new scheme for hybrid force-position control, Robotica, Cambridge University Press, 1993, vol.11, pp

66 Passive mode The robot is a limb manipulator with a non null force control for secure movement Active mode The patient does the movement himself, the robot is used for performance assessment. Transparency is achieved through zero force control Share mode is possible Some directions are handled by the robot, others are left to the patient

67 Hybrid position/force control

68 Solid space No DOF pure force control Free space No constraint pure position control Constrained space Partial freedom 8

69 X d Force and moment control Position and orientation control Combination of commands Open loop system F X 9

70 Task configuration = Set of constraints Natural constraints Mechanical and geometric characteristics Artificial constraints Desired motion and force patterns M. T. Mason, Compliance and force control for computer controlled manipulators, Transactions on Systems, Man and Cybernetics, vol. SMS-11, n, June 1981, pp

71 Turning a screwdriver Natural constraints 1 Artificial constraints x y z 2 4 v x v y v z! x! y! z 3 5 = v x v y v z! x! y! z 3 5 = 2 4 0!! f x f y f z m x m y m z 3 5 = f f x f y f z m x m y m z 3 5 =

72 Generalized surface defined in an N DOF constraint space Position constraints along the normals Force constraints along the tangents Partition of the possible hand motion DOF into two orthogonal sets Twist Wrench t = apple! v w = apple f m t T apple O3 I 3 I 3 O 3 w =0 2

73 N DOF Cartesian system defined with respect to the task geometry Constraint frame y X d environment Reference frame x 3

74 Selection of position and force controlled directions in constraint frame S = v x v y v z! x! y! z I S = =! 5 4 5! 2 4 f x f y f z m x m y m z 3 =

75 X d Each DOF in {C} is controlled by only one loop, both sets of loops act cooperatively to control each manipulator joint + - Force and I S moment controller Position and + S orientation - controller Combination of commands Open loop system F X M. H. Raibert and J. J. Craig, Hybrid position/force control of manipulators, Transactions of ASME, vol. 102, June 1981, pp

76 Various combinations of commands As torques Raibert and Craig, 1981 J 1 J T + + As forces Khatib, J T As velocities Reboulet and Robert, J 1

77 Whenever the environment is not well know or move during the task The selection matrix avoids Conflicts at actuator level The selection matrix does not avoid Force errors in non constrained directions Motion Position errors in constrained directions Mechanical constraints or contact loss

78 Constraint frame motion Example : turning a crank Many variable geometric transformations involved in the control structure 8

79 Definition of setpoints in the constraint frame Controller design Task and arm configuration dependant Kinematic instability Control structure commutation during a complete task Position controlled Force controlled Hybrid Position/force controlled 9

80 Force Xand moment control Combination of setpoints Position and orientation control Actual position and orientation Open loop system G( ) F X d Perdereau,

81 Definition of set-points in the reference frame Hierarchical control structure Easy design of control loops independent on task and arm configuration Same control structure in various phases Implementation on industrial robots 81