RobotDoc B A S I C M O T O R C O N T R O L

Transcription

1 RobotDoc B A S I C M O T O R C O N T R O L

2 We d like to Describe a single joint of an otherwise complex robot Describe a controller for the single joint Describe how to deal with the math of multiple joints Describe which approximations we re taking in this simple description of the robot Show that we could do better if we re ready to do more complex stuff

3 Notation d F ( mv) mx Since links are physical objects with mass dt J J = moment of inertia F r Fr

4 Moment of inertia Around an axis m1 m3 m2 J N m r i1 2 i i density J volume 2 r dv

5 Parallel axis theorem J J Mr c 2 Through the center of gravity

6 Example z y x Mass M, M J x 0 l y l / l /2 Ml 3 l /2 12 l /2 J r dv x dx x Ml l l J M M yl/2

7 Sum of J y c x c z a x a b M J x a b 12 M 2 2 J y ( a c ) 12 M 2 2 J z ( b c ) ( ) M a Jtopx ( a c ) Mtop ( L) 12 2 e.g. top J J J J hand x topx side x bottom x L

8 Experimental estimation of J object torsional spring Use a photodiode and a computer to measure the frequency Requires calibration from known J f 1 2 K J

9 Experimental estimation of J object h center of gravity f 1 2 Mgh J

10 Work and power E const if Fext 0 W K s2 Fds W E, E energy 1 2 s1 P mv 2 kinetic energy dw Power P Fv dt

11 Rotational case E const if ext 0 W K 2 d W E, E energy P J 2 kinetic energy dw Power P dt

12 Single joint model end-point link/load motor reduction gears

13 Motor Let s imagine for now that it is something that generates a given torque

14 Mechanical transmission Gears Belts Lead screws Cables Cams etc.

15 Gears 1 2 N1 N2 Distance traveled is the same: r1 1 r22 Because the size of teeth is the same: N1 N2 r r 1 2

16 Furthermore r r N r N 1 2 r 1 2 No loss of energy

17 Combining N N r r # of teeth Inverse relationship between speed and torque output input 2 1 N N 2 1 TR N N mechanical parameter 1 2

18 Equivalent J N J J N N2 N2 N N J J J N2 N2 J TR J J as seen from the motor

19 In reality TR Where is the efficiency of the mechanism (from 0 to 1) is related to power, speed ratio doesn t change is also the ratio of input power vs. power at the output

20 For example input 20% output

21 Example

22 Motion conversion Start with N N1 Design TR, more torque (usually) TR 1 N N 2 1 J1 J2 2 1

23 Viscous friction Easy: viscous 2 2 eq _ viscous viscous 2 2 eq B TR TR B B TR B B TR B Coulomb friction: eq 2 eq TRF c sgn( ) 2

24 Lead screw Rotary to linear motion conversion (P=pitch in #of turns/mm or inches) x mass/load [ rad] 2Px 2Px 1 1 E E M v J rot lin load J M load (2 P) 2 motor

25 Harmonic drives From the harmonic drive website

26 Gearhead (for real) Standard (serial) Planetary

27 Example Designing the single joint Given: J J J TR 2 max eq max eq max load max Then taking into account some more realistic components: max J load 2 TR max

28 Example (continued) max J load 2 TR max P given max max get P motor power, from catalog This guarantees that the motor can still deliver maximum torque at maximum speed

29 More on real world components Efficiency Eccentricity Backlash Vibrations To get better results during design mechanical systems can be simulated

30 Control of a single joint microprocessor actuator mechanics sensor reference amplifier

31 Components Digital microprocessor: Microcontroller, processor + special interfaces Amplifier (drives the motor) Turns control signals into power signals Actuator E.g. electric motor Mechanics/load The robot! Sensors For intelligence

32 Actuators Various types: AC, DC, stepper, etc. DC Brushless With brushes We ll have a look at the DC with brushes, simple to control, widely used in robotics

33 DC-brushless

34 DC with brushes

35 Modeling the DC motor Speed-torque and torque-current relationships are linear

36 In particular No-load speed current-torque speed-torque stall torque

37 Real numbers!

38 Electrical diagram Ra La V arm Rl + Eg E g () t K E

39 Meaning of components R a V arm R l E g L a Armature resistance (including brushes) Armature voltage Losses due to magnetic field Back EMF produced by the rotation of the armature in the field Coil inductance

40 We can write V R I L I () t K arm a a a a E for R R l a which is the case at the frequency of interest, and we also have KI T a

41 On torque and current F ilb F qv B lorentz

42 Thus for many coils

43 Back to motor modeling ( J J ) ( t) B( t) M L f gr J M J L f gr Torque generated Inertia of the motor Inertia of the load Friction Gravity

44 Furthermore V R I L I () t K arm a a a a E KI ( J J ) ( t) B( t) T M L f gr a

45 Consequently Ra K V E arm I L L I L a a a a a KT B f gr J M J L J M J L JM JL A linear system of two equations (differential) Q: can you write a transfer function from these equations? Q: can you transform the equations into a block diagram?

46 By Laplace-transforming Varm ( s) ( s) KE Varm ( s) Ra Ia( s) La Ia( s) s ( s) KE Ia( s) R L s KI Varm ( s) ( s) KE K ( J J ) ( s) s B( s) R L s T T M L f gr a a a a a

47 and finally () s K L J V s s R J L B L J s K K R B L J T a T 2 arm( ) [( a T a ) a T ] ( T E a ) a T Considering gravity and friction as additional inputs

48 Block diagram f gr Varm 1 I - a 1 - R a sl a K T B sj T K E

49 Analysis tools Control: determine Va so to move the motor as desired Root locus Pole placement Frequency response Etc.

50 First block diagram * - A V Km (1 s ) (1 s ) a m 1 s ˆ K p H open _ loop A K K m 1 s 1 s s a m p

51 Root locus A K K H K AK K m p open _ loop m p 1sa 1sm s j 1 a 1 m

52 Changing K small K t higher K t

53 Let s add something, second diagram * - - A V Km (1 s ) (1 s ) a m 1 s ˆ K g K p H open _ loop AKm( K p skg ) (1 s )(1 s ) s a m

54 Analysis H open _ loop AK K m p Kg (1 s ) K (1 s )(1 s ) s a m p K AK K m p Z feedback K K g p

55 Root locus (case 1) j 1 a 1 m K K p g K K p g 1 m

56 Root locus (case 2) j 1 a K K p g 1 m K K p g 1 m

57 Overall f gr * A Varm 1 Ia s a - R a sl a K T B sj T s K E K g K p

58 Error and performance d M() s s KT ( R sl )( B sj ) K K a a T E T closed loop (position) 1 ( s) ( s) s 1 () s () s s 1 1 ( sk ) p s () s A M () s 1 s a A 1 M ( s ) K 1 s a closed loop (velocity) g

59 finally lim sh( s) lim h( t) s0 t 1 d s () s d lim s ( s) lim s s s0 s s0 1 1 ( sk ) p s d K p For zero error K must be 1 or the control structure must be different

60 Same line of reasoning final gr T R a AK K p Final value due to friction and gravity R K R gr a gr a max p T p AKTmax AK K K pmin AK gr T R a max

61 PID controller f gr * K p sk d V arm 1 Ia K i s - R a sl a K T K E B sj T s

62 PID controller We now know why we need the proportional We also know why we need the derivative Finally, we add the integral Integrates the error, in practice needs to be limited

63 Interpreting the PID Proportional: to go where required, linked to the steady-state error Derivative: damping Integral: to reduce the steady-state error

64 Global view microprocessor actuator mechanics sensor reference amplifier OK OK

65 About the amplifiers Linear amplifiers H type T type PWM (switching) amplifiers

66 Let s consider the linear as a starting point V cc

67 H-type The motor doesn t have a reference to ground (floating) It s difficult to get feedback signals (e.g. to measure the current flowing through the motor)

68 T-type V cc V cc

69 On the T-type Bipolar DC supply Dead band (around zero) Need to avoid simultaneous conduction (short circuit)

70 Things not shown Transistor protection (currents flowing back from the motor) Power dissipation and heat sink Cooling Sudden stop due to obstacles High currents current limits and timeouts

71 T-type V cc I c R V transisor cc R motor V cc

72 PWM amplifiers P V I ce c same current

73 PWM signal P V I ce c Transistors either on or off When off, current is very low, little power too When on, V is low, working point close to (or in) saturation, power dissipation is low

74 Comparison 12W for a 6A current using a switching amplifier 72W for a corresponding linear amplifier

75 Why does it work? () s K L J V s s R J L B L J s K K R B L J T a T 2 arm( ) [( a T a ) a T ] ( T E a ) a T In practice the motor transfer function is a lowpass filter T s with f s f e ( f s 100 f e ) Switching frequency must be high enough (s=switching, e=electric pole)

76 PWM signal V cc T s V cc T switching T s

77 Feedback in servo amplifiers V cc - V in + V cc Voltage feedback amplifier

78 Operating characteristic V out RL R 1 RL R 2 AV in R R 2 1 I L

79 We ve already seen this V in A 1 s a arm - R a 1 sl a Ia 1 V K T B sj T K E () s K L J A V s s R J L B L J s K K R B L J s T a T v 2 in( ) [( a T a ) a T ] ( T E a ) a T (1 a)

80 Current feedback - V in + V out

81 Current feedback V out RL R 1 RL R 2 R R 2 1 AV in I L

82 Motor driven by a current amplifier V in A 1 s a Ia 1 K T B sj T () s KTAi V ( s) ( sj B)(1 s ) in T a

83 Back to the global view microprocessor actuator mechanics sensor reference amplifier OK OK OK

84 Sensors Potentiometers Encoders Tachometers Inertial sensors Strain gauges Hall-effect sensors and many more

85 Potentiometer - r thin resistive film V out r V R cc V cc V out + Simple but noisy Requires A/D conversion Absolute position (good!)

86 Note TR turns Encoder Motor Gearbox N1 2 1 TR N turn The resolution of the sensor multiplied by TR N (most of the time) N N1 N N2 N

87 Encoder Absolute Incremental

88 Absolute encoder phototransistors LEDs transparent motor x x x x x x x x x x x x motor shaft 13 bits required for degrees opaque

89 Incremental encoder Disk single track instead of multiple No absolute position Usually an index marks the beginning of a turn

90 Incremental encoder output TTL t t Sensitive to the amount of light collected The direction of motion is not measured

91 Two-channel encoder 2 channels 90 degrees apart (quadrature signals) allow measuring the direction of motion A B t t

92 Moreover There are differential encoders Taking the difference of two sensors 180 degrees apart Typically A, B, Index channel A, B, Index (differential) A counter is used to compute the position from an incremental encoder

93 Increasing resolution Counting UP and DOWN edges X2 or X4 circuits

94 Absolute position A potentiometer and incremental encoder can be used simultaneously: the potentiometer for the absolute reference, and the encoder because of good resolution and robustness to noise

95 Analog locking Use digital encoder as much as possible Get to zero error or so using the digital signal When close to zeroing the error: Switch to analog: use the analog signal coming from the photodetector (roughly sinusoidal/triangular) Much higher resolution, precise positioning

96 Tachometer Use a DC motor The moving coils in the magnetic field will get an induced EMF In practice is better to design a special purpose DC motor for measuring velocity Ripple: typ. 3%

97 As already seen e e ripple

98 Measuring speed with digital encoders Frequency to voltage converters Costly (additional electronics) Much better: in software Take the derivative (for free!) v( kt ) p( kt ) p(( k 1) T) T

99 Inertial sensors Accelerometers: position sensor K K Ma 2Kx a 2Kx M a

100 Gyroscopes Quartz forks driver (causing oscillations) amp V F F 2m V

101 Strain gauges Principle: deformation R (resistance) Example: conductive paint (Al, Cu) The paint covers a deformable non-conducting substrate L R L, A const R A conductivity

102 Reading from a strain gauge V cc strain gauge R 2 a b R 1 balance V cc R R R R V 1 2 g b ab 0 V f ( R ) ab g

103 Hall-effect sensors + - V out charges B F if electrons F qv B lorentz

104 Example Measuring angles (magnetic encoders)

105 Back to the global view microprocessor actuator mechanics sensor reference amplifier OK OK OK OK

106 Microprocessors Special DSPs for motion control Some are barely programmable (the control law is fixed) Others are general purpose and they are mixed mode (analog and digital in a single chip)

107 Example DSP 16 bit ALU and instruction set PWM generator (simply attach this to either T or H amplifier) A/D conversion CAN bus, Serial ports, digital I/O Encoder counters Flash memory and RAM on-board Enough of all these to control two motors (either brush- or brushless)

108 Kinematics Kinematics: Given the joint angles, compute the hand position x Inverse kinematics: ( q) Given the hand position, compute the joint angles to attain that position q 1 ( x) As usual, inverse problems might be troublesome! RA 2005

109 Kinematics Inverting: Geometrically: closed form solution exists in certain cases By minimization: J 1 2 x ( q) q arg min 2 q J Kinematic redundancy: more joints than constraints E.g. a rigid body (hand) in space is described by 6 numbers (position + orientation). A robot (or human) arm might have 7 or more joints (degrees of freedom) RA 2005

110 Representing kinematics Representing rotations and translations between coordinate frames of reference A v [?] B v x B x A z A z B y A y B v [ x y z ] v R v B A A A A A B A B B B B B x R x R [1,0,0] A A B A T B B B B RA 2005

111 Rotation matrix R ( R ) I ( R ) ( R ) R A A T A T A 1 B B B B B A Orthogonal matrix x A Example: rotation along the Z axis x B A x B cos sin 0 cos sin 0 sin cos x B x A z A y z A y B B RA 2005

112 More simple rotations Example: rotation along the Y axis cos 0 sin sin 0 cos cos sin 0 sin cos Example: rotation along the X axis RA 2005

113 Representing 3D rotations Sequences of elementary rotations Euler angles: z, y, z or z, x, z Roll, pitch, yaw angles: z, y, x Vector (axis of rotation) and angle RA 2005

114 Roto-translation Rotation combined with translation v R v o A A B A B B x B z A B o x A B z A ya y B RA 2005

115 Homogeneous representation To make things uniform v R v o A A B A B B A A A B v RB o B v v T v dim( v) 4 A A B B RA 2005

116 Clearly Composition of transforms Inverse of a rototranslation v T T v C A A A B C B C A A 1 A T A T A RB ob RB RB ob A T 1 B B T A RA 2005

117 Direct kinematics 3 e T ( q ) T ( q ) 0 n ( x, y, z) T ( 1, 2, 3, 4) (0,0,0) T e q q q q x( q) orientation ( q) n n RA 2005

118 Conventions For placing the reference frames on each link Denavit-Hartenberg Many times DH parameters are given for a manipulator (and various useful equations are also given wrt DH convention) RA 2005

119 Inverse kinematics Direct approach Geometric Minimization Neural network, learning RA 2005

120 Inverse kinematics Direct approach Try solving: x NL ( q, q, q, q ) x y NL ( q, q, q, q ) y z NL ( q, q, q, q ) z for q, q, q, q RA 2005

121 Geometric approach For certain manipulator the solution exists in close form Decomposable structures (e.g. translation and rotations can be handled separately) Rotations follow certain rules Many industrial manipulators were designed with inverse kinematics in mind RA 2005

122 Minimization Find the solution to: 1 2 J x ( q) q arg min 2 q Neural network/learning: ( qx, ) 1 J Approximate the inverse out of a family of functions (NN approach) starting from examples RA 2005

123 What about velocity? Jacobian matrix x dx1 dx1 dq1 dq m dx ( q) dt dxn dxn dq 1 dqm dq dt dx dt J( q) dq dt RA 2005

124 Note on representing velocities If x is: RA 2005 x ( x, y, z,,, ) Position + Euler angles v ( vx, vy, vz,,, ) Euler angles derivatives do not have any clear physical meaning v ( vx, vy, vz, ω) Angular velocity (rate of rotation along the axis

125 Anyway Just make sure the representation and the equations are consistent v ( v, v, v,,, ) J x y z r v ( v, v, v, ω) J x y z v RA 2005

126 Jacobian Formula Given the DH representation of transformations Considering only rotational joints RA 2005 J i J [ J J J ] for n joints o v z i o o z i 1 2 p E, i p p p o o o E, i E i 0 n 1 o p i 2 o p E 3 e

127 Having written 0 T i x y z p i i i i T T T T i 1 i 1 2 i RA 2005

128 When J is invertible Can compute the joint velocities to obtain a certain hand velocity q J 1 x If n>6, redundancy: q J x ( I J J) k k is a constant vector RA 2005

129 Troubles Even if n6 there are many situations where J cannot be inverted (singularities) Movement singularities (chain of rotations) J not invertible because certain elements go to zero RA 2005

130 Resolved rate controller * s - q 1 J 1 s q Joint controllers s Sensors RA 2005

131 Static Relationship between forces and torques dx d T q τ Jdq d T x F d d J T T T q τ q F T τ J F Imagining the integrals where appropriate RA 2005

132 Another idea τ J T F Use this equation to design a force controller: Given F compute the torques to drive the joints RA 2005

133 Dynamics Two methods to derive the equation of motion (differential equations) Newton-Euler Lagrange formalism RA 2005

134 Newton-Euler Start from: F τ d dt d dt ( m v) ( I ω) d F ( m v) dt d τ ( I ω) ω ( I ω) I ω dt kinematics Write down every equation (6): find the angular velocity and I with respect to a base frame RA 2005

135 Lagrange formulation Lagrange equations: L K P x d L L x x ( q q, t) 1 F q i dt q i q i N External forces (no potential) 1 T 1 T K mvv ω Iω 2 2 RA 2005

136 For a manipulator Take the joint angles as variable, write the position x of the links, write down K, P and the external forces External forces (control) Inertia (generalized) τ M( q) q h( q, q) q g( q) Coriolis, centrifugal effects Gravity RA 2005

137 Complexity Newton-Euler: on ( ) Lagrange: 4 on ( ) Estimation Kinematics just measure the params Dynamics estimate from data RA 2005

138 Dynamics Direct dynamics: ( t) q( t) Simulation (integrate the equations Runge-Kutta, Euler, etc.) Inverse dynamics: q( t) ( t) RA 2005

139 Dynamics and control Case 1: parameters are such that feedback gain at each joint is >> gravity, Coriolis, centrifugal, disturbances, etc. Case 2: feedback in not enough for high-speed, precision, etc. compensation is required RA 2005

140 Case 1 Approx behavior: * Aq Bq k[ q q ] 0 Can design k or a PID controller to make this system behave as desired RA 2005

141 Case 2 Let s imagine we know all the parameters with a certain precision: τ M( q) q h( q, q) q g( q) τcontrol M( q) u h( q, q) q g( q) M( q) q h( q, q) q g( q) M( q) u h( q, q) q g( q) M( q) q M( q) u u q k ( q q) k ( q q) * * * d p RA 2005

142 Case 2 (continued) q u u q k ( q q) k ( q q) * * * d p e q q k ( q q) k ( q q) * * * d p * e q q 0 kd kp e e e t Appropriate design of the gains can get arbitrary exponential behavior of the error RA 2005