双电机有感（带QEP位置传感器）FOC 控制DEMO软件

一、项目概述

本项目基于成都翌创微电子 ET6000 系列 MCU，采用 FOC（磁场定向控制） 算法， 实现了 双路永磁同步电机 (PMSM) 的闭环驱动控制。系统支持两路电机独立启停、 速度/转矩控制、编码器位置反馈及完整的故障保护功能。

  • MCU 型号: ET6039 (Cortex-M7 内核)

  • 电机型号: 42JSF6310AS-1000 (PMSM)

  • 驱动板型号: ATK-PD6010B

  • 开发板型号: ETTB-6039PV-V1

  • 控制方式: 速度闭环 FOC (磁场定向控制)

  • 位置传感器: 正交编码器 (Quadrature Encoder)

  • 电流采样: 三电阻独立采样 (Three-Shunt Independent)

  • 开发环境: Keil MDK / IAR EWARM

应用层代码目录:

Application/
├── Inc/
│   ├── drive_parameters.h            # 双电机驱动参数定义
│   ├── pmsm_motor_parameters.h       # 电机电气与传感器参数
│   ├── power_stage_parameters.h      # 功率级参数 (母线/电流/温度)
│   ├── mc_parameters.h/.c            # 电流采样与 PWM 配置
│   ├── mc_config.h                   # 控制组件句柄声明
│   ├── init_config.h                 # ADC/SRPWM/EQEP 端口配置宏
│   ├── mc_api.h/.c                   # 上层 API 接口
│   ├── mc_tasks.h                    # 任务调度声明
│   ├── motorcontrol.h/.c             # MC 子系统初始化入口
│   └── main.h/.c                     # 主程序
└── Src/
    ├── main.c                        # main() 入口, 按键调速逻辑
    ├── mc_tasks.c                    # 中频/高频任务, FOC 核心算法
    ├── mc_api.c                      # 双电机 API 实现
    ├── init_config.c                 # ADC/SRPWM/EQEP 初始化
    └── ...

二、电机参数

两台电机均为 4 对极永磁同步电机 (PMSM)，参数基本一致。

电机 1 (M1) 参数

参数名称

参数值

说明

POLE_PAIR_NUM

4

极对数

RS

1.02 Ω

定子相电阻

LS

0.000590 H

定子相电感 (Ld = Lq)

MOTOR_VOLTAGE_CONSTANT

4.3 Vrms/kRPM

相反电动势常数 (线电压 RMS)

MOTOR_MAX_SPEED_RPM

3000 RPM

最大额定转速

MAX_APPLICATION_SPEED_RPM

4000 RPM

应用最大转速

NOMINAL_CURRENT

9532 (s16A)

额定电流 (16位定点格式)

ID_DEMAG

-9532 (s16A)

退磁电流

编码器 PPR

1000

正交编码器每转脉冲数

电机 2 (M2) 参数

参数名称

参数值

说明

POLE_PAIR_NUM2

4

极对数

RS2

1.02 Ω

定子相电阻

LS2

0.000590 H

定子相电感 (Ld = Lq)

MOTOR_VOLTAGE_CONSTANT2

4.3 Vrms/kRPM

相反电动势常数 (线电压 RMS)

MOTOR_MAX_SPEED_RPM2

4000 RPM

最大额定转速

MAX_APPLICATION_SPEED_RPM2

4000 RPM

应用最大转速

NOMINAL_CURRENT2

9532 (s16A)

额定电流 (16位定点格式)

ID_DEMAG2

-9532 (s16A)

退磁电流

编码器 PPR

1000

正交编码器每转脉冲数

功率级参数 (两电机共用)

参数名称

参数值

说明

NOMINAL_BUS_VOLTAGE

24 V

额定母线电压

VBUS_PARTITIONING_FACTOR

0.0400

母线电压分压比

RSHUNT

0.02 Ω

采样电阻值

AMPLIFICATION_GAIN

6.00

电流采样放大增益

TNOISE_NS

2550 ns

噪声时间

TRISE_NS

2550 ns

上升时间

HW_DEAD_TIME_NS

100 ns

硬件死区时间

SW_DEADTIME_NS

500 ns

软件死区时间

温度传感器参数

参数

M1 值

M2 值

V0 (T0 时电压)

1.055 V

1.055 V

T0 (参考温度)

25 ℃

25 ℃

dV/dT (温漂系数)

0.023 V/℃

0.025 V/℃

过温保护阈值

110 ℃

110 ℃

过温迟滞

10 ℃

10 ℃

三、FOC 基本控制原理

FOC (Field Oriented Control，磁场定向控制) 通过将三相交流电机的定子电流分解为 励磁分量 Id 和 转矩分量 Iq，实现类似于直流电机的解耦控制。FOC 核心算法流程如下：

  1. 电流采样与 Clarke 变换 通过三电阻采样获得 U/V/W 三相电流 Ia、Ib、Ic，经 Clarke 变换将三相静止 坐标系电流变换为两相静止坐标系下的 Iα、Iβ:

    • Iα = Ia

    • Iβ = -(Ia + 2×Ib) / √3

    （Clarke 变换是电机控制领域的标准数学变换）

  2. Park 变换 利用编码器提供的转子电角度 θ，经 Park 变换将静止坐标系 Iα/Iβ 变换到同步 旋转坐标系下的 Id/Iq:

    • Id = Iα×cos(θ) + Iβ×sin(θ)

    • Iq = -Iα×sin(θ) + Iβ×cos(θ)

    （Park 变换是电机控制领域的标准数学变换）

  3. 电流 PI 调节 在旋转坐标系下，分别对 Id 和 Iq 进行 PI 闭环控制:

    • Id 环: 目标 Id_ref = 0 (默认 Id=0 控制策略)，PI 输出 Vd

    • Iq 环: 目标 Iq_ref 由速度环输出 (转矩参考)，PI 输出 Vq

  4. 前馈解耦补偿 对 Vd/Vq 进行前馈解耦补偿，抵消 dq 轴之间的交叉耦合项。 补偿量根据电机电气参数（电阻、电感）和当前 Id/Iq 实时计算。

  5. 圆形电压限制 对 Vd/Vq 合成的电压矢量幅值进行限制，确保输出电压不超出直流母线电压所能 提供的最大不失真正弦电压范围:

    • 若 Vd² + Vq² > Vmax²，则等比例缩小 Vd/Vq

    （圆形限制是 SVPWM 过调制处理的标准方法）

  6. 逆 Park 变换 将旋转坐标系下的 Vd/Vq 变换回静止坐标系 Vα/Vβ:

    • Vα = Vd×cos(θ) - Vq×sin(θ)

    • Vβ = Vd×sin(θ) + Vq×cos(θ)

    （逆 Park 变换是电机控制领域的标准数学变换）

  7. SVPWM 输出 将 Vα/Vβ 经空间矢量脉宽调制 (SVPWM) 转换为三相 PWM 占空比，驱动逆变器 功率管，实现对电机的正弦波电流驱动。

转速速度单位

系统使用 0.1 Hz 作为速度单位 (SPEED_UNIT = _01HZ = 10):

  • 1 Hz = 10 × 0.1Hz

  • 1 Hz = 60 RPM

换算关系:

速度值(0.1Hz) = 目标转速(RPM) × SPEED_UNIT(10) / _RPM(60)

e.g. 3000 RPM → 3000 × 10 / 60 = 500 (0.1Hz 单位)

控制时序

  • 电流环 (FOC 核心): 每个 PWM 周期执行一次 (16kHz = 62.5μs 周期)

  • 速度环 (PID): 1000 Hz (每 1ms 执行一次)

  • 安全检测任务: 每 1ms 执行一次

四、核心控制功能

4.1 状态机

每个电机有独立的状态机，按顺序管理电机的启动、运行、关断及故障处理全过程。

主要状态节点:

  • IDLE: 空闲，等待启动命令

  • 预充电 & 校准: 自举电容预充电、ADC 电流采样偏移校准

  • 启动: 清零编码器计数和 FOC 变量，开启 PWM 输出

  • 编码器对齐: 若编码器未对齐，先执行对齐程序（注入 Id 电流锁定转子位置），完成后再自动重新走启动流程

  • RUN: 正常运行状态，执行缓冲命令（速度/转矩斜坡）和 FOC 电流参考计算

  • 停机: 关闭 PWM，清除 FOC 变量，经停机保持时间后返回 IDLE

  • 故障处理: 任何状态均可跳转至故障状态，立即关断 PWM

4.2 电流环 (PI 控制)

  • Id 电流环: PI 调节器，Kp=2769, Ki=1915 (M1) / Ki=2394 (M2), Kd=100

  • Iq 电流环: PI 调节器，Kp=2769, Ki=1915 (M1) / Ki=2394 (M2), Kd=100

  • PI 参数分频值: KPDIV=2048, KIDIV=16384, KDDIV=8192

  • 微分项: 未启用

  • Iq 最大限幅: 9532 (s16A)

4.3 速度环 (PI 控制)

  • 速度 PI 控制器，执行频率 1000Hz

  • M1: Kp=230, Ki=90, Kd=0

  • M2: Kp=230, Ki=90, Kd=0

  • PI 参数分频值: KPDIV=16, KIDIV=256, KDDIV=16

  • 速度环输出为 Iq 参考 (转矩电流参考)

4.4 编码器对齐

启动前自动检测编码器是否已对齐。若未对齐，先执行对齐程序: 向电机注入固定电角度的直流电流，将转子锁定到已知位置，完成编码器零位校准。 对齐完成后再自动进入正常启动流程。

  • 对齐时间: 700 ms

  • 对齐电角度: 90°

  • 对齐电流: 4646 (s16A)

4.5 故障保护

支持以下故障检测和保护:

故障类型

说明

过压保护 (OV)

M1/M2 阈值 60V，关断 PWM

欠压保护 (UV)

M1/M2 阈值 12V

过温保护 (OT)

M1: 110℃, M2: 110℃ (10℃ 迟滞)

过流保护 (OC)

硬件过流检测，返回 MC_BREAK_IN

速度反馈故障

连续 3 次测量错误后触发

FOC 执行超时

若 FOC 计算超过 PWM 周期，触发 MC_FOC_DURATION

紧急制动

支持通过 GPIO (SHUTDOWN) 紧急关断

故障检测内容包括: - NTC 温度检测 - 硬件过流检测 - 母线电压检测

4.6 前馈解耦控制

前馈控制用于解耦 dq 轴之间的交叉耦合，提高电流环动态响应。两电机均启用了 此前馈功能。前馈补偿量 = 电机电感 × 电流 / (母线电压 × 采样周期)。

4.7 编码器速度/位置反馈

  • 传感器类型: 正交编码器 (Quadrature Encoder)

  • 分辨率: 1000 PPR × 4 = 4000 CPR (4倍频)

  • 速度计算: 使用 16 深度 FIFO 滑动平均滤波

  • 速度分辨率: 0.1Hz

五、EVB 板端口连接

5.1 电机 1 (M1) 端口连接

PWM 输出 (三路互补输出)

功能

上桥 GPIO

下桥 GPIO

SRPWM

通道

U 相 (PWM_U)

GPIO28

GPIO29

SRPWM6

CH6

V 相 (PWM_V)

GPIO14

GPIO15

SRPWM7

CH7

W 相 (PWM_W)

GPIO8

GPIO9

SRPWM4

CH4

PWM 参数: - 时钟基准: SRPWM0 (PWM_CLK_TRIGGER) - ADC 触发: SRPWM1, CH1 - PWM 频率: 16kHz - 死区时间: 500ns (软件 DB), 100ns (硬件 DB) - 计数模式: 增减计数 (Up-Down) - PWM 同步: PWM_CLK_TRIGGER 的 ZERO 事件同步所有 PWM 模块

编码器接口 (EQEP1)

信号

GPIO

引脚功能

EQEP A

GPIO11

GPIO11_MODE_EQEP1_A

EQEP B

GPIO33

GPIO33_MODE_EQEP1_B

EQEP Z (Index)

GPIO26

GPIO26_MODE_EQEP1_INDEX

编码器配置: - 译码模式: 2倍分辨率 + 正交计数 (EQEP_CONFIG_2X_RESOLUTION | EQEP_CONFIG_QUADRATURE) - 位置计数器复位模式: 达到最大位置后归零 - 捕获分频: CLK/1, 位置事件/1

电流采样 ADC

ADC0 (主要三相电流):

采样项

模拟通道

虚拟通道

U 相电流

CH14

ADC_VIRTUAL_CHANNEL0

V 相电流

CH7

ADC_VIRTUAL_CHANNEL1

W 相电流

CH12

ADC_VIRTUAL_CHANNEL2

ADC2 (辅助三相电流):

采样项

模拟通道

虚拟通道

U 相电流

CH4

ADC_VIRTUAL_CHANNEL0

V 相电流

CH3

ADC_VIRTUAL_CHANNEL1

W 相电流

CH5

ADC_VIRTUAL_CHANNEL2

ADC1 (母线电压 + ADC0 温度):

采样项

模拟通道

虚拟通道

VBus 母线电压

CH2

ADC_VIRTUAL_CHANNEL3

ADC0 温度采样:

采样项

模拟通道

虚拟通道

NTC 温度

CH9

ADC_VIRTUAL_CHANNEL3

ADC 配置: - ADC 采样时间: 4 个时钟周期 - 触发源: SRPWM ADC_TRIGGER_NUM1 (ADC0/ADC2), SRPWM ADC_TRIGGER_NUM0 (ADC1) - ADC0/ADC2 触发模式: One-Shot - ADC1 触发模式: Continuous - 优先级模式: Low Queue

关断控制 (Shutdown)

信号

GPIO

说明

SHUTDOWN1

GPIO27

高电平有效，紧急关断 M1

5.2 电机 2 (M2) 端口连接

PWM 输出 (三路互补输出)

功能

上桥 GPIO

下桥 GPIO

SRPWM

通道

A 相 (PWM_A)

GPIO4

GPIO5

SRPWM2

CH2

B 相 (PWM_B)

GPIO22

GPIO23

SRPWM3

CH3

C 相 (PWM_C)

GPIO55

GPIO60

SRPWM9

CH9

PWM 参数: - 时钟基准: SRPWM0 (共用) - ADC 触发: SRPWM1, CH1 - PWM 频率: 16kHz - 死区时间: 500ns (软件 DB), 100ns (硬件 DB) - 计数模式: 增减计数 (Up-Down)

编码器接口 (EQEP0)

信号

GPIO

引脚功能

EQEP A

GPIO50

GPIO50_MODE_EQEP0_A

EQEP B

GPIO51

GPIO51_MODE_EQEP0_B

EQEP Z (Index)

GPIO53

GPIO53_MODE_EQEP0_INDEX

电流采样 ADC

ADC0 (主要三相电流):

采样项

模拟通道

虚拟通道

A 相电流

CH2

ADC_VIRTUAL_CHANNEL4

B 相电流

CH3

ADC_VIRTUAL_CHANNEL5

C 相电流

CH11

ADC_VIRTUAL_CHANNEL6

ADC2 (辅助三相电流):

采样项

模拟通道

虚拟通道

A 相电流

CH9

ADC_VIRTUAL_CHANNEL4

B 相电流

CH7

ADC_VIRTUAL_CHANNEL5

C 相电流

CH0

ADC_VIRTUAL_CHANNEL6

ADC1 (母线电压):

采样项

模拟通道

虚拟通道

VBus 母线电压

CH0

ADC_VIRTUAL_CHANNEL7

ADC0 温度采样:

采样项

模拟通道

虚拟通道

NTC 温度

CH6

ADC_VIRTUAL_CHANNEL7

关断控制 (Shutdown)

信号

GPIO

说明

SHUTDOWN2

GPIO56

高电平有效，紧急关断 M2

5.3 UART 通讯接口

信号

GPIO

引脚功能

UART TX

GPIO2

GPIO2_MODE_UART0_TX

UART RX

GPIO25

GPIO25_MODE_UART0_RX

UART 配置: 115200 bps, 8N1, 使能 FIFO

5.4 中断优先级配置

中断源

优先级

说明

EPWM_EPWMCOM_INTR6 (M1_PWM_U)

0 (最高)

M1 PWM 过零事件触发 SVPWM

EPWM_EPWMCOM_INTR3 (M2_PWM_A)

0 (最高)

M2 PWM 过零事件触发 SVPWM

SARC_INTR0 (ADC)

0 (最高)

ADC 采样完成触发 FOC 电流环

EQEP_INTR1 (M1 Encoder)

3

M1 编码器溢出中断

EQEP_INTR0 (M2 Encoder)

3

M2 编码器溢出中断

UART0

4 (最低)

通讯中断

六、速度调节方法

6.1 按键调速 (当前默认方式)

速度调节通过板载按键实现，逻辑见 [main.c:109-156](project/6002_PMSM_SPE_DM_M12/Application/Src/main.c#L109-L156):

  • KEY1: 控制电机 1 (M1)

  • KEY2: 控制电机 2 (M2)

  • 调速步长: ±300 RPM (SpdStep)

  • 调速范围: -3000 RPM 到 +3000 RPM

  • 调速方式: 每次按键在当前位置上累加/递减 300 RPM

  • 最高速时自动反转方向 (3000→-3000，反之亦然)

  • 经过 0 RPM 时自动停止电机

调速逻辑流程:

按键按下 → 启动电机 → 目标速度 += 步长
├─ 目标速度 == 0    → 停止电机
├─ 目标速度 >= 3000  → 目标速度 = 3000, 步长反转 (下一个步长 -300)
├─ 目标速度 <= -3000 → 目标速度 = -3000, 步长反转 (下一个步长 +300)
└─ 调用 MC_ProgramSpeedRampMotor1/2(hSetSpeed, 0)   (0ms 立即生效)

6.2 API 函数调速

系统提供了完整的速度控制 API，上层应用可通过以下函数进行编程调速:

速度斜坡控制 (Speed Ramp)

void MC_ProgramSpeedRampMotor1(int16_t hFinalSpeed, uint16_t hDurationms);
void MC_ProgramSpeedRampMotor2(int16_t hFinalSpeed, uint16_t hDurationms);

  • hFinalSpeed: 目标机械转速，单位为 0.1Hz (SPEED_UNIT = _01HZ)

  • hDurationms: 斜坡持续时间 (ms)，设为 0 表示立即跳变

  • 斜坡期间速度从当前值线性变化到目标值

换算公式:

目标速度值(0.1Hz) = 目标转速(RPM) × SPEED_UNIT(10) / _RPM(60)

例如: 设置 1500 RPM → hFinalSpeed = 1500 × 10 / 60 = 250

转矩斜坡控制 (Torque Ramp)

void MC_ProgramTorqueRampMotor1(int16_t hFinalTorque, uint16_t hDurationms);
void MC_ProgramTorqueRampMotor2(int16_t hFinalTorque, uint16_t hDurationms);

  • hFinalTorque: 目标 Iq 电流值 (s16A 格式)，正值正转/负值反转

直接电流参考

void MC_SetCurrentReferenceMotor1(qd_t Iqdref);
void MC_SetCurrentReferenceMotor2(qd_t Iqdref);

  • 直接设置 dq 轴电流参考值 (qd_t 结构体包含 .q 和 .d 分量)

速度查询

int16_t MC_GetMecSpeedReferenceMotor1(void);  // 当前速度参考值
int16_t MC_GetMecSpeedAverageMotor1(void);    // 当前平均实际速度

停止和暂停

bool MC_StopMotor1(void);          // 停止电机
bool MC_StopSpeedRampMotor1(void); // 暂停当前斜坡, 保持当前速度
void MC_StopRampMotor1(void);      // 停止斜坡

6.3 控制模式设定

系统支持两种控制模式，通过 drive_parameters.h 中的宏定义:

  • 速度模式 (STC_SPEED_MODE, 默认): 速度 PI 环输出 Iq 参考

  • 转矩模式 (STC_TORQUE_MODE): 直接控制 Iq 电流

默认控制模式: 速度模式

默认目标速度: 300 RPM (DEFAULT_TARGET_SPEED_RPM)

6.4 速度控制框图

七、任务调度架构

系统采用三级任务调度:

  1. 高频任务 — 电流环 FOC - 触发: ADC 转换完成中断 - 频率: 16kHz (每个 PWM 周期) - 内容: Clarke/Park 变换 → PI 电流调节 → 前馈补偿 → 圆形电压限制 → 逆 Park → SVPWM 输出 - 双电机通过 FIFO 队列交替执行电流环

  2. 中频任务 — 速度环与状态机 - 触发: SysTick 定时器 (1000Hz) - 内容: 状态机运行、编码器速度计算、功率测量、速度环 PID

  3. 安全任务 — 故障检测 - 触发: 每 1ms (在中频任务之后) - 内容: 温度检测、过流检测、母线电压检测、故障状态更新

八、关键数据流

FOC 控制中主要涉及以下数据:

  • 三相电流 Ia/Ib/Ic → Clarke 变换 → 静止坐标系电流 Iα/Iβ

  • Iα/Iβ + 电角度 θ → Park 变换 → 旋转坐标系电流 Id/Iq

  • 电流参考 Id_ref / Iq_ref → PI 调节器 → 旋转坐标系电压 Vd/Vq

  • Vd/Vq + 电角度 θ → 逆 Park 变换 → 静止坐标系电压 Vα/Vβ

  • Vα/Vβ → SVPWM → 三相 PWM 占空比

  • 编码器 → 电角度 θ、机械转速 (0.1Hz 单位)

九、应用层源文件列表

文件

内容

[drive_parameters.h](project/6002_PMSM_SPE_DM_M12/

双电机驱动参数 (PID/保护/编码器)

[pmsm_motor_parameters.h](project/6002_PMSM_SPE_DM

电机电气参数 (R/L/极对数/编码器PPR)

[power_stage_parameters.h](project/6002_PMSM_SPE_D

功率级参数 (母线电压/采样电阻/放大增益)

[mc_config.h](project/6002_PMSM_SPE_DM_M12/Applica

控制组件句柄声明 (2路电机)

[init_config.h](project/6002_PMSM_SPE_DM_M12/Appli

ADC/PWM/EQEP 通道宏定义

[init_config.c](project/6002_PMSM_SPE_DM_M12/Appli

外设初始化 (ADC/SRPWM/EQEP/UART/GPIO)

[mc_parameters.c](project/6002_PMSM_SPE_DM_M12/App

电流采样参数结构体 (M1/M2)

[mc_api.c](project/6002_PMSM_SPE_DM_M12/Applicatio

双电机上层 API 实现

[mc_tasks.c](project/6002_PMSM_SPE_DM_M12/Applicat

FOC 核心算法 + 任务调度 + 状态机

[main.c](project/6002_PMSM_SPE_DM_M12/Application/

主程序入口 + 按键调速

[motorcontrol.c](project/6002_PMSM_SPE_DM_M12/Appl

MC 子系统初始化

工程固件