四旋翼无人机三轴位置 + 偏航角的串级PID控制仿真
文章目录
运行结果
三维轨迹图：
位置跟踪曲线：
姿态曲线：
其他结果：
命令行截图：
程序详解
代码概览
- 项目名称：无人机PID控制巡航仿真（单文件版）
- 核心功能：实现四旋翼无人机在三维空间内的自动起飞、航点巡航及降落。
- 控制架构 ：采用经典的串级PID控制（Cascaded PID） 结构。
- 外环（位置环）：根据目标位置与当前位置的误差，计算出期望的加速度，进而解算出期望的姿态角（滚转、俯仰）和总推力。
- 内环（姿态环）：根据期望姿态与实际姿态的误差，计算出控制力矩，驱动无人机调整姿态。
- 动力学模型：内置了完整的六自由度（6-DOF）刚体动力学模型，考虑了重力、空气阻尼（线速度与角速度阻尼）以及欧拉角运动学耦合。
主要功能模块详解
- 仿真参数与物理模型
- 时间设置 ：仿真步长
dt = 0.005s(200Hz)，总时长 20秒。 - 物理参数 ：定义了无人机的质量（0.2kg）、转动惯量（ I x x , I y y , I z z I_{xx}, I_{yy}, I_{zz} Ixx,Iyy,Izz）以及线性/角速度阻尼系数，模拟真实物理环境。
- 航点规划：预设了7个关键航点（WP1-WP7），包含垂直起飞、水平移动、爬升、返航及降落动作。
-
控制逻辑流程
代码在主循环中按以下步骤实时计算：
-
航点管理：检测当前无人机位置与目标航点的距离，若小于阈值（0.3m）则自动切换至下一个航点。
-
外环解算（位置→姿态）：
- 利用位置PID计算期望加速度。
- 通过几何关系将期望加速度映射为期望的滚转角（Roll） 、俯仰角（Pitch）和总推力（Thrust）。
- 包含加速度限幅和姿态角限幅（最大倾斜30度），确保飞行安全。
-
内环解算（姿态→力矩）：
- 利用姿态PID计算所需的控制力矩（ τ r o l l , τ p i t c h , τ y a w \tau_{roll}, \tau_{pitch}, \tau_{yaw} τroll,τpitch,τyaw）。
- 包含积分抗饱和处理和输出限幅。
-
动力学积分：
- 基于欧拉法更新速度、位置、角速度和欧拉角。
- 处理了坐标系转换（机体系到世界系）和陀螺效应（ ω × I ω \omega \times I\omega ω×Iω）。
-
辅助函数库
代码底部封装了多个核心函数，使主逻辑清晰简洁：
pid_init/pid_update：标准化的PID控制器实现，包含积分限幅和微分计算。drone_dynamics：核心物理引擎，解算牛顿 - 欧拉方程。clamp/angle_wrap：数值限幅与角度归一化（处理 ± π \pm \pi ±π 跳变）工具。plot_results/plot_3d_trajectory：可视化模块，自动生成位置跟踪曲线、姿态响应图、控制量曲线以及三维飞行轨迹动画数据。
输出与可视化
运行结束后，代码将生成三个主要图表窗口：
- 位置跟踪图：展示X、Y、Z三轴的实际位置与期望轨迹的对比，直观反映控制精度。
- 姿态角响应图：展示飞行过程中的滚转、俯仰和偏航角变化，验证姿态稳定性。
- 控制量与误差图：显示总推力、三轴力矩输出以及三维空间的位置误差收敛情况。
- 三维轨迹图：在3D空间中绘制飞行路径、航点标记、起点和终点，提供直观的飞行效果预览。
适用场景
- 教学演示：适合用于自动控制原理课程中，讲解串级控制、PID参数整定及飞行器动力学。
- 算法验证：作为基础基准（Baseline），用于测试新的控制算法或轨迹规划策略。
- 快速原型：无需配置Simulink模型，单文件即可运行，便于快速修改参数进行实验。
MATLAB代码与界面
部分代码如下：
完整代码：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者