Basic compute torque control

COMPUTED TORQUE CONTROL Manipulator in the standard form Mx cx M (q)q Inertia matrix kx F V (q, q ) G (q ) Coriolis/centripetal vector Gravity vector * M(q) is symmetric Computed-Torque Control (⇒ feedback linearization of nonlinear system) Consider robot arm dynamics : M (q)q  N (q, q )   d   ,where d : -① disturbance/noise,  : control torque Define tracking error as e(t )  qd (t )  q(t ) then, e  q d  q e  qd  q ①⇒ e  qd  M 1 ( N   d   ) M (qd  e)  ( N   d   ) qd  e   M 1 ( N   d   )  e  qd  M 1 ( N   d   ) Defining control input u and disturbance function w , u  qd  M 1 ( N   ) -② w  M 1 d then e  u  w Defining state x : e  x  e  Then, tracking error dynamics : d dt e  0 I  e  0 0 e   0 0 e    I  u   I  w          -③ ↖linear error system ★ The dynamic sytem itself is nonlinear, but the tracking error dynamic is linear ② looks like feedback linearizing transformation u  qd  M 1 ( N   ) ②   M (qd  u)  N ②⇒ -④ ↘called "Computed-torque control law." ★ If we selects u(t) stabilizing ③ so that e(t) goes to zero, then non trajectory (1) Selecting control input u(t) as PD : ④    M (qd  uPD )  N uPD Kp e Then, equation ④ becomes M (qd Kd e Kd e K p e) N Now, we want to find gains for u Closed-loop error dynamics e e Kd e Kp e Kp e Kd e w e  u  w (←③): Kd e w e Kd e Kp e Kp e w w  in state-space form : d e dt e I e o Kp Kd e I w Closed-loop characteristic equation : Using | sI  A |  c ( s)  S  K v S  K p  Where, K v  diag{K vi } , K p  diag{K pi } ∴ Error system is asymptotically stable if K vi and K pi are all positive (using Routh-Hurwitz table) Dynamics of a Two-Link Planar Elbow Arm Elearning Exercise (m1  m2 ) a12  m2 a2  2m2 a1a2  cos   m2 a2  m2 a1a2  cos   m2 a2  m2 a1a2  cos   1      m2 a     m2 a1a2 ( 212  12 ) sin   (m1  m2 ) ga1  cos 1  m2 ga2  cos(1   )      sin  m2 ga2  cos(1   ) m a a   2        1   For the robot dynamic equation derived above, desired trajectory qd (t ) has the components: 1d  g1 sin( 2t / T ) 2d  g2 cos(2t / T ) ,where T=2s , amplitudes g1=g2=0.1 rad, kp=100, kd=20, m1=1, m2=1, a1=1, a2=1, initial condition, x=[0 0 0]:theta1, theta2, dtheta1,dtheta2 Simulate PID computed torque control ... M (qd  u)  N ②⇒ -④ ↘called "Computed -torque control law." ★ If we selects u(t) stabilizing ③ so that e(t) goes to zero, then non trajectory (1) Selecting control input u(t) as PD : ④  ... m2=1, a1=1, a2=1, initial condition, x=[0 0 0]:theta1, theta2, dtheta1,dtheta2 Simulate PID computed torque control