手把手教你学simulink（28.1）--四旋翼无人机景示例：基于Simulink的四旋翼无人机（Quadcopter）编队飞行控制系统

 function [u_x, u_y, u_z] = consensusController(positions, velocities, neighbors, kp, kv)

    
     % 获取当前无人机的位置和速度
    
     x_i = positions(1);
    
     y_i = positions(2);
    
     z_i = positions(3);
    
     vx_i = velocities(1);
    
     vy_i = velocities(2);
    
     vz_i = velocities(3);
    
  
    
     % 初始化控制信号
    
     u_x = 0;
    
     u_y = 0;
    
     u_z = 0;
    
  
    
     % 计算一致性控制输入
    
     for j = 1:length(neighbors)
    
     x_j = positions(neighbors(j), 1);
    
     y_j = positions(neighbors(j), 2);
    
     z_j = positions(neighbors(j), 3);
    
     vx_j = velocities(neighbors(j), 1);
    
     vy_j = velocities(neighbors(j), 2);
    
     vz_j = velocities(neighbors(j), 3);
    
  
    
     u_x = u_x - kp * (x_i - x_j) - kv * (vx_i - vx_j);
    
     u_y = u_y - kp * (y_i - y_j) - kv * (vy_i - vy_j);
    
     u_z = u_z - kp * (z_i - z_j) - kv * (vz_i - vz_j);
    
     end
    
 end

 % 添加协同控制器模块

    
 add_block('simulink/User-Defined Functions/MATLAB Function', 'Consensus_Controller');
    
 set_param('Consensus_Controller', 'FunctionName', 'consensusController');
    
  
    
 % 连接协同控制器模块
    
 connect_blocks('Drone_Positions', 'Consensus_Controller');
    
 connect_blocks('Drone_Velocities', 'Consensus_Controller');
    
 connect_blocks('Neighbor_List', 'Consensus_Controller');

 % 添加UDP发送模块

    
 add_block('simulink/Signal Routing/UDP Send', 'UDP_Sender');
    
 set_param('UDP_Sender', 'RemoteIPAddress', '127.0.0.1'); % 设置远程IP地址
    
 set_param('UDP_Sender', 'RemotePort', '5000'); % 设置远程端口号
    
  
    
 % 添加UDP接收模块
    
 add_block('simulink/Signal Routing/UDP Receive', 'UDP_Receiver');
    
 set_param('UDP_Receiver', 'LocalPort', '5000'); % 设置本地端口号
    
 set_param('UDP_Receiver', 'OutputBufferSize', '1024'); % 设置输出缓冲区大小
    
  
    
 % 连接通信模块
    
 connect_blocks('Drone_State', 'UDP_Sender');
    
 connect_blocks('UDP_Receiver', 'Drone_State');

 function [u_x, u_y, u_z] = apfController(goal_position, drone_positions, obstacles, k_att, k_rep, d_0)

    
     % 计算引力场
    
     F_att = k_att * (goal_position - drone_positions);
    
  
    
     % 计算斥力场
    
     F_rep = zeros(size(drone_positions));
    
     for i = 1:length(obstacles)
    
     d_i = norm(drone_positions - obstacles(i, :));
    
     if d_i < d_0
    
         F_rep = F_rep + k_rep / (d_i - d_0)^2 * (drone_positions - obstacles(i, :));
    
     end
    
     end
    
  
    
     % 计算总力
    
     F_total = F_att + F_rep;
    
  
    
     % 输出控制信号
    
     u_x = F_total(:, 1);
    
     u_y = F_total(:, 2);
    
     u_z = F_total(:, 3);
    
 end

 % 添加路径规划模块

    
 add_block('simulink/User-Defined Functions/MATLAB Function', 'Path_Planning');
    
 set_param('Path_Planning', 'FunctionName', 'apfController');
    
  
    
 % 连接路径规划模块
    
 connect_blocks('Goal_Position', 'Path_Planning');
    
 connect_blocks('Drone_Positions', 'Path_Planning');
    
 connect_blocks('Obstacle_List', 'Path_Planning');

 % 添加设定值显示模块

    
 add_block('simulink/Sources/Signal Builder', 'Desired_Trajectory');
    
 set_param('Desired_Trajectory', 'TimeValues', [0, 10], 'SignalValues', [0, 100]); % 设定位置随时间变化
    
  
    
 % 添加实际值显示模块
    
 add_block('simulink/Sinks/Scope', 'Position_Display');
    
 connect_blocks('GPS_Sensor', 'Position_Display');
    
  
    
 add_block('simulink/Sinks/Scope', 'Attitude_Display');
    
 connect_blocks('IMU_Sensor', 'Attitude_Display');
    
  
    
 % 添加误差显示模块
    
 add_block('simulink/Math Operations/Subtract', 'Error_Calculator');
    
 connect_blocks('Desired_Trajectory', 'Error_Calculator');
    
 connect_blocks('GPS_Sensor', 'Error_Calculator');
    
  
    
 % 添加控制信号显示模块
    
 add_block('simulink/Sinks/Scope', 'Control_Signal_Display');
    
 connect_blocks('Control_Signal_Allocation', 'Control_Signal_Display');
    
  
    
 % 启动仿真
    
 sim('Quadcopter_Formation_Flight_System');
    
  
    
 % 获取仿真结果
    
 position_data = get_param('Position_Display', 'SimulationData');
    
 attitude_data = get_param('Attitude_Display', 'SimulationData');
    
 error_data = get_param('Error_Calculator', 'SimulationData');
    
 control_signal_data = get_param('Control_Signal_Display', 'SimulationData');
    
  
    
 % 绘制结果图表
    
 figure;
    
 subplot(3,1,1);
    
 plot(position_data.Time, position_data.Data, 'b');
    
 hold on;
    
 plot([0, 10], [0, 100], 'r--');
    
 title('Position Response');
    
 xlabel('Time (s)');
    
 ylabel('Position (m)');
    
 legend('Estimated Position', 'Setpoint');
    
  
    
 subplot(3,1,2);
    
 plot(attitude_data.Time, attitude_data.Data, 'g');
    
 title('Attitude Response');
    
 xlabel('Time (s)');
    
 ylabel('Attitude Angle (°)');
    
  
    
 subplot(3,1,3);
    
 plot(error_data.Time, error_data.Data, 'm');
    
 title('Error');
    
 xlabel('Time (s)');
    
 ylabel('Error (m)');

 % 添加四旋翼无人机动力学模型

    
 add_block('aero/Aerospace Blocks/Flight Control/Unmanned Aerial Vehicle Dynamics Model/Quadcopter Dynamics Model', 'Drone_Dynamics_Model');
    
  
    
 % 设置无人机参数
    
 set_param('Drone_Dynamics_Model', 'Mass', '1.5'); % 质量 (kg)
    
 set_param('Drone_Dynamics_Model', 'Inertia', '[0.0820, 0.0845, 0.1377]'); % 转动惯量 (kg·m²)
    
 set_param('Drone_Dynamics_Model', 'ArmLength', '0.225'); % 机臂长度 (m)
    
 set_param('Drone_Dynamics_Model', 'ThrustCoefficient', '8.54858e-06'); % 升力系数
    
 set_param('Drone_Dynamics_Model', 'TorqueCoefficient', '1.6e-07'); % 力矩系数

 % 添加分布式模型预测控制模块

    
 add_block('simulink/User-Defined Functions/MATLAB Function', 'Distributed_MPC');
    
 set_param('Distributed_MPC', 'FunctionName', 'distributedMPC');
    
  
    
 % 连接分布式模型预测控制模块
    
 connect_blocks('Drone_Positions', 'Distributed_MPC');
    
 connect_blocks('Drone_Velocities', 'Distributed_MPC');
    
 connect_blocks('Neighbor_List', 'Distributed_MPC');

 % 添加通信延迟模块

    
 add_block('simulink/Discrete/Unit Delay', 'Communication_Delay');
    
 set_param('Communication_Delay', 'InitialCondition', '0'); % 设置初始条件
    
 set_param('Communication_Delay', 'SampleTime', '0.1'); % 设置采样时间
    
  
    
 % 连接通信延迟模块
    
 connect_blocks('UDP_Sender', 'Communication_Delay');
    
 connect_blocks('Communication_Delay', 'UDP_Receiver');

 % 启用数据记录

    
 set_param('Quadcopter_Formation_Flight_System', 'DataRecordedToWorkspace', 'on');
    
 set_param('Quadcopter_Formation_Flight_System', 'SaveFormat', 'Dataset');
    
  
    
 % 启动仿真
    
 sim('Quadcopter_Formation_Flight_System');
    
  
    
 % 使用Simulink Data Inspector查看结果
    
 open('Quadcopter_Formation_Flight_System_DataInspector');