SimpleFOC之ESP32（六）—— 双电机协同控制实战

1. 为什么你需要双电机协同控制如果你玩过机器人、做过云台或者捣鼓过任何需要两个轮子或两个关节同步动作的小玩意儿那你肯定遇到过这样的烦恼一个电机转得快一个电机转得慢走起路来歪歪扭扭云台转动起来一卡一卡的。这背后的核心问题就是两个电机没有“协同”好。所谓协同控制远不止是让两个电机同时转起来那么简单。它要求两个电机在速度、位置或者出力上能够像一对训练有素的舞伴严格保持你设定的关系。比如在差速转向的机器人底盘上两个驱动轮需要精确地以不同速度旋转来实现平滑转弯在一个两轴云台上俯仰电机和横滚电机需要默契配合才能让摄像头稳定对准目标。这些场景单靠分别控制两个电机是远远不够的你必须让它们“对话”让它们“同步”。ESP32这颗芯片凭借其双核处理能力和丰富的外设接口天生就是干这活的能手。而SimpleFOC库则把复杂的电机磁场定向控制FOC算法封装成了简单易用的函数让我们这些开发者可以更专注于应用逻辑而不是深陷数学公式。把这两者结合起来实现双电机的协同工作就成了一个既强大又相对容易上手的方案。今天我就以最常用的速度同步和位置同步为例带你一步步实现这个目标并分享几个我调试时踩过的坑和填坑方法。2. 硬件搭建两种主流方案详解动手写代码之前硬件连接是基础这一步错了后面全是白费劲。针对ESP32和SimpleFOC主要有两种硬件方案它们各有特点适合不同的场景。2.1 方案一ESP32drive-D 一体式驱动板这是我个人非常喜欢的一种方案因为它足够“省心”。ESP32drive-D把ESP32主控、两个电机的驱动电路、电源管理都集成在了一块板子上相当于一个“双电机控制一体机”。它的核心优势在于连接极其简单你只需要接上电机、编码器和电源几乎不用跳线。对于快速原型验证或者不想在硬件连接上花费太多时间的朋友来说这简直是福音。布局工整干扰小板载设计使得电机驱动的大电流路径和ESP32的弱电信号路径经过了优化相比自己用杜邦线连接两个独立驱动板电磁干扰的问题会少很多。引脚定义清晰固定两个电机对应的使能、PWM引脚都是固定的在代码中配置时不容易出错。比如电机1的PWM引脚可能固定是GPIO32、33电机2是GPIO25、26记下来就行。你需要准备的物料清单很简单ESP32drive-D 开发板 x1带编码器的直流无刷电机BLDC with encoderx212V直流电源电流根据电机功率定建议5A以上x1USB数据线用于给ESP32烧录程序和供电x1硬件连接步骤将两个电机的三相线U, V, W分别接入驱动板上对应的M1和M2接口。将两个电机的编码器接口通常包含VCC, GND, SDA, SCL对于I2C编码器连接到驱动板上对应的编码器端口。将12V电源接入驱动板的电源输入端子。最后用USB线连接电脑和ESP32drive-D的Type-C口。上电后驱动板上的指示灯应该正常亮起。这里有个关键细节ESP32drive-D上两个电机的使能ENABLE引脚逻辑可能不同。有些设计是M1的三个半桥驱动器有独立的使能脚而M2是共用一个使能脚。这需要在初始化代码中注意给M1的每个相位单独使能而M2只需要一个使能信号。好在SimpleFOC库的BLDCDriver3PWM和BLDCDriver6PWM类已经考虑到了这种差异你只需要在定义驱动对象时传入正确的引脚编号即可。2.2 方案二ESP32 双SimpleFOC Shield 堆叠方案这是更灵活、也更通用的方案。你使用一块标准的ESP32开发板如ESP32-DevKitC然后通过排针堆叠两块SimpleFOC Shield扩展板。这种方案适合你已经拥有SimpleFOC Shield或者需要更灵活地选择主控板的情况。这种方案的特点灵活性高你可以更换不同的ESP32开发板甚至使用其他兼容SimpleFOC库的板卡。模块化每个Shield驱动一个电机结构清晰。坏了一块也方便更换。需要仔细配置引脚这是最大的挑战。两块Shield堆叠在同一个ESP32上意味着你需要手动分配和避免引脚冲突绝对不能把两个电机需要的PWM引脚或SPI引脚配置到同一个GPIO上。硬件连接详解首先你需要对照ESP32开发板的引脚图和你使用的SimpleFOC Shield版本原理图。假设我们使用最常见的ESP32-DevKitC V4和SimpleFOC Shield V2.0.4。对于电机1底层ShieldPWM引脚通常使用GPIO32、33、25。这三个引脚需要连接到Shield上对应驱动芯片的输入。编码器如果使用I2C编码器如AS5600则需要指定一组I2C引脚例如SDA21,SCL22。使能引脚通常用一个GPIO比如EN14。对于电机2上层ShieldPWM引脚必须选择与电机1不同的GPIO。例如使用GPIO26、27、12。编码器需要第二组I2C接口。ESP32的硬件I2C只有两组Wire(默认GPIO21, 22) 和Wire1(默认GPIO32, 33)。但GPIO32/33可能已经被电机1的PWM占用了。所以我们通常使用Wire2122给电机1然后为电机2的I2C配置一个“软件I2C”或者重新映射Wire1到其他空闲引脚如GPIO1819。这是双电机配置的第一个核心难点我们后面代码部分会重点攻克。使能引脚例如EN13。电源连接两块Shield的电源是并联的共同接入同一个12V电源。同时确保ESP32开发板通过USB或Vin引脚获得了5V供电。3. 攻克核心难点配置双I2C接口正如上面提到的当两个电机都使用I2C接口的编码器时如何让ESP32同时与两个编码器通信是第一个拦路虎。很多朋友在这里编译就报错或者运行时只有一个编码器能读到数据。3.1 理解ESP32的I2C资源ESP32通常支持两个硬件I2C总线在Arduino环境中对应Wire和Wire1对象。但问题是Wire1的默认引脚GPIO32 GPIO33经常被我们用作PWM输出。所以我们不能简单地用默认配置。解决方案是自定义引脚映射。ESP32的I2C控制器非常灵活几乎可以将I2C功能映射到任何空闲的GPIO引脚上。3.2 代码实战配置两个I2C实例我们假设电机1使用默认的Wire引脚2122电机2我们需要自定义一个I2C比如使用引脚18和19。#include Wire.h #include SimpleFOC.h // 电机1的编码器 (使用默认I2C: Wire, 引脚21, 22) MagneticSensorI2C sensor1 MagneticSensorI2C(AS5600_I2C); // 首先为电机2创建一个新的 TwoWire 实例 TwoWire I2Ctwo TwoWire(1); // 使用I2C端口1 // 电机2的编码器传入我们自定义的 I2Ctwo 对象 MagneticSensorI2C sensor2 MagneticSensorI2C(AS5600_I2C); void setup() { Serial.begin(115200); // 初始化电机1的I2C使用默认Wire引脚2122 Wire.begin(); // 默认SDA21, SCL22 sensor1.init(); // 初始化电机2的I2C // 关键步骤在begin之前使用setPins指定SDA和SCL引脚 I2Ctwo.begin(19, 18); // SDA19, SCL18 (注意顺序SDA, SCL) // 然后将sensor2与这个自定义的I2C总线关联起来 // 通常传感器库的init()函数内部会调用Wire.begin所以我们需要确保先配置好I2Ctwo // 对于SimpleFOC库我们需要在init()前告诉传感器使用哪个Wire对象。 // 有些库构造函数允许传入Wire指针具体要看库的实现。 // 一种通用的方法是如果库不支持可能需要修改库文件或者寻找支持多I2C的传感器库版本。 // 幸运的是SimpleFOC的MagneticSensorI2C类通常会在init()时使用预定义的‘Wire’ // 所以我们需要在全局用‘Wire1’替换‘Wire’的定义不更好的方法是使用库提供的接口。 // 实际上SimpleFOC库的MagneticSensorI2C构造函数可以接受一个TwoWire*参数。 }上面代码中有一个关键点MagneticSensorI2C的构造函数是否支持传入自定义的TwoWire对象。在我使用的版本中你需要检查库的头文件。如果支持代码应该是这样的// 创建自定义I2C对象 TwoWire I2Ctwo TwoWire(1); // 在构造函数中传入自定义的Wire指针 MagneticSensorI2C sensor2 MagneticSensorI2C(AS5600_I2C, I2Ctwo);如果库不支持你可能需要稍微修改一下库的源代码或者确保你的SimpleFOC库是最新版本因为多I2C支持是双电机控制的常见需求社区可能已经更新了。初始化顺序很重要一定要先执行I2Ctwo.begin(19, 18)然后再调用sensor2.init()。否则sensor2.init()内部可能会去初始化默认的Wire对象。3.3 验证测试读取两个编码器角度配置好之后不要急着接电机先写个简单的测试程序在loop()里打印两个编码器的角度值。void loop() { // 更新传感器数据 sensor1.update(); sensor2.update(); Serial.print(Motor1 Angle: ); Serial.print(sensor1.getAngle()); Serial.print(\t Motor2 Angle: ); Serial.println(sensor2.getAngle()); delay(10); }上传代码打开串口监视器。用手分别转动两个电机你应该能看到两个角度值独立、平滑地变化。如果只有一个有变化或者数据全为0请检查I2C地址是否正确AS5600默认地址是0x36。引脚连接是否牢固VCC和GND是否接好。上拉电阻I2C总线需要上拉电阻通常4.7kΩ到10kΩ。开发板或Shield上可能已经集成如果没有你需要在外围电路加上。代码中引脚号是否写反SDA和SCL。4. 双电机速度同步控制实战当两个编码器都能正确读数后我们就可以进入真正的电机控制了。我们先从最常见的速度同步开始让两个电机以相同的转速旋转或者保持一个固定的速度比例。4.1 初始化两个完整的FOC电机对象这里以3PWM驱动为例你需要为每个电机定义驱动、传感器、电机本体、速度环PID。#include SimpleFOC.h // 电机1 BLDCDriver3PWM driver1 BLDCDriver3PWM(32, 33, 25, 14); // PWM引脚: 32,33,25, 使能引脚: 14 MagneticSensorI2C sensor1 MagneticSensorI2C(AS5600_I2C); BLDCMotor motor1 BLDCMotor(7); // 7对极电机 // 速度环PID PIDController speed_pid1 PIDController(0.1, 2, 0, 1000, 12); // P0.1, I2, 输出限幅12V // 电机2 - 使用不同的PWM引脚和自定义I2C TwoWire I2Ctwo TwoWire(1); BLDCDriver3PWM driver2 BLDCDriver3PWM(26, 27, 12, 13); // PWM引脚: 26,27,12, 使能引脚: 13 MagneticSensorI2C sensor2 MagneticSensorI2C(AS5600_I2C, I2Ctwo); // 传入自定义I2C BLDCMotor motor2 BLDCMotor(7); PIDController speed_pid2 PIDController(0.1, 2, 0, 1000, 12); // 目标速度弧度/秒 float target_speed 0.0; void setup() { Serial.begin(115200); // 初始化电机1 Wire.begin(); sensor1.init(); driver1.init(); motor1.linkDriver(driver1); motor1.linkSensor(sensor1); motor1.controller MotionControlType::velocity; // 设置为速度控制模式 motor1.PID_velocity speed_pid1; motor1.init(); motor1.initFOC(); // 初始化电机2 I2Ctwo.begin(19, 18); // 先初始化I2C总线 sensor2.init(); driver2.init(); motor2.linkDriver(driver2); motor2.linkSensor(sensor2); motor2.controller MotionControlType::velocity; motor2.PID_velocity speed_pid2; motor2.init(); motor2.initFOC(); Serial.println(双电机FOC初始化完成); Serial.println(发送 A 或 B 加数字设置单个电机速度如 A6.28); Serial.println(发送 S 加数字设置同步速度如 S10.0); } void loop() { // 必须循环执行FOC motor1.loopFOC(); motor2.loopFOC(); // 速度控制 motor1.move(target_speed); motor2.move(target_speed); // 此时两个电机目标速度相同 // 简单的串口命令解析 if (Serial.available()) { char cmd Serial.read(); if (cmd S || cmd s) { target_speed Serial.parseFloat(); Serial.print(设置同步目标速度: ); Serial.println(target_speed); } else if (cmd A || cmd a) { float speedA Serial.parseFloat(); motor1.move(speedA); Serial.print(设置电机A速度: ); Serial.println(speedA); } else if (cmd B || cmd b) { float speedB Serial.parseFloat(); motor2.move(speedB); Serial.print(设置电机B速度: ); Serial.println(speedB); } } }4.2 实现比例速度同步上面的代码让两个电机完全同速。但更多时候我们需要的是比例同步比如机器人差速转向时左轮和右轮速度需要满足V_left / V_right k的关系。这需要在loop()中做一些计算// 全局变量 float base_speed 0.0; // 基准速度 float speed_ratio 1.0; // 速度比电机2速度 电机1速度 * ratio void loop() { motor1.loopFOC(); motor2.loopFOC(); // 计算各自的目标速度 float target_speed1 base_speed; float target_speed2 base_speed * speed_ratio; motor1.move(target_speed1); motor2.move(target_speed2); // 串口可以设置基准速度和比例 // 例如: BASE 5.0 设置基准速度5 rad/s // RATIO 0.8 设置速度比为0.8即电机2是电机1速度的80% }这样你只需要动态调整base_speed和speed_ratio就能轻松实现复杂的协同运动比如让一个电机正转另一个反转ratio -1实现原地旋转。5. 双电机位置同步与跟随控制速度同步解决了“转得一样快”的问题而位置同步则解决“转到一样的位置”或“保持固定的角度差”的问题。这在机械臂关节、精密云台上至关重要。5.1 位置同步模式让两个电机同时到达指定的绝对角度。我们需要将控制模式改为MotionControlType::angle。// 在setup()中修改控制模式 motor1.controller MotionControlType::angle; motor2.controller MotionControlType::angle; // 需要配置位置环PID PIDController angle_pid1 PIDController(10, 0, 0.1, 20, 12); // 参数需要调试 motor1.PID_angle angle_pid1; // 同样为motor2配置 void loop() { motor1.loopFOC(); motor2.loopFOC(); float target_angle 3.14; // 目标位置例如π弧度180度 motor1.move(target_angle); motor2.move(target_angle); // 同时运动到同一位置 }5.2 主从跟随模式这是更高级的协同一个电机从电机的位置始终跟随另一个电机主电机。这在模仿人类关节如肘关节跟随肩关节时非常有用。实现思路是在每一个控制周期读取主电机当前的实际角度将其或经过一个偏移量计算后设置为从电机的目标角度。void loop() { // 更新传感器数据获取实时角度 sensor1.update(); sensor2.update(); float master_angle sensor1.getAngle(); // 主电机当前角度 // 从电机的目标角度 主电机角度 固定偏移例如90度 float slave_target_angle master_angle 1.57; // 1.57弧度 90度 motor1.loopFOC(); motor2.loopFOC(); // 主电机可以自由控制这里假设它由其他逻辑驱动 // motor1.move(some_target); // 从电机紧紧跟随 motor2.move(slave_target_angle); }这种模式下从电机会动态地、实时地追踪主电机的运动形成一种机械上的“耦合”关系。调试的关键在于位置环PID的参数整定P值太小跟随慢、有滞后P值太大会产生振荡。6. 调试技巧与常见问题排查双电机系统调试起来比单电机复杂问题可能出在硬件、软件或者两者之间的干扰上。下面是我总结的几个常见坑点和解决方法。问题一只有一个电机转或者两个电机转动不同步抖动严重。检查电源功率这是最常见的原因两个电机同时工作启动和换相时电流很大。你的12V电源是否提供了足够的电流建议使用额定电流至少是单个电机额定电流3倍以上的开关电源。电源功率不足会导致电压被拉低ESP32重启或驱动器保护。检查PID参数双电机系统的负载可能不完全对称。电机1的PID参数调好了直接复制给电机2可能不工作。你需要分别调试两个电机的PID参数。先用串口命令单独控制每个电机微调其速度环和位置环的P、I值直到每个电机单独运行都平稳、响应迅速且无振荡。检查时序确保motor1.loopFOC()和motor2.loopFOC()在loop()中被依次、无延迟地调用。不要在它们中间插入长时间的delay()。FOC算法需要高频率、稳定的执行。问题二I2C编码器读数失败或跳动。总线干扰电机驱动产生的PWM信号是强烈的噪声源。确保I2C信号线SDA SCL远离电机的大电流线路。如果使用杜邦线尽量使用双绞线或屏蔽线。电源噪声为编码器提供干净的3.3V电源。可以尝试在编码器的VCC和GND之间并联一个10uF和0.1uF的电容进行滤波。上拉电阻确认I2C总线上有合适的上拉电阻通常4.7kΩ。如果线缆较长可以适当减小阻值如2.2kΩ以增强驱动能力。问题三协同运动时出现周期性抖动或异响。控制频率不一致两个电机的FOC计算是否以相同的频率进行在loop()中确保两者的loopFOC()和move()调用间隔稳定。可以考虑使用micros()函数实现一个精确的定时循环例如固定每1ms执行一次控制逻辑。机械耦合如果两个电机在机械上是连接的比如同一个底盘上的两个轮子那么一个电机的振动会通过机械结构传递给另一个电机形成反馈。这时需要适当降低PID的增益特别是D值或者考虑加入低通滤波器来平滑目标指令。一个实用的调试流程分而治之务必先让每个电机在开环模式下单独正常旋转。这能排除最基本的硬件连接和驱动问题。独立闭环然后让每个电机独立进行速度闭环、位置闭环控制并调好各自的PID参数。简单协同最后再尝试让它们协同工作从简单的同速开始逐步增加复杂度如比例同步、位置跟随。数据监控充分利用SimpleFOC Studio或自己编写串口数据发送代码将两个电机的目标值、实际值、PID输出等关键数据实时绘制出来。图形化的数据比看数字直观一百倍能帮你快速定位是哪个环节出现了抖动、滞后或超调。最后别忘了ESP32有双核。对于极其复杂的协同算法你可以考虑将两个电机的FOC计算任务分别放在两个核心上用FreeRTOS的任务来管理这能极大提高控制频率和实时性。当然这属于进阶玩法在大部分应用场景中单核顺序执行已经足够稳定可靠。