1. 四旋翼无人机建模基础
四旋翼无人机作为一种典型的欠驱动系统，其动力学建模需要考虑刚体运动学和旋翼空气动力学特性。在MATLAB中建立精确的仿真模型，需要从以下几个核心方面入手：
1.1 坐标系定义与转换
无人机运动分析涉及两个主要坐标系：机体坐标系(Body Frame)和地面坐标系(Earth Frame)。机体坐标系原点位于无人机质心，X轴指向机头方向，Y轴指向右侧机臂，Z轴根据右手定则向下。地面坐标系通常采用北东地(NED)坐标系。
坐标系转换通过旋转矩阵实现：
matlab复制% 旋转矩阵计算函数
function R = rotationMatrix(phi, theta, psi)
R_roll = [1 0 0; 0 cos(phi) sin(phi); 0 -sin(phi) cos(phi)];
R_pitch = [cos(theta) 0 -sin(theta); 0 1 0; sin(theta) 0 cos(theta)];
R_yaw = [cos(psi) sin(psi) 0; -sin(psi) cos(psi) 0; 0 0 1];
R = R_yaw * R_pitch * R_roll; % Z-Y-X旋转顺序
end
1.2 动力学方程建立
四旋翼无人机的非线性动力学模型包含平移运动和旋转运动两部分：
平移动力学：
$$
m\ddot{\mathbf{p}} = m\mathbf{g} + \mathbf{R}\mathbf{F}_b
$$
其中$\mathbf{p}=[x,y,z]^T$为位置向量，$\mathbf{g}=[0,0,g]^T$为重力加速度，$\mathbf{F}_b$为机体坐标系下的总升力。
旋转动力学：
$$
\mathbf{I}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega}\times\mathbf{I}\boldsymbol{\omega} = \boldsymbol{\tau}
$$
其中$\boldsymbol{\omega}=[p,q,r]^T$为角速度，$\boldsymbol{\tau}$为总力矩。
在MATLAB中实现这些方程时，需要考虑惯性矩阵$\mathbf{I}$的对称性：
matlab复制I = [Ixx 0 -Ixz; 0 Iyy 0; -Ixz 0 Izz]; % 惯性矩阵
2. 旋翼动力学建模
2.1 旋翼升力与力矩计算
每个旋翼产生的升力$F_i$与转速平方成正比：
$$
F_i = k_f \omega_i^2
$$
其中$k_f$为升力系数，$\omega_i$为第i个旋翼的转速。
反扭矩$Q_i$同样与转速平方相关：
$$
Q_i = k_m \omega_i^2
$$
$k_m$为扭矩系数，与电机特性相关。
MATLAB实现示例：
matlab复制kf = 1.293e-5; % 升力系数(N/(rad/s)^2)
km = 1.833e-6; % 扭矩系数(Nm/(rad/s)^2)
% 计算四个旋翼的升力
F1 = kf * w1^2;
F2 = kf * w2^2;
F3 = kf * w3^2;
F4 = kf * w4^2;
% 计算总升力
F_total = F1 + F2 + F3 + F4;
2.2 旋翼布局与力矩分配
典型的X型布局中，旋翼1和3顺时针旋转，旋翼2和4逆时针旋转。机体受到的力矩可表示为：
$$
\begin{aligned}
\tau_\phi &= l(F_2 - F_4) \
\tau_\theta &= l(F_3 - F_1) \
\tau_\psi &= Q_1 - Q_2 + Q_3 - Q_4
\end{aligned}
$$
其中$l$为旋翼到质心的距离。
MATLAB实现：
matlab复制l = 0.25; % 机臂长度(m)
tau_phi = l * (F2 - F4);
tau_theta = l * (F3 - F1);
tau_psi = km * (w1^2 - w2^2 + w3^2 - w4^2);
3. 控制系统设计
3.1 PID控制器实现
四旋翼无人机通常采用级联PID控制结构，分为外环位置控制和内环姿态控制。
姿态控制器示例：
matlab复制% PID参数
Kp_phi = 1.2; Ki_phi = 0.1; Kd_phi = 0.3;
Kp_theta = 1.2; Ki_theta = 0.1; Kd_theta = 0.3;
Kp_psi = 1.0; Ki_psi = 0.05; Kd_psi = 0.2;
% 误差计算
e_phi = phi_des - phi;
e_theta = theta_des - theta;
e_psi = psi_des - psi;
% PID输出
u_phi = Kp_phi*e_phi + Ki_phi*integral(e_phi) + Kd_phi*derivative(e_phi);
u_theta = Kp_theta*e_theta + Ki_theta*integral(e_theta) + Kd_theta*derivative(e_theta);
u_psi = Kp_psi*e_psi + Ki_psi*integral(e_psi) + Kd_psi*derivative(e_psi);
3.2 控制分配
将控制量分配到四个旋翼：
matlab复制% 控制分配矩阵
M = [1 1 1 1;
0 -1 0 1;
-1 0 1 0;
1 -1 1 -1];
% 控制输入向量
U = [F_total; u_phi; u_theta; u_psi];
% 求解旋翼转速平方
w_sq = pinv(M) * U;
% 确保非负
w_sq = max(w_sq, 0);
% 计算实际转速
w1 = sqrt(w_sq(1));
w2 = sqrt(w_sq(2));
w3 = sqrt(w_sq(3));
w4 = sqrt(w_sq(4));
4. MATLAB仿真实现
4.1 Simulink模型搭建
建议采用分层建模方法：
- 顶层：包含姿态控制器、位置控制器和无人机模型
- 中间层：实现具体的控制算法和动力学方程
- 底层：电机和旋翼模型
关键模块配置：
- 使用ODE4(Runge-Kutta)求解器
- 固定步长设置为0.01s
- 启用零交叉检测
4.2 可视化实现
利用MATLAB 3D动画工具箱实现飞行轨迹可视化：
matlab复制function plot_quadrotor(pos, angles)
% 位置和姿态
x = pos(1); y = pos(2); z = pos(3);
phi = angles(1); theta = angles(2); psi = angles(3);
% 机体尺寸参数
arm_length = 0.25;
body_width = 0.1;
% 绘制机臂
R = rotationMatrix(phi, theta, psi);
arm1 = R * [arm_length; 0; 0];
arm2 = R * [0; arm_length; 0];
arm3 = R * [-arm_length; 0; 0];
arm4 = R * [0; -arm_length; 0];
% 绘制旋翼
plot3([x, x+arm1(1)], [y, y+arm1(2)], [z, z+arm1(3)], 'b-', 'LineWidth', 2);
hold on;
plot3([x, x+arm2(1)], [y, y+arm2(2)], [z, z+arm2(3)], 'b-', 'LineWidth', 2);
plot3([x, x+arm3(1)], [y, y+arm3(2)], [z, z+arm3(3)], 'b-', 'LineWidth', 2);
plot3([x, x+arm4(1)], [y, y+arm4(2)], [z, z+arm4(3)], 'b-', 'LineWidth', 2);
% 绘制机体中心
plot3(x, y, z, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
axis equal; grid on;
xlabel('X'); ylabel('Y'); zlabel('Z');
view(30, 30);
end
5. 仿真调试与优化
5.1 参数调优方法
- 先调内环后调外环：先确保姿态控制稳定，再调试位置控制
- 先比例后积分微分：先调整P参数使系统响应快速但不振荡，再加入D抑制超调，最后加入I消除稳态误差
- 频域分析：使用bode图分析系统稳定裕度
5.2 常见问题排查
-
发散问题：
- 检查动力学方程符号是否正确
- 验证惯性矩阵是否正定
- 减小仿真步长
-
振荡问题：
- 适当增加微分增益
- 检查传感器噪声滤波
- 验证控制分配矩阵是否可逆
-
稳态误差：
- 增加积分增益
- 检查是否有未建模的摩擦力
- 验证执行器是否饱和
6. 高级功能扩展
6.1 独立旋翼倾斜控制
为实现更灵活的机动，可以扩展模型使每个旋翼能够独立倾斜：
matlab复制% 旋翼倾斜角度
alpha1 = 0; % 前旋翼
alpha2 = 0; % 右旋翼
alpha3 = 0; % 后旋翼
alpha4 = 0; % 左旋翼
% 倾斜后的升力方向
F1_dir = R * rotY(alpha1) * [0; 0; -F1];
F2_dir = R * rotY(alpha2) * [0; 0; -F2];
F3_dir = R * rotY(alpha3) * [0; 0; -F3];
F4_dir = R * rotY(alpha4) * [0; 0; -F4];
function Ry = rotY(a)
Ry = [cos(a) 0 sin(a); 0 1 0; -sin(a) 0 cos(a)];
end
6.2 风扰模拟
添加风扰模型提高仿真真实性：
matlab复制% 风场模型
wind_velocity = [2; 1; 0]; % 恒定风速[m/s]
wind_gust = 0.5 * randn(3,1); % 随机阵风
% 计算气动力
V_air = V - (wind_velocity + wind_gust);
F_drag = 0.5 * rho * norm(V_air) * Cd * S * V_air;
在实际项目中，我发现旋翼动力学参数的准确性对仿真结果影响很大。通过实验数据辨识得到的kf和km系数，比理论计算值更能反映真实系统特性。建议在初期就进行参数辨识实验，使用最小二乘法拟合转速-升力曲线。