L ECTURE 10: F EEDBACK - BASED C ONTROL 2 I NSTRUCTOR : G IANNI A. D - PowerPoint PPT Presentation

16-311-Q I NTRODUCTION TO R OBOTICS L ECTURE 10: F EEDBACK - BASED C ONTROL 2 I NSTRUCTOR : G IANNI A. D I C ARO

FOLLOWING A PATH (PI CONTROLLER) Goal: Follow a general parametric curve (x*(t),y*(t)) on the plane (e.g., generated by a path planner or resulting from sensors) Control inputs: 𝛿 (t), v(t) Current known state: Pose [x y 𝜾 ](t) and the next point in the path (in the {W} frame) Error vector: Distance and angle from next point in the path Pursuit strategy: The next point (x*(t),y*(t)) on the path defines the goals state and keeps moving along the goal path, next point with the robot keeping following it. The problem becomes therefore equivalent to a sequence of “moving to a goal point” problems, arriving with non-zero velocity y Carrot on a stick! {W} 2 x

FOLLOWING A PATH (PI CONTROLLER) Velocity proportional to the linear distance 𝜍 Control inputs: 𝛿 (t), v(t) from the pursuit point, that should be Current known state: Pose [x,y, 𝜾 ](t) and the maintained at a defined distance d* next point in the path (in the {W} frame) ( x ∗ ( t ) − x ( t )) 2 + ( y ∗ ( t ) − y ( t )) 2 − d ∗ p ε ρ ( t ) = Error vector: Distance (with some o ff set) and angle from next point in the path The linear velocity PI controller aims to keep the robot at the distance d* from the next point. The integral term guarantees a finite (x*(t),y*(t)) velocity when the error on distance goes to 0 d* Z v ∗ ( t ) = K ρ ε ρ ( t ) + K i ε ρ ( t ) dt Steering proportional to the angular di ff erence between robot and target point y ⇣ y ∗ − y ⌘ θ ∗ = atan2 x ∗ − x {W} γ ( t ) = K θ ( θ ∗ θ ) , K θ > 0 3 x

FOLLOWING A PATH (PI CONTROLLER) Robot trajectory ~ stationary Target path d ∗ = 0 . 05 , K ρ = 4 , K i = 2 , K θ = 5 4

MOVING TO A POSE (P CONTROLLER) Goal: Move to a specific pose W (x*, y*, 𝜄 *) in the plane Control inputs: v(t) and 𝛿 (t) Current known state: Current pose [x y 𝜾 ](t) and goal pose (in the {W} frame) Error vector: Distance from the goal, orientation with respect to the goal pose orientation Without loss of generality, the goal pose {G} is assumed to be coinciding with the world inertial frame {W}. Therefore, moving to the goal pose is the same as to make the transformation from frame {V} to frame {G} Polar coordinates are convenient to represent the transformation between {V} and {G}: • 𝜍 > 0 is the the error distance between robot and goal state • 𝛽 is the the orientation of the vector wrt {V} ⇢ ~ • β = - 𝜄 - 𝛽 is the orientation of the vector wrt {G} ⇢ ~ 5

MOVING TO A POSE (P CONTROLLER) Control inputs: v(t) and 𝛿 (t) Current known state: Current pose [x y 𝜾 ](t) and goal pose , {G} Polar error vector: Distance 𝜍 , orientation of wrt to {V} and wrt {G} (by β , 𝛽 ) ~ ⇢ Cartesian to Polar coordinates: 2 q 3 2 3 ∆ 2 x + ∆ 2 ρ y 6 7 ⇣ ∆ y 6 7 ⌘ α 5 = 6 7 6 7 − θ atan 6 7 4 ∆ x 4 5 β − α − θ 2 3 − cos( α ) 0 2 3 x = v ( t ) cos( θ ( t )) ˙ ˙ ρ 6 sin( α ) 7 " # v 6 7 − 1 6 7 y = v ( t ) sin( θ ( t )) ˙ 5 = ˙ α 6 7 ρ 6 7 6 7 ω 4 6 7 ˙ − sin( α ) θ = v ( t ) β 4 5 ˙ 0 tan γ ( t ) = ω ρ L − π 2 , π ⇣ i for {G} in front of {V} ↔ α ∈ 2 for {G} not in front of {V} ↦ the forward direction is redefined by setting v = -v, ω = - ω , and all matrix coe ffi cients change sign 6

MOVING TO A POSE (P CONTROLLER) 2 3 − cos( α ) 0 2 3 ˙ ρ 6 sin( α ) 7 " # v 6 7 − 1 6 7 ˙ 5 = α 6 7 ρ 6 7 6 7 ω 4 6 7 ˙ − sin( α ) β 4 5 0 ρ − π 2 , π ⇣ i for {G} in front of {V} ↔ α ∈ 2 Control laws on v and 𝛿 that aim to bring to zero the polar coordinates, meaning {V} ≣ {G} v ( t ) = K ρ ρ ( t ) γ ( t ) = K α α ( t ) + K β β ( t ) For reaching a generic goal pose, the following transformation can be used: x 0 = x − x ⇤ , y 0 = y − y ⇤ , θ 0 = θ , β = β 0 + θ ⇤ 7

MOVING TO A POSE (P CONTROLLER) What are good values for the gains? Is the dynamic system stable? Does it converge? v ( t ) = K ρ ρ ( t ) γ ( t ) = K α α ( t ) + K β β ( t ) 2 3 − cos( α ) 0 2 3 ˙ ρ 6 sin( α ) 7 " # v 6 7 − 1 6 7 ˙ 5 = α 6 7 ρ 6 7 6 7 ω 4 6 7 ˙ − sin( α ) β 4 5 0 ρ     − K ρ ρ cos( α ) ˙ ρ     − K ρ sin( α ) − K α α − K β β ˙  = α        ˙ β − K ρ sin( α ) Asymptotically stable as long as: Dynamic system K ρ > 0 , K β < 0 , K α − K ρ > 0 8

MOVING TO A POSE (P CONTROLLER) In some cases the robot has backed into the goal pose K ρ = 3 , K β = − 3 , K α = 8 9

MOVING TO A POSE (P CONTROLLER) Y [mm] Di ff erent goal orientations produce rather di ff erent trajectories K ρ = 3 , K β = − 1 . 5 , K α = 8 10

I S S U E S A N D N E X T S T E P S … . • The systems describing robot and error dynamics used to solve inverse general kinematics problems (with feedback-based control) are not linear • Feedback-based approaches require setting a number of gain parameters when adopting a PID controller: how do we set the gain values? • Is the (linear) PID approach appropriate , in general, for the type of inverse kinematics problems we are dealing with? • How do we get some guarantees about the trajectory followed by the robot: does the robot converge to the desired setpoint? Once reached, is the setpoint stable? Will convergence happen in finite time? Next steps: 1. PIDs more in depth: properties and parameter tuning 2. Overview (brief) about control and equilibrium in dynamic systems 3. Linearization techniques 11

L I M I TAT I O N S O F P I D C O N T R O L L E R S Z t de ( t ) u ( t ) = K p e ( t ) + K I e ( t ) dt + K D dt 0 Z t ! 1 de ( t ) P u ( t ) = K p e ( t ) + e ( t ) dt + ∆ T D K P ∆ T I dt 0 control error ( t ) output ( t ) setpoint I X X Plant K I , ∆ T I u ( t ) D K D , ∆ T D − 1 • Work without the help/need of a model of the dynamics of the system/plant being controlled → Potentially (usually) sub-optimal • Provide a feedback-based linear control, since the control signal u(t) is linear in the error (and in linear operators of the error) → In general, sub-optimal when plant’s dynamics is not linear: Goal: | s ( t ) − o ( t ) | → 0 o ( t ) = A ( t ) o ( t − 1) + u ( t ) + ν ( t ) ⇒ u ( t ) ≈ s ( t ) − A ( t ) o ( t − 1) 12

P I D ’ S R E S P O N S E F I G U R E S control u(t) output(t) Response factors: 1. Step response gain: Amount of output change resulting from a step unit change in control u(t) 2. Step response time: Time from when u(t) changes until the change starts to be seen in output(t) 3. Step characteristic time: The time for output(t) to reach its new level. Because the output usually approaches its new level asymptotically, the characteristic time constant = time it takes the system's step response to reach (-1)1/e, or about 63% of the distance from the initial to its final value 13

P I D ’ S R E S P O N S E F I G U R E S output(t) control u(t) Linear Non-Linear With Hysteresis Response type: 1. Linearity of response: The same change in u(t) through the whole scale results in a similar change in output(t) at each point 2. Non Linearity of response: A change on one part of the u(t) range results in more output(t) change than the same u(t) change in a di ff erent range 3. Hysteresis of response: A di ff erent output(t) is produced for the same control input u(t) depending on “history”, whether the u(t) went up or down right before 14

T U N I N G P I D ’ S G A I N S : P G A I N • Proportional gain: Contributes to the control signal u(t) with a value proportional to e(t) , or, equivalently to output(t) (it’s just a matter of scale given that the (setpoint - output(t)) is linear). Corrects u(t) based on upsets as they happen. • High gain: Strong reaction to error, Lower steady-state error, Higher overshot • Low gain: Less sensitivity, High steady-state error Device temperature control Precision actuator Motor shaft control by DC Short step response time, Long step response time, Short step response time Steady-state error, u(t) not enough) Non-linear dynamics 15 External disturbance

T U N I N G P I D ’ S G A I N S : I G A I N • Integral gain: Removes steady-state error by integrating the error over time. It is used to add long-term precision to a control loop, and it is almost always used in conjunction with proportional control. Alone, it usually doesn’t drive the system to stability since it brings “inertia” • High gain: More oscillatory, Faster reduction of SS error • Low gain: Less oscillatory, Slower reduction of SS error Device temperature control Motor shaft control by DC 16

T U N I N G P I D ’ S G A I N S : P - I G A I N • Integral gain + Proportional Gain: React fast (P) and let the steady-state error going to zero (I), but overall hits the setpoint later than when using P Motor shaft control by DC Device temperature control 17

T U N I N G P I D ’ S G A I N S : D G A I N • Di ff erential gain: It is also called preact because it allows the loop to “anticipate” upsets as they begin to happen, spot by a rate of change in the error, and then react quickly. • Reduces the oscillatory behavior and overshot of the response . 
 Temperature control with PID Precision actuator with PD 18