用C++把eVTOL六自由度动力学仿真跑通：建模、积分与控制调试

简介：eVTOL_Simulation 是一份基于 C++ 编写的 eVTOL 飞行器运营调度模拟程序，面向对离散事件仿真、面向对象编程感兴趣的开发者。程序内置 5 家公司的飞机参数，涵盖巡航速度、电池容量、充电时间、能耗及故障概率，模拟 20 架飞机同时起飞、电量耗尽后排队充电、充满再飞行的完整流程；时间上以三分钟代表实际三小时，节奏紧凑。代码实现飞机类与充电站类，飞机在飞行、等待、充电三个状态间切换，充电站维护 3 个充电器的空闲队列，并配有单线程主程序和单元测试，可校验飞行、等待、充电帧数合计是否等于总帧数；项目附带 makefile 构建脚本与飞机状态示意图，能辅助理解状态机与调度队列的交互过程。资源共 21 个文件，主体为 8 个 cpp 源文件和 7 个头文件，另有数据文件、说明文档等，压缩包仅 115KB，结构清晰，便于快速阅读与编译运行。目前已有 334 人学习下载，适合作为 C++ 课程设计、仿真系统入门或多个体调度场景的参考实现。

1. 把 eVTOL_Simulation 跑起来：先搞清楚这套 C++ 工程能帮你什么

很多第一次接触 eVTOL_Simulation 的朋友，会把它当成一个可视化程序，双击就能看到电动垂直起降飞行器在三维环境里飞来飞去。实际拿到手会发现，它是一套用 C++ 写好的六自由度飞行动力学仿真工程，默认输出的是命令行日志和 CSV 数据，三维显示只是可选模块。这套资源的核心价值，是让你在没有真机、没有风洞数据的情况下，先把控制律能不能稳住飞机这件事验证清楚。

它适合两类人：一类是做 eVTOL 或大型无人机控制系统的开发者，需要快速跑通垂起段和巡航线段的控制算法；另一类是刚接触飞行仿真的学生，想通过阅读一个不算太长的 C++ 工程，真正搞明白动力学方程是怎么落到代码上的。如果你之前用过 Simulink 里的 Aerospace 工具箱，再看这套 C++ 工程，会发现它把黑匣子拆开了，每一个积分步都看得见。

2. 飞行动力学建模与 C++ 选型：把六自由度方程落到代码前需要想清楚的三件事

2.1 为什么是六自由度而不是简化质点模型

eVTOL 的飞行包线比固定翼复杂得多。垂起阶段完全靠旋翼拉力克服重力，过渡阶段机翼逐渐产生升力，旋翼倾转角度和螺旋桨滑流都会改变受力。如果用位置和速度两轴质点模型，只能看到飞机整体轨迹，姿态角有没有发散完全不知道，这会直接导致控制律验证失去意义。所以仿真工程最少要把六自由度刚体动力学写进去：三个平动自由度，三个转动自由度。

在代码里，我习惯用一个 State 结构体同时保存位置、姿态和速度。姿态用四元数而不是欧拉角，四元数没有万向节锁问题，而且一个四元数乘法就能完成坐标旋转，插值和归一化也很方便。下面这个结构体是整套工程的核心，后面的所有模块都围绕它展开。

// state.h #pragma once #include <Eigen/Dense> struct State { Eigen::Vector3d pos; // NED坐标系位置 (m) Eigen::Quaterniond quat; // 机体到NED的姿态四元数 Eigen::Vector3d vel; // NED系线速度 (m/s) Eigen::Vector3d omega; // 机体系角速度 (rad/s) }; struct Input { double throttle; // 总油门 0~1 Eigen::Vector3d moment; // 期望力矩 (N·m) };

这里的 pos 用 NED 坐标系，也就是北东地坐标，飞行器仿真里比 z 向上的世界坐标直观。throttle 会经过一个拉力分配函数，转换成四个旋翼各自的转速指令；moment 则由控制律给出，实际作用到飞机上的是旋翼转速变化产生的力矩。把输入和状态分开定义，后续加扩展模块就不用改主循环代码。

有一点新手容易忽略：四元数在每次积分之后必须重新归一化。即使微分方程里写了归一化，数值积分累计误差也会让四元数模长慢慢偏离 1，导致姿态转换矩阵变奇怪。我通常在每个积分步末尾补一句out.quat.normalize()，花不了几个时钟周期，但避免了很多玄学翻车。

状态导数的写法是仿真里最容易出错的地方。参考常见的六自由度运动学方程，线加速度要加上重力加速度，角加速度由惯性张量和外力矩决定。工程里通常把力和力矩的计算拆到单独的动力学模块，不在积分器里写死，这样你后续换一套旋翼参数，只改配置表，不需要动积分代码。

// dynamics.cpp Eigen::VectorXd state_derivative(const State& st, const Input& in, double mass, double max_thrust, const Eigen::Matrix3d& J_inv) { Eigen::VectorXd dx(13); dx.segment<3>(0) = st.vel; // 位置导数 double T = in.throttle * max_thrust; // 总拉力 Eigen::Vector3d body_force(0, 0, -T); // 假设拉力沿机体-z Eigen::Vector3d ned_force = st.quat * body_force; Eigen::Vector3d accel = ned_force / mass; accel.z() += GRAVITY; // NED系重力向下为正 dx.segment<3>(7) = accel; Eigen::Quaterniond omega_q(0, st.omega.x(), st.omega.y(), st.omega.z()); Eigen::Quaterniond dq = omega_q * st.quat; dx.segment<4>(3) = 0.5 * dq.coeffs(); dx.segment<3>(10) = J_inv * in.moment; // 角加速度 return dx; }

实际工程中，拉力和力矩的方向要看坐标系定义。如果机体坐标系是 x 前、y 右、z 下，旋翼拉力通常沿 z 负方向，代码里的body_force(0,0,-T)是合理假设。四元数微分顺序容易翻车，一定要想明白omega_q是左乘还是右乘；最稳妥的办法是先写一个测试用例，让飞机保持任意姿态且 omega=0，检查四元数导数为零。

注意：这段代码如果直接编译，GRAVITY需要定义为常量，mass、max_thrust、J_inv要么作为参数传入，要么做成类成员变量。dx向量长度 13：3 位置 + 4 四元数 + 3 线速度 + 3 角速度。这个顺序要和初始化代码保持严格一致，否则积分出来的状态会错位，通常表现就是位置在乱跳但姿态完全不变。建议在代码里写一个状态打包和解包函数，不要直接靠索引硬背。

2.2 空气动力学：机翼、螺旋桨滑流和地效在仿真里的取舍

很多开源仿真工程把旋翼拉力简化成 T = k * ω²，机翼升力简化成 0.5 * ρ * V² * S * CL(α)，这套做法足够用来验证控制链路。但如果你把参数表分辨率设得太粗，会看到高度曲线一抖一抖的。我一般会把攻角范围设置在 -15° 到 45°，间隔 1°，拟合精度在 5% 以内就可以。

这个表的作用是让你在改配置时有参照，不同飞行阶段可以用同一张表。实际工程会根据速度分段修正，比如垂起时螺旋桨滑流会把机翼一部分吹成充分分离区，这时候直接套用固定翼 CL 曲线会高估升力，必须给滑流区一个衰减因子。很多初学的人在这块吃亏，只调旋翼参数不管机翼参数，导致过渡段掉高特别厉害。

资源里默认的模型不会做到 CFD 级别精度，只保证趋势正确。我一般会在代码里加一个interp1d函数，让参数表越界时返回边界值而不是 NaN，这个问题在巡航阶段风速突变时特别常见。

// utils.h #pragma once #include <vector> double interp1d(const std::vector<double>& x, const std::vector<double>& y, double xi) { if (x.empty() || y.empty()) return 0.0; if (xi <= x.front()) return y.front(); // 左边界夹紧 if (xi >= x.back()) return y.back(); // 右边界夹紧 for (size_t i = 0; i < x.size() - 1; ++i) { if (xi >= x[i] && xi <= x[i + 1]) { double t = (xi - x[i]) / (x[i + 1] - x[i]); return y[i] + t * (y[i + 1] - y[i]); } } return y.back(); }

这里的边界夹紧是我个人习惯，看起来不起眼，但在数据表边界外会返回稳定值，不会突然冒出 NaN 把积分器打翻。代码里所有查表都走这个函数，替换数据表时不用改调用逻辑。电机响应也不能忽略。真实电机转速不可能瞬间跟上指令，仿真里我用一阶惯性环节模拟延迟，时间常数一般取 0.1~0.2 秒。如果把这个延迟去掉，PID 带宽设计会过于乐观。

// motor.h double motor_rpm = 0.0; void update_motor(double target_rpm, double dt) { double tau = 0.15; // 一阶惯性时间常数 motor_rpm += (target_rpm - motor_rpm) * dt / tau; }

代码里的 tau 就是时间常数。tau 越小电机响应越快，但真实系统有延迟，太小的 tau 会让仿真变得过于理想。我一般会根据厂商电机数据表去反推，没有数据时就先用 0.15 跑通，再单独做参数敏感性分析。

2.3 为什么用 C++ 而不是 MATLAB/Python

选型这件事不是越新越好，而是看你的目标。MATLAB 的 Aerospace 工具箱仿真精度不错，但授权成本高，生成的 C 代码可读性差；Python 的 NumPy/SciPy 写起来快，但实时控制回路里线程调度抖动大，你永远不知道某一步会不会被垃圾回收卡一下。C++ 加 Eigen 库的优势在于性能和可移植性，而且代码能直接嵌入到飞控单片机之外的其他运行环境。

常见做法是先用 C++ 搭建动力学模型，把控制律写成与硬件无关的函数，等模型跑通后再把控制律原封不动移植到飞控。这样可以避免两套语言造成的对不齐问题。我一般会把仿真主程序和控制器分开编译成两个库，主程序只负责积分和数据记录，控制器库单独做单元测试。这种结构让我在调参数时不必反复重新编译整棵依赖树。

3. 把工程跑起来：从拿到源码到调出第一个垂直起降曲线

3.1 目录结构与编译准备

解压资源包以后，典型的目录结构长这样：

eVTOL_Simulation/ ├── CMakeLists.txt ├── src/ │ ├── state.h │ ├── dynamics.cpp │ ├── controller.cpp │ └── main.cpp ├── config/ │ ├── hover.json │ └── cruise.json └── tools/ └── plot_trajectory.py

这份资源把控制、动力、积分器分开放，改动一个模块不影响其它模块。编译前需要确保 Eigen 库已经安装。在 Ubuntu 类系统可以用包管理器，macOS 用 Homebrew，Windows 用 vcpkg。我用得最多的命令是：

mkdir -p build && cd build cmake .. -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3 make -j$(nproc)

如果 Eigen 已经装到系统默认路径，-DEIGEN3_INCLUDE_DIR可以省略。Release模式非常重要，Debug 模式下 Eigen 的向量化会慢四五倍，仿真一座机时间从分钟变成小时。实际跑起来以后，控制频率和时间步长都会写在main.cpp里，一般不需要再动 CMake。

3.2 仿真主循环：状态更新与控制输入怎么接

主循环通常长这样：

// main.cpp 中的核心循环 #include "state.h" #include "dynamics.h" #include "controller.h" State st; st.pos = Eigen::Vector3d(0, 0, 0); st.quat = Eigen::Quaterniond::Identity(); double dt = 0.002; // 500 Hz 控制频率 int steps = static_cast<int>(20.0 / dt); // 仿真20秒 for (int i = 0; i < steps; ++i) { Input in = controller.compute(st, ref); st = rk4_step(st, in, dt); logger.write(i * dt, st, in); }

这里的rk4_step是四阶龙格库塔积分器，里面调用了前面讲过的state_derivative函数。为什么不用欧拉？因为动力学方程里有高频的旋转项，欧拉法在一阶模型上可能发散，RK4 在刚体仿真里综合表现最稳。如果以后要加入弹性结构或更小的时间步长，再考虑换更高级的积分器。

具体实现如下，注意四元数积分后的归一化：

// integrator.h #include <Eigen/Dense> State rk4_step(const State& st, const Input& in, double dt, Eigen::VectorXd (*dyn)(const State&, const Input&)) { // 这里为了可读性直接对 State 操作，实际工程更常用向量加权 State k1_state = st; // k1 = f(st) Eigen::VectorXd k1 = dyn(st, in); State s2 = st; // 四元数部分需要单独处理，不能用简单加法 s2.pos += 0.5 * dt * st.vel; Eigen::VectorXd k2 = dyn(s2, in); State s3 = st; s3.pos += 0.5 * dt * (st.vel + 0.5 * dt * k1.segment<3>(0)); Eigen::VectorXd k3 = dyn(s3, in); State s4 = st; s4.pos += dt * (st.vel + 0.5 * dt * k2.segment<3>(0)); Eigen::VectorXd k4 = dyn(s4, in); State out; out.pos = st.pos + (dt / 6.0) * (k1.segment<3>(0) + 2 * k2.segment<3>(0) + 2 * k3.segment<3>(0) + k4.segment<3>(0)); auto dq = 0.5 * dt * (k1.segment<4>(3) + 2 * k2.segment<4>(3) + 2 * k3.segment<4>(3) + k4.segment<4>(3)); out.quat = st.quat * Eigen::Quaterniond(dq(0), dq(1), dq(2), dq(3)); out.quat.normalize(); return out; }

上面是简化版本，真正的工程里会把 State 先打包成 13 维向量再统一加权，否则四元数和位置、速度的耦合会很难管理。rk4_step的代码本身只是骨架，重要的是你想清楚每一步中间状态怎么算出来。如果动力学函数内部有查表插值，那么中间状态也要通过查表更新，不能只改位置而忽略姿态变化。

PID 控制器可以非常简单，核心是误差计算和积分限幅：

// controller.cpp Input compute_pid(const State& st, const State& ref, double dt) { Input in; double kp = 6.0, ki = 0.8, kd = 2.5; double err_z = ref.pos.z() - st.pos.z(); double last_err = err_z; // 正常应定义为状态变量 static double int_z = 0; int_z += err_z * dt; int_z = std::clamp(int_z, -10.0, 10.0); // 积分限幅 in.throttle = kp * err_z + ki * int_z + kd * (err_z - last_err) / dt; in.throttle = std::clamp(in.throttle, 0.0, 1.0); return in; }

这里把 throttle 限制在 0~1 是飞行控制器的执行器约束。积分限幅的作用是防止 windup，如果你在仿真里看到某个状态下油门持续顶到上限，多半是积分项在撞击限幅，后面避坑章节里会展开说。

3.3 通过配置文件切换垂直起降和巡航场景

资源包提供了两个典型的 JSON 配置文件，一个垂起一个巡航。我用 hover.json 演示：

{ "mass_kg": 1200, "thrust_coeff": 8.8e-5, "wing_area_m2": 6.4, "cl_alpha": 4.3, "cmd": "hover", "duration_s": 20.0 }

注意thrust_coeff的单位取决于你螺旋桨转速用什么量化。工程里默认用 rad/s 的二次方，所以量级在 1e-4 附近。很多人在这一步翻车，把 8.8e-5 当成 0.088 填进去，结果起飞直接爆表。切换巡航模式只需要把cmd改成"cruise"，代码里会换一套状态机，上升到 500m 高度后开始前飞，机翼升力逐渐接管。

我一般会先用 hover.json 调完 PID，再切到 cruise 验证过渡段，不要一上来就飞全包线。命令行运行方式是：

./eVTOL_sim --config ../config/hover.json

4. 避坑/常见问题：eVTOL_Simulation 里最容易翻车的五个点

这套工程我实际调下来遇到过不少问题，按现象分类整理成下面几个点。每一条都是可以直接对照排查的经验，比在日志里大海捞针快不少。

4.1 动力学发散类问题

现象：起飞瞬间高度发散，十几秒内数值溢出。你会在终端里看到高度从几米突然变成 -1e30，或者直接打印出 inf。原因通常不是重力加速度的问题，而是控制律给的油门变化率太大，姿态角瞬间改变近 90°，旋转矩阵出现奇异。解决的办法是给油门加一阶低通滤波，限制油门斜率为每秒约 0.2 的量级；同时在每个积分步之后对四元数做归一化。这一步看起来简单，能滤掉很多不稳定的高频振荡。

现象：巡航阶段阻力为零或升力突跳，速度曲线出现台阶。原因基本都在参数表查表函数上：CL、CD 表在做插值时越界，或者interp1d的判断条件写反，边界返回了 NaN。解决方法是统一使用前面utils.h里带边界夹紧的查表函数，让攻角超出表范围时返回最近边界值。另外可以在代码里加一条断言，检测输入攻角是否落在预期范围内，这样问题一旦复现，能立刻定位到是哪次查表出了问题。

现象：飞机在过渡阶段出现持续等幅振荡，频率大约 2Hz，看着像控制环在自激。原因通常是控制频率和动力学特征频率重叠，姿态环采样频率太低。解决方法是把仿真的控制环路从 200Hz 提高到 500Hz，同时检查角速度滤波器的截止频率是不是设置得过高。常见做法是把低通滤波截止频率设成旋翼响应带宽的三倍，不要盲目追求高带宽去压噪声。

4.2 编译与控制参数类问题

现象：程序一运行到包含 State 的 vector 就崩溃，报错信息里有 Eigen alignment 字样。原因很简单：Eigen 的固定尺寸类型在 std::vector 里没有正确对齐，常见于std::vector<State>。解决方法是给 vector 加 Eigen 内存分配器，或者使用std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>。在 C++17 里也可以直接开满编译器的-std=c++17，大部分情况能覆盖对齐问题，但不能完全替代正确做法。

现象：PID 调试时，高度跟踪误差不大，但油门指令在极限值附近来回顶。原因是积分项无限累积，控制器输出超过执行器范围。这种饱和最容易引发极限环。解决方法是用抗积分饱和：当输出达到限幅时，把积分器回退到上一时刻的值，或者直接用积分分离算法，在指令误差大时完全不累加积分项。仿真里遇到这类问题，先看 CSV 里的 throttle 曲线，如果它呈现规则的锯齿状，基本就是积分饱和没处理好。

现象：CMake 阶段报找不到 Eigen3_DIR。原因是 Eigen 库安装路径不在系统搜索范围。解决方法是在cmake ..时显式指定-DEIGEN3_INCLUDE_DIR，或者用INCLUDE_DIRECTORIES加入 Eigen 头文件所在目录。Windows 上用 vcpkg 安装后，通常还需要在 CMake 里find_package之前设置工具链文件，不想折腾就下载预编译 include 目录，省心很多。

5. 把数据导出来：用 CSV + Python 验证你调的每一组参数

5.1 在 C++ 里加一个 CSV 记录器

仿真程序默认不会把所有中间变量都打印出来，但调试时需要看到高度、姿态、油门指令随时间的变化。我通常会在 logger 模块里加一个write函数，把时间、位置、四元数、角速度和控制输出写入 CSV。代码非常简单：

void write(std::ofstream& out, double t, const State& st, const Input& in) { out << t << "," << st.pos.x() << "," << st.pos.z() << "," << st.quat.w() << "," << st.quat.x() << "," << st.quat.y() << "," << st.quat.z() << "," << in.throttle << "\n"; }

这里四元数的 w,x,y,z 顺序千万别搞反。四元数存储常见的是 w 在前，但有些库把 x,y,z,w 放前面，如果你用 Python 读取后解析姿态角，顺序错一列，画出来的高度曲线看起来正常，姿态曲线却完全是乱的。我习惯在 CSV 第一列放时间，后面按固定顺序放状态，后续脚本只用列索引，不用动态解析变量名，省去很多麻烦。

5.2 用 Python 快速画图验证

拿到 CSV 后，我通常用一段不到二十行的 Python 脚本画高度和油门指令的响应曲线：

import pandas as pd import matplotlib.pyplot as plt df = pd.read_csv("data/test_run.csv", names=["t", "x", "z", "qw", "qx", "qy", "qz", "throttle"]) plt.figure(figsize=(10, 4)) plt.subplot(1, 2, 1) plt.plot(df["t"], df["z"], label="height") plt.legend() plt.subplot(1, 2, 2) plt.plot(df["t"], df["throttle"], label="throttle") plt.legend() plt.show()

如果高度曲线在前两秒内出现大的抖动，先看 throttle 是不是在饱和限幅边缘震荡，再看四元数分量有没有突变。通常控制参数的问题在 CSV 里一眼就能看出来，比看终端里滚动的数字快得多。数据记录也方便做参数对比：把两组不同 PID 参数运行后的 CSV 并排画在同一张图里，就能直观看到响应速度与超调量。

这套 CSV + Python 验证流程，是我做所有仿真调试的底线。每次调完一组参数，我都会强制把数据导出来看一遍，哪怕只是扫一眼曲线形状；因为光看终端里的数字，积分器偶尔给出一个看起来平均稳定的结果，实际上包含了高频振荡。从那以后我每次改完逻辑，都先跑一遍数据记录，再谈优化。希望帮到你。

本文还有配套的精品资源，点击获取