1
$\begingroup$

I derived a bicopter dynamical model with two servos and two BLDC motors. And now am trying to simulate it using Matlab. As base for simulation I used this paper and this code

Unfortunately, the obtained model always blowing up in case of asymmetrical control input signals. For example, with identical speed on both propellers and 5 degrees tilting angle for one of the thrust vectors, the system is blowing up in less the half second.

Here is simulation plots: dt = 0.0001, first 1300 points. sim 1300, dt = 0.0001

This result obtained for constant 5 degree tilting angle for one of the motors and equal motor speeds. Angular acceleration just grows to the infinity in less than 1 second.

Matlab code:

% Perform a simulation of a copter, from t = 0 through t = 1.
% As an argument, take a controller function. This function must accept
% a struct containing the physical parameters of the system and the current
% gyro readings. The controller may use the strust to store persistent state, and
% return this state as a second output value. If no controller is given,
% a simulation is run with some pre-determined inputs.
% The output of this function is a data struct.

function result = simulate(controller, tstart, tend, dt)
    % Physical constants.
    g = 9.81;
    m = 0.95;

    d = 0.35;
    h = 0.08;

    I_body = diag([5e-3, 5e-3, 10e-3]);
    I_react=1e-3;
    I_gyro=1e-3;

    I = struct('I_body', I_body, 'I_react', I_react, 'I_gyro', I_gyro);

    kd = 0.25;

    kt = 0.00001*9.8;
    kf = 0.0015*9.8;

    % Simulation times, in seconds.
    if nargin < 4
        tstart = 0;
        tend = 1;
        dt = 0.0001;
    end
    ts = tstart:dt:tend;

    % Number of points in the simulation.
    N = numel(ts);

    % Output values, recorded as the simulation runs.
    xout = zeros(3, N);
    xdotout = zeros(3, N);
    thetaout = zeros(3, N);
    thetadotout = zeros(3, N);

    inputout_beta = zeros(2, N);
    inputout_motor_w = zeros(2, N);

    inputout_beta_speed = zeros(2, N);
    inputout_beta_acc = zeros(2, N);

    inputout = struct('inputout_beta', inputout_beta, 'inputout_beta_speed', inputout_beta_speed ,...
        'inputout_beta_acc', inputout_beta_acc,'inputout_motor_w', inputout_motor_w);

    % Struct given to the controller. Controller may store its persistent state in it.
    controller_params = struct('dt', dt, 'I', I, 'kf', kf, 'd', d, 'h', h, 'm', m, 'g', g);

    % Initial system state.
    x = [0; 0; 10];
    xdot = zeros(3, 1);
    theta = zeros(3, 1);

    % If we are running without a controller, do not disturb the system.
    if nargin == 0
        thetadot = zeros(3, 1);
    else
        % With a control, give a random deviation in the angular velocity.
        % Deviation is in degrees/sec.
        deviation = 300;
        thetadot = deg2rad(2 * deviation * rand(3, 1) - deviation);
    end

    ind = 0;
    for t = ts
        ind = ind + 1;

        % Get input from built-in input or controller.
        if nargin == 0
            i = input(t); %built-in generator
        else
            [i, controller_params] = controller(controller_params, thetadot);
        end

                        % Store simulation state for output.
        % don't go through the ground
            if (x(3) < 0)
                x(3) = 0;
                theta(1) = 0; % avoid integration windup
                theta(2) = 0;
            end

        %%%%% Compute forces, torques, and accelerations.
        omega = thetadot2omega(thetadot, theta); 
        a = acceleration(i, theta, m, g, kf, kt, kd, xdot);
        omegadot = angular_acceleration(i, omega, I, d, h, kf, kt);

        %%%% Advance system state. Euler method.
        omega = omega + dt * omegadot;
        thetadot = omega2thetadot(omega, theta);

        theta = theta + dt * thetadot;
        xdot = xdot + dt * a;
        x = x + dt * xdot;

        linacc(:, ind) = a;
        angacc(:, ind) = omegadot;a
        xout(:, ind) = x;
        xdotout(:, ind) = xdot;
        thetaout(:, ind) = theta;
        omegaout(:, ind) = omega;
        inputout(:, ind) = i;


    end
    % Put all simulation variables into an output struct.
    result = struct('linear_coord', xout, 'angular_coord', thetaout, 'linear_velocity', xdotout, ...
                    'angular_velocity',omegaout ,'angular_acc', angacc, 'linear_acc', linacc,'t', ts, 'dt', dt, 'input', inputout);

end

% Arbitrary test input.
function in = input(t)
inputout_beta= zeros(2, 1);
inputout_motor_w= zeros(2, 1);

    inputout_beta(1) = 0.15;%-0.1*cos(t);
    inputout_beta(2) = 0;%0.1*cos(t);
    inputout_motor_w(1)= 3000*pi/30;%100*pi/30*cos(t) %rpm to radian per second 
    inputout_motor_w(2)= 3000*pi/30;%101*pi/30*cos(t) %rpm to radian per second 

    inputout_beta_speed(1)=0;
    inputout_beta_speed(2)=inputout_beta_speed(1);
    inputout_beta_acc = zeros(2, 1);

    in = struct('inputout_beta', inputout_beta, 'inputout_beta_speed', inputout_beta_speed ,...
        'inputout_beta_acc', inputout_beta_acc,'inputout_motor_w', inputout_motor_w);
end

% Compute thrust given current inputs and thrust coefficient.
function F = force(w_motor, beta_motor, kf)
     w_l=w_motor(1);
     w_r=w_motor(2);
     beta_l=beta_motor(1);
     beta_r=beta_motor(2);

    %Right power plant force
    f_r=kf*w_r;
    %Left power plant force
    f_l=kf*w_l;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %Left motor force rot mat
    R_Pl_Bm=[cos(beta_l), 0, sin(beta_l);
          0,          1,       0; 
         -sin(beta_l),0, cos(beta_l)];
    %Right motor force rot mat    
    R_Pr_Bm=[cos(beta_r), 0, sin(beta_r);
          0,          1,       0; 
         -sin(beta_r),0, cos(beta_r)] ;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    F_l=[0; 0; f_l];
    F_r=[0; 0; f_r];
    F=R_Pl_Bm*F_l+R_Pr_Bm*F_r;
end

% Compute torques, given current inputs, length, drag coefficient, and thrust coefficient.
function T_Bm = torque(input, d, h, k_f, k_d, I)
     w_l=input.inputout_motor_w(1);
     w_r=input.inputout_motor_w(2);

     beta_l=input.inputout_beta(1);
     beta_r=input.inputout_beta(2);

     beta_l_speed=input.inputout_beta_speed(1);
     beta_r_speed=input.inputout_beta_speed(2);

     beta_l_acc=input.inputout_beta_acc(1);
     beta_r_acc=input.inputout_beta_acc(2);

    %Right power plant torque
    tau_drag_r=k_d*w_r;
    %Left power plant torque
    tau_drag_l=k_d*w_l;

    %Right power plant force
    f_r=k_f*w_r;
    %Left power plant force
    f_l=k_f*w_l;

    F_l=[0; 0; f_l];
    F_r=[0; 0; f_r];

    OlOb=[0;d;h];
    OrOb=[0;-d;h];

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %Left motor force rot mat
    R_Pl_Bm=[cos(beta_l), 0, sin(beta_l);
              0,          1,       0; 
             -sin(beta_l),0, cos(beta_l)];
    %Right motor force rot mat    
    R_Pr_Bm=[cos(beta_r), 0, sin(beta_r);
             0,          1,       0; 
             -sin(beta_r),0, cos(beta_r)] ;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     I_gyro = I.I_gyro;
     I_react = I.I_react;

     % TORQUES
        %Gyroscopic moments, longitudinal tilting
        Mgyro_Pl_beta_l=[0;-I_gyro*w_l*beta_l_speed;0];
        Mgyro_Pr_beta_r=[0;I_gyro*w_r*beta_r_speed;0];
        %Gyroscopic momentin body frame
        Mgyro_Bm=R_Pl_Bm*Mgyro_Pl_beta_l+R_Pr_Bm*Mgyro_Pr_beta_r;

        %Fan torques due aerodynamic drag of the propeller
        Mdrugr_Pr=[0;0;-tau_drag_r];
        Mdrugl_Pl=[0;0;tau_drag_l];
        %Drag torques in body frame
        Mdrug_Bm=R_Pl_Bm*Mdrugl_Pl+R_Pr_Bm*Mdrugr_Pr;

        %Moment from trust vectoring in body frame
        M_thrv_Bm=cross((R_Pl_Bm*F_l),OlOb)+cross((R_Pr_Bm*F_r),OrOb);

        %Adverse Inertial Reaction moment
        M_react_Pr=[0;-I_react*beta_r_acc;0];
        M_react_Pl=[0;-I_react*beta_l_acc;0];
        %Adverse Inertial Reaction moment, body frame
        M_react_Bm=R_Pr_Bm*M_react_Pr+R_Pl_Bm*M_react_Pl;
        %FULL TORQUE
        %Body frame
        T_Bm=Mgyro_Bm+Mdrug_Bm+M_thrv_Bm+M_react_Bm;
end

% Compute acceleration in inertial reference frame
% Parameters:
%   g: gravity acceleration
%   m: mass of quadcopter
%   kf: thrust coefficient
%   kt: global drag coefficient
function A = acceleration(inputs, angles, m, g, kf, kt, kd, linear_velocity)
    F_g = [0; 0; -g];
    R_Bm_Wi = rotation(angles);
    F_Bm = force(inputs.inputout_motor_w, inputs.inputout_beta, kf);
    %Friction as a force proportional to the linear velocity in each direction.
    F_a = -kd * linear_velocity;
    %mV=R*F
    A = R_Bm_Wi * F_Bm * 1/m + F_g + F_a;
end

% Compute angular acceleration in body frame
% Parameters:
%   I: inertia matrix
function omegad = angular_acceleration(inputs, omega, I, d, h, kf, kt)
    T = torque(inputs, d, h, kf, kt, I);
    omegad = I.I_body\(T - cross(omega, I.I_body * omega));
end

% Convert derivatives of roll, pitch, yaw to omega.
function omega = thetadot2omega(thetadot, angles)
    phi = angles(1);
    theta = angles(2);
    psi = angles(3);
    W = [1,   0,                   -sin(phi);
         0    cos(phi),    cos(phi)*sin(phi);
         0   -sin(phi), cos(theta)/cos(phi)];                    
    omega = W * thetadot;
end

% Convert omega to roll, pitch, yaw derivatives
function thetadot = omega2thetadot(omega, angles)
    phi = angles(1);
    theta = angles(2);
    psi = angles(3);
    W = [1,     sin(phi)*tan(theta),cos(phi)*tan(theta);
         0      cos(phi),                     -sin(phi);
         0      sin(phi)/cos(theta), cos(phi)/cos(phi)];
    thetadot = W * omega;
end

Newton-Euler equations:

$$ \ddot{x} m=[K_f w_l c(\beta_l) + K_f w_r c(\beta_r)] [s(\phi) s(\psi) + c(\phi) c(\psi) s(\theta)] + c(\psi) c(\theta) [K_f w_l s(\beta_l) + K_f w_r s(\beta_r)] $$

$$ \ddot{y} m =c(\theta) s(\psi) [K_f w_l s(\beta_l) + K_f w_r s(\beta_r)] - [K_f w_l c(\beta_l) + K_f w_r c(\beta_r)] [c(\psi) s(\phi) - c(\phi) s(\psi) s(\theta)] $$

$$ \ddot{z} m = c(\phi) c(\theta) (K_f w_l c(\beta_l) + K_f w_r c(\beta_r)) - s(\theta) (K_f w_l s(\beta_l) + K_f w_r s(\beta_r)) - g m $$

$$ I_{xx} \ddot{\phi}+\dot{\theta} \dot{\psi} (I_{zz}-I_{yy} )=K_t w_l s(\beta_l) - K_t w_r s(\beta_r) + K_f d w_l c(\beta_l) - K_f d w_r c(\beta_r) $$

$$ I_{yy} \ddot{\theta}+\dot{\psi} \dot{\phi} (I_{xx}-I_{zz} )=I_g \dot{\beta_r} w_r - I_r \ddot{\beta_r} - I_g \dot{\beta_l} w_l - I_r \ddot{\beta_l} - K_f h w_l s(\beta_l) - K_f h w_r s(\beta_r) $$

$$ I_{zz} \ddot{\psi}+\dot{\theta} \dot{\phi} (I_{zz}-I_{yy} )=K_t w_l c(\beta_l) - K_t w_r c(\beta_r) - K_f d w_l s(\beta_l) + K_f d w_r s(\beta_r) $$

Where: $d,h$ - distances to CG

$I$- Moments of inertia

$k_f,k_t$ - experimentally obtained coefficients

enter image description here Problem is independent from simulation step used. I also tried RK4 realization with the same result. How can I correct this behavior? What is the cause of the singularity?

$\endgroup$
  • 2
    $\begingroup$ Have you tried your code on a simpler test case than the full problem? Your $dt = 0.05$ could be a bit large, especially since you are using an explicit method. Does it get better if you use a much smaller dt, like $10^{-4}$ or $10^{-5}$? $\endgroup$ – Kirill Aug 29 '15 at 19:48
  • $\begingroup$ Can you add the equations for your model? Also, can you add the results as graphics instead of tables? $\endgroup$ – nicoguaro Aug 29 '15 at 20:01
  • $\begingroup$ That dt tho.... $\endgroup$ – Loonuh Aug 29 '15 at 20:35
  • $\begingroup$ >Does it get better if you use a much smaller dt Unfortunately, no. $\endgroup$ – Andrew Aug 29 '15 at 21:43
  • $\begingroup$ What is the line thetadot = omega2thetadot(omega, theta) doing? Why is $\dot\theta$ a function of only $\omega, \theta$, assuming I'm guessing correctly what those variables are? Also, when replying to a comment please add @-mentions ("@Username" at the beginning) so that Username gets a notification. The angular velocity graph never moves even for blue-dashed, for which angular acceleration is constant and large, which seems suspicious. $\endgroup$ – Kirill Aug 29 '15 at 22:09

Your Answer

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

Browse other questions tagged or ask your own question.