# position control for linear model of quadrotor (problem with tracking task)

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;

% 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;
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

• $\phi, \theta$ are inside the intervall -90,90 deg. All the linearization's story is about the fact that the system doesn't go to much far from the linearization's point $\phi = 0 \theta = 0$. About the tracking problem: please try again with the following gains: P = 8, I = 0, D = 35 . I had a lot of problem due to the integrating term. And be sure that the timing is right. 'For example the point to be reached must be not be too far from the last point. Small steps. May 23, 2015 at 10:33
• And if I were you I would still implement a non linear controller. I was really happy with the following one: math.ucsd.edu/~mleok/pdf/LeLeMc2010_quadrotor.pdf You need just a trajectory with position, velocity, acceleration. Snap and jerk are not necessary. That was the first controller I implemented, that was working "out of the box". I was using C++. If you use Matlab, then you have it really fast May 23, 2015 at 10:39
• @Dave, I will try your suggestions. Is there a way to share your code in C++? My ultimate goal is to reimplement the system in C++. Also, which ODE solver did you use? Regarding the nonlinear controller, I've tried my best but always arcsin yields undefined number based on the approach in this link researchgate.net/publication/… May 23, 2015 at 15:15
• Cont., I have seen one of your posts that you are implementing backstepping controller based on the approach in the aforementioned paper, if you can help me to carry out the experiment, I will be more than happy. Thanks. May 23, 2015 at 15:18
• About timing, could you please elaborate a bit since everything in my code in one for loop. May 23, 2015 at 15:20

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


there is a jump when the altitude reachs 2. The x coordinates goes in only one single step from the value 0 to the value stored in the variable radius.

Please arrange your code and let the generation of a much more soft trajectory in your code, in which you have more interpolated points between the position 0 and the radius. As a general rule, you must not have too much distance between two contiguous points. In the case of a quadrotor the fact that those points too far away are, leads to a very huge pitch and/or roll angle (the quadrotor tries to do its best to reach the next point in a very small time, so it accelerates suddenly by setting a very steep angle). This causes the asin function to not work properly, since the arguments are not anymore in the wanted function's domain ($]-1,1[$)

An example could be:

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 = ( i / 6000 ) * radius * cos(angle);
y = ( i / 6000 ) * 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


regards

• this is useful, because it shows the error in the algorithm. it would be beneficial to also define some constraints for a "good, followable trajectory". Jul 23, 2015 at 19:35