无人机飞行控制程序设计与运动参数求解（MATLAB实现）

%% 参数定义
m = 1.5;        % 质量 (kg)
g = 9.81;       % 重力加速度 (m/s²)
I = diag([0.082, 0.084, 0.138]);  % 惯性矩阵 (kg·m²)
dt = 0.01;      % 时间步长 (s)
​
%% 状态变量初始化
state = [0; 0; 2;   % 位置 (x,y,z)
         0; 0; 0;   % 速度 (vx,vy,vz)
         0; 0; 0;   % 欧拉角 (phi,theta,psi)
         0; 0; 0];  % 角速度 (p,q,r)
​
%% 控制输入（PID控制器）
Kp_att = [5,5,5];  % 姿态PID比例系数
Ki_att = [0.1,0.1,0.1];
Kd_att = [0.5,0.5,0.5];
​
%% 仿真循环
t = 0:dt:10;
states = zeros(length(t), 12);
states(1,:) = state;
​
for i = 1:length(t)-1
    % 计算控制输入（示例：悬停控制）
    thrust = m*g;  % 垂直推力平衡重力
    tau = [0; 0; thrust*l - 0.1*state(9)];  % 力矩计算（l为力臂）
    
    % 数值积分（欧拉法）
    dstate = dynamics(state, tau, I, m, g);
    state = state + dstate*dt;
    states(i+1,:) = state;
end
​
%% 可视化
plot3(states(:,1), states(:,2), states(:,3));
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('无人机飞行轨迹');
​
%% 动力学方程函数
function dstate = dynamics(state, tau, I, m, g)
    % 解包状态变量
    x = state(1:3);
    v = state(4:6);
    phi = state(7); theta = state(8); psi = state(9);
    p = state(10); q = state(11); r = state(12);
    
    % 平动方程
    F_gravity = [0; 0; -m*g];
    F_total = [0; 0; thrust] + F_gravity;
    dvdt = F_total/m;
    
    % 旋转矩阵（从机体到惯性坐标系）
    R = rotation_matrix(phi, theta, psi);
    
    % 转动方程
    I_inv = inv(I);
    angular_accel = I_inv*(tau - cross(R*p, I*R*p));
    dpdt = angular_accel(1);
    dqdt = angular_accel(2);
    drdt = angular_accel(3);
    
    % 运动学更新
    dphidt = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta);
    dthetadt = q*cos(phi) - r*sin(phi);
    dpsidt = (q*sin(phi) + r*cos(phi))/cos(theta);
    
    % 组合导数
    dstate = [dvdt; angular_accel; dphidt; dthetadt; dpsidt];
end
​
%% 旋转矩阵生成函数
function R = rotation_matrix(phi, theta, psi)
    % Z-Y-X欧拉角旋转矩阵
    Rx = [1 0 0; 0 cos(phi) -sin(phi); 0 sin(phi) cos(phi)];
    Ry = [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)];
    Rz = [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1];
    R = Rz * Ry * Rx;
end

% 姿态控制（以俯仰角为例）
error = theta_des - theta;
integral = integral + error*dt;
derivative = (error - prev_error)/dt;
tau_pitch = Kp_att(2)*error + Ki_att(2)*integral + Kd_att(2)*derivative;

% 状态空间模型
A = [0 0 0 1 0 0 0 0 0;
     0 0 0 0 1 0 0 0 0;
     0 0 0 0 0 1 0 0 0;
     0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0];
B = [0;0;0;0;0;0;0;0;1/m];
Q = diag([100,100,100,1,1,1,0,0,0]);
R = 0.1;
[K,S,e] = lqr(A,B,Q,R);

% 恒定风场
wind = [2, 0, 0];  % 东向风速2m/s
v_true = v + wind;

% 添加高斯噪声
noise_level = 0.1;
accel_noise = noise_level * randn(size(accel));

% 领航-跟随策略
for i = 2:num_drones
    relative_pos = drones(i).pos - drones(1).pos;
    desired_heading = atan2(relative_pos(2), relative_pos(1));
    tau_yaw = Kp_yaw * (desired_heading - drones(i).psi);
end