C++ eVTOL仿真工程：六自由度动力学与控制分配实战

简介：eVTOL_Simulation 是一份面向 C++ 学习者与仿真爱好者的轻量级项目源码，围绕电动垂直起降飞行器的调度场景展开。程序设定 5 家公司共 20 架飞机同时起飞，每架飞机拥有巡航速度、电池容量、充电时间、巡航能耗及每小时故障概率等参数，电量耗尽后依次进入仅含 3 个充电桩的充电站排队充电，充满后重新起飞，飞行速度保持恒定，并以三分钟模拟实际三小时。资源包共 21 个文件，以 8 个 cc 源文件与 7 个 h 头文件为核心，辅以 makefile、company_data、globa_data 等数据与构建文件，以及说明文档和状态示意图，压缩包约 115KB。代码采用单线程实现，包含飞机级 FLY、充电、等待状态与充电站级队列调度逻辑，并配有单元测试，校验飞行帧数、等待帧数与充电帧数之和是否等于总帧数。目前已有 335 人学习，适合通过阅读与运行源码理解面向对象建模、状态机与离散事件仿真思路。

1. eVTOL_Simulation：一份用 C++ 把飞行器动力学跑起来的仿真工程

如果你正在找一个能把 eVTOL 飞行动力学、控制分配、状态估计串起来跑的 C++ 仿真骨架，而不是又一份只画了个方块在天上飘的玩具，那这份 eVTOL_Simulation 值得你花半小时拆一遍。它解决的核心问题是：把六自由度刚体动力学、旋翼拉力模型、电机响应延迟和简单的姿态控制器放进同一个可编译、可调参、可复现的工程里，让你能在本地把“飞行器从悬停到前飞”这条链路跑通。适合谁？做飞控算法验证的、写嵌入式仿真测试的、以及想从 MATLAB/Simulink 迁移到 C++ 做实时仿真的从业者。它不承诺气动精度，但承诺结构清晰、参数可改、编译即跑。

2. 拆开工程看骨架：C++ 模块划分与动力学主循环

2.1 为什么用 C++ 而不是脚本语言做 eVTOL 仿真

常见做法是先用 Python 或 MATLAB 快速验证控制律，再往 C++ 迁移。但 eVTOL 仿真有个绕不开的点：控制周期通常在 1kHz 到 4kHz 之间，电机响应时间常数在毫秒级，如果仿真步长大于 2ms，姿态环的相位裕度会被数值积分误差吃掉，调出来的 PID 参数上真机就翻车。脚本语言单步开销大，跑 10 秒仿真可能要等几分钟，迭代效率低。

这份工程用 C++ 写，核心诉求是单步计算可控。它没有依赖大型仿真框架，而是自己实现了一个固定步长的积分器，配合简单的欧拉或 RK4 推进。好处是编译后单步耗时在微秒级，你可以把仿真跑得比实时还快，批量扫参。另一个理由是部署路径短：仿真里验证过的控制分配逻辑，改改接口就能塞进飞控板子的固件里，不用重写。

选型上它避开了 Eigen 这类重型线性代数库，姿态更新用手写的四元数乘法，矩阵运算只在控制分配里出现小规模伪逆。这不是偷懒，是为了让工程在嵌入式交叉编译环境里也能过。如果你习惯用 Eigen，替换掉那几处矩阵运算也不难，但要注意对齐和内存分配，实时循环里别动态申请内存。

2.2 工程目录与核心类职责

拿到源码包，先别急着编译。花五分钟看目录结构，能省掉后面半小时的“为什么链接报错”。典型布局是这样的：

eVTOL_Simulation/ ├── include/ │ ├── dynamics.h // 六自由度刚体动力学 │ ├── rotor.h // 旋翼拉力/反扭矩模型 │ ├── motor.h // 电机一阶响应 │ ├── controller.h // 姿态/位置控制器 │ ├── allocator.h // 控制分配 │ └── math_utils.h // 四元数、向量、小矩阵 ├── src/ │ ├── dynamics.cpp │ ├── rotor.cpp │ ├── motor.cpp │ ├── controller.cpp │ ├── allocator.cpp │ └── main.cpp // 仿真主循环 ├── config/ │ └── params.yaml // 质量、惯量、旋翼位置、PID 增益 └── CMakeLists.txt

dynamics负责状态导数计算：输入力和力矩，输出加速度和角加速度。rotor把转速映射成拉力和反扭矩，常见做法是拉力正比于转速平方，反扭矩也正比于转速平方再乘个系数。motor是一阶惯性环节，给定期望转速，输出实际转速，时间常数在参数文件里改。controller读状态误差，算期望力矩和总拉力。allocator把期望力矩和总拉力分配到各个旋翼的转速上，这是 eVTOL 和直升机最大的不同：多旋翼要靠分配矩阵解耦。

main.cpp里的主循环是固定步长，常见写法：

// main.cpp 仿真主循环片段 const double dt = 0.001; // 1kHz 步长 const double t_end = 20.0; // 仿真 20 秒 double t = 0.0; State state = init_state(); // 初始悬停状态 Controller ctrl(params); Allocator alloc(params); while (t < t_end) { // 1. 读期望指令（这里用阶跃或正弦扫频） Command cmd = generate_command(t); // 2. 控制器算期望力矩和总拉力 Wrench desired = ctrl.update(state, cmd, dt); // 3. 控制分配：力矩+拉力 -> 各旋翼期望转速 std::vector<double> omega_des = alloc.solve(desired); // 4. 电机响应：期望转速 -> 实际转速 std::vector<double> omega_act = motor_update(omega_des, dt); // 5. 旋翼模型：实际转速 -> 力和力矩 Wrench actual = rotor_forces(omega_act, params); // 6. 动力学积分：力和力矩 -> 新状态 state = integrate(state, actual, dt); // 7. 记录日志 log(t, state, omega_act); t += dt; }

这段代码的逻辑是“指令→控制→分配→执行→动力学→状态更新”的闭环。参数说明：dt是仿真步长，改小精度高但耗时线性增加；t_end控制仿真时长；generate_command里可以换成你想要的激励信号，比如 1Hz 正弦看姿态跟踪带宽。注意alloc.solve返回的是期望转速，不是直接给力，因为电机和旋翼有动态，跳过电机模型直接给力会让仿真过于理想，调出来的增益偏激进。

2.3 编译与第一次跑通

工程用 CMake 管理，依赖只有标准库和 yaml-cpp（读参数文件）。如果你不想装 yaml-cpp，把参数硬编码进params.h也能跑，但改参数要重编译，扫参时很痛苦。常见做法是保留 yaml 解析，用yaml-cpp的find_package找库。

# 编译步骤 mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Release make -j4 ./evtol_sim ../config/params.yaml

CMAKE_BUILD_TYPE=Release一定要开，Debug 模式下单步耗时可能翻十倍，跑 20 秒仿真会等到怀疑人生。-j4按你机器核数改。运行后程序会输出一个 CSV 日志，列包括时间、位置、速度、姿态四元数、角速度、各旋翼转速。用 Python 或 Excel 画一下姿态角响应，如果俯仰角在阶跃指令下能稳定到期望值附近，说明控制器和分配矩阵基本对上了。

第一次跑常见现象：姿态发散。先别改 PID，去检查分配矩阵的旋翼位置符号。eVTOL 的旋翼布局有十字、X 型、Y 型，位置坐标的正负号决定了力矩方向。如果俯仰通道的符号反了，控制器越修越偏，几秒内就翻。参数文件里rotor_positions的 x/y/z 坐标要和你的布局一致，z 向上为正还是向下为正，全工程要统一。

3. 控制分配与电机模型：参数怎么设、坑在哪

3.1 控制分配矩阵的构造与伪逆求解

多旋翼 eVTOL 的控制分配要解决一个欠定或超定问题：期望的滚转、俯仰、偏航力矩和总拉力是 4 个量，旋翼数量通常大于 4，所以有无穷多组转速解。常见做法是用伪逆求最小范数解，再叠加一个零空间项去优化其他目标，比如让转速尽量均匀、避免饱和。

构造分配矩阵时，每一列对应一个旋翼，行对应总拉力、滚转力矩、俯仰力矩、偏航力矩。拉力行全是 1（假设拉力系数相同），力矩行由旋翼位置和旋转方向决定。代码里通常这样建：

// allocator.cpp 分配矩阵构造 // 行顺序: [总拉力, 滚转力矩, 俯仰力矩, 偏航力矩] // 列: 每个旋翼 Matrix B(4, n_rotors); for (int i = 0; i < n_rotors; ++i) { double x = params.rotor_pos[i].x; // 前为正 double y = params.rotor_pos[i].y; // 右为正 double kT = params.kT; // 拉力系数 double kQ = params.kQ; // 反扭矩系数 int dir = params.spin_dir[i]; // +1 或 -1 B(0, i) = kT; // 总拉力 B(1, i) = -y * kT; // 滚转力矩 = -y * 拉力 B(2, i) = x * kT; // 俯仰力矩 = x * 拉力 B(3, i) = -dir * kQ; // 偏航力矩 = -旋向 * 反扭矩 }

逻辑说明：滚转力矩由 y 方向力臂产生，俯仰由 x 方向力臂产生，偏航由反扭矩产生。符号约定要和你动力学里的力矩定义一致，否则分配出来的转速方向反了，姿态控制直接发散。参数说明：kT和kQ可以从旋翼厂家数据里估，没有数据就先用 1.0 跑通逻辑，再按实际拉力需求缩放。spin_dir是旋翼旋转方向，相邻旋翼通常反向，用来抵消偏航反扭矩。

求解时用B的伪逆乘期望 wrench：

// 伪逆求解，u = B^+ * desired Matrix B_pinv = pseudo_inverse(B); Vector u = B_pinv * desired; // u 是各旋翼拉力平方（或转速平方）

注意伪逆对矩阵条件数敏感。如果旋翼布局接近奇异，比如所有旋翼几乎在一条线上，伪逆会给出巨大的转速指令，电机模型一饱和，实际响应和期望差很远。检查方法：算一下B的条件数，超过 1e4 就要警惕。常见做法是加一个小的正则化项，用B^T (B B^T + λI)^-1代替纯伪逆，λ 取 1e-4 到 1e-2，牺牲一点分配精度换数值稳定。

3.2 电机一阶模型的时间常数与饱和处理

电机模型看着简单，但它是仿真里最容易“看起来对、实际错”的环节。一阶惯性环节的离散更新：

// motor.cpp 一阶响应 double alpha = dt / (tau + dt); // tau 是时间常数 omega_act[i] = omega_act[i] + alpha * (omega_des[i] - omega_act[i]); // 饱和限幅 omega_act[i] = clamp(omega_act[i], omega_min, omega_max);

tau的典型值：小型多旋翼电机加桨，时间常数在 20ms 到 80ms 之间；eVTOL 用的高惯量桨，可能到 100ms 以上。这个参数直接影响力矩响应带宽。如果你把tau设成 1ms，仿真里姿态跟踪会非常快，但上真机就会发现实际响应慢半拍，控制器增益显得过高，振荡。血泪经验：tau宁大勿小，先用 50ms 跑，再根据实际电机数据调整。

饱和限幅不能省。omega_min通常取 0 或怠速转速，omega_max是电机最大转速。如果不限幅，分配器在力矩需求大时会给出负转速或超限转速，动力学里拉力变成负数，飞行器会“反向飞”，仿真结果完全不可信。限幅后要注意积分饱和：控制器里的积分项在饱和期间会继续累积，退出饱和时产生大超调。常见做法是加抗积分饱和逻辑，饱和时停止积分或回退积分量。

3.3 参数文件怎么改：从悬停到前飞的配置

参数文件是这份工程的调参入口。典型字段：

从悬停切前飞，改的是指令生成部分：给一个前向速度期望，位置控制器输出俯仰角期望，姿态控制器跟踪。如果前飞时高度掉，检查总拉力补偿：前飞需要倾斜，垂直分量减小，总拉力要除以 cos(倾角) 来维持高度。这个补偿在控制器里加，不在参数文件里。

提示：改完参数先跑 5 秒悬停，确认姿态角在 ±0.5 度内再跑前飞。跳过悬停直接前飞，姿态发散了你分不清是分配矩阵错还是前飞补偿错。

4. 避坑与排查：仿真跑不通时先看这几处

4.1 现象：编译通过但运行时报“NaN”或姿态瞬间发散

原因通常是分配矩阵奇异或伪逆求解除了零。检查旋翼位置是否所有 x 或所有 y 相同，那样滚转或俯仰通道没有力臂，矩阵秩不足。解决：确认布局参数，十字布局至少有两个不同 x 和两个不同 y。另一个原因是kT或kQ设成了 0，矩阵整行全零。把参数文件里这两个值恢复默认。

4.2 现象：姿态能稳定但高度缓慢漂移

高度漂移多半是总拉力标定不对。悬停时总拉力应该等于重力，如果kT偏小，控制器会积分到最大转速还是掉高。检查方法：打印悬停稳态时各旋翼转速，反推总拉力，和mass * g比。差超过 5% 就调kT。注意g取 9.81，别用 10，累积误差在 20 秒仿真里能差出几米。

4.3 现象：前飞时偏航角慢慢转，需要不断修正

这是反扭矩补偿没做对。前飞时各旋翼转速不同，反扭矩之和不为零，偏航通道有净力矩。分配矩阵里偏航行如果符号或系数错，控制器会一直修。检查spin_dir和kQ的乘积符号，确保悬停时偏航力矩和为零。常见做法是在分配器里加一个偏航配平项，或者让控制器积分项去补，但积分补会牺牲偏航响应带宽。

4.4 现象：仿真步长改小后结果反而变差

数值积分不稳定。欧拉积分在步长太小时，舍入误差累积可能让四元数范数漂移。解决：每步积分后对四元数归一化，代码里加一行q.normalize()。如果用的是 RK4，检查中间状态的物理量是否越界，比如转速为负。RK4 对刚性系统不友好，电机时间常数远小于步长时，用欧拉反而更稳。步长选择经验：dt小于最小时间常数的 1/5，tau=50ms时dt=1ms足够。

4.5 现象：日志画出来姿态角有高频抖动

不是控制器问题，是日志采样和仿真步长一致，1kHz 数据里混了数值噪声。常见做法是日志降采样到 100Hz 再画，或者加一个一阶低通滤波。别急着调 D 增益，先确认抖动频率是不是等于 1/dt，如果是，那是数值噪声，调增益没用。

5. 进阶用法：用扫频和批量仿真验证控制带宽

把基础悬停跑通后，别停在“能飞”就结束。这份工程真正的价值是能批量跑参数、扫频、验证边界。我一般会做两件事：一是给姿态指令加正弦扫频，从 0.1Hz 到 20Hz，看跟踪幅值衰减到 -3dB 的频率，那就是姿态环带宽；二是批量改kp_att和kd_att，跑 50 组，记录超调量和调节时间，找帕累托前沿。

扫频的代码改动很小，在generate_command里换成正弦：

// 扫频指令：频率随时间线性增加 double freq = 0.1 + (20.0 - 0.1) * t / t_end; // 0.1Hz -> 20Hz double amp = 0.1; // 弧度，约 5.7 度 cmd.roll = amp * sin(2.0 * M_PI * freq * t);

跑完后对日志做 FFT，或者简单点，找每个频率点的幅值比。如果 5Hz 时幅值已经掉到 0.7 以下，说明带宽不够，前飞时遇到阵风恢复慢。这时候再回去调增益，比盲调有方向。

批量仿真用 shell 脚本循环改 yaml 再跑：

# 批量扫参 for kp in 3 5 8 10; do for kd in 0.3 0.5 0.8; do sed -i "s/kp_att:.*/kp_att: $kp/" config/params.yaml sed -i "s/kd_att:.*/kd_att: $kd/" config/params.yaml ./evtol_sim config/params.yaml > log_kp${kp}_kd${kd}.csv done done

跑完用 Python 读 CSV，算每个组合的超调量和稳态误差，画散点图。注意sed -i在 macOS 上要加''，Linux 不用。批量跑之前先把日志输出改成只写关键列，不然磁盘很快满。

从那以后我每次改完控制器参数，都强制走一遍悬停 5 秒加扫频 20 秒的流程，确认带宽和超调都在预期内，再去看前飞。这个习惯帮我省掉了无数次“上真机才发现振荡”的后悔药。希望帮到你。

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