一、项目概述

四旋翼无人机（Quadrotor）因其结构简单、机动性强，成为无人机领域的研究热点。本项目旨在构建一个完整的四旋翼无人机仿真系统，包含非线性动力学模型、级联PID控制器、实时可视化界面和轨迹跟踪功能。系统采用C++编写，依赖Eigen进行矩阵运算，OpenCV实现3D渲染与数据绘图，最终形成一个可交互的仿真平台。

主要功能：

• 精确模拟四旋翼六自由度运动（位置、速度、姿态、角速度）

• 级联PID控制器实现位置/速度/姿态/角速度闭环

• 实时3D显示无人机姿态、轨迹、旋翼转速

• 动态绘制高度、姿态角历史曲线

• 可配置的轨迹跟踪任务（起飞→方波飞行→降落）

技术栈：

• C++11/14

• Eigen3（线性代数）

• OpenCV（可视化）

• CMake（构建）

---

二、系统架构

系统采用模块化设计，主要分为四个核心类：

类名	职责说明
DroneDynamics	无人机动力学模型：状态存储、运动方程、RK4积分、参数配置
PIDController	基础PID控制器：比例/积分/微分计算、抗积分饱和
DroneController	级联PID控制器：位置→速度→姿态→角速度的参考值生成，最终输出力和力矩
Visualizer	可视化模块：3D投影绘制无人机、轨迹、旋翼；数据曲线绘制
Simulator	仿真主控：时间步进、目标轨迹生成、实时循环、键盘交互

数据流图如下：

目标轨迹 → DroneController → 控制输入 → DroneDynamics → 状态更新 → Visualizer
        ↑                       ↓           ↓
        └──────── 反馈状态 ──────┘           └→ 实时显示

---

三、核心实现详解

1. 无人机动力学模型 (DroneDynamics)

状态定义

struct State {
    Eigen::Vector3d position;      // 位置 (x,y,z)
    Eigen::Vector3d velocity;      // 速度 (vx,vy,vz)
    Eigen::Vector3d euler_angles;  // 欧拉角 (φ,θ,ψ)
    Eigen::Vector3d omega;         // 角速度 (ωx,ωy,ωz)
    Eigen::Vector4d rotor_speeds;  // 旋翼转速 (rpm)
};

运动方程

根据牛顿-欧拉公式：

• 平动：m * dv/dt = R * F_b + m * g

• 转动：I * dω/dt + ω × (Iω) = τ

其中旋转矩阵 R 由欧拉角计算，机体坐标系下升力 F_b = [0,0,thrust]^T，重力 g = [0,0,-9.81]^T。

数值积分

采用四阶Runge-Kutta方法，保证仿真精度：

void update(double dt, const ControlInput& input) {
    State k1 = derivative(current_state_, input);
    State k2 = derivative(current_state_ + 0.5*dt*k1, input);
    State k3 = derivative(current_state_ + 0.5*dt*k2, input);
    State k4 = derivative(current_state_ + dt*k3, input);
    current_state_ += dt/6.0 * (k1 + 2*k2 + 2*k3 + k4);
}

2. 级联PID控制器 (DroneController)

控制器采用经典的四级串联结构：

1. 位置环 → 期望速度
   v_des = PID_pos(p_target - p_meas)

2. 速度环 → 期望加速度 → 期望姿态（通过简化动力学反解）
   a_des = PID_vel(v_target - v_meas)
θ_des = (a_des_x·cosψ + a_des_y·sinψ)/g
φ_des = (a_des_x·sinψ - a_des_y·cosψ)/g

3. 姿态环 → 期望角速度
   ω_des = PID_att(att_target - att_meas)

4. 角速度环 → 期望力矩
   τ = PID_omega(ω_target - ω_meas)

最终升力由重力补偿给出：thrust = m·g / (cosφ·cosθ)。

每个环均使用独立的PID控制器，参数可独立调节。

3. 可视化模块 (Visualizer)

3D投影

采用简单的正交投影加视角旋转：
将三维点 (x,y,z) 绕Y轴旋转 azimuth，绕X轴旋转 elevation，然后缩放平移至屏幕坐标。

无人机绘制

• 机身中心：绿色圆点

• 四个旋翼：黄色圆点，并带旋转指示线（根据转速长度变化）

• 旋翼转速：右侧条形图实时显示

数据曲线

绘制高度、滚转、俯仰、偏航的历史数据，帮助分析控制效果。

4. 仿真器 (Simulator)

• 时间步长：0.01秒（100Hz）

• 目标轨迹：预先生成起飞→悬停→方形运动→降落的轨迹点

• 实时同步：通过std::this_thread::sleep_for控制仿真速度与真实时间同步

• 键盘交互：ESC退出，P暂停

---

四、编译与运行

环境依赖

• Ubuntu 18.04/20.04 或 Windows + MinGW

• CMake ≥ 3.10

• Eigen3：sudo apt install libeigen3-dev

• OpenCV：sudo apt install libopencv-dev

编译步骤

mkdir build && cd build
cmake ..
make -j4
./drone_sim

CMakeLists.txt 关键配置

find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
add_executable(drone_sim src/main.cpp src/DroneDynamics.cpp src/Controller.cpp src/Simulator.cpp src/Visualizer.cpp)
target_link_libraries(drone_sim ${OpenCV_LIBS})

---

五、遇到的问题及解决方案

在开发过程中，遇到了两个典型的编译错误，记录了完整的排查过程。

问题1：PIDParams does not name a type

错误日志：

/home/.../Controller.h:33:9: error: ‘PIDParams’ does not name a type
   33 |         PIDParams pos_x;

原因分析：
PIDParams 是 PIDController 类的内部结构体，在 DroneController::ControllerParams 中直接使用 PIDParams 时，编译器无法找到该类型。C++ 的作用域规则要求使用内部类型时必须加上外部类限定符。

解决方案：
将 ControllerParams 中的所有 PIDParams 改为 PIDController::PIDParams。

struct ControllerParams {
    PIDController::PIDParams pos_x;
    PIDController::PIDParams pos_y;
    // ... 其他成员
};

同时，在构造函数初始化时也要对应修改：

params_.pos_x = PIDController::PIDParams{2.0, 0.1, 0.5, 2.0, 5.0};

问题2：reset was not declared in this scope

错误日志：

/home/.../DroneDynamics.cpp:7:5: error: ‘reset’ was not declared in this scope
    7 |     reset();

原因分析：
DroneDynamics 类的构造函数中调用了 reset() 函数，但在头文件中没有声明该成员函数。C++ 要求函数必须先声明后使用。

解决方案：
在 DroneDynamics.h 的 public 部分添加 reset() 的声明。

class DroneDynamics {
public:
    // ...
    void reset();  // 新增声明
};

问题3：积分过程中欧拉角奇异（隐含问题）

在 derivative() 函数中，计算欧拉角导数时需要用到 1/cos(theta)，当 theta 接近 ±90° 时会出现数值溢出。虽然实际飞行中很少达到如此大的俯仰角，但为了数值稳定性，添加了保护判断：

if (fabs(cos(theta)) < 1e-6) {
    dstate.euler_angles = Eigen::Vector3d(0, 0, 0);  // 简化处理
} else {
    dstate.euler_angles = T_inv.inverse() * state.omega;
}

问题4：抗积分饱和实现

积分项在误差持续存在时会无限增大，导致超调。在 PIDController 中通过限制积分项幅值解决：

if (integral_ > params_.max_integral) integral_ = params_.max_integral;
if (integral_ < -params_.max_integral) integral_ = -params_.max_integral;

问题5：旋翼力分配矩阵逆运算

在 calculateRotorSpeeds 中，需要对分配矩阵求逆。由于分配矩阵是固定的，理论上可以预计算逆矩阵，但当前实现每次调用都求逆，效率略低。若需优化，可改为硬编码逆矩阵。

---

六、仿真效果展示

运行程序后，将出现两个OpenCV窗口：

• Drone Simulation 3D：显示无人机三维模型、轨迹、坐标轴、旋翼转速条形图和控制输入。

• Drone Data Plots：实时绘制高度（绿色）、滚转角（红色）、俯仰角（蓝色）、偏航角（黄色）历史曲线。

无人机按照预设轨迹起飞→向前→向右→向后→向左→降落，控制器使无人机平滑跟踪目标点。通过调整PID参数可观察不同响应特性。

---

七、总结与展望

本项目实现了一个功能完整的四旋翼无人机仿真系统，涵盖了动力学建模、级联PID控制、实时可视化等关键模块。通过解决编译错误和数值问题，保证了代码的健壮性和可扩展性。

未来可扩展方向：

1. 添加传感器模型：IMU、GPS、视觉等，模拟真实噪声。

2. 引入状态估计器：如EKF将传感器数据融合。

3. 高级控制算法：LQR、模型预测控制（MPC）、自适应控制等。

4. 多无人机协同仿真：通过修改Simulator支持多架无人机。

5. 更丰富的环境：障碍物、风场干扰等。

---

写作说明：本文详细介绍了无人机仿真系统的设计与实现，并对开发过程中的典型错误进行了深入剖析。希望能为相关领域的学习者提供参考。如果有任何疑问或建议，欢迎留言交流。