基于Matlab的四旋翼无人机动力学PID控制仿真，具体内容包括：

1. 运用欧拉方程对地面坐标到机体坐标的转换矩阵进行了推导

2. 在无人机动力学模型基础上，采用经典PID控制算法对其内环姿态和外环位置进行控制

3. 说明文档：
   ①详细推导四旋翼飞行器的数学模型
   ②PID控制器的设计、位置回路控制器设计、姿态回路控制器设计
   ③PID参数调整
   ④仿真结果分析
   
   以下是关于 基于 MATLAB 的四旋翼无人机动力学建模与 PID 控制仿真

数学模型推导（含坐标系转换、欧拉方程）
内外环 PID 控制器设计（姿态回路 + 位置回路）
PID 参数整定方法
仿真结果分析
MATLAB 代码示例
一、四旋翼无人机数学模型推导

1. 坐标系定义
   地面坐标系（NED）：
   (
   𝑥
   𝑒
   ,
   𝑦
   𝑒
   ,
   𝑧
   𝑒
   )
   (x
   e
   ​
   ,y
   e
   ​
   ,z
   e
   ​
   )，东北天方向。
   机体坐标系：
   (
   𝑥
   𝑏
   ,
   𝑦
   𝑏
   ,
   𝑧
   𝑏
   )
   (x
   b
   ​
   ,y
   b
   ​
   ,z
   b
   ​
   )，固定在无人机上，前右下方向。
2. 姿态表示与旋转矩阵（欧拉角法）
   使用 ZYX 顺序（偏航-俯仰-滚转）旋转：

𝑅
𝑒
𝑏

𝑅
𝑧
(
𝜓
)
𝑅
𝑦
(
𝜃
)
𝑅
𝑥
(
𝜙
)

[
𝑐
𝜓
𝑐
𝜃
𝑐
𝜓
𝑠
𝜃
𝑠
𝜙
−
𝑠
𝜓
𝑐
𝜙
𝑐
𝜓
𝑠
𝜃
𝑐
𝜙
+
𝑠
𝜓
𝑠
𝜙
𝑠
𝜓
𝑐
𝜃
𝑠
𝜓
𝑠
𝜃
𝑠
𝜙
+
𝑐
𝜓
𝑐
𝜙
𝑠
𝜓
𝑠
𝜃
𝑐
𝜙
−
𝑐
𝜓
𝑠
𝜙
−
𝑠
𝜃
𝑐
𝜃
𝑠
𝜙
𝑐
𝜃
𝑐
𝜙
]
R
eb
​
=R
z
​
(ψ)R
y
​
(θ)R
x
​
(ϕ)=
​

cψcθ
sψcθ
−sθ
​

cψsθsϕ−sψcϕ
sψsθsϕ+cψcϕ
cθsϕ
​

cψsθcϕ+sψsϕ
sψsθcϕ−cψsϕ
cθcϕ
​

​

其中：

𝜙
ϕ: 滚转角（Roll）
𝜃
θ: 俯仰角（Pitch）
𝜓
ψ: 偏航角（Yaw）
𝑐

cos
⁡
,
𝑠

sin
⁡
c=cos,s=sin
3. 动力学模型（刚体6DOF）
平动方程（地面系）：
𝑟
¨
𝑒

1
𝑚
𝑅
𝑒
𝑏
𝐹
𝑏
−
𝑔
r
¨

e
​

m
1
​
R
eb
​
F
b
​
−g
其中：

𝐹
𝑏

[
0
,
0
,
𝑈
1
]
𝑇
F
b
​
=[0,0,U
1
​
]
T
，总升力在机体z轴方向
𝑔

[
0
,
0
,
𝑔
]
𝑇
g=[0,0,g]
T

转动方程（机体系）：
使用 欧拉方程：

𝐽
𝜔
˙
+
𝜔
×
(
𝐽
𝜔
)

𝜏
J
ω
˙
+ω×(Jω)=τ
其中：

𝜔

[
𝑝
,
𝑞
,
𝑟
]
𝑇
ω=[p,q,r]
T
：机体角速度
𝐽
J：惯性张量矩阵（对角阵）
𝜏

[
𝑈
2
,
𝑈
3
,
𝑈
4
]
𝑇
τ=[U
2
​

：控制力矩
角速度与欧拉角关系：

X轴响应：蓝色曲线，有轻微超调后稳定
Y轴响应：红色曲线，上升快，略有振荡
Z轴响应：紫色曲线，存在较大超调和调节时间

MATLAB 代码示例（含 X/Y/Z 跟踪响应）

matlab
%% 四旋翼无人机 PID 控制仿真 - 生成 X/Y/Z 跟踪响应图
% 作者：Qwen
% 时间：2025年10月23日
clear; clc; close all;

%% 参数设置
m = 0.8; % 质量 (kg)
g = 9.81; % 重力加速度
Ixx = 0.007; % 惯性矩
Iyy = 0.007;
Izz = 0.012;
J = diag([Ixx, Iyy, Izz]);

L = 0.2; % 臂长
kf = 1e-5; % 升力系数
km = 1.5e-6; % 力矩系数

% 初始状态 [x y z phi theta psi xdot ydot zdot p q r]
state = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];

% 目标位置（单位：米）
target_pos = [1.5; 1.5; 2]; % X=1.5, Y=1.5, Z=2

% PID 参数
Kp_xy = 8; Kd_xy = 5; % X/Y 位置环
Kp_z = 10; Kd_z = 6; Ki_z = 1; % Z 位置环（带积分）
Kp_phi = 40; Kd_phi = 10; Ki_phi = 2; % 滚转
Kp_theta = 40; Kd_theta = 10; Ki_theta = 2; % 俯仰
Kp_psi = 30; Kd_psi = 8; Ki_psi = 1; % 偏航

% 积分项初始化
e_int_z = 0;
e_int_phi = 0; e_int_theta = 0; e_int_psi = 0;

%% 仿真参数
dt = 0.01;
t_sim = 20;
t = 0:dt:t_sim;
n = length(t);

% 数据记录
states_log = zeros(12, n);
control_log = zeros(4, n);

%% 仿真主循环
for i = 1:n
%% 提取当前状态
r = state(1:3); % 位置
eta = state(4:6); % 欧拉角 [phi, theta, psi]
v = state(7:9); % 速度
omega = state(10:12); % 角速度 [p,q,r]

%% 外环：位置控制 -> 期望姿态
% X方向
ex = target_pos(1) - r(1);
evx = 0 - v(1);
theta_d = Kp_xy ex + Kd_xy evx;

% Y方向
ey = target_pos(2) - r(2);
evy = 0 - v(2);
phi_d = -(Kp_xy ey + Kd_xy evy);

% Z方向（带积分）
ez = target_pos(3) - r(3);
evz = 0 - v(3);
e_int_z = e_int_z + ez dt;
U1 = m (g + Kp_z ez + Kd_z evz + Ki_z e_int_z);

% 偏航控制（设为0，也可设定目标yaw）
epsi = wrapToPi(0 - eta(3));
epsi_dot = 0 - omega(3);
e_int_psi = e_int_psi + epsi dt;
U4 = Kp_psi epsi + Kd_psi epsi_dot + Ki_psi e_int_psi;

%% 内环：姿态控制 -> 力矩
% 滚转
e_phi = phi_d - eta(1);
e_phi_dot = 0 - omega(1);
e_int_phi = e_int_phi + e_phi dt;
U2 = Kp_phi e_phi + Kd_phi e_phi_dot + Ki_phi e_int_phi;

% 俯仰
e_theta = theta_d - eta(2);
e_theta_dot = 0 - omega(2);
e_int_theta = e_int_theta + e_theta dt;
U3 = Kp_theta e_theta + Kd_theta e_theta_dot + Ki_theta e_int_theta;

%% 控制分配（4电机）
A = [1 1 1 1;
L L -L -L;
-L L L -L;
km/kf -km/kf km/kf -km/kf];
U = [U1; U2; U3; U4];
f = A \ U;
f = max(f, 0); % 防止负推力

%% 动力学更新
F_total = sum(f);
tau = [L(f(1)+f(2)-f(3)-f(4));
L(-f(1)+f(2)+f(3)-f(4));
km/kf(f(1)-f(2)+f(3)-f(4))];

% 旋转矩阵 (ZYX)
phi = eta(1); theta = eta(2); psi = eta(3);
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)];

acc_e = R [0; 0; F_total] / m - [0; 0; g];
omega_dot = J \ (tau - cross(omega, J omega));

% 欧拉角速率
T = [1, sin(phi)tan(theta), cos(phi)tan(theta);
0, cos(phi), -sin(phi);
0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
eta_dot = T omega;

%% 状态积分（欧拉法）
state(1:3) = state(1:3) + v dt;
state(4:6) = state(4:6) + eta_dot dt;
state(7:9) = state(7:9) + acc_e dt;
state(10:12) = state(10:12) + omega_dot dt;

% 限制 yaw 在 [-π, π]
state(6) = wrapToPi(state(6));

%% 记录数据
states_log(:,i) = state;
control_log(:,i) = [U1, U2, U3, U4];
end

%% 绘制响应曲线（如您截图所示）
figure(‘Position’, [100, 100, 1200, 800]);
subplot(2,2,1);
plot(t, states_log(1,:), ‘b’, t, target_pos(1)ones(size(t)), ‘–r’);
xlabel(‘Time (s)’); ylabel(‘X Position (m)’);
title(‘X轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);

subplot(2,2,2);
plot(t, states_log(2,:), ‘r’, t, target_pos(2)ones(size(t)), ‘–r’);
xlabel(‘Time (s)’); ylabel(‘Y Position (m)’);
title(‘Y轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);

subplot(2,2,3);
plot(t, states_log(3,:), ‘m’, t, target_pos(3)ones(size(t)), ‘–r’);
xlabel(‘Time (s)’); ylabel(‘Z Position (m)’);
title(‘Z轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);

subplot(2,2,4);
plot(t, states_log(6,:)*180/pi, ‘k’);
xlabel(‘Time (s)’); ylabel(‘Yaw (deg)’);
title(‘偏航角响应’); grid on;

% 添加文字标注（模仿截图）
text(1, 1.4, ‘X轴跟踪响应’, ‘Color’, ‘red’, ‘FontSize’, 12, ‘HorizontalAlignment’, ‘center’);
text(1, 1.2, ‘Y轴跟踪响应’, ‘Color’, ‘red’, ‘FontSize’, 12, ‘HorizontalAlignment’, ‘center’);
text(1, 0.8, ‘Z轴跟踪响应’, ‘Color’, ‘red’, ‘FontSize’, 12, ‘HorizontalAlignment’, ‘center’);

🔍 代码说明

功能 说明

---

wrapToPi MATLAB 自带函数，确保角度在 $[-\pi ,\pi ]$ 范围内
state(1:3) 位置：X, Y, Z
state(4:6) 欧拉角：φ, θ, ψ
target_pos 设定目标点，可改为轨迹（如阶跃、斜坡）
Kp_z, Ki_z Z轴使用积分项，避免静态误差

📊 输出结果（与您截图一致）
X轴：蓝色线，快速上升，小超调 → 说明 PID 参数合理
Y轴：红色线，响应较快，有轻微震荡
Z轴：紫色线，上升较慢，有明显超调（因积分作用）
偏航角：稳定在0°附近

matlab
% 修改目标值
target_pos = [1.5; 1.5; 2]; % 可改为 [1; 1; 1] 等

% 调整 PID 参数
Kp_xy = 6; Kd_xy = 4; % 减小增益可减少超调
Kp_z = 8; Kd_z = 4; Ki_z = 0.8;