Professional Documents
Culture Documents
Controlling a 2D Robot
Shilpa Marti
EE 5143 Linear Systems and Controls
December 9, 2014
Abstract
The equations governing the motion of an aircraft and the 2D Robot requires complex
nonlinear coupled differential equations [1] . However, under certain assumptions, they can
be decoupled and linearized into longitudinal and lateral equations for aircraft and position,
velocity for robot in x and y direction. In this part of the project we will focus on the
simplified linear equation of modeling the longitudinal motion of a transport aircraft and
designing the trajectory following controller for Robot.
Page 1 of 20
Introduction
Longitudinal Control of Aircraft
An aircraft is assumed to be disturbed from its steady state flight condition. These
perturbations are considered to be small from the reference position in order to obtain a set
of linear differential equations.
In longitudinal mode the oscillating motions can be described by two parameters[2]
Phugoid and Short-term.
In Phugoid mode the oscillations have longer periods and large amplitude variation of airspeed, pitch angle, and altitude, though the damping is weak the period is very long for the
pilot to control the oscillations of the aircraft.
The linear differential equation of the longitudinal motion in Phugoid mode is given below
0
0
0
1
0
0
0
(1.0)
Where =
=
=
=
Page 2 of 20
()
()
=
0
0
0
0
0
0
0
0
1
0
0
0
0
0
1 () 0
+
0
1
0
0
0
0 ()
[
]
0 ()
1
(1.1)
THEORY
Designing a trajectory following controller that follows a 2D Robot that has the
coordinates for starting at a certain location and ending at another.
Page 3 of 20
METHODOLOGY
Eigenvalues and Eigen Vectors
The small disturbance equations for the longitudinal control of aircraft are given by[3]
= +
(2)
(1)
A is (N x N) system matrix
The solutions for the first order differential equations is
X(t) = X0et
(3)
(4)
Depending upon the value of one can determine whether the system has a static instability
or divergence that increase with time, or a subsidence or a convergence that ultimately
diminishes with time and the disturbance quantity approaches to zero.
This evaluation of stability is obtained from the inspection of eigenvalues. If there are no
positive real parts of the s then the system is stable.
Eigen vectors are useful in modal decomposition of the system, thus determining the effect
of eigenvalue on a particular state variables.
In Phugoid mode it is observed from the eigenvectors that altitude velocity change (w) and
pitch angular velocity (q) remain small but u and are present with significant
magnitude.
Transition Matrix
In control theory, the state-transition matrix is a matrix whose product with the state
vector at an initial time gives at a later time. The state-transition matrix can be used to
obtain the general solution of linear dynamical systems. It is also known as the matrix
exponential. [5]
Page 4 of 20
Control Inputs
The importance of knowing if a system is controllable is that then it is known that the
system can be driven from one state to the next. In order to do this we must find a controller
to in the form of:
()=^()1()(0)
(5)
To take the state from x(0) to x(100sec),as the problem requires(this is shown in Problem 1
part 4 in the appendix). The equation above requires that ^(), , and 1 be
calculated for. The first two are easy to find, but the last one requires the use of the
following equation:
= 0 ()
(6)
In order to find the sequence of inputs for a given time interval, the t and tf must be changed
to the corresponding time interval. In this case it would be 0.5sec and 1.0sec, respectively
(calculations are shown in Problem 1 part 6 of the appendix).
Longitudinal Control of an Aircraft and
Controlling a 2D Robot | Shilpa Marti
Page 5 of 20
State Trajectories
Evolution of dynamic system corresponds to a trajectory in the phase system. Different
initial states result in different trajectories. Now in order to be able to plot the state
trajectories, the discrete time form must be solved for and used to be modeled into MATLAB
(graphs are shown in Problem 1 part 7 of the appendix). Lastly, the change of altitude can be
found by using the equation:
= + _
Next, a controller is used to follow a designated trajectory that follows different sets of
equations that that move it in the x and y direction. In order to simulate the state dynamics
of the continuous time model it should be converted into a discrete time model (shown in
Problem 2 part 5 of the appendix). Then dynamics of the states can be observed for 30
seconds; the graphs are displayed in Problem 2 part 6 of the appendix.
Page 6 of 20
0.0212 - 0.0167i
-0.9983 + 0.0000i
-0.9983 + 0.0000i
0.9996 + 0.0000i
0.9996 + 0.0000i
-0.0573 + 0.0097i
-0.0573 - 0.0097i
-0.0001 + 0.0011i
-0.0001 - 0.0011i
-0.0001 - 0.0000i
-0.0001 + 0.0000i
0.0011 - 0.0004i
0.0011 + 0.0004i
0.0001 + 0.0021i
0.0001 - 0.0021i
Page 7 of 20
Stability of Eigenvalues:
Figure 1
Eigen values lie to the left hand side of the S plane, hence stable.
The system is controllable. Rank is 4
Wc =
-0.0663
-17.8500
-1.1580
0.5145
-1.1580
0.5145 0.0011
0.6933 0.0004
Page 8 of 20
ad =
0.0029 0.9657
-6.1238 -0.0042
-0.1131 0.0000
-0.0057 0.0000
Figure 2
Page 9 of 20
Figure 3
Figure 4
Page 10 of 20
Figure 5
Figure 6
Page 11 of 20
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
System is controllable
Co =
12.0000
0
7.0000
2.0000
ad =
1
0
0
0
0
1
0
0
2
0
1
0
3.0000
0
2
0
1
bd =
2
0
2
0
0
2
0
2
Page 12 of 20
Figure 7
Figure 8
Figure 9
Page 13 of 20
Figure 10
The above results are for sample period 2 sec and the system becomes unstable for sample
period 0.1 sec
Future research
The significance of being able to calculate the motion of a robot allows for the
understanding for more complex and precise intelligent robots that not only follow what
the user inputs but also learns from its past inputs to follow a different path. Robots are
used in many different types of applications that help improve the lives of people it was
created for. Knowing how to control a 2D robot simply leads the way to building more
complex designs that will improve the lives of others.
Page 14 of 20
Conclusion
Linearized model of the longitudinal variables of aircraft are analyzed. The determining
factors of stability were analyzed. The state trajectories obtained from the linear simulation
portrayed the variation of state parameters over time. The feedback controller was
designed to enable the trajectory following controller. The trajectories for position and
velocity were observed and obtained the desired output.
Page 15 of 20
Appendices
MATLAB CODE:
%%-------linear modelling of longitudinal control of aircraft--------%%%
%%Author--Shilpa Marti / @01423803
%%
% * Eigen values
% * Eigen vectors
% * Stability
%delta_u=velocity change
%w= altitude velocity change
%q=pich angular velociy change
%delta_0=pitch angle change
clc;
clear all; close all;
A= [ -0.006868 0.01395 0 -32.3;
-0.09055 -0.3151 773.98 0;
0.0001187 -0.001026 -0.4285 0;
0 0 1 0];
B= [-0.000187 9.66; -17.85 0; -1.158 0; 0 0 ];
X0=[0;0;0;0];
Xf=[50;5;1*(pi/180);15*(pi/180)];
[V,D]= eig(A)
disp (eig(A))
plot( eig(A), 'o')
% State Transition Matrix using similarity transformation;
% M= [Re(V1),Im(V1),Re(V2),Im(V2)]
M= [0.0212 0.9996 -0.0001 0.0011;0.0167 0 0.0011 -0.0004;
-0.9983 -0.0573 -0.001 0;0 0.0097 -0 0.0021]
syms t;
k=sym('k');
exAt= [exp(-0.372*t)*cos(0.888*t) exp(-0.372*t)*sin(0.888*t) 0 0 ;
-exp(-0.372*t)*sin(0.888*t) exp(-0.372*t)*cos(0.888*t) 0 0 ;
0 0 exp(-0003*t)*cos(0.067*t) -exp(-0.003*t)*sin(0.067*t);
0 0 exp(-0.003*t)*sin(0.067*t) exp(-0.003*t)*cos(0.067*t)]
Q= simplify(exAt)
phi= M*Q/M
trans_matrix=simplify(phi)
%%
% *CONTROLLABILITY*
% system is controllable if the pair (A,B) is controllable
% Rank of the matrix $$ Wc= [ B, AB,A^2B,A^3B]=n $$
Wc= ctrb(A,B)
rank (Wc)
Page 16 of 20
[ad,bd]=c2d(A,B,0.1)
%%
% * CONTROL INPUT*
%to compute the control input to bring the state from x(0)= [0 0 0 0]T to
%x(f)= [ 50m/s 5m/s 1/s 15]
% we use
tau=sym('tau');
wctf=int(expm(A*(100-tau))*B*(B')*expm(A'*(100-tau)),0,100);
wctf1=inv(wctf);
for i=0.5:0.5:100
k=i*2;
u(:,k)=-((B')*expm(A'*(100-i))/wctf)*(expm(A*100)*X0-Xf);
end
U=double(u);
sys = ss(a,b,c,d);
[y,tsim,x] = lsim(sys,U,T);
figure(1);
%7
plot(tsim,x(:,1));
xlabel('Time');
ylabel('Velocity Magnitude Change');
figure(2);
plot(tsim,x(:,2));
xlabel('Time');
ylabel('Altitude Velocity Change');
figure(3);
plot(tsim,x(:,3));
xlabel('Time');
ylabel('Pitch Angular Velocity');
figure(4);
plot(tsim,x(:,4));
xlabel('Time');
ylabel('Pitch Angle Change');
%6
figure(5);
plot(tsim,U);
title('Input Sequence');
xlabel('Time');
ylabel('Inputs');
Page 17 of 20
x(t) +
[ 0
0
1
0
0
0
0
1]
[ux(t)
uy(t) ]
%%
clc;
clear all;
close all;
A =[0 0 1 0;0 0 0 1;0 0 0 0;0 0 0 0];
B=[0 0;0 0;1 0;0 1];
C=[1 0 0 0;0 1 0 0];
D= [0 0 ; 0 0]
sys = ss ( A, B, C ,D)
Ob= obsv(sys)
if(rank(Ob)==4)
sprintf('observable')
else
sprintf('unobservable')
end
Co = ctrb(sys)
if(rank(Ob)==4)
sprintf('controllable')
else
sprintf('uncontrollable')
end
%%
tfsys= tf(sys)
pole(sys)
Pnew =[-1; -2; -3; -4]; % for asymptotic stability poles should
%negative real numbers.
k = [A,B,Pnew]
%P=(C*((-A+B*k)^-1)*B)^-1;
t=2;
[ad,bd]=c2d(A,B,t)
Page 18 of 20
and
py(t) = 5t + 2t2
Figure 11
Page 19 of 20
References
1. [Bernard Etkin and Lloyd Duff Reid].[1996]. [Dynamics of Flight stability and
control] 3rd edition. [ 110-115]
2. [Bernard Etkin and Lloyd Duff Reid].[1996]. [Dynamics of Flight stability and
control] 3rd edition. [ 170-180]
3. [Bernard Etkin and Lloyd Duff Reid].[1996]. [Dynamics of Flight stability and
control] 3rd edition. [ 185-188]
4. Korn, Granino A.; Korn, Theresa M. (2000), "Mathematical Handbook for Scientists
and Engineers: Definitions, Theorems, and Formulas for Reference and
Review", New York: McGraw-Hill (1152 p., Dover Publications, 2 Revised edition),
5. http://www.mathworks.com/help/matlab/
6.
Page 20 of 20