【飞机】自主无人机飞行稳定和轨迹跟踪Matlab实现
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现点击：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
1. 四旋翼无人机飞行动力学基础
- 牛顿运动定律
：用于描述无人机的线性运动。根据牛顿第二定律 F=ma，在三维空间中，无人机在 x、y、z 方向上的合力分别产生相应方向的加速度。在代码中，通过计算控制加速度（考虑位置误差、速度误差以及风干扰）来更新无人机的速度和位置，体现了该定律的应用。例如，ax=Kpx∗ex+Kdx∗evx+0.5∗windX(i) 计算出 x 方向的加速度，随后用于更新 x 方向的速度 vx(i+1)=vx(i)+ax∗dt 和位置 x(i+1)=x(i)+vx(i)∗dt。
- 刚体动力学
：四旋翼无人机可看作刚体，其姿态运动遵循刚体动力学原理。姿态角（滚转 roll、俯仰 pitch、偏航 yaw）的变化由作用在机体上的力矩决定。在代码里，通过计算期望姿态角、角度误差，并结合角速度的更新和积分来模拟姿态的变化。如根据 x、y 方向的加速度计算期望滚转和俯仰角：rollref=−0.04∗ay，pitchref=0.04∗ax，再通过角度误差更新角速度，进而积分得到姿态角的变化。
2. 轨迹跟踪控制策略
- 比例 - 微分（PD）控制
：这是一种常用的反馈控制策略，在代码中用于位置控制。比例（P）环节根据当前位置与参考位置的误差来产生控制量，误差越大，控制量越大，它能快速对误差做出响应，但可能存在稳态误差。微分（D）环节则基于误差的变化率来调整控制量，能预测误差的变化趋势，提前做出反应，有助于减少超调并提高系统的稳定性。例如，在 x 方向控制加速度的计算：ax=Kpx∗ex+Kdx∗evx+0.5∗windX(i)，其中 Kpx 是比例增益，Kdx 是微分增益，ex 是位置误差，evx 是速度误差，通过这两个环节共同作用使无人机跟踪参考轨迹。
- 前馈控制思想的体现
：虽然代码中未明确标记为前馈控制，但在考虑风干扰时体现了类似思想。通过将风干扰纳入控制加速度的计算（如 ax 和 ay 的计算中包含风干扰项），提前对可能影响无人机运动的因素做出补偿，有助于提高轨迹跟踪的精度。
3. 姿态控制原理
- 姿态与力 / 力矩的关系
：无人机的姿态变化会影响其产生的升力方向，从而实现不同方向的运动控制。例如，滚转和俯仰姿态的改变会使升力在水平方向产生分量，用于控制 x 和 y 方向的运动。在代码中，根据期望的水平加速度计算期望的滚转和俯仰角，以此来调整姿态以实现相应方向的运动控制。
- 角度与角速度的积分关系
：姿态角的变化是通过对角速度进行积分得到的。在代码里，先根据角度误差更新角速度（如 p(i+1)=p(i)+4∗eroll∗dt 对于滚转角速度的更新），然后通过积分（如 roll(i+1)=roll(i)+p(i+1)∗dt）得到姿态角的变化，从而模拟无人机姿态的动态变化过程。
4. 干扰处理与稳定性维持
- 风干扰模拟
：实际飞行中，风会对无人机产生干扰，影响其飞行轨迹和稳定性。代码通过定义 x 和 y 方向的风干扰函数（windX=0.2∗sin(0.5∗t) 和 windY=0.2∗cos(0.3∗t)），并将其纳入控制加速度的计算，使无人机在模拟过程中能够应对风的影响，体现了对实际干扰因素的考虑。
- 限制加速度
：为确保无人机运动在合理范围内，防止因控制量过大导致不稳定，对计算得到的控制加速度进行限制（如 ax=max(min(ax,3),−3)）。这有助于维持无人机的飞行稳定性，避免出现异常的加速或减速情况。
- 防止地面碰撞
：通过检测高度（z 坐标），当 z(i+1)<0 时，将高度和垂直速度强制设为零（z(i+1)=0，vz(i+1)=0），防止无人机与地面碰撞，这是保障无人机安全飞行的重要措施。
⛳️ 运行结果
📣 部分代码
%% ADVANCED REPOSITORY 01% Autonomous Drone Flight Control System% Stable 2D Drone Simulationclc;clear;close all;%% TIME SETTINGSdt = 0.02;tEnd = 20;t = 0:dt:tEnd;N = length(t);%% DRONE PARAMETERSm = 1.0;g = 9.81;%% STATESx = zeros(1,N);z = zeros(1,N);vx = zeros(1,N);vz = zeros(1,N);%% INITIAL CONDITIONSx(1) = 0;z(1) = 1;%% REFERENCE TRAJECTORYx_ref = linspace(0,20,N);z_ref = 5 + 0.5*sin(0.4*t);%% CONTROLLER GAINSKp_x = 1.2;Kd_x = 1.8;Kp_z = 4.5;Kd_z = 3.0;%% WIND DISTURBANCEwind = 0.3*sin(0.5*t);%% CONTROL INPUTSax_cmd = zeros(1,N);az_cmd = zeros(1,N);%% MAIN SIMULATION LOOPfor i = 1:N-1%% POSITION ERRORSex = x_ref(i) - x(i);ez = z_ref(i) - z(i);%% VELOCITY ERRORSevx = -vx(i);evz = -vz(i);%% CONTROLLERax_cmd(i) = ...Kp_x*ex + Kd_x*evx;az_cmd(i) = ...Kp_z*ez + Kd_z*evz;%% LIMIT ACCELERATIONax_cmd(i) = ...max(min(ax_cmd(i),3),-3);az_cmd(i) = ...max(min(az_cmd(i),5),-5);%% SYSTEM DYNAMICSax = ax_cmd(i) + wind(i);az = az_cmd(i);%% INTEGRATEvx(i+1) = vx(i) + ax*dt;vz(i+1) = vz(i) + az*dt;x(i+1) = x(i) + vx(i)*dt;z(i+1) = z(i) + vz(i)*dt;%% PREVENT GROUND COLLISIONif z(i+1) < 0z(i+1) = 0;vz(i+1) = 0;endend%% TRAJECTORY FIGUREfigure(1);plot(x_ref,z_ref,'--','LineWidth',2);hold on;plot(x,z,'LineWidth',2);grid on;xlabel('Horizontal Position (m)');ylabel('Vertical Position (m)');title('Drone Trajectory Tracking');legend({'Reference','Drone Path'});%% SAVE FIGURE 1saveas(gcf,'drone_trajectory.png');%% STATES FIGUREfigure(2);subplot(2,1,1);plot(t,x,'LineWidth',1.5);hold on;plot(t,x_ref,'--');grid on;ylabel('X Position');legend({'Actual','Reference'});subplot(2,1,2);plot(t,z,'LineWidth',1.5);hold on;plot(t,z_ref,'--');grid on;ylabel('Z Position');xlabel('Time (s)');legend({'Actual','Reference'});%% SAVE FIGURE 2saveas(gcf,'drone_states.png');%% ANIMATIONfigure(3);filename = 'drone_animation.gif';for i = 1:10:Nclf;plot(x_ref,z_ref,'--');hold on;plot(x(1:i),z(1:i),'b','LineWidth',2);% DRONE BODYdroneWidth = 0.6;droneX = [x(i)-droneWidth ...x(i)+droneWidth];droneZ = [z(i) z(i)];plot(droneX,droneZ,'r','LineWidth',5);% DRONE CENTERplot(x(i),z(i),'ko',...'MarkerSize',8,...'MarkerFaceColor','k');xlim([0 22]);ylim([0 8]);xlabel('Horizontal Position (m)');ylabel('Vertical Position (m)');title('Autonomous Drone Animation');grid on;drawnow;frame = getframe(gcf);im = frame2im(frame);[A,map] = rgb2ind(im,256);if i == 1imwrite(A,map,filename,...'gif',...'LoopCount',Inf,...'DelayTime',0.05);elseimwrite(A,map,filename,...'gif',...'WriteMode','append',...'DelayTime',0.05);endend%% PERFORMANCE METRICStrajectoryError = mean( ...sqrt((x_ref - x).^2 + ...(z_ref - z).^2));%% RESULTSfprintf('\nAUTONOMOUS DRONE RESULTS:\n');fprintf('Final X Position = %.4f m\n',x(end));fprintf('Final Z Position = %.4f m\n',z(end));fprintf('Mean Trajectory Error = %.4f m\n',trajectoryError);fprintf('\nFiles saved successfully:\n');fprintf('1. drone_trajectory.png\n');fprintf('2. drone_states.png\n');fprintf('3. drone_animation.gif\n');%% END