基于 ZOH 离散化与增量 PID 的四旋翼无人机轨迹跟踪控制研究，MATLAB代码

✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍一、四旋翼无人机轨迹跟踪的重要性与挑战重要性四旋翼无人机凭借其灵活的机动性和广泛的应用场景在航拍、物流配送、农业植保、电力巡检等众多领域发挥着关键作用。在这些应用中精确的轨迹跟踪能力是确保无人机完成任务的核心要素。例如在电力巡检中无人机需要沿着预设的输电线路轨迹飞行以便清晰拍摄线路设备及时发现潜在故障在物流配送里准确按照规划轨迹飞行能保证货物准确送达目的地。挑战四旋翼无人机是一个高度非线性、强耦合的复杂系统。其四个旋翼产生的升力不仅决定了无人机的垂直运动还会对其姿态俯仰、滚转、偏航产生影响各运动状态之间相互关联。此外飞行过程中会受到多种外界干扰如气流变化、阵风等这些干扰增加了实现精确轨迹跟踪的难度。传统的控制方法难以应对这种复杂的系统动态和干扰因此需要更有效的控制策略。二、四旋翼无人机的离散化建模四、基于增量 PID 的轨迹跟踪实现跟踪原理在四旋翼无人机轨迹跟踪过程中将期望轨迹按照离散时间步进行划分每个时刻的期望位置、姿态等信息作为参考输入。增量 PID 控制器实时计算无人机实际状态与期望状态的误差根据上述增量 PID 算法计算出控制量增量通过调整四个旋翼的转速来改变无人机的受力和力矩从而调整其运动状态使其逐渐逼近期望轨迹。例如当无人机的实际位置偏离期望轨迹时增量 PID 控制器会根据位置误差计算出相应的控制量增量增加或减少旋翼升力使无人机回到期望轨迹上。优势与效果基于增量 PID 的控制策略结合四旋翼无人机的离散化建模能够较好地应对无人机系统的非线性和强耦合特性。离散化建模为增量 PID 控制器提供了适合数字处理的系统模型而增量 PID 控制器能够根据实时误差快速调整控制输入有效抑制外界干扰对轨迹跟踪的影响实现较为精确的轨迹跟踪效果。同时增量 PID 算法相对简单计算量小适合在无人机搭载的有限计算资源平台上运行保证了控制的实时性。⛳️ 运行结果 部分代码function x_dotEOMs(in)% Forces and Moments, and Equations of Motion% *** QUANTITY ****** UNITS *********************************************% *** mass - {slugs}% *** length - {ft}% *** area - {ft^2}% *** velocity - {ft/s}% *** acceleration- {ft/s^2}% *** density - {slugs/ft^3}% *** force - {lbf}% *** moments - {lbf-ft}% *** angles - {radians} (calculations)% *** velocity - {ft/s}% *** ang. vel. - {rad/s}% *** ang. accel. - {rad/s^2}% ***********************************************************************global maneuver alpha_dot% maneuver is a parameter that sets version to sim (glide or turn)%x_dotzeros(12,1);% extract inputs from input vectorT in(1); % Thrustde in(2); % Elevator Deflection (down is ) {deg}drt in(3); % Rudder Deflection {deg}da in(4); % Aileron Deflection (deg)% extract states from input vectorV in(5); % Velocity {ft/s}gamma in(6); % Flight path angle {rad}alpha in(7); % Angle of attack {rad}q in(8); % Pitch Rate {rad/s}p in(9); % Roll Rate {rad/s}mu in(10); % Bank Angle (About Velocity Vector) {rad}beta in(11); % Sideslip Angle {rad}r in(12); % Yaw Rate {rad/s}chi in(13); % Heading angle {rads}north in(14); % North Position {ft}east in(15); % East Position {ft}h in(16); % Altitude {ft}% extract simulation time{sec} from input(used to calculate windup thrust)tm in(17);% DEFINE ANY NEEDED TERMS THAT APPEAR IN THE E.O.M.S% Define and/or Calculate Necessary Constantsd2r pi/180; %Convert Degrees to Rads -- Although it s already%coded in Matlab (DEG2RAD) - Checkedr2d 180/pi; %Convert Rads to Degrees -- Although it s already%coded in Matlab (RAD2DEG) - Checkedrho .0023081; %Air Density (Slugs per ft^3) - Checkedg 32.17; %Gravity - Checkedm .487669; %Slugs - Empty Mass of A/C (w/o fuel)(7.117 Kg empty)Iyy 1.5523; %Inertias Experimentally determined using Empty MassIxx 1.9480; %Inertia Units are (slugs*ft^2) - CheckedIzz 1.9166; % - CheckedIxz 0; %Assumed zero due to symmetric aircraft - AssumedS 10.56; %Square Feet - Wing Area Converted from%Manuf 1520 sq in area - Checked (also Matches DigDat)CLow .421; %from Look up table for Eppler 193 at AoA0,%DigDat .421, Line # 277 - Checked CL columnCLaw 4.59; %Coef of Lift for finite wing Cla/(1 (Cla/(pi*ARw*e)%DigDat 4.59, Line #276-277, CLA column- CheckedCDminw .011; %DigDat Min Drag of Wing at AoA -6 deg,%Dig Dat Line# 274- CheckedARw 7.9456; %Aspect Ratio AR (b^2)/S- Checkede .75; %Span efficiency factor -estimation- CheckedKw 1/(pi*ARw*e); % 1/(pi*AR*e)- CheckedCmw -0.005; %DigDat AoA 0, Line #277- Checkedcgw -.416; %Distance Aero Center is back from CG, 5 Inchesc 1.3333; %Feet - Root Chord of Wing (16)- Checkedb 9.16; %Feet - Span (110)- Checkedlambda .72955; %Taper Ratio from S(Cr*(1Lambda)*b)/2- CheckedCLat .76; %Dig Dat, Line #346, CLA Column- CheckedCDmint .002; %Dig Dat, Line #345, at AoA -2 degKt .446; %1/(pi*e*AR) SET SAME AS WING OR DIGDAT From BLAKEit 2*d2r; %Tail incidence 2 degreesTe .422; %Blake from DigDatnt 1; %Blake from DigDatSt S; %Horiz Tail Area Square FeetReference Area Wing Areacgt 3.5; %Distance tail Aero Center back from A/C CG,%42 inches - MeasuredCLavt .0969; %Dig Dat Line #190CDminvt .001; %Dig Dat Line #409, AOA -10 deg, Column CDSvt S; %Vert Tail Area Square Feet Reference Area Wing AreaTr .434; %Blake from DigDatnvt nt; %Same as Hori Tailcgvt cgt; %Same as Hori tailCmaf .114; %Dig Dat, Line#209, at AoA 0CDf .005; %Dig Dat, Line#209, at AoA 0Cnda -0.0128; %per rad DigDatClda 0.244; %per rad DigDat% Define/calculate any needed coefficients, forces, etc.CLw CLowCLaw*alpha;CDw CDminwKw*CLw^2;E 2*(CLowCLaw*(alpha-alpha_dot*(cgtcgw)/V))/(pi*ARw);alphat alphaitTe*deq*cgt/V-E;CLt CLat*alphat;CDt CDmintKt*CLt^2;Cmf Cmaf*alpha;CDvt CDminvt;Clp -1/12*CLaw*(13*lambda)/(1lambda);Clb -.1;Clr .01;qb .5*rho*V^2;Lw qb*S*CLw;Dw qb*S*CDw;Mw qb*S*c*Cmw;Lt nt*qb*St*CLt;Dt nt*qb*St*CDt;Df qb*S*CDf;Mf qb*S*c*Cmf;Dvt nt*qb*Svt*CDvt;% Calculate Forces and Moments% LiftL LwLt*cos(E-q*cgt/V)-(DtDvt)*sin(E-q*cgt/V);% DragD Dw(DtDvt)*cos(E-q*cgt/V)Lt*sin(E-q*cgt/V)Df;% Side ForceY nvt*qb*Svt*CLavt*(-betaTr*drtr*cgvt/V);% Pitch MomentMc Lw*cgw*cos(alpha)Dw*cgw*sin(alpha)Mw-Lt*cgt*...cos(alpha-Eq*cgt/V)-(DtDvt)*cgt*sin(alpha-Eq*cgt/V)Mf;% Yaw MomentNc -qb*nvt*Svt*CLavt*(-betaTr*drtr*cgvt/V)*cgvt(-qb*S*b*Cnda*da);% Roll MomentLc qb*S*b^2/(2*V)*(Clp*p2*V/b*Clb*betaClr*drtClda*da*2*V/b);% ------ NONLINEAR 6-DOF EQUATION OF MOTION (EOMs) ---------%% These are the state derivative equations; the comment names the state,% but the equation is for its derivative (rate)%% The equations are arranged by aircraft mode%% NOTE: These assume that Ixz0, if not, then eqs need to be modified% Longitudinal (phugoid and short period): V, gamma, q, alpha% Phugoid: V, gamma% Velocityx_dot 1/m*(-D*cos(beta)Y*sin(beta)T*cos(beta)*cos(alpha))-...g*sin(gamma);% Flight Path Anglegamma_dot 1/(m*V)*(-D*sin(beta)*sin(mu)-Y*sin(mu)*cos(beta)...L*cos(mu)T*(cos(mu)*sin(alpha)sin(mu)*sin(beta)*cos(alpha)))...-g/V*cos(gamma);% Short Period: alpha, q% Angle of Attachalpha_dot q-tan(beta)*(p*cos(alpha)r*sin(alpha))-1/(m*V*cos(beta))...*(LT*sin(alpha))g*cos(gamma)*cos(mu)/(V*cos(beta));% Pitch Rateq_dot Mc/Iyy1/Iyy*(Izz*p*r-Ixx*r*p);% Lateral (roll) - Directional (yaw): p, mu, beta, r% Roll: p, mu% Roll Ratep_dot Lc/Ixx1/Ixx*(Iyy*r*q-Izz*q*r);% Bank Angle (about velocity vector)mu_dot 1/cos(beta)*(p*cos(alpha)r*sin(alpha))1/(m*V)*(D*sin(beta)...*cos(mu)*tan(gamma)Y*tan(gamma)*cos(mu)*cos(beta)L*(tan(beta)...tan(gamma)*sin(mu))T*(sin(alpha)*tan(gamma)*sin(mu)sin(alpha)*...tan(beta)-cos(alpha)*tan(gamma)*cos(mu)*sin(beta)))-...g/V*cos(gamma)*cos(mu)*tan(beta);% Dutch Roll: beta, r% Side Slip Anglebeta_dot -r*cos(alpha)p*sin(alpha)1/(m*V)*(D*sin(beta)Y*cos...(beta)-T*sin(beta)*cos(alpha))g/V*cos(gamma)*sin(mu);% Yaw Rater_dot Nc/Izz1/Izz*(Ixx*p*q-Iyy*p*q);% Heading Angle (from North)chi_dot 1/(m*V*cos(gamma))*(D*sin(beta)*cos(mu)Y*cos(mu)*cos(beta)...L*sin(mu)T*(sin(mu)*sin(alpha)-cos(mu)*sin(beta)*cos(alpha)));% Kinematic Equations% North Positionn_dot V*cos(gamma)*cos(chi);% East Positione_dot V*cos(gamma)*sin(chi);% Altitudeh_dot V*sin(gamma);% Pack derivatives into output vector x_dotx_dot(1) V_dot;x_dot(2) gamma_dot;x_dot(3) alpha_dot;x_dot(4) q_dot;x_dot(5) p_dot;x_dot(6) mu_dot;x_dot(7) beta_dot;x_dot(8) r_dot;x_dot(9) chi_dot;x_dot(10) n_dot;x_dot(11) e_dot;x_dot(12) h_dot;end 参考文献[1]张伟,张三乐,宋小康,等.四旋翼无人机的运动控制与轨迹规划[J].西安文理学院学报(自然科学版), 2023, 26(4):40-48.DOI:10.3969/j.issn.1008-5564.2023.04.007.往期回顾扫扫下方二维码