四旋翼无人机三轴位置 + 偏航角的串级PID控制仿真

运行结果

三维轨迹图：

位置跟踪曲线：

姿态曲线：

其他结果：

命令行截图：

程序详解

代码概览

• 项目名称：无人机PID控制巡航仿真（单文件版）
• 核心功能：实现四旋翼无人机在三维空间内的自动起飞、航点巡航及降落。
• 控制架构：采用经典的串级PID控制（Cascaded PID） 结构。
  • 外环（位置环）：根据目标位置与当前位置的误差，计算出期望的加速度，进而解算出期望的姿态角（滚转、俯仰）和总推力。
  • 内环（姿态环）：根据期望姿态与实际姿态的误差，计算出控制力矩，驱动无人机调整姿态。
• 动力学模型：内置了完整的六自由度（6-DOF）刚体动力学模型，考虑了重力、空气阻尼（线速度与角速度阻尼）以及欧拉角运动学耦合。

主要功能模块详解

1. 仿真参数与物理模型

• 时间设置：仿真步长 dt = 0.005s (200Hz)，总时长 20秒。
• 物理参数：定义了无人机的质量（0.2kg）、转动惯量（$I_{xx},I_{yy},I_{zz}$）以及线性/角速度阻尼系数，模拟真实物理环境。
• 航点规划：预设了7个关键航点（WP1-WP7），包含垂直起飞、水平移动、爬升、返航及降落动作。

2. 控制逻辑流程
   代码在主循环中按以下步骤实时计算：

3. 航点管理：检测当前无人机位置与目标航点的距离，若小于阈值（0.3m）则自动切换至下一个航点。

4. 外环解算（位置→姿态）：

   • 利用位置PID计算期望加速度。
   • 通过几何关系将期望加速度映射为期望的滚转角（Roll）、俯仰角（Pitch）和总推力（Thrust）。
   • 包含加速度限幅和姿态角限幅（最大倾斜30度），确保飞行安全。

5. 内环解算（姿态→力矩）：

   • 利用姿态PID计算所需的控制力矩（${\tau }_{roll},{\tau }_{pitch},{\tau }_{yaw}$）。
   • 包含积分抗饱和处理和输出限幅。

6. 动力学积分：

   • 基于欧拉法更新速度、位置、角速度和欧拉角。
   • 处理了坐标系转换（机体系到世界系）和陀螺效应（$\omega \times I\omega$）。

7. 辅助函数库
   代码底部封装了多个核心函数，使主逻辑清晰简洁：

• pid_init / pid_update：标准化的PID控制器实现，包含积分限幅和微分计算。
• drone_dynamics：核心物理引擎，解算牛顿 - 欧拉方程。
• clamp / angle_wrap：数值限幅与角度归一化（处理 $\pm \pi$ 跳变）工具。
• plot_results / plot_3d_trajectory：可视化模块，自动生成位置跟踪曲线、姿态响应图、控制量曲线以及三维飞行轨迹动画数据。

输出与可视化

运行结束后，代码将生成三个主要图表窗口：

1. 位置跟踪图：展示X、Y、Z三轴的实际位置与期望轨迹的对比，直观反映控制精度。
2. 姿态角响应图：展示飞行过程中的滚转、俯仰和偏航角变化，验证姿态稳定性。
3. 控制量与误差图：显示总推力、三轴力矩输出以及三维空间的位置误差收敛情况。
4. 三维轨迹图：在3D空间中绘制飞行路径、航点标记、起点和终点，提供直观的飞行效果预览。

适用场景

• 教学演示：适合用于自动控制原理课程中，讲解串级控制、PID参数整定及飞行器动力学。
• 算法验证：作为基础基准（Baseline），用于测试新的控制算法或轨迹规划策略。
• 快速原型：无需配置Simulink模型，单文件即可运行，便于快速修改参数进行实验。

MATLAB代码与界面

部分代码如下：

完整代码：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者