The MATLAB simulation is as follows.
In this simulation the model includes dc motor modeling for the quadcopter.
Let Fi represents the thrust generated by ith propeller and ωi represents the angular velocity of ith propeller.
Then, for our quad rotor, relation between thrust and angular velocity of the propeller is obtained as
follows:-
Fi = kf*ωi2, [N]
Where k is a constant that relates angular velocity and thrust generated by propellers. Value of k is
constant as k= 5.7 · 10−8 N/rpm2 and the
Relation between thrust and torque generated by propellers also have to be defined. Let Ti represent the
torque generated by ith propeller, Then torque generated by ith propeller is given by:-
Ti = kt*Fi, [N · m]
Where km is a constant that relates thrust and torque generated by propellers. Value of kt= 0.016 m. Since
kt and kf are constants, it can be concluded that, relation between thrust and torque generated by
propellers and angular velocity of propellers is quadratic.
MATLAB script to determine gain K and test controllability
kf=5.7e-8; % a constant that relates angular velocity and thrust generated by propellers i
kt=0.016; % a constant that relates thrust and torque generated by propellers
km=0.35; %motor constant
J=0.025; %motor inertia
b=0.002; %motor friction constant
ra=3.5; %motor resistance
la=1.5e-5; % motor inductance
m=0.5; %mass of quad
Ix=0.3125; % inertia of quad about x axis
Iy=0.3125; % inertia of quad about x axis
Iz=0.3125; % inertia of quad about x axis
d=0.5; % arm length of quad
kx=0.016; % a constant that relates angular velocity and thrust generated by propellers in x
ky=0.016; % a constant that relates angular velocity and thrust generated by propellers in y
kz=0.016; % a constant that relates angular velocity and thrust generated by propellers in z
g=9.81;
A=[ 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 -g 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 g 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1;
0 0 0 0 0 0 0 0 0 0 0 0];
B=[0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
-1/m 0 0 0;
0 0 0 0;
0 1/Ix 0 0;
0 0 0 0;
0 0 1/Iy 0;
0 0 0 0;
0 0 0 1/Iz];
B1=[0 0 0 0 0 0;
1/m 0 0 0 0 0;
0 0 0 0 0 0;
0 1/m 0 0 0 0;
0 0 0 0 0 0;
0 0 1/m 0 0 0;
0 0 0 0 0 0;
0 0 0 1/Ix 0 0;
0 0 0 0 0 0;
0 0 0 0 1/Iy 0;
0 0 0 0 0 0;
0 0 0 0 0 1/Iz];
C=[1 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0];
D=[0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0];
e=eig(A)
sys_ss=ss(A,B,C,D)
Q=C'*C;
Q(1,1)=5000;
Q(3,3)=5000;
Q(5,5)=5000;
Q(7,7)=5000;
Q(9,9)=5000;
Q(11,11)=5000;
R=eye(4);
K=lqr (A, B, Q, R)
sys_tf_uc = tf(sys_ss);
A_c=A-B*K;
pole_zero_unc=zpk(sys_ss)
co=ctrb(sys_ss)
controllability=rank(co)
Ob = obsv(A,C)
Ac=(A-B*K);
Bc=B;
Cc=C;
Dc=D;
sys_cl=ss(A,B,C,D);
sys_tf_c = tf(sys_cl)
pole_zero_c=zpk(sys_cl)
MATLAB script for orientation block
function[phi_ddot,theta_ddot,si_ddot]=
fcn(phi_dot,theta_dot,si_dot,U_2,U_3,U_4)
Ix=0.3125;
Iy=0.3125;
Iz=0.3125;
d=0.5;
phi_ddot=((Iy-Iz)*theta_dot*si_dot/Ix) + U_2*d/Ix;
theta_ddot=((Iz-Ix)*phi_dot*si_dot/Iy) + U_3*d/Iy;
si_ddot=((Ix-Iy)*phi_dot*theta_dot/Iz) + U_4*d/Iz;
MATLAB script for possition block
function[x_ddot,y_ddot,z_ddot] = fcn(U_1,phi,theta,si,x_dot,y_dot,z_dot)
m=0.5;
kx=0.016;
ky=0.016;
kz=0.016;
x_ddot=U_1*(cos(phi)*sin(theta)*cos(si) + sin(phi)*sin(si) - kx*x_dot)/m;
y_ddot=U_1*(cos(phi)*sin(theta)*sin(si) - sin(phi)*cos(si) - ky*y_dot)/m;
z_ddot=U_1*(cos(phi)*cos(theta)- kz*z_dot)/m;