MATLAB来计算和仿真无人机飞行过程

使用MATLAB来计算和仿真无人机飞行过程中的运动参数是一个极其常见且强大的方法。这通常被称为无人机建模与仿真，是无人机飞控算法开发中不可或缺的一环。

一、核心原理：无人机动力学与运动学模型

要计算运动参数，首先需要建立描述无人机运动的数学模型。对于多旋翼无人机（如四旋翼），模型通常分为两部分：

1. 动力学模型：描述力和力矩如何引起无人机的运动和旋转（与力相关）。
2. 运动学模型：描述无人机的状态（位置、速度、姿态）如何随时间变化（与几何相关）。

1. 坐标系定义

• 机体坐标系：固连在无人机上。原点在重心，X轴向前，Y轴向左，Z轴向上。
• 世界坐标系：固定的参考系，例如北东地。

2. 状态变量

我们需要一个状态向量 X 来描述无人机在某一时刻的所有信息：
X = [x, y, z, u, v, w, φ, θ, ψ, p, q, r]^T

• 位置：x, y, z (在世界坐标系下)
• 速度：u, v, w (在机体坐标系下的线速度)
• 姿态：φ, θ, ψ (横滚、俯仰、偏航角)
• 角速度：p, q, r (在机体坐标系下的角速度)

3. 模型方程（以四旋翼为例）

a) 位置动力学（牛顿第二定律）
机体坐标系下的线速度变化：
m * [du/dt; dv/dt; dw/dt] = m * [rv - qw; pw - ru; qu - pv] + R^T * [0; 0; -mg] + [0; 0; T]

• m：质量
• g：重力加速度
• T：总推力
• R：从世界系到机体的旋转矩阵

b) 姿态动力学（欧拉方程）
机体坐标系下的角速度变化：
I * [dp/dt; dq/dt; dr/dt] = -[p; q; r] × (I * [p; q; r]) + [τ_φ; τ_θ; τ_ψ]

• I：惯性张量
• τ_φ, τ_θ, τ_ψ：作用在机体上的控制力矩（横滚、俯仰、偏航力矩）

c) 运动学

• 位置变化率：[dx/dt; dy/dt; dz/dt] = R * [u; v; w]
• 姿态变化率：[dφ/dt; dθ/dt; dψ/dt] = E * [p; q; r]
  • 其中 E 是欧拉角速率到机体角速率的转换矩阵。

二、实现

在MATLAB中，我们通常通过数值积分（如ode45）来求解上述微分方程组，从而得到状态随时间的变化。

步骤 1：定义模型参数和初始状态

% 无人机物理参数
params.m = 1.0; % 质量 (kg)
params.g = 9.81; % 重力加速度 (m/s^2)
params.Ixx = 0.01; % X轴转动惯量 (kg*m^2)
params.Iyy = 0.01; % Y轴转动惯量
params.Izz = 0.02; % Z轴转动惯量
params.I = diag([params.Ixx, params.Iyy, params.Izz]); % 惯性张量矩阵

% 初始状态向量 X0 = [x, y, z, u, v, w, φ, θ, ψ, p, q, r]
X0 = [0;  % x
      0;  % y
      0;  % z
      0;  % u
      0;  % v
      0;  % w
      0;  % φ (roll)
      0;  % θ (pitch)
      0;  % ψ (yaw)
      0;  % p
      0;  % q
      0]; % r

% 仿真时间
tspan = [0 10]; % 仿真10秒

步骤 2：定义控制输入（推力与力矩）

控制输入是飞控算法的输出。为了仿真，我们可以简单假设一个输入。

% 假设的控制输入 [总推力 T, 横滚力矩 τ_φ, 俯仰力矩 τ_θ, 偏航力矩 τ_ψ]
% 例如：让无人机以一定推力上升，并施加一个小的横滚力矩
T = 12; % 总推力 (N) (稍大于mg=9.81N以实现上升)
tau_phi = 0.1; % 横滚力矩 (N*m)
tau_theta = 0.0; % 俯仰力矩
tau_psi = 0.0; % 偏航力矩
U = [T, tau_phi, tau_theta, tau_psi]; % 控制输入向量

步骤 3：编写微分方程函数

这是最核心的一步，实现上面提到的模型方程。

function dXdt = droneDynamics(t, X, U, params)
    % 解包状态向量
    x = X(1); y = X(2); z = X(3);
    u = X(4); v = X(5); w = X(6);
    phi = X(7); theta = X(8); psi = X(9);
    p = X(10); q = X(11); r = X(12);

    % 解包控制输入
    T = U(1);
    tau_phi = U(2);
    tau_theta = U(3);
    tau_psi = U(4);

    % 1. 计算旋转矩阵 R (从世界系到机体系)
    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; % 通常旋转顺序为Z-Y-X (偏航-俯仰-横滚)

    % 2. 计算欧拉角变化率矩阵 E
    E = [1, sin(phi)*tan(theta), cos(phi)*tan(theta);
         0, cos(phi),           -sin(phi);
         0, sin(phi)/cos(theta), cos(phi)/cos(theta)];

    % 3. 计算线加速度 (在机体系下) - 位置动力学
    gravity_force_world = [0; 0; params.m * params.g];
    gravity_force_body = R * gravity_force_world; % 将重力转换到机体系
    thrust_force_body = [0; 0; -T]; % 推力总是在机体系Z轴负方向

    % 科里奥利项 (由于旋转)
    coriolis_force = params.m * cross([p; q; r], [u; v; w]);

    % 机体系下的线加速度 du/dt, dv/dt, dw/dt
    acc_body = (-gravity_force_body + thrust_force_body - coriolis_force) / params.m;

    % 4. 计算角加速度 (在机体系下) - 姿态动力学
    omega = [p; q; r];
    torques = [tau_phi; tau_theta; tau_psi];
    % 角加速度 dp/dt, dq/dt, dr/dt
    ang_acc_body = params.I \ (torques - cross(omega, params.I * omega));

    % 5. 计算世界系下的线速度变化率 dx/dt, dy/dt, dz/dt
    vel_body = [u; v; w];
    vel_world = R' * vel_body; % 将机体速度转换到世界系

    % 6. 计算欧拉角变化率 dφ/dt, dθ/dt, dψ/dt
    euler_rates = E * omega;

    % 7. 组装状态导数向量 dX/dt
    dXdt = zeros(12, 1);
    dXdt(1:3) = vel_world; % dx/dt, dy/dt, dz/dt
    dXdt(4:6) = acc_body; % du/dt, dv/dt, dw/dt
    dXdt(7:9) = euler_rates; % dφ/dt, dθ/dt, dψ/dt
    dXdt(10:12) = ang_acc_body; % dp/dt, dq/dt, dr/dt
end

步骤 4：使用 ODE 求解器进行仿真

% 使用 ode45 进行数值积分
[t, X_history] = ode45(@(t, X) droneDynamics(t, X, U, params), tspan, X0);

% X_history 的每一行是时间 t(i) 对应的状态向量
% 提取结果以便绘图
x = X_history(:, 1);
y = X_history(:, 2);
z = X_history(:, 3);
phi = rad2deg(X_history(:, 7)); % 转换为角度制
theta = rad2deg(X_history(:, 8));
psi = rad2deg(X_history(:, 9));

步骤 5：可视化与分析结果

% 绘制位置变化
figure(1);
subplot(3,1,1);
plot(t, x, 'r'); ylabel('x (m)'); title('Position');
subplot(3,1,2);
plot(t, y, 'g'); ylabel('y (m)');
subplot(3,1,3);
plot(t, z, 'b'); ylabel('z (m)'); xlabel('Time (s)');

% 绘制姿态变化
figure(2);
subplot(3,1,1);
plot(t, phi, 'r'); ylabel('Roll φ (deg)'); title('Attitude (Euler Angles)');
subplot(3,1,2);
plot(t, theta, 'g'); ylabel('Pitch θ (deg)');
subplot(3,1,3);
plot(t, psi, 'b'); ylabel('Yaw ψ (deg)'); xlabel('Time (s)');

% 绘制3D轨迹
figure(3);
plot3(x, y, z, 'b-', 'LineWidth', 1.5);
hold on; grid on;
plot3(x(1), y(1), z(1), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); % 起点
plot3(x(end), y(end), z(end), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); % 终点
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('3D Flight Trajectory');
axis equal;

参考代码 无人机飞行控制程序 www.youwenfan.com/contentald/53537.html

三、进阶：与飞控算法结合

上面的例子是开环仿真（给定固定的控制输入）。一个完整的飞控仿真还包括闭环控制：

1. 控制器：根据期望状态（如目标高度、目标位置）和当前状态，计算出所需的控制输入 U。
   • 例如，一个简单的PID高度控制器：T = params.m * params.g + Kp * (z_desired - z_current)
2. 在 droneDynamics 函数的每一步，都调用这个控制器来计算当前时刻的 U。
3. 这通常需要在Simulink中搭建模型，或者使用更复杂的ODE求解方法来实现反馈循环。

总结

通过MATLAB求解无人机运动参数的过程是：

1. 建立数学模型：定义状态变量和动力学/运动学微分方程。
2. 实现方程：在MATLAB中编写一个函数来计算状态的导数 dXdt。
3. 数值积分：使用 ode45 等求解器对微分方程进行积分，得到状态随时间变化的历史数据 X_history。
4. 可视化分析：从 X_history 中提取位置、姿态等参数进行绘图和分析。

这个方法非常适合算法验证、控制器设计、性能评估和教育研究。您可以通过修改模型参数、初始条件和控制输入，来模拟各种不同的飞行场景和故障情况。对于更复杂的仿真（如加入风扰、传感器模型），可以考虑使用 Simulink 或 Aerospace Toolbox。