从零实现矢量控制飞控：FOC电机控制与姿态闭环全链路解析

如果你打算从零做一套矢量控制飞控算法系统，最该先想清楚的，不是要用什么高级公式，而是你最终想让它控制什么对象。矢量控制飞控，简单说就是把 FOC 电机控制和飞行器姿态控制放在同一条链路上，从姿态角、角速度一直闭环到电机三相电流。它解决的不是“怎么飞起来”，而是普通飞控只发油门、不直接管电机力矩的问题。这套系统适合做高性能无人机、云台、机器人关节这类对出力精度和响应速度有要求的场景。我大三那年用一年时间从硬件选型做到整机联调，后面这些内容基本是我踩完坑之后的整理版本。如果你也打算做同类学生项目，或者想把自研飞控的控制链路做深，可以直接按章节读。

1. 先搞懂矢量控制飞控到底在控什么

1.1 矢量控制不是改个公式，是控制结构变了

很多初学者会把 FOC 理解成“把方波驱动换成正弦波驱动”，其实不止。矢量控制的本质是针对永磁同步电机建立旋转坐标系，让电机的励磁分量和力矩分量解耦，最终实现力矩的快速、精确控制。

在飞行器上，这意味着飞控输出的不再是一路占空比油门，而是每个电机的期望力矩。这个期望力矩会继续经过电流环、坐标变换、SVPWM，变成三相逆变器的六个开关信号。普通电调方案里，飞控只发一个油门值给电调，电调内部自己决定怎么换相。矢量控制飞控则把电机换相和电流反馈的控制权拿到了飞控这一侧。所以它不是一个技巧，而是一整套控制结构的变化。

之前看别人的代码，总觉得“FOC 也不难，无非就是 Clark 变换、Park 变换、SVPWM”，真正跑了之后才明白，这几个变换只是中间一段，前后还挂着电流采样、PID 环路、编码器角度、状态机、故障保护和实时调度。任何一个环节抖动，最终都会在电机轴上传出来。

1.2 和普通飞控的差别：从“油门输出”变成“力矩输出”

普通多旋翼飞控的任务链路一般是这样的：传感器数据 -> 姿态解算 -> 角度环和角速度环 -> 输出油门期望。油门期望送给电调，电调再通过反电动势或霍尔信号自行换相。这个方案的好处是简单、兼容性好，大多数消费级整机采用这种架构。

矢量控制飞控的链路会更长：传感器数据 -> 姿态解算 -> 角度环和角速度环 -> 力矩分配 -> 每个电机的电流环 -> Park 逆变换 -> SVPWM -> 逆变器。多出来的这一段，让系统必须知道每个电机的转子角度、三相电流和母线电压。代价是硬件更复杂、计算量更大、调试项更多，换来的是响应更快、力矩输出更平滑、低速性能更好。

这里有个容易误判的点：不是所有项目都需要矢量控制。如果你只是做一架 FPV 穿越机娱乐飞行，普通飞控加成品电调是最稳的方案。如果你要做自研四旋翼、小型涵道、云台或者机器人关节，想从底层验证“从姿态到电流”的完整链条，那矢量控制飞控才是有价值的。先确定对象，再决定控制结构，能省掉很多白干的活。

2. 硬件选型和软件环境怎么定

2.1 主控芯片：F103 能不能用

做自研矢量控制飞控，主控选型直接影响后续开发效率。很多人看到 STM32F103C8T6 最小系统板便宜，就想着先用它跑通 FOC。这件事要分开看：如果只是验证单个电机的开环转动、跑通坐标变换和 SVPWM，芯片完全够用。但如果要在同一颗芯片上同时跑姿态解算、MAVLink 通信、四路电机 FOC、遥控器输入和故障保护，F103 的算力会非常紧张。

我建议用两阶段思路：第一阶段用 F103 最小系统板做单电机调试，纯粹熟悉驱动流程；第二阶段换到算力更充足的 F405、F407 或者 G4 系列，再开始做完整飞控。G4 系列本身带浮点运算和高级定时器，做 FOC 会更顺手。这里要强调一下，不是型号越高越好，而是看你跑的环路数量、控制频率和通信负载。

主控选型时还要注意几个硬指标：浮点运算能力、定时器数量、ADC 通道数量、串口数量、是否有硬件除法、PWM 分辨率、Flash 和 RAM 大小。飞控不是只跑一个算法，还要把日志数据和调试信息存下来，Flash 太小会很痛苦。

2.2 传感器、驱动板和编码器怎么配

矢量控制飞控需要两类核心传感器。一类是姿态传感器，负责测飞行器的角速度和加速度；另一类是电机转子角度传感器，负责告诉控制器当前转子的电气角度。不少初学者容易把这两类传感器混在一起，导致电机不转或电流失控。

姿态传感器最常见的入门型号是 MPU6050，资料多、便宜，但噪声和温漂在长时间运行时会比较明显。更高阶一点可以换 BMI088、ICM42688 等，零偏和噪声会好不少。对自研飞控来说，传感器数据质量比数据手册标称精度更重要，因为控制性能最终取决于你对数据做了多少滤波和融合。

电机转子角度传感器建议优先选磁编码器，比如常见 AS5600、TLE5012B 等，而不是靠反电动势过零检测。反电动势方案在低速下很难做稳，矢量控制更需要精确角度。AS5600 这类磁编码器接线简单，输出可以直接接主控的 SPI 或 I2C，也能按 PWM 方式读取。用的时候要自己先做“零点校准”，找到电机 A 相和编码器零点的固定偏移，否则电流环会失控。

电调侧也要特别注意。普通 PWM 电调输入的是油门信号，不能直接用来做 FOC，因为它们内部已经自己换相了。你需要自己搭三相全桥加栅极驱动，或者使用支持外部 FOC 控制的电机驱动板。栅极驱动要选带电流采样的方案，常见做法是下桥臂串联采样电阻配合运放放大，再把相电流送到主控 ADC。

2.3 开发环境与实时性要求

矢量控制飞控的开发环境通常还是以 Keil、STM32CubeIDE 或者 VSCode 加 CMake 为主。学生项目建议别折腾太复杂的构建系统，先把逻辑跑通，再用自动化脚本补充。HAL 库用起来方便，但中断回调多、代码路径长；如果做 FOC 对延迟敏感，有些地方直接操作寄存器会更可控。我不是说 HAL 不能用，而是要学会看底层到底做了什么，避免在电流采样中断里调用很重的库函数。

实时性要求需要在项目开始前定下来。常见参考范围是：电流环 10kHz 到 20kHz，角速度环 1kHz 到 4kHz，角度环 500Hz 到 1kHz，MAVLink 发送 10Hz 到 50Hz。这个数值不是官方标准，是多数 FOC 类无人机控制项目的常用区间。控制频率定太高，中断负载大，日志也会被冲掉；定太低，电机力矩会抖，姿态也不稳。

我个人的建议是先把电流环跑在 10kHz，角速度环 2kHz，角度环 1kHz，其余任务用优先级最低的形式去处理。实测时再根据 CPU 占用和电机发热情况调整。

注意：第一版不要急着把四个电机全部接上电。先用一个电机、固定夹具加电流环验证，确认坐标变换方向和编码器零点正确之后，再碰整机。否则一次接错线，轻则炸管，重则烧驱动。

3. 核心算法拆解与实现顺序

3.1 姿态解算：先选互补滤波还是 Mahony

姿态解算是飞控最前面的一层，负责把陀螺仪、加速度计的数据融合成可靠的姿态角。常见做法有两种：互补滤波和 Mahony 姿态解算，还有一种更重的卡尔曼滤波。很多新手一上来就想上卡尔曼，其实没必要。FOC 飞控调试阶段，姿态解算更看重的是稳定、低延迟、可预测，互补滤波和 Mahony 的资料多、参数直观，适合先跑起来。

互补滤波的核心思路是利用陀螺仪短时间准确、加速度计长时间稳定的特点，用高通和低通把两者融合。它需要调的参数不多，一般就是比例系数。Mahony 本质也是互补滤波的一个变体，通过 PI 修正陀螺仪偏差，在 IMU 融合里很常见。我建议先写 Mahony，因为在工程里验证过的人很多，后续出问题也容易排查。

除非你要做长时间高精度自主悬停，否则不急着上卡尔曼。卡尔曼不是万能的，参数不收敛时，输出可能比互补滤波更难判断。写姿态解算之前，先把传感器原始数据用串口看一遍，确保加速度计在静止时读数稳定、陀螺仪零偏在一个可接受的范围内。数据没干净之前，任何滤波都只是把噪声包装得更圆滑。

3.2 嵌套控制回路：角度环、角速度环、电流环

矢量控制飞控的控制回路通常是一个嵌套结构。最外层是角度环，输出角速度期望；中间是角速度环，输出力矩期望；最内层是电流环，直接控制电机的三相电流。嵌套的原因是内环响应快、外环响应慢，这样可以让外环的修正逐步传导到电机上，避免直接对角度输出 PWM 导致的剧烈振荡。

PID 参数调整的顺序也有讲究。先调最内层电流环，让它对阶跃力矩指令响应迅速且不振荡；再调角速度环，最后调角度环。如果反过来先调角度环，你会发现任何异常都被角度控制器放大，很难判断是哪一层出了问题。学生项目里遇到“姿态一直抖”的情况，大概率是角速度环还没调好，外环就开始大量给指令。

PID 的具体系数没有固定答案，和电机、桨叶、电池、机架刚度都有关。我能给的建议是：先固定采样频率，再用纯 P 控制慢慢加，直到出现等幅振荡，然后退回到振荡值的百分之六七十，再加一点 D 抑制超调。I 项最后加，它能消除静差，但加多了会让系统变迟钝，甚至引发低频振荡。

3.3 FOC 坐标变换：Clark、Park 和 SVPWM

FOC 的数学链路可以拆成三个明显步骤。第一步是 Clark 变换，把三相电流从 abc 坐标转到静止的 αβ 坐标；第二步是 Park 变换，根据转子电角度，把 αβ 坐标转到旋转的 dq 坐标；第三步在 dq 坐标系里做电流控制，然后通过 Park 逆变换和 SVPWM，输出三相逆变器的开关时间。

Clark 变换和 Park 变换本身不复杂，公式也很多资料可以查。难点在于方向约定：电流采样方向、编码器旋转方向、电机三相顺序，只要其中一个是反的，整个环就会失控。调试时你会发现，平面图上的正方向和你实际电机转动方向不一定一致，必须对着电机的实际旋转去修正符号。最好的办法是准备一台可固定的电机，用一个固定的电角度持续输出，观察转子是否和理论方向一致。

SVPWM 的作用是把 dq 坐标系下算出来的电压矢量转换成三相半桥的占空比。它比简单的 SPWM 在母线电压利用率和谐波表现上更好，也是 FOC 里比较成熟的一步。实现时只要控制好 PWM 周期和死区时间，一般不会出太大问题。死区时间太短会桥臂直通，太长会增加电流谐波，具体值要看驱动芯片的规格，以不炸管和不明显发热为准。

注意：坐标变换的三角函数计算会消耗不少 CPU 时间。如果在低端芯片上跑，建议提前算好正弦表，或者使用查表加插值的方式，把电流环中断时间控制在安全范围内。

4. 从最小系统到整机调试

4.1 单电机开环验证

整个项目的第一个里程碑不是把飞控写完整，而是让一个电机在 FOC 驱动下能连续转动。这个阶段建议用开环控制，不接电流反馈，只给定一个固定的 Vd 和 Vq，或者直接跑 SVPWM 产生旋转磁场，看电机是否跟着起动。

开环调试前要把电机固定在台钳或支架上，绝不能用手轻拿着通电。开始转动后，逐步提高转速指令，观察电流是否平滑、有没有啸叫、模块有没有过热。如果电机只振动不转，先查三相线序；如果转起来电流很大但力矩很小，先查编码器零点和角度方向。开环测试是排查硬件接线的最好窗口，因为还没有闭环控制帮忙掩盖问题。

这个阶段我会习惯性地记录母线电压、电流和温度。如果母线电流持续上升，说明某个地方短路或占空比不对，应该立即断电检查，而不是继续调参数。单电机开环稳定之后，再开启电流闭环，把转矩电流 Id 控制到接近零，把交轴 Iq 的响应调好。

4.2 电流环和速度环调试顺序

电流环是 FOC 里最内层、频率最高的环。调试时可以先给定一个固定的 Iq，观察实际电流能不能快速跟上去。如果电流有大幅超调，减少 PID 的 P 值；如果电流滞后，检查环路频率是否太低。电流环调好之后，电机在负载变化时应该能保持输出力矩稳定。

速度环其实在很多飞行器项目里不是必须的，因为四旋翼悬停时更依赖角速度环和角度环。但如果你还做云台或机器人关节，速度环会很有用。速度环调试可以在电流环的上面叠加，先让电机转速跟随一个目标值，看能不能平稳到达并保持。不要急着追求极高的速度，先把低速稳定性和加减速过程做好。

很多项目死在“电流环还没稳定就跑到整机姿态测试”。电流环一旦有延迟或振荡，后面的角度环、角速度环无论怎么调都压不住。所以我的经验是：电流环花掉整机调试一半时间都不亏。先用示波器看电流波形，再调代码参数，不要闭着眼睛调 PID。

4.3 姿态闭环和整机联调

单电机、电流环、角速度环都稳定之后，才会考虑整机姿态闭环。整机联调先不要上桨，或者只上很小的保护桨，把机架绑在一个可旋转的支架上，让飞控只在一个轴向上测试。先做横滚轴或俯仰轴，验证姿态解算方向、电机旋转方向、控制输出方向三者一致，再切换到真机。

很多第一次做飞行器的人，总想在地面站里看到完美的“水平仪”，再把油门推上去。但真正的风险不在姿态解算显示，而在闭环极性。有一次我周旋了一下午，发现俯仰角增大时电机输出反而往错误方向走。这个只能靠逐步加小指令、观察姿态响应方向来确认。方向对了，再谈 PID 精度；方向反了，给 P 越大，炸得越快。

整机联调时建议把油门限制和倾角限制写死在代码里，防止初期失控后姿态翻转过快。同时接一个物理开关或者遥控器急停通道，出现抖动、啸叫、异响时第一时间断电。地面站日志也要在这个阶段正式启用，至少记录姿态角、角速度、每个电机的电流指令和实际电流。没有日志的飞行调试，都只是靠运气。

5. 飞控通信和地面站

5.1 MAVLink 心跳和消息解析

矢量控制飞控要在地面站里联调，多数情况会走 MAVLink 协议。MAVLink 是一种面向无人机和地面站之间的消息协议，规定了心跳、姿态、系统状态、指令等消息格式。自研飞控不可能一上来就实现全部消息，先实现心跳、姿态、系统状态和简单的命令接收就够了。

心跳消息是最基础的一环，地面站靠它判断飞控是否在线。如果 Mission Planner 连不上飞控，先看串口号、波特率、心跳是否连续 1Hz 发送，再看消息校验和帧格式。很多人第一次连地面站失败，不是协议实现问题，而是 USB 转串口驱动没装好，或者波特率不对。先确保心跳消息连续发送，再跑姿态消息，否则地面站页面一直显示“离线”。

开发时建议把 MAVLink 解析放在主循环的较低优先级上，不要在电流环中断里做字符串解析。MAVLink 对实时性要求不高，就算偶尔丢几帧，控制也不会出问题。反而在中断里做重量级协议解析，会把电机控制周期打乱。

5.2 Mission Planner 联调要点

Mission Planner 是常见的飞控地面站软件，可以接收 MAVLink 消息，也可以向飞控发送 MAVLink 信息。自研飞控和 Mission Planner 联调时，先做只读联调：飞控回传姿态和状态，地面站能正常显示。之后再做写入联调：地面站发送解锁、起飞、切换模式等指令，飞控正确解析并回应 ACK。

这里要注意，Mission Planner 对自研飞控并不像对 PX4/ArduPilot 那样有完整功能支持。它能显示多少内容、支持哪些页面，取决于你实现了哪些 MAVLink 消息。所以不要把“用 Mission Planner 飞起来”当成目标，更适合把 Mission Planner 当成姿态查看器和指令下发工具，真正的飞控逻辑仍然在自己代码里验证。

联调时遇到“能收到姿态但发指令没反应”，先检查飞控是否把接收到的消息打印到了调试串口，再看消息 ID 和校验和是否正确。不要怀疑地面站，多数情况下是自研飞控的消息筛选和应答逻辑没做全。

5.3 日志记录与数据回放

飞行器项目里最容易忽略的就是日志系统。没有日志，电机抖动、姿态漂移、通信中断这些问题都只能靠猜。自研飞控至少需要保存几类数据：时间戳、姿态角、角速度、每个电机的期望电流、实际电流、母线电压、 CPU 占用率、通信丢包率。

日志存储方式要按芯片能力来选择。Flash 容量大，可以写循环日志；如果 Flash 小，可以把日志通过串口传输到地面站或者 SD 卡。回放时我最常用的方式是把日志导出成 CSV 或者直接画时间曲线。调 PID 时，只看一次响应曲线是不够的，要连续记录多次，看趋势是否一致。能复现的结果才有价值。

6. 项目中最容易翻车的几个点

6.1 参数调不好，先查数据链路，再调软件

很多同学调 FOC 时，遇到电机抖动、电流异常，第一反应是改 PID。但根据我的经验，大部分问题不在 PID，而在数据链路上。要么编码器角度读取有延迟，要么相电流采样时机不对，要么 ADC 的参考电压不稳定。数据没采对，控制算法再好也没用。

排查顺序我一般固定成五步：先看现象，是振动、啸叫、堵转还是发热；再看传感器，编码器角度曲线是否连续，电流波形是否干净；然后查控制链路方向，三相顺序、旋转方向、正负号；接着看环路频率，是否被中断或其他任务拖慢；最后才看 PID 参数。如果一上来就调 P、I、D，很容易把一个参数错配的现象掩盖成另一个参数错误，越调越乱。

6.2 实时任务调度和资源占用

自研飞控的项目一旦进入整机调试，CPU 占用会迅速上升。电流环、角速度环、姿态解算、MAVLink、日志、遥控器解码、电池电压监测，每一个功能都想抢时间。如果没有一个清晰的调度表，后面的功能加得越多，系统越不稳定。很多时候电机突然抖一下，不是算法问题，而是某个低优先级任务占用了太长的时间，把电流环的周期打断了。

建议从第一版代码就把任务调度表规划好。电流环放最高优先级定时器中断，姿态解算放次一级定时器中断，MAVLink、日志、状态机放主循环轮询。每次加新功能，都要看 CPU 占用率和最坏情况下的中断响应时间。如果 CPU 占用长期超过百分之七八十，就开始考虑降频、精简日志或换更强主控。

6.3 学生项目规划建议

用一个学年做矢量控制飞控，时间其实不宽裕。我建议把项目切成四个阶段：前三个月把电机驱动、电流环和编码器调通；中间三个月做姿态解算和角速度环；再三个月做整机姿态闭环和地面站联调；最后两个月留出缓冲，写文档、做演示、处理电机损坏和模块烧毁的意外。不要把所有时间压到最后半学期，飞控项目几乎一定会延期。

如果时间实在不够，可以适当砍功能。砍掉速度环，优先保证电流环和角度环；砍掉复杂卡尔曼，先用 Mahony；砍掉自定义地面站，只对接 Mission Planner。这个项目真正出彩的地方是“从姿态到电流的完整闭环”，而不是功能数量。把一条链路做到稳定，比把十个功能做到半成品更有说服力。

最后说一句实际的：这个项目周期长、硬件风险高、调试时成就感也不像写网页那样即时。但它能让你把传感器、嵌入式、控制理论、通信协议和系统工程串在一起。只要方向不差、数据链路清晰、风险控制到位，就算最后没有达到商业级效果，整个一年下来学到的东西也足够回本。

如果让我重做一遍，我会把更多时间放在自动化日志和分析工具上。飞控这种东西，能稳定复现一次结果，比写出十个炫酷算法都重要。矢量控制飞控的难度不在某一个算法，而在所有环节同时稳定地协同工作。这一点想明白了，后面的路会顺很多。