四旋翼无人机 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 纯代码示例（核心循环）

%% 参数设置
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 绘制飞行轨迹和姿态