1. 四旋翼无人机建模与控制概述
四旋翼无人机作为一种典型的欠驱动系统，凭借其结构简单、机动性强等特点，在军事侦察、农业植保、电力巡检等领域获得了广泛应用。这类飞行器通过四个独立控制的旋翼实现六自由度运动，其动力学特性呈现出典型的非线性、强耦合特征。本文将基于MATLAB平台，详细解析四旋翼的离散化建模方法、增量式PID控制算法设计以及三维轨迹跟踪的实现过程。
注意：四旋翼控制系统的设计难点在于如何处理姿态与位置之间的耦合关系，以及如何应对系统固有的非线性特性。本文采用的增量PID算法能有效抑制积分饱和问题，特别适合无人机这类需要快速响应的应用场景。
2. 四旋翼动力学建模
2.1 坐标系定义与转换
建立四旋翼数学模型首先需要定义以下坐标系：
-
惯性坐标系（世界坐标系）：固定于地面，通常采用东北天(ENU)或前右下(FRD)约定。Z轴垂直地面向上，X轴指向地理北向，Y轴完成右手系。
-
机体坐标系：固连在无人机质心，X轴指向机头方向，Y轴指向右侧，Z轴向下完成右手系。旋翼位置在该坐标系中描述。
坐标系间转换通过旋转矩阵实现：
matlab复制% 欧拉角到旋转矩阵的转换函数
function R = euler2rotmat(phi, theta, psi)
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)];
end
2.2 刚体动力学方程
考虑无人机为刚体，忽略弹性变形，建立六自由度运动方程：
平移动力学：
$$
m \ddot{\boldsymbol{\xi}} = \boldsymbol{R} \begin{bmatrix}0\0\ \sum F_i\end{bmatrix} - m g \boldsymbol{e}_3
$$
其中$\boldsymbol{\xi}=[x,y,z]^T$为位置向量，$F_i$为第i个旋翼升力，$g$为重力加速度。
旋转动力学：
$$
\boldsymbol{I} \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times \boldsymbol{I} \boldsymbol{\omega} = \boldsymbol{\tau}
$$
$\boldsymbol{\omega}$为角速度向量，$\boldsymbol{\tau}$为总力矩，惯性张量$\boldsymbol{I}$需根据实际机体结构计算。
2.3 旋翼动力学特性
每个旋翼产生的升力和力矩可建模为：
$$
F_i = k_f \omega_i^2, \quad Q_i = k_m \omega_i^2
$$
其中$\omega_i$为转速，$k_f$和$k_m$分别为升力系数和扭矩系数，通过实验测定。
四个旋翼的转速组合与总控制量关系为：
$$
\begin{bmatrix}
U_1 \ U_2 \ U_3 \ U_4
\end
\begin{bmatrix}
1 & 1 & 1 & 1 \
0 & -l & 0 & l \
l & 0 & -l & 0 \
-c_\tau & c_\tau & -c_\tau & c_\tau
\end{bmatrix}
\begin{bmatrix}
F_1 \ F_2 \ F_3 \ F_4
\end{bmatrix}
$$
$l$为旋翼到质心的距离，$c_\tau$为力/力矩换算系数。
3. 离散化建模实现
3.1 连续系统离散化
采用零阶保持器法(ZOH)对连续模型离散化：
matlab复制% 系统状态空间离散化
function [Ad, Bd] = discretize_system(Ac, Bc, Ts)
n = size(Ac,1);
M = expm([Ac Bc; zeros(size(Bc,2),n+size(Bc,2))]*Ts);
Ad = M(1:n,1:n);
Bd = M(1:n,n+1:end);
end
3.2 模型参数辨识
通过实验数据辨识关键参数：
- 悬停实验确定基础升力系数
- 阶跃响应实验获取转动惯量
- 频率扫描实验验证气动导数
典型参数如表所示：
| 参数 | 物理意义 | 值 | 单位 |
|---|---|---|---|
| m | 质量 | 1.2 | kg |
| Ixx | X轴惯量 | 0.023 | kg·m² |
| Iyy | Y轴惯量 | 0.025 | kg·m² |
| Izz | Z轴惯量 | 0.029 | kg·m² |
| kf | 升力系数 | 8.5e-6 | N/(rad/s)² |
| km | 扭矩系数 | 1.6e-7 | Nm/(rad/s)² |
4. 增量PID控制器设计
4.1 标准PID与增量PID对比
增量PID算法通过计算控制量增量避免积分饱和：
$$
\Delta u(k) = K_p[e(k)-e(k-1)] + K_i e(k) + K_d[e(k)-2e(k-1)+e(k-2)]
$$
MATLAB实现核心代码：
matlab复制function [u, du] = incr_pid(e, e_prev, e_prev2, Kp, Ki, Kd)
du = Kp*(e - e_prev) + Ki*e + Kd*(e - 2*e_prev + e_prev2);
u = u_prev + du;
end
4.2 串级控制结构
采用内外环串级控制：
- 外环位置控制：生成期望姿态角
- 内环姿态控制：输出电机转速指令
控制框图如下：
code复制[位置控制器] → [姿态控制器] → [混控器] → [电机驱动]
↑ ↑ ↑
[位置反馈] [姿态反馈] [转速反馈]
4.3 参数整定方法
- 试凑法：先调P使系统快速响应，再加D抑制超调，最后用I消除静差
- 临界比例度法：逐渐增大P直至等幅振荡，按Z-N公式计算参数
- 模型参考法：基于辨识模型计算初始参数
实测PID参数范围参考：
| 控制环 | Kp | Ki | Kd |
|---|---|---|---|
| 高度 | 1.2-2.5 | 0.05-0.2 | 0.3-0.8 |
| 横滚 | 3.5-6.0 | 0.1-0.5 | 0.5-1.2 |
| 俯仰 | 3.8-6.5 | 0.1-0.5 | 0.6-1.3 |
| 偏航 | 2.0-4.0 | 0.05-0.3 | 0.4-1.0 |
5. 轨迹跟踪实现
5.1 三维路径规划
采用多项式插值生成平滑轨迹：
matlab复制% 三次样条轨迹生成
function [ref_pos, ref_vel] = gen_trajectory(waypoints, t, total_time)
n = length(waypoints)-1;
t_seg = linspace(0, total_time, n+1);
coeff = spline(t_seg, waypoints);
ref_pos = ppval(coeff, t);
ref_vel = ppval(fnder(coeff,1), t);
end
5.2 跟踪误差计算
定义误差项包括：
- 位置误差：$e_p = p_{des} - p_{act}$
- 速度误差：$e_v = v_{des} - v_{act}$
- 姿态误差：$e_R = 0.5(R_{des}^T R_{act} - R_{act}^T R_{des})^\vee$
5.3 抗干扰措施
- 干扰观测器设计：
matlab复制function d_hat = disturbance_observer(x, u, Ts) persistent x_prev d_hat_prev if isempty(x_prev) x_prev = x; d_hat_prev = zeros(size(x)); end d_hat = d_hat_prev + 0.1*(x - x_prev - Ts*(f(x_prev)+g(x_prev)*u)); x_prev = x; d_hat_prev = d_hat; end - 自适应控制增益调整
- 滑模变结构控制增强鲁棒性
6. MATLAB仿真实现
6.1 仿真框架搭建
主要模块组成：
- 无人机模型模块
- 控制器模块
- 环境扰动模块
- 可视化模块
仿真流程：
mermaid复制graph TD
A[初始化参数] --> B[生成参考轨迹]
B --> C[主仿真循环]
C --> D[传感器模型]
D --> E[控制器计算]
E --> F[动力学更新]
F --> G[数据记录]
G --> H{达到终时?}
H --否--> C
H --是--> I[结果可视化]
6.2 关键代码解析
电机混控实现：
matlab复制function [w1, w2, w3, w4] = mixer(U1, U2, U3, U4)
% 解耦控制量到电机转速
A = [1 1 1 1;
0 -1 0 1;
1 0 -1 0;
-1 1 -1 1];
F = pinv(A) * [U1; U2; U3; U4];
w1 = sqrt(max(0, F(1)/kf));
w2 = sqrt(max(0, F(2)/kf));
w3 = sqrt(max(0, F(3)/kf));
w4 = sqrt(max(0, F(4)/kf));
end
6.3 仿真结果分析
典型性能指标：
- 位置跟踪误差：<0.1m（静态），<0.3m（动态）
- 姿态稳定时间：<1.5s（20°阶跃）
- 抗风性能：可抵抗6m/s突风
实操技巧：仿真时建议先测试单通道控制性能，再逐步增加耦合项。实时显示状态变量变化曲线有助于快速定位问题。
7. 实际部署注意事项
-
硬件在环测试：
- 使用Pixhawk等飞控进行HIL测试
- 逐步提高仿真逼真度（从理想模型到高保真模型）
-
采样时间选择：
- 姿态控制环：2-5ms
- 位置控制环：10-20ms
- 确保小于系统最小时间常数
-
安全机制：
matlab复制function [safe_cmd] = safety_check(cmd, state) % 指令限幅 cmd = min(max(cmd, cmd_min), cmd_max); % 低电量保护 if state.battery < 0.2 cmd = hover_cmd; end % 姿态保护 if any(abs(state.attitude) > 0.5) % 约30度 cmd = stabilize_cmd; end safe_cmd = cmd; end -
参数现场微调：
- 先进行悬停调参
- 再测试前飞/侧飞动态性能
- 最后验证轨迹跟踪效果
8. 常见问题排查
8.1 无人机震荡问题
可能原因及解决方案：
-
微分增益过大：表现为高频小幅振荡
- 逐步减小Kd直至振荡消失
- 增加低通滤波环节
-
传感器噪声：随机性抖动
- 检查IMU安装是否牢固
- 优化卡尔曼滤波参数
-
结构共振：特定频率下剧烈振动
- 进行频响测试识别共振点
- 增加减震措施或避开该频段
8.2 轨迹跟踪发散
调试步骤：
- 检查参考轨迹合理性（加速度是否超出无人机能力）
- 验证控制器输出是否饱和
- 分析姿态跟踪是否准确
- 检查模型参数准确性
8.3 电机响应不一致
校准方法：
- 单独测试每个电机转速-指令曲线
- 更新混控矩阵中的效率系数
- 检查电源系统电压稳定性
我在实际项目中发现，采用SDK日志记录飞行数据并配合MATLAB离线分析工具能极大提高调试效率。建议建立标准测试流程，每次只变更一个参数，通过对比飞行数据找出最优配置。