多旋翼无人机六自由度飞行控制全数字仿真平台

资 源 简 介

本项目旨在构建一个高保真度、模块化的无人机飞行控制系统仿真环境，解决目前网络上缺乏完整且可定制化飞行控制器源码的问题。系统核心功能包括：1. 动力学建模：基于牛顿-欧拉方程建立多旋翼无人机（以四旋翼为例）的非线性六自由度（6DOF）数学模型，包含刚体运动学、动力学以及电机推力模型，支持从地球坐标系到机体坐标系的转换计算。2. 控制系统设计：实现分层控制架构，开发基于四元数或欧拉角的姿态控制内环（Attitude Loop）和基于位置误差的轨迹跟踪外环（Position Loop）。控制算法不仅包含经典的串级PID控制，还集成线性二次型调节器（LQR）和滑模控制（SMC）算法，以便进行性能对比分析。3. 传感器与环境模拟：内置IMU（加速度计、陀螺仪）、GPS和气压计的传感器模型，包含随机噪声、零偏和漂移特性，并集成了卡尔曼滤波（Kalman Filter）用于状态估计。同时模拟阵风干扰和气动力效应，测试控制器的鲁棒性。4. 可视化与分析：并通过MATLAB绘图工具和Simulink 3D Animation实时显示无人机的三维飞行轨迹、姿态响应曲线、控制量输出（电机转速/PWM）及位置跟踪误差，为飞控算法的参数整定和定点代码自动生成提供理论验证与数据支持。

详 情 说 明

多旋翼无人机六自由度飞行控制全数字仿真平台

项目简介

本项目是一个基于MATLAB开发的高保真度多旋翼无人机（Quadrotor）飞行控制仿真系统。该平台旨在提供一个模块化、可定制的各种算法验证环境，核心基于非线性六自由度（6DOF）动力学模型，集成了位置与姿态的串级控制架构、环境干扰模拟、传感器噪声模型以及状态估计算法框架。

通过该仿真平台，用户可以深入理解无人机物理建模过程，验证PID、LQR等控制算法的性能，并观察无人机在风场干扰和传感器噪声下的鲁棒性表现。

主要功能特性

• 高保真非线性动力学模型：基于牛顿-欧拉方程构建，包含重力、气动阻力、旋翼推力及力矩。特别考虑了机体转动惯量耦合以及螺旋桨的陀螺效应（Gyroscopic Effect），使仿真更贴近真实物理特性。
• 模块化控制架构：实现了串级控制系统框架。外环为位置控制，内环为姿态控制。代码中初始化了串级PID参数，并预计算了LQR（线性二次型调节器）增益矩阵，支持不同控制策略的扩展与对比。
• 环境与传感器仿真：

* 风场干扰：内置阵风模型，支持恒定风与特定时间段的正弦突发阵风叠加。 * 传感器模型：模拟了GPS（位置、速度）和IMU（陀螺仪、欧拉角观测）数据，包含零偏（Bias）和随机高斯白噪声。

• 导航与制导：包含基本的航点（Waypoint）导航逻辑，支持多目标点的自动切换与偏航角（Yaw）对准策略。
• 数值解算稳定：采用四阶龙格-库塔法（RK4）进行微分方程的数值积分，确保长时间仿真的数值稳定性。

系统逻辑与核心实现

本项目的主要执行入口包含完整的仿真流程，以下为核心模块的详细技术实现说明：

1. 系统初始化与配置

仿真开始前，系统通过独立的配置函数加载关键参数：

• 仿真参数：设定时间步长（默认为5ms）和总仿真时长。
• 无人机物理参数：定义了质量、机臂长度、转动惯量（包含对角线元素及螺旋桨惯量）、推力/力矩系数以及空气阻力系数。
• 控制参数：

* PID配置：分别定义了位置环（P/PD）和姿态环（PID）的比例、积分、微分增益，以及积分限幅值。 * LQR配置：基于悬停点的线性化模型，构建状态空间矩阵（A, B），并通过权重矩阵Q和R计算LQR最优反馈增益矩阵 $K$，为高级控制算法提供基础。

2. 主仿真循环

核心循环模拟了无人机实时运行的每一个节拍（Time Step），主要包含以下环节：

#### A. 航点导航逻辑 系统根据当前估计位置与目标航点的欧氏距离（判定阈值为0.5m）判断是否切换至下一航点。同时，计算指向下一位置的方向向量，自动生成期望的偏航角（Yaw Target），使机头始终朝向飞行方向。

#### B. 传感器测量模型 模拟真实硬件的输出特性：

• GPS：在真实位置和速度上叠加低频、大方差的随机噪声。
• IMU (陀螺仪)：在真实角速度基础上叠加静态漂移（Bias）和高频噪声。
• 姿态观测：模拟磁力计或融合后的欧拉角输出，带有一定的观测噪声。

#### C. 状态估计 (EKF框架) 调用扩展卡尔曼滤波（EKF）接口，结合当前的先验估计、观测数据（GPS/IMU），更新系统的状态向量（位置、速度、姿态、角速度）及协方差矩阵。

#### D. 串级控制系统 控制逻辑采用内外环结构：

1. 位置环（外环）：输入期望位置与估计位置的误差。采用抗饱和的PID控制律，输出期望的加速度向量（NED坐标系）。
2. 姿态转换：将期望加速度转换为期望的俯仰角（Pitch）和滚转角（Roll），并结合期望推力。
3. 姿态环（内环）：代码预留了PID与LQR的切换接口。输入角度和角速度误差，输出各控制轴的期望力矩。
4. 控制分配：将总推力和三轴力矩解算为4个电机的期望转速平方值，并进行非负约束限制。

#### E. 环境干扰模型

• 基础风：施加恒定的水平风力。
• 阵风：在仿真时间的第5秒至第10秒区间，叠加正弦波形式的突变阵风，用于测试控制器的抗扰动性能。

#### F. 六自由度动力学解算 (RK4) 这是仿真的物理核心，输入包含电机转速和外部风力/气动力：

• 力与力矩计算：根据电机转速计算总推力和机体三轴力矩。
• 运动学：处理位置与速度的导数关系。
• 动力学：根据牛顿第二定律计算线加速度（包含重力、推力、阻力、风力）。
• 姿态动力学：基于欧拉方程求解角加速度，显式计算了由机体旋转产生的陀螺力矩项以及螺旋桨旋转产生的陀螺效应项。
• 地面约束：包含简单的地面碰撞检测，防止无人机跌落至地平面（Z > 0）以下。

3. 数据记录与可视化

仿真过程中实时记录真值、估计值、参考轨迹、控制量（推力/力矩）、电机转速及误差数据。仿真结束后，调用绘图接口展示：

• 三维空间飞行轨迹与规划航点的对比。
• 位置、速度、姿态的时间响应曲线。
• 控制输入量的变化情况，便于分析控制器是否存在饱和或震荡。

使用方法

1. 确保安装了MATLAB环境（推荐R2018b及以上版本）。
2. 直接运行主脚本/入口函数。
3. 程序将自动执行参数初始化、长时间步长的物理仿真解算。
4. 控制台将输出仿真进度，计算完成后自动弹出波形图和3D轨迹图。

系统要求

• MATLAB: 核心代码通过标准MATLAB函数实现，依赖基本的矩阵运算功能。
• Toolboxes:

* *Control System Toolbox*: 用于计算LQR增益（lqr函数）。 * 若无该工具箱，需手动注释掉 initControlParams 中的LQR计算部分，仅使用PID模式。