一、仿真代码
%% 四旋翼串级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+1t = 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)) == 0fprintf(' 进度: %.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:6h.FaceColor = 'flat';h.CData(i,:) = bar_colors(i,:);
endylabel('RMSE (m 或 °)');
title('性能指标(RMSE)');
set(gca, 'XTick', 1:6, 'XTickLabel', bar_labels);
grid on;% 添加数值标签
for i = 1:6if i <= 3text(i, bar_data(i), sprintf('%.3f', bar_data(i)), ...'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom');elsetext(i, bar_data(i), sprintf('%.2f°', bar_data(i)), ...'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom');end
end%% 8. 动画演示
fprintf('生成飞行轨迹动画...\n');animate_quadrotor = true;
if animate_quadrotorfigure('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:4prop_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');
二、简化版本（快速测试）
%% 四旋翼串级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/contentcnu/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 添加风扰
% 在动力学方程中添加风扰
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 电机饱和限制
% 限制电机转速
w_max = 700; % 最大转速
w_min = 100; % 最小转速
w(:,k) = max(min(w(:,k), w_max), w_min);
5.3 轨迹跟踪性能评估
% 计算轨迹跟踪性能指标
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:3idx = find(abs(error(i,:)) < threshold, 1);if ~isempty(idx)stable_time(i) = time(idx);elsestable_time(i) = inf;endendmetrics.RMSE = RMSE;metrics.max_error = max_error;metrics.stable_time = stable_time;
end