1. 四旋翼无人机建模基础
四旋翼无人机作为一种典型的欠驱动系统,其建模过程需要综合考虑空气动力学、刚体力学和控制理论等多个学科知识。在MATLAB环境下建立准确的飞行仿真模型,是验证控制算法有效性的关键前提。
1.1 坐标系定义与转换
在建立动力学模型前,首先需要明确三个基本坐标系:
- 地面惯性坐标系(OXYZ):固定于地面,Z轴垂直地面向上,作为所有运动描述的参考基准
- 机体坐标系(oxyz):固连在无人机上,原点位于重心,x轴指向机头方向
- 旋翼坐标系:每个旋翼的局部坐标系,用于描述单个旋翼产生的力和力矩
坐标系间的转换通过旋转矩阵实现。以常见的Z-Y-X欧拉角旋转顺序为例:
matlab复制function R = rotationMatrix(phi, theta, psi)
% 欧拉角转旋转矩阵
R = [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi), ...
cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi);
sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi), ...
sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi);
-sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi)];
end
1.2 刚体动力学方程
基于牛顿-欧拉方程,建立无人机六自由度运动方程:
平移动力学:
m(dV/dt + ω×V) = F_ext
其中m为质量,V为速度向量,ω为角速度,F_ext为外力总和
旋转动力学:
I(dω/dt) + ω×(Iω) = M_ext
I为惯性张量,M_ext为外力矩总和
在MATLAB中实现时,通常将方程改写为状态空间形式:
matlab复制function dx = dynamics(t, x, u, params)
% 状态变量: x = [位置; 姿态角; 线速度; 角速度]
% 控制输入: u = [四个旋翼的转速平方]
% 解包参数
m = params.mass;
I = params.inertia;
g = params.gravity;
kf = params.thrustCoeff;
km = params.torqueCoeff;
L = params.armLength;
% 解包状态
pos = x(1:3);
euler = x(4:6);
vel = x(7:9);
omega = x(10:12);
% 旋转矩阵
R = rotationMatrix(euler(1), euler(2), euler(3));
% 旋翼总推力
T = kf * sum(u);
% 外力计算
F_ext = R * [0; 0; T] - [0; 0; m*g];
% 力矩计算
tau = [
L*kf*(u(1)-u(3))
L*kf*(u(2)-u(4))
km*(u(1)-u(2)+u(3)-u(4))
];
% 状态导数
dpos = vel;
deuler = eulerRates(euler, omega);
dvel = F_ext / m;
domega = I \ (tau - cross(omega, I*omega));
dx = [dpos; deuler; dvel; domega];
end
注意:惯性矩阵I的准确性对仿真结果影响很大,需要通过实际测量或CAD软件计算获得。对于对称布局的四旋翼,通常Ixy = Ixz = Iyz = 0。
2. 旋翼空气动力学建模
2.1 旋翼推力模型
每个旋翼产生的推力与转速平方成正比:
F_i = k_f * ω_i^2
其中k_f为推力系数,ω_i为第i个旋翼转速
推力系数可通过实验或理论计算获得:
matlab复制% 实验数据拟合推力系数示例
rpm = [1000 2000 3000 4000]; % 转速(RPM)
thrust = [0.5 2.1 4.7 8.3]; % 对应推力(N)
p = polyfit(rpm.^2, thrust, 1);
kf = p(1); % 获得推力系数
2.2 旋翼扭矩模型
旋翼旋转时会产生反扭矩,与推力类似:
M_i = k_m * ω_i^2
k_m为扭矩系数,通常与k_f存在关系:k_m = C_Q / C_T * R * k_f
其中C_Q、C_T为扭矩和推力系数,R为旋翼半径
2.3 地面效应与涡环状态
在实际飞行中需要考虑两种特殊状态:
- 地面效应(IGE):当无人机离地高度小于旋翼直径时,推力会增加约10-15%
- 涡环状态(VRS):在快速下降时可能出现的不稳定状态,会导致升力突然减小
在仿真中可以添加修正因子:
matlab复制% 地面效应修正
function thrust = groundEffectCorrection(thrust, height, diameter)
if height < diameter
thrust = thrust * (1 + 0.15*(1 - height/diameter));
end
end
3. 控制系统设计与实现
3.1 分层控制架构
典型的四旋翼控制系统采用分层结构:
- 外环位置控制:生成期望的姿态角
- 内环姿态控制:控制无人机达到期望姿态
- 转速分配:将控制量分配到四个电机
matlab复制function u = controller(t, x, xd, params)
% 解包期望状态
pos_d = xd(1:3);
vel_d = xd(4:6);
yaw_d = xd(7);
% 当前位置和速度
pos = x(1:3);
vel = x(7:9);
euler = x(4:6);
% 位置控制器 (PID)
acc_d = params.kp_pos*(pos_d - pos) + params.kd_pos*(vel_d - vel);
% 计算期望推力
R = rotationMatrix(euler(1), euler(2), euler(3));
thrust_d = (params.mass*acc_d(3) + params.mass*params.gravity) / R(3,3);
% 计算期望姿态
phi_d = (acc_d(1)*sin(yaw_d) - acc_d(2)*cos(yaw_d)) / params.gravity;
theta_d = (acc_d(1)*cos(yaw_d) + acc_d(2)*sin(yaw_d)) / params.gravity;
% 姿态控制器
euler_d = [phi_d; theta_d; yaw_d];
tau = params.kp_att*(euler_d - euler) - params.kd_att*x(10:12);
% 控制分配
u = [thrust_d + tau(1)/params.L + tau(3)/params.km;
thrust_d + tau(2)/params.L - tau(3)/params.km;
thrust_d - tau(1)/params.L + tau(3)/params.km;
thrust_d - tau(2)/params.L - tau(3)/params.km];
% 转换为转速平方
u = max(0, u / params.kf);
end
3.2 PID参数整定方法
-
Ziegler-Nichols方法:
- 先设置Ki=Kd=0,增大Kp直到系统开始振荡
- 记录临界增益Ku和振荡周期Tu
- 根据规则设置PID参数
-
试凑法:
- 先调节P使系统快速响应但不过冲
- 加入D抑制超调
- 最后加入I消除稳态误差
matlab复制% 自动调参示例
opt = pidtuneOptions('PhaseMargin',70);
C = pidtune(sys,'PID',opt);
4. MATLAB仿真实现
4.1 仿真环境搭建
使用ODE45求解微分方程:
matlab复制% 仿真参数
params.mass = 1.2; % kg
params.inertia = diag([0.02, 0.02, 0.04]); % kg*m^2
params.gravity = 9.81; % m/s^2
params.armLength = 0.25; % m
params.kf = 8.5e-6; % 推力系数
params.km = 1.6e-2; % 扭矩系数
% 初始状态
x0 = [0; 0; 0; % 位置
0; 0; 0; % 姿态
0; 0; 0; % 速度
0; 0; 0]; % 角速度
% 期望状态
xd = [1; 1; 1; % 位置
0; 0; 0; % 速度
0]; % 偏航角
% 仿真
tspan = [0 10];
[t, x] = ode45(@(t,x) dynamics(t, x, controller(t,x,xd,params), params), tspan, x0);
4.2 可视化实现
使用MATLAB三维动画展示飞行轨迹:
matlab复制function animateQuadrotor(t, x)
figure;
h = plot3(x(1,1),x(1,2),x(1,3),'b-',...
x(1,1),x(1,2),x(1,3),'ro');
axis([-2 2 -2 2 0 2]);
grid on;
xlabel('X'); ylabel('Y'); zlabel('Z');
for i = 1:length(t)
set(h(1),'XData',x(1:i,1),'YData',x(1:i,2),'ZData',x(1:i,3));
set(h(2),'XData',x(i,1),'YData',x(i,2),'ZData',x(i,3));
drawnow;
% 控制帧率
if i < length(t)
pause(t(i+1)-t(i));
end
end
end
5. 进阶主题与问题排查
5.1 常见仿真问题
-
数值发散:
- 检查时间步长,尝试减小ODE45的RelTol/AbsTol
- 验证惯性矩阵是否正定
- 检查控制器输出是否合理
-
奇异姿态:
- 当俯仰角接近±90°时会出现万向节锁
- 可改用四元数表示姿态
matlab复制% 四元数动力学示例
function dq = quaternionDerivative(q, omega)
Omega = [0, -omega(1), -omega(2), -omega(3);
omega(1), 0, omega(3), -omega(2);
omega(2), -omega(3), 0, omega(1);
omega(3), omega(2), -omega(1), 0];
dq = 0.5 * Omega * q;
end
5.2 硬件在环测试
当需要连接实际飞控时,可采用以下流程:
- 建立Simulink模型生成代码
- 使用MATLAB Coder生成嵌入式代码
- 通过UART或USB与飞控通信
- 实时记录和可视化数据
matlab复制% 串口通信示例
s = serialport('COM3',115200);
configureTerminator(s,"CR/LF");
flush(s);
while true
data = readline(s);
% 解析并处理数据
end
我在实际项目中发现,仿真与实机测试的差异主要来自三个方面:电机响应延迟、传感器噪声和机架柔性。建议在仿真中加入这些因素的模型,如给电机转速增加一阶滞后环节:
matlab复制function omega = motorDynamics(u, omega_prev, dt, tau)
% u: 输入指令
% omega_prev: 上一时刻转速
% dt: 时间步长
% tau: 电机时间常数
omega = omega_prev + (u - omega_prev) * (1 - exp(-dt/tau));
end
对于需要更高精度的应用,可以考虑使用系统辨识工具获得更准确的动力学模型。MATLAB的System Identification Toolbox提供了便捷的界面和函数:
matlab复制% 系统辨识示例
data = iddata(y, u, Ts);
model = tfest(data, 2); % 估计二阶传递函数
compare(data, model);