Control of Robot Manipulators in Joint Space R. Kelly, V. Santibanez and A. Loria

[ Pobierz całość w formacie PDF ]
.2.The components of the vector of gravitationaltorques g(q) are given byg1(q) =(m1lc1 + m2l1)g sin(q1) +m2lc2g sin(q1 + q2)g2(q) =m2lc2g sin(q1 + q2).Consider the P D control law with gravity compensation on thisrobot for position control and where the design matrices Kp, Kv, A, Bare taken diagonal and positive definite.In particular, pickKp = diag{kp} = diag{30} [Nm/rad] ,Kv = diag{kv} = diag{7, 3} [Nm s/rad] ,A = diag{ai} = diag{30, 70} [1/s] ,B = diag{bi} = diag{30, 70} [1/s].The components of the control input � are given by�1 = kpq1 - kv�1 + g1(q)��2 = kpq2 - kv�2 + g2(q)��1 = -a1x1 - a1b1q1�2 = -a2x2 - a2b2q2�1 = x1 + b1q1�2 = x2 + b2q2.The initial conditions corresponding to the positions, velocities andstates of the filters, are chosen asq1(0) = 0, q2(0) = 0q1(0) = 0, q2(0) = 0� �x1(0) = 0, x2(0) = 0.13.1 P D Control with Gravity Compensation 299[rad]0.4.3.q1.�.2.�.q2.1.0587.00.0151-0.10.0 0.5 1.0 1.5 2.0t [s]Figure 13.2.Graphs of position errors q1(t) and q2(t)� �The desired joint positions are chosen asqd1 = �/10, qd2 = �/30 [rad].In terms of the state vector of the closed-loop equation, the initialstate is�# �# �# �# �# �#b1�/10 9.423�(0)�# �# �# �# �# �#b2�/30 7.329�# �# �# �# �# �#�# �# �# �# �# �#�/10 0.3141�# �# �# �# �# �#= =.�q(0)�# �# �# �# �# �#�/30 0.1047�# �# �# �# �# �#�# �# �# �# �# �#0 0�q(0)0 0Figure 13.2 presents the experimental results and shows that the�components of the position error q(t) tend asymptotically to a smallnonzero constant.Although we expected that the error would tendto zero, the experimental behavior is mainly due to the presence ofunmodeled friction at the joints.f&In a real implementation of a controller on an ordinary personal computer(as is the case of Example 13.1) typically the joint position q is sampledperiodically by optical encoders and this is used to compute the joint velocity�q.Indeed, if we denote by h the sampling period, the joint velocity at theinstant kh is obtained asq(kh) - q(kh - h)�q(kh) = ,hdthat is, the differential operator p = is replaced by (1 - z-1)/h, where z-1dtis the delay operator that is, z-1q(kh) =q(kh - h).By the same argument,300 P D Controlin the implementation of the P D control law with gravity compensation,(13.2) (13.3), the variable � at instant kh may be computed asq(kh) - q(kh - h) 1�(kh) = + �(kh - h)h 2where we chose A = diag{ai} = diag{h-1} and B = diag{bi} = diag{2/h}.13.2 P D Control with Desired Gravity CompensationIn this section we present a modification of PD control with desired gravitycompensation, studied in Chapter 7, and whose characteristic is that it does�not require the velocity term q in its control law.The original references onthis controller are cited at the end of the chapter.This controller, that we call here P D control with desired gravity com-pensation, is described by� �� = Kpq + Kv [qd - �] +g(qd) (13.16)� = -Ax - ABq� = x + Bq (13.17)where Kp, Kv " IRn�n are diagonal positive definite matrices, A = diag{ai}and B = diag{bi} with ai and bi real strictly positive constants but otherwisearbitrary for all i =1, 2, � � � , n.Figure 13.3 shows the block-diagram of the P D control with desiredgravity compensation applied to robots.Notice that the measurement of the�joint velocity q is not required by the controller.g(qd)�� qROBOTBKv Kp�� qd �(pI +A)-1ABqd �Figure 13.3.Block-diagram: P D control with desired gravity compensation13.2 P D Control with Desired Gravity Compensation 301Comparing P D control with gravity compensation given by (13.2) (13.3)with P D control with desired gravity compensation (13.16) (13.17), we im-mediately notice the replacement of the term g(q) by the feedforward termg(qd).The analysis of the control system in closed loop is similar to that fromSection 13.1.The most noticeable difference is in the Lyapunov function con-sidered for the proof of stability.Given the relative importance of the controller(13.16) (13.17), we present next its complete study.Define � = x + Bqd.The equation that describes the behavior in closedloop is obtained by combining Equations (III.1) and (13.16) (13.17), whichT� �may be expressed in terms of the state vector �T qT qT as�# �# �# �#� �� -A� + ABq + Bqd�# �# �# �#d�# �#=�# �#��q ��# �# �# q �#dt �# �# �# �#�� q qd-M(q)-1[Kpq+Kv[qd-�+Bq]+g(qd)-C(q, q)q-g(q)]TT�� A sufficient condition for the origin �T qT q = 0 " IR3n to be aunique equilibrium of the closed-loop equation is that the desired joint positionqd is a constant vector.In what follows of this section we assume that this isthe case.Notice that in this scenario, the control law may be expressed bybip�� = Kpq - Kvdiag q + g(qd),p + aiwhich is very close to PD with desired gravity compensation control law (8.1)when the desired position qd is constant.The only difference is the substitution�of the velocity term q bybip� = diag q,p + ai�thereby avoiding the use of velocity measurements q(t) in the control law.As we show below, if the matrix Kp is chosen so that�min{Kp} >kg ,then the P D controller with desired gravity compensation verifies the posi-tion control objective, that is,lim q(t) =qdt�!"for any constant vector qd " IRn.302 P D ControlConsidering the desired position qd to be constant, the closed-loop equa-T� �tion may then be written in terms of the new state vector �T qT qT as�# �# �# �#�� -A� + ABq�# �# �# �#d�# �#=�# �#�q ��# �# �# -q �#dt �# �# �# �#� � � � � � �q M(q)-1 [Kpq-Kv[�-Bq]+g(qd)-C(qd-q, q)q-g(qd-q)](13.18)which, since qd is constant, is an autonomous differential equation.Sincethe matrix Kp has been picked so that �min{Kp} > kg, then the originT� ��T qT qT = 0 " IR3n is the unique equilibrium of this equation (seethe arguments in Section 8.2).In order to study the stability of the origin, consider the Lyapunov functioncandidate1� � � � � � �V (�, q, q) =K(qd - q, q) +f(q) + (� - Bq)T KvB-1 (� - Bq) (13.19)2where1� � � � �K(qd - q, q) = qT M(qd - q)q21� � � � �f(q) =U(qd - q) -U(qd) +g(qd)T q + qTKpq.2Notice first that the diagonal matrix KvB-1 is positive definite.Since it�has been assumed that �min{Kp} >kg, we have from Lemma 8.1 that f(q) is a� � �(globally) positive definite function of q.Consequently, the function V (�, q, q)is also globally positive definite.The time derivative of the Lyapunov function candidate yields1 T� � �� V (�, q, q) = qTM(q)q + qT @(q)q - q g(qd - q) +g(qd)T q2� � �� + qTKpq +[� - Bq]T KvB-1 � - Bq.� �� Using the closed-loop Equation (13.18) to solve for �, q and M(q)q, andcanceling out some terms, we obtain�� V (�, q, q) =- (� - Bq)T KvB-1A (� - Bq)�# �# �# �# �# �#T� KvB-1A -KvA 0 ��# �# �# �# �# �#� �= - q -KvA BKvA 0 q (13.20)� �q 00 0 qwhere we used (cf.Property 4.2)13.2 P D Control with Desired Gravity Compensation 3031� � �qT @(q) - C(q, q) q =0.2�� Clearly, the time derivative V (�, q, q) of the Lyapunov function candidateis a globally semidefinite negative function.For this reason, according to theTheorem 2.3, the origin of the closed-loop Equation (13.18) is stable.Since the closed-loop Equation (13.18) is autonomous, direct applicationof La Salle s Theorem 2.7 allows one to guarantee global asymptotic stabilityof the origin corresponding to the state space of the closed-loop system (cf.Problem 4 at the end of the chapter) [ Pobierz całość w formacie PDF ]

Podstrony