基于GD32F303的两轮平衡车：FOC三环控制与姿态融合算法的C语言实现

两轮平衡车是一个经典的欠驱动控制系统——它通过两个独立驱动的车轮维持车身直立，在静态不稳定与动态可控之间寻找平衡。当控制核心从传统的51单片机升级为基于Cortex-M4内核的GD32F303时，系统的控制维度也相应提升：直流有刷电机的简单PWM调速不再是唯一选择，磁场定向控制(FOC)配合姿态融合算法，使无刷电机在平衡车上实现了力矩、速度、位置的三环闭环。本文将从程序框架出发，系统阐述基于GD32F303的两轮平衡车FOC三环控制与姿态融合算法的C语言实现。

系统架构与程序框架

平衡车控制程序的分层架构遵循"硬件抽象→算法核心→应用逻辑"的三层结构。底层是GD32F303的外设驱动层，负责PWM输出、ADC电流采样、磁编码器读取和MPU6500姿态数据获取。中间层是算法核心层，包含FOC三环控制(电流环、速度环、位置环)和姿态融合(卡尔曼滤波/互补滤波)。顶层是应用逻辑层，处理遥控指令、状态切换和运行模式管理。

GD32F303的硬件配置充分利用了其120MHz主频和硬件浮点单元的优势。PWM采用高级定时器TIMER0的中心对齐模式，输出三路互补PWM驱动三相半桥逆变电路，死区时间由硬件自动插入。电流采样使用ADC的注入组在PWM周期的特定时刻(如下溢点)触发，采集两相(或三相)低侧电阻的差分电压。位置反馈由AS5600磁编码器通过I2C接口提供12bit分辨率的角度数据，同时增量式编码器用于速度环的转速测量。

姿态融合算法：从原始数据到稳定角度

数据采集与预处理

MPU6500六轴传感器是姿态估计的核心硬件。加速度计测量重力在三个轴上的分量，提供绝对角度参考但易受运动加速度干扰;陀螺仪测量角速度，动态响应快但存在积分漂移。原始数据读取后需进行零点偏移校正：在静止状态下采集100组数据求平均作为零点，后续读数减去零点值后再乘以灵敏度系数转换为物理单位。

卡尔曼滤波的代码实现

卡尔曼滤波的目标是融合加速度计的角度测量值与陀螺仪的角速度积分值，得到最优姿态估计。滤波器的状态向量定义为[angle, q_bias]^T，其中angle是当前倾角，q_bias是陀螺仪的漂移偏差。

预测步骤根据上一时刻状态和陀螺仪数据推算当前角度：

// 状态预测：角度 = 上一时刻角度 + (角速度 - 漂移) * dt

float angle_err = gyro_rate - q_bias;

angle += angle_err * dt;

协方差预测与卡尔曼增益计算是滤波器的核心数学运算：

// P矩阵预测，其中Q_angle和Q_gyro是过程噪声协方差

P[0][0] += (P[1][1] * dt * dt - P[0][1] - P[1][0] + Q_angle);

P[0][1] -= P[1][1] * dt;

P[1][0] -= P[1][1] * dt;

P[1][1] += Q_gyro;

// 计算卡尔曼增益

float K[2] = {0};

K[0] = P[0][0] / (P[0][0] + R_measure);

K[1] = P[1][0] / (P[0][0] + R_measure);

更新步骤利用加速度计测量的角度值修正预测结果：

// 角度修正

float angle_error = accel_angle - angle;

angle += K[0] * angle_error;

q_bias += K[1] * angle_error;

// 更新协方差矩阵

float P00_temp = P[0][0];

float P01_temp = P[0][1];

P[0][0] -= K[0] * P00_temp;

P[0][1] -= K[0] * P01_temp;

P[1][0] -= K[1] * P00_temp;

P[1][1] -= K[1] * P01_temp;

在实际调参中，Q_angle和Q_gyro分别控制加速度计和陀螺仪的信任权重，R_measure代表加速度计的测量噪声方差。对于两轮平衡车，Q_angle通常在0.001-0.005之间，Q_gyro约0.003，R_measure约0.03。

### 互补滤波作为备选方案

互补滤波器以更低的计算开销实现相近的姿态估计效果。其核心公式为：

float angle = 0.98 * (angle + gyro_rate * dt) + 0.02 * accel_angle;

0.98的高通系数信任陀螺仪的短期积分，0.02的低通系数信任加速度计的长期基准。互补滤波在资源受限场景下是卡尔曼滤波的有效替代。

FOC三环控制的软件实现

电流采样与坐标变换

FOC算法需要将三相电流从静止坐标系变换到旋转坐标系。电流采样在PWM下溢点触发，确保采样时刻对应有效矢量状态。采样值经ADC转换后，首先进行Clarke变换将三相电流映射到αβ两相坐标系：

i_alpha = i_a;

i_beta = (i_a + 2 * i_b) / sqrt(3);

随后通过Park变换利用转子角度θ将电流转换至dq旋转坐标系：

i_d = i_alpha * cos(theta) + i_beta * sin(theta);

i_q = -i_alpha * sin(theta) + i_beta * cos(theta);

d轴电流控制磁通，q轴电流控制力矩——两者相互解耦，这是FOC矢量控制的核心优势。

三环嵌套的PID实现

三环控制由内而外依次为电流环、速度环和位置环。电流环响应最快(运行于PWM中断中)，速度环次之，位置环最慢。

增量式PID是三环控制的通用形式。其输出为控制量的变化值而非绝对值，便于实现无扰切换和积分限幅：

float delta_u = Kp * (err - err_prev) + Ki * err + Kd * (err - 2 * err_prev + err_prev2);

u += delta_u;

电流环的输出作为电压参考值，经逆Park变换和SVPWM调制生成三相PWM占空比。速度环的输出作为q轴电流的给定值。位置环的输出作为速度环的给定值，形成完整的串级控制结构。

SVPWM调制

SVPWM(空间矢量脉宽调制)在软件层面完成电压矢量的合成。根据dq轴电压参考值，计算目标电压矢量的扇区和作用时间，生成对应的PWM比较值。GD32F303的高级定时器支持中心对齐PWM模式和死区插入，SVPWM的软件实现主要集中在扇区判断和占空比计算两个环节。

平衡与运动控制的融合

直立控制

平衡车直立控制的本质是一个单轴倒立摆问题。从卡尔曼滤波得到精确的车身倾角后，PD控制器输出PWM控制量驱动电机抵抗重力：

float balance_pwm = Kp_balance * (target_angle - current_angle) + Kd_balance * gyro_rate;

实际调试中通常只使用PD(比例微分)控制，积分项I在直立控制中容易引入振荡。

速度与方向闭环

直立控制保证车身平衡，速度和方向控制则决定小车的运动行为。速度环使用编码器测得轮速作为反馈，位置环使用遥控器输入的期望速度作为给定。速度环的输出叠加到直立控制的PWM指令上，使小车在保持平衡的同时完成前进、后退和转向运动。

基于GD32F303的两轮平衡车程序实现，展示了Cortex-M4内核在实时控制场景下的完整能力：硬件浮点单元加速FOC坐标变换中的三角函数运算，高级定时器简化SVPWM和电流采样时序，DMA实现传感器数据的零CPU开销搬运。从MPU6500的I2C读取，到卡尔曼滤波的矩阵运算，再到三环嵌套PID与SVPWM调制，整套程序框架体现了嵌入式控制系统的分层设计思想。对于有志于深入电机控制和机器人学的开发者而言，这是一个从理论到实践、从算法到代码的完整学习平台。