Gaurav Yadav
quadcopter (Project Leader)
 · MumbaiIndia
Get Link

Complete Mathematical modelling and Control equations for Quadcopter .

Hello guys, I have finally completed the mathematical modelling for the quadcopter. Below is the detailed description for the same. We have z-axis pointing down, x-axis pointing left to right and y-axis perpendicular to the screen. Let rotation along the x-axis be denoted with phi, the rotation along the y-axis be denoted with theta and rotation along the z-axis be denoted by psi. The angular velocities along these axes x,y and z be denoted by p, q and r respectively.

  1. Balancing the THRUST: In order to balance the thrust, we have to apply appropriate acceleration in the z-axis
    along with mg.
    F (or THRUST) = mass
    (gravity+ des_state.acc(z)+Kdz*(des_state.vel(z)-state.vel(z))+Kpz*(des_state.pos(z)-state.pos(z));
    des_state.acc(z) = Desired acceleration in z-direction,
    des_state.vel(z) = Desired velocity in in z-direction,
    des_state.pos(z) = Desired position in z-direction,
    state.vel(z) = Actual velocity in z-direction,
    state.pos(z) = Actual position in z-direction,
    Kdz = Derivative gain in z- direction,
    kpz = Proportional gain in z-direction.

  2. Balancing the MOMENT : We need to balance the moments in the x, y and z axis in order to keep the quadcopter stable. Below is the equation for the same.
    M(or MOMENT) =[Kpphi*(phi_des-state.rot(x)) + Kdphi*(0-p);
    Kptheta*(theta_des-state.rot(y)) + Kdtheta*(0-q);
    Kppsi*(des_state.yaw-state.rot(z)) + Kdpsi*(des_state.yawdot-r)];

    where :
    r_x_des_ddot = des_state.acc(x)+Kdx*(des_state.vel(x)-state.vel(x))+Kpx*(des_state.pos(x)-state.pos(x));
    r_y_des_ddot = des_state.acc(y)+Kdy*(des_state.vel(y)-state.vel(y))+Kpy*(des_state.pos(y)-state.pos(y));
    phi_des = (r_x_des_ddotdes_state.yaw - r_y_des_ddot)/gravity ;
    theta_des = (r_x_des_ddot + r_y_des_ddot
    des_state.yaw)/gravity ;

    phi_des = Desired phi angle(along x axis),
    state.rot(x) = Actual phi angle(along x-axis),
    p = Actual angular velocity along x-axis,
    Kpphi = Proportional gain in phi angle,
    kdphi = Derivative gain in angular velocity along x-axis,
    theta_des = Desired theta angle(along y axis),
    state.rot(y) = Actual theta angle(along y-axis),
    q = Actual angular velocity along y-axis,
    Kptheta = Proportional gain in theta angle,
    kdtheta = Derivative gain in angular velocity along y-axis,
    des_state.yaw = Desired psi angle(along z axis),
    state.rot(z) = Actual psi angle(along z-axis),
    des_state.yawdot= Desired angular velocity along z-axis,
    r = Actual angular velocity along z-axis,
    Kppsi = Proportional gain in psi angle,
    kdpsi = Derivative gain in angular velocity along z-axis,

I do understand that these equations are not easy to understand in a go, so please give it some time. For any doubt, please comment. Also, let me know if there is requirement for a video if you do not get the complete clarity.

1 like 
Award Contribution
Leave a reply...
New Post
Start Project
Online Users
Share Link