四旋翼无人机动力学建模与Matlab仿真实践

1. 四旋翼无人机建模基础

四旋翼无人机的动力学建模是飞行控制算法开发的基础环节。与固定翼无人机不同，四旋翼通过调节四个电机的转速差来实现姿态和位置控制，这种独特的驱动方式带来了特殊的动力学特性。

1.1 坐标系定义

在建立动力学模型前，我们需要明确两个关键坐标系：

1. 惯性坐标系（地面坐标系）：固定在地面上的参考系，通常用OXYZ表示
2. 机体坐标系：固定在无人机上的参考系，通常用oxyz表示

这两个坐标系之间的转换通过欧拉角（滚转φ、俯仰θ、偏航ψ）来描述。在Matlab中，我们常用旋转矩阵来实现坐标系转换：

matlab复制function R = rotationMatrix(phi, theta, psi)
% 计算从机体坐标系到惯性坐标系的旋转矩阵
R = [cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), ...;
     cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), ...;
     -cos(phi)*sin(theta);
     -cos(phi)*sin(psi), ...
     cos(phi)*cos(psi), ...
     sin(phi);
     cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), ...
     sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), ...
     cos(phi)*cos(theta)];
end

1.2 基本假设

为简化模型，我们通常做以下合理假设：

• 无人机是刚体结构
• 质量分布对称
• 忽略地面效应
• 螺旋桨升力与转速平方成正比
• 忽略陀螺效应

这些假设虽然简化了模型，但仍能准确描述无人机的主要动力学特性。在实际应用中，我们可以通过参数辨识和自适应控制来补偿这些简化带来的误差。

2. 非线性刚体动力学模型

2.1 平移运动方程

根据牛顿第二定律，在惯性坐标系下，无人机的平移运动方程为：

code复制m * d²X/dt² = [0; 0; -mg] + R * [0; 0; T]

其中：

• m为无人机质量
• X = [x; y; z]为位置向量
• R为旋转矩阵
• T为总升力

在Matlab中实现平移运动方程：

matlab复制function dX = translationalDynamics(X, R, T, m, g)
% 计算平移运动微分方程
gravity = [0; 0; -m*g];
thrust = R * [0; 0; T];
dX = (gravity + thrust) / m;
end

2.2 旋转运动方程

旋转运动由欧拉方程描述：

code复制I * dω/dt + ω × (I * ω) = τ

其中：

• I为惯性矩阵
• ω为角速度向量
• τ为总力矩

惯性矩阵I通常为对角矩阵：

code复制I = diag([Ixx, Iyy, Izz])

在Matlab中实现旋转运动方程：

matlab复制function dw = rotationalDynamics(w, I, tau)
% 计算旋转运动微分方程
dw = inv(I) * (tau - cross(w, I*w));
end

2.3 电机模型

每个电机产生的升力与转速平方成正比：

code复制Fi = kf * ωi²

反扭矩与升力成正比：

code复制Mi = km * Fi

在Matlab中实现电机模型：

matlab复制function [F, M] = motorModel(omega, kf, km)
% 计算电机升力和反扭矩
F = kf * omega.^2;
M = km * F;
end

3. 模型线性化与解耦

3.1 小扰动线性化

在悬停状态附近（φ≈0，θ≈0），我们可以对非线性模型进行线性化：

1. 姿态角小角度近似：

   • sin(θ) ≈ θ
   • cos(θ) ≈ 1
   • 高阶项忽略

2. 角速度乘积项忽略

这样可以得到解耦的线性化模型，便于设计控制器。

3.2 传递函数推导

通过拉普拉斯变换，可以得到各通道的传递函数：

1. 高度通道：

   code复制Gz(s) = 1/(s²)
   

2. 俯仰/滚转通道：

   code复制Gθ(s) = 1/(s²)
   

3. 偏航通道：

   code复制Gψ(s) = 1/(s²)
   

在Matlab中获取传递函数：

matlab复制s = tf('s');
Gz = 1/s^2;  % 高度通道
Gtheta = 1/s^2;  % 俯仰通道
Gphi = 1/s^2;  % 滚转通道
Gpsi = 1/s^2;  % 偏航通道

4. 仿真实现与可视化

4.1 Simulink仿真框架

完整的仿真模型应包括：

1. 控制器模块
2. 电机混合器
3. 动力学模型
4. 环境扰动模型
5. 可视化模块

4.2 可视化实现

Matlab提供了强大的3D可视化工具：

matlab复制function visualizeDrone(X, R, time)
% 无人机3D可视化
persistent fig quad;

if isempty(fig) || ~isvalid(fig)
    fig = figure;
    ax = axes('Parent', fig);
    quad = createQuadrotor(ax);
end

updateQuadrotor(quad, X, R);
title(sprintf('Time: %.2f s', time));
drawnow;
end

4.3 典型仿真结果分析

通过仿真我们可以观察到：

1. 阶跃响应中的超调量
2. 抗扰动的能力
3. 各通道间的耦合程度
4. 控制器的稳定裕度

5. 控制算法实现

5.1 PID控制器设计

基于解耦的传递函数，可以设计独立的PID控制器：

matlab复制function u = pidController(error, Kp, Ki, Kd, persistent_vars)
% PID控制器实现
persistent integral last_error;

if isempty(persistent_vars)
    integral = 0;
    last_error = 0;
else
    integral = persistent_vars.integral;
    last_error = persistent_vars.last_error;
end

integral = integral + error;
derivative = error - last_error;

u = Kp*error + Ki*integral + Kd*derivative;

persistent_vars.integral = integral;
persistent_vars.last_error = error;
end

5.2 参数整定技巧

1. 先调比例项，确保快速响应
2. 再调微分项，抑制超调
3. 最后调积分项，消除稳态误差
4. 各通道独立调试后再测试耦合影响

6. 实际应用中的注意事项

1. 采样时间选择：通常选择1-10ms，过大会导致系统不稳定
2. 传感器噪声处理：必须添加低通滤波器
3. 执行器饱和：需要考虑电机转速限制
4. 计算延迟：算法复杂度要适应机载计算机性能

7. 完整代码结构

一个完整的仿真项目通常包含以下文件：

code复制/drone_simulation
    ├── main.m                % 主脚本
    ├── drone_dynamics.m      % 动力学模型
    ├── controller.m          % 控制器
    ├── visualization.m       % 可视化
    ├── utilities.m           % 工具函数
    └── parameters.m          % 系统参数

在main.m中组织仿真流程：

matlab复制% 初始化
params = initializeParameters();
state = initializeState();

% 仿真循环
for t = 0:params.dt:params.Tf
    % 获取参考输入
    ref = getReference(t);
    
    % 计算控制量
    u = controller(state, ref, params);
    
    % 更新状态
    state = updateDynamics(state, u, params);
    
    % 可视化
    if mod(t, 0.1) < params.dt
        visualize(state, t);
    end
end

8. 性能优化技巧

1. 向量化运算：避免循环，使用矩阵运算
2. 预分配内存：对于增长数组预先分配空间
3. 并行计算：使用parfor加速蒙特卡洛仿真
4. Mex文件：对关键算法用C实现

matlab复制% 向量化运算示例
omega = [w1, w2, w3, w4];  % 四个电机转速
F = kf * omega.^2;         % 向量化计算升力

9. 扩展功能实现

9.1 风扰模型

加入风扰可以测试控制器的鲁棒性：

matlab复制function F_wind = windModel(t, params)
% 简化的风扰模型
if t > 5 && t < 10
    F_wind = params.wind_gust * randn(3,1);
else
    F_wind = zeros(3,1);
end
end

9.2 故障注入

模拟电机故障：

matlab复制function omega = injectFault(t, omega, params)
% 电机故障注入
if t > params.fault_time && params.fault_motor > 0
    omega(params.fault_motor) = omega(params.fault_motor) * params.fault_level;
end
end

10. 结果分析与报告生成

Matlab提供了丰富的工具来分析仿真结果：

1. 时域分析：step、impulse等函数
2. 频域分析：bode、nyquist等函数
3. 数据导出：将结果导出为Excel或CSV
4. 自动报告生成：使用publish功能

matlab复制% 生成分析报告
options = struct('format','pdf','outputDir','report');
publish('analysis_script.m', options);

通过系统化的建模、仿真和分析，我们可以深入理解四旋翼无人机的动力学特性，为实际控制系统的设计提供可靠的理论依据和验证手段。这种基于模型的设计方法大大降低了实际飞行测试的风险和成本。