"1. Sliding Mode Control Warm-Up We will work on the systemx? = x + ax sin(x )1 2 1 1x? = bx x + u (1)2 1 2where a and b are unknown constants, but we know that 0 = |a| = 2 and 1 = |b| = 3.ˆ (a) Show that this system is state feedback linearizable. Assume that aˆ = 1 and b =1.5 are nominal values of a and b that you will use for your feedback linearizingˆ controller; furthermore, assume for now that a = aˆ and b = b. Simulate yourcontroller by regulating the states to zero. ? = ? + ? ? sin??(1)1 2 1 1 ? = ? ? ? + ?(2)2 1 2The system given by the equations (1) and (2) has unknown constants, but weknow their boundaries 0 = |a| = 2 and 1 = |b| = 3. Here in this case we want to use Inputstate Linearization method. However, we will treat the constants a, b as they are known.Now we need to find ? ? that transform the system to linear model. More over the transformation ? ? must be diffeomorphism, because ? and ? are positive numbers wecan work in the same state equations.? + ? ? sin? (? ) 0 2 1 1 ? ? = ,? ? = ? ? ? 1 1 2 ? ? ? ? 1 2 The transformation T(x) must satisfies these conditions ? = 0, ? ? 0 and? ? ? ? ? ? 1 ? ? = ? .2 ? ? ? ? 1 From the condition ? = 0 we get ? ? ? ? ? ? ? ? ? ? 1 1 0 1 1? = = = 0 (3)? ? ? ? ? ? ? ? 1 2 1 2 ? ? 2 This shows that ? is independent of ? and from the condition ? ? 0 we get1 2 ? ?? ? ? ? ? ? 0 ? ? 2 2 2 2 ? = = ? 0(4)? ? ? ? ? ? ? ? 1 2 1 2 ? ? 1 Let ? = ? this implies= 11 2 ? ? ? ? 1 From equation (3) and the condition ? ? = ? we get 2 ? ? ? ? ? ? ? ? 1 1 ? + ? ? sin? (? ) 1 2 1 1? = =? + ? ? sin? (? ) = ? + ? ? sin? (? )(5)2 2 1 1 2 1 1 ? ? ? ? ? ? ? 1 2 ? ? 1 2 1 From equation (5) we can obtain the transformation function as it is shown in equation (6). Note1 -1that? ? ? ? . In order to insure that the transform function is diffeomorphism ? (? ) must be1 exist and it is also belongs to? .? ? ? 1 1 1 ? ? = = = (6)? + ? ? sin? (? ) ? ? 2 2 2 1 1 ? = ? 1 1 ? = ? + ? ? sin? (? )2 2 1 1= ? + ? ? sin? (? ) ? (7)2 1 1? = ? - ? ? sin? (? )2 2 1 1 -1 1From equation (7) we obtain ? (? ) and it is also ? ? ,? ? 1 1 -1 ? (? ) = = (8)? ? - ? ? sin? (? ) 2 2 1 1Verifying ? = ? 1 1= ? + ? ? sin? (? )2 1 1= ? - ? ? sin ? + ? ? sin? (? )(9)2 1 1 1 1 ? = ?1 2 ? = ? + ? (? ? cos ? + sin ? ? ))2 2 1 1 1 1 1 = ? ? ? + ? + ? ? ? + ? ? sin ? )cos? (? ) + ? sin ? (? + ? ? sin? (? ))1 2 1 2 1 1 1 1 2 1 1 2 2 2 2= ? ? ? + ? + ? ? ? cos? (? ) + ? ? sin ? cos? (? ) + ? ? sin ? + ? ? ? ? ? (? ) (10)1 2 1 2 1 1 1 2 1 1 1 1 2 2= ? ? ? - ? ? sin ? + ? + ? ? ? - ? ? sin ? cos? (? ) + ? ? sin ? cos? (? ) + 1 2 1 1 1 2 1 1 1 1 1 1 2 2 ? sin ?? - ? ? sin z+ ? ? ? ? ? ??1 2 1 1 1 1 2? = ? ? ? - ? ? ? sin ? + ? ? ? cos? (? ) + ? ? sin ? + ?2 1 2 1 1 1 2 1 2 1From equation (9) and (10) we calculate that the system is state feedback linearizable.Now we can introduce the control law.2 ? = ? ? ? sin ? - ? ? ? - ? ? ? cos? (? ) - ? ? sin ? + ?(11)1 1 2 1 2 1 2 1 1where ? = -? ? - ? ? . 1 1 2 2 Choose ? and ? values such that the system poles must be in the lift half of the plane. Then the1 2 states ? and ? will converge to zero that implies the system state in x-domain will also converge1 3 to zero. Program Code:close all; clear all; clc;init_conds=[-1 1 ] ;tf=5;[t,states]=ode23('Dynamics1_a', [0 tf], init_conds);figurehold onplot(t,states(:,1),'k');plot(t,states(:,2),'b'); xlabel('Time Seconds'); ylabel('Initial conditions');legend('states (1)','states (2)')grid;title('plant versus time');hold off%%x1= states(:,1);x2= states(:,2);figureplot(x1,x2,'b');xlabel('x1');ylabel('x2');legend('x1-x2')grid;title('phase portrait x1-x2');function Dynamics1_a= Dynamics1_a(t,ini)x= ini(1:2);% controller parametersk1=8;k2=6;a=1;b=1.5;v =-k1*x(1)-k2*(x(2)+a*x(1)*(sin(x(1))));gx= 1; fx=b*x(1)*x(2)+a*x(1)*x(2)+a^2*x(1)^2*sin(x(1))+a*x(2)*sin(x(1))+a^2*x(1)*(sin (x(1)))^2;u=(1/gx)*(-fx+v);xdot(1) = x(2)+ a*x(1)*(sin(x(1))) ;xdot(2) = b*x(1)*x(2)+u;Dynamics1_a(1:2)=xdot(1:2);Dynamics1_a = Dynamics1_a';Simulated results: (b) Now suppose that ? ? ?and ? ? ? and that the real values of ? and ? are = -? , ? = ? . Using Matlab plot the phase portrait of the open loop system (1) and identify where thesystem exhibits different behaviors.? - ? sin? (? ) 0 2 1 1? ? = ,? ? = 2? ? 1 1 2 ? ? ? ? 1 2 The transformation T(x) must satisfies these conditions ? = 0, ? ? 0 and? ? ? ? ? ? 1? ? = ? .2 ? ? ? ? 1 From the condition ? = 0 we get ? ? ? ? ? ? ? ? 1 1 0 ? ? 1 1? = = = 0 (12)? ? ? ? ? ? ? ? 1 2 1 2 ? ? 2 This shows that ? is independent of ? and from the condition ? ? 0 we get1 2 ? ? ? ? ? ? ? ? 0 ? ? 2 2 2 2 ? = = ? 0(13)? ? ? ? ? ? ? ? 1 2 1 2 ? ? 1 Let ? = ? this implies= 11 2 ? ? ? ? 1 From equation (12) and the condition ? ? = ? we get 2 ? ? ? ? ? ? ? ? 1 1 ? -? sin? (? )- 1 2 1 1? = =? - ? sin? (? ) = ? - ? sin? (? )(14)2 2 1 1 2 1 1 ? ? ? ? 2? ? 1 2 ? ? 1 2 1 From equation (14) we can obtain the transformation function as it is shown in equation (15).1Note that? ? ? ? . In order to insure that the transform function is diffeomorphism-1 1 ? (? ) must be exist and it is also belongs to? .? ? ? 1 1 1 ? ? = = = (15)? - ? sin? (? ) ? ? 2 1 1 2 2 ? = ? 1 1 ? = ? + ? ? sin? (? )2 2 1 1 = ? + ? ? sin? (? ) ? (16)2 1 1? = ? - ? ? sin? (? )2 2 1 1 -1 1From equation (7) we obtain ? (? ) and it is also ? ? ,? ? 1 1 -1 ? (? ) = = (17)? ? - ? ? sin? (? ) 2 2 1 1Verifying ? = ? 1 1= ? + ? ? sin? (? )2 1 1 = ? - ? ? sin ? + ? ? sin? (? )(18)2 1 1 1 1 ? = ?1 2 ? = ? - (? ? cos ? + sin ? ? )2 2 1 1 1 1 1= 2? ? + ? - ? cos ?? - ? sin ?- sin ? (? - ? sin? (? ))1 2 1 1 2 1 1 1 2 1 1 2 2= 2? ? + ? - ? ? cos ? + ? sin ? cos ? - ? sin ? + ? ? ? ? (? )(19)1 2 1 2 1 1 1 1 2 1 1 1 2= 2? ? + ? sin ? + ? - ? ? + ? sin ? cos ? + ? sin ? cos ? 1 2 1 1 1 2 1 1 1 1 1 1 - sin ?? + ? sin z+ ? sin?? 1 2 1 1 1 1 2 ? = 2? ? + 2? sin ? - ? ? cos ? - ? sin ? + ?2 1 2 1 1 1 2 1 2 1 Now we can introduce control law2? = -? sin ? - 2? ? + ? ? cos ? + ? sin ? + ? (20)1 1 1 2 1 2 1 2 1where ? = -? ? - ? ? . 1 1 2 2Program code:close all;clear all; clc; init_conds=[-1 1 ] ;tf=5;[t,states]=ode23('Dynamics1_b', [0 tf], init_conds);figurehold onplot(t,states(:,1),'k');plot(t,states(:,2),'b');xlabel('Time [Seconds]');ylabel('Initial conditions');legend('states (1)','states (2)')grid;title('plant versus time');hold off%%x1= states(:,1);x2= states(:,2);figureplot(x1,x2,'b'); xlabel('x1');ylabel('x2');legend('x1-x2')grid;title('phase portrait x1-x2');function Dynamics1_b= Dynamics1_b(t,ini) x= ini(1:2);% controller parametersk1=8;k2=6;v =-k1*x(1)-k2*(x(2)-x(1)*(sin(x(1))));gx= 1;fx=2*x(1)*x(2)-x(1)*x(2)*cos(x(1))+x(1)^2+x(2)*sin(x(1))-x(1)*(sin(x(1)))^2;u=(1/gx)*(-fx+v);xdot(1) = x(2)-x(1)*(sin(x(1))) ;xdot(2) = 2*x(1)*x(2)+u;Dynamics1_b(1:2)=xdot(1:2);Dynamics1_b = Dynamics1_b'; Simulated result: (c) Explain why it is not possible anymore to use your controller from (a) when a ? aˆ and bˆ ? b and guarantee convergence to zero. In particular pay attention to the diffeomorphismand the controller itself. Verify by setting a = -1, b = 2 in your simulation with the same uˆ and diffeomorphism you got in (a). Hint: be sure to use aˆ = 1 and b = 1.5 to do yourcoordinate transformation and to compute your controller, since you assume not to know thereal values for control design purposes! At the same time, use the real a = -1 and b = 2 valuesin your simulation. By setting ? = ?and ? = ? we obtain the controller as2 ? = ? ? ? sin ? - ? ? ? - ? ? ? cos? (? ) - ? ? sin ? + ?1 1 2 1 2 1 2 1 1 When the values of ? ,? changes to be ? = -1 and ? = 2 then the control law changes as2? = -? sin ? - 2? ? + ? ? cos ? + ? sin ? + ?1 1 1 2 1 2 1 2 1 There is change in the diffeomorphism of equation (15) and (17) with respect to (6) and (8).Therefore the controller from case (a) no longer valid to regulate the system states to zero. Program code:close all; clear all; clc;init_conds=[-1 1 ] ;tf=5;[t,states]=ode23('Dynamics1_c', [0 tf], init_conds);x1= states(:,1);x2= states(:,2);figurehold onplot(t,x1,'r','LineWidth',2); plot(t,x2,'b','LineWidth',2);xlabel('Time [Seconds]'); ylabel('Initial conditions');legend('states (1)','states (2)')grid;title('plant versus time');hold offfunction Dynamics1_c= Dynamics1_c(t,ini)x= ini(1:2);% controller parametersk1=8;k2=6;a=-1;b=2;v =-k1*x(1)-k2*(x(2)+x(1)*(sin(x(1))));gx= 1;% controller when a and b > 0fx=1.5*x(1)*x(2)+x(1)*x(2)*cos(x(1))+x(1)^2+x(2)*sin(x(1))+x(1)*(sin(x(1)))^2; u=(1/gx)*(-fx+v);xdot(1) = x(2)+ a*x(1)*(sin(x(1))) ;xdot(2) = b*x(1)*x(2)+u;Dynamics1_c(1:2)=xdot(1:2);Dynamics1_c = Dynamics1_c'; Simulation result: (d) Instead of feedback linearization, you will now implement a sliding mode controller.The approach is to figure out what x should be if we considered it as an input to the x? 2 1equation so that x is driven to zero. Design a linear manifold of the form s = cx +x that1 1 2does the job by considering what happens when s = 0. Next, analyze the dynamics of s anddetermine a sliding mode controller that drives s to zero (and consequently x to zero). Now we use sliding mode control to design a control law such that the system stateregulates to zero. In this approach ? considered like an input for the system. In order to obtain ?1 2 we want? (? ) ? 0. Now we try to perform the Lyapunove stability to insure that with1 1 2 Lyapunove candidate? = ? . 1 2 ? = ? ? 1 1= ? ? + ? ? sin? (? )1 2 1 1 2 = ? ? + ? sin? (? )1 2 1 1Let we find the upper bound for ?, we know that 0 = ? = 2, which leads to2 ? = ? ? + 2?1 2 1 Now we choose ? such that ? is in negative definite.2 Let ? = ? ? , where ? > 2 2 1 The sliding main fold can be defined ? = ? ? + ? , where ? > 21 2 ? = ? ? + ? 1 2 ? = ? ? + ? ? sin ? + ? ? ? + ?(22)2 1 1 1 2 1 2 Using Lyapunove study stability of s with Lyapunove candidate ? = ? . 2 ? = ? ?? = ? (? ? + ? ? ? sin ? + ? ? ? + ? )(23)2 1 1 1 2 The upper bound for ? is ? = ? ? ? + ? ? ? ? sin ? + ? ? ? ? + ? ?2 1 1 1 2 ? = ? ? ? + ? (? 2? + 3? ? ) + ? ?(24)2 1 1 2Where, 0 = ? = 2, 0 = ? = 3 and c is known constant larger than 2. Now we define a control law that ? is negative definite? = -? ? ? ?-? 2? - 3? ? - ? ? - ? ?1 1 2 2 2 Where ? > 0 this lead to ? = -? ? The controller law in equation will make? ? 0. So the system state will go to zero.(e) Simulate your sliding mode design. Note that if you use the “sign” function in Matlab,ode45 will take a very long time to run. Instead, it is suggested that you use theapproximation Program code:close all; clear all; clc; init_conds=[-1 1] ;tf=10;[t,states]=ode45('Dynamics1_e', [0 tf], init_conds); x1= states(:,1);x2= states(:,2);figureplot(x1,x2,'b','LineWidth',2);xlabel('x1');ylabel('x2');legend('x1-x2')grid;title('phase portrait X1-x2');%%figurehold onplot(t,states(:,1),'r','LineWidth',2);plot(t,states(:,2),'b','LineWidth',2); xlabel('Time [Seconds]');ylabel('Initial conditions');legend('states (1)','states (2)')grid;title('plant versus time');hold offfunction Dynamics1_e= Dynamics1_e(t,ini) x= ini(1:2);% controller parametersc=5;k=2;a=-1;b=2; s=c*x(1)+x(2);% gx=1;% u=(1/gx)*(-c*x(2)-c*a*x(1)*sin(x(1))-b*x(1)*x(2)-sat(s)); u= -sat(s)*(-c*2*x(1)-3*x(1)*x(2))-c*x(1)-k*s; xdot(1) = x(2)+ a*x(1)*(sat(x(1))) ;xdot(2) = b*x(1)*x(2)+u;Dynamics1_e(1:2)=xdot(1:2);Dynamics1_e = Dynamics1_e'; Simulation result: 2(a)Pick values for and µ, and verify via simulation that for u = 1 the van der Pol systemexhibits a stable limit cycle outside the surface , and that for u = -1 thereexists an unstable limit cycle outside the same surface.The controlled van der Pol system is given by? = ? (1)1 2 2 2 2 2 ? = -? ? + ? ? (1 - ? ? )? ?(2)2 1 1 2 Where ? , ? and ? are positive constants and u is the control input.By choosing ? = 0.2, ? = 1 and ? = 0.2 with controller u=1. The van der pol system wikk havestable limit cycle.1 The orbit circle radius is , where all trajectories in this region will converge to the orbit.? In order to prove that we need to transit the system equation to polar coordinates.2 ? 2 2 2 ? = ? +(3)2 2 ? ? 2 2 2? ? = 1 - ? ? ? (4)1 2 ? Program code:close all; clear all; clc;init_conds=[-1 1] ;tf=50;[t,states]=ode45('Dynamics_2a1', [0 tf], init_conds);x1= states(:,1);x2= states(:,2);figure; plot(x1,x2,'b','LineWidth',2); xlabel('x1');ylabel('x2');legend('x1-x2')grid;title('phase portrait X1-x2');figure; hold onplot(t,states(:,1),'k','LineWidth',2);plot(t,states(:,2),'b','LineWidth',2);xlabel('Time [Seconds]');ylabel('Initial conditions');legend('states (1)','states (2)')grid;title('plant versus time');hold offfunction Dynamics_2a1= Dynamics_2a1(t,ini) x= ini(1:2);% controller parameterse=0.2;w=1;mu=0.2; u=1;xdot(1) = x(2) ;xdot(2) = -w^2*x(1)+e*w*(1-mu^2*(x(1))^2)*x(2)*u; Dynamics_2a1(1:2)=xdot(1:2); Dynamics_2a1 = Dynamics_2a1'; Simulation result:(Stable limit Cycle Behavior) Program code:close all; clear all; clc;init_conds=[0 1] ;tf=50;[t,states]=ode45('Dynamics_2a2', [0 tf], init_conds);x1= states(:,1);x2= states(:,2);plot(x1,x2,'b','LineWidth',2);xlabel('x1');ylabel('x2');legend('x1-x2')grid;title('phase portrait X1-x2');hold offfigure(2)hold onplot(t,states(:,1),'k','LineWidth',2);plot(t,states(:,2),'b','LineWidth',2);xlabel('Time [Seconds]');ylabel('Initial conditions');legend('states (1)','states (2)')grid;title('plant versus time'); hold offfunction Dynamics_2a2= Dynamics_2a2(t,ini) x= ini(1:2);% controller parameterse=0.2;w=1;mu=0.2; u=-1;xdot(1) = x(2) ;xdot(2) = -(w^2)*x(1)+(e*w*(1-(mu^2*(x(1))^2))*x(2)*u); Dynamics_2a2(1:2)=xdot(1:2);Dynamics_2a2 = Dynamics_2a2'; Simulation result:(Unstable limit Cycle Behavior) 2 (b )Define the sliding manifold , where r < 1/µ. Show analytically that ifwe restrict the motion of the system to the surface s = 0 (that is, we force s(t) = 0), then theresulting behavior is that of the linear harmonic oscillator2 x? = x x? = -? x ,1 2 2 1 which exhibits a sinusoidal oscillation of frequency ? and amplitude r. The sliding manifold equation is given by2 ? 2 2 2 ? = ? + - ?2 2 ? 1 Where ? > . So ensure that s goes to zero? Let stud the dynamics of it.2 ? = 2? ? + ? ? 1 1 2 2 2 ? 2 2 2 2 2? = 2? ? + ? (-? ? + ? ? (1 - ? ? )? ? ) (6)1 2 2 1 1 2 2 ? 2? 2 2 2 ? = ((1 - ? ? )? ? )1 2 ? When ? (? ) = 0 then ? (? ) = 0 and from equation (6) we can observe that ? = 0. Therefore thestate equation will reduce to a harmonic oscillation.? = ?1 2 2 ? = -? ?2 1 2 2 2 Because the term ? ? (1 - ? ? )? ? tends to zero when u = 0.1 22 (c) Design a sliding mode controller that drives all trajectories whose initial condition is2withinthe region {x ? R : |x | < 1/µ} to the manifold s = 0, and then has the states slide on1 that manifold towards the origin. Simulate your controller and verify that it is able toregulate the state to zero. Show your plots of x vs time and x vs x to demonstrate your2 1design.2 To design sliding mode controller driver all trajectries in the region {x ? R : |x | < 1/µ}.1 The origin ? = 0 should be globally stabilized. The sliding mainfold defined 1 ? = ? ? + ?(7)1 2 In order to ensure that ? ? 0Let study the dynamics from equation (7)? = ? ? + ?1 2 2 2 2 2? = ? - ? ? + ? ? (1 - ? ? )? ? (8)2 1 1 2 1 2 Let ? = ? be a Lyapunov function candidate for?2 From eqauation (9) we obtain 2 2 2 2 ? = ? ? - ? ? + ? ? (1 - ? ? )? ?(9)2 1 1 2 We choose controller law such that 1 2 ? = -? + ? ? + ? ?(10)2 1 2 2 2 ? ? (1-? ? )? 1 2Program code:close all; clear all; clc; init_conds=[1 1] ;tf=5; [t,states]=ode45('Dynamics_2c', [0 tf], init_conds);x1= states(:,1);x2= states(:,2);figure(1)hold onplot(x1,x2,'b','LineWidth',2);xlabel('x1');ylabel('x2');legend('x1-x2');title('phase portrait X1-x2');grid onhold offfigure(2)hold onplot(t,states(:,1),'k','LineWidth',2);plot(t,states(:,2),'b','LineWidth',2);xlabel('Time [Seconds]'); ylabel('Initial conditions');legend('states (1)','states (2)') title('plant versus time');grid; hold offfunction Dynamics_2c= Dynamics_2c(t,ini)x= ini(1:2);% controller parameterse=0.2;w=1;mu=0.2;a=4;s =a*x(1)+x(2);k=5;gx =(e*w*(1-(mu^2*(x(1))^2))*x(2));u= 1/gx*(-sat(s)-a*x(1)+w^2*x(2)-k*s);xdot(1) = x(2) ;xdot(2) = -(w^2)*x(1)+(e*w*(1-(mu^2*(x(1))^2))*x(2)*u);Dynamics_2c(1:2)=xdot(1:2);Dynamics_2c = Dynamics_2c'; Simulation result: 3 MIMO Control of a Two-Link Planar Arm(a) First, you will do joint space MIMO control of the arm, where the inputs are the twotorquesand the outputs are the joint angles ? and ? . Write a MIMO state space1 2 representation of the system dynamics. Then design the MIMO state feedback controllers(t and t ) for this system that regulate all the states to zero, and simulate the closed loop1 2 system for the initial conditions . Make sure youtune well the parameters of controllers, plot your controllers and all the states versus time,and provide an interpretation of your plot.In object is to control a robot arm the as to joint. The arm has no movement in z axes,which mean the arm moves oly in the plan x y. The input on the system is torques ? AND ?1 2 the system dynamics can be derived using Lagrangian equation which is? ? ? ? ? -? ? -? ? - ? ? ? 1 1 11 12 1 2 1 2 1 ++= (1).? ? ? ?2 2 21 22 ? ? ? 0 ? 2 1 2 H11 = I1 + I2 + m1lc21 + m2[l12 + lc22 + 2l1lc2 cos( ?2)],H22 = I2 + m2lc22,H12 = H21 = I2 + m2[lc22 + l1lc2 cos( ?2)], h = m l l sin( ? ),2 1 c2 2 g = m l g cos( ? ) + m g[l cos( ? + ? ) + l cos( ? )], 1 1 c1 1 2 c2 1 2 1 1 g = m l g cos( ? + ? ).2 2 c2 1 2 In this section we will do joint space control, where the output of the system is ? and ? while1 2inputs are the two torques. The system state space equation defined as ? = ? , ? = ? , 1 1 3 2? = ? and ? = ?2 1 4 2 2? ? -? 2? ? ? + ? ? - ? + ? 1 1 2 2 1 1 1 22 12 = (2)2 2 H H -H -? ? 11 22 12 ? 21 112 -? ? - ? + ? 1 2 2 H 2 H 2 22 12 ? = 2? ? ? + ? ? - ? + ? - (-? ? - ? + ? )(3)1 2 1 2 2 1 1 2 1 2 2 H H -H H H -H 11 22 12 11 22 12-H 2 H 2 21 12 ? = 2? ? ? + ? ? - ? + ? + (-? ? - ? + ? )(4)2 2 1 2 2 1 1 2 1 2 2 H H -H H H -H 11 22 12 11 22 12 Let ? = ? and ? = ? then the system state space equation defended by1 1 2 2 ? = ?1 2 H H 22 12 2 2 ? = 2? ? ? + ? ? - ? + ? - (-? ? - ? + ? ) 2 2 4 4 1 1 2 2 2 2 2 H H - H H H - H 11 22 12 11 22 12 ? = ?3 4 -H H 21 11 2 2 ? = 2? ? ? + ? ? - ? + ? + (-? ? - ? + ? ) 4 2 4 4 1 1 2 2 2 2 2 H H - H H H - H 11 22 12 11 22 12 To design a control law for that system we need to differentiate the two outputs until theinput appears for the first time and do not vanish.? = ?1 1 ? = ? = ? (5)1 1 2 H H 22 21 2 2 ? = 2? ? ? + ? ? - ? + ? - (-? ? - ? + ? )1 2 4 4 1 1 2 2 2 2 2 H H - H H H - H 11 22 12 11 22 12 ? = ?2 3 ? = ? = ?(6)2 3 4 -H H 21 11 2 2? = 2? ? ? + ? ? - ? + ? - (-? ? - ? + ? )2 2 4 4 1 1 2 2 2 2 2 H H - H H H - H 11 22 12 11 22 12 From equation (5) and (6) we getH H H -H 22 12 22 21 2 22? ? ? + ? ? - ? - (-? ? - ? ) 2 2 4 4 1 2 2 2 2 2 ? ?H H -H H H -H H H -H H H -H 1 1 11 22 12 11 22 12 11 22 12 11 22 12 =+ (7)-H H -H H ? ? 21 2 11 2 21 11 2 22? ? ? + ? ? - ? + (-? ? - ? ) 2 2 4 4 1 2 2 2 2 2 H H -H H H -H H H -H H H -H 11 22 12 11 22 12 11 22 12 11 22 12From equation (7) we obtain the control law in the form? 1 -1 = ? ?-? ? - ? ? ? 2 ? 1 ? 0 0 1 ? ? 11 12 Where ? = ? =? ? ? 2 0 0 21 22 ?2 Program code:close all; clear all; clc;global m1 m2 l1 l2 lc1 lc2 I1 I2 g;m1=1;m2=1;l1=1;l2=1;lc1=0.5;lc2=0.5;I1=0.2;I2=0.2;g=9.80; init_conds=[pi/2 0 -pi/2 0];tf=10;[t,x]=ode45('derivatives3a', [0 tf], init_conds); figure(1)hold onplot(t,x(:,1),'k','LineWidth',2);plot(t,x(:,2),'b','LineWidth',2);plot(t,x(:,3),'r','LineWidth',2);plot(t,x(:,4),'m','LineWidth',2);xlabel('Time [Seconds]'); ylabel('Angular velocity [rad/s]');legend('x 1','x 2','x 3','x 4')grid;title('plant versus time');hold off%% Re-compute controlH11=I1+I2+m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(x(3)));H22=I2+m2*lc2^2;H12=I2+m2*(lc2^2+l1*lc2*cos(x(3)));H21=I2+m2*(lc2^2+l1*lc2*cos(x(3)));h=m2*l1*lc2*sin(x(3));g1=m1*lc1*g*cos(x(1))+m2*g*(lc2*cos(x(1)+x(3))+l1*cos(x(1)));g2=m2*lc2*g*cos(x(1)+x(3)); y1=x(:,1);y2=x(:,3);y1dot=x(:,2);y2dot=x(:,4);K=[24 50 0 0;0 0 35 10];for i=1:length(t)B=[(H22/(H11*H22-H12^2)) (-H21/(H11*H22-H12^2)); (-H21/(H11*H22-H12^2))(H11/(H11*H22-H12^2))];A=[(((H22/(H11*H22-H12^2)).*(2*h.*x(i,2).*x(i,4)+h.*x(i,4).^2-g1))+((- H21/(H11*H22-H12^2)).*(-h.*x(i,2).^2-g2)))...; (((-H21/(H11*H22-H12^2)).*(2*h.*x(i,2).*x(i,4)+h.*x(i,4).^2- g1))+((H11/(H11*H22-H12^2)).*(-h.*x(i,2).^2-g2)))];v=-K*[y1(i);y1dot(i);y2(i);y2dot(i)];u(:,i)=inv(B)*(-A+v); endu=u';figure(2)plot(t,u,'LineWidth',2);grid;legend('u_1','u_2'); xlabel('Time [Seconds]'); ylabel('Angular velocity [rad/s]');function x_dot = derivatives3a(t,x)%% Function variablesglobal m1 m2 l1 l2 lc1 lc2 I1 I2 g ;m1=1;m2=1;l1=1;l2=1;lc1=0.5;lc2=0.5;I1=0.2;I2=0.2;g=9.80;H11=I1+I2+m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(x(3)));H22=I2+m2*lc2^2;H12=I2+m2*(lc2^2+l1*lc2*cos(x(3)));H21=I2+m2*(lc2^2+l1*lc2*cos(x(3)));h=m2*l1*lc2*sin(x(3));g1=m1*lc1*g*cos(x(1))+m2*g*(lc2*cos(x(1)+x(3))+l1*cos(x(1)));g2=m2*lc2*g*cos(x(1)+x(3));%% Control computation% Output vector and output derivativesy1=x(1);y2=x(3);y1dot=x(2); y2dot=x(4);% Input-output dynamicsB=[(H22/(H11*H22-H12^2)) (-H21/(H11*H22-H12^2)); (-H21/(H11*H22-H12^2))(H11/(H11*H22-H12^2))];A=[(((H22/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2-g1))+((-H21/(H11*H22- H12^2))*(-h*x(2)^2-g2)))...; (((-H21/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2-g1))+((H11/(H11*H22- H12^2))*(-h*x(2)^2-g2)))];% Controk gain and stabilization vector vK=[1 2 0 0;0 0 2 1];v=-K*[y1;y1dot;y2;y2dot];% IOFL controlleru=inv(B)*(-A+v);%% Plant dynamicsx1_dot=x(2);x2_dot=((H22/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2-g1+u(1)))+((- H21/(H11*H22-H12^2))*(-h*x(2)^2-g2+u(2)));x3_dot=x(4);x4_dot=((-H21/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2- g1+u(1)))+((H11/(H11*H22-H12^2))*(-h*x(2)^2-g2+u(2)));x_dot = [x1_dot;x2_dot;x3_dot;x4_dot]; Simulated results: 4 (b).In this part we want to control the end-effector of the arm, which meat the outputsare the position coordinates x and y. The (x; y) coordinates of the effector can be represented inthe owing equations ? = ? cos ? + ? ? ? ? ? + ? (8)1 1 2 1 2? = ? sin ? + ? ? ? ? ? + ? (9)1 1 2 1 2 Differentiate the position equation (8) and (9) to get the velocities ? = -? ? sin ? + ? ? + ? ? ? ? ? + ? (10)1 1 1 2 1 2 1 2? = ? ? cos ? + ? ? + ? ? ? ? ? + ? (11)1 1 1 2 1 2 1 2 From equation (10) and (11) we can show the relationship between the vectors of linearvelocities of the end effector velocities. -? sin ? - ? ? ? ? ? + ? -? ? ? ? ? + ? ? ?1 1 2 1 2 2 1 2 1 = (12)? ? cos ? + ? ? ? ? ? + ? ? ? ? ? ? + ? 1 1 2 1 2 2 1 2 ? 2To obtain the control law we need to differentiate the output until the input papers forthe first tile and do not vanish. ? = ? cos ? + ? cos ? + ? 1 1 1 2 1 3 ? = -? ? sin ? - ? + ?? sin ? + ? 1 1 1 1 1 3 2 1 2 2 2 ? = ? ? cos ? - ? ? sin ? - ? ? cos ? + ? - ? ? ? cos ? + ?1 2 2 1 1 2 1 2 2 1 3 2 2 4 1 3 - ? ? sin ? + ? - ? ? ? cos ? + ? - ? ? sin ? + ? 2 2 1 3 2 2 4 1 3 2 4 1 3? = ? sin ? + ? sin ? + ?2 1 1 2 1 3? = ? ? cos ? + ? + ? ? cos ? + ?2 1 1 1 1 3 2 1 2 2 2? = -? ? sin ? + ? ? cos ? + ? ? cos ? + ? + ? ? ? sin ? + ? 1 2 2 1 1 2 1 2 2 1 3 2 2 4 1 3 - ? ? cos ? + ? + ? ? ? sin ? + ? - ? ? cos ? + ? 2 2 1 3 2 2 4 1 3 2 4 1 3We can simplify the system in this form ? = ? ? + ? ?(13)Where J is the jacobian matrix ,We want to track the reverens? = 2 ?? ? ? = ? ? ??30 ? = ? - ?1 1 1? = ? - ? (14)1 1 1 ? = ? - ? 1 1 1 ? = ? - ?2 2 2 ? = ? - ?(15)2 2 2 ? = ? - ? 2 2 2 From (13),(14) and (15) we obtain control law will be in the form-1 -1 ? = ? -? ? + ? - ? ?(16)Assume that ? = ? + ? - ? and ? = -? ? - ? ?Program code:close all; clear all; clc;x0 = [-pi/4 0 0 0]; % joint variables[t,x]=ode45('derivatives3b',30,x0);figure(5) for j=1:length(t);% Feed all the dynamic states herex1 = x(j,1);x2 = x(j,3);[T1x(j),T1y(j),T2x(j),T2y(j)]=ForwardKinematics_2DOF(x1,x2);clf;axis([-2 2 -2 2]); grid; hold online([0, T1x(j)], [0, T1y(j)]);line([T1x(j), T2x(j)], [T1y(j), T2y(j)]);line([sqrt(2), sqrt(2)], [1, -1],'LineWidth',2.5,'Color','red');xlabel('Horizontal position (m)'),ylabel('Vertical position (m)');title('Two-link arm animation demo')drawnow;plot(0,0,'ro');plot(T1x(j),T1y(j),'ro');animation_mov(j)=getframe(5);end;function x_dot = derivatives3b(t,x)%% Function variablesglobal m1 m2 l1 l2 lc1 lc2 I1 I2 g ;m1=1;m2=1;l1=1;l2=1;lc1=0.5;lc2=0.5;I1=0.2;I2=0.2;g=9.80;H11=I1+I2+m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(x(3))); H22=I2+m2*lc2^2;H12=I2+m2*(lc2^2+l1*lc2*cos(x(3)));H21=I2+m2*(lc2^2+l1*lc2*cos(x(3)));h=m2*l1*lc2*sin(x(3));g1=m1*lc1*g*cos(x(1))+m2*g*(lc2*cos(x(1)+x(3))+l1*cos(x(1)));g2=m2*lc2*g*cos(x(1)+x(3));%% Control computation% Output vector and output derivativesy1=l1*cos(x(1))+l2*cos(x(1)+x(3));y2=l1*sin(x(1))+l2*sin(x(1)+x(3));y1dot=-l1*x(2)*sin(x(1))-l2*(x(2)+x(4))*sin(x(1)+x(3));y2dot=l1*x(2)*cos(x(1))+l2*(x(2)+x(4))*cos(x(1)+x(3));e1=y1-2^(1/2);e1dot=y1dot-0 ;e2=y2-sin(pi*t/30);e2dot=y2dot- (pi*t/30)*cos(pi*t/30);J=[(-l1*sin(x(1))-l2*sin(x(1)+x(3))) (-l2*sin(x(1)+x(3))) ; (l1*cos(x(1))- l2*cos(x(1)+x(3))) (l2*cos(x(1)+x(3)))];thetadot=[x(2); x(4)];Jdot=[(-l1*cos(x(1))*x(2)-l2*cos(x(1)+x(3)))*(x(2)+x(4)) (- l2*cos(x(1)+x(3))*(x(2)+x(4))) ; (-l1*sin(x(1))*x(2)-l2*sin(x(1)+x(3))*(x(2)+x(4))) (- l2*sin(x(1)+x(3))*(x(2)+x(4)))]; %%% Input-output dynamicsB=[(H22/(H11*H22-H12^2)) (-H21/(H11*H22-H12^2)); (-H21/(H11*H22-H12^2))(H11/(H11*H22-H12^2))];%%A=[(((H22/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2-g1))+((-H21/(H11*H22- H12^2))*(-h*x(2)^2-g2)))...; (((-H21/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2-g1))+((H11/(H11*H22- H12^2))*(-h*x(2)^2-g2)))];%%% Controk gain and stabilization vector vK=[1 2 0 0;0 0 2 1];v=-K*[e1;e1dot;e2;e2dot];%%% IOFL controlleru=inv(B)*((inv(J)*(-Jdot*thetadot+v))-A); %% Plant dynamicsx1_dot=x(2);x2_dot=((H22/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2-g1+u(1)))+((- H21/(H11*H22-H12^2))*(-h*x(2)^2-g2+u(2)));x3_dot=x(4); x4_dot=((-H21/(H11*H22-H12^2))*(2*h*x(2)*x(4)+h*x(4)^2- g1+u(1)))+((H11/(H11*H22-H12^2))*(-h*x(2)^2-g2+u(2)));x_dot = [x1_dot;x2_dot;x3_dot;x4_dot];Simulation result: "