# 四旋翼无人机 PID 控制仿真实现指南
四旋翼动力学模型、串级PID控制器设计和 MATLAB/Simulink 实现步骤。
—
## 1. 四旋翼动力学模型（简化版）
### 1.1 坐标系与变量定义
– 机体坐标系：x向前，y向右，z向上（右手系）
– 状态向量：
$$X = [x,y,z,\phi,\theta,\psi,\dot x,\dot y,\dot z,p,q,r]^T$$
– 控制输入：四个电机转速平方 $U = [\Omega_1^2,\Omega_2^2,\Omega_3^2,\Omega_4^2]^T$
或映射为总推力 $T$ 和三个力矩 $\tau_\phi,\tau_\theta,\tau_\psi$
### 1.2 力和力矩方程（刚体+旋翼空气动力）
**平动动力学**（机体系→惯性系）：
$$
\begin{bmatrix}
\ddot x \\ \ddot y \\ \ddot z
\end{bmatrix}
=
\frac{1}{m}
\begin{bmatrix}
\cos\phi\sin\theta\cos\psi+\sin\phi\sin\psi \\
\cos\phi\sin\theta\sin\psi-\sin\phi\cos\psi \\
\cos\phi\cos\theta
\end{bmatrix}
T
–
\begin{bmatrix}
0\\0\\g
\end{bmatrix}
–
\frac{k_d}{m}\begin{bmatrix}\dot x\\\dot y\\\dot z\end{bmatrix}
$$
**转动动力学**（假设小角度近似，或使用四元数避免奇点）：
$$
\begin{aligned}
\ddot\phi &= \frac{I_y-I_z}{I_x}\dot\theta\dot\psi + \frac{\tau_\phi}{I_x} – \frac{J_r}{I_x}\dot\theta\Omega_r \\
\ddot\theta &= \frac{I_z-I_x}{I_y}\dot\phi\dot\psi + \frac{\tau_\theta}{I_y} + \frac{J_r}{I_y}\dot\phi\Omega_r \\
\ddot\psi &= \frac{I_x-I_y}{I_z}\dot\phi\dot\theta + \frac{\tau_\psi}{I_z}
\end{aligned}
$$
其中 $\Omega_r = \Omega_1-\Omega_2+\Omega_3-\Omega_4$ 为转子相对转速引起的陀螺效应。
### 1.3 控制分配矩阵（Mixer）
给定期望的 $[T,\tau_\phi,\tau_\theta,\tau_\psi]$，反解各电机转速：
$$
\begin{bmatrix}
T \\ \tau_\phi \\ \tau_\theta \\ \tau_\psi
\end{bmatrix}
=
\begin{bmatrix}
k_T & k_T & k_T & k_T \\
0 & -l k_T & 0 & l k_T \\
-l k_T & 0 & l k_T & 0 \\
k_D & -k_D & k_D & -k_D
\end{bmatrix}
\begin{bmatrix}
\Omega_1^2 \\ \Omega_2^2 \\ \Omega_3^2 \\ \Omega_4^2
\end{bmatrix}
$$
其中 $l$ 为臂长，$k_T$ 推力系数，$k_D$ 扭矩系数。
—
## 2. PID 控制结构：串级控制
### 2.1 整体框图
“`
位置期望 ──► [位置PID] ──► 姿态角期望 ──► [姿态PID] ──► 力矩/推力 ──► 混控器 ──► 电机 ──► 四旋翼
▲ ▲ ▲
└── 位置反馈 ──────────────┘ │
└── IMU/传感器
“`
### 2.2 位置控制器（外环）
– 输入：$[x_d,y_d,z_d]$ 与当前 $[x,y,z]$
– 输出：期望滚转角 $\phi_d$、俯仰角 $\theta_d$、偏航角 $\psi_d$（通常偏航单独控制）以及总推力 $T_d$
**水平位置控制**（通过倾斜产生水平加速度）：
$$
\begin{aligned}
\ddot x_d &= K_{p,x}(x_d-x) + K_{i,x}\int (x_d-x)dt + K_{d,x}(\dot x_d-\dot x) \\
\ddot y_d &= K_{p,y}(y_d-y) + K_{i,y}\int (y_d-y)dt + K_{d,y}(\dot y_d-\dot y)
\end{aligned}
$$
然后转换为姿态角（小角度近似）：
$$
\begin{aligned}
\phi_d &= +\frac{1}{g}(\ddot x_d\sin\psi_d – \ddot y_d\cos\psi_d) \\
\theta_d &= -\frac{1}{g}(\ddot x_d\cos\psi_d + \ddot y_d\sin\psi_d)
\end{aligned}
$$
**高度控制**：
$$
T_d = m\left(g + K_{p,z}(z_d-z) + K_{i,z}\int (z_d-z)dt + K_{d,z}(\dot z_d-\dot z)\right)
$$
### 2.3 姿态控制器（内环）
– 输入：$\phi_d,\theta_d,\psi_d$ 与当前 $\phi,\theta,\psi$
– 输出：$\tau_\phi,\tau_\theta,\tau_\psi$
以滚转为例：
$$
\tau_\phi = K_{p,\phi}(\phi_d-\phi) + K_{i,\phi}\int (\phi_d-\phi)dt + K_{d,\phi}(p_d-p)
$$
其中 $p_d$ 可由期望角速率得到，或直接微分角度误差。
> **注意**：姿态内环带宽应比外环高 3~5 倍，否则易振荡。
—
## 3. MATLAB/Simulink 仿真实现
### 3.1 Simulink 模型结构（推荐）
“`
[Desired Trajectory] ──► [Position Controller] ──► [Attitude Controller] ──► [Control Allocation] ──► [Quadrotor Dynamics] ──► [Sensor Model]
▲ │
└──────────────────────────────────────────────────────────────────────────┘
“`
关键模块：
– **Quadrotor Dynamics**：用 S-function 或连续状态空间模块实现上述微分方程
– **Sensor Model**：添加高斯噪声和低通滤波模拟IMU
– **Controller**：离散PID，采样率 100~400 Hz
### 3.2 MATLAB 纯代码示例（核心循环）
“`matlab
%% 参数设置
m = 1.5; g = 9.81; Ixx = 0.01; Iyy = 0.01; Izz = 0.02; l = 0.25;
kT = 1.0e-5; kD = 1.0e-6; Jr = 1.0e-5; kd = 0.01;
% PID增益（需调参）
Kp_xy = 2; Ki_xy = 0; Kd_xy = 1.5;
Kp_z = 4; Ki_z = 0.2; Kd_z = 2;
Kp_att = [30,30,20]; Ki_att = [0,0,0]; Kd_att = [8,8,5];
% 初始状态
state = zeros(12,1); % [x,y,z,phi,theta,psi,xd,yd,zd,p,q,r]
dt = 0.005; t_end = 20;
for t = 0:dt:t_end
%% 期望轨迹（例如悬停或圆轨迹）
pos_des = [2*sin(0.5*t); 2*cos(0.5*t); 2];
vel_des = [cos(0.5*t); -sin(0.5*t); 0];
%% 位置控制器
pos_err = pos_des – state(1:3);
vel_err = vel_des – state(7:9);
acc_des = Kp_xy.*pos_err(1:2) + Kd_xy.*vel_err(1:2); % 水平
acc_des(3) = Kp_z*pos_err(3) + Kd_z*vel_err(3);
% 积分项（用累加实现）
integral_pos = integral_pos + pos_err*dt;
acc_des = acc_des + Ki_xy.*integral_pos(1:2);
acc_des(3) = acc_des(3) + Ki_z*integral_pos(3);
% 转换为期望姿态和推力
psi_des = 0; % 偏航固定
phi_des = (acc_des(1)*sin(psi_des) – acc_des(2)*cos(psi_des))/g;
theta_des = -(acc_des(1)*cos(psi_des) + acc_des(2)*sin(psi_des))/g;
T_des = m*(g + acc_des(3));
%% 姿态控制器
att_err = [phi_des-state(4); theta_des-state(5); psi_des-state(6)];
rate_err = -state(10:12); % 期望角速率=0
torque = Kp_att’.*att_err + Kd_att’.*rate_err;
%% 控制分配 -> 电机转速
mix_mat = [kT,kT,kT,kT; 0,-l*kT,0,l*kT; -l*kT,0,l*kT,0; kD,-kD,kD,-kD];
U = mix_mat \ [T_des; torque]; % U = [Ω1^2; Ω2^2; Ω3^2; Ω4^2]
Omega = sqrt(max(U,0)); % 确保非负
%% 动力学更新（欧拉法）
state = quadrotor_dynamics(state, Omega, dt, m,g,Ixx,Iyy,Izz,l,kT,kD,Jr,kd);
%% 记录数据
history(:,end+1) = state;
end
“`
其中 `quadrotor_dynamics` 函数实现前面给出的微分方程。
—
## 4. PID 参数整定口诀
| 层级 | 先调参数 | 后调参数 | 技巧 |
| ——– | ————– | —————— | ———————————————————— |
| 姿态内环 | $K_p$（比例） | $K_d$（微分） | 先只给P，让系统轻微振荡，此时P约为临界值的60%；再加D消除振荡 |
| 高度外环 | $K_p$ 和 $K_d$ | $K_i$ | 高度积分只在有稳态误差时才加，且限幅防止windup |
| 水平位置 | $K_p$ 和 $K_d$ | 无积分（除非有风） | 位置环带宽应低于姿态环3倍以上，否则容易耦合振荡 |
常见初值（以1.5kg四旋翼为例）：
– 姿态：$K_p=[20,20,15],\ K_d=[5,5,3]$
– 位置：$K_p=[1.5,1.5,3],\ K_d=[1,1,1.5]$
参考代码 利用PID实现四旋翼仿真实现以及GUI界面 www.youwenfan.com/contentcnv/81387.html
## 5. 常见问题与解决
| 问题 | 原因 | 对策 |
| ————– | ————————- | ———————————— |
| 起飞时剧烈抖动 | 姿态环P过大或D不足 | 降低姿态P，增加D |
| 悬停时高度漂移 | 缺少积分或推力补偿不准 | 加入Ki，或使用气压计/声纳融合 |
| 水平跟踪滞后 | 位置环P太小或速度前馈缺失 | 增加位置P，或加入期望速度前馈 |
| 偏航响应慢 | 偏航力矩较小 | 单独提高偏航通道增益，或使用更大电机 |
—
## 6. 进阶建议
– **考虑执行器饱和**：在PID输出后限幅，并加入抗积分饱和（anti-windup）
– **使用四元数代替欧拉角**：避免万向锁，适合大机动仿真
– **添加风扰模型**：在平动动力学中加入随机力
– **可视化**：使用 `plot3` 或 `quiver` 绘制飞行轨迹和姿态
