目录

• 目录
• 1. 项目简介
• 2. 环境要求
• 3. 项目结构
• 4. 运行步骤
• 5. 代码说明
  • 5.1. 核心模块
  • 5.2. 仿真参数
  • 5.3. 仿真控制选项
• 6. 仿真结果说明
  • 图像说明
• 7. 视频输出
• 8. 注意事项

---

1. 项目简介

该项目用于仿真四旋翼无人机的轨迹跟踪。通过设定无人机的初始状态及预定义轨迹，并使用控制器生成控制输入，实现无人机运动的动态仿真。支持多种轨迹（如圆形、菱形），并实时绘制三维运动轨迹，同时可保存仿真过程为视频文件。

---

2. 环境要求

要运行此项目，您需要以下依赖项：

• Python 3.6 及以上版本
• NumPy：用于科学计算
• Matplotlib：用于绘图
• SciPy：用于数值积分和微分方程求解
• OpenCV：用于视频生成
• utils.py：包含无人机仿真的辅助函数
• trajectories.py：用于生成预定义轨迹
• controller.py：实现控制器逻辑

安装依赖项：

pip install numpy matplotlib scipy opencv-python

---

3. 项目结构

.
├── run.py                  # 仿真主文件
├── utils.py                 # 无人机相关辅助函数
├── trajectories.py          # 轨迹生成函数
├── controller.py            # 控制器逻辑
├── diamond.avi              # 仿真输出的视频文件（可选）
└── README.md                # 项目说明文件

---

4. 运行步骤

1. 安装所有依赖项。
2. 下载项目文件并确保文件结构完整，特别是 utils.py、trajectories.py 和 controller.py。
3. 在终端中运行 run.py 以启动仿真：

python run.py

4. 仿真期间会实时绘制无人机的三维运动轨迹，仿真结束后会展示轨迹跟踪结果。如果启用了视频输出，程序还会生成视频文件。

---

5. 代码说明

5.1. 核心模块

• QuadPlot：实时更新并绘制无人机的三维运动轨迹。
• init_state：初始化无人机的初始状态（位置、速度等）。
• crazyflie：定义无人机的物理参数（如质量和惯性矩）。
• controller：生成控制输入以跟踪轨迹。
• trajhandle：轨迹生成函数（支持圆形轨迹和菱形轨迹）。
• quadEOM：无人机动力学模型，定义了运动方程。
• terminate_check：检查仿真是否达到终止条件。

5.2. 仿真参数

• nquad：无人机数量，当前仅支持单个四旋翼无人机仿真。
• time_tol：仿真时间上限（默认25秒）。
• pos_tol 和 vel_tol：位置和速度容差，决定是否达到目标点。

5.3. 仿真控制选项

• 实时仿真：将 real_time 设置为 True 启用实时仿真。
• 仿真步长：通过 tstep 设置仿真时间步长（默认0.01秒）。

---

6. 仿真结果说明

仿真完成后，将展示以下内容：

• 三维轨迹图：无人机实际轨迹与期望轨迹的对比。
• 位置变化图：显示无人机在 x、y、z 方向上的实际位置与期望位置的对比。
• 速度变化图：展示无人机在 x、y、z 方向的实际速度与期望速度的差异。

图像说明

• 红色实线：表示无人机的实际位置或速度。
• 蓝色虚线：表示无人机的期望位置或速度。

---

7. 视频输出

仿真支持视频输出。将 OUTPUT_TO_VIDEO 设置为 1，程序会自动记录仿真过程，并以 .avi 格式保存（默认文件名为 diamond.avi）。

---

8. 注意事项

1. 轨迹类型选择：支持不同轨迹（如圆形 circle 和菱形 diamond），可通过修改 trajhandle 变量选择轨迹。
2. ODE 解算器稳定性：仿真使用 scipy.integrate.odeint 进行数值求解，如遇解算器不稳定情况，请调节容差参数 atol 和 rtol。
3. 仿真步长和时长：根据需求合理设置 tstep 和仿真时长，确保仿真结果准确。