一、仿真代码
matlab
%% 四旋翼串级PID控制算法MATLAB仿真
% 功能：实现四旋翼无人机的串级PID控制仿真
% 包括位置外环和姿态内环的PID控制
clear; clc; close all;
fprintf('=== 四旋翼串级PID控制仿真开始 ===\n');
%% 1. 四旋翼参数定义
fprintf('定义四旋翼参数...\n');
% 物理参数
params.m = 1.2; % 质量 (kg)
params.g = 9.81; % 重力加速度 (m/s^2)
params.Ixx = 0.023; % 绕x轴转动惯量 (kg·m^2)
params.Iyy = 0.023; % 绕y轴转动惯量 (kg·m^2)
params.Izz = 0.046; % 绕z轴转动惯量 (kg·m^2)
params.k = 1.4e-5; % 升力系数 (N/(rad/s)^2)
params.b = 1.1e-7; % 阻力系数 (N·m/(rad/s)^2)
params.l = 0.225; % 机臂长度 (m)
params.Jr = 6e-5; % 转子转动惯量 (kg·m^2)
params.Omega_max = 700; % 电机最大转速 (rad/s)
% 仿真参数
dt = 0.01; % 时间步长 (s)
T = 20; % 仿真时间 (s)
N = T/dt; % 总步数
time = 0:dt:T; % 时间向量
%% 2. 状态变量初始化
fprintf('初始化状态变量...\n');
% 位置和速度
x = zeros(3, N+1); % 位置 [x; y; z]
v = zeros(3, N+1); % 速度 [vx; vy; vz]
% 欧拉角（姿态）
phi = zeros(1, N+1); % 滚转角 (绕x轴)
theta = zeros(1, N+1); % 俯仰角 (绕y轴)
psi = zeros(1, N+1); % 偏航角 (绕z轴)
% 角速度
p = zeros(1, N+1); % 滚转角速度
q = zeros(1, N+1); % 俯仰角速度
r = zeros(1, N+1); % 偏航角速度
% 电机转速
w = zeros(4, N+1); % 四个电机的转速
w(:,1) = sqrt(params.m*params.g/(4*params.k)); % 悬停转速
% 控制输入
U1 = zeros(1, N+1); % 总升力
U2 = zeros(1, N+1); % 滚转力矩
U3 = zeros(1, N+1); % 俯仰力矩
U4 = zeros(1, N+1); % 偏航力矩
% 期望轨迹
xd = zeros(3, N+1); % 期望位置
vd = zeros(3, N+1); % 期望速度
ad = zeros(3, N+1); % 期望加速度
phid = zeros(1, N+1); % 期望滚转角
thetad = zeros(1, N+1); % 期望俯仰角
psid = zeros(1, N+1); % 期望偏航角
%% 3. 生成期望轨迹
fprintf('生成期望轨迹...\n');
% 3.1 正方形轨迹
for k = 1:N+1
t = time(k);
if t < 5
% 第一阶段：上升
xd(1,k) = 0;
xd(2,k) = 0;
xd(3,k) = 2 * (t/5);
psid(k) = 0;
elseif t < 10
% 第二阶段：向前移动
xd(1,k) = 2 * ((t-5)/5);
xd(2,k) = 0;
xd(3,k) = 2;
psid(k) = 0;
elseif t < 15
% 第三阶段：向右移动
xd(1,k) = 2;
xd(2,k) = 2 * ((t-10)/5);
xd(3,k) = 2;
psid(k) = 0;
else
% 第四阶段：盘旋
xd(1,k) = 2;
xd(2,k) = 2;
xd(3,k) = 2;
psid(k) = 0.2 * sin(0.5*t);
end
end
% 3.2 圆形轨迹（可选）
% R = 3; % 半径
% omega = 0.5; % 角频率
% for k = 1:N+1
% t = time(k);
% xd(1,k) = R * sin(omega*t);
% xd(2,k) = R * (1 - cos(omega*t));
% xd(3,k) = 2;
% psid(k) = 0;
% end
%% 4. PID控制器设计
fprintf('设计串级PID控制器...\n');
% 4.1 位置外环PID参数
Kp_x = 1.5; Ki_x = 0.1; Kd_x = 0.8; % x方向
Kp_y = 1.5; Ki_y = 0.1; Kd_y = 0.8; % y方向
Kp_z = 2.0; Ki_z = 0.2; Kd_z = 1.2; % z方向
% 4.2 姿态内环PID参数
Kp_phi = 6; Ki_phi = 1.5; Kd_phi = 2; % 滚转角
Kp_theta = 6; Ki_theta = 1.5; Kd_theta = 2; % 俯仰角
Kp_psi = 3; Ki_psi = 0.5; Kd_psi = 1; % 偏航角
% 4.3 初始化PID误差积分项
int_error_x = 0; int_error_y = 0; int_error_z = 0;
int_error_phi = 0; int_error_theta = 0; int_error_psi = 0;
% 4.4 初始化前一时刻的误差
prev_error_x = 0; prev_error_y = 0; prev_error_z = 0;
prev_error_phi = 0; prev_error_theta = 0; prev_error_psi = 0;
%% 5. 主仿真循环
fprintf('开始仿真循环...\n');
for k = 1:N
% 5.1 位置控制（外环）
% 计算位置误差
error_x = xd(1,k) - x(1,k);
error_y = xd(2,k) - x(2,k);
error_z = xd(3,k) - x(3,k);
% 更新积分项
int_error_x = int_error_x + error_x * dt;
int_error_y = int_error_y + error_y * dt;
int_error_z = int_error_z + error_z * dt;
% 计算微分项
derror_x = (error_x - prev_error_x) / dt;
derror_y = (error_y - prev_error_y) / dt;
derror_z = (error_z - prev_error_z) / dt;
% PID控制
ax_cmd = Kp_x * error_x + Ki_x * int_error_x + Kd_x * derror_x;
ay_cmd = Kp_y * error_y + Ki_y * int_error_y + Kd_y * derror_y;
az_cmd = Kp_z * error_z + Ki_z * int_error_z + Kd_z * derror_z;
% 保存当前误差
prev_error_x = error_x;
prev_error_y = error_y;
prev_error_z = error_z;
% 5.2 计算期望姿态
% 总推力计算
U1(k) = params.m * (params.g + az_cmd);
% 限制总推力
U1_max = params.m * params.g * 2; % 最大2g加速度
U1_min = 0.1 * params.m * params.g; % 最小0.1g加速度
U1(k) = max(min(U1(k), U1_max), U1_min);
% 计算期望的俯仰和滚转角
thetad(k) = (1/params.g) * (ax_cmd * sin(psid(k)) - ay_cmd * cos(psid(k)));
phid(k) = (1/params.g) * (ax_cmd * cos(psid(k)) + ay_cmd * sin(psid(k)));
% 限制期望姿态角
max_angle = deg2rad(30); % 最大30度
thetad(k) = max(min(thetad(k), max_angle), -max_angle);
phid(k) = max(min(phid(k), max_angle), -max_angle);
% 5.3 姿态控制（内环）
% 计算姿态误差
error_phi = phid(k) - phi(k);
error_theta = thetad(k) - theta(k);
error_psi = psid(k) - psi(k);
% 更新积分项
int_error_phi = int_error_phi + error_phi * dt;
int_error_theta = int_error_theta + error_theta * dt;
int_error_psi = int_error_psi + error_psi * dt;
% 计算微分项
derror_phi = (error_phi - prev_error_phi) / dt;
derror_theta = (error_theta - prev_error_theta) / dt;
derror_psi = (error_psi - prev_error_psi) / dt;
% PID控制
p_cmd = Kp_phi * error_phi + Ki_phi * int_error_phi + Kd_phi * derror_phi;
q_cmd = Kp_theta * error_theta + Ki_theta * int_error_theta + Kd_theta * derror_theta;
r_cmd = Kp_psi * error_psi + Ki_psi * int_error_psi + Kd_psi * derror_psi;
% 保存当前误差
prev_error_phi = error_phi;
prev_error_theta = error_theta;
prev_error_psi = error_psi;
% 5.4 计算控制力矩
U2(k) = p_cmd;
U3(k) = q_cmd;
U4(k) = r_cmd;
% 5.5 将控制输入转换为电机转速
% 控制分配矩阵
Gamma = [params.k, params.k, params.k, params.k;
0, -params.k*params.l, 0, params.k*params.l;
-params.k*params.l, 0, params.k*params.l, 0;
params.b, -params.b, params.b, -params.b];
% 控制输入向量
U = [U1(k); U2(k); U3(k); U4(k)];
% 计算电机转速平方
w_sq = Gamma \ U;
% 确保非负
w_sq = max(w_sq, 0);
% 计算电机转速
w(:,k) = sqrt(w_sq);
% 5.6 四旋翼动力学更新
% 获取当前状态
state = [x(:,k); v(:,k); phi(k); theta(k); psi(k); p(k); q(k); r(k)];
% 使用RK4积分更新状态
k1 = quadcopter_dynamics(state, w(:,k), params, dt);
k2 = quadcopter_dynamics(state + 0.5*dt*k1, w(:,k), params, dt);
k3 = quadcopter_dynamics(state + 0.5*dt*k2, w(:,k), params, dt);
k4 = quadcopter_dynamics(state + dt*k3, w(:,k), params, dt);
state_new = state + (dt/6)*(k1 + 2*k2 + 2*k3 + k4);
% 更新状态
x(:,k+1) = state_new(1:3);
v(:,k+1) = state_new(4:6);
phi(k+1) = state_new(7);
theta(k+1) = state_new(8);
psi(k+1) = state_new(9);
p(k+1) = state_new(10);
q(k+1) = state_new(11);
r(k+1) = state_new(12);
% 5.7 显示进度
if mod(k, floor(N/10)) == 0
fprintf(' 进度: %.0f%%\n', k/N*100);
end
end
%% 6. 动力学模型函数
function dxdt = quadcopter_dynamics(state, w, params, dt)
% 四旋翼动力学方程
% 状态: [x; y; z; vx; vy; vz; phi; theta; psi; p; q; r]
% 提取状态
x = state(1:3);
v = state(4:6);
phi = state(7);
theta = state(8);
psi = state(9);
p = state(10);
q = state(11);
r = state(12);
% 总升力
F = params.k * sum(w.^2);
% 总力矩
tau_phi = params.k * params.l * (w(4)^2 - w(2)^2);
tau_theta = params.k * params.l * (w(3)^2 - w(1)^2);
tau_psi = params.b * (w(2)^2 + w(4)^2 - w(1)^2 - w(3)^2);
% 陀螺力矩
tau_gyro = params.Jr * [q*(w(1)-w(2)+w(3)-w(4));
-p*(w(1)-w(2)+w(3)-w(4));
0];
% 旋转矩阵（机体到惯性系）
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)];
% 重力向量
g = [0; 0; params.g];
% 位置导数
dxdt(1:3,1) = v;
% 速度导数
dxdt(4:6,1) = (1/params.m) * (R * [0; 0; F]) - g;
% 欧拉角导数
T = [1, sin(phi)*tan(theta), cos(phi)*tan(theta);
0, cos(phi), -sin(phi);
0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
dxdt(7:9,1) = T * [p; q; r];
% 角加速度
dxdt(10,1) = (1/params.Ixx) * (tau_phi - (params.Izz-params.Iyy)*q*r - tau_gyro(1));
dxdt(11,1) = (1/params.Iyy) * (tau_theta - (params.Ixx-params.Izz)*p*r - tau_gyro(2));
dxdt(12,1) = (1/params.Izz) * (tau_psi - (params.Iyy-params.Ixx)*p*q - tau_gyro(3));
end
%% 7. 结果可视化
fprintf('可视化仿真结果...\n');
% 7.1 位置跟踪结果
figure('Position', [100, 100, 1200, 800]);
% 子图1：3D轨迹
subplot(3, 4, [1,2,5,6]);
plot3(xd(1,:), xd(2,:), xd(3,:), 'r--', 'LineWidth', 2, 'DisplayName', '期望轨迹');
hold on;
plot3(x(1,:), x(2,:), x(3,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '实际轨迹');
scatter3(xd(1,1), xd(2,1), xd(3,1), 100, 'g^', 'filled', 'DisplayName', '起点');
scatter3(xd(1,end), xd(2,end), xd(3,end), 100, 'rs', 'filled', 'DisplayName', '终点');
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('3D轨迹跟踪');
legend('Location', 'best');
grid on; axis equal;
view(45, 30);
% 子图2：X轴位置跟踪
subplot(3, 4, 3);
plot(time, xd(1,:), 'r--', 'LineWidth', 2, 'DisplayName', '期望');
hold on;
plot(time, x(1,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '实际');
xlabel('时间 (s)'); ylabel('X位置 (m)');
title('X轴位置跟踪');
legend; grid on;
% 子图3：Y轴位置跟踪
subplot(3, 4, 4);
plot(time, xd(2,:), 'r--', 'LineWidth', 2, 'DisplayName', '期望');
hold on;
plot(time, x(2,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '实际');
xlabel('时间 (s)'); ylabel('Y位置 (m)');
title('Y轴位置跟踪');
legend; grid on;
% 子图4：Z轴位置跟踪
subplot(3, 4, 7);
plot(time, xd(3,:), 'r--', 'LineWidth', 2, 'DisplayName', '期望');
hold on;
plot(time, x(3,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '实际');
xlabel('时间 (s)'); ylabel('Z位置 (m)');
title('Z轴位置跟踪');
legend; grid on;
% 子图5：姿态角
subplot(3, 4, 8);
plot(time, rad2deg(phi), 'b-', 'LineWidth', 1.5, 'DisplayName', '\phi');
hold on;
plot(time, rad2deg(theta), 'r-', 'LineWidth', 1.5, 'DisplayName', '\theta');
plot(time, rad2deg(psi), 'g-', 'LineWidth', 1.5, 'DisplayName', '\psi');
xlabel('时间 (s)'); ylabel('角度 (°)');
title('姿态角响应');
legend; grid on;
% 子图6：控制输入
subplot(3, 4, 9);
plot(time(1:end-1), U1(1:end-1), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('U1 (N)');
title('总升力控制输入');
grid on;
% 子图7：滚转控制
subplot(3, 4, 10);
plot(time(1:end-1), U2(1:end-1), 'r-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('U2 (N·m)');
title('滚转控制力矩');
grid on;
% 子图8：俯仰控制
subplot(3, 4, 11);
plot(time(1:end-1), U3(1:end-1), 'g-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('U3 (N·m)');
title('俯仰控制力矩');
grid on;
% 子图9：偏航控制
subplot(3, 4, 12);
plot(time(1:end-1), U4(1:end-1), 'm-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('U4 (N·m)');
title('偏航控制力矩');
grid on;
% 7.2 误差分析
figure('Position', [100, 100, 1200, 400]);
% 位置误差
subplot(1, 3, 1);
error_x = xd(1,:) - x(1,:);
error_y = xd(2,:) - x(2,:);
error_z = xd(3,:) - x(3,:);
plot(time, error_x, 'r-', 'LineWidth', 1.5, 'DisplayName', 'X误差');
hold on;
plot(time, error_y, 'g-', 'LineWidth', 1.5, 'DisplayName', 'Y误差');
plot(time, error_z, 'b-', 'LineWidth', 1.5, 'DisplayName', 'Z误差');
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('位置跟踪误差');
legend; grid on;
% 姿态误差
subplot(1, 3, 2);
error_phi = phid - phi;
error_theta = thetad - theta;
error_psi = psid - psi;
plot(time, rad2deg(error_phi), 'r-', 'LineWidth', 1.5, 'DisplayName', '\phi误差');
hold on;
plot(time, rad2deg(error_theta), 'g-', 'LineWidth', 1.5, 'DisplayName', '\theta误差');
plot(time, rad2deg(error_psi), 'b-', 'LineWidth', 1.5, 'DisplayName', '\psi误差');
xlabel('时间 (s)'); ylabel('姿态误差 (°)');
title('姿态跟踪误差');
legend; grid on;
% 性能指标
subplot(1, 3, 3);
RMSE_x = sqrt(mean(error_x.^2));
RMSE_y = sqrt(mean(error_y.^2));
RMSE_z = sqrt(mean(error_z.^2));
RMSE_phi = sqrt(mean(error_phi.^2));
RMSE_theta = sqrt(mean(error_theta.^2));
RMSE_psi = sqrt(mean(error_psi.^2));
bar_data = [RMSE_x, RMSE_y, RMSE_z, rad2deg(RMSE_phi), rad2deg(RMSE_theta), rad2deg(RMSE_psi)];
bar_labels = {'X RMSE', 'Y RMSE', 'Z RMSE', '\phi RMSE', '\theta RMSE', '\psi RMSE'};
bar_colors = [1 0 0; 0 1 0; 0 0 1; 1 0.5 0; 0.5 0 1; 0 0.5 0.5];
h = bar(1:6, bar_data);
for i = 1:6
h.FaceColor = 'flat';
h.CData(i,:) = bar_colors(i,:);
end
ylabel('RMSE (m 或 °)');
title('性能指标(RMSE)');
set(gca, 'XTick', 1:6, 'XTickLabel', bar_labels);
grid on;
% 添加数值标签
for i = 1:6
if i <= 3
text(i, bar_data(i), sprintf('%.3f', bar_data(i)), ...
'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom');
else
text(i, bar_data(i), sprintf('%.2f°', bar_data(i)), ...
'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom');
end
end
%% 8. 动画演示
fprintf('生成飞行轨迹动画...\n');
animate_quadrotor = true;
if animate_quadrotor
figure('Position', [100, 100, 800, 600]);
% 创建四旋翼模型
arm_length = params.l;
propeller_radius = 0.1;
% 动画参数
skip_frames = 5; % 跳帧，加快动画速度
anim_time = time(1:skip_frames:end);
anim_x = x(:,1:skip_frames:end);
anim_phi = phi(1:skip_frames:end);
anim_theta = theta(1:skip_frames:end);
anim_psi = psi(1:skip_frames:end);
for i = 1:length(anim_time)
clf;
% 绘制轨迹
plot3(xd(1,:), xd(2,:), xd(3,:), 'r--', 'LineWidth', 1, 'DisplayName', '期望轨迹');
hold on;
plot3(anim_x(1,1:i), anim_x(2,1:i), anim_x(3,1:i), 'b-', 'LineWidth', 1.5, 'DisplayName', '实际轨迹');
% 获取当前姿态
curr_phi = anim_phi(i);
curr_theta = anim_theta(i);
curr_psi = anim_psi(i);
% 计算旋转矩阵
R_anim = eul2rotm([curr_psi, curr_theta, curr_phi], 'ZYX');
% 四旋翼机身框架
body_points = arm_length * [1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0]';
body_points_rotated = R_anim * body_points;
body_points_translated = body_points_rotated + anim_x(:,i);
% 绘制机身
plot3([body_points_translated(1,1), body_points_translated(1,2)], ...
[body_points_translated(2,1), body_points_translated(2,2)], ...
[body_points_translated(3,1), body_points_translated(3,2)], ...
'k-', 'LineWidth', 3);
plot3([body_points_translated(1,3), body_points_translated(1,4)], ...
[body_points_translated(2,3), body_points_translated(2,4)], ...
[body_points_translated(3,3), body_points_translated(3,4)], ...
'k-', 'LineWidth', 3);
% 绘制螺旋桨
angles = linspace(0, 2*pi, 20);
for j = 1:4
prop_points = propeller_radius * [cos(angles); sin(angles); zeros(1,20)];
prop_points_rotated = R_anim * prop_points;
prop_points_translated = prop_points_rotated + body_points_translated(:,j);
fill3(prop_points_translated(1,:), prop_points_translated(2,:), ...
prop_points_translated(3,:), [0.5, 0.5, 0.5], 'FaceAlpha', 0.5);
end
% 绘制坐标轴
axis_length = 0.5;
quiver3(anim_x(1,i), anim_x(2,i), anim_x(3,i), ...
R_anim(1,1)*axis_length, R_anim(2,1)*axis_length, R_anim(3,1)*axis_length, ...
'r', 'LineWidth', 2, 'MaxHeadSize', 0.5);
quiver3(anim_x(1,i), anim_x(2,i), anim_x(3,i), ...
R_anim(1,2)*axis_length, R_anim(2,2)*axis_length, R_anim(3,2)*axis_length, ...
'g', 'LineWidth', 2, 'MaxHeadSize', 0.5);
quiver3(anim_x(1,i), anim_x(2,i), anim_x(3,i), ...
R_anim(1,3)*axis_length, R_anim(2,3)*axis_length, R_anim(3,3)*axis_length, ...
'b', 'LineWidth', 2, 'MaxHeadSize', 0.5);
% 设置图形属性
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title(sprintf('四旋翼飞行动画 (t = %.1f s)', anim_time(i)));
legend('Location', 'best');
grid on; axis equal;
xlim([min(xd(1,:))-1, max(xd(1,:))+1]);
ylim([min(xd(2,:))-1, max(xd(2,:))+1]);
zlim([0, max(xd(3,:))+1]);
view(45, 30);
drawnow;
% 暂停，控制动画速度
pause(0.01);
end
end
%% 9. 鲁棒性测试
fprintf('进行鲁棒性测试...\n');
% 测试不同扰动下的控制器性能
test_disturbance = true;
if test_disturbance
% 复制参数
params_dist = params;
% 添加质量不确定性（+20%）
params_dist.m = params.m * 1.2;
% 重新初始化状态
x_dist = zeros(3, N+1);
v_dist = zeros(3, N+1);
phi_dist = zeros(1, N+1);
theta_dist = zeros(1, N+1);
psi_dist = zeros(1, N+1);
% 重新运行仿真（简化版本）
fprintf(' 运行质量扰动测试...\n');
for k = 1:N
% 简化的控制律（使用相同PID参数）
error_x = xd(1,k) - x_dist(1,k);
error_y = xd(2,k) - x_dist(2,k);
error_z = xd(3,k) - x_dist(3,k);
ax_cmd = Kp_x * error_x;
ay_cmd = Kp_y * error_y;
az_cmd = Kp_z * error_z;
U1_dist = params_dist.m * (params_dist.g + az_cmd);
thetad_dist = (1/params_dist.g) * (ax_cmd * sin(psid(k)) - ay_cmd * cos(psid(k)));
phid_dist = (1/params_dist.g) * (ax_cmd * cos(psid(k)) + ay_cmd * sin(psid(k)));
% 简化的动力学更新
x_dist(:,k+1) = x_dist(:,k) + v_dist(:,k) * dt;
v_dist(1,k+1) = v_dist(1,k) + (U1_dist/params_dist.m * (thetad_dist*cos(psid(k)) - phid_dist*sin(psid(k)))) * dt;
v_dist(2,k+1) = v_dist(2,k) + (U1_dist/params_dist.m * (thetad_dist*sin(psid(k)) + phid_dist*cos(psid(k)))) * dt;
v_dist(3,k+1) = v_dist(3,k) + (U1_dist/params_dist.m - params_dist.g) * dt;
end
% 绘制比较结果
figure('Position', [100, 100, 1200, 300]);
subplot(1, 3, 1);
plot(time, xd(1,:), 'k--', 'LineWidth', 2, 'DisplayName', '期望');
hold on;
plot(time, x(1,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '标称质量');
plot(time, x_dist(1,:), 'r-', 'LineWidth', 1.5, 'DisplayName', '质量+20%');
xlabel('时间 (s)'); ylabel('X位置 (m)');
title('X轴位置（质量扰动）');
legend; grid on;
subplot(1, 3, 2);
plot(time, xd(2,:), 'k--', 'LineWidth', 2, 'DisplayName', '期望');
hold on;
plot(time, x(2,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '标称质量');
plot(time, x_dist(2,:), 'r-', 'LineWidth', 1.5, 'DisplayName', '质量+20%');
xlabel('时间 (s)'); ylabel('Y位置 (m)');
title('Y轴位置（质量扰动）');
legend; grid on;
subplot(1, 3, 3);
plot(time, xd(3,:), 'k--', 'LineWidth', 2, 'DisplayName', '期望');
hold on;
plot(time, x(3,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '标称质量');
plot(time, x_dist(3,:), 'r-', 'LineWidth', 1.5, 'DisplayName', '质量+20%');
xlabel('时间 (s)'); ylabel('Z位置 (m)');
title('Z轴位置（质量扰动）');
legend; grid on;
end
%% 10. 结果总结
fprintf('\n=== 仿真结果总结 ===\n');
fprintf('仿真时间: %.1f 秒\n', T);
fprintf('时间步长: %.3f 秒\n', dt);
fprintf('总步数: %d\n', N);
fprintf('\n性能指标:\n');
fprintf(' X位置RMSE: %.4f m\n', RMSE_x);
fprintf(' Y位置RMSE: %.4f m\n', RMSE_y);
fprintf(' Z位置RMSE: %.4f m\n', RMSE_z);
fprintf(' 滚转角RMSE: %.2f°\n', rad2deg(RMSE_phi));
fprintf(' 俯仰角RMSE: %.2f°\n', rad2deg(RMSE_theta));
fprintf(' 偏航角RMSE: %.2f°\n', rad2deg(RMSE_psi));
% 计算稳定时间
threshold = 0.05; % 5cm阈值
stable_time_x = find(abs(error_x) < threshold, 1) * dt;
stable_time_y = find(abs(error_y) < threshold, 1) * dt;
stable_time_z = find(abs(error_z) < threshold, 1) * dt;
fprintf('\n稳定时间(%.2f m误差内):\n', threshold);
fprintf(' X方向: %.2f 秒\n', stable_time_x);
fprintf(' Y方向: %.2f 秒\n', stable_time_y);
fprintf(' Z方向: %.2f 秒\n', stable_time_z);
fprintf('\n=== 仿真完成 ===\n');二、简化版本（快速测试）
matlab
%% 四旋翼串级PID控制简化仿真
% 适用于快速测试和参数调整
clear; clc; close all;
% 参数设置
dt = 0.01; % 时间步长
T = 10; % 仿真时间
N = T/dt; % 总步数
time = 0:dt:T; % 时间向量
% 四旋翼参数
m = 1.2; % 质量
g = 9.81; % 重力加速度
Ixx = 0.023; % 转动惯量
Iyy = 0.023;
Izz = 0.046;
% 初始化状态
x = zeros(3, N+1); % 位置
v = zeros(3, N+1); % 速度
phi = zeros(1, N+1); % 滚转角
theta = zeros(1, N+1); % 俯仰角
psi = zeros(1, N+1); % 偏航角
% 期望轨迹
xd = zeros(3, N+1);
xd(3,:) = 2; % 保持2米高度
psid = zeros(1, N+1);
% PID参数
% 位置环
Kp_pos = 1.5; Ki_pos = 0.1; Kd_pos = 0.8;
% 姿态环
Kp_att = 6; Ki_att = 1.5; Kd_att = 2;
% 初始化误差
int_error_pos = zeros(3,1);
prev_error_pos = zeros(3,1);
int_error_att = zeros(3,1);
prev_error_att = zeros(3,1);
% 主循环
for k = 1:N
% 位置控制
error_pos = xd(:,k) - x(:,k);
int_error_pos = int_error_pos + error_pos * dt;
derror_pos = (error_pos - prev_error_pos) / dt;
acc_cmd = Kp_pos * error_pos + Ki_pos * int_error_pos + Kd_pos * derror_pos;
prev_error_pos = error_pos;
% 计算期望姿态
U1 = m * (g + acc_cmd(3));
thetad = (1/g) * (acc_cmd(1) * sin(psid(k)) - acc_cmd(2) * cos(psid(k)));
phid = (1/g) * (acc_cmd(1) * cos(psid(k)) + acc_cmd(2) * sin(psid(k)));
% 姿态控制
att = [phi(k); theta(k); psi(k)];
att_d = [phid; thetad; psid(k)];
error_att = att_d - att;
int_error_att = int_error_att + error_att * dt;
derror_att = (error_att - prev_error_att) / dt;
tau_cmd = Kp_att * error_att + Ki_att * int_error_att + Kd_att * derror_att;
prev_error_att = error_att;
% 简化的动力学更新
x(:,k+1) = x(:,k) + v(:,k) * dt;
v(1,k+1) = v(1,k) + (U1/m * (thetad*cos(psid(k)) - phid*sin(psid(k)))) * dt;
v(2,k+1) = v(2,k) + (U1/m * (thetad*sin(psid(k)) + phid*cos(psid(k)))) * dt;
v(3,k+1) = v(3,k) + (U1/m - g) * dt;
phi(k+1) = phi(k) + tau_cmd(1)/Ixx * dt;
theta(k+1) = theta(k) + tau_cmd(2)/Iyy * dt;
psi(k+1) = psi(k) + tau_cmd(3)/Izz * dt;
end
% 绘制结果
figure;
subplot(2,2,1);
plot(time, xd(3,:), 'r--', 'LineWidth', 2); hold on;
plot(time, x(3,:), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('高度 (m)');
title('高度跟踪');
legend('期望', '实际'); grid on;
subplot(2,2,2);
plot(time, rad2deg(phi), 'r-', 'LineWidth', 1.5); hold on;
plot(time, rad2deg(theta), 'b-', 'LineWidth', 1.5);
plot(time, rad2deg(psi), 'g-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('角度 (°)');
title('姿态角');
legend('\phi', '\theta', '\psi'); grid on;
subplot(2,2,3);
plot(x(1,:), x(2,:), 'b-', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)');
title('水平轨迹'); grid on; axis equal;
subplot(2,2,4);
error_z = xd(3,:) - x(3,:);
plot(time, error_z, 'k-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('高度误差 (m)');
title('跟踪误差'); grid on;三、关键知识点说明
3.1 串级PID控制结构
期望位置 → 位置控制器 → 期望姿态 → 姿态控制器 → 控制力矩 → 四旋翼动力学 → 实际状态
↑ ↓
└───────── 反馈 ───────────────────────┘3.2 控制分配矩阵
[U1] [ k k k k ] [w1²]
[U2] = [ 0 -kL 0 kL] * [w2²]
[U3] [-kL 0 kL 0] [w3²]
[U4] [ b -b b -b] [w4²]其中：
- U1: 总升力
- U2: 滚转力矩
- U3: 俯仰力矩
- U4: 偏航力矩
- k: 升力系数
- L: 机臂长度
- b: 阻力系数
3.3 旋转矩阵（ZYX欧拉角）
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θ]参考 四旋翼串级PID控制算法的MATLAB仿真 www.youwenfan.com/contentcsu/59780.html
四、参数整定建议
4.1 位置环PID整定
- 先调Z轴：单独调试高度控制
- 再调XY平面：调试水平位置控制
- 整定顺序：P → D → I
- 经验值：Kp=1~3, Ki=0.1~0.5, Kd=0.5~2
4.2 姿态环PID整定
- 先调偏航角：通常偏航角响应较慢
- 再调滚转和俯仰：这两个通常对称
- 整定顺序：P → D → I
- 经验值：Kp=5~10, Ki=1~3, Kd=1~3
4.3 常见问题及解决
| 现象 | 可能原因 | 解决方法 |
|---|---|---|
| 振荡 | P过大 | 减小P值 |
| 响应慢 | P过小 | 增大P值 |
| 稳态误差 | I过小 | 增大I值 |
| 超调大 | D过小 | 增大D值 |
| 不稳定 | 参数过大 | 减小所有参数 |
五、扩展功能
5.1 添加风扰
matlab
% 在动力学方程中添加风扰
wind_force = [0.5*sin(0.5*time(k)); 0.3*cos(0.3*time(k)); 0];
dxdt(4:6,1) = (1/params.m) * (R * [0; 0; F]) - g + wind_force;5.2 电机饱和限制
matlab
% 限制电机转速
w_max = 700; % 最大转速
w_min = 100; % 最小转速
w(:,k) = max(min(w(:,k), w_max), w_min);5.3 轨迹跟踪性能评估
matlab
% 计算轨迹跟踪性能指标
function metrics = evaluate_performance(time, xd, x)
error = xd - x;
% 均方根误差
RMSE = sqrt(mean(error.^2, 2));
% 最大误差
max_error = max(abs(error), [], 2);
% 稳定时间（进入5cm误差带的时间）
threshold = 0.05;
stable_time = zeros(3,1);
for i = 1:3
idx = find(abs(error(i,:)) < threshold, 1);
if ~isempty(idx)
stable_time(i) = time(idx);
else
stable_time(i) = inf;
end
end
metrics.RMSE = RMSE;
metrics.max_error = max_error;
metrics.stable_time = stable_time;
end