· MumbaiIndia
MILESTONE

# 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));
where:
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 ;

here,
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.

2 likes
Like
Award Contribution