作者13705694697
智能配送、工业分拣和自动化搬运等场景要求机器人完成零件识别、抓取、搬运、分类和精准码放等任务。机械臂关节作为执行链路的核心单元，其低速稳定性、位置精度、负载感知能力和异常保护能力，直接影响机械臂动作的平稳性和可靠性。
本项目围绕自研机械臂的关节执行需求展开，目标是为机械臂关节提供一套集驱动、感知、控制和通信于一体的执行节点。相比单独的电机驱动板，关节驱动模块不仅需要完成三相电机驱动，还需要结合位置反馈、电流采样和状态监测，为后续速度闭环、位置闭环、负载判断和异常保护提供基础。因此，该类模块对于提升机械臂关节的集成度、可控性和扩展性具有实际工程意义。图1-1为原型机的总览图。图1-2为驱动模块的实物图。
图1-1原型机的总览图
图1-2驱动模块的实物图
1.3系统框图
系统总体架构如图1-3所示由机器人主控、关节智能驱动节点、三相电机及机械臂关节机构组成。机器人主控负责上层任务规划、运动指令下发和状态监测；关节智能驱动节点作为底层执行单元，接收主控指令后完成电机驱动、闭环控制、状态采样和异常保护等功能；三相电机将驱动节点输出的电能转换为关节运动所需的转矩和转速，并通过机械结构带动关节完成抓取、搬运和码放等动作。
图1-3 系统总体架构图
关节智能驱动节点内部主要包括通信接口、闭环控制、功率驱动和感知采样等功能模块。通信接口用于实现与机器人主控之间的指令交互和状态上报；闭环控制模块根据位置反馈、电流反馈和目标指令生成 PWM 控制量；功率驱动模块完成三相电机驱动输出；感知采样模块采集电机位置、电流及后续扩展的母线电压、温度等状态信息，为速度闭环、位置闭环和保护策略提供依据。
2. 系统设计
本项目初始方案为“面向智能配送与分拣机械臂的关节智能驱动与感知一体化模块”。系统规划采用 48 V 母线供电，并通过多路 BUCK 生成 12 V、5 V 和 3.3 V 等电源，为功率级、驱动级、控制级和通信接口提供分级供电。核心链路包括 MP1921 三相栅极驱动、外置 MOSFET 三相桥、MCS1823 电流检测、MA600A 磁编码位置反馈以及 CAN/CAN-FD 通信扩展。
该方案以三相电机驱动为核心，结合位置反馈和电流采样，为电流环、速度环、位置环及 FOC 控制提供硬件基础。系统同时预留母线电压检测、温度检测和通信接口，为后续实现欠压、过温、堵转、过载等异常状态识别与保护。
2.1硬件框架设计
2.2硬件设计
2.2.1电源设计
方案采用分级降压与电源域分离设计。系统主输入首先经 MP4583 降压生成 12 V 电源，用于驱动级及后续电源转换；随后通过 MPM3632 生成 5 V 电源，通过 MPM3804 生成数字 3.3 V 电源，并通过 MP20045 生成模拟 A3V3 电源。同时，系统预留隔离 5 V 电源，用于 CAN 通信隔离侧供电。通过将功率驱动、数字控制、模拟采样和隔离通信划分到不同电源域，可降低开关电源纹波、功率回路干扰对电流采样、位置反馈等传输线的影响。
2.2.2三相功率级与电流采样设计
U、V、W 三个半桥分别由三颗 MP1921 半桥栅极驱动器驱动，共驱动六个 N 沟道 MOSFET 组成三相逆变桥。STM32G474RBT6 通过高级定时器 TIM1 输出三组中心对齐互补 PWM 信号，分别控制 U/V/W 三相上、下桥臂。TIM1 内部配置硬件死区，使同一半桥的上管和下管在换向过程中不会同时导通，从而降低母线直通风险。
MP1921 负责完成 MCU 3.3 V 控制信号到 MOSFET 栅极驱动电平的转换，其中低侧驱动以功率地为参考，高侧驱动以对应相的 SW 节点为参考，并通过自举电容实现高侧 N 沟道 MOSFET 的驱动。三相桥臂的 SW_U、SW_V、SW_W 节点作为电机三相输出端，直接连接到电机绕组。该部分电路是整个驱动板的功率输出核心，其设计重点包括栅极驱动供电、PWM 互补逻辑、死区时间、功率回路面积、母线去耦和 MOSFET 开关瞬态控制。
2.2.3三相功率级与电流采样设计
三相电流传感器分别串接在 U、V、W 三相功率路径中，用于获取各相电流信息。每个电流传感器一方面向 STM32G474RBT6 的 ADC 通道输出模拟电压信号，用于三相电流零点校准、软件过流判断、Clarke/Park 变换以及后续 Id/Iq 电流闭环控制；另一方面提供低有效 OCD 过流信号，用于硬件级异常保护。三个电流传感器的 OCD 信号采用开漏方式汇总后接入 MCU 的 PB12/TIM1_BKIN 引脚。当任意一路电流传感器检测到异常过流时，BKIN 可触发 TIM1 的 Break 保护，使 PWM 输出快速关断。该保护链路独立于主循环软件判断，能够在严重异常时为功率级提供更直接的硬件关断通道。
除电流反馈外，系统还通过 MA600 磁编码器获取转子机械角度。MA600 通过 SPI1 与 STM32G474RBT6 通信，输出 14 bit 角度数据。固件根据编码器原始角度、14 极对数以及标定得到的电角度偏置计算电角度，并用于六步换向、正弦矢量调制、速度估算和输出端位置闭环控制。电流采样、OCD/BKIN 保护和 MA600 角度反馈共同构成后续 FOC 电流环、速度环和位置环的反馈基础。
最小系统电路以 STM32G474RBT6 为主控核心，主要包括 MCU 供电、复位、时钟、调试下载、串口通信以及基础外设引脚连接。MCU 工作电源由板上 3.3 V 电源提供，相关 VDD/VDDA 管脚均布置去耦电容，以保证数字逻辑、ADC 采样和高速定时器运行时的电源稳定性。NRST 复位管脚预留上拉和复位电路，便于在烧录、调试或异常状态下对 MCU 进行可靠复位。
时钟部分采用外部 8 MHz 晶振作为 HSE 时钟源，固件通过 PLL 将系统主频配置到 170 MHz。相比仅使用内部 HSI，外部晶振能够为 PWM 定时、ADC 同步采样、SPI 编码器读取和串口通信提供更稳定的时基。本项目在最小系统测试阶段已单独验证 HSE + PLL 配置，确认 MCU 能够稳定运行在 170 MHz 后，才继续启动后续功率控制外设。调试下载接口采用 SWD 方式，引出 SWDIO、SWCLK、NRST、3.3 V 和 GND，用于通过 ST-LINK 进行程序下载、芯片识别和在线调试。调试阶段首先使用仅包含串口打印和 PC6 心跳的安全固件验证 MCU 最小系统，确认 ST-LINK 可连接、目标电压正常、芯片 DEV_ID/UID 可读、外部时钟工作正常后，再逐步启用 PWM、ADC、SPI 和功率驱动相关功能。
2.4 PCB设计
关节电机驱动模组采用四层 PCB 紧凑布局设计，层叠结构为 TOP-SIGNAL01 / GND02 / VCC03 / BOTTOM-SIGNAL04。在满足功率载流能力、控制信号完整性和板卡尺寸约束的前提下，四层板相比更多层数的方案具有成本更低、加工周期更短和装配复杂度更低的优势，适合本项目当前原型验证和小批量迭代阶段。其中，顶层和底层主要用于功率器件、驱动器、MCU、传感器及外部接口的布置和信号走线；第二层设置为完整 GND 平面，为 MCU、栅极驱动、电流采样和编码器通信提供低阻抗参考地，并减小高速 PWM 开关和 SPI 信号的回流路径面积；第三层作为主要电源层，用于分配 48 V 母线、12 V 驱动电源、5 V 和 3.3 V 等电源网络。
在布局上，BUCK的功率回路面积要尽量小而连续，三相 MOSFET栅极驱动回路在 MOSFET 开通和关断瞬间会产生较大的充放电电流，瞬态电流可达到 A 级别，因此驱动器输出、栅极电阻、MOSFET 栅极与源极回流路径应尽量短而紧凑，减小回路面积，以降低寄生电感引起的栅极振铃、过冲和误导通风险。STM32G474RBT6、MA600 编码器接口、电流采样信号、OCD/BKIN 保护信号以及串口通信等属于控制与反馈链路，应尽量避开母线输入、三相输出、SW 开关节点和 MOSFET 功率器件区域，降低功率回路噪声的耦合。三相输出、母线输入和电流检测路径根据实际电流需求加宽铜箔，并在新板中对关键载流区域进行了增强，以避免旧板中局部铜箔瓶颈造成的压降和发热问题。
3软件设计
驱动板软件以“安全上电、分层放行、闭环控制、异常关断”为核心原则进行设计。由于三相功率级直接连接 48 V 母线和电机绕组，一旦 PWM 极性、死区、电流采样或保护逻辑出现错误，可能导致 MOSFET 直通、母线过流或电机失控。因此，固件并未采用上电后立即输出 PWM 的方式，而是将所有功率动作都纳入统一的软件状态机管理。
软件整体包括系统初始化、串口命令解析、ADC 零点校准、MA600 编码器读取、TIM1 互补 PWM 输出、电流环、速度环、位置环和故障保护等模块。其中，TIM1 负责产生三相中心对齐互补 PWM，ADC 由定时器同步触发采样三相电流，MA600 通过 SPI1 提供转子角度，串口用于调试命令输入和状态日志输出。控制算法按照“电流环—速度环—位置环”的层级逐步建立，外环只生成内环参考值，不直接控制 PWM 输出。
3.1主命令状态机
主命令状态机用于管理驱动板从上电初始化到电机闭环运行的完整软件流程，如图 3-1 所示。系统上电或复位后首先进入 BOOT 状态，随后进入 INIT 状态完成时钟、GPIO、ADC、SPI、TIM 和 UART 等外设初始化，为后续位置读取、电流采样、PWM 输出和串口调试建立基础运行环境。
图 3-1 主命令状态机
初始化完成后，系统进入 WAIT COMMAND 状态，等待串口或上位机下发控制命令。当前命令包括正向启动、反向启动、正向输出转速以及反向输出转速等形式，其中启动命令用于进入驱动准备流程，转速命令用于设置目标速度方向和大小。系统在收到启动命令后不会立即进入闭环运行，而是先进入 INTERLOCK 状态进行互锁检查，判断是否满足启动条件，包括使能状态是否正确、母线或驱动状态是否允许、位置反馈与电流采样是否正常等。若检查不通过，则进入 LOCK / REJECT 状态，关闭 PWM 输出并拒绝本次启动请求，随后回到安全关断或等待命令状态。
当互锁条件满足后，系统进入 ARMED 状态，打开目标方向的导通与限制，并进行短时间等待，使驱动链路进入可控状态。若在规定时间内未满足继续运行条件，状态机会进入 ARM TIMEOUT，关闭全部输出，避免电机在不确定状态下运行。若准备过程正常完成，则进入 CONTROL RUN 状态，执行 FOC 速度闭环控制。该状态下，控制器根据 MA600 位置反馈和三相电流采样结果计算电角度、电流误差和速度误差，并输出 SVPWM 占空比驱动三相桥，使电机按照目标方向和速度运行。
在运行过程中，若检测到停止命令、方向切换请求、异常保护触发或控制退出条件，状态机会进入 RESULT + SAFE OFF 状态，记录本次运行结果和状态标志，并关闭 PWM 与驱动输出，最终回到 SAFE OFF 或等待命令状态。SAFE OFF 是整个状态机的安全落点，所有异常、拒绝、超时和完成退出均会进入该状态，确保 MOS 栅极关断、电机输出释放，并等待下一次有效命令。
3.2位置环设计
位置环控制流程如图3-2所示，主要用于实现电机按给定角度目标进行定点运行。系统通过串口接收位置控制命令，其中 A 命令用于设定正向目标角度，B 命令用于设定反向目标角度，目标范围为 1°～360°。接收到有效命令后，控制器根据目标角度、当前机械角度以及电机极对数，将机械角目标转换为电机控制所需的目标计数值或目标电角度，并确定本次运动方向。
图3-2位置环控制流程
在目标设定完成后，系统首先确定当前位置对应的零点和起始位置，并通过 MA600 磁编码器持续获取实际转子位置。控制器将目标位置与当前位置进行比较，计算位置误差 error = target - motor_position。若目标误差超过允许范围，系统进入位置控制过程；若命令异常、方向不满足要求或误差超出设定安全边界，则进入 POSITION ABORT 状态，记录错误信息并关闭输出，避免电机执行错误动作。
为了提高定位过程的稳定性，位置环没有直接以固定速度运行，而是根据剩余角度进行分段速度规划。当目标距离较远时，系统给定较高速度，使电机快速接近目标位置；随着位置误差逐渐减小，目标速度逐步降低，避免靠近目标点时因惯性产生过冲。图中给出了不同误差区间对应的速度参考值，例如大角度误差时采用较高转速，小角度误差时降低到低速接近，最终在接近目标位置时将速度参考降为零。
3.3速度环设计
速度环控制流程如图3-3所示，主要用于实现电机按照给定方向和目标转速运行。系统通过串口接收速度控制命令，其中 F1～F75 表示正向运行，R1～R75 表示反向运行，数字部分用于表示目标转速等级或目标转速指令。接收到命令后，程序首先进行范围检查，判断目标转速是否处于允许范围内。若命令超出设定范围，则进入 REJECT 状态，拒绝本次运行请求并关闭输出，避免电机在非法目标下启动。
图3-3速度环控制流程
当速度命令有效时，系统根据命令方向和目标值计算目标机械转速，并进一步换算为电机控制所需的目标电角速度。图中目标换算关系为 motor_rpm = output_rpm × 8，同时对目标速度进行限幅，实验阶段转速范围限定在约 100～600 rpm，以保证低功率调试过程中的安全性和稳定性。完成目标换算后，系统进入 ARMED → V 状态，进行短时间启动准备。该阶段持续约 5 s，用于完成方向确认、驱动准备和启动等待，随后进入速度控制运行。速度参考值采用斜坡给定方式逐步上升，图中设置为约 100 motor-rpm/s，可避免目标速度突变导致电流冲击和电机抖动，使电机加速过程更加平滑。
速度闭环采用级联控制结构。外层为 500 Hz 速度 PI，根据目标速度与实际速度之间的误差计算转矩电流参考 Iq_ref；随后对 Iq_ref 进行限幅，图中启动阶段限幅约为 ±0.5 A，正常运行电流参考限制在约 1 A 范围内，以避免低功率验证阶段出现过大电流。内层为 5 kHz 电流 PI，根据 Iq_ref 与实际 q 轴电流之间的误差调节 Vq/Vd 输出，并结合 SVPWM 生成三相 PWM，占空比受限在约 40% 以内。在运行过程中，系统持续执行运行监测，判断方向、速度、电流和编码器反馈是否正常，并监测是否出现堵转、失速或速度误差过大等异常情况。若检测到异常，则进入 FAULT 状态，记录故障原因并关闭全部输出；若电机在设定运行时间内保持正常，则进入 SPEED_DURATION_DONE → ALL OFF 状态，表示本次速度闭环运行完成，并关闭 PWM 输出。
4硬件测试
这里测试篇幅较长，本人选取较为重要的点进行描述，本章节主要测量栅极驱动电路的波形，因为这里的振铃的影响在电路中较为重要,在观察 MOS 栅极或 SW 节点波形时，探头接地方式会显著影响测量结果。若使用普通长地线夹，地线自身会在高速开关瞬态下引入额外寄生电感，使测量回路面积增大，从而放大上升沿或下降沿的振铃与过冲。此时示波器显示的尖峰不一定完全来自真实电路，也可能包含探头接地回路带来的测量误差。
图 4-1 长地线测得的低侧栅极振铃
图 4-2 地弹簧测得的低侧栅极振铃
5软硬件联调与有感 FOC 控制验证
开环六步换向是三相驱动链路调试的起点，主要用于确认 MCU PWM 输出、MP1921 栅极驱动、MOS 半桥以及电机三相连接关系是否正确。在该阶段，控制器不依赖位置反馈，而是按照固定换向顺序依次驱动 U、V、W 三相，从而观察电机是否能够按照预期方向产生连续转矩。
5.1 .1三相零点与电流通道
U/V/W 三相功率桥通过后，拆除初期旁路铜桥并焊接三相电流传感器。输出全关断、无相电流条件下累计 1048 个样本完成零点校准；最终典型零点均回到 12 bit ADC 中点附近。
5.1.2 OCD/BKIN 保护链与 BIF 假触发
三相低有效开漏 OCD 汇总后进入 PB12/TIM1_BKIN，BKF=8 对输入进行数字滤波，AOE=0 禁止 Break 后自动恢复。Break 发生后，即使 PB12 已恢复高电平，也必须由软件重新经过安全状态机。U/V/W OCD（开漏汇总） → PB12 / TIM1_BKIN → BKF=8→ TIM1 Break → MOE清零/六路关断 → 故障锁存与串口快照
早期超慢六步在定位阶段反复出现 BIF，但 PB12 当前为 HIGH、EXTI12 未见下降沿，示波器也未捕获 OCD 下降。寄存器检查发现 BKF 初始化没有形成预期最终状态；修正到 BDTR=0x020810F5、BKF=8 后，BIF 恢复为 0，原有定位即 Break 消失。
5.2. MA600 编码器闭环六步换向
在开环换向验证通过后，系统开始引入 MA600 磁编码器反馈。MA600 安装在电机高速侧，用于提供转子机械角度信息。由于 6010 电机为多极对结构，控制器需要根据机械角度和极对数换算电角度，才能将位置反馈用于换向和 FOC 控制。
5.2.1 MA600编码器测试
MA600 首先以只读 logger 方式接入，避免把 SPI 模式、数据格式、方向和极对数同时引入控制。最终配置为 SPI1 Mode 0、分频 16、PA4 片选；返回值右移 2 位得到 14 bit 机械角，每圈 16384 count。
5.2.3 编码器闭环六步与连续正弦矢量
固定时间换相随后改为由 MA600 电角度选择扇区。这里的“闭环”仅指换相角度闭环，PWM 幅值仍由固定占空比决定。正转两次代表性测试约 132 rpm、739/740 次换相，SPI error 与 overrun 为 0；反转同样通过。
第一版连续矢量使用 offset=5958。通过正反转电流统计和偏置扫描，最终调整为 5731；两者差 −227 count，约为 −4.99° 电角度。theta_e = wrap(-14 × raw14 + 5731)
samples=1000errors=0overruns=0first=4815
last=4814
mean=4815
span=5 counts (109 mdeg)
result=PASS
开环六步期间对 raw14 展开后，当前定义的正转对应 MA600 原始角递减，反转对应原始角递增；两个方向的机械转角和速度量级对称。
5.3.同步采样、Id/Iq 识别与电流 PI
5.3.1 采样频率正确仍不等于采样时刻正确
三相电流传感器位于低侧支路。若 ADC 落在低侧关闭、续流路径改变或开关瞬态附近，即使 ADC 频率与 PWM 相同，也可能得到接近零或符号混乱的数据。必须同时约束触发窗口、边沿距离、三相零点/极性和 PWM 周期对齐。
10 kHz 基础阶段采用 TIM1 中心对齐 PWM、PSC=1、ARR=4489、RCR=1，ADC 注入组由 TIM1 触发。无电机、48 V、0.5 A 限流、三相 50% 互补 PWM 条件下，两次 2000 样本测试均约 10 kHz、errors=0、三相均值位于零点附近。
5.3.2 六矢量极性实验
峰值窗口实验把 CCR4 设为 3500，JEOS 后 CNT 约 3761～3780 且方向为向上计数，说明采样落在目标低侧导通窗口。每个六步矢量施加不超过约 2 ms，源相与汇相电流符号相反，浮空相接近零，六个扇区均 PASS。
5.3.3 Clarke/Park 与电流闭环
三相电流经 Clarke 变换得到 α/β，再使用最终电角度做 Park 变换得到 Id/Iq。当前约定 Id_ref=0，Iq_ref 的正负决定转矩方向；正反转只改变 Iq/Vq 符号，不改变三相电流极性。
Iq_ref=41 counts (500 mA)
Id mean≈0，range约−20..20
Iq mean=48，range=2..44
PI_OUT Vq_mean=400
saturated=14/500
CURRENT PEAK U/V/W≈0.49/0.51/0.50 A
result=PASS
三相峰值接近目标，Id 平均接近零，说明电角度、电流方向和 D/Q 轴控制基本一致。Iq 平均未始终等于参考，受短测试时间、转子加速反电势与安全输出限幅共同影响。
5.4 有感 FOC 速度闭环建立
5.4.1 级联结构与频率安排
在电流环能够稳定工作的基础上，系统进一步建立速度闭环。速度环使用 MA600 编码器角度展开估算电机转速，外层速度 PI 以约 500 Hz 频率运行，根据目标速度与实际速度误差输出 Iq_ref；内层电流 PI 以约 5 kHz 频率运行，根据 Iq_ref 调节三相 PWM 输出。该结构符合典型级联控制方式，即速度环只生成电流参考，不直接修改 PWM 占空比。
5.4.2 从 10 kHz 到 20 kHz
10 kHz 已能闭环转动，但存在明显尖锐可听声。提升到 20 kHz 后，20 rpm 实测约 19.9 rpm，原尖声消失。升频后死区并未同步盲目减小，而是在无电机、三相 50% 互补 PWM、三次自动测试和边沿复核后，将 DTG 设置为 0xDF，约 2.965 µs。
5.4.3 升频后的两个关键问题
5.4.3.1 ADC_STALE 假故障
初版运行时通过主循环读取 TIM1 CNT/DIR 判断 ADC 样本是否过期。20 kHz 周期缩短后，CPU 读取时刻可能已离开触发窗口，于是把“读取时间”误当作“采样时间”。最终改用 DWT 周期计数记录真实 JEOS 时间戳；采样间隔超过 75 µs 且连续两次才触发 ADC_MISSED。H/Z 预检仍保留严格 CNT/DIR 相位检查。
5.4.3.2 100 rpm 真实超速与 FASTRELEASE
修复 ADC 监测后，30/60 rpm 程序一度在减速与转矩卸载阶段触发 100 rpm 硬停止。原因不是简单的比例增益过高，而是 Iq_ref 对称慢斜率使正转矩释放过慢。最终使用非对称斜率：增加驱动转矩时每 2 ms 增加 0.5 current count；减小转矩或建立制动力时每 2 ms 改变 2 counts。
result=PASS
flags=0
PB12=HIGH
BIF=0
SPI errors=0
timing_overruns=0
5.7 输出端相对位置闭环验证
在速度环验证完成后，系统进一步进行输出端相对位置闭环测试。由于 6010 电机内部带有约 8:1 减速结构，且 MA600 安装在电机高速侧，因此编码器测得的是电机侧角度，而机械臂关节实际关注的是输出端角度。为避免命令含义混乱，本项目将位置命令统一定义为输出端相对角度，再由固件换算为电机侧目标计数。
motor_target_counts = command_deg / 360 × 8 × 16384
A30/B30 → motor_target = 10923 counts
A50/B50 → motor_target = 18204 counts
位置环采用级联控制结构：外层位置环根据目标位置与当前位置误差生成速度参考，速度 PI 输出 Iq_ref，电流 PI 再根据 Iq_ref 调节三相 PWM。该结构没有让位置环直接控制 PWM，而是沿用“位置环 → 速度环 → 电流环 → PWM”的分层控制方式，便于逐级调试并限制输出风险。
30° 输出端位置闭环结果如下：
POSITION RESULT: motor_target=10923 counts
motor_final=10686 counts
motor_error=237 counts
output_error=650 mdeg
settled=1
result=PASS
结果表明，系统能够完成输出端 30° 相对位置动作，最终误差约为 0.65°，说明减速比换算、编码器反馈和位置闭环链路基本正确。后续 50° 测试中，目标计数已按 50° 正确换算，但视觉动作差异不明显，主要与动态超时时间偏短、低速末端转矩限制保守以及减速机构摩擦有关。
4源码
源码.zip (1.6 MB)
5展示视频