四旋翼飞行器动力学建模与简单PID控制

四旋翼飞行器动力学建模与PID控制方案

一、动力学建模

1. 坐标系定义

• 地球坐标系 (Inertial Frame) 原点位于地面参考点，坐标轴与地理坐标系对齐，用于描述飞行器绝对位置和速度。
• 机体坐标系 (Body Frame) 原点位于质心，x轴指向机头，y轴指向右舷，z轴垂直向下，用于分析姿态变化。

2. 运动学方程

\begin{cases} \dot{X} = V_x \\ \dot{Y} = V_y \\ \dot{Z} = V_z \\ \dot{\phi} = p + q\sin\phi\tan\theta + r\cos\phi\tan\theta \\ \dot{\theta} = q\cos\phi - r\sin\phi \\ \dot{\psi} = \frac{q\sin\phi + r\cos\phi}{\cos\phi\cos\theta} \end{cases}

其中p,q,r 为机体坐标系下的角速度分量。

3. 动力学方程

牛顿-欧拉方程推导：

\begin{cases} m\dot{V} = F_g + F_{thrust} + F_{drag} \\ J\dot{\omega} = \tau_{motor} + \tau_{ext} \end{cases}

• 质量与惯性矩阵 m=0.284\,kg （典型质量），J=\text{diag}(0.0023,0.0023,0.004) （转动惯量）
• 推力模型 F_{thrust} = K_t \cdot (Ω_1^2 + Ω_2^2 + Ω_3^2 + Ω_4^2) K_t=6.5\times10^{-7}\,N\cdot s^2/rad^2 （推力系数）

4. 线性化处理

在悬停点（θ=0, φ=0, ψ=0） 进行泰勒展开，得到线性化模型：

\begin{bmatrix} \ddot{X} \\ \ddot{Y} \\ \ddot{Z} \\ \ddot{\phi} \\ \ddot{\theta} \\ \ddot{\psi} \end{bmatrix} = A \begin{bmatrix} X \\ Y \\ Z \\ \phi \\ \theta \\ \psi \end{bmatrix} + B \begin{bmatrix} Ω_1 \\ Ω_2 \\ Ω_3 \\ Ω_4 \end{bmatrix}

其中状态矩阵A和控制矩阵B可通过符号计算工具箱推导。

二、PID控制器设计

1. 控制架构

[位置环] → [速度环] → [姿态环] → [电机驱动]

• 外环（位置环）：跟踪期望位置（X,Y,Z）
• 中间环（速度环）：调节飞行器速度
• 内环（姿态环）：控制欧拉角（φ,θ,ψ）

2. 姿态控制设计

串级PID结构：

% 角度环（外环）
phi_ref = 0; theta_ref = 0; psi_ref = 0;
[phi_err, theta_err, psi_err] = angle_error(phi_ref, theta_ref, psi_ref);

% 角速度环（内环）
p_ref = Kp_phi * phi_err + Ki_phi * integral_phi + Kd_phi * derivative_phi;
q_ref = Kp_theta * theta_err + Ki_theta * integral_theta + Kd_theta * derivative_theta;
r_ref = Kp_psi * psi_err + Ki_psi * integral_psi + Kd_psi * derivative_psi;

• 典型参数：K_p=0.5, K_i=0.1, K_d=0.05（需根据实际系统调整）

3. 位置控制设计

双闭环PID实现：

% 位置环
X_ref = 10; Y_ref = 5; Z_ref = 2;
[X_err, Y_err, Z_err] = position_error(X_ref, Y_ref, Z_ref);

% 速度环
Vx_ref = Kp_x * X_err + Ki_x * integral_x + Kd_x * derivative_x;
Vy_ref = Kp_y * Y_err + Ki_y * integral_y + Kd_y * derivative_y;
Vz_ref = Kp_z * Z_err + Ki_z * integral_z + Kd_z * derivative_z;

4. 参数整定方法

三、MATLAB/Simulink实现

1. 模型搭建步骤

1. 新建模型：创建包含动力学方程和PID控制器的子系统
2. 配置求解器：选择ode45求解器，设置仿真时间（建议>10秒）
3. 添加传感器模块：模拟IMU数据（加速度计、陀螺仪）
4. 可视化设置：添加3D动画模块显示飞行姿态

2. 核心代码片段

% 四旋翼动力学模型
m = 0.284; % 质量(kg)
g = 9.81;  % 重力加速度(m/s²)
J = diag([0.0023, 0.0023, 0.004]); % 转动惯量(kg·m²)

% PID控制器参数
Kp_att = [0.5, 0.5, 0.3];  % 姿态环比例增益
Ki_att = [0.1, 0.1, 0.05]; % 姿态环积分增益
Kd_att = [0.05, 0.05, 0.03]; % 姿态环微分增益

% 仿真设置
sim('Quadcopter_PID_Model.slx');
plot(time, phi, time, theta, time, psi); % 绘制姿态角曲线

3. 仿真结果分析

四、工程优化策略

1. 抗饱和处理

% 积分项限幅
integral_phi = max(min(integral_phi, integral_max), integral_min);

% 前馈补偿
F_ff = Kff * (Phi_ref - Phi) + Kff_dot * (dPhi_ref/dt - dPhi/dt);

2. 传感器融合

• 卡尔曼滤波：融合IMU与GPS数据 [x_est, P] = kalman_filter(x, P, z, Q, R);
• 互补滤波：平衡高频噪声与低频漂移 \theta_{fusion} = \alpha \cdot \theta_{IMU} + (1-\alpha) \cdot \theta_{GPS}

3. 硬件在环测试

• dSPACE实时平台：部署PID控制器代码
• 故障注入测试：模拟电机失效、传感器噪声等场景

参考模型 四旋翼飞行器动力学建模与简单PID控制 www.youwenfan.com/contentad/50724.html