1. 四旋翼无人机轨迹跟踪控制入门
四旋翼无人机（UAV）作为当前最流行的飞行器平台之一，其控制系统的设计一直是研究热点。轨迹跟踪控制作为无人机自主飞行的基础功能，要求无人机能够精确跟踪预设的三维空间轨迹。PD控制器因其结构简单、参数物理意义明确，成为初学者理解无人机控制的理想切入点。
我在实验室带本科生做无人机项目时发现，很多同学在刚接触飞行控制时，往往会被复杂的动力学模型吓退。实际上，通过Matlab/Simulink搭建仿真环境，可以直观地理解PD控制器的调节过程。本文将分享如何从零开始构建完整的仿真系统，包括动力学建模、控制器设计、参数整定等关键环节。
2. 系统建模与问题分析
2.1 四旋翼动力学模型简化
四旋翼的完整动力学模型包含6自由度（位置x,y,z和姿态φ,θ,ψ）的非线性耦合方程。为简化分析，我们通常做以下合理假设：
- 机体对称且质量分布均匀
- 忽略空气阻力矩和陀螺效应
- 小角度近似（俯仰/横滚角<10°）
基于牛顿-欧拉方程，可得到简化后的动力学模型：
matlab复制% 位置动力学方程
ddot_x = (sinψ*sinφ + cosψ*sinθ*cosφ)*U1/m
ddot_y = (-cosψ*sinφ + sinψ*sinθ*cosφ)*U1/m
ddot_z = g - (cosθ*cosφ)*U1/m
% 姿态动力学方程
ddot_φ = (Iyy-Izz)/Ixx * dotθ*dotψ + U2/Ixx
ddot_θ = (Izz-Ixx)/Iyy * dotφ*dotψ + U3/Iyy
ddot_ψ = (Ixx-Iyy)/Izz * dotφ*dotθ + U4/Izz
2.2 控制分配问题
四旋翼通过调节四个电机的转速产生控制输入：
- U1：总升力（垂直方向）
- U2：横滚力矩
- U3：俯仰力矩
- U4：偏航力矩
控制分配矩阵将控制器输出的虚拟控制量转换为各电机PWM信号：
code复制[U1; U2; U3; U4] = [1 1 1 1;
0 -l 0 l;
-l 0 l 0;
-c c -c c] * [ω1²; ω2²; ω3²; ω4²]
提示：实际项目中需考虑电机动力学延迟，通常增加一阶惯性环节模拟该特性。
3. PD控制器设计与实现
3.1 串级控制结构
四旋翼控制通常采用内外环串级结构：
- 外环位置控制：生成期望姿态角
- 内环姿态控制：跟踪期望姿态
mermaid复制graph TD
A[轨迹发生器] --> B[位置控制器]
B --> C[姿态角生成]
C --> D[姿态控制器]
D --> E[控制分配]
E --> F[电机模型]
3.2 位置环PD设计
以高度z控制为例，PD控制律为：
matlab复制e_z = z_des - z_actual;
U1 = m*(g + Kp_z*e_z + Kd_z*(de_z/dt))/cosθcosφ;
参数整定经验：
- 先调Kp使系统有响应但不振荡
- 再调Kd抑制超调
- 典型初始值：Kp=1.5~3.0, Kd=0.8~1.5
3.3 姿态环PD设计
横滚角φ控制示例：
matlab复制e_φ = φ_des - φ_actual;
U2 = Ixx*(Kp_φ*e_φ + Kd_φ*(de_φ/dt));
注意：姿态环响应速度应比位置环快5-10倍，通常Kp_φ=8~15，Kd_φ=2~5
4. Simulink仿真实现
4.1 模型架构设计
完整仿真模型包含以下子系统：
- 轨迹生成器（Signal Builder或MATLAB Function）
- 位置控制器（PD模块）
- 姿态控制器（PD模块）
- 动力学模型（6DOF Eular Equations）
- 传感器模型（添加白噪声）
- 可视化模块（Aerospace Blockset）
4.2 关键模块配置
姿态控制器子系统的实现示例：
matlab复制function [U2,U3,U4] = attitude_control(phi_d,theta_d,psi_d,phi,theta,psi,p,q,r)
persistent last_error
if isempty(last_error)
last_error = zeros(3,1);
end
Kp = diag([12, 12, 8]);
Kd = diag([3, 3, 2]);
error = [phi_d - phi; theta_d - theta; psi_d - psi];
error_deriv = [p; q; r];
U = Kp*error + Kd*error_deriv;
last_error = error;
U2 = U(1); U3 = U(2); U4 = U(3);
4.3 参数调试技巧
-
使用PID Tuner工具自动整定
-
阶跃响应调试法：
- 给0.5m阶跃指令
- 观察上升时间、超调量
- 调整Kp使上升时间≈2s
- 调整Kd使超调<10%
-
频域分析法：
- 检查开环Bode图
- 相位裕度应>45°
- 截止频率位置环1-2rad/s，姿态环10-20rad/s
5. 典型问题与解决方案
5.1 电机饱和现象
症状：跟踪大角度指令时控制失效
解决方法：
- 限制期望姿态角范围（|φ|,|θ|<30°）
- 加入抗饱和补偿算法
- 优化轨迹生成（减小加速度）
5.2 耦合振荡问题
症状：调节x位置引起z方向震荡
排查步骤：
- 检查姿态环响应速度
- 验证小角度近似是否成立
- 添加前馈补偿项：
matlab复制U1_ff = m*g/(cosθ_d*cosφ_d);
5.3 传感器噪声影响
改进方案：
- 添加低通滤波器（截止频率20Hz）
- 采用状态观测器（如卡尔曼滤波）
- 硬件层面选择更高精度IMU
6. 进阶优化方向
当基础PD控制器实现稳定跟踪后，可考虑以下改进：
-
自适应PD：根据跟踪误差动态调整参数
matlab复制
Kp_adapt = Kp_base + α*|e|; -
模糊PD：用模糊规则调整参数
matlab复制if abs(e) large and abs(de/dt) small then Kp++ -
轨迹预测：加入前馈控制提高跟踪精度
-
鲁棒性增强：添加扰动观测器补偿模型不确定性
我在实际项目中发现，对于大多数教育级无人机，经过精心调试的PD控制器配合合理的轨迹规划，已经能够满足90%的应用场景需求。关键在于理解每个参数的物理意义，并通过系统化的调试方法找到最佳组合。