Rigid Manipulator Control The control problem consists in the design of control algorithms for the robot motors, such that the TCP motion follows a specified task in the cartesian space Two types of task can be defined:. tasks that do not require an interaction with the environment (free space motion); the manipulator moves its TCP following cartesian trajectories, with constraint on positions, velocities and accelerations due to the manipulator itself or the task requirements q ( t ) i Sometimes it is sufficient to move the joints from a specified value to another specified value q 0 ( t ) without following a specific geometric path i f 2. tasks that require and interaction with the environment, i.e., where the TCP shall move in some cartesian subspace while subject to forces or torques from the environment We will consider only the first type of task The control may take place at joint level l (joint i t space control) or at cartesian level l (task space control) Basilio Bona DAUIN Politecnico di Torino 005/
Joint Space control The transducer measures the value of the joint quantities (angles, displacements) and compares them with the desired ones, obtained, if necessary, from the desired cartesian quantities The Inverse Kinematics block transforms the desired task space positions and velocities into desired joint space reference values Basilio Bona DAUIN Politecnico di Torino 005/2
Task Space control pd Controller Actuator Gearbox Robot Transducer In this case, the transducer must measure the task space quantities in order to compare them with desired ones Usually this is not an easy task, since it requires environment-aware sensors; the most used ones are digital camera sensors (vision-based control) or other types of exteroceptive sensors (infra-red, ultra-sonic,...) p Basilio Bona DAUIN Politecnico di Torino 005/3
Joint Space Control Architectures Two main joint space control architectures are possible Decentralized control or independent joint control: each i-th joint motor has a local controller that takes into account only local variables, i.e., the joint position q () t and i velocity q () t i The control is of SISO type, usually based on a P, PD or PID architecture The controller is designed considering only an approximated model of the i-th joint This scheme is very common in industrial robots, due to its simplicity, modularity and robustness The classical PUMA robot architecture is shown in the following slide Centralized control: there is only one MIMO controller that t generates a command vector for joint motors; it is based on the complete model of the manipulator and takes into account the entire vector of measured positions and velocities q () t q () t Basilio Bona DAUIN Politecnico di Torino 005/4
Decentralized Control 02CFIC CY CFIDV Decentralized Joint Control task joint space space joint reference joint 2 reference controller controller 2 joint 6 reference 6 controller 6 q(t) q(t) 2 q(t) Basilio Bona DAUIN Politecnico di Torino 005/5
Decentralized Control Terminal Disk Teach pendant Accessories PUMA Control 02CFIC CY μg D/A Amplifier Motor CFIDV DLV-J EPROM RAM CPU Interface T=28 ms Reference angles T=0.875 ms Encoder μg D/A Amplifier Motor 6 T=0.875 ms Encoder COMPUTER ROBOT CONTROL Basilio Bona DAUIN Politecnico di Torino 005/6
Motor and Gearbox Model Gearbox = Geartrain Motor τ m r= N N N; ω τ m r Friction Gearbox τ r Inertia Friction Inertia N;ω m τω r m τω r m τ m Robot Basilio Bona DAUIN Politecnico di Torino 005/7
Motor and Gearbox Model i a v a R L Γ τ a a N b m i e E τ τ m ω m ; τ r β b Γ m N β m ω ; τ m r Basilio Bona DAUIN Politecnico di Torino 005/8
Losses in Geared Motor Motor side Joint side vi Ei ω τ a a Armature a Motor m r m r Gearbox circuit inertia d L i Ri η a a a a dt voltage drop d Γ ω β ω dt m m m m τ p gearbox efficiency ω τ ω τ m m Joint inertia d Γ ω β ω dt b m b m τ p Basilio Bona DAUIN Politecnico di Torino 005/9
Gearbox Model ω m τ r Joint side ρ θ θ Motor side ρ ω τ m r ρθ = ρθ ρ N r = = ρω = ρω ρ N Power in ω τ m Ideal gearbox: η = r GEARBOX ω τ τ = rτ r r ω ω = m r Power out = ηω τ m r m r m Basilio Bona DAUIN Politecnico di Torino 005/0
CC Motor Equations () MOTOR SIDE d L i = v Ri E a dt a a a a φ = Ki φ e E = kφω = K ω m ω m τ = k φ i = K i m a τ a τ m = i a K τ k k K K τ = Γ θ β θ = Γ ω β ω τ = τ τ p m m m m m m m m r m p ω τ Basilio Bona DAUIN Politecnico di Torino 005/
CC Motor Equations (2) MOTOR SIDE τ = τ r r r = ( τ τ ) m p r = m b m b m r τ Γ ω β ω = τ Γ ω β ω r r r = τ ( Γ ω β ω 2 ) m b m b m r r m b m b m τ GEARBOX SIDE r = rτ r = r( τ τ ) m p = r τ Γ ω β ω m m m m m = r τ Γ ( rω ) β ( rω ) m m m m m = rτ r 2 ( Γ ω β ω ) m m m m m Basilio Bona DAUIN Politecnico di Torino 005/2
Control Equation H ii T Hqq ( ) Cqqq (, ) Bqq ( ) gq ( ) J F = τ n n n component-wise joint torques H ( q ) q h ( q ) q q β q g ( q ) τ = τ ij j ijk j k bi i i fi ri j= j= k= n n n ( q) q H ( q) q h ( q) q q β q g ( q) τ = τ i ij j ijk j k bi i i fi j i j= k= e ri Inertial torques Coriolis o & centripetal torques Friction, gravity & external torques Basilio Bona DAUIN Politecnico di Torino 005/3
Control Equation 02CFIC CY CFIDV n n n H ( q) q H ( q) q h ( q) q q β q g ( q) τ = τ ij j ijk j k i ii i bi i fi ri j i j= k= H ( q) q β q τ τ τ τ = τ ii i bi i Mi ci gi fi r Modelled torques Disturbance torques i Basilio Bona DAUIN Politecnico di Torino 005/4
From single motor model to robot control equation q θ ω ω = q = q = r r r mi mi mi i i i i i i Gearbox transformation n τ = Hq β q Hq τ τ τ τ ω mi mi ω = Γ β τ τ τ τ bi bi Mi ci gi fi r r i i ω ω mi mi = Γ β τ bi bi di r r ri ii i bi i ij j Mi ci gi fi j i i i Structured disturbance Equation seen at the joint side Basilio Bona DAUIN Politecnico di Torino 005/5
From single motor model to robot control equation Equation seen at the motor side since and we obtain τ = τ = ( Γ ω β ω ) τ 2 r r r ri ri bi mi bi mi di i i i ( ω ) τ = τ τ = τ Γ β ω ri mi pi mi mi mi mi mi τ = τ τ = Γ Γ ω β β ω τ 2 2 r r r mi ri pi bi mi mi bi mi mi di i i i Total inertia Total friction Basilio Bona DAUIN Politecnico di Torino 005/6
From single motor model to robot control equation τ = τ τ = Γ ω β ω τ mi pi di ti mi ti mi di τ = τ τ = Γ ω B ω τ Motor side m p d t m t m Γ B t = t 2 bi mi r Γ Γ 2 bi mi r β β i i d Basilio Bona DAUIN Politecnico di Torino 005/7
From single motor model to robot control equation 02CFIC CY CFIDV τ = τ τ = Γ ω β ω τ mi pi di ti mi ti mi di τ = τ τ = Γ ω B ω τ m p d t m t m d Γ = t B ( 2 Γ r Γ ) ( 2 β r β ) bi i mi t bi i mi Joint side Basilio Bona DAUIN Politecnico di Torino 005/8
Block Diagram of open-loop CC Motor (motor side) For simplicity we drop the prime symbol for the motor side quantities, and we considerthe generic i-th motor v a E Taking the Laplace transform of the involved variables, we have R a ( sγ β ) ω () s = τ () s τ () s τ = m sl a t t m m d ia Ki τ a τ d τ τ K m r ω m τ β sγ s t t θ m () s = ω () s m s θ m K ω Basilio Bona DAUIN Politecnico di Torino 005/9
Block Diagram of open-loop CC Motor (motor side) The armature circuit it inductance is small and usually can be neglected L 0 v Ri = K ω i a a a a a ω m = τ K m τ τ m v R = K ω a a ω m Kτ τ = τ Γ ω βω m d t m t m R R R a a a v τ = Γ s K ω d t t K K β ω K τ τ τ a d t t m Basilio Bona DAUIN Politecnico di Torino 005/20
Block Diagram of open-loop CC Motor (motor side) R a β = t ω ω ω Kτ K K K since R β a t ω K τ m K ω ω m friction torque βω t m Ri K ω a a ω m Ki armature losses τ a torque τ m armature e.m.f. Basilio Bona DAUIN Politecnico di Torino 005/2
Block Diagram of open-loop CC Motor (motor side) R Γ R a t a s ω = v KK K KK m a d τ ω ω τ ω va K d τ d τ where ω θ G ( s) ω m G ( s ) = ω Kω s ( st ) m R a Γ t T = KK τ ω Ra K = d K K τ Basilio Bona DAUIN Politecnico di Torino 005/22
Matrix Formulation (joint side) Lagrange Equation and where T Hqq ( ) Cqqq (, ) Bq gq ( ) J ( qf ) = τ τ = R ( τ τ ) r m m p 2 R τ = R Kv R K q m m m a a m ω R R q B q R q B q 2 τ = ( Γ ) = ( Γ ) m p m m m m m m m m Motor side Joint side 0 0 0 0 0 0 0 0 K K K i i i R = r ; 0 τ 0 ; 0 τ ω 0 m i K = a ω R K = R ai a i 0 0 0 0 0 0 e τ Basilio Bona DAUIN Politecnico di Torino 005/23
Matrix Formulation (joint side) Then, we have Mass matrix Friction matrix Mq ( ) Fq ( ) ( 2 ) ( 2 ( ) (, ) ( )) Hq RΓ q Cqq B R K B q m m m ω m Gravity Often we use this symbol to indicate the velocity dependent terms ( ) hqq (, ) = Cqq (, ) Fq ( ) q T gq ( ) J ( qf ) e = RKv m a a Interaction u c Command input Basilio Bona DAUIN Politecnico di Torino 005/24
Matrix Formulation (joint side) Gravity and interaction No interaction T M( qq ) h ( qq, ) gq ( ) J ( q) F = u No gravity, no interaction Mqq ( ) hqq (, ) gq ( ) = uc Mqq ( ) hqq (, ) = uc Control Design Problem u =...? c e c Basilio Bona DAUIN Politecnico di Torino 005/25
Assuming, for simplicity it Decentralized Joint Control If Then Γ M ( qq ) h ( qq, ) = u c ( 2 ) 2 Γ Γ diagonal R I H( q) R R m m m m Cqqq (, ) small disturbance 2 q Fq τ = u with Γ = RJ... t d c ( Γ F) t s ω = u c t m m The model is diagonal, i.e., naturally decoupled Each joint can be controlled by local controllers Basilio Bona DAUIN Politecnico di Torino 005/26
Decentralized Joint Control Local Controller This is the proportional velocity controller or velocity compensator reference voltage vr e K D va K d K t τ d ( s ) G ω ( s) ωm s θ m Transducer T.F. ( ) (tachimetric sensor) K s K t t Basilio Bona DAUIN Politecnico di Torino 005/27
Open Loop vs Closed Loop ω () s m = () G s = ω v () s K ( st ) a ω ω () s m T = G () s = K G () s = d d ω τ () s Γ ( st ) d ω () s αk m D = G () s = ω v () s K ( s α T ) ω () s K K T () α () α m d d = G s = = G s = d ω τ () s K ( sαt) K Γ ( sαt) d ω D t t r K ω α = < K K K ω D t ω Basilio Bona DAUIN Politecnico di Torino 005/28
The closed-loop system τ d vr K d K D τ d G ( s) ωm ω s v a K d θ m G ω ( s ) Open loop ωm s θ m Closed loop Basilio Bona DAUIN Politecnico di Torino 005/29
The closed-loop system Time constant is reduced α T < T 02CFIC CY Disturbance gain is reduced K d K K d D CFIDV vr when K = d e K D va K d τ d G ω ( s) ωm Design parameter s θ m Basilio Bona DAUIN Politecnico di Torino 005/30
Position Compensator 02CFIC CY Controller K d τ d CFIDV θ r e K K P K D K θ va K t K t G ω ( s ) ωm s θ m K θ θ Basilio Bona DAUIN Politecnico di Torino 005/3
Position Compensator θ where Second-order TF with θ θ m r () s K = G () s = α 2 s s s T K () () s m = G () s = 2 2 () s d Γ ( s s α T K ) t τ Γ α K K K K K D P D P τ K = = TK R Γ ω a t KK τ ω ζ = = 2 α T K 2 α R K K K Γ Configuration dependent a D P τ t ω = K = n K K K D R P a τ Γ t Basilio Bona DAUIN Politecnico di Torino 005/32
The damping coefficient and the natural frequency are inversely proportional to the square root of the inertia moment, that may vary in time when the angles vary Γ = Γ Γ t 2 b m r Γ = H q() t b ii ( ) Since the damping coefficient and the natural frequency are often used as control specifications, we can design a controller computing the maximum inertia moment and adjusting the two gains K, K in such a way that the damping ratio ζ is P D satisfactory, e.g., no overshoot appears in the step response Γ t,max Basilio Bona DAUIN Politecnico di Torino 005/33
An alternative τ d θr Controller 2 e K sk P D va K d v () t = K e() t K e () t a P D G ω ( s) θ () s / K K s K K m D τ P D = G () s = 3 2 θ () s R Γ r a t s s αt K K R Γ P τ a t θ () s m = G () s = 4 2 τ () s Γ d t s s αt K K R Γ P τ a t ωm s θ m A zero appears Basilio Bona DAUIN Politecnico di Torino 005/34
Another alternative τ d 02CFIC CY CFIDV CA 0 OBOTIC RO θr e Controller 3 K sk P D K D v a K d G ω ( s) ω m s θ m Basilio Bona DAUIN Politecnico di Torino 005/35
Comparison Controller v () t = K K e () t K ω () t a D P D m Controller 2 v () t = K e() t K ω () t K ω () t a P D r D m Controller 3 ( ) v () t = K K e() t K K ω () t K K K ω () t a D P D D r D D D m Basilio Bona DAUIN Politecnico di Torino 005/36
Practical Issues CY 02CFIC CFIDV. Saturating actuators 2. Elasticity of the structure 3. Nonlinear friction at joints 4. Sensorsoramplifierswithfiniteband finite Basilio Bona DAUIN Politecnico di Torino 005/37
Saturating Actuators It is a nonlinear effect, difficult to be considered a-priori ut () yt () saturation ut () yt () saturation Linearityit s if u () t > u max max yt () = kut () if u ut () u min s if u() t < u min min max Basilio Bona DAUIN Politecnico di Torino 005/38
Elasticity of the structure Although we have considered rigid bodies, the elasticity is a phenomenon that limits the closed loop band We cannot design controllers that are too fast without ih taking explicitly il into consideration some sort of elastic model. Recall that t when we use a simplified model Γ θ () t k θ () t = 0 t m e m the proper structural resonance (or natural) frequency is ω r = k Γ e t Basilio Bona DAUIN Politecnico di Torino 005/39
CY CA 0CFIDV 02CFIC OBOTIC RO Elasticity of the structure Basilio Bona DAUIN Politecnico di Torino 005/40
Nonlinear friction It is a nonlinear effect between velocity and friction force 02CFIC CY CFIDV vt () stiction f () t total viscous coulomb vt () ft () Basilio Bona DAUIN Politecnico di Torino 005/4
Finite band in sensors and amplifiers 02CFIC CY CFIDV Sensors and amplifiers are often modelled d as simple gains, while in the real world they have a finite band, nonlinearities, saturations, etc. These effects must be taken into account when the simulated and real behaviours differ. Fortunately, very often the band of sensors and amplifiers is much wider than the final closed loop band of the controlled system. Basilio Bona DAUIN Politecnico di Torino 005/42
Inverse Dynamics Control Consider again the following simplified dynamic model of the robot q r v c M ( qq ) h ( qq, ) = uc MˆM ĥ ROBOT O u c q u M ( q v ) h c r c q Basilio Bona DAUIN Politecnico di Torino 005/43
Approximate Linearization Hence Mqq ( ) hqq (, ) = M ( q v ) h ( ) ( ) q v h r c q = M q M M q h q q ( ) ( ) (, ) M ( q) M = I E( q) Eq ( ) = M ( qm ) I r c hqq (, ) h = Δhqq (, ) ( ) Basilio Bona DAUIN Politecnico di Torino 005/44
Conclusions q ( q q, q v ) = q v ) η,, ( r c r c If we can cancel this part, the system becomes linear and decoupled, but unstable Structured disturbance η (,,, ) = ( ) q v ( ) qqq v Eq ( ) M q hq (, q ) Δ r c r c Approximation in Approximation in inertia modelling Coriolis modelling Basilio Bona DAUIN Politecnico di Torino 005/45
CY CA 0CFIDV 02CFIC OBOTIC RO State Variable Representation Basilio Bona DAUIN Politecnico di Torino 005/46
CY CA 0CFIDV 02CFIC OBOTIC RO Error Variable Representation This term is equivalent to the injection into the system of a structured nonlinear disturbance that can make it unstable in spite of the control design Basilio Bona DAUIN Politecnico di Torino 005/47
CY CA 0CFIDV 02CFIC OBOTIC RO Controller Design Basilio Bona DAUIN Politecnico di Torino 005/48
Controller Design 02CFIC CY CFIDV q r q h Inner Loop ROBOT q M u M, h c q v c Outer Loop Basilio Bona DAUIN Politecnico di Torino 005/49
CY CA 0CFIDV 02CFIC OBOTIC RO Inner-Outer Loop (nonlinear linearizing control) Basilio Bona DAUIN Politecnico di Torino 005/50
CY CA 0CFIDV 02CFIC OBOTIC RO Exact Linearization Basilio Bona DAUIN Politecnico di Torino 005/5
Exact Linearization h ( qq, ) 02CFIC CY CFIDV q r v c M( q) u c Inner Loop ROBOT M, h q q q r v c s s q q n s s q q n Control design Basilio Bona DAUIN Politecnico di Torino 005/52
CY CA 0CFIDV 02CFIC OBOTIC RO PD Outer Loop Control Design Basilio Bona DAUIN Politecnico di Torino 005/53
CY CA 0CFIDV 02CFIC OBOTIC RO PD Outer Loop Control Design Basilio Bona DAUIN Politecnico di Torino 005/54
PD Outer Loop Control Design hqq (, ) q r v c Inner Loop M( q) u c ROBOT M, h q q q r q q r K D K P Outer Loop Basilio Bona DAUIN Politecnico di Torino 005/55
ROBOTICA 0CFIDV 02CFICY Basilio Bona DAUIN Politecnico di Torino 005/56
h 02CFIC CY CFIDV q r v c M u c ROBOT q q q q r q r K D K P K s I Basilio Bona DAUIN Politecnico di Torino 005/57
g ( q) 02CFIC CY u c ROBOT q q CFIDV v c q r q r K D K P Outer Loop Basilio Bona DAUIN Politecnico di Torino 005/58
02CFIC CY u c = v c ROBOT q q q CFIDV q q r q r K D K P Outer Loop Basilio Bona DAUIN Politecnico di Torino 005/59
(, q ) h q r r q r vc M ( q r ) u c Inner Loop ROBOT q q OBOTIC CA 0 q rro K D q r K P Outer Loop Basilio Bona DAUIN Politecnico di Torino 005/60