基于MATLAB/Simulink的四旋翼无人机仿真程序实现

一、系统架构设计

1. Simulink模型结构

% 主模型结构（quadcopter_sim.slx）
Model Workspace参数管理
├─ Reference Generator（轨迹生成）
├─ Controller Subsystem（PID/ADRC控制器）
├─ Quadrotor Dynamics（6自由度动力学模型）
├─ Motor Mixing Module（电机控制）
├─ 3D Visualization（Simulink 3D Animation）
└─ Data Logging（状态记录）

2. 动力学建模

基于牛顿-欧拉方程建立6自由度模型：

% quad_dynamics.m（核心动力学函数）
function dx = quad_dynamics(~,x,u)
    m = 1.0; g = 9.81; Ixx=0.007; Iyy=0.007; Izz=0.012; L=0.2;
    F = u(1); tau_phi=u(2); tau_theta=u(3); tau_psi=u(4);

    % 位置导数
    pos_dot = x(4:6);

    % 姿态转换矩阵
    R = [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi), ... 
         sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi)];

    % 加速度计算
    acc = R * [0;0;F/m] - [0;0;g];
    vel_dot = acc;

    % 角度导数
    angle_dot = [1, sin(phi)*tan(theta), cos(phi)*tan(theta); 
                 0, cos(phi), -sin(phi); 
                 0, sin(phi)/cos(theta), cos(phi)/cos(theta)] * x(10:12);

    % 角速度导数
    omega_dot = [tau_phi/Ixx; tau_theta/Iyy; tau_psi/Izz];

    dx = [pos_dot; vel_dot; angle_dot; omega_dot];
end

二、控制器设计

1. PID控制器（串级结构）

% position_pid.m（外环位置控制）
function u_out = position_pid(ref, x)
    Kp_pos = [2,2,5]; Kd_pos = [2,2,3]; Ki_pos = [0.1,0.1,0.2];
    e_pos = ref - x(1:3); e_vel = -x(4:6);
    acc_des = Kp_pos.*e_pos + Kd_pos.*e_vel;
    acc_des(3) = acc_des(3) + g; % 重力补偿
    phi_des = (acc_des(1)*sin(x(9)) - acc_des(2)*cos(x(9)))/g;
    theta_des = (acc_des(1)*cos(x(9)) + acc_des(2)*sin(x(9)))/g;
    u_out = [acc_des(3), phi_des, theta_des, 0]; % 输出总升力和姿态角
end

% attitude_pid.m（内环姿态控制）
function u = attitude_pid(ref_att, x)
    Kp_att = [5,5,3]; Kd_att = [1,1,1];
    e_att = ref_att(1:3) - x(7:9); e_omega = -x(10:12);
    tau = Kp_att.*e_att + Kd_att.*e_omega;
    u = [ref_att(4), tau(1), tau(2), tau(3)]; % 总升力+力矩
end

2. ADRC控制器（非线性扩展）

% adrc_controller.m（姿态控制）
function u = adrc_controller(ref, y, v, params)
    % 参数提取
    w0 = params.w0; beta1=params.beta1; beta2=params.beta2;
    h = params.h; b0=params.b0;

    % 跟踪微分器（TD）
    v(1) = v(1) + h*v(2);
    v(2) = v(2) + h*(-beta1*fal(v(1)-ref,0.5,0.01) - beta2*abs(v(2))^0.5*sign(v(2)));

    % 扩张状态观测器（ESO）
    e = z1 - y; z1 = z1 + h*(z2 - beta1*e + b0*u_prev);
    z2 = z2 + h*(-beta2*e);

    % 非线性状态反馈（NLSEF）
    e1 = ref - y; e2 = v(2) - z2;
    u0 = beta03*fal(e1,0.5,0.01) + beta04*fal(e2,0.25,0.01);
    u = (u0 - z2)/b0;
end

三、仿真实现步骤

1. 轨迹生成

   生成螺旋/阶跃/正弦轨迹：

   t = 0:0.01:10;
   switch traj_type
       case 'spiral'
           x_ref = 2*sin(t); y_ref = 2*cos(t); z_ref = 0.5*t;
       case 'step'
           x_ref = 1*(t>=1); y_ref = 1*(t>=1); z_ref = 2*(t>=1);
   end
   

2. 控制器参数整定

   • 位置环：Kp=2-5, Kd=1-3
   • 姿态环：Kp=30-50, Kd=5-10
   • ADRC参数：w0=50-100, beta1=2*w0, beta2=w0^2

3. 模型搭建

   % 创建子系统
   add_block('simulink/Sources/Step', 'quad_model/Reference');
   add_block('simulink/Continuous/Transfer Fcn', 'quad_model/Dynamics');
   add_block('simulink/User-Defined Functions/MATLAB Function', 'quad_model/PID_Controller');
   

4. 可视化配置

   • 3D动画：使用Simulink 3D Animation模块
   • 状态曲线：添加Scope显示位置/姿态/角速度

四、性能对比与优化

优化建议：

1. 添加风扰模型测试鲁棒性
2. 采用LQR/MPC替代PID提升动态性能
3. 使用ROS工具箱实现硬件在环测试

参考代码 四旋翼无人机MATLAB的simulink仿真程序 www.youwenfan.com/contentalh/45313.html

五、扩展功能实现

1. 路径规划

   结合A*算法生成避障路径：

   function path = astar_planner(start,goal,map)
       % 实现A*算法（引用自）
       % 返回平滑路径点
   end
   

2. 集群控制

   多机编队控制算法：

   % 虚拟结构法
   for i = 1:N
       leader_pos = positions(leader_idx,:);
       follower_des = leader_pos + formation_pattern(i,:);
       % 计算相对位置误差并调整控制量
   end
   

六、完整代码结构

project_root/
├── controllers/       # 控制算法
│   ├── pid_controller.m
│   └── adrc_controller.m
├── dynamics/          # 动力学模型
│   └── quad_dynamics.m
├── simulations/       # 仿真脚本
│   └── quad_sim.slx
├── visualization/     # 3D动画
│   └── animate.m
└── reference/         # 轨迹数据
    └── spiral_traj.mat