Lately, if you notice I have posted some questions regarding position tracking for nonlinear model. I couldn't do it. I've switched to linear model, hope I can do it. For regulation problem, the position control seems working but once I switch to tracking, the system starts oscillating. I don't know why. I have stated what I've done below hope someone guides me to the correct path.
The linear model of the quadrotor is provided here which is
$$ \begin{align} \ddot{x} &= g \theta \ \ \ \ \ \ \ \ \ \ (1)\\ \ddot{y} &= - g \phi \ \ \ \ \ \ \ \ \ \ (2)\\ \ddot{z} &= \frac{U_{1}}{m} - g \\ \ddot{\phi} &= \frac{L}{J_{x}} U_{2} \\ \ddot{\theta} &= \frac{L}{J_{y}} U_{2} \\ \ddot{\psi} &= \frac{1}{J_{z}} U_{2} \\ \end{align} $$
In this paper, the position control based on PD is provided. In the aforementioned paper, from (1) and (2) the desired angles $\phi^{d}$ and $\theta^{d}$ are obtained, therefore,
$$ \begin{align} \theta^{d} &= \frac{\ddot{x}^{d}}{g} \\ \phi^{d} &= - \frac{\ddot{y}^{d}}{g} \end{align} $$
where
$$ \begin{align} \ddot{x}^{d} &= Kp(x^{d} - x) + Kd( \dot{x}^{d} - \dot{x} ) \\ \ddot{y}^{d} &= Kp(y^{d} - y) + Kd( \dot{y}^{d} - \dot{y} ) \\ U_{1} &= Kp(z^{d} - z) + Kd( \dot{z}^{d} - \dot{z} ) \\ U_{2} &= Kp(\phi^{d} - \phi) + Kd( \dot{\phi}^{d} - \dot{\phi} ) \\ U_{3} &= Kp(\theta^{d} - \theta) + Kd( \dot{\theta}^{d} - \dot{\theta} ) \\ U_{4} &= Kp(\psi^{d} - \psi) + Kd( \dot{\psi}^{d} - \dot{\psi} ) \\ \end{align} $$
with regulation problem where $x^{d} = 2.5 m, \ y^{d} = 3.5 m$ and $z^{d} = 4.5 m$, the results are
Now if I change the problem to the tracking one, the results are messed up.
In the last paper, they state
A saturation function is needed to ensure that the reference roll and pitch angles are within specified limits
Unfortunately, the max value for $\phi$ and $\theta$ are not stated in the paper but since they use Euler angles, I believe $\phi$ in this range $(-\frac{\pi}{2},\frac{\pi}{2})$ and $\theta$ in this range $[-\pi, \pi]$ I'm using Euler method as an ODE solver because the step size is fixed. For the derivative, Euler method is used.
This is my code
%######################( PD Controller & Atittude )%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
dt = 0.001;
t = 0;
% initial values of the system
x = 0;
dx = 0;
y = 0;
dy = 0;
z = 0;
dz = 0;
Phi = 0;
dPhi = 0;
Theta = 0;
dTheta = 0;
Psi = pi/3;
dPsi = 0;
%System Parameters:
m = 0.75; % mass (Kg)
L = 0.25; % arm length (m)
Jx = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jy = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jz = 0.039380; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81; % acceleration due to gravity m/s^2
errorSumX = 0;
errorSumY = 0;
errorSumZ = 0;
errorSumPhi = 0;
errorSumTheta = 0;
pose = load('xyTrajectory.txt');
% Set desired position for tracking task
DesiredX = pose(:,1);
DesiredY = pose(:,2);
DesiredZ = pose(:,3);
% Set desired position for regulation task
% DesiredX(:,1) = 2.5;
% DesiredY(:,1) = 5;
% DesiredZ(:,1) = 7.2;
dDesiredX = 0;
dDesiredY = 0;
dDesiredZ = 0;
DesiredXpre = 0;
DesiredYpre = 0;
DesiredZpre = 0;
dDesiredPhi = 0;
dDesiredTheta = 0;
DesiredPhipre = 0;
DesiredThetapre = 0;
for i = 1:6000
% torque input
%&&&&&&&&&&&&( Ux )&&&&&&&&&&&&&&&&&&
Kpx = 90; Kdx = 25; Kix = 0.0001;
errorSumX = errorSumX + ( DesiredX(i) - x );
% Euler Method Derivative
dDesiredX = ( DesiredX(i) - DesiredXpre ) / dt;
DesiredXpre = DesiredX(i);
Ux = Kpx*( DesiredX(i) - x ) + Kdx*( dDesiredX - dx ) + Kix*errorSumX;
%&&&&&&&&&&&&( Uy )&&&&&&&&&&&&&&&&&&
Kpy = 90; Kdy = 25; Kiy = 0.0001;
errorSumY = errorSumY + ( DesiredY(i) - y );
% Euler Method Derivative
dDesiredY = ( DesiredY(i) - DesiredYpre ) / dt;
DesiredYpre = DesiredY(i);
Uy = Kpy*( DesiredY(i) - y ) + Kdy*( dDesiredY - dy ) + Kiy*errorSumY;
%&&&&&&&&&&&&( U1 )&&&&&&&&&&&&&&&&&&
Kpz = 90; Kdz = 25; Kiz = 0;
errorSumZ = errorSumZ + ( DesiredZ(i) - z );
dDesiredZ = ( DesiredZ(i) - DesiredZpre ) / dt;
DesiredZpre = DesiredZ(i);
U1 = Kpz*( DesiredZ(i) - z ) + Kdz*( dDesiredZ - dz ) + Kiz*errorSumZ;
%#######################################################################
%#######################################################################
%#######################################################################
% Desired Phi and Theta
%disp('before')
DesiredPhi = -Uy/g;
DesiredTheta = Ux/g;
%&&&&&&&&&&&&( U2 )&&&&&&&&&&&&&&&&&&
KpP = 20; KdP = 5; KiP = 0.001;
errorSumPhi = errorSumPhi + ( DesiredPhi - Phi );
% Euler Method Derivative
dDesiredPhi = ( DesiredPhi - DesiredPhipre ) / dt;
DesiredPhipre = DesiredPhi;
U2 = KpP*( DesiredPhi - Phi ) + KdP*( dDesiredPhi - dPhi ) + KiP*errorSumPhi;
%--------------------------------------
%&&&&&&&&&&&&( U3 )&&&&&&&&&&&&&&&&&&
KpT = 90; KdT = 10; KiT = 0.001;
errorSumTheta = errorSumTheta + ( DesiredTheta - Theta );
% Euler Method Derivative
dDesiredTheta = ( DesiredTheta - DesiredThetapre ) / dt;
DesiredThetapre = DesiredTheta;
U3 = KpT*( DesiredTheta - Theta ) + KdP*( dDesiredTheta - dTheta ) + KiT*errorSumTheta;
%--------------------------------------
%&&&&&&&&&&&&( U4 )&&&&&&&&&&&&&&&&&&
KpS = 90; KdS = 10; KiS = 0; DesiredPsi = 0; dDesiredPsi = 0;
U4 = KpS*( DesiredPsi - Psi ) + KdS*( dDesiredPsi - dPsi );
%###################( ODE Equations of Quadrotor )###################
ddx = g * Theta;
dx = dx + ddx*dt;
x = x + dx*dt;
%=======================================================================
ddy = -g * Phi;
dy = dy + ddy*dt;
y = y + dy*dt;
%=======================================================================
ddz = (U1/m) - g;
dz = dz + ddz*dt;
z = z + dz*dt;
%=======================================================================
ddPhi = ( L/Jx )*U2;
dPhi = dPhi + ddPhi*dt;
Phi = Phi + dPhi*dt;
%=======================================================================
ddTheta = ( L/Jy )*U3;
dTheta = dTheta + ddTheta*dt;
Theta = Theta + dTheta*dt;
%=======================================================================
ddPsi = (1/Jz)*U4;
dPsi = dPsi + ddPsi*dt;
Psi = Psi + dPsi*dt;
%=======================================================================
%store the erro
ErrorX(i) = ( x - DesiredX(i) );
ErrorY(i) = ( y - DesiredY(i) );
ErrorZ(i) = ( z - DesiredZ(i) );
ErrorPsi(i) = ( Psi - 0 );
X(i) = x;
Y(i) = y;
Z(i) = z;
T(i) = t;
t = t + dt;
end
Figure1 = figure(1);
set(Figure1,'defaulttextinterpreter','latex');
subplot(2,2,1)
plot(T, ErrorX, 'LineWidth', 2)
title('Error in $x$-axis Position (m)')
xlabel('time (sec)')
ylabel('$x_{d}(t) - x(t)$', 'LineWidth', 2)
subplot(2,2,2)
plot(T, ErrorY, 'LineWidth', 2)
title('Error in $y$-axis Position (m)')
xlabel('time (sec)')
ylabel('$y_{d}(t) - y(t)$', 'LineWidth', 2)
subplot(2,2,3)
plot(T, ErrorZ, 'LineWidth', 2)
title('Error in $z$-axis Position (m)')
xlabel('time (sec)')
ylabel('$z_{d} - z(t)$', 'LineWidth', 2)
subplot(2,2,4)
plot(T, ErrorPsi, 'LineWidth', 2)
title('Error in $\psi$ (m)')
xlabel('time (sec)')
ylabel('$\psi_{d} - \psi(t)$','FontSize',12);
grid on
Figure2 = figure(2);
set(Figure2,'units','normalized','outerposition',[0 0 1 1]);
figure(2)
plot3(X,Y,Z, 'b')
grid on
hold on
plot3(DesiredX, DesiredY, DesiredZ, 'r')
pos = get(Figure2,'Position');
set(Figure2,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3),pos(4)]);
print(Figure2,'output2','-dpdf','-r0');
For the trajectory code
clear all;
clc;
fileID = fopen('xyTrajectory.txt','w');
angle = -pi;
radius = 3;
z = 0;
t = 0;
for i = 1:6000
if ( z < 2 )
z = z + 0.1;
x = 0;
y = 0;
end
if ( z >= 2 )
angle = angle + 0.1;
angle = wrapToPi(angle);
x = radius * cos(angle);
y = radius * sin(angle);
z = 2;
end
X(i) = x;
Y(i) = y;
Z(i) = z;
fprintf(fileID,'%f \t %f \t %f\n',x, y, z);
end
fclose(fileID);
plot3(X,Y,Z)
grid on
arcsinyields undefined number based on the approach in this link researchgate.net/publication/… $\endgroup$