1
$\begingroup$

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

enter image description here enter image description here

Now if I change the problem to the tracking one, the results are messed up.

enter image description here enter image description here

In the last paper, they state

A saturation function is needed to ensure that the reference roll and pitch angles are within specified limits

enter image description here

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
$\endgroup$
  • $\begingroup$ $\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. $\endgroup$ – Dave May 23 '15 at 10:33
  • $\begingroup$ 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 $\endgroup$ – Dave May 23 '15 at 10:39
  • $\begingroup$ @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/… $\endgroup$ – CroCo May 23 '15 at 15:15
  • $\begingroup$ 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. $\endgroup$ – CroCo May 23 '15 at 15:18
  • $\begingroup$ About timing, could you please elaborate a bit since everything in my code in one for loop. $\endgroup$ – CroCo May 23 '15 at 15:20
1
$\begingroup$

In your code:

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

$\endgroup$
  • $\begingroup$ 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". $\endgroup$ – Gürkan Çetin Jul 23 '15 at 19:35

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.