矢量控制飞控算法系统听起来像一个把电机控制和无人机导航揉在一起的大命题。实际拆解下来它确实不是一两个库函数能搞定的要做永磁同步电机的电流闭环要把姿态传感器数据变成稳定的欧拉角还要让地面站通过 MAVLink 协议控制飞控。如果一个项目能把这三层全部打通那基本就把飞控系统的骨架掌握了。下面以一个从大三开始历时一年完成的矢量控制飞控算法系统为背景从电机控制、姿态解算、MAVLink 通信、调试排错四个方面复盘整个系统的设计和实现。读完整篇文章后你会得到一条完整的学习路径先复现 FOC 电流闭环再接入姿态环最后连接 Mission Planner 观察和下发指令最终实现对无刷电机甚至四旋翼的稳定控制。这类项目最容易出现的问题是电路能转、串口能打印但姿态不稳定或者地面站连不上。根本原因通常不是某个算法太难而是各层之间的衔接没有做扎实。因此这篇文章不会只讲某个公式而是把系统拆开按实际开发顺序讲清楚每一层的输入、输出和验证方法。1. 先理解矢量控制飞控系统的分层架构1.1 为什么飞控底层要选择矢量控制飞控系统的执行机构是电机。传统电调大多采用方波驱动通过六步换相让电机转起来。这种驱动方式实现简单但力矩脉动大低速时不够平滑响应速度也不够快。矢量控制也就是磁场定向控制 FOC则通过坐标变换和电流闭环让定子电流矢量始终与转子磁场保持最优角度从而获得接近线性的力矩输出。在飞控场景中线性力矩意味着更稳定的姿态控制能力。电机转速变化越快姿态环的调节余量就越大。尤其是四旋翼这种经常需要急加速、急减速的载体底层电机如果响应慢上层姿态环就会被拖累出现“给油抬头、收油低头”的滞后现象。所以把 FOC 作为飞控底层的电机控制方案本质上是把执行机构的性能余量做大。学习时不需要先入为主地背公式而是要始终记住一个目标控制三相电流让电机产生的力矩又快又准。1.2 从地面站指令到电机输出的完整数据链路整个系统可以拆成五个层级地面站层Mission Planner 通过 MAVLink 发送姿态指令或位置指令。位置环与姿态环飞控把目标位置或目标姿态转换成期望力矩。混控分配把期望力矩分配到每个电机。FOC 电流环每个电机的控制器把力矩指令转换成期望电流。功率驱动层SVPWM 生成三相 PWM驱动逆变器输出到电机。反馈链路也值得理清楚三相电流经过 Clark、Park 变换进入电流环陀螺仪角速度进入角速度环加速度计、陀螺仪、磁力计经过姿态解算输出欧拉角进入角度环如果启用了 GPS 或光流位置信息进入位置环。这实际上是一个级联 PID 结构。底层环路的频率高顶层环路的频率低。写代码之前先画出这个数据流图能避免后面把模块之间接口设计得不一致。1.3 单 MCU 与双 MCU 架构STM32F103C8T6 的能力边界项目标题里出现的 stm32f103c8t6 最小系统板是很多学生项目的主力主控。它确实能跑 FOC但性能边界必须提前确认。主频72 MHz Flash128 KB RAM20 KB FPU无 高级定时器TIM1可输出互补 PWM ADC2 个 12 位 ADC 串口3 个 USART在无 FPU 的情况下float 运算需要软件浮点库完成耗时明显高于 M4 内核。如果电流环做到 10 kHz姿态解算做到 500 Hz再加上 MAVLink 解析与发送单颗 F103 的负载已经接近 80%。这意味着每一个中断里都不能做多余的事情。实际项目中常见做法有两种方案一单 MCU 简化方案 使用 STM32F103C8T6电流环 8~10 kHz姿态环 500 HzMAVLink 按需发送。 特点硬件简单适合学习和验证完整链路。 方案二双 MCU 方案 STM32G4 或 F4 负责 FOC 电流环F103 负责姿态解算、通信和任务调度。 特点负载更均衡扩展性好但调试复杂度上升。如果项目目标是验证“矢量控制飞控算法”这个完整概念单 F103 方案完全可行。但如果目标是长时间悬停、加入定位、路径规划建议直接换用带 FPU 的 M4 或 M7 主控或者走双 MCU 分工。注意不要一上来就铺开写全部模块。先把 F103 的最小系统、电流采样、PWM 输出跑通再逐步叠加姿态和通信这样每一步出了问题都能快速定位。2. 永磁同步电机 FOC 控制从坐标变换到 SVPWM2.1 Clark、Park 坐标变换的意义与 C 实现三相永磁同步电机的定子电流是三相交流量。直接对三相电流分别做 PID本质上并不可靠因为三相信号之间存在耦合。更好的思路是把三相交流量先变成两相静止坐标再变成与转子同步旋转的 dq 坐标。在 dq 坐标下id 代表励磁分量iq 代表转矩分量。对于表贴式 PMSM通常让 id 等于 0只通过 iq 控制力矩。Clark 变换把三相电流变换到两相静止坐标系#define SQRT3_2 0.86602540378443864676f void clark_transform(float ia, float ib, float ic, float *i_alpha, float *i_beta) { *i_alpha 2.0f / 3.0f * (ia - 0.5f * ib - 0.5f * ic); *i_beta 2.0f / 3.0f * (SQRT3_2 * ib - SQRT3_2 * ic); }Park 变换把两相静止坐标变换到旋转 dq 坐标这里的 theta 是转子电角度#include math.h void park_transform(float i_alpha, float i_beta, float theta, float *id, float *iq) { float cos_t cosf(theta); float sin_t sinf(theta); *id i_alpha * cos_t i_beta * sin_t; *iq -i_alpha * sin_t i_beta * cos_t; }反 Park 变换用于把电流环输出的 Vd、Vq 转换回静止坐标系再交给 SVPWMvoid inverse_park_transform(float vd, float vq, float theta, float *v_alpha, float *v_beta) { float cos_t cosf(theta); float sin_t sinf(theta); *v_alpha vd * cos_t - vq * sin_t; *v_beta vd * sin_t vq * cos_t; }这里特别要注意电角度和机械角度的关系。如果电机有 7 对极那么转子每转一圈电角度变化 7 圈。得到机械角度之后必须乘上极对数才能用于 Park 变换。2.2 三相电流采样、PWM 输出与 SVPWM 实现要点FOC 控制的效果很大程度取决于电流采样质量。常见做法是使用低侧采样电阻配合运放将微小的电流信号放大到 ADC 可测范围。STM32F103 的 ADC 需要借助定时器触发在 PWM 中心对齐的时刻采样因为此时电流变化最平缓采样值最接近真实相电流。// 典型配置思路 // 1. TIM1 配置为中央对齐 PWM输出互补 PWM死区时间根据功率管参数设置 // 2. ADC1 配置为注入组采样由 TIM1 TRGO 或 CC 事件触发 // 3. 在 ADC 注入组转换完成中断中读取三相电流 // 4. 使用 DMA 搬运采样结果避免在中断里做过多处理SVPWM 是 FOC 里最接近硬件的一层。它的目标是根据 V_alpha、V_beta 计算出三个桥臂的占空比。SVPWM 的基本思路是把电压矢量放在六个扇区中用相邻两个非零矢量和零矢量合成目标矢量。// SVPWM 核心步骤 // 1. 根据 V_alpha、V_beta 判断扇区 // 2. 计算相邻矢量的作用时间 T1、T2 // 3. 对 T1、T2 做限幅防止过调制 // 4. 把 T1、T2 映射到三个比较寄存器完整的 SVPWM 代码较长关键在于扇区判断和矢量作用时间计算。建议先用 MATLAB 或 Python 验证 SVPWM 公式再移植到 C避免直接在单片机上调试数学问题。2.3 电流环 PI 控制器与抗积分饱和电流环是飞控里最内层的闭环典型频率可以做到 10 kHz 到 20 kHz。电流环的 PI 输出是 Vd、Vq控制量必须限幅否则输出超出母线电压后PWM 会进入过调制区域电流波形失真。typedef struct { float kp; float ki; float integral; float out_max; } pi_controller_t; float pi_update(pi_controller_t *pi, float target, float feedback) { float error target - feedback; float out pi-kp * error pi-ki * pi-integral; // 抗积分饱和 if (out pi-out_max) { out pi-out_max; } else if (out -pi-out_max) { out -pi-out_max; } else { pi-integral error; } return out; }这段代码采用“输出饱和时停止积分”的简单策略能避免积分饱和导致的大超调。实际项目中还可以加入积分限幅和变积分系数但初学者先跑通这个版本就够了。电流环调参建议参数调节方式观察现象Kp从小到大逐步增加Kp 过大会导致电流震荡、电机啸叫KiKp 稳定后慢慢增加Ki 用于消除稳态误差过大会导致低频振荡输出限幅设置为主电源电压对应占空比上限限幅过小会导致电流闭环无法到达目标2.4 开环验证到电流闭环的切换顺序FOC 最容易出现的错误是相线接错或编码器角度方向不对。如果不先做开环验证直接切闭环电机会出现剧烈抖动甚至过流。推荐顺序开环给电压手动递增电角度让电机缓慢转动。确认相序如果电机反转交换任意两相线或者改变电角度增量方向。确认电角度用手转动电机观察估算角度是否连续变化。切电流环给定 id 等于 0逐步增加 iq观察电流是否跟随。切速度环电流环稳定后再加速度环。// 开环到闭环的切换逻辑示例 control_mode MODE_OPENLOOP; // 初始为开环 // 主循环或控制中断中 if (control_mode MODE_OPENLOOP) { openloop_angle 0.01f; // 手动递增电角度 v_alpha 1.5f * cosf(openloop_angle); v_beta 1.5f * sinf(openloop_angle); svpwm_set(v_alpha, v_beta); } else if (control_mode MODE_FOC_CLOSEDLOOP) { // 读取三相电流 // Clark、Park 变换 // 电流 PI 运算 // 反 Park、SVPWM }开环阶段不依赖电流反馈只要 PWM 和电机线序正确电机就能转。这一步能快速排除硬件接线问题避免直接调闭环时撞上两个叠加的故障。3. 姿态解算与姿态控制环3.1 传感器数据预处理零偏、量程与低通滤波姿态环的基础是传感器数据。MPU6050 内部集成了三轴陀螺仪和三轴加速度计在初始化时需要配置量程和采样率。陀螺仪量程可以配置为 ±500°/s 或 ±1000°/s加速度计量程通常配置为 ±4g 或 ±8g。读取原始数据后需要转换成物理量// 以 MPU6050 为例配置量程为 ±500°/s // 原始值范围 -32768 到 32767 float gyro_rate (float)raw_gyro / 65.5f; // 单位°/s // 配置量程为 ±4g float accel (float)raw_accel / 8192.0f; // 单位g陀螺仪存在零偏静态时读数可能不是 0。最简单的零偏标定方法是上电后保持静止采样一段时间取平均再在运行时减去这个零偏值。加速度计数据通常叠加了高频振动噪声需要做低通滤波。一阶低通滤波的 C 实现非常轻量float lowpass_filter(float input, float last_output, float alpha) { return alpha * input (1.0f - alpha) * last_output; }alpha 的值越小滤波越平滑但延迟越大。在飞控里低通滤波的延迟会进入控制环因此 alpha 不能太小。3.2 基于四元数的 Mahony 姿态解算姿态解算的方法很多有欧拉角直接积分、方向余弦矩阵、四元数互补滤波、扩展卡尔曼滤波等。在资源有限的 F103 上Mahony 四元数互补滤波是性价比较高的方案。四元数用四个变量表示旋转避免了欧拉角的万向锁问题。Mahony 滤波的核心思想是陀螺仪积分提供短期姿态加速度计和磁力计提供长期参考方向两者通过 PI 补偿融合。typedef struct { float q0, q1, q2, q3; } quaternion_t; quaternion_t q {1.0f, 0.0f, 0.0f, 0.0f}; void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float kp, float ki, float dt) { float norm; float vx, vy, vz; float ex, ey, ez; float ex_int 0, ey_int 0, ez_int 0; // 归一化加速度计 norm sqrtf(ax * ax ay * ay az * az); if (norm 1e-6f) return; ax / norm; ay / norm; az / norm; // 归一化磁力计此处按需求决定是否使用 if (mx ! 0.0f || my ! 0.0f || mz ! 0.0f) { norm sqrtf(mx * mx my * my mz * mz); mx / norm; my / norm; mz / norm; } // 由四元数计算重力方向 vx 2.0f * (q.q1 * q.q3 - q.q0 * q.q2); vy 2.0f * (q.q0 * q.q1 q.q2 * q.q3); vz q.q0 * q.q0 - q.q1 * q.q1 - q.q2 * q.q2 q.q3 * q.q3; // 向量叉积作为误差 ex ay * vz - az * vy; ey az * vx - ax * vz; ez ax * vy - ay * vx; // 积分误差 ex_int ex * ki * dt; ey_int ey * ki * dt; ez_int ez * ki * dt; // 用误差修正陀螺仪 gx kp * ex ex_int; gy kp * ey ey_int; gz kp * ez ez_int; // 四元数微分方程更新 float q0 q.q0, q1 q.q1, q2 q.q2, q3 q.q3; q.q0 q0 0.5f * dt * (-q1 * gx - q2 * gy - q3 * gz); q.q1 q1 0.5f * dt * ( q0 * gx q2 * gz - q3 * gy); q.q2 q2 0.5f * dt * ( q0 * gy - q1 * gz q3 * gx); q.q3 q3 0.5f * dt * ( q0 * gz q1 * gy - q2 * gx); // 归一化四元数 norm sqrtf(q.q0 * q.q0 q.q1 * q.q1 q.q2 * q.q2 q.q3 * q.q3); q.q0 / norm; q.q1 / norm; q.q2 / norm; q.q3 / norm; }这段代码是 Mahony 滤波的最简形式。实际使用时还需要把四元数转换成欧拉角用于调试和 MAVLink 消息发送。void quaternion_to_euler(quaternion_t *q, float *roll, float *pitch, float *yaw) { *roll atan2f(2.0f * (q-q0 * q-q1 q-q2 * q-q3), 1.0f - 2.0f * (q-q1 * q-q1 q-q2 * q-q2)); *pitch asinf(2.0f * (q-q0 * q-q2 - q-q3 * q-q1)); *yaw atan2f(2.0f * (q-q0 * q-q3 q-q1 * q-q2), 1.0f - 2.0f * (q-q2 * q-q2 q-q3 * q-q3)); }3.3 角度环和角速度环的双环 PID 设计姿态控制通常采用内外环结构。内环是角速度环输出期望力矩外环是角度环输出角速度目标值。内外环分离的好处是角速度环能快速抑制扰动角度环负责把角度拉回目标。常见控制频率范围控制环典型频率作用角速度环400 Hz ~ 1 kHz抑制角速度误差角度环100 Hz ~ 200 Hz稳定姿态角位置环10 Hz ~ 50 Hz控制无人机位置角速度环的 PID 可以直接复用前面写的 PI 控制器但通常需要加入微分项用于抑制高频抖动。角度环一般只使用 P 和少量 D积分项在姿态控制中要非常小心以免在悬停时形成周期性的修正过冲。3.4 四旋翼混控分配姿态指令转电机推力如果项目载体是四旋翼混控分配是把姿态环输出的力矩指令变成四个电机期望推力的关键模块。以 X 型四旋翼为例电机编号按逆时针排列前右为 1后右为 2后左为 3前左为 4。常见的混控关系如下m1 throttle roll - pitch yaw m2 throttle - roll - pitch - yaw m3 throttle - roll pitch yaw m4 throttle roll pitch - yaw这里的 roll、pitch、yaw 是姿态环输出的归一化力矩值throttle 是总油门。实际飞控里throttle、roll、pitch、yaw 都有各自的限幅相加后还需要做饱和处理保证输出不会超过 PWM 最大占空比。需要强调的是混控公式的符号取决于电机的旋转方向和机架布局。不同机架的电机方向、编号不同公式符号也可能不同。调机时先确认电机转向再进行混控测试。4. 飞控与 Mission Planner 的 MAVLink 通信链路4.1 MAVLink 协议帧格式与常用消息MAVLink 是飞控领域最常见的通信协议Mission Planner 通过它读取飞控状态并下发控制指令。MAVLink 2 的帧结构以 0xFD 开头MAVLink 1 以 0xFE 开头。帧里包含长度、序列号、系统 ID、组件 ID、消息 ID、载荷和 CRC 校验。// MAVLink 1 帧的基本结构 typedef struct { uint8_t magic; // 0xFE uint8_t len; // 载荷长度 uint8_t seq; // 消息序号 uint8_t sysid; // 系统 ID uint8_t compid; // 组件 ID uint8_t msgid; // 消息 ID uint8_t payload[255]; uint16_t checksum; } mavlink_frame_t;实际开发中建议直接使用官方 mavlink 库生成 C 代码避免手动处理 CRC 和打包逻辑。下面是一个极简的发送示例说明消息发送的关键流程void send_attitude_message(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; mavlink_message_t msg; mavlink_attitude_t attitude; attitude.time_boot_ms millis(); attitude.roll roll; attitude.pitch pitch; attitude.yaw yaw; attitude.rollspeed rollspeed; attitude.pitchspeed pitchspeed; attitude.yawspeed yawspeed; mavlink_msg_attitude_encode(SYSID, COMPID, msg, attitude); uint16_t len mavlink_msg_to_send_buffer(buffer, msg); uart_write_bytes(buffer, len); }常用 MAVLink 消息消息消息 ID方向作用HEARTBEAT0飞控到地面站心跳表示飞控在线SYS_STATUS1飞控到地面站电量、传感器状态ATTITUDE30飞控到地面站姿态角与角速度LOCAL_POSITION_NED32飞控到地面站位置信息SET_POSITION_TARGET_LOCAL_NED84地面站到飞控下发位置目标4.2 Mission Planner 连接自定义飞控的配置步骤Mission Planner 默认支持 PX4、ArduPilot 等飞控但它本质上是通过 MAVLink 通信的。只要自己的飞控能够按 MAVLink 帧格式发送 HEARTBEAT 和 ATTITUDEMission Planner 就能识别并显示姿态。连接步骤准备一个 USB-TTL 串口模块连接飞控的 UART 引脚。查看电脑设备管理器确定串口编号。打开 Mission Planner选择右上方端口和波特率。波特率常用 115200点击 Connect。连接成功后HUD 界面会显示飞控姿态、电量、飞行模式等信息。如果 Mission Planner 弹出版本兼容问题需要确认飞控发送的 MAVLink 版本。大多数情况下发送 MAVLink 1 和 MAVLink 2 都能被兼容但 sysid 和 compid 必须配置正确。4.3 地面站指令如何作用到电机从 SET_POSITION 到 FOCMission Planner 发送 MAVLink 信息给飞控后飞控并不会直接把接收到的数值写到电机。以 SET_POSITION_TARGET_LOCAL_NED 为例消息里包含位置目标、速度目标和加速度目标。飞控收到后先经过位置环计算速度目标再经过速度环计算姿态目标接着进入姿态环和角速度环最后通过混控分配到 FOC 电流环。这个过程可以理解成逐级缩小误差位置环负责把位置误差变成速度误差速度环把速度误差变成姿态角误差姿态环把角度误差变成角速度误差角速度环把角速度误差变成力矩目标FOC 再把力矩目标变成电流目标。在代码上至少需要维护一个外部指令的接收标志typedef struct { float x; float y; float z; float vx; float vy; float vz; uint16_t type_mask; } position_target_t; position_target_t cmd {0 
长河君 · 2026/9/1 2:10:24 · 阅读 1.2k 矢量控制飞控算法系统实现：FOC、姿态解算与MAVLink通信实战
矢量控制飞控算法系统听起来像一个把电机控制和无人机导航揉在一起的大命题。实际拆解下来它确实不是一两个库函数能搞定的要做永磁同步电机的电流闭环要把姿态传感器数据变成稳定的欧拉角还要让地面站通过 MAVLink 协议控制飞控。如果一个项目能把这三层全部打通那基本就把飞控系统的骨架掌握了。下面以一个从大三开始历时一年完成的矢量控制飞控算法系统为背景从电机控制、姿态解算、MAVLink 通信、调试排错四个方面复盘整个系统的设计和实现。读完整篇文章后你会得到一条完整的学习路径先复现 FOC 电流闭环再接入姿态环最后连接 Mission Planner 观察和下发指令最终实现对无刷电机甚至四旋翼的稳定控制。这类项目最容易出现的问题是电路能转、串口能打印但姿态不稳定或者地面站连不上。根本原因通常不是某个算法太难而是各层之间的衔接没有做扎实。因此这篇文章不会只讲某个公式而是把系统拆开按实际开发顺序讲清楚每一层的输入、输出和验证方法。1. 先理解矢量控制飞控系统的分层架构1.1 为什么飞控底层要选择矢量控制飞控系统的执行机构是电机。传统电调大多采用方波驱动通过六步换相让电机转起来。这种驱动方式实现简单但力矩脉动大低速时不够平滑响应速度也不够快。矢量控制也就是磁场定向控制 FOC则通过坐标变换和电流闭环让定子电流矢量始终与转子磁场保持最优角度从而获得接近线性的力矩输出。在飞控场景中线性力矩意味着更稳定的姿态控制能力。电机转速变化越快姿态环的调节余量就越大。尤其是四旋翼这种经常需要急加速、急减速的载体底层电机如果响应慢上层姿态环就会被拖累出现“给油抬头、收油低头”的滞后现象。所以把 FOC 作为飞控底层的电机控制方案本质上是把执行机构的性能余量做大。学习时不需要先入为主地背公式而是要始终记住一个目标控制三相电流让电机产生的力矩又快又准。1.2 从地面站指令到电机输出的完整数据链路整个系统可以拆成五个层级地面站层Mission Planner 通过 MAVLink 发送姿态指令或位置指令。位置环与姿态环飞控把目标位置或目标姿态转换成期望力矩。混控分配把期望力矩分配到每个电机。FOC 电流环每个电机的控制器把力矩指令转换成期望电流。功率驱动层SVPWM 生成三相 PWM驱动逆变器输出到电机。反馈链路也值得理清楚三相电流经过 Clark、Park 变换进入电流环陀螺仪角速度进入角速度环加速度计、陀螺仪、磁力计经过姿态解算输出欧拉角进入角度环如果启用了 GPS 或光流位置信息进入位置环。这实际上是一个级联 PID 结构。底层环路的频率高顶层环路的频率低。写代码之前先画出这个数据流图能避免后面把模块之间接口设计得不一致。1.3 单 MCU 与双 MCU 架构STM32F103C8T6 的能力边界项目标题里出现的 stm32f103c8t6 最小系统板是很多学生项目的主力主控。它确实能跑 FOC但性能边界必须提前确认。主频72 MHz Flash128 KB RAM20 KB FPU无 高级定时器TIM1可输出互补 PWM ADC2 个 12 位 ADC 串口3 个 USART在无 FPU 的情况下float 运算需要软件浮点库完成耗时明显高于 M4 内核。如果电流环做到 10 kHz姿态解算做到 500 Hz再加上 MAVLink 解析与发送单颗 F103 的负载已经接近 80%。这意味着每一个中断里都不能做多余的事情。实际项目中常见做法有两种方案一单 MCU 简化方案 使用 STM32F103C8T6电流环 8~10 kHz姿态环 500 HzMAVLink 按需发送。 特点硬件简单适合学习和验证完整链路。 方案二双 MCU 方案 STM32G4 或 F4 负责 FOC 电流环F103 负责姿态解算、通信和任务调度。 特点负载更均衡扩展性好但调试复杂度上升。如果项目目标是验证“矢量控制飞控算法”这个完整概念单 F103 方案完全可行。但如果目标是长时间悬停、加入定位、路径规划建议直接换用带 FPU 的 M4 或 M7 主控或者走双 MCU 分工。注意不要一上来就铺开写全部模块。先把 F103 的最小系统、电流采样、PWM 输出跑通再逐步叠加姿态和通信这样每一步出了问题都能快速定位。2. 永磁同步电机 FOC 控制从坐标变换到 SVPWM2.1 Clark、Park 坐标变换的意义与 C 实现三相永磁同步电机的定子电流是三相交流量。直接对三相电流分别做 PID本质上并不可靠因为三相信号之间存在耦合。更好的思路是把三相交流量先变成两相静止坐标再变成与转子同步旋转的 dq 坐标。在 dq 坐标下id 代表励磁分量iq 代表转矩分量。对于表贴式 PMSM通常让 id 等于 0只通过 iq 控制力矩。Clark 变换把三相电流变换到两相静止坐标系#define SQRT3_2 0.86602540378443864676f void clark_transform(float ia, float ib, float ic, float *i_alpha, float *i_beta) { *i_alpha 2.0f / 3.0f * (ia - 0.5f * ib - 0.5f * ic); *i_beta 2.0f / 3.0f * (SQRT3_2 * ib - SQRT3_2 * ic); }Park 变换把两相静止坐标变换到旋转 dq 坐标这里的 theta 是转子电角度#include math.h void park_transform(float i_alpha, float i_beta, float theta, float *id, float *iq) { float cos_t cosf(theta); float sin_t sinf(theta); *id i_alpha * cos_t i_beta * sin_t; *iq -i_alpha * sin_t i_beta * cos_t; }反 Park 变换用于把电流环输出的 Vd、Vq 转换回静止坐标系再交给 SVPWMvoid inverse_park_transform(float vd, float vq, float theta, float *v_alpha, float *v_beta) { float cos_t cosf(theta); float sin_t sinf(theta); *v_alpha vd * cos_t - vq * sin_t; *v_beta vd * sin_t vq * cos_t; }这里特别要注意电角度和机械角度的关系。如果电机有 7 对极那么转子每转一圈电角度变化 7 圈。得到机械角度之后必须乘上极对数才能用于 Park 变换。2.2 三相电流采样、PWM 输出与 SVPWM 实现要点FOC 控制的效果很大程度取决于电流采样质量。常见做法是使用低侧采样电阻配合运放将微小的电流信号放大到 ADC 可测范围。STM32F103 的 ADC 需要借助定时器触发在 PWM 中心对齐的时刻采样因为此时电流变化最平缓采样值最接近真实相电流。// 典型配置思路 // 1. TIM1 配置为中央对齐 PWM输出互补 PWM死区时间根据功率管参数设置 // 2. ADC1 配置为注入组采样由 TIM1 TRGO 或 CC 事件触发 // 3. 在 ADC 注入组转换完成中断中读取三相电流 // 4. 使用 DMA 搬运采样结果避免在中断里做过多处理SVPWM 是 FOC 里最接近硬件的一层。它的目标是根据 V_alpha、V_beta 计算出三个桥臂的占空比。SVPWM 的基本思路是把电压矢量放在六个扇区中用相邻两个非零矢量和零矢量合成目标矢量。// SVPWM 核心步骤 // 1. 根据 V_alpha、V_beta 判断扇区 // 2. 计算相邻矢量的作用时间 T1、T2 // 3. 对 T1、T2 做限幅防止过调制 // 4. 把 T1、T2 映射到三个比较寄存器完整的 SVPWM 代码较长关键在于扇区判断和矢量作用时间计算。建议先用 MATLAB 或 Python 验证 SVPWM 公式再移植到 C避免直接在单片机上调试数学问题。2.3 电流环 PI 控制器与抗积分饱和电流环是飞控里最内层的闭环典型频率可以做到 10 kHz 到 20 kHz。电流环的 PI 输出是 Vd、Vq控制量必须限幅否则输出超出母线电压后PWM 会进入过调制区域电流波形失真。typedef struct { float kp; float ki; float integral; float out_max; } pi_controller_t; float pi_update(pi_controller_t *pi, float target, float feedback) { float error target - feedback; float out pi-kp * error pi-ki * pi-integral; // 抗积分饱和 if (out pi-out_max) { out pi-out_max; } else if (out -pi-out_max) { out -pi-out_max; } else { pi-integral error; } return out; }这段代码采用“输出饱和时停止积分”的简单策略能避免积分饱和导致的大超调。实际项目中还可以加入积分限幅和变积分系数但初学者先跑通这个版本就够了。电流环调参建议参数调节方式观察现象Kp从小到大逐步增加Kp 过大会导致电流震荡、电机啸叫KiKp 稳定后慢慢增加Ki 用于消除稳态误差过大会导致低频振荡输出限幅设置为主电源电压对应占空比上限限幅过小会导致电流闭环无法到达目标2.4 开环验证到电流闭环的切换顺序FOC 最容易出现的错误是相线接错或编码器角度方向不对。如果不先做开环验证直接切闭环电机会出现剧烈抖动甚至过流。推荐顺序开环给电压手动递增电角度让电机缓慢转动。确认相序如果电机反转交换任意两相线或者改变电角度增量方向。确认电角度用手转动电机观察估算角度是否连续变化。切电流环给定 id 等于 0逐步增加 iq观察电流是否跟随。切速度环电流环稳定后再加速度环。// 开环到闭环的切换逻辑示例 control_mode MODE_OPENLOOP; // 初始为开环 // 主循环或控制中断中 if (control_mode MODE_OPENLOOP) { openloop_angle 0.01f; // 手动递增电角度 v_alpha 1.5f * cosf(openloop_angle); v_beta 1.5f * sinf(openloop_angle); svpwm_set(v_alpha, v_beta); } else if (control_mode MODE_FOC_CLOSEDLOOP) { // 读取三相电流 // Clark、Park 变换 // 电流 PI 运算 // 反 Park、SVPWM }开环阶段不依赖电流反馈只要 PWM 和电机线序正确电机就能转。这一步能快速排除硬件接线问题避免直接调闭环时撞上两个叠加的故障。3. 姿态解算与姿态控制环3.1 传感器数据预处理零偏、量程与低通滤波姿态环的基础是传感器数据。MPU6050 内部集成了三轴陀螺仪和三轴加速度计在初始化时需要配置量程和采样率。陀螺仪量程可以配置为 ±500°/s 或 ±1000°/s加速度计量程通常配置为 ±4g 或 ±8g。读取原始数据后需要转换成物理量// 以 MPU6050 为例配置量程为 ±500°/s // 原始值范围 -32768 到 32767 float gyro_rate (float)raw_gyro / 65.5f; // 单位°/s // 配置量程为 ±4g float accel (float)raw_accel / 8192.0f; // 单位g陀螺仪存在零偏静态时读数可能不是 0。最简单的零偏标定方法是上电后保持静止采样一段时间取平均再在运行时减去这个零偏值。加速度计数据通常叠加了高频振动噪声需要做低通滤波。一阶低通滤波的 C 实现非常轻量float lowpass_filter(float input, float last_output, float alpha) { return alpha * input (1.0f - alpha) * last_output; }alpha 的值越小滤波越平滑但延迟越大。在飞控里低通滤波的延迟会进入控制环因此 alpha 不能太小。3.2 基于四元数的 Mahony 姿态解算姿态解算的方法很多有欧拉角直接积分、方向余弦矩阵、四元数互补滤波、扩展卡尔曼滤波等。在资源有限的 F103 上Mahony 四元数互补滤波是性价比较高的方案。四元数用四个变量表示旋转避免了欧拉角的万向锁问题。Mahony 滤波的核心思想是陀螺仪积分提供短期姿态加速度计和磁力计提供长期参考方向两者通过 PI 补偿融合。typedef struct { float q0, q1, q2, q3; } quaternion_t; quaternion_t q {1.0f, 0.0f, 0.0f, 0.0f}; void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float kp, float ki, float dt) { float norm; float vx, vy, vz; float ex, ey, ez; float ex_int 0, ey_int 0, ez_int 0; // 归一化加速度计 norm sqrtf(ax * ax ay * ay az * az); if (norm 1e-6f) return; ax / norm; ay / norm; az / norm; // 归一化磁力计此处按需求决定是否使用 if (mx ! 0.0f || my ! 0.0f || mz ! 0.0f) { norm sqrtf(mx * mx my * my mz * mz); mx / norm; my / norm; mz / norm; } // 由四元数计算重力方向 vx 2.0f * (q.q1 * q.q3 - q.q0 * q.q2); vy 2.0f * (q.q0 * q.q1 q.q2 * q.q3); vz q.q0 * q.q0 - q.q1 * q.q1 - q.q2 * q.q2 q.q3 * q.q3; // 向量叉积作为误差 ex ay * vz - az * vy; ey az * vx - ax * vz; ez ax * vy - ay * vx; // 积分误差 ex_int ex * ki * dt; ey_int ey * ki * dt; ez_int ez * ki * dt; // 用误差修正陀螺仪 gx kp * ex ex_int; gy kp * ey ey_int; gz kp * ez ez_int; // 四元数微分方程更新 float q0 q.q0, q1 q.q1, q2 q.q2, q3 q.q3; q.q0 q0 0.5f * dt * (-q1 * gx - q2 * gy - q3 * gz); q.q1 q1 0.5f * dt * ( q0 * gx q2 * gz - q3 * gy); q.q2 q2 0.5f * dt * ( q0 * gy - q1 * gz q3 * gx); q.q3 q3 0.5f * dt * ( q0 * gz q1 * gy - q2 * gx); // 归一化四元数 norm sqrtf(q.q0 * q.q0 q.q1 * q.q1 q.q2 * q.q2 q.q3 * q.q3); q.q0 / norm; q.q1 / norm; q.q2 / norm; q.q3 / norm; }这段代码是 Mahony 滤波的最简形式。实际使用时还需要把四元数转换成欧拉角用于调试和 MAVLink 消息发送。void quaternion_to_euler(quaternion_t *q, float *roll, float *pitch, float *yaw) { *roll atan2f(2.0f * (q-q0 * q-q1 q-q2 * q-q3), 1.0f - 2.0f * (q-q1 * q-q1 q-q2 * q-q2)); *pitch asinf(2.0f * (q-q0 * q-q2 - q-q3 * q-q1)); *yaw atan2f(2.0f * (q-q0 * q-q3 q-q1 * q-q2), 1.0f - 2.0f * (q-q2 * q-q2 q-q3 * q-q3)); }3.3 角度环和角速度环的双环 PID 设计姿态控制通常采用内外环结构。内环是角速度环输出期望力矩外环是角度环输出角速度目标值。内外环分离的好处是角速度环能快速抑制扰动角度环负责把角度拉回目标。常见控制频率范围控制环典型频率作用角速度环400 Hz ~ 1 kHz抑制角速度误差角度环100 Hz ~ 200 Hz稳定姿态角位置环10 Hz ~ 50 Hz控制无人机位置角速度环的 PID 可以直接复用前面写的 PI 控制器但通常需要加入微分项用于抑制高频抖动。角度环一般只使用 P 和少量 D积分项在姿态控制中要非常小心以免在悬停时形成周期性的修正过冲。3.4 四旋翼混控分配姿态指令转电机推力如果项目载体是四旋翼混控分配是把姿态环输出的力矩指令变成四个电机期望推力的关键模块。以 X 型四旋翼为例电机编号按逆时针排列前右为 1后右为 2后左为 3前左为 4。常见的混控关系如下m1 throttle roll - pitch yaw m2 throttle - roll - pitch - yaw m3 throttle - roll pitch yaw m4 throttle roll pitch - yaw这里的 roll、pitch、yaw 是姿态环输出的归一化力矩值throttle 是总油门。实际飞控里throttle、roll、pitch、yaw 都有各自的限幅相加后还需要做饱和处理保证输出不会超过 PWM 最大占空比。需要强调的是混控公式的符号取决于电机的旋转方向和机架布局。不同机架的电机方向、编号不同公式符号也可能不同。调机时先确认电机转向再进行混控测试。4. 飞控与 Mission Planner 的 MAVLink 通信链路4.1 MAVLink 协议帧格式与常用消息MAVLink 是飞控领域最常见的通信协议Mission Planner 通过它读取飞控状态并下发控制指令。MAVLink 2 的帧结构以 0xFD 开头MAVLink 1 以 0xFE 开头。帧里包含长度、序列号、系统 ID、组件 ID、消息 ID、载荷和 CRC 校验。// MAVLink 1 帧的基本结构 typedef struct { uint8_t magic; // 0xFE uint8_t len; // 载荷长度 uint8_t seq; // 消息序号 uint8_t sysid; // 系统 ID uint8_t compid; // 组件 ID uint8_t msgid; // 消息 ID uint8_t payload[255]; uint16_t checksum; } mavlink_frame_t;实际开发中建议直接使用官方 mavlink 库生成 C 代码避免手动处理 CRC 和打包逻辑。下面是一个极简的发送示例说明消息发送的关键流程void send_attitude_message(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; mavlink_message_t msg; mavlink_attitude_t attitude; attitude.time_boot_ms millis(); attitude.roll roll; attitude.pitch pitch; attitude.yaw yaw; attitude.rollspeed rollspeed; attitude.pitchspeed pitchspeed; attitude.yawspeed yawspeed; mavlink_msg_attitude_encode(SYSID, COMPID, msg, attitude); uint16_t len mavlink_msg_to_send_buffer(buffer, msg); uart_write_bytes(buffer, len); }常用 MAVLink 消息消息消息 ID方向作用HEARTBEAT0飞控到地面站心跳表示飞控在线SYS_STATUS1飞控到地面站电量、传感器状态ATTITUDE30飞控到地面站姿态角与角速度LOCAL_POSITION_NED32飞控到地面站位置信息SET_POSITION_TARGET_LOCAL_NED84地面站到飞控下发位置目标4.2 Mission Planner 连接自定义飞控的配置步骤Mission Planner 默认支持 PX4、ArduPilot 等飞控但它本质上是通过 MAVLink 通信的。只要自己的飞控能够按 MAVLink 帧格式发送 HEARTBEAT 和 ATTITUDEMission Planner 就能识别并显示姿态。连接步骤准备一个 USB-TTL 串口模块连接飞控的 UART 引脚。查看电脑设备管理器确定串口编号。打开 Mission Planner选择右上方端口和波特率。波特率常用 115200点击 Connect。连接成功后HUD 界面会显示飞控姿态、电量、飞行模式等信息。如果 Mission Planner 弹出版本兼容问题需要确认飞控发送的 MAVLink 版本。大多数情况下发送 MAVLink 1 和 MAVLink 2 都能被兼容但 sysid 和 compid 必须配置正确。4.3 地面站指令如何作用到电机从 SET_POSITION 到 FOCMission Planner 发送 MAVLink 信息给飞控后飞控并不会直接把接收到的数值写到电机。以 SET_POSITION_TARGET_LOCAL_NED 为例消息里包含位置目标、速度目标和加速度目标。飞控收到后先经过位置环计算速度目标再经过速度环计算姿态目标接着进入姿态环和角速度环最后通过混控分配到 FOC 电流环。这个过程可以理解成逐级缩小误差位置环负责把位置误差变成速度误差速度环把速度误差变成姿态角误差姿态环把角度误差变成角速度误差角速度环把角速度误差变成力矩目标FOC 再把力矩目标变成电流目标。在代码上至少需要维护一个外部指令的接收标志typedef struct { float x; float y; float z; float vx; float vy; float vz; uint16_t type_mask; } position_target_t; position_target_t cmd {0