www.gusucode.com > 简单的PD控制器仿真源码程序 > 简单的PD控制器仿真源码程序/PDControlOfQuadrotor/PDControlOfQuadrotor/quad_control_read_fn.m
% The function used with the program: Quadrotor control function xdot = quad_control_fn(t,x) global Jtp Ixx Iyy Izz b d l m g Kpz Kdz Kpp Kdp Kpt Kdt Kpps Kdps ZdF PhidF ThetadF PsidF ztime phitime thetatime psitime Zinit Phiinit Thetainit Psiinit Uone Utwo Uthree Ufour Ez Ep Et Eps % The desired values % Changes in Z start at t = 3, changes in Phi start at t = 1, changes in % Theta start at the origin, and chanfes in Psi start at t= 2 % If you want that all start at the origin simply remove the conditions % time for change start of each variable ztime = 3; phitime = 1; thetatime = 0.2; psitime = 2; %%% HEIGHT %%% if t < ztime Zd = Zinit; end if t >= ztime Zd = ZdF; end %%%%%%%%%%%%%% %%% Phi %%% if t < phitime Phid = Phiinit; end if t >= phitime Phid = PhidF; end %%%%%%%%%%%%%% %%% Theta %%% if t < thetatime Thetad = Thetainit; end if t >= thetatime Thetad = ThetadF; end %%%%%%%%%%%%%% %%% Psi %%% if t < psitime Psid = Psiinit; end if t >= psitime Psid = PsidF; end %%%%%%%%%%%%%% PsidF = 2*pi; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% thetaddot = 0; phiddot = 0; psiddot = 0; Zddot = 0; % Zd = 10; % Bounding the angles within the -2*pi / 2*pi range if (x(7)> 2*pi || x(7)< - 2*pi) x(7) = rem(x(7),2*pi); end if (x(9)> 2*pi || x(9)< - 2*pi) x(9) = rem(x(9),2*pi); end if (x(11)> 2*pi || x(11)< - 2*pi) x(11) = rem(x(11),2*pi); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%PD-Z-Control%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Evaluate the Controls U = []; % The control vector U(1) = m*(g + Kpz*(Zd - x(5)) + Kdz*( - x(6)))/(cos(x(9))*cos(x(7))); % Total Thrust on the body along z-axis U(2) = (Kpp*(Phid - x(7)) + Kdp*( - x(8))); % Roll input U(3) = (Kpt*(Thetad - x(9)) + Kdt*( - x(10))); % Pitch input U(4) = (Kpps*(Psid - x(11)) + Kdps*( - x(12))); % Yawing moment U = real(U); U = [U(1);U(2);U(3);U(4)]; % The control vector % % Bounding the controls % if U(1) > 15.7 % U(1) = 15.7; % end % % if U(1) < 0 % U(1) = 0; % end % % for j = 2:4 % if U(j) > 1 % U(j) = 1; % end % % if U(j) < -1 % U(j) = -1; % end % end % Calculation of angular velocities omegasqr(1) = (1/4*b)*U(1) + (1/2*b*l)*U(3) - (1/4*d)*U(4); omegasqr(2) = (1/4*b)*U(1) - (1/2*b*l)*U(2) + (1/4*d)*U(4); omegasqr(3) = (1/4*b)*U(1) - (1/2*b*l)*U(3) - (1/4*d)*U(4); omegasqr(4) = (1/4*b)*U(1) + (1/2*b*l)*U(2) + (1/4*d)*U(4); omegasqr = real(omegasqr); omega(1) = sqrt(omegasqr(1)); omega(2) = sqrt(omegasqr(2)); omega(3) = sqrt(omegasqr(3)); omega(4) = sqrt(omegasqr(4)); % Bounding the angular velocities for j = 1:4 if omega(j) > 523 omega(j) = 523; end if omega(j) < 125 omega(j) = 125; end end omegasqr(1) = (omegasqr(1))^2; omegasqr(2) = (omegasqr(2))^2; omegasqr(3) = (omegasqr(3))^2; omegasqr(4) = (omegasqr(4))^2; % % Bounding the angular velocities % for j = 1:4 % if omegasqr(j) > 523 % omegasqr(j) = 523; % end % % if omegasqr(j) < 125 % omegasqr(j) = 125; % end % end % Disturbance Omega = d*(- sqrt(omegasqr(1)) + sqrt(omegasqr(2)) - sqrt(omegasqr(3)) + sqrt(omegasqr(4))); % Evaluation of the State space wrt H-frame xdot(1) = x(2); % Xdot xdot(2) = (sin(x(11))*sin(x(7)) + cos(x(11))*sin(x(9))*cos(x(7)))*(U(1)/m); % Xdotdot xdot(3) = x(4); % Ydot xdot(4) = (-cos(x(11))*sin(x(7)) + sin(x(11))*sin(x(9))*cos(x(7)))*(U(1)/m); % Ydotdot xdot(5) = x(6); % Zdot xdot(6) = - g + (cos(x(9))*cos(x(7)))*(U(1)/m); % Zdotdot xdot(7) = x(8); % phydot xdot(8) = ((Iyy - Izz)/Ixx)*x(10)*x(12) - (Jtp/Ixx)*x(10)*Omega + (U(2)/Ixx); % pdot = phydotdot xdot(9) = x(10); % thetadot xdot(10) = ((Izz - Ixx)/Iyy)*x(8)*x(12) + (Jtp/Iyy)*x(8)*Omega + (U(3)/Iyy); % qdot = thetadotdot xdot(11) = x(12); % thetadot xdot(12) = ((Ixx - Iyy)/Izz)*x(8)*x(10) + (U(4)/Izz); % rdot = psidotdot xdot = xdot';