1. 四旋翼飞行器建模基础与MATLAB实现
四旋翼飞行器的动力学建模是整个仿真实验的基石。在机体坐标系下,我们需要同时考虑平移和旋转运动,这就涉及到牛顿-欧拉方程的建立。让我分享一个经过实际验证的建模方法。
1.1 动力学方程推导
四旋翼的动力学模型通常包含以下关键方程:
平移运动方程:
code复制m·d²x/dt² = (sinψ·sinφ + cosψ·sinθ·cosφ)·U₁
m·d²y/dt² = (-cosψ·sinφ + sinψ·sinθ·cosφ)·U₁
m·d²z/dt² = cosθ·cosφ·U₁ - mg
旋转运动方程:
code复制Ixx·d²φ/dt² = θ̇·ψ̇(Iyy - Izz) + l·U₂
Iyy·d²θ/dt² = φ̇·ψ̇(Izz - Ixx) + l·U₃
Izz·d²ψ/dt² = φ̇·θ̇(Ixx - Iyy) + U₄
其中,U₁到U₄是控制输入,与四个电机的转速平方(ω₁²到ω₄²)相关:
code复制U₁ = kF·(ω₁² + ω₂² + ω₃² + ω₄²)
U₂ = kF·(-ω₂² + ω₄²)
U₃ = kF·(ω₁² - ω₃²)
U₄ = kM·(-ω₁² + ω₂² - ω₃² + ω₄²)
1.2 MATLAB参数配置技巧
在MATLAB中,我习惯用结构体存储所有参数,这样代码更清晰:
matlab复制quad.params = struct(...
'mass', 1.2, ... % 质量(kg)
'Ixx', 0.023, ... % X轴转动惯量
'Iyy', 0.023, ... % Y轴转动惯量
'Izz', 0.046, ... % Z轴转动惯量
'arm_length', 0.225, ... % 机臂长度(m)
'kF', 1.5e-5, ... % 升力系数
'kM', 3.75e-7, ... % 扭矩系数
'g', 9.81); % 重力加速度
重要提示:kF和kM的比例必须准确,通常kM/kF ≈ 0.25。我曾因单位混淆导致仿真异常,飞机在空中疯狂旋转,调试了整整一天才发现是系数单位不一致。
1.3 状态空间实现
在MATLAB中实现状态空间模型时,推荐使用ODE45求解器:
matlab复制function dx = quad_dynamics(t, x, u, params)
% 状态变量分解
phi = x(4); theta = x(5); psi = x(6);
p = x(10); q = x(11); r = x(12);
% 旋转矩阵
R = [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)];
% 平移加速度
acc = (R * [0; 0; u(1)] - [0; 0; params.mass*params.g]) / params.mass;
% 角加速度
tau = [u(2); u(3); u(4)];
omega = [p; q; r];
omega_dot = inv([params.Ixx 0 0; 0 params.Iyy 0; 0 0 params.Izz]) * ...
(tau - cross(omega, [params.Ixx; params.Iyy; params.Izz].*omega));
dx = [x(7:9); ... % 位置导数=速度
p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta); ... % φ̇
q*cos(phi) - r*sin(phi); ... % θ̇
q*sin(phi)/cos(theta) + r*cos(phi)/cos(theta); ... % ψ̇
acc; ... % 速度导数=加速度
omega_dot]; % 角速度导数=角加速度
end
2. 定点悬停控制策略与实现
2.1 分层控制架构
四旋翼控制通常采用内外环结构:
- 外环:位置控制(生成期望姿态)
- 内环:姿态控制(生成电机指令)
mermaid复制graph TD
A[位置控制器] --> B[期望姿态]
B --> C[姿态控制器]
C --> D[电机混控]
2.2 PID控制器实现
高度控制器的MATLAB实现需要特别注意积分抗饱和:
matlab复制function u = altitude_control(z_des, z, dz, dt)
persistent i_error;
if isempty(i_error)
i_error = 0;
end
% PID参数
Kp = 2.8; Ki = 0.15; Kd = 1.2;
error = z_des - z;
% 积分限幅
if abs(i_error + error*dt) < 0.5
i_error = i_error + error*dt;
end
% 输出控制量
u = Kp*error + Ki*i_error - Kd*dz;
end
实际经验:dt必须与仿真步长严格一致。我曾使用变步长求解器导致积分项计算异常,飞机像跳跳糖一样上下抖动。建议固定步长为0.01s。
2.3 姿态控制器优化
针对四旋翼强耦合特性,建议采用解耦PID:
matlab复制function tau = attitude_control(phi_d, theta_d, psi_d, ...
phi, theta, psi, p, q, r)
% 滚转控制
Kp_phi = 8.5; Kd_phi = 2.3;
tau_x = Kp_phi*(phi_d - phi) - Kd_phi*p;
% 俯仰控制
Kp_theta = 8.5; Kd_theta = 2.3;
tau_y = Kp_theta*(theta_d - theta) - Kd_theta*q;
% 偏航控制
Kp_psi = 3.0; Kd_psi = 0.8;
tau_z = Kp_psi*(psi_d - psi) - Kd_psi*r;
tau = [tau_x; tau_y; tau_z];
end
3. 航路跟踪控制技术
3.1 轨迹生成方法
三次样条插值是生成平滑轨迹的常用方法:
matlab复制% 航路点
waypoints = [0 0 1;
2 1 1.5;
4 0 2;
6 2 1];
% 生成三次样条
t = linspace(0, 10, size(waypoints,1));
tt = linspace(0, 10, 100);
x_traj = spline(t, waypoints(:,1)', tt);
y_traj = spline(t, waypoints(:,2)', tt);
z_traj = spline(t, waypoints(:,3)', tt);
3.2 前馈补偿设计
通过轨迹微分计算前馈项可显著提高跟踪精度:
matlab复制% 计算前馈角度
dx_des = gradient(x_traj, tt);
dy_des = gradient(y_traj, tt);
ddx_des = gradient(dx_des, tt);
ddy_des = gradient(dy_des, tt);
phi_ff = atan2(ddy_des, 9.81); % 滚转前馈
theta_ff = atan2(-ddx_des, 9.81); % 俯仰前馈
3.3 全状态反馈线性化
对于高性能跟踪,可考虑反馈线性化方法:
matlab复制% 计算虚拟控制量
ax_des = Kp_x*(x_des - x) - Kd_x*dx + ddx_des;
ay_des = Kp_y*(y_des - y) - Kd_y*dy + ddy_des;
% 计算期望姿态
theta_d = atan2(-ax_des, sqrt(ay_des^2 + (az_des + g)^2));
phi_d = atan2(ay_des, az_des + g);
4. 编队控制实现方案
4.1 虚拟领航者架构
将编队控制转换为每个跟随者跟踪虚拟领航者的形式:
matlab复制% 虚拟领航者位置
leader_pos = [x_leader; y_leader; z_leader];
% 期望相对位置 (队形配置)
formation_offset = [0; -1; 0];
% 跟随者的目标位置
target_pos = leader_pos + rotation_matrix(psi_leader) * formation_offset;
4.2 通信延迟补偿
实际系统中必须考虑通信延迟的影响:
matlab复制% 数据包结构
packet.data = [pos; vel; acc];
packet.timestamp = current_time;
% 接收端处理
valid_idx = find([rx_buffer.timestamp] > current_time - max_delay);
if ~isempty(valid_idx)
latest_packet = rx_buffer(valid_idx(end));
delta_t = current_time - latest_packet.timestamp;
% 状态预测
pred_pos = latest_packet.data(1:3) + ...
latest_packet.data(4:6)*delta_t + ...
0.5*latest_packet.data(7:9)*delta_t^2;
end
4.3 编队避障策略
结合势场法实现避障功能:
matlab复制function F = potential_field(q, q_obs)
% 排斥力计算
d = norm(q - q_obs);
if d < safe_distance
F = repulsive_gain * (1/d - 1/safe_distance) * (1/d^2) * (q - q_obs)/d;
else
F = [0; 0; 0];
end
end
5. 仿真实验中的常见问题与解决
5.1 电机动态特性补偿
实际电机响应存在延迟,需要在控制器中加入补偿:
matlab复制% 一阶惯性环节模拟电机动态
function omega_actual = motor_dynamics(omega_cmd, prev_omega, dt)
tau_motor = 0.08; % 电机时间常数
omega_actual = prev_omega + (omega_cmd - prev_omega)*dt/tau_motor;
end
5.2 传感器噪声处理
仿真中需要添加合理的噪声模型:
matlab复制% 添加高斯噪声
noisy_pos = true_pos + pos_noise_gain * randn(3,1);
noisy_vel = true_vel + vel_noise_gain * randn(3,1);
5.3 数值稳定性问题
使用四元数避免欧拉角奇异点:
matlab复制% 欧拉角转四元数
function q = euler2quat(phi, theta, psi)
q = [cos(phi/2)*cos(theta/2)*cos(psi/2) + sin(phi/2)*sin(theta/2)*sin(psi/2);
sin(phi/2)*cos(theta/2)*cos(psi/2) - cos(phi/2)*sin(theta/2)*sin(psi/2);
cos(phi/2)*sin(theta/2)*cos(psi/2) + sin(phi/2)*cos(theta/2)*sin(psi/2);
cos(phi/2)*cos(theta/2)*sin(psi/2) - sin(phi/2)*sin(theta/2)*cos(psi/2)];
end
6. 实验报告撰写要点
6.1 结果分析方法
- 绘制时间响应曲线对比期望与实际轨迹
- 计算RMSE等性能指标
- 分析控制量变化是否平滑
matlab复制% 计算RMSE
pos_error = sqrt(mean((x_traj - x_sim).^2 + (y_traj - y_sim).^2 + (z_traj - z_sim).^2));
6.2 动画生成技巧
使用MATLAB的Animation功能展示三维轨迹:
matlab复制figure;
view(3);
grid on;
axis equal;
hold on;
% 绘制轨迹
plot3(x_sim, y_sim, z_sim, 'b-');
% 实时动画
for k = 1:length(t_sim)
R = euler2rot(phi_sim(k), theta_sim(k), psi_sim(k));
draw_quadrotor(x_sim(k), y_sim(k), z_sim(k), R);
pause(0.01);
end
6.3 参数整定经验
- 先调内环(姿态),再调外环(位置)
- 先比例(P),再微分(D),最后积分(I)
- 在10%-20%油门附近线性化模型
经过多次实验,我总结出一套适用于1.2kg四旋翼的初始PID参数:
- 高度控制:Kp=2.8, Ki=0.15, Kd=1.2
- 姿态控制:Kp=8.5, Kd=2.3
- 位置控制:Kp=1.5, Kd=0.8
这些参数可以作为调试的起点,实际应用中需要根据具体飞行器特性进行调整。特别是在负载变化或电池电压下降时,控制参数可能需要在线调整。
