FOC 磁场定向控制算法
FOC 简介
FOC（Field Oriented Control，磁场定向控制）是一种用于直流无刷电机（BLDC）和交流感应电动机控制的高级技术。它通过将电动机的电流分解为产生转矩的分量和产生磁通的分量，实现高效和精确的电机控制。
工作原理
FOC 的核心思想是：
- 检测电机转子位置 - 通过编码器或霍尔传感器
- 坐标变换 - 将三相电流转换到 dq 坐标系
- 独立控制 - 分别控制 d 轴和 q 轴电流
- 逆变换 - 将控制信号转换回三相信号
LeBot 中的应用
LeBot 的腿部和轮子驱动采用直流无刷电机，FOC 用于：
- 精确的转速控制
- 扭矩控制
- 能量效率提升
- 低速平稳运行
- 减小电磁噪声
FOC 基本原理
三相电机模型
交流电动机的三相电流可以表示为：
三个相位相差 120°。
Clark 变换（三相到两相）
将三相电流 (i_a, i_b, i_c) 转换到 (α, β) 静止坐标系：
Park 变换（两相到旋转坐标系）
从静止的 (α, β) 坐标系转换到随转子旋转的 (d, q) 坐标系：
其中 θ 是转子磁通方向角。
逆 Park 和逆 Clark 变换
将控制电压从 dq 坐标系转换回三相：
Python 中的 FOC 实现
基础 FOC 控制器
python
import numpy as np
from typing import Tuple
class FOCController:
"""磁场定向控制（FOC）控制器"""
def __init__(self,
motor_poles: int = 4,
kp_id: float = 1.0,
ki_id: float = 0.1,
kp_iq: float = 1.0,
ki_iq: float = 0.1):
"""
初始化 FOC 控制器
参数：
motor_poles: 电机极对数
kp_id, ki_id: d 轴 PI 控制器参数
kp_iq, ki_iq: q 轴 PI 控制器参数
"""
self.poles = motor_poles
# PI 控制器参数
self.kp_id = kp_id
self.ki_id = ki_id
self.kp_iq = kp_iq
self.ki_iq = ki_iq
# 积分项累加
self.integral_id = 0.0
self.integral_iq = 0.0
# 反正切查找表（用于位置估计）
self.rotor_angle = 0.0
def clark_transform(self,
ia: float, ib: float, ic: float) -> Tuple[float, float]:
"""
Clark 变换：三相到两相
参数：
ia, ib, ic: 三相电流
返回：
(i_alpha, i_beta)
"""
i_alpha = ia
i_beta = (ia + 2*ib) / np.sqrt(3)
return i_alpha, i_beta
def park_transform(self,
i_alpha: float,
i_beta: float,
theta: float) -> Tuple[float, float]:
"""
Park 变换：两相到旋转坐标系
参数：
i_alpha, i_beta: 静止坐标系电流
theta: 转子位置角
返回：
(i_d, i_q)
"""
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
i_d = i_alpha * cos_theta + i_beta * sin_theta
i_q = -i_alpha * sin_theta + i_beta * cos_theta
return i_d, i_q
def inverse_park_transform(self,
v_d: float,
v_q: float,
theta: float) -> Tuple[float, float]:
"""
逆 Park 变换
参数：
v_d, v_q: 旋转坐标系电压
theta: 转子位置角
返回：
(v_alpha, v_beta)
"""
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
v_alpha = v_d * cos_theta - v_q * sin_theta
v_beta = v_d * sin_theta + v_q * cos_theta
return v_alpha, v_beta
def inverse_clark_transform(self,
v_alpha: float,
v_beta: float) -> Tuple[float, float, float]:
"""
逆 Clark 变换：两相到三相
参数：
v_alpha, v_beta: 静止坐标系电压
返回：
(v_a, v_b, v_c)
"""
v_a = v_alpha
v_b = -v_alpha/2 + np.sqrt(3)*v_beta/2
v_c = -v_alpha/2 - np.sqrt(3)*v_beta/2
return v_a, v_b, v_c
def pi_controller(self,
error: float,
kp: float,
ki: float,
integral: float) -> Tuple[float, float]:
"""
PI 控制器
参数：
error: 控制误差
kp: 比例系数
ki: 积分系数
integral: 积分项
返回：
(output, new_integral)
"""
integral += error
integral = np.clip(integral, -1.0, 1.0) # 防止积分饱和
output = kp * error + ki * integral
return output, integral
def control_step(self,
ia: float, ib: float, ic: float,
id_ref: float, iq_ref: float,
rotor_angle: float,
dt: float = 0.001) -> Tuple[float, float, float]:
"""
执行一步 FOC 控制
参数：
ia, ib, ic: 三相电流
id_ref, iq_ref: 参考 d、q 轴电流
rotor_angle: 转子位置角
dt: 时间步长
返回：
(va, vb, vc) - 三相输出电压
"""
# 步骤 1：Clark 变换（三相 -> 两相）
i_alpha, i_beta = self.clark_transform(ia, ib, ic)
# 步骤 2：Park 变换（两相 -> dq）
i_d, i_q = self.park_transform(i_alpha, i_beta, rotor_angle)
# 步骤 3：dq 电流控制（PI 控制器）
error_id = id_ref - i_d
v_d, self.integral_id = self.pi_controller(
error_id, self.kp_id, self.ki_id, self.integral_id
)
error_iq = iq_ref - i_q
v_q, self.integral_iq = self.pi_controller(
error_iq, self.kp_iq, self.ki_iq, self.integral_iq
)
# 步骤 4：逆 Park 变换（dq -> 两相）
v_alpha, v_beta = self.inverse_park_transform(v_d, v_q, rotor_angle)
# 步骤 5：逆 Clark 变换（两相 -> 三相）
va, vb, vc = self.inverse_clark_transform(v_alpha, v_beta)
# 归一化（电压限制）
v_max = 1.0
v_scale = max(abs(va), abs(vb), abs(vc), v_max)
if v_scale > v_max:
va /= v_scale
vb /= v_scale
vc /= v_scale
return va, vb, vc
class BLDCMotorWithFOC:
"""带 FOC 控制的直流无刷电机"""
def __init__(self,
motor_poles: int = 4,
motor_inertia: float = 0.01,
motor_resistance: float = 1.0):
"""
初始化 BLDC 电机
参数：
motor_poles: 极对数
motor_inertia: 转动惯量
motor_resistance: 电阻
"""
self.poles = motor_poles
self.J = motor_inertia
self.R = motor_resistance
self.Ke = 0.1 # 反电动势常数
self.Kt = 0.1 # 转矩常数
# 电机状态
self.omega = 0.0 # 转速
self.theta = 0.0 # 转子位置
# FOC 控制器
self.foc = FOCController(motor_poles)
def update(self,
ia: float, ib: float, ic: float,
id_ref: float, iq_ref: float,
load_torque: float = 0.0,
dt: float = 0.001):
"""
更新电机状态
参数：
ia, ib, ic: 三相电流
id_ref, iq_ref: 参考电流
load_torque: 负载扭矩
dt: 时间步长
"""
# 执行 FOC 控制
va, vb, vc = self.foc.control_step(
ia, ib, ic, id_ref, iq_ref, self.theta, dt
)
# 计算电机转矩
# 对于 BLDC 电机，转矩正比于 q 轴电流
torque = self.Kt * iq_ref
# 计算加速度
angular_accel = (torque - load_torque) / self.J
# 更新转速和位置
self.omega += angular_accel * dt
self.theta += self.omega * dt
# 位置归一化到 [0, 2π)
self.theta = self.theta % (2 * np.pi)
# 使用示例
if __name__ == "__main__":
# 创建带 FOC 的 BLDC 电机
motor = BLDCMotorWithFOC(motor_poles=4)
# 模拟控制
dt = 0.001 # 1 ms
t_total = 5.0 # 5 秒
steps = int(t_total / dt)
id_ref = 0.0 # 不需要磁通
iq_ref = 0.5 # 50% 转矩
time_log = []
omega_log = []
for step in range(steps):
t = step * dt
# 假设测量的三相电流（简化）
ia = 0.1 * np.sin(motor.theta)
ib = 0.1 * np.sin(motor.theta - 2*np.pi/3)
ic = 0.1 * np.sin(motor.theta - 4*np.pi/3)
# 更新电机
motor.update(ia, ib, ic, id_ref, iq_ref, load_torque=0.0, dt=dt)
# 记录
time_log.append(t)
omega_log.append(motor.omega)
if step % 5000 == 0:
print(f"t={t:.2f}s, ω={motor.omega:.3f} rad/s, θ={motor.theta:.3f} rad")
print(f"\n最终转速: {motor.omega:.3f} rad/s")FOC 的优势
- 高效率 - 最大化转矩输出
- 低纹波 - 平稳运行，低噪声
- 精确控制 - 独立控制磁通和转矩
- 宽速度范围 - 从低速到高速都能工作
FOC 与传统控制的对比
| 特性 | 六步换向 | FOC |
|---|---|---|
| 控制复杂度 | 低 | 高 |
| 效率 | 中等 | 高 |
| 转矩纹波 | 大 | 小 |
| 噪声 | 大 | 小 |
| 实时计算量 | 小 | 中等 |
LeBot 电机控制架构
转速参考 ωref
↓
┌──────────────────┐
│ 速度控制器 │ → 电流参考 iq_ref
└──────────────────┘
↓
┌──────────────────┐
│ FOC 控制器 │
│ - Clark 变换 │
│ - Park 变换 │
│ - PI 控制 │
└──────────────────┘
↓
┌──────────────────┐
│ PWM 调制 │ → 三相 PWM 信号
└──────────────────┘
↓
BLDC 电机参数调整
- Kp_iq：控制转矩响应速度，范围 0.5 - 2.0
- Ki_iq：消除稳态误差，范围 0.01 - 0.2
- Kp_id：控制磁通响应，通常设为 0
- Ki_id：通常不使用
总结
FOC 是实现高性能直流无刷电机控制的关键技术：
- 基于坐标变换的解耦控制
- 在 LeBot 中实现精确的运动控制
- 提高能源利用效率
- 降低电磁噪声
在 LeBot 的所有电动机（腿驱动、轮驱动）上都应该应用 FOC 以获得最佳性能。
参考资源
- Texas Instruments FOC 算法库
- ST Microelectronics BLDC/PMSM 控制应用手册
- "Field-Oriented Control of 3-Phase AC Motors"