7
$\begingroup$

The quadrotor system is multi-ODEs equations. The linearized model is usually used especially for position tracking, therefore one can determine the desired x-y positions based on the roll and pitch angles. As a result, one nested loop which has inner and outer controllers is needed for controlling the quadrotor. For implementation, do I have to put while-loop inside ode45 for the inner attitude controller? I'm asking this because I've read in a paper that the inner attitude controller must run faster (i.e. 1kHz) than the position controller (i.e. 100-200 Hz). In my code, both loops run at 1kHz, therefore inside ode45 there is no while-loop. Is this correct for position tracking? If not, do I have to insert while-loop inside ode45 for running the inner loop? Could you please suggest me a pseudocode for position tracking?

To be more thorough, the dynamics equations of the nonlinear model of the quadrotor is provided here, if we assume the small angles, the model is reduced to the following equations

$$ \begin{align} \ddot{x} &= \frac{U_{1}}{m} ( \theta \cos\psi + \phi \sin\psi) \\ \ddot{y} &= \frac{U_{1}}{m} ( \theta \sin\psi - \phi \cos\psi) \\ \ddot{z} &= \frac{U_{1}}{m} - g \\ \ddot{\phi} &= \frac{l}{I_{x}} U_{2} \\ \ddot{\theta} &= \frac{l}{I_{y}} U_{3} \\ \ddot{\psi} &= \frac{1}{I_{z}} U_{4} \\ \end{align} $$

The aforementioned equations are linear. For position tracking, we need to control $x,y,$ and $z$, therefore we choose the desired roll and pitch (i.e. $\phi^{d} \ \text{and} \ \theta^{d}$)

$$ \begin{align} \ddot{x}^{d} &= \frac{U_{1}}{m} ( \theta^{d} \cos\psi + \phi^{d} \sin\psi) \\ \ddot{y}^{d} &= \frac{U_{1}}{m} ( \theta^{d} \sin\psi - \phi^{d} \cos\psi) \\ \end{align} $$

Therefore, the closed form for the desired angles can be obtained as follows

$$ \begin{bmatrix} \phi_{d} \\ \theta_{d} \end{bmatrix} = \begin{bmatrix} \sin\psi & \cos\psi \\ -\cos\psi & \sin\psi \end{bmatrix}^{-1} \left( \frac{m}{U_{1}}\right) \begin{bmatrix} \ddot{x}^{d} \\ \ddot{y}^{d} \end{bmatrix} $$

My desired trajectory is shown below

enter image description here

The results are

enter image description here

And the actual trajectory vs the desired one is

enter image description here

My code for this experiment is

%%
%######################( Position Controller )%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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');

DesiredX = pose(:,1);
DesiredY = pose(:,2);
DesiredZ = pose(:,3);

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 = 50; Kdx = 8; Kix = 0; 
   Ux = Kpx*( DesiredX(i) - x  ) + Kdx*( dDesiredX - dx ) + Kix*errorSumX;

   errorSumX = errorSumX + ( DesiredX(i) - x );

     dDesiredX = ( DesiredX(i) - DesiredXpre ) / dt;
   DesiredXpre = DesiredX(i);

   %&&&&&&&&&&&&( Uy )&&&&&&&&&&&&&&&&&&
   Kpy = 100; Kdy = 10; Kiy = 0; 
   Uy = Kpy*( DesiredY(i) - y  ) + Kdy*( dDesiredY - dy ) + Kiy*errorSumY;


   errorSumY = errorSumY + ( DesiredY(i) - y );

     dDesiredY = ( DesiredY(i) - DesiredYpre ) / dt;
   DesiredYpre = DesiredY(i);



   %&&&&&&&&&&&&( U1 )&&&&&&&&&&&&&&&&&&
   Kpz = 100; Kdz = 20; Kiz = 0; 
   U1 = Kpz*( DesiredZ(i) - z ) + Kdz*( dDesiredZ - dz ) + Kiz*errorSumZ; 

   errorSumZ = errorSumZ + ( DesiredZ(i) - z );

      dDesiredZ = ( DesiredZ(i) - DesiredZpre ) / dt;
   DesiredZpre = DesiredZ(i);

   %#######################################################################
   %#######################################################################
   %#######################################################################
   % Desired Phi and Theta
   R = [  sin(Psi),cos(Psi); 
         -cos(Psi),sin(Psi)];


   DAngles = R\( (m/U1)*[Ux; Uy]);


   %Wrap angles
   DesiredPhi   = wrapToPi( DAngles(1) ) /2;
   DesiredTheta = wrapToPi( DAngles(2) );


   %&&&&&&&&&&&&( U2 )&&&&&&&&&&&&&&&&&&
   KpP = 100; KdP = 10; KiP = 0;
   U2 = KpP*( DesiredPhi - Phi ) + KdP*( dDesiredPhi - dPhi )  + KiP*errorSumPhi;

   errorSumPhi = errorSumPhi + ( DesiredPhi - Phi );

     dDesiredPhi = ( DesiredPhi - DesiredPhipre ) / dt;
   DesiredPhipre = DesiredPhi;


   %--------------------------------------
   %&&&&&&&&&&&&( U3 )&&&&&&&&&&&&&&&&&&

   KpT = 100; KdT = 10; KiT = 0;
   U3 = KpT*( DesiredTheta - Theta ) + KdP*( dDesiredTheta - dTheta ) + KiT*errorSumTheta;

    errorSumTheta = errorSumTheta + ( DesiredTheta - Theta );

     dDesiredTheta = ( DesiredTheta - DesiredThetapre ) / dt;
   DesiredThetapre = DesiredTheta;



   %--------------------------------------
   %&&&&&&&&&&&&( U4 )&&&&&&&&&&&&&&&&&&
   KpS = 80; KdS = 20.0; KiS = 0.08;
   U4 = KpS*( 0 - Psi ) + KdS*( 0 - dPsi );


   %###################( ODE Equations of Quadrotor )###################
   %===================( X )=====================
   ddx = (U1/m)*( Theta*cos(Psi) + Phi*sin(Psi) );

    dx = dx + ddx*dt;
     x =  x +  dx*dt;
   %===================( Y )=====================
   ddy = (U1/m)*( Theta*sin(Psi) - Phi*cos(Psi) );

    dy = dy + ddy*dt;
     y =  y +  dy*dt;

   %===================( Z )=====================
   ddz = (U1/m) - g;

    dz = dz + ddz*dt;
     z =  z +  dz*dt;

   %===================( Phi )=====================
   ddPhi = ( L/Jx )*U2;

    dPhi = dPhi + ddPhi*dt;
     Phi =  Phi +  dPhi*dt;

   %===================( Theta )=====================
   ddTheta =  ( L/Jy )*U3;

    dTheta =  dTheta + ddTheta*dt;
     Theta =   Theta +  dTheta*dt;

   %===================( Psi )=====================
   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) );
%    ErrorPhi(i)   = ( Phi - pi/4 );
%    ErrorTheta(i) = ( Theta - pi/4 );
     ErrorPsi(i)   = ( Psi - 0 );


   X(i) = x;
   Y(i) = y;
   Z(i) = z;

   T(i) = t;

%    drawnow 
%    plot3(DesiredX, DesiredY, DesiredZ, 'r')
%    hold on
%    plot3(X, Y, Z, 'b')
   t = t + dt; 


end


Figure1 = figure(1);
set(Figure1,'defaulttextinterpreter','latex');
%set(Figure1,'units','normalized','outerposition',[0 0 1 1]);

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');

legend('actual', 'desired')

The code of the desired trajectory is

clear all; clc;

fileID = fopen('xyTrajectory.txt','w');

 angle = -pi; radius = 5; 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$ Can you upload and link your "xyTrajectory.txt" file? $\endgroup$ – acs May 17 '15 at 8:22
  • $\begingroup$ @acs, I did. The last code. $\endgroup$ – CroCo May 17 '15 at 8:24
  • $\begingroup$ Ok. I couldnt see the last two equation in the paper. $\endgroup$ – acs May 17 '15 at 8:44
  • $\begingroup$ I've posted the linearized model of the quadrotor in my post, therefore the code is for the linearized version. $\endgroup$ – CroCo May 17 '15 at 9:03
4
$\begingroup$

As extension to this answer, I'd like to share with the community a Simulink model implementing the system described above. It does not represent a solution for the code reported by CroCo but it may give a better insight and, as such, it might be helpful.

The system is depicted below: system

In red we have the blocks running at $1\,\text{KHz}$, which deal with the attitude control, whereas in green are shown those blocks that allow tracking the linear displacement and run at $100\,\text{Hz}$.

As side note, since we have to ensure that the "small angles" assumption remains valid throughout the simulation, the take-off phase (i.e. reaching the desired height $z_d$) needs to be almost complete before commencing the displacement tracking of the $x_d\left(t\right)$ and $y_d\left(t\right)$ time-varying linear coordinates (a circle of radius $5\,\text{m}$ in the model). Further, for the same reason, the desired circular trajectory must start from the quadcopter's initial position $\left(x\left(0\right),y\left(0\right)\right)$.

Depending on the controllers' gains, we can get quite nice tracking results: results

The first top three diagrams shows the $x$, $y$ and $z$ components (green), respectively, while following the targets (blue). The last plot reports on the roll (blue), pitch (green) and yaw (red). Time is expressed in seconds, displacements are in meters and angles are in degrees.

$\endgroup$
  • $\begingroup$ thank you so much. It seems that I'm getting somewhere. My ultimate goal is to re-implement the project in C++. In my Matlab code, both controllers are running at 1KHz which means it is hard to carry out aggressive maneuver with linear model. For nonlinear model, I'm using backstepping controller, for very careful and smooth trajectory, it seems the quadrotor follows the trajectory but not very fast. $\endgroup$ – CroCo May 26 '15 at 15:47
  • $\begingroup$ Cont., for using joystick, it is hard to tune the backstepping controller to let the quadrotor follows the trajectory as fast as possible because this mission requires the platform to carry out aggressive maneuver which means the small angles assumption don't hold any more. $\endgroup$ – CroCo May 26 '15 at 15:50
  • $\begingroup$ In the "small angles" assumption (still nonlinear model, yet simplified), the "aggressiveness" of the final maneuvers will depend on the the maximum angles you can reach before invalidating the assumption itself, and not on the rate of the outer loop, I'd say. For the complete nonlinear model as well as the back-stepping controller, I suppose you're referring to the cited paper, I didn't read though. Do you know that with the Embedded Coder toolbox you can obtain production C++ code automatically from Simulink models, ready to be run on your systems? $\endgroup$ – Ugo Pattacini May 26 '15 at 17:09
0
$\begingroup$

Your pseudocode for PD like this:

Kp Kd
erroSum=errorSum+(desired-actual);
ddesired=(desired-predesired)/dt;
desiredpre=desired;
PD= Kp*(desired-actual)+Kd*(ddesired-dactual)

Try to apply this:

previosuerror=0
Kp Kd
error=(desired-actual); //for angles use atan2(sin(desired),cos(actual)) neglect sign  
derivative=(previouserror-error)/dt; 
previouserror=error;
P=Kp*error;
D=Kd*derivative;
PD=P+D;

Reference: first 3 module of https://class.coursera.org/conrob-001

$\endgroup$
  • $\begingroup$ What is the problem with my approach? For the link, what is the first 3 module ? I opened the link but it is not clear what you are referring me to. $\endgroup$ – CroCo May 17 '15 at 23:47
  • $\begingroup$ I think your PD codes are wrong. Try to write pseudocode which shown as"try to apply this". that pseudocode wirtten according to reference. Also you have too many PD calculations although you need only three. $\endgroup$ – acs May 18 '15 at 8:42
  • $\begingroup$ "I think your PD codes are wrong." What is the problem? It is just different approach for implementing the controller. I believe I need six controllers. Three for the outer loop (x,y,z) and three for inner loop (phi, theta, psi). $\endgroup$ – CroCo May 18 '15 at 9:16
  • $\begingroup$ ok you can apply 3 PD but it works with wrong PD code and calculates wrong angle in inner loop.Try my pseudocode to see wrong or right. I'm trying to help you. You must know that no one will write all your code $\endgroup$ – acs May 18 '15 at 19:00
  • $\begingroup$ I'm totally sure the problem is not from the way PD is coded in my case but I've tried your suggestion with no luck. The problem is more complicated than I thought. The controllers must be tuned carefully and whether the linear model suits aggressive trajectories is the key. I've moved to nonlinear system with backstepping and with careful trajectory and tuning, I'm getting good results but the response is rather slow. $\endgroup$ – CroCo May 26 '15 at 15:58

protected by Community Jan 23 '17 at 11:59

Thank you for your interest in this question. Because it has attracted low-quality or spam answers that had to be removed, posting an answer now requires 10 reputation on this site (the association bonus does not count).

Would you like to answer one of these unanswered questions instead?

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