8803 - Mobile Manipulation: Control Mike Stilman Robotics & - PDF document

8803 - Mobile Manipulation: Control • Mike Stilman • Robotics & Intelligent Machines @ GT • Georgia Institute of Technology • Atlanta, GA 30332-0760 • February 19, 2008 1 Mike Stilman (RIM@GT) 8803 Lecture 13 How do we make the robot move? Today: Control of Dynamical Systems & Robots Thursday: Strategies for Combining Position & Force 2 Mike Stilman (RIM@GT) 8803 Lecture 13 1

The Robot is a Dynamic System What is a Dynamic System? – A system whose variables are time-dependent – Inputs and Outputs vary with time How can we describe a dynamic system? – Input-output differential equations – Free-body diagrams – State-variable matrix – Transfer functions – Block diagrams 3 Mike Stilman (RIM@GT) 8803 Lecture 13 Control What is Control? Given the system state, choose input to get a desired output. 4 Mike Stilman (RIM@GT) 8803 Lecture 13 2

Car Cruise Control x m¨ x b ˙ x State Variable Form: f £ ¤ £ 1 ¤ − b x = ¨ x + ˙ f m ¨ x + b ˙ x = f m m x + b f ¨ m ˙ x = m 5 Mike Stilman (RIM@GT) 8803 Lecture 13 Car Cruise Control (Open Loop) x m¨ x b ˙ x State Variable Form: f £ ¤ £ 1 ¤ − b x = ¨ x + ˙ f m ¨ x + b ˙ x = f m m x = 0 ¨ Steady State → Let 0 + b ˙ x ss = f f x r = ˙ ˙ x ss = b u = f = b ˙ x r 6 Mike Stilman (RIM@GT) 8803 Lecture 13 3

Car Cruise Control (Open Loop) x x r ˙ x ˙ f Ctrl Plant m¨ x b ˙ x State Variable Form: f £ ¤ £ 1 ¤ − b x = ¨ x + ˙ f m ¨ x + b ˙ x = f m m Steady State → Let ¨ x = 0 £ ¤ £ b ¤ − b x = ¨ x + ˙ x r ˙ 0 + b ˙ x ss = f m m f x r = ˙ ˙ x ss = b f = b ˙ x r 7 Mike Stilman (RIM@GT) 8803 Lecture 13 Car Cruise Control (Open Loop) x x r ˙ x ˙ f Ctrl Plant m¨ x b ˙ x State Variable Form: f £ ¤ £ b ¤ − b x = ¨ x + ˙ x r ˙ m ¨ x + b ˙ x = f m m 30 velocity (m/s) 20 10 0 m = 1500kg b = 50Ns/m v r = 27m/s 0 100 200 300 time (s) 8 Mike Stilman (RIM@GT) 8803 Lecture 13 4

Car Steering J ¨ θ τ State Variable Form: b ˙ θ · ˙ ¸ · 0 ¸ · θ ¸ · 0 ¸ θ θ 1 = + τ ¨ ˙ 1 − b 0 θ θ J J J ¨ b ˙ θ + θ = τ − b θ + 1 ¨ ˙ θ = J τ J 9 Mike Stilman (RIM@GT) 8803 Lecture 13 Car Steering (Proportional Control) e u θ r θ − Ctrl Plant J ¨ θ τ + State Variable Form: b ˙ θ · ˙ ¸ · ¸ · θ ¸ · 0 ¸ θ 0 1 θ = + θ r ¨ − K p ˙ K p − b θ θ J J J J ¨ b ˙ θ + θ = τ − b θ + 1 ¨ ˙ θ = J τ J τ = − K p ( θ − θ r ) − b θ − K p J θ + K p ¨ ˙ θ = J θ r J 10 Mike Stilman (RIM@GT) 8803 Lecture 13 5

Car Steering (Proportional Control) e u θ r θ − Ctrl Plant J ¨ θ τ + State Variable Form: · ˙ ¸ · ¸ · θ ¸ · 0 ¸ 0 1 θ b ˙ θ = + θ r ¨ − K p ˙ K p − b θ θ θ J J J J = .1kgm 2 b = .1Nms θ r = .5rad J ¨ b ˙ θ + θ = τ 1 − b θ + 1 ¨ ˙ θ = J τ 0.8 J position (rad) 0.6 τ = − K p ( θ − θ r ) 0.4 0.2 − b θ − K p J θ + K p ¨ ˙ θ = J θ r 0 J 0 2 4 6 8 10 time (s) 11 Mike Stilman (RIM@GT) 8803 Lecture 13 Car Steering (Proportional Control) e u θ r θ − Ctrl Plant J ¨ θ τ + τ = − K p ( θ − θ r ) b ˙ θ θ − b θ − K p J θ + K p ¨ ˙ θ = J θ r J Stability? 1 0.8 position (rad) 0.6 Exponential Convergence? 0.4 0.2 Limitations? 0 0 2 4 6 8 10 time (s) 12 Mike Stilman (RIM@GT) 8803 Lecture 13 6

Car Steering (PD Control) e u θ r − Plant J ¨ Ctrl θ θ ˙ τ + θ ˙ θ r = 0 − + State Variable Form: b ˙ θ · ˙ ¸ · ¸ · θ ¸ · 0 ¸ θ θ 0 1 = + θ r ¨ − K p ˙ K p − b + K d θ θ J J J J ¨ b ˙ θ + θ = τ b θ + 1 ¨ ˙ θ = J τ 0.6 J 0.5 position (rad) τ = − K d ( ˙ θ − ˙ θ r ) − K p ( θ − θ r ) 0.4 0.3 0.2 − b θ − K d θ − K p J θ + K p 0.1 ¨ ˙ ˙ θ = J θ r 0 J J 0 2 4 6 8 10 time (s) 13 Mike Stilman (RIM@GT) 8803 Lecture 13 PD Control 0.6 0.6 0.5 0.5 position (rad) position (rad) 0.4 0.4 0.3 0.3 0.2 0.2 0.1 0.1 0 0 2 4 6 8 10 0 time (s) 0 2 4 6 8 10 time (s) Under-damped Over-damped 0.6 0.5 position (rad) 0.4 0.3 0.2 0.1 0 0 2 4 6 8 10 time (s) Critically damped (Just Right) 14 Mike Stilman (RIM@GT) 8803 Lecture 13 7

PD Control (Selecting Gains) − b + K d θ − K p ¨ ˙ θ = J ( θ − θ r ) ¨ d ˙ θ + K 0 θ + K 0 J p θ = K p θ r ¨ d ˙ − K 0 θ − K 0 θ = p ( θ − θ r ) s 2 + K 0 d s + K 0 Characteristic Polynomial: p ξ Damping Ratio: s 2 + 2 ξω n s + ω 2 Standard Form: Natural Frequency: ω n n K 0 p = ω 2 K 0 p = ω 2 n n q ξ = 1 K 0 d = 2 ξω n K 0 K 0 d = 2 ω n = 2 p 15 Mike Stilman (RIM@GT) 8803 Lecture 13 PD Control (Selecting Gains) Starting Point: K 0 p = ω 2 n q K 0 K 0 d = 2 ω n = 2 p ω n What is ? Gains are limited by: • structural flexibility • time delay • sampling rate • actuator saturation Optimal Control – Linear Quadratic Regulator Tuning! 16 Mike Stilman (RIM@GT) 8803 Lecture 13 8

Car Cruise Control (Closed Loop) x m¨ x b ˙ x f m ¨ x + b ˙ x = f f = − bK p ( ˙ x − ˙ x r ) Open Loop: 30 30 velocity (m/s) velocity (m/s) 20 20 K=20 10 10 0 0 0 5 10 15 20 0 100 200 300 time (s) time (s) 17 Mike Stilman (RIM@GT) 8803 Lecture 13 Car Cruise Control (Closed Loop) x m¨ x b ˙ x f m ¨ x + b ˙ x = f f = − bK p ( ˙ x − ˙ x r ) Open Loop: 30 30 velocity (m/s) velocity (m/s) 20 20 K=20 10 10 0 0 0 5 10 15 20 0 100 200 300 time (s) time (s) 18 Mike Stilman (RIM@GT) 8803 Lecture 13 9

Another Example x m¨ x b ˙ x − K p ( z − z r ) − K d ˙ z f m m ¨ x + b ˙ x = f mg f = − bK p ( ˙ x − ˙ x r ) 19 Mike Stilman (RIM@GT) 8803 Lecture 13 Manipulator Control (Gravity Compensation) x 1 = d 1 cos( θ 1 ) z d 1 m 1 ¨ θ 1 = − d 1 cos( θ 1 ) m 1 g + τ 1 θ 1 τ 1 x 20 Mike Stilman (RIM@GT) 8803 Lecture 13 10

Manipulator Control (Gravity Compensation) x 1 = d 1 cos( θ 1 ) z d 1 m 1 ¨ θ 1 = − d 1 cos( θ 1 ) m 1 g + τ 1 θ 1 τ 1 x Acceleration due to gravity: τ g = − d 1 cos( θ 1 ) m 1 g How do we do position control? τ 1 = d 1 c 1 m 1 g − K p ( θ 1 − θ 1 d ) − K d ˙ θ 1 21 Mike Stilman (RIM@GT) 8803 Lecture 13 Manipulator Control (Gravity Compensation) θ 2 x 1 = d 1 c 1 l 1 d 2 x 2 = l 1 c 1 + d 2 c 2 z d 1 m 2 m 1 Torque due to gravity: θ 1 x 22 Mike Stilman (RIM@GT) 8803 Lecture 13 11

Manipulator Control (Gravity Compensation) θ 2 x 1 = d 1 c 1 l 1 d 2 x 2 = l 1 c 1 + d 2 c 2 z d 1 m 2 m 1 Torque due to gravity: θ 1 x τ 2 = − d 2 c 12 m 2 g − d 1 c 1 m 1 g − ( l 1 c 1 + d 2 c 12 ) m 2 g τ 1 = 23 Mike Stilman (RIM@GT) 8803 Lecture 13 Manipulator Control (Gravity Compensation) θ 2 x 1 = d 1 c 1 l 1 d 2 x 2 = l 1 c 1 + d 2 c 12 z d 1 m 2 m 1 Torque due to gravity: θ 1 x − d 2 c 12 m 2 g τ 2 = τ 1 = − d 1 c 1 m 1 g − ( l 1 c 1 + d 2 c 12 ) m 2 g How do we do position control? · ¸ · ¸ · ¸ · ˙ ¸ τ 1 d 2 c 12 m 2 θ 1 − θ 1 d θ 1 = g − K p − K d ˙ θ 2 − θ 2 d τ 2 d 1 c 1 m 1 + ( l 1 c 1 + d 2 c 12 ) m 2 θ 2 Gains are typically diagonal matrices 24 Mike Stilman (RIM@GT) 8803 Lecture 13 12

Manipulator Control (Full Model) M ( q ) ¨ q + C ( q , ˙ q ) ˙ q + G ( q ) = τ 25 Mike Stilman (RIM@GT) 8803 Lecture 13 Manipulator Control (Full Model) M ( q ) ¨ q + C ( q , ˙ q ) ˙ q + G ( q ) = τ Augmented PD: q d + G ( q ) − K d ( ˙ q − ˙ q d ) − K p ( q − q d ) = τ M ( q ) ¨ q d + C ( q , ˙ q ) ˙ Computed Torque: M ( q )( ¨ q d − K d ( ˙ q − ˙ q d ) − K p ( q − q d )) + C ( q , ˙ q ) ˙ q + G ( q ) = τ 26 Mike Stilman (RIM@GT) 8803 Lecture 13 13

More Jacobians (Virtual Work) δ x = J δ q W = F T x δ W = F T δ x δ W = τ T δ q F T δ x = τ T δ q F T J δ q = τ T δ q τ = J T F Open Loop Force Control 27 Mike Stilman (RIM@GT) 8803 Lecture 13 More Jacobians (Virtual Work) δ x = J δ q W = F T x δ W = F T δ x δ W = τ T δ q F T δ x = τ T δ q F T J δ q = τ T δ q τ = J T F Open Loop Force Control J T J vi vi g =? Velocity Jacobian for Link Mass i : G = − ( J T v 1 ( m 1 g ) + J T v 2 ( m 2 g ) ... J T vn ( m n g )) τ = G ( q ) + J T F 28 Mike Stilman (RIM@GT) 8803 Lecture 13 14

τ = J T F Manipulator Workspace Control J − T MJ T ¯ M = M ( q ) ¨ q + C ( q , ˙ q ) ˙ q + G ( q ) = τ J − T ( CJ − 1 + M ˙ ¯ J − 1 ) C = J − T G ¯ G = ¯ x + ¯ x + ¯ M ( q ) ¨ C ( q , ˙ q ) ˙ G ( q ) = F J − T τ F = F Khatib ’80, Murray ‘94 29 Mike Stilman (RIM@GT) 8803 Lecture 13 τ = J T F Manipulator Workspace Control J − T MJ T ¯ M = M ( q ) ¨ q + C ( q , ˙ q ) ˙ q + G ( q ) = τ J − T ( CJ − 1 + M ˙ ¯ J − 1 ) C = J − T G ¯ G = ¯ x + ¯ x + ¯ M ( q ) ¨ C ( q , ˙ q ) ˙ G ( q ) = F J − T τ F = Computed Torque: F = ¯ x d ) − K p ( x − x d )) + ¯ x + ¯ M ( q )( ¨ x d − K d ( ˙ x − ˙ C ( q , ˙ q ) ˙ G ( q ) τ = J T F Khatib ’80, Murray ‘94 30 Mike Stilman (RIM@GT) 8803 Lecture 13 15