Download - EML6934_Ramin_Shamshiri_Project1_Part1
1 Ramin Shamshiri EML 6934, Nonlinear control systems
EML 6934, Nonlinear Control Systems
Final project
Spring 2010
Instructor: Dr. W. Dixon
Tracking Control of a Robot Manipulator
with uncertainties and disturbances
RAMIN SHAMSHIRI
UFID#: 9021-3353
Due: 05.April.2010
2 Ramin Shamshiri EML 6934, Nonlinear control systems
I. Problem Overview
The objective of this project was to design and
simulate the performance of a tracking controller for
a two-link robot manipulator shown in Fig. 1, with
uncertain system parameters, and in the presence of
bounded disturbances.
Figure 1. Two link robot setup
II. Dynamic Model
The dynamics associated with the two-link robot are
given by (1),
π1
π1 =
π1 + 2π3π2 π2 + π3π2
π2 + π3π2 π2 π 1π 2
+ βπ3π 2π 2 βπ3π 2 π 1 + π 2
π3π 2π 1 0 π 1π 2
+ ππ1 00 ππ2
π 1π 2 +
π 1π 2
(1)
where π1 = 3.473 ππ.π2 , π2 = 0.196 ππ.π2 ,
π3 = 0.242 ππ.π2 , ππ1 = 5.3 ππ. π ππ , ππ2 =1.1 ππ. π ππ , π1 , π2 denotes the angular position of
the robot links, π2 = cos(π2), π 2 = sin(π2), π1 and
π2 represents torque control inputs.
The Euclidean position of the endpoints of the second
robot link is denoted by π₯ β [π₯1 π₯2] which is
related to the joint space through the following
kinematic relationship
π₯ = π½ π π (2)
Where the manipulator Jacobian π½ π β β2Γ2 is
given by (3)
π½ π =
βπ1 sin π1 β π2 sin π1 + π2 βπ2 sin π1 + π2
βπ1 cos π1 + π2 cos π1 + π2 π2 cos π1 + π2
(3)
The link lengths π1 , π2 of the robot are selected as
π1 = π2 = 0.5π
III. Control Objective
To design tracking controllers for the following two
tasks;
1. The robot tracks the desired joint space trajectory
given by
ππ = cos π‘ cos π‘ π (4)
2. The robot tracks the desired task space trajectory
given by
π₯π = 0.5sin π‘ 1 π (5)
Assume that the system parameters π1 , π2 , π3 , ππ1,ππ2 are unknown bounded positive constants (except
for the EMK case where these constants are exactly
known). Design and implement the following
controllers to achieve the control objective:
1. Exact Model Knowledge (EMK) controller.
2. Adaptive controller.
3. Sliding mode controller.
4. High gain controller.
In each case, comment on how the bounded
disturbance ππ = 2 sin π‘ 0.5 sin π‘ π affects the
stability of the system.
3 Ramin Shamshiri EML 6934, Nonlinear control systems
IV. Project Report
Abstract
The objective of this project was to design and
simulate a tracking controller to track a desired
trajectory given by ππ = πππ π‘ πππ π‘ π for a
two-link robot manipulator with uncertain system
parameters, and in the absence and presence of
bounded disturbances. Four different controllers
were designed for the manipulator in the presence of
uncertain system parameters (except the EMK
controller, where the system is exactly described).
Simulation of the controllers and manipulator were
done in MATLAB SIMULINK and adaptive estimates
(where necessary) obtained. The Lyapunov stability
analysis for each controller was performed. Finally,
the controllers were compared against each other
based on their performance and control effort.
Report summary
The exact model knowledge controller showed the
best stability result (GES), compared with all other
controllers. It also had low control effort. In this
approach, all the nonlinearities that exist in the
system are known exactly, thus we used feedback
linearization to cancel them. The problem with this
design is that we can never model a physical system
perfectly to have exact knowledge of the system
parameters. In addition to that, when disturbances are
introduced in the system, the controller will lose its
perfect performance and result in a globally
uniformly ultimately bounded stability. In summary,
while EMK controller might be easy to implement, it
is not the best design option for real nonlinear
systems.
The designed adaptive controller yielded a globally
asymptotically stability which means errors go to
zero asymptotically. In this design, in order to
improve performance, we need to tune the gains to
ensure that the control torques are within the actuator
limits while maintaining a good tracking
performance. Using high gain would come at the cost
of increasing control effort, however it also ensure
that any error estimate of the parameters will be
compensated. Simulation result showed that the
adaptive controller is more efficient especially in the
presence of disturbances, which a globally
exponentially stability result was concluded.
In the sliding mode controller (SMC) design, the
controller provides fast switching action to cancel the
nonlinearities and disturbances in the system. This
design gave a globally exponentially stability result,
however this comes at a high control effort and
require an actuator to switch infinitely fast in time
which makes it almost impossible to implement in
reality. Because of the discontinuous nature of this
controller, tuning of gains is almost useless to archive
GES result, which theoretically should have been the
case.
The high gain controller gave uniformly ultimately
bounded stability result in the presence of uncertain
model dynamics and bounded disturbances. While
EMK controllers cannot handle disturbances at all
and Adaptive controllers give a GAS result, high gain
controllers crush the uncertainties at the cost of high
control effort. Perhaps one advantage of this design
was the easiness of the control implementation and
gain tuning.
4 Ramin Shamshiri EML 6934, Nonlinear control systems
4.1. Description of the dynamic model
The dynamics of the two-link robot given in (1) can
be written as
π(π)π + ππ (π, π )π + πππ + ππ(π‘) = π (6)
Where π β β2Γ2 is the inertia matrix, ππ β β2Γ2
denotes the centripetal-Coriolis matrix, ππ β β2
denotes friction, ππ(π‘) is a general nonlinear
bounded disturbance modeled as
ππ = 2sin π‘ 0.5sin π‘ π , π(π‘) represents the
torque input control vector and π π‘ , π π‘ , π π‘ denote the link position, velocity, and acceleration
vectors, respectively. 4.2. Development of the error system
We define a position tracking error (7) and filtered
tracking error (8) as follow:
π = π β ππ (7)
π = π + πΌπ (8)
where πΌ is a positive diagonal gain matrix.
Differentiation (8) with respect to time and
multiplying by π yields the open loop error system
as given in (9):
ππ = ππ + ππΌπ
= ππ β ππ π + ππΌπ (9)
Substituting for ππ using the dynamic in (6):
ππ = βππ π, π π β πππ β ππ π‘ + π β ππ π
+ ππΌπ (10)
Substituting for π using time differentiation of (7)
and (8):
ππ = βπππ π + πππΌπ β πππ β πππ β ππ π
β ππ π‘ + π + ππΌπ (11)
We can write (11) in the following form:
ππ = ππ β πππ β ππ π‘ + π (12)
where ππ = βπππ π + πππΌπ β πππ β ππ π + ππΌπ . The controllers will be designed based on the open
loop error system given in (12).
4.3. Assumptions
The development of the controllers is based on the
following assumptions:
o q(t) and q (t) are measurable.
o M q , Vm q, q , ππ and Οd (t) are unknown.
o q, q , q d and q d exists and β Lβ .
o Οd , Ο d , Ο d β Lβ
4.4. Properties
o The inertia matrix π(π) is symmetric and
positive definite (PD) and satisfies the following
inequality βy t β βn :
π1 π¦ 2 β€ π¦ππ π π¦ β€ π π π¦ 2 (13.1)
where m1 β β is a known positive constant, m q ββ is a known positive function, and . denotes the
standard Euclidean norm.
o If q t , q (t) β Lβ , then the first and second
partial derivative of the elements of M q and
Vm q, q with respect to q and the first and
second partial derivative of the elements
Vm q, q and fd with respect to q exist and are
bounded.
o The time derivative of the inertia matrix and the
centripetal Coriolis matrix satisfy the following
skew symmetric relationship
ππ 1
2π π β ππ π, π π = 0 βπ β βπ (13.2)
5 Ramin Shamshiri EML 6934, Nonlinear control systems
4.5. Exact Model knowledge (EMK) controller
4.5.1. Controller design
Assuming to have exact knowledge about dynamic of
the system, we design a controller to cancel all the
nonlinearities in the system, thus the external
disturbance will be the only term left undefined. To
this purpose, using the dynamic in (12), the control
torque is designed such that π β 0,
π = βππβπΎπ (14)
where K is the control gain constant matrix (K > 0).
Substituting this torque in the dynamic yields the
closed loop error system as follow:
ππ = βπππ β πΎπ β ππ (15)
4.5.2. Lyapunov stability analysis
Considering a PD, continuously differentiable
candidate Lyapunov function as
π =1
2ππππ (16)
and differentiating it with respect to time,
π =1
2πππ π + ππππ (17)
Substituting the expression for ππ into (17);
π =1
2πππ π + ππ(βπππ β πΎπ β ππ) (18)
Using the property, ππ 1
2π π β Vm q, q π = 0,
where π = π in this case, we can simplify and write
(18) as follow:
π =1
2πππ π β πππππ β πππΎπ β ππππ
= βππ(πΎπ + ππ)
(19)
Now, we can upper bound π as:
π β€ β K π 2 β πTππ (20)
In the absence of disturbances ( ππ = 0 ), the
inequality in (20) will reduce to:
π β€ β K π 2 (21)
Using the expression for V In (16), we can write (21)
as the following differential equation;
π β€ β 2Kπ (22)
which has the solution of the form
π π‘ β€ π 0 ππ₯π β2πΎπ‘
ππππ₯ π (23)
From (23), we can conclude that the system is
Globally exponentially stable (GES) using this
controller. It can be observed that π π‘ exponentially
goes to zero and is β πΏβ , also π and π
exponentially go to zero and are β πΏβ . Since ππ is a
function of π and π , it is also β πΏβ , therefore the
control input torque (π) is also β πΏβ .
In the presence of disturbances (ππ β 0), we can
upper bound π in (20) by
π π‘ β€ β K π 2β π π (24)
where c is a positive constant such that Οd β€ c. In
this case, we can conclude a Globally uniformly
ultimately bounded (GUUB) stability result where
the ultimate bound is given by π =π
πΎ.
4.5.3. Simulation
The performance of the designed EMK controller
was simulated in MATLAB in the presence and
absence of external disturbance (Figure 2 and 3). The
corresponding error and torque plots are shown in
Figure 4 through 7.
Figure 2. Error plots, EMK controller without
external disturbances
6 Ramin Shamshiri EML 6934, Nonlinear control systems
Figure 3. Torque plots, EMK controller without
external disturbances
Figure 4. Error plots, EMK controller with
external disturbances
Figure 5. Torque plots, EMK controller without
external disturbances
4.5.4. Discussion:
We assumed exact knowledge of the model
parameters to design a controller to cancel out the
nonlinearities in the system using feedback
linearization which yielded a globally exponential
stability result. It can be observed from Figure 2 that
error exponentially converges to zero when there is
no external disturbances to the system. In the
presence of unknown, bounded disturbances, as seen
from Figure 4, the control torque designed based on
the known parameters is not capable of producing
exactly zero error result. Therefore, errors converge
to a small region and stay within that region. In the
other words, not all nonlinearities are cancelled and
we have globally ultimately bounded result. It can
also be seen from Figure 3 and 5 that torques are
bounded and in the range of -10 Nm to 30Nm for the
no disturbance case and within a range of -50Nm to
150 Nm in the presence of disturbance. The
simulation blocks are shown in Figure 6 and 7.
7 Ramin Shamshiri EML 6934, Nonlinear control systems
Figure 6. Exact Model Knowledge control simulation diagram without external disturbance
Figure 7. Exact Model Knowledge control simulation diagram with external disturbance
8 Ramin Shamshiri EML 6934, Nonlinear control systems
4.6. Adaptive controller
The objective of adaptive control is to achieve a
trajectory tracking task in the presence of unknown
parameters, thus, the controller should be designed
based on an estimate of the system model.
4.6.1. Controller design
From (12), we design the control torque as follow:
π = βππ β πΎπ (25)
where K is a positive control gain matrix and
ΞΈ represents an estimate of the system parameters.
Substituting (25) in (12), yields the following closed
loop error system.
ππ = βππ β πππ β Kr β ππ (26)
where π = π β π is he difference between the actual
and estimated parameters. Differentiation ΞΈ with
respect to time gives ΞΈ = βΞΈ (since ΞΈ is assumed to
be a constant). In order to design π , we consider a continuously differentiable, positive-definite Lyapunov function candidate given by
π =1
2ππππ +
1
2π ππ€β1π (27)
Differentiating (27) with respect to time and
substituting for ΞΈ with βΞΈ yields;
π =1
2πππ π +
1
2π πππ +
1
2ππππ β π π€β1ΞΈ (28)
Substituting for ππ with (26) ;
π =1
2πππ π + ππ(βππ β πππ β Kr β ππ)
β π π€β1ΞΈ (29)
Using the skew symmetric property (13.2),
( 1
2πππ π β πππππ = 0) we can write (29) as:
π = βππππ β ππ Kr β ππππ β π π€β1ΞΈ (30)
Now we can design π as:
π = βπ€πππ (31)
The expression for ΞΈ can then be find by integrating
π , ( ΞΈ = βπ€πππ ππ‘).
4.6.2. Lyapunov stability analysis:
Substituting the designed π in the expression for π given by (30) yields;
π = βπΎπ2 β ππππ (32)
We can now upper-bound (32) by:
π β€ β K π 2β ππππ (33)
In the absence of external disturbance, ππ = 0 hence
the expression in (33) reduces to π β€ β K π 2 .
Since π is a positive definite function and π is
negative semi definite, we can conclude that π β πΏβ .
In addition, since π is a function of π and π , then π
and π are also β πΏβ . Being π bounded, we have
π, π β πΏβ , thus π, π β πΏβ which means π β πΏβ .
Also since π ,π β πΏβ , then π β πΏβ , thus π β πΏβ .
Therefore, π is continuously differentiable Since π is
square integrable (π β πΏ2 ) and uniformly continuous,
using the corollary to Barbalatβs lemma, we can
prove that π β 0 as π‘ β 0 , hence we can conclude
that using this controller, and in the absence of
external disturbances, the system is asymptotically
stable. The input controller torque in (25) is a
function of bounded terms and is therefore bounded.
In the presence of a bounded external disturbance,
ππ β 0, assuming that ππ < π, we can upper bound
(32) by
π β€ β K π 2β π π (33.1)
Which results in a globally uniformly ultimately
bounded (GUUB) stability result. To calculate the
ultimate bound, we set β K π 2 β π π = 0, which
yields π >π
πΎ.
9 Ramin Shamshiri EML 6934, Nonlinear control systems
4.6.3. Simulation
The performance of the designed adaptive controller was simulated in MATLAB in the presence and absence of
external disturbance (Figure 8 and 9).
Figure 8. Adaptive control simulation diagram without external disturbance
Figure 9. Adaptive control simulation diagram with external disturbance
10 Ramin Shamshiri EML 6934, Nonlinear control systems
Figure 10. Error plots, adaptive controller without
external disturbances
Figure 11. Error plots, adaptive controller with
external disturbances
Figure 12. Torque plots, adaptive controller
without external disturbances
Figure 13. Torque plots, adaptive controller with
external disturbances
Figure 14. Plots of estimated parameters, p1, p2
and p3, adaptive controller, no disturbance
Figure 15. Plots of estimated parameters, p1, p2
and p3, adaptive controller, with disturbance
11 Ramin Shamshiri EML 6934, Nonlinear control systems
Figure 16. Plots of estimated parameters, fd1 and
fd2, adaptive controller, no disturbance
Figure 17. Plots of estimated parameters, fd1 and
fd3, adaptive controller, with disturbance
4.6.4. Discussion
Since we have uncertain parameters in the system, it
is necessary to use adaptive control to design control
torque. As we can see in Figure 14, 15, 16 and 17,
ultimately, the parameter estimates reach close to the
actual values given by the problem. It can also be
seen from Figure 10 and 11 that the error
asymptotically converges to zero in the absence of
external disturbances. In the case of having
disturbance in the system, the error wiggles around
zero, indicating an ultimately bounded stability
result. It can also be seen from the torque profiles that
the torques are within the prescribed limits.
4.7. Sliding mode control
When the parameters in the model are all unknown,
nonlinear robust sliding mode control is an approach
to compensate uncertainties in the system by crushing
them through fast switching. This approach comes at
the cost of high or even infinite control effort. In the
other words, we would need an actuator that switches
infinitely.
4.7.1. Controller Design:
For this design, we need to consider the following
upper-bounds to the dynamic expressed in (11):
ππ = cos π‘ cos π‘ π
β΄ ππ , π π , π π β€ 1,
β΄ πππ , ππ π , ππ π β€ π,
β΅ π β€ π (34)
ππ = 2 sin t 0.5 sin t π
β΄ ππ β€ 1
We can also group the terms in (12) as follow:
ππ = π β πmr + π½ (35)
where π½ = ππ β ππ can be upper bounded by
π β€ π1 + π2 π§ + π3 π§ 2 , where π§ = ππ ππ π .
Therefore we design the control torque as:
π = βπΎπ β ππ ππ(π) (36)
where πΎ is a positive control gain matrix, π is a
positive constant and π ππ(π) is the sign function of
π . Substituting (36) in (35) yields the closed loop
error system:
ππ = βπΎπ β ππ ππ(π) β πmr + π½ (37)
4.7.2. Stability Analysis:
Considering a continuously differentiable, positive-
definite Lyapunov function candidate given by
π =1
2ππππ (38)
Differentiating (38) with respect to time,
π =1
2πππ π + ππππ (39)
12 Ramin Shamshiri EML 6934, Nonlinear control systems
Substituting (4.37) in (39):
π =1
2πππ π + ππ(βπΎπ β ππ ππ(π) β πmr + π½) (40)
using the skew symmetry property given by (13.2) to
simplify (40)
π = ππ(βπΎπ β ππ ππ(π) + π½) (40.1)
Now we can upper bound π as follow:
π β€ βπΎ π 2 β ππππ ππ(π) + πππ½
β΄ π β€ βπΎ π 2 β π π + π π
β΄ π β€ βπΎ π 2
(41)
From (41), we can get π β€ β2πΎ
ππππ₯ π π(t), where
ππππ₯ {π} gives the maximum eigenvalue of matrix
π. Therefore, we have:
π(t) β€ π 0 eβ
2πΎπ‘ππππ₯ {π} (42)
From (42), it can be concluded that the system is
Global Exponential stability (GES). Since π(π‘)
exponentially converges to 0, π also exponentially
converges to 0. Therefore, π β 0 ππ π‘ β 0. Since
the disturbance was included within the uncertain
model, no separate stability analysis is necessary for
a case with disturbance. The constants chosen squash
all the unknown nonlinearities in the system.
However, robust controllers for a system with and
without disturbances are modeled and simulated.
4.7.3. Simulations
Figure 18. Error plots, Sliding mode controller
without external disturbances
Figure 19. Error plots, Sliding mode controller
with external disturbances
Figure 20. Torque plots, Sliding mode controller
without external disturbances
Figure 21. Torque plots, Sliding mode controller
with external disturbances
13 Ramin Shamshiri EML 6934, Nonlinear control systems
4.7.4. Discussion
In this design, we can theoretically achieve a globally
exponentially stability result, however errors do not
exactly converge to zero, because of the
discontinuous nature of the controller and since the
torque limitations impose constraints on the gain
selection. Having upper bounded the unknown
dynamics and disturbances by functions of states; the
sliding mode controller manages to crush the
nonlinearities, however it is not possible to achieve
GES when the unknown dynamics cannot be
modeled as a function of states, but upper-bounded
by a constant. In this case, we can conclude a
globally uniformly ultimately bounded stability
result. From the torque plots, we can see that it has
been switching infinitely which in reality, there is no
actuator to handle such a bandwidth, thus this
controller is not implementable on a real system.
Figure 22. Sliding mode control simulation diagram without external disturbance
Figure 23. Sliding mode control simulation diagram without external disturbance
14 Ramin Shamshiri EML 6934, Nonlinear control systems
4.8. High gain control
In this approach, we design torque control input in
such a way that it crushes the uncertainties in the
system through high gain. This design leads to a high
control effort. 4.8.1. Control design
Using the dynamic in (35), we design the control
torque as:
π = βπΎπ (43)
where πΎ is a positive control gain matrix.
Substituting (43) in (35) yields the closed loop error
system as follow:
ππ = βπΎπ β πmr + π½ (44)
4.8.2. Stability Analysis
Considering a continuously differentiable, positive-
definite Lyapunov function candidate given by (38)
and its time derivative given in (39), substitution for
ππ in (39) using (44) yields;
π =1
2πππ π + ππ(βπΎπ β πmr + π½) (45)
using the skew symmetry property given by (13.2) to
simplify (40)
π = ππ(βπΎπ + π½) (46)
Now we can upper bound π as follow:
π β€ βπΎ π 2 + πππ½ β΄ π β€ βπΎ π 2 + π π
(47)
From (47), we can see that βπΎ π 2 dominates π π
by large πΎ, and as long as this happens, π(π‘) will be
decreasing. But because we have π 2 , the term
βπΎ π 2 will eventually be dominated by π . In the
other words, at some point, π will be small enough
that βπΎ π 2 will be dominated by π π , and since π
is a positive definite, radially unbounded and
decrescent function, we conclude uniformly ultimate
bound stability result. This can be shown by letting
πΎ = π1 + π2 and completing the square term in (47)
as follow:
π β€ βπ1 π 2 β π2 π
2 β π π +π2
4π22 +
π2
4π2 (48)
The inequality in (48) can be written as
π β€ β2π1
π.1
2πππ + πΆπ
β΄ π β€ βπΆππ + πΆπ
(49)
where πΆπ =2π1
π πππ πΆπ =
π2
4π2. The solution to (49) is
π(π‘) β€ πΆππ(0)eβ
π1π‘
ππππ₯ {π } + πΆπ , where ππππ₯ {π}
gives the maximum eigenvalue of matrix π .
Therefore, we have uniformly ultimate bounded
(UUB) stability result.
4.8.3. Simulations
Figure 24. Error plots, high gain controller,
without disturbances, black:K=1, red:K=10,
blue:K=50, green:K=500.
Figure 25. Error plots, high gain controller, with
disturbances, K=25
15 Ramin Shamshiri EML 6934, Nonlinear control systems
Figure 26. Torque plots, high gain controller,
without disturbances, K=10.
Figure 27. Torque plots, high gain controller,
without disturbances, K=25.
4.8.4. Discussions:
Plots of error for different gains are shown in Figure 24. It can be seen that, as we increase the gain, error get closer
to zero. In reality, increasing gain require high control effort. One advantage of this controller is its simple design
structure which makes it easy to tune and implement. The controller is robust to the uncertainties in the system,
however it only yields a uniformly ultimately bounded stability result.
16 Ramin Shamshiri EML 6934, Nonlinear control systems
V. Appendix
The Robot model remains the same for all the controllers; the MATLAB code for the robot model is provided once,
with the EMK controller. For the sake of keeping the report short, only the controllers with disturbance are shown.
The controllers without disturbance in the system can be modeled with Td = 0.
% April.05.2010, Ramin Shamshiri, [email protected]
% Dept of Ag & Bio Eng
% University of Florida, Gainesville, Florida
APPENDIX 1. Robot dynamic
% Robot manipulator dynamics.
function q_ddot = Manipulator_dynamics(Torque, Td, q, q_dot)
%% Parameter description:
p1 = 3.473; p2 = 0.196; p3 = 0.242;
fd1 = 5.3; fd2 = 1.1;
% Joint position and velocity signals:
q1 = q(1); q2 = q(2); % Joint angles
q1_dot = q_dot(1); q2_dot = q_dot(2); % Joint angular velocities
% Model description:
M = [p1+2*p3*cos(q2) p2+p3*cos(q2); p2+p3*cos(q2) p2 ];
Vm = [-p3*sin(q2)*q2_dot -p3*sin(q2)*(q1_dot+q2_dot);
p3*sin(q2)*q1_dot 0 ];
Fd = [fd1 0;0 fd2];
% Output for feedback - "q_ddot"
% From the given equation of dynamics
q_ddot = inv(M)*(Torque - Vm*q_dot - Fd*q_dot - Td);
APPENDIX 2:. EMK Controller
function [Torque, e]= EMK_controller(t, alf, K, q, q_dot)
% Desired trajectory:
qd = [cos(t);cos(t)];
qd_dot = [-sin(t);-sin(t)];
qd_ddot = [-cos(t);-cos(t)];
% Error definition for the Tracking controller
e = q - qd;
e_dot = q_dot - qd_dot;
% Filter Tracking Error
r = e_dot + alf*e;
% Parameter description, given Exact Model Knowledge:
p1 = 3.473; p2 = 0.196; p3 = 0.242;
fd1 = 5.3; fd2 = 1.1;
% Joint position and velocity signals:
q1 = q(1); q2 = q(2); % Joint angles
q1_dot = q_dot(1); q2_dot = q_dot(2); % Joint angular velocities
% Model description:
M = [p1+2*p3*cos(q2) p2+p3*cos(q2);p2+p3*cos(q2) p2 ];
Vm = [-p3*sin(q2)*q2_dot -p3*sin(q2)*(q1_dot+q2_dot);
p3*sin(q2)*q1_dot 0 ];
Fd = [fd1 0;0 fd2];
% Control Torque based on exact model knowledge
Torque = Vm*qd_dot + Fd*q_dot + M*qd_ddot - Vm*alf*e - alf*M*e_dot - K*r;
17 Ramin Shamshiri EML 6934, Nonlinear control systems
APPENDIX 3. Adaptive controller
function [Torque, e, theta_hat_dot]= Adaptive_controller(t, alf, K, q, q_dot, gamma, theta_hat)
% Desired trajectory:
qd = [cos(t);cos(t)];
qd_dot = [-sin(t);-sin(t)];
qd_ddot = [-cos(t);-cos(t)];
% Error definition for the Tracking controller
e = q - qd;
e_dot = q_dot - qd_dot;
% Filter Tracking Error
r = e_dot + alf*e;
% Joint position and velocity signals:
q1 = q(1); q2 = q(2); % Joint angles
q1_dot = q_dot(1); q2_dot = q_dot(2); % Joint angular velocities
% Linear parametrization
% Defining the terms in Y and theta
qd1_dot = qd_dot(1); qd2_dot = qd_dot(2);
qd1_ddot = qd_ddot(1); qd2_ddot = qd_ddot(2);
e1_dot = e_dot(1); e2_dot = e_dot(2);
alf1 = alf(1,1); alf2 = alf(2,2);
A1 = -qd1_ddot + alf1*e1_dot;
B1 = -qd2_ddot + alf2*e2_dot;
C1 = cos(q2)*(-2*qd1_ddot - qd2_ddot + 2*alf1*e1_dot + alf2*e2_dot) + sin(q2)*(-
alf1*e1_dot*q2_dot - alf2*e2_dot*(q1_dot + q2_dot) + q2_dot*qd1_dot + (q1_dot + q2_dot)*qd2_dot);
D1 = -q1_dot;
E1 = 0;
A2 = 0;
B2 = - qd1_ddot - qd2_ddot + alf1*e1_dot + alf2*e2_dot;
C2 = cos(q2)*(-qd1_ddot + alf1*e1_dot) + sin(q2)*(alf1*e1_dot*q1_dot - q1_dot*qd1_dot);
D2 = 0;
E2 = -q2_dot;
Y = [A1 B1 C1 D1 E1;
A2 B2 C2 D2 E2];
Yt = Y';
% Control Torque based on estimate plant model
Feedback_term = K*r;
Torque = -(Y*theta_hat + Feedback_term);
% Designing theta_hat_dot to cancel the cross-over term in the Lyapunov analysis
theta_hat_dot = gamma*Yt*r;
18 Ramin Shamshiri EML 6934, Nonlinear control systems
APPENDIX 4: Robust, SMC controller
function [Torque, e]= Robust_controller(t, alf, K, q, q_dot)
%% Desired trajectory:
qd = [cos(t);cos(t)];
qd_dot = [-sin(t);-sin(t)];
%% Error definition for the Tracking controller
e = q - qd;
e_dot = q_dot - qd_dot;
% Filter Tracking Error
r = e_dot + alf*e;
%% Control Torque based on estimate plant model
c1 = 10; c2 = 7; c3 = 1;
norm_z = norm([r,e],'fro'); %Frobenius norm
p = c1+c2*norm_z+c3*norm_z^2;
Torque = -p*sign(r)- K*r;
APPENDIX 5: Robust, High gain controller
function [Torque, e]= Robust_controller(t, alf, K, q, q_dot)
%% Desired trajectory:
qd = [cos(t);cos(t)];
qd_dot = [-sin(t);-sin(t)];
%% Error definition for the Tracking controller
e = q - qd;
e_dot = q_dot - qd_dot;
% Filter Tracking Error
r = e_dot + alf*e;
%% Control Torque based on estimate plant model
Torque = -K*r;
19 Ramin Shamshiri EML 6934, Nonlinear control systems
APPENDIX 6: Plotting
% Simulation variables assigned
%-------------------------------------------------------------------------%
% EMK_no_dist --> error1, torque1
% EMK_with_dist --> error2, torque2
% Adaptive_no_dist --> error3, torque3, estimates1
% Adaptive_with_dist --> error4, torque4, estimates2
% SMC_no_dist --> error5, torque5
% SMC_with_dist --> error6, torque6
% Robust_no_dist --> error7_K(i)*, torque7_K10
% Robust_with_dist --> error8, torque8
% * Run simulation with different K, i.e. K=25 and store output as
% error7_K25
%-------------------------------------------------------------------------%
close all
% Exact Model Knowledge (EMK) controller plots without disturbances
%-----------------------------------------------------------------------%
dt=size(error1,1);
t1=0:20/(dt-1):20;
t1=t1';
% Plots of Error i joint angles
figure1 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t1,error1(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, EMK controller without external disturbances');
subplot (2,1,2)
plot(t1,error1(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
% Plot of torque 1 and torque 2
figure2 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t1,torque1(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 1, (Nm)');
title('Torque plots, EMK controller without external disturbances');
subplot (2,1,2)
plot(t1,torque1(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 2, (Nm)');
%-----------------------------------------------------------------------%
% Exact Model Knowledge controller plots With disturbances
%-----------------------------------------------------------------------%
dt=size(error2,1);
t2=0:20/(dt-1):20;
t2=t2';
% Plots of Error i joint angles
figure3 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t2,error2(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, EMK controller with external disturbances');
subplot (2,1,2)
plot(t2,error2(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
20 Ramin Shamshiri EML 6934, Nonlinear control systems
% Plot of torque 1 and torque 2
figure4 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t2,torque2(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 1, (Nm)');
title('Torque plots, EMK controller with external disturbances');
subplot (2,1,2)
plot(t2,torque2(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 2, (Nm)');
%-----------------------------------------------------------------------%
% Adaptive controller plots Without disturbances
%-----------------------------------------------------------------------%
dt=size(error3,1);
t3=0:20/(dt-1):20;
t3=t3';
% Plots of Error i joint angles
figure5 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t3,error3(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, Adaptive controller without external disturbances');
subplot (2,1,2)
plot(t3,error3(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
% Plot of torque 1 and torque 2
figure6 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t3,torque3(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 1, (Nm)');
title('Torque plots, Adaptive controller without external disturbances');
subplot (2,1,2)
plot(t3,torque3(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 2, (Nm)');
% Plot of estimated parameters, p1, p2, p3
figure7 = figure('Color',[1 1 1]);
subplot(3,1,1);
plot(t3,estimates1(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of p1');
title('Parameter estimate plots, Adaptive controller without external disturbances');
subplot(3,1,2);
plot(t3,estimates1(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of p2');
subplot(3,1,3);
plot(t3,estimates1(1:dt,3),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of p3');
% Plot of estimated parameters, fd1,fd2
figure8 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t3,estimates1(1:dt,4),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of fd1');
title('Parameter estimate plots, Adaptive controller without external disturbances');
subplot(2,1,2);
plot(t3,estimates1(1:dt,5),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of fd2');
%-----------------------------------------------------------------------%
21 Ramin Shamshiri EML 6934, Nonlinear control systems
% Adaptive controller plots With disturbances
%-----------------------------------------------------------------------%
dt=size(error4,1);
t4=0:20/(dt-1):20;
t4=t4';
% Plots of Error i joint angles
figure9 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t4,error4(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, Adaptive controller with external disturbances');
subplot (2,1,2)
plot(t4,error4(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
% Plot of torque 1 and torque 2
figure10 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t4,torque4(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 1, (Nm)');
title('Torque plots, Adaptive controller with external disturbances');
subplot (2,1,2)
plot(t4,torque4(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 2, (Nm)');
% Plot of estimated parameters, p1, p2, p3
figure11 = figure('Color',[1 1 1]);
subplot(3,1,1);
plot(t4,estimates2(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of p1');
title('Parameter estimate plots, Adaptive controller with external disturbances');
subplot(3,1,2);
plot(t4,estimates2(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of p2');
subplot(3,1,3);
plot(t4,estimates2(1:dt,3),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of p3');
% Plot of estimated parameters, fd1,fd2
figure12 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t4,estimates2(1:dt,4),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of fd1');
title('Parameter estimate plots, Adaptive controller with external disturbances');
subplot(2,1,2);
plot(t4,estimates2(1:dt,5),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Estimate of fd2');
%-----------------------------------------------------------------------%
22 Ramin Shamshiri EML 6934, Nonlinear control systems
% Sliding Mode Controller (SMC) plots Without disturbances
%-----------------------------------------------------------------------%
dt=size(error5,1);
t5=0:20/(dt-1):20;
t5=t5';
% Plots of Error i joint angles
figure13 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t5,error5(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, Sliding Mode Controller without external disturbances');
subplot (2,1,2)
plot(t5,error5(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
% Plot of torque 1 and torque 2
figure14 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t5,torque5(1:dt,1),'b','LineWidth',0.5); grid on
xlabel('Time (sec)');
ylabel('Torque 1, (Nm)');
title('Torque plots, Sliding Mode Controller without external disturbances');
subplot (2,1,2)
plot(t5,torque5(1:dt,2),'b','LineWidth',0.5); grid on
xlabel('Time (sec)');
ylabel('Torque 2, (Nm)');
%-----------------------------------------------------------------------%
% Sliding Mode Controller (SMC) plots With disturbances
%-----------------------------------------------------------------------%
dt=size(error6,1);
t6=0:20/(dt-1):20;
t6=t6';
% Plots of Error i joint angles
figure15 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t6,error6(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, Sliding Mode Controller with external disturbances');
subplot (2,1,2)
plot(t6,error6(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
% Plot of torque 1 and torque 2
figure16 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t6,torque6(1:dt,1),'b','LineWidth',0.5); grid on
xlabel('Time (sec)');
ylabel('Torque 1, (Nm)');
title('Torque plots, Sliding Mode Controller with external disturbances');
subplot (2,1,2)
plot(t6,torque6(1:dt,2),'b','LineWidth',0.5); grid on
xlabel('Time (sec)');
ylabel('Torque 2, (Nm)');
%-----------------------------------------------------------------------%
23 Ramin Shamshiri EML 6934, Nonlinear control systems
% Robust High gain Controller plots Without disturbances
%-----------------------------------------------------------------------%
dt=size(error7_K1,1);
t7=0:20/(dt-1):20;
t7=t7';
% Plots of Error i joint angles
figure17 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t7,error7_K1(1:dt,1),'b','LineWidth',0.5); grid on % K=1
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, high gain Controller without external disturbances');
hold on
subplot (2,1,2)
plot(t7,error7_K1(1:dt,2),'b','LineWidth',0.5); grid on % K=1
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
hold on
dt=size(error7_K10,1);
t7=0:20/(dt-1):20;
t7=t7';
subplot(2,1,1);
plot(t7,error7_K10(1:dt,1),'r','LineWidth',0.5); % K=10
hold on
subplot (2,1,2)
plot(t7,error7_K10(1:dt,2),'r','LineWidth',0.5); % K=10
hold on
dt=size(error7_K50,1);
t7=0:20/(dt-1):20;
t7=t7';
subplot(2,1,1);
plot(t7,error7_K50(1:dt,1),'b','LineWidth',0.5); % K=50
hold on
subplot (2,1,2)
plot(t7,error7_K50(1:dt,2),'b','LineWidth',0.5); % K=50
hold on
dt=size(error7_K500,1);
t7=0:20/(dt-1):20;
t7=t7';
subplot(2,1,1);
plot(t7,error7_K500(1:dt,1),'g','LineWidth',0.5); % K=500
hold off
subplot (2,1,2)
plot(t7,error7_K500(1:dt,2),'g','LineWidth',0.5); % K=500
hold off
% Plot of torque 1 and torque 2
dt=size(error7_K10,1);
t7=0:20/(dt-1):20;
t7=t7';
figure18 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t7,torque7_K10(1:dt,1),'b','LineWidth',2); grid on % K=10
xlabel('Time (sec)');
ylabel('Torque in joint angle q1');
title('Torque plots, high gain Controller without external disturbances');
subplot (2,1,2)
plot(t7,torque7_K10(1:dt,2),'b','LineWidth',2); grid on % K=10
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
%-----------------------------------------------------------------------%
24 Ramin Shamshiri EML 6934, Nonlinear control systems
% Robust High gain Controller plots With disturbances
%-----------------------------------------------------------------------%
dt=size(error8,1);
t8=0:20/(dt-1):20;
t8=t8';
% Plots of Error i joint angles
figure19 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t8,error8(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q1');
title('Error plots, Robust High gain Controller with external disturbances');
subplot (2,1,2)
plot(t8,error8(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Error in joint angle q2');
% Plot of torque 1 and torque 2
figure20 = figure('Color',[1 1 1]);
subplot(2,1,1);
plot(t8,torque8(1:dt,1),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 1, (Nm)');
title('Torque plots, Robust High gain Controller with external disturbances');
subplot (2,1,2)
plot(t8,torque8(1:dt,2),'b','LineWidth',2); grid on
xlabel('Time (sec)');
ylabel('Torque 2, (Nm)');
%-----------------------------------------------------------------------%