1. 四旋翼无人机建模基础
四旋翼无人机作为一种典型的欠驱动系统,其建模过程需要综合考虑空气动力学、刚体力学和控制理论等多个学科知识。在MATLAB环境下建立准确的飞行仿真模型,是验证控制算法有效性的重要前提。
1.1 坐标系定义与转换
在无人机建模中,我们通常使用两类坐标系:
-
地面惯性坐标系(NED坐标系):
- X轴:指向地理北向
- Y轴:指向地理东向
- Z轴:垂直地面向下
- 该坐标系视为固定不动的参考系
-
机体坐标系(Body坐标系):
- X轴:指向无人机机头方向
- Y轴:指向无人机右侧
- Z轴:垂直机身向下
- 该坐标系随无人机运动而改变
两坐标系间的转换通过旋转矩阵实现,常用的欧拉角包括:
- 滚转角φ(Roll):绕X轴旋转
- 俯仰角θ(Pitch):绕Y轴旋转
- 偏航角ψ(Yaw):绕Z轴旋转
旋转矩阵R的计算公式为:
matlab复制R = [cosθ*cosψ, sinφ*sinθ*cosψ-cosφ*sinψ, cosφ*sinθ*cosψ+sinφ*sinψ;
cosθ*sinψ, sinφ*sinθ*sinψ+cosφ*cosψ, cosφ*sinθ*sinψ-sinφ*cosψ;
-sinθ, sinφ*cosθ, cosφ*cosθ];
1.2 动力学方程建立
基于牛顿-欧拉方程,我们可以建立四旋翼的六自由度动力学模型:
平移运动方程:
matlab复制m * dv/dt = R * [0; 0; T] - [0; 0; m*g] - 0.5*ρ*Cd*A*|v|*v
其中:
- m:无人机质量
- v:速度向量
- T:总升力
- ρ:空气密度
- Cd:阻力系数
- A:等效迎风面积
旋转运动方程:
matlab复制I * dω/dt + ω × (I*ω) = τ
其中:
- I:惯性矩阵
- ω:角速度向量
- τ:总力矩向量
2. 旋翼动力学建模
2.1 单旋翼升力模型
每个旋翼产生的升力F_i可表示为:
matlab复制F_i = k_f * ω_i^2
其中:
- k_f:升力系数(与旋翼几何参数和空气密度相关)
- ω_i:第i个旋翼的转速(rad/s)
在MATLAB中,我们可以通过实验数据拟合k_f值:
matlab复制% 实验数据:转速与对应升力
rpm = [1000 2000 3000 4000 5000];
force = [0.5 2.1 4.7 8.3 13.0]; % N
% 转换为角速度(rad/s)和力(N)
omega = rpm * 2*pi/60;
p = polyfit(omega.^2, force, 1);
k_f = p(1); % 获取升力系数
2.2 反扭矩效应建模
旋翼旋转时会产生反作用力矩,其大小与转速平方成正比:
matlab复制Q_i = k_m * ω_i^2
其中k_m为扭矩系数,可通过实验测定。
2.3 旋翼动力学响应
实际电机-旋翼系统存在动态响应过程,可用一阶惯性环节近似:
matlab复制dω_i/dt = (ω_cmd_i - ω_i) / τ_m
其中:
- ω_cmd_i:电机控制指令对应的目标转速
- τ_m:电机时间常数(通常50-200ms)
3. 控制分配与混控逻辑
3.1 基本控制量计算
四旋翼的控制输入通常包括:
- 总升力U1
- 滚转力矩U2
- 俯仰力矩U3
- 偏航力矩U4
这些控制量与四个旋翼转速的关系为:
matlab复制U1 = k_f*(ω1^2 + ω2^2 + ω3^2 + ω4^2)
U2 = k_f*l*(ω4^2 - ω2^2)
U3 = k_f*l*(ω3^2 - ω1^2)
U4 = k_m*(-ω1^2 + ω2^2 - ω3^2 + ω4^2)
其中l为旋翼中心到无人机重心的距离。
3.2 混控矩阵实现
将上述关系表示为矩阵形式:
matlab复制[U1; U2; U3; U4] = M * [ω1^2; ω2^2; ω3^2; ω4^2]
其中混控矩阵M为:
matlab复制M = [k_f, k_f, k_f, k_f;
0, -k_f*l, 0, k_f*l;
-k_f*l, 0, k_f*l, 0;
-k_m, k_m, -k_m, k_m]
在MATLAB中实现控制分配:
matlab复制function [w1, w2, w3, w4] = controlAllocation(U1, U2, U3, U4, k_f, k_m, l)
M = [k_f, k_f, k_f, k_f;
0, -k_f*l, 0, k_f*l;
-k_f*l, 0, k_f*l, 0;
-k_m, k_m, -k_m, k_m];
w_squared = pinv(M) * [U1; U2; U3; U4];
w = sqrt(max(w_squared, 0)); % 防止负值
w1 = w(1); w2 = w(2); w3 = w(3); w4 = w(4);
end
4. 飞行控制算法实现
4.1 PID控制器设计
针对四旋翼的六个自由度,我们通常设计级联PID控制器:
外环(位置控制):
matlab复制function [theta_d, phi_d] = positionController(x_err, y_err, Kp_xy, Ki_xy, Kd_xy)
persistent integral_xy prev_err_xy
if isempty(integral_xy)
integral_xy = [0; 0];
prev_err_xy = [0; 0];
end
integral_xy = integral_xy + [x_err; y_err];
derivative_xy = [x_err; y_err] - prev_err_xy;
prev_err_xy = [x_err; y_err];
% 生成姿态指令
phi_d = Kp_xy(1)*x_err + Ki_xy(1)*integral_xy(1) + Kd_xy(1)*derivative_xy(1);
theta_d = Kp_xy(2)*y_err + Ki_xy(2)*integral_xy(2) + Kd_xy(2)*derivative_xy(2);
% 限制指令范围
phi_d = constrain(phi_d, -30*pi/180, 30*pi/180);
theta_d = constrain(theta_d, -30*pi/180, 30*pi/180);
end
内环(姿态控制):
matlab复制function [U2, U3, U4] = attitudeController(phi, theta, psi, ...
phi_d, theta_d, psi_d, ...
Kp_ang, Ki_ang, Kd_ang)
persistent integral_ang prev_err_ang
if isempty(integral_ang)
integral_ang = [0; 0; 0];
prev_err_ang = [0; 0; 0];
end
err_ang = [phi_d - phi; theta_d - theta; psi_d - psi];
integral_ang = integral_ang + err_ang;
derivative_ang = err_ang - prev_err_ang;
prev_err_ang = err_ang;
% 生成力矩指令
U2 = Kp_ang(1)*err_ang(1) + Ki_ang(1)*integral_ang(1) + Kd_ang(1)*derivative_ang(1);
U3 = Kp_ang(2)*err_ang(2) + Ki_ang(2)*integral_ang(2) + Kd_ang(2)*derivative_ang(2);
U4 = Kp_ang(3)*err_ang(3) + Ki_ang(3)*integral_ang(3) + Kd_ang(3)*derivative_ang(3);
end
4.2 高度控制实现
高度控制独立于水平位置控制,直接调节总升力:
matlab复制function U1 = altitudeController(z, z_d, dz, dz_d, Kp_z, Ki_z, Kd_z)
persistent integral_z prev_err_z
if isempty(integral_z)
integral_z = 0;
prev_err_z = 0;
end
err_z = z_d - z;
err_dz = dz_d - dz;
integral_z = integral_z + err_z;
% 考虑重力补偿
g = 9.81;
U1 = Kp_z*err_z + Ki_z*integral_z + Kd_z*err_dz + g;
% 限制升力范围
U1 = constrain(U1, 0, 2*g);
end
5. MATLAB仿真实现
5.1 仿真环境搭建
使用MATLAB的ODE求解器进行动力学仿真:
matlab复制function simQuadrotor()
% 初始化参数
params = struct();
params.m = 1.2; % 质量(kg)
params.g = 9.81; % 重力加速度
params.l = 0.25; % 旋翼到重心距离(m)
params.k_f = 8.55e-6; % 升力系数
params.k_m = 1.6e-2; % 扭矩系数
params.I = diag([0.03, 0.03, 0.04]); % 惯性矩阵
% 初始状态
x0 = [0; 0; 5; % 初始位置(NED)
0; 0; 0; % 初始速度
0; 0; 0; % 初始欧拉角
0; 0; 0]; % 初始角速度
% 仿真时间
tspan = [0 10];
% 控制指令
ref = @(t) [5*sin(0.5*t); 5*cos(0.5*t); 5; 0]; % 圆形轨迹
% 运行仿真
[t, x] = ode45(@(t,x) quadDynamics(t, x, ref(t), params), tspan, x0);
% 可视化结果
plotResults(t, x);
end
function dx = quadDynamics(t, x, ref, params)
% 状态分解
pos = x(1:3);
vel = x(4:6);
ang = x(7:9);
omega = x(10:12);
% 控制器计算
[U1, U2, U3, U4] = quadController(pos, vel, ang, omega, ref, params);
% 动力学方程
[dv, domega] = quadEquationsOfMotion(vel, ang, omega, [U1; U2; U3; U4], params);
% 状态导数
dx = zeros(12,1);
dx(1:3) = vel;
dx(4:6) = dv;
dx(7:9) = eulerKinematics(ang, omega);
dx(10:12) = domega;
end
5.2 可视化实现
使用MATLAB的3D动画工具展示无人机运动:
matlab复制function plotResults(t, x)
figure;
% 轨迹绘制
subplot(2,2,[1 3]);
plot3(x(:,2), x(:,1), -x(:,3));
xlabel('East (m)'); ylabel('North (m)'); zlabel('Altitude (m)');
title('飞行轨迹');
grid on; axis equal;
% 姿态角绘制
subplot(2,2,2);
plot(t, x(:,7:9)*180/pi);
legend('Roll', 'Pitch', 'Yaw');
xlabel('时间(s)'); ylabel('角度(°)');
title('姿态角变化');
% 高度绘制
subplot(2,2,4);
plot(t, -x(:,3));
xlabel('时间(s)'); ylabel('高度(m)');
title('高度变化');
% 3D动画
figure;
h = plotQuadrotor(x(1,1:3), x(1,7:9));
axis([-10 10 -10 10 0 10]);
view(3); grid on;
for k = 1:10:length(t)
updateQuadrotor(h, x(k,1:3), x(k,7:9));
drawnow;
end
end
6. 仿真结果分析与优化
6.1 性能指标评估
在分析仿真结果时,我们关注以下关键指标:
-
轨迹跟踪误差:
matlab复制pos_error = sqrt(sum((actual_pos - desired_pos).^2, 2)); mean_error = mean(pos_error); max_error = max(pos_error); -
姿态稳定时间:
matlab复制settle_time = find(abs(roll) < 2*pi/180, 1, 'first') * dt; -
能量消耗:
matlab复制energy = sum(omega1.^2 + omega2.^2 + omega3.^2 + omega4.^2) * dt;
6.2 控制器参数整定
使用Ziegler-Nichols方法进行PID参数初步整定:
- 先设置Ki=Kd=0,逐渐增大Kp直到系统开始持续振荡
- 记录此时的临界增益Ku和振荡周期Tu
- 根据下表确定PID参数:
| 控制器类型 | Kp | Ki | Kd |
|---|---|---|---|
| P | 0.5Ku | 0 | 0 |
| PI | 0.45Ku | 0.54Ku/Tu | 0 |
| PID | 0.6Ku | 1.2Ku/Tu | 0.075Ku*Tu |
在MATLAB中实现自动整定:
matlab复制function [Kp, Ki, Kd] = autoTune(plant, output_index)
% 创建调节器对象
st = systuneOptions('RandomStart', 5);
% 定义调节目标
req1 = TuningGoal.Tracking('r', 'y', 0.5); % 跟踪带宽0.5rad/s
req2 = TuningGoal.StepResponse('r', 'y', 0.8, 0); % 80%响应时间
% 自动调节
[~,~,~,info] = systune(plant, [req1 req2], [], st);
% 获取PID参数
Kp = info.PID(output_index).Kp;
Ki = info.PID(output_index).Ki;
Kd = info.PID(output_index).Kd;
end
6.3 抗风扰设计
为增强无人机在风扰下的稳定性,可增加扰动观测器:
matlab复制function tau_hat = disturbanceObserver(omega, tau_cmd, dt)
persistent prev_omega prev_tau_hat
if isempty(prev_omega)
prev_omega = zeros(3,1);
prev_tau_hat = zeros(3,1);
end
% 观测器参数
L = 10; % 观测器增益
% 简化惯性矩阵
J = diag([0.03, 0.03, 0.04]);
% 扰动估计
domega = (omega - prev_omega)/dt;
tau_hat = prev_tau_hat + L*(J*domega + cross(omega, J*omega) - tau_cmd)*dt;
% 更新状态
prev_omega = omega;
prev_tau_hat = tau_hat;
end
在控制器中使用估计的扰动进行前馈补偿:
matlab复制tau_cmd = tau_pid - tau_hat;
7. 高级控制策略探索
7.1 模型预测控制(MPC)实现
MPC能够显式处理控制约束,适合四旋翼控制:
matlab复制function [U1, U2, U3, U4] = mpcController(x, ref, params)
% 创建MPC对象
mpcobj = mpc(params.model, params.Ts);
% 设置预测时域和控制时域
mpcobj.PredictionHorizon = 10;
mpcobj.ControlHorizon = 3;
% 设置约束
mpcobj.MV(1).Min = 0; % 最小升力
mpcobj.MV(1).Max = 20; % 最大升力
mpcobj.MV(2:4).Min = -2; % 最小力矩
mpcobj.MV(2:4).Max = 2; % 最大力矩
% 设置权重
mpcobj.Weights.OV = [10 10 10 1 1 1]; % 位置和姿态权重
mpcobj.Weights.MV = [0.1 1 1 1]; % 控制量权重
% 当前状态
x0 = x;
% 参考轨迹
refSignal = repmat(ref', 10, 1);
% 计算控制量
[u, ~] = mpcmove(mpcobj, x0, [], refSignal);
U1 = u(1); U2 = u(2); U3 = u(3); U4 = u(4);
end
7.2 自适应控制设计
针对模型不确定性,可采用模型参考自适应控制:
matlab复制function [U, theta_hat] = adaptiveController(x, x_ref, theta_hat, dt, params)
% 参考模型
xm_dot = params.Am * (x_ref - x);
% 跟踪误差
e = x - x_ref;
% 自适应律
P = lyap(params.Am', eye(6)); % 解Lyapunov方程
theta_hat_dot = -params.gamma * params.B' * P * e * x';
theta_hat = theta_hat + theta_hat_dot * dt;
% 控制律
U = -theta_hat' * x + params.K * (x_ref - x);
end
7.3 强化学习控制
使用深度确定性策略梯度(DDPG)算法:
matlab复制function agent = trainDDPG(env, params)
% 创建Actor网络
actorNetwork = [
featureInputLayer(params.obsDim)
fullyConnectedLayer(128)
reluLayer
fullyConnectedLayer(128)
reluLayer
fullyConnectedLayer(params.actDim)
tanhLayer
scalingLayer('Scale', params.actLimit)
];
% 创建Critic网络
criticNetwork = [
featureInputLayer(params.obsDim + params.actDim)
fullyConnectedLayer(128)
reluLayer
fullyConnectedLayer(128)
reluLayer
fullyConnectedLayer(1)
];
% 创建DDPG agent
agent = rlDDPGAgent(...
rlActorRepresentation(actorNetwork, env.ObservationInfo, env.ActionInfo),...
rlCriticRepresentation(criticNetwork, env.ObservationInfo, env.ActionInfo));
% 训练参数
trainOpts = rlTrainingOptions(...
'MaxEpisodes', 1000, ...
'ScoreAveragingWindowLength', 10);
% 训练agent
trainingStats = train(agent, env, trainOpts);
end