以专业的视角来看，基于 Arduino 生态（通常以 ESP32、STM32 等高性能 MCU 为核心）的 BLDC 机器人主从协调、位置同步与力矩分配系统，是多轴协同控制领域的核心工程方案。该系统通过“主轴指令 + 从轴跟随 + 力矩动态均衡”的架构，解决了刚性连接多电机系统中的“互搏”问题，实现了多执行器的柔性协同。

一、 主要特点

1. 主轴速度环 + 从轴扭矩环的主从架构
   对于刚性连接的多电机系统（如双驱 AGV、龙门机构），若双轴均采用速度闭环，会因机械装配误差导致“互搏”（一个推一个拉）。主从策略采用“主轴速度环 + 从轴扭矩环”的架构：主轴负责精确跟踪目标速度/位置，从轴则通过扭矩环跟踪主轴，利用扭矩差自动消除位置误差。这是实现柔性跟随、解决机械装配误差的黄金法则。
2. 高精度位置同步与交叉耦合补偿
   系统通过编码器实时采集各轴位置，计算轴间位置误差。在交叉耦合控制中，系统计算两轴位置误差的差值，动态补偿滞后轴，确保多轴“同时启动、同时结束”。在工业级应用中，轴间同步误差可控制在微秒级，满足精密装配与高速分拣的需求。
3. 基于电流环的力矩分配与柔顺控制
   BLDC 电机的输出力矩与 q 轴电流（Iq）成正比（T = Kt × Iq）。系统通过 FOC 矢量控制，精确调节各轴的 Iq 电流，实现力矩的精准分配。在力矩分配策略中，主控实时采集各轴的力反馈（通过电流估算或力传感器），通过 PID 算法动态调整各轴输出，避免单轴过载。在柔顺控制场景下，可在力控外环叠加阻抗模型（如弹簧-阻尼模型），实现人机协作的安全交互。
4. 分层控制与异构计算架构
   系统通常采用三层分层控制架构：

上位机负责全局轨迹规划，MCU 负责实时力矩分配与位置同步，驱动板负责底层 FOC 执行，三者通过 CAN/EtherCAT 总线进行低延迟通信。
二、 应用场景

1. 双驱 AGV / 龙门搬运机器人
   在双驱 AGV 或龙门搬运机器人中，左右两侧电机刚性连接同一负载。主从协调确保两侧电机输出力矩均衡，避免“一推一拉”的互搏现象。位置同步确保两侧行程一致，防止龙门机构卡死或偏斜。
2. 多关节机械臂协同控制
   在六轴或多轴机械臂中，各关节需协同运动以完成末端轨迹跟踪。主从协调确保各关节按预定轨迹同步运动，力矩分配根据各关节负载动态调整，避免某一关节过载。
3. 双机器人协同搬运
   在双机器人协同搬运场景中，主机器人主动规划轨迹，从机器人实时跟踪主机器人位置。同时，通过力传感器检测协同力偏差，动态调整扭矩，确保双机器人平稳承接负载，避免单侧过载。
4. 仿生双足机器人步态控制
   在双足机器人中，左右腿需协同完成步态周期。主从协调确保双腿交替支撑与摆动，力矩分配根据足底压力传感器动态调整，实现稳定的行走与平衡。
5. 张力控制与卷绕系统
   在放卷/收卷系统中，主从协调确保放卷与收卷电机的速度同步，力矩分配根据张力传感器反馈动态调整，保持材料张力恒定。
   三、 需要注意的事项
6. 主控算力瓶颈与实时性保障
   FOC 算法需要高频计算（通常 > 1kHz），标准 Arduino Uno（16MHz）无法胜任。强烈建议采用 ESP32（双核 240MHz）、STM32F4/H7 或 Teensy 4.x 等高性能板卡（主频 ≥ 200MHz，支持硬件 FPU）。控制频率需足够高，且安全逻辑（如过流切断、急停）必须具有最高优先级，必要时使用硬件比较器或中断直接关断 PWM，以防软件死机无法保护硬件。
7. 通信延迟与同步精度
   主从协调高度依赖低延迟通信。I2C 延迟 < 10ms，nRF24L01 延迟 < 20ms，WiFi 延迟较高（50-100ms）。力共享控制周期建议 ≤ 50ms 以保持同步。必须采用 CAN/EtherCAT 等工业总线，确保通信延迟 ≤ 1ms，实现微秒级轴间同步。
8. 力矩分配策略与过载保护
   力矩分配策略需根据实际负载动态调整。主控应实时采集从机力反馈（如电流/力矩），通过 PID 算法动态调整自身 PWM 输出，避免过载或力分配不均。必须加入电流超限保护（ESC 内置过流保护），防止单点故障导致设备损坏。
9. 机械装配误差与系统标定
   机械装配误差会导致轴间位置偏差，影响同步精度。必须进行系统标定：开环测试各轴同 PWM 下的转速差异并计算补偿系数；闭环通过编码器反馈动态调整速度差。对于刚性连接的多电机系统，必须采用主从扭矩环策略，利用扭矩差消除位置误差。
10. 电源管理与电磁兼容
    多 BLDC 电机同时运行或紧急制动时，会产生巨大的电流冲击和电磁干扰，极易导致主控复位或传感器数据跳变。必须使用独立的 DC-DC 降压模块为逻辑电路供电，严禁直接使用电机电池供电；在 ESC 电源输入端并联大容量低 ESR 电解电容吸收电流尖峰；强电与弱电线路严格分离，编码器信号线使用屏蔽线并加硬件滤波。
11. 安全机制与容错设计
    必须设计完善的安全策略：硬件层面配备急停按钮、过流/过压/过温保护；软件层面设置最大电流/速度限幅、心跳检测（如 200ms 无通信则停车）、通信丢失时自动停机。对于协作机器人，还需在力控外环叠加阻抗模型，实现人机协作的安全交互。

1、线控转向虚拟链接——双电机位置同步与“虚拟弹簧”力矩分配
适用场景：双电机通过机械结构刚性连接，需维持精确角度同步并防止“互搏”（一推一拉导致电流激增）。该案例直接参考了SimpleFOC官方示例中的线控转向虚拟链接方案。
核心逻辑：主电机与从电机均设为扭矩（电压）控制模式，通过编码器实时读取各自角度。控制律根据两电机角度差施加比例力矩——角度差越大，纠正力矩越强。这本质上是一种“虚拟弹簧”连接，实现柔性同步且不会发生刚性互搏。

#include <SimpleFOC.h>
#include <PciManager.h>
#include <PciListenerImp.h>

// ==================== 主电机（带编码器）====================
BLDCMotor motorMaster(7);
BLDCDriver3PWM driverMaster(9, 10, 11, 8);
Encoder encoderMaster(18, 19, 2048, 20); // A,B,PPR,Index
void doMA(){ encoderMaster.handleA(); }
void doMB(){ encoderMaster.handleB(); }
void doMI(){ encoderMaster.handleIndex(); }

// ==================== 从电机（磁传感器I2C）====================
BLDCMotor motorSlave(7);
BLDCDriver3PWM driverSlave(3, 5, 6, 7);
MagneticSensorI2C sensorSlave(0x36, 12, 0x0E, 4); // AS5600

// ==================== 同步参数 ====================
const float SYNC_GAIN = 5.0;  // 虚拟链接刚度，越大越“刚性”

void setup() {
    Serial.begin(115200);
    
    // ---- 主电机初始化 ----
    encoderMaster.init();
    PciManager.registerListener(&PciListenerImp(encoderMaster.pinA, doMA));
    PciManager.registerListener(&PciListenerImp(encoderMaster.pinB, doMB));
    PciManager.registerListener(&PciListenerImp(encoderMaster.index_pin, doMI));
    motorMaster.linkSensor(&encoderMaster);
    motorMaster.linkDriver(&driverMaster);
    motorMaster.controller = MotionControlType::torque;  // 关键：扭矩模式
    motorMaster.init();
    motorMaster.initFOC();
    
    // ---- 从电机初始化 ----
    sensorSlave.init();
    motorSlave.linkSensor(&sensorSlave);
    motorSlave.linkDriver(&driverSlave);
    motorSlave.controller = MotionControlType::torque;
    motorSlave.init();
    motorSlave.initFOC();
}

void loop() {
    // 1. FOC更新
    motorMaster.loopFOC();
    motorSlave.loopFOC();
    
    // 2. 核心：虚拟链接同步控制
    // 两电机根据角度差施加相反的纠正力矩，维持位置同步
    float angleDiff = motorSlave.shaft_angle - motorMaster.shaft_angle;
    
    motorMaster.move( SYNC_GAIN * angleDiff);  // 主电机追赶从机
    motorSlave.move( -SYNC_GAIN * angleDiff);  // 从电机追赶主机
    
    // 3. 调试输出
    Serial.print("Master:"); Serial.print(motorMaster.shaft_angle);
    Serial.print(" Slave:"); Serial.print(motorSlave.shaft_angle);
    Serial.print(" Diff:"); Serial.println(angleDiff);
    
    delay(10);
}

2、主速度-从扭矩同步模式（防止刚性连接互搏）
适用场景：双电机通过齿轮箱耦合到同一输出轴（如消防机器人驱动轮），负载差异大时若双机均为速度模式会产生“互搏”。本案例参考了主从模式扭矩同步的典型设计思路。
核心逻辑：主轴运行速度闭环（主速度模式），从轴运行扭矩闭环（从扭矩模式）。从轴根据主轴的实际速度与自身速度的偏差，动态调整输出扭矩——若从轴慢了，增加正向扭矩；若从轴快了，减小扭矩甚至施加反向制动扭矩。这种“柔性跟随”策略有效防止刚性耦合下的电流激增。

#include <SimpleFOC.h>

// ==================== 主从电机定义 ====================
BLDCMotor motorMaster(7), motorSlave(7);
BLDCDriver3PWM driverMaster(9,10,11,8), driverSlave(3,5,6,7);
Encoder encoderMaster(18,19,2048), encoderSlave(20,21,2048);

// ==================== 同步参数 ====================
const float SYNC_GAIN = 0.25;       // 同步补偿系数
const float BASE_TORQUE = 0.5;      // 从轴基础扭矩指令
float targetSlaveTorque = 0;

void setup() {
    Serial.begin(115200);
    
    // ---- 主轴：速度模式 ----
    motorMaster.linkSensor(&encoderMaster);
    motorMaster.linkDriver(&driverMaster);
    motorMaster.controller = MotionControlType::velocity;
    motorMaster.init();
    motorMaster.initFOC();
    
    // ---- 从轴：扭矩模式（柔性跟随） ----
    motorSlave.linkSensor(&encoderSlave);
    motorSlave.linkDriver(&driverSlave);
    motorSlave.controller = MotionControlType::torque;  // 关键
    motorSlave.init();
    motorSlave.initFOC();
}

void loop() {
    // 1. FOC更新
    motorMaster.loopFOC();
    motorSlave.loopFOC();
    
    // 2. 主轴运行（速度闭环）
    motorMaster.move(2.0);  // 目标速度 2.0 rad/s
    
    // 3. 从轴扭矩动态补偿
    float masterVel = motorMaster.shaft_velocity;
    float slaveVel = motorSlave.shaft_velocity;
    float velError = masterVel - slaveVel;  // 正=从轴慢了
    
    // 柔性跟随：根据速度差动态调整扭矩
    targetSlaveTorque = BASE_TORQUE + (velError * SYNC_GAIN);
    targetSlaveTorque = constrain(targetSlaveTorque, -1.5, 1.5);
    
    motorSlave.move(targetSlaveTorque);
    
    // 调试
    Serial.print("M:"); Serial.print(masterVel);
    Serial.print(" S:"); Serial.print(slaveVel);
    Serial.print(" Err:"); Serial.print(velError);
    Serial.print(" Torq:"); Serial.println(targetSlaveTorque);
    
    delay(10);
}

3、CAN总线主从多轴同步——位置/速度双环跟随
适用场景：多关节机器人或多机协同搬运，需实现严格的相位同步和位置跟踪。本案例参考了CAN总线多电机同步控制的主从架构设计思路。
核心逻辑：一台主机通过CAN总线定期广播同步帧（含目标位置、速度），多台从机接收后执行位置环+速度环双环控制。从机定期反馈实际状态，主机监测同步误差并在超限时发出调整指令。

// ==================== 主机代码（Arduino Mega + CAN）====================
#include <SPI.h>
#include <MCP2515.h>
#include <SimpleFOC.h>

// BLDC电机定义
BLDCMotor motorMaster(7);
BLDCDriver3PWM driverMaster(3,5,6,8);
Encoder encoderMaster(18,19,2048);

// CAN通信
MCP2515 canBus(10, 2);  // CS, INT

// 双环PID参数（示例）
float Kp_pos = 2.0, Ki_pos = 0.05, Kd_pos = 0.1;
float Kp_vel = 0.8, Ki_vel = 0.02, Kd_vel = 0.05;

float targetPos = 0, currentPos = 0;
float targetVel = 0, currentVel = 0;
float lastPosError = 0, posIntegral = 0;

void setup() {
    Serial.begin(115200);
    
    // 初始化BLDC
    motorMaster.linkSensor(&encoderMaster);
    motorMaster.linkDriver(&driverMaster);
    motorMaster.controller = MotionControlType::velocity;
    motorMaster.init();
    motorMaster.initFOC();
    
    // 初始化CAN
    canBus.reset();
    canBus.setBitrate(CAN_500KBPS);
    canBus.setNormalMode();
}

// 位置环计算
float positionLoop(float target, float current, float dt) {
    float error = target - current;
    posIntegral += error * dt;
    posIntegral = constrain(posIntegral, -5.0, 5.0);
    float derivative = (error - lastPosError) / (dt + 0.0001);
    lastPosError = error;
    return Kp_pos * error + Ki_pos * posIntegral + Kd_pos * derivative;
}

void loop() {
    motorMaster.loopFOC();
    static unsigned long lastTime = millis();
    float dt = (millis() - lastTime) / 1000.0;
    lastTime = millis();
    
    // 1. 更新自身状态
    currentPos = motorMaster.shaft_angle;
    currentVel = motorMaster.shaft_velocity;
    
    // 2. 位置环：计算目标速度
    targetVel = positionLoop(targetPos, currentPos, dt);
    targetVel = constrain(targetVel, -3.0, 3.0);
    
    // 3. 速度环执行
    motorMaster.move(targetVel);
    
    // 4. 广播同步帧到CAN总线
    CanMsg syncFrame;
    syncFrame.id = 0x100;
    syncFrame.data[0] = (uint8_t)(targetPos * 100) >> 8;
    syncFrame.data[1] = (uint8_t)(targetPos * 100) & 0xFF;
    syncFrame.data[2] = (uint8_t)(targetVel * 100) >> 8;
    syncFrame.data[3] = (uint8_t)(targetVel * 100) & 0xFF;
    canBus.sendMessage(syncFrame);
    
    // 5. 接收从机反馈
    if (canBus.available()) {
        CanMsg feedback = canBus.readMessage();
        float slavePos = ((feedback.data[0] << 8) | feedback.data[1]) / 100.0;
        float syncError = abs(targetPos - slavePos);
        if (syncError > 0.1) {
            // 误差超限：发送调整指令
            CanMsg adjust;
            adjust.id = 0x101;
            adjust.data[0] = (uint8_t)(syncError * 10);
            canBus.sendMessage(adjust);
        }
    }
    
    delay(10);
}

// ==================== 从机代码（Arduino Uno + CAN）====================
#include <SPI.h>
#include <MCP2515.h>
#include <SimpleFOC.h>

BLDCMotor motorSlave(7);
BLDCDriver3PWM driverSlave(9,10,11,8);
Encoder encoderSlave(18,19,2048);

MCP2515 canBus(10, 2);

float syncPos = 0, syncVel = 0;
float currentPos = 0, currentVel = 0;

// 双环PID（同主机）
float Kp_pos = 2.0, Ki_pos = 0.05, Kd_pos = 0.1;
float Kp_vel = 0.8, Ki_vel = 0.02, Kd_vel = 0.05;
float lastPosError = 0, posIntegral = 0;

void setup() {
    motorSlave.linkSensor(&encoderSlave);
    motorSlave.linkDriver(&driverSlave);
    motorSlave.controller = MotionControlType::velocity;
    motorSlave.init();
    motorSlave.initFOC();
    
    canBus.reset();
    canBus.setBitrate(CAN_500KBPS);
    canBus.setListenOnlyMode();
}

void loop() {
    motorSlave.loopFOC();
    currentPos = motorSlave.shaft_angle;
    currentVel = motorSlave.shaft_velocity;
    
    // 接收同步帧
    if (canBus.available()) {
        CanMsg frame = canBus.readMessage();
        if (frame.id == 0x100) {
            syncPos = ((frame.data[0] << 8) | frame.data[1]) / 100.0;
            syncVel = ((frame.data[2] << 8) | frame.data[3]) / 100.0;
        }
    }
    
    // 执行位置环+速度环同步跟踪
    float targetVel = positionLoop(syncPos, currentPos, 0.01);
    targetVel = constrain(targetVel, -3.0, 3.0);
    motorSlave.move(targetVel);
    
    delay(10);
}

要点解读
主从协调的本质是“一个控状态，一个控力”：当双电机刚性耦合时，若两者均为速度模式，微小差异即引发“互搏”（电流激增、发热）。采用主速度-从扭矩策略，让从轴以“柔性”方式跟随主轴，是实现刚性耦合下多电机协同的工程标准方案。SimpleFOC官方示例中的“虚拟链接”实质上也是基于此思想，通过位置差生成纠正力矩。

位置同步的“虚拟弹簧”模型实现柔性连接：案例一中motorMaster.move(SYNC_GAIN * angleDiff)和motorSlave.move(-SYNC_GAIN * angleDiff)构成了一对“虚拟弹簧”——角度差越大，纠正力矩越强，使两电机维持动态平衡。增益SYNC_GAIN的取值需要经验调试：过高会导致震荡，过低则同步刚度不足。

力矩分配需区分“负载均衡”与“防互搏”两种场景：同轴并联驱动（如案例一、二）的目标是防止互搏，采用从轴动态补偿策略；多轮独立驱动（如四轮AGV）的目标是动态分配驱动力，需引入扭矩矢量控制（如通过IMU检测车身姿态，调整左右轮扭矩比例防止打滑）。

CAN总线是工业级多轴同步的通信基石：对于严格的位置/速度同步（如印刷机辊轴、多关节机器人），必须采用CAN FD或EtherCAT等总线协议。CAN的广播帧机制天然支持“一发多收”的同步模式，配合双环PID控制（位置环+速度环），可实现从机对主机指令的高精度跟随。

同步误差监测与自适应补偿是系统稳定性的保障：案例三中主机定期检查从机反馈的同步误差，超限时发送调整指令。更完善的系统可引入自整定PID——根据同步误差大小动态调整PID增益，使系统在不同负载条件下保持一致的同步精度。工程实践中，应同时设置软件限位和急停逻辑作为安全兜底。

4、主从式恒力装配与位置补偿系统（精密电子装配场景）
适用场景：精密电子装配（如PCB元器件插件、微小螺丝锁付），主机器人负责核心装配动作，从机器人辅助定位，需严格维持恒定接触力，避免硬碰撞损伤精密零件。
核心逻辑：主机器人执行装配轨迹，从机器人通过末端力传感器实时监测接触力，以力闭环为内环、位置补偿为外环，将力误差转化为位置修正量，叠加到主机器人轨迹中；主从机器人通过共享位置与力信息实现同步，力矩按负载比例分配，确保接触力恒定。
参考代码（Arduino ESP32 + SimpleFOC，硬件：双BLDC关节、末端拉压力传感器）：

#include <SimpleFOC.h>
#include <Wire.h>

// 硬件：主从双BLDC关节（主：joint1，从：joint2）+ 末端力传感器
BLDCMotor joint1(7), joint2(8);
BLDCDriver3PWM driver1(3, 5, 6), driver2(9, 10, 11);
Encoder enc1(18, 19, 2048), enc2(20, 21, 2048);

// 力控参数
const int FORCE_PIN = A0;
float forceTarget = 0.5;  // 目标接触力（N，适配精密装配）
float forceActual = 0;
float positionOffset = 0;
float maxOffset = 0.5;     // 最大位置补偿量（mm）

// 主从同步通信（简化：共享力与位置数据，实际可扩展Modbus/串口）
float masterPos = 0, slavePos = 0, masterForce = 0;

void setup() {
  Serial.begin(115200);
  // 初始化主从关节（力矩模式）
  joint1.controller = MotionControlType::torque;
  joint2.controller = MotionControlType::torque;
  joint1.linkDriver(&driver1); joint2.linkDriver(&driver2);
  joint1.linkSensor(&enc1); joint2.linkSensor(&enc2);
  joint1.init(); joint1.initFOC();
  joint2.init(); joint2.initFOC();
  enc1.init(); enc2.init();
}

void loop() {
  float dt = 0.01;  // 100Hz控制周期
  joint1.loopFOC();
  joint2.loopFOC();

  // 1. 从机器人读取力传感器（主机器人同步获取自身力）
  forceActual = analogRead(FORCE_PIN) * 10.0 / 1023.0;  // 校准系数适配
  masterForce = forceActual;

  // 2. 力误差计算与位置补偿（从机器人核心逻辑）
  float forceError = forceTarget - forceActual;
  positionOffset += forceError * 0.1 * dt;  // 位置补偿增益
  positionOffset = constrain(positionOffset, -maxOffset, maxOffset);

  // 3. 主从位置同步（主机器人发送目标位置，从机器人跟踪）
  masterPos = 30.0;  // 主机器人目标位置（mm）
  slavePos = masterPos + positionOffset;  // 从机器人叠加补偿量

  // 4. 力矩分配（按主从负载比例，主60%，从40%）
  float torqueCmd = forceError * 0.05;  // 力矩指令（基于力误差）
  joint1.move(torqueCmd * 0.6);  // 主关节力矩
  joint2.move(torqueCmd * 0.4);  // 从关节力矩

  // 5. 安全保护：力超限急停
  if (forceActual > forceTarget * 2.0) {
    joint1.move(0);
    joint2.move(0);
    Serial.println("Overforce! Emergency stop.");
  }

  delay(10);
}

5、双机动态跟随与协同任务分配系统（智能仓储拣选场景）
适用场景：智能仓储多机器人协同拣选，领航机器人（主）跟随拣货员移动，跟随机器人（从）保持队形，需实时同步位置，动态分配任务（如主机器人负载满时，从机器人主动接管）。
核心逻辑：主从机器人通过UWB双向测距获取相对位置，主机器人广播自身位置与任务状态，从机器人跟踪主机器人轨迹，通过位置同步算法保持队形；任务分配引擎根据主从机器人的负载、电量动态调整任务归属，力矩按任务优先级分配，确保协同效率。
参考代码（Arduino ESP32 + SimpleFOC + ESP-NOW通信，硬件：双BLDC差速底盘、UWB模块）：

#include <SimpleFOC.h>
#include <esp_now.h>

// 硬件：主从BLDC差速底盘 + ESP-NOW通信
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(3, 5, 6), drvR(9, 10, 11);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);

// 主从状态结构体（通过ESP-NOW共享）
struct RobotState {
  float x, y, heading;  // 位置与航向
  bool isBusy;          // 任务状态（主：true=执行，从：true=空闲）
  float torqueReq;      // 力矩需求
} masterState, slaveState;

// 跟随参数
const float DESIRED_OFFSET = 0.8;  // 期望队形间距（m）
const float Kp_pos = 1.5, Kp_ang = 2.0;  // 位置/角度同步增益

void setup() {
  Serial.begin(115200);
  // 初始化BLDC差速底盘（速度模式）
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  encL.init(); encR.init();

  // ESP-NOW初始化（省略具体配置，核心是绑定主从MAC地址）
  // esp_now_init();
  // esp_now_register_send_cb(sendCallback);
}

// ESP-NOW发送回调（主机器人发送状态，从机器人接收）
void sendCallback(const uint8_t *mac, const esp_now_send_status_t status) {
  // 主机器人广播自身状态，从机器人接收后更新slaveState
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();

  // 1. 主从状态同步（从机器人接收主机器人数据）
  // 此处简化为手动赋值，实际通过ESP-NOW接收
  if (masterState.isBusy) {  // 主机器人执行任务，从机器人跟随
    // 2. 位置同步计算：从机器人跟踪主机器人后方0.8m
    float dx = masterState.x - slaveState.x;
    float dy = masterState.y - slaveState.y;
    float distanceError = sqrt(dx*dx + dy*dy) - DESIRED_OFFSET;
    float angleError = atan2(dy, dx) - slaveState.heading;
    if (angleError > M_PI) angleError -= 2*M_PI;
    if (angleError < -M_PI) angleError += 2*M_PI;

    // 3. 差速驱动（位置同步输出速度）
    float linearSpeed = Kp_pos * distanceError;
    float angularSpeed = Kp_ang * angleError;
    float wheelBase = 0.25;  // 轮距（m）
    motorL.move(linearSpeed - angularSpeed * wheelBase / 2);
    motorR.move(linearSpeed + angularSpeed * wheelBase / 2);

    // 4. 任务动态分配：主机器人负载满时，从机器人接管
    if (masterState.torqueReq > 0.8) {
      masterState.isBusy = false;
      slaveState.isBusy = true;
      // 力矩切换：从机器人接管后，力矩分配比例反转
      motorL.move(linearSpeed * 0.4);
      motorR.move(linearSpeed * 0.4);
    }
  }

  delay(50);
}

6、崎岖地形多轮力矩动态分配系统（野外巡检场景）
适用场景：野外巡检机器人（四轮独立BLDC驱动）在山地、泥地等崎岖地形行驶，需根据地形姿态动态分配各轮力矩，避免打滑，保持位置同步与姿态稳定。
核心逻辑：主控机器人（主）通过IMU感知整体姿态，从控轮组（从）通过编码器监测轮速，主从协同计算各轮负载比例；基于滑移率补偿与坡度负载转移，动态调整各轮力矩，实现负载均衡，同时通过轮速闭环保持位置同步，确保机器人在复杂地形稳定行驶。
参考代码（Arduino ESP32 + SimpleFOC + MPU6050，硬件：四轮BLDC驱动、IMU）：

#include <SimpleFOC.h>
#include <MPU6050.h>

// 硬件：四轮BLDC（简化为左右双轮，核心逻辑通用）+ MPU6050
BLDCMotor leftMotor(7), rightMotor(8);
BLDCDriver3PWM drvL(3, 5, 6), drvR(9, 10, 11);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
MPU6050 imu;

// 力矩分配参数
const float BASE_TORQUE = 0.6;  // 基础力矩（归一化）
float wheelLoad[2] = {1.0, 1.0};  // 左右轮负载系数
const float SLIP_THRESHOLD = 50;  // 滑移率阈值（rpm）
float Kp_slip = 0.1;  // 滑移补偿增益

void setup() {
  Serial.begin(115200);
  // 初始化BLDC（力矩模式）
  leftMotor.controller = MotionControlType::torque;
  rightMotor.controller = MotionControlType::torque;
  leftMotor.linkDriver(&drvL); rightMotor.linkDriver(&drvR);
  leftMotor.linkSensor(&encL); rightMotor.linkSensor(&encR);
  leftMotor.init(); leftMotor.initFOC();
  rightMotor.init(); rightMotor.initFOC();
  encL.init(); encR.init();

  // 初始化IMU
  imu.initialize();
}

void loop() {
  leftMotor.loopFOC();
  rightMotor.loopFOC();

  // 1. IMU获取姿态（坡度角计算）
  sensors_event_t accel;
  imu.getEvent(&accel);
  float rollAngle = atan2(accel.acceleration.y, accel.acceleration.z) * RAD_TO_DEG;

  // 2. 动态更新负载系数（坡度补偿）
  float correction = map(rollAngle, -30, 30, 0.7, 1.3);
  correction = constrain(correction, 0.5, 1.5);
  wheelLoad[0] = correction;
  wheelLoad[1] = 2.0f - wheelLoad[0];  // 左右负载互补

  // 3. 滑移率检测与力矩补偿（从轮协同）
  float velLeft = encL.getVelocity();
  float velRight = encR.getVelocity();
  float slipDiff = abs(velLeft - velRight);

  if (slipDiff > SLIP_THRESHOLD) {
    float compensation = Kp_slip * (velRight - velLeft);  // 滑移补偿量
    // 力矩动态分配：打滑轮减力矩，附着力轮加力矩
    leftMotor.move(BASE_TORQUE / wheelLoad[0] - compensation);
    rightMotor.move(BASE_TORQUE / wheelLoad[1] + compensation);
  } else {
    // 常规负载分配
    leftMotor.move(BASE_TORQUE / wheelLoad[0]);
    rightMotor.move(BASE_TORQUE / wheelLoad[1]);
  }

  // 4. 位置同步保护：轮速偏差过大时强制同步
  if (slipDiff > SLIP_THRESHOLD * 2) {
    float avgVel = (velLeft + velRight) / 2;
    leftMotor.move(avgVel * 0.01);
    rightMotor.move(avgVel * 0.01);
  }

  delay(20);
}

要点解读

1. 控制架构分层：主从协同与闭环嵌套，保障系统逻辑清晰
   主从协调+位置同步+力矩分配的核心是分层闭环控制，需严格区分主从角色与控制层级，避免逻辑耦合导致系统失控：
   主从角色分层：主机器人负责全局决策（轨迹规划、任务分配、姿态感知），从机器人负责局部执行（位置跟踪、力矩响应、状态反馈），二者通过轻量级通信（如ESP-NOW、Modbus）共享状态，确保决策与执行的一致性。
   闭环嵌套逻辑：通常采用“外环位置同步+内环力矩控制”的嵌套架构，外环根据主从位置误差生成力矩指令，内环通过FOC电流环快速响应，确保力矩精准输出；例如案例4中，从机器人以力误差为输入生成位置补偿量，再通过力矩环执行，实现力与位置的双重闭环。
   算力合理分配：主机器人承担复杂运算（如运动学解算、任务调度），从机器人聚焦实时控制（力矩计算、电机驱动），避免算力过载；若用标准Arduino，建议采用“上位机（ESP32/STM32）+下位机（Arduino）”架构，上位机负责主从协调，下位机负责BLDC底层控制。
2. 位置同步核心：误差闭环与动态补偿，适配多场景需求
   位置同步是主从协同的基础，需结合场景特性设计同步算法，确保从机器人精准跟踪主机器人轨迹，同时具备动态补偿能力：
   同步算法适配场景：
   精密装配场景（案例4）：采用力驱动的位置补偿，将力误差转化为位置修正量，避免硬碰撞，适合接触力敏感场景；
   跟随场景（案例5）：采用相对位置闭环，通过距离误差和角度误差计算速度指令，保持期望队形，适合移动协同场景；
   崎岖地形场景（案例6）：采用轮速闭环同步，通过编码器监测轮速偏差，强制平均速度，避免因打滑导致的位置失步，适合复杂地形。
   动态补偿机制：需引入实时误差补偿，如案例4中限制位置补偿量，防止过度补偿；案例5中引入角度归一化处理，避免角度误差跳变；案例3中通过IMU姿态数据动态调整负载系数，补偿地形带来的位置偏差。
   同步周期与实时性：位置同步周期需匹配BLDC控制频率（通常50-100Hz），避免延迟导致同步失效；需采用非阻塞代码设计，禁用长延时函数，确保控制周期稳定。
3. 力矩分配原则：负载适配与动态调节，兼顾效率与安全
   力矩分配是主从协同的关键执行环节，需根据场景需求（如恒力控制、负载均衡、防滑）设计分配策略，平衡动力输出与安全边界：
   分配策略适配场景：
   恒力控制场景（案例4）：按负载比例分配力矩，主从机器人根据力误差协同输出力矩，维持恒定接触力，避免过载；
   协同跟随场景（案例5）：按任务优先级分配力矩，主机器人优先分配力矩，从机器人在主机器人负载满时接管，确保任务连续性；
   崎岖地形场景（案例6）：按负载与滑移率动态分配，负载大的轮子分配更高力矩，打滑轮子降低力矩，提升附着力，避免空转。
   动态调节机制：引入实时反馈调节，如案例6中通过滑移率检测调整力矩，滑移率超过阈值时，将快速轮的力矩转移至慢速轮，类似限滑差速器逻辑；同时结合IMU姿态数据，动态修正负载系数，确保力矩分配与地形匹配。
   安全边界约束：力矩分配必须设置硬限幅，防止电机过载烧毁；同时设置软限幅，避免力矩突变导致机械冲击，如案例4中力超限2倍时触发急停，保障系统安全。
4. 安全机制保障：软硬件双重防护，筑牢系统安全底线
   主从协同涉及多机器人交互与力控操作，安全机制是落地的核心保障，需从硬件、软件双重设计，覆盖过载、碰撞、失步等风险：
   硬件安全防护：
   物理急停与过载保护：配备硬件急停按钮，直连电机驱动器使能端，紧急情况瞬间切断动力；电机驱动器设置电流限幅，防止过载烧毁；
   传感器冗余：关键传感器（力传感器、编码器、IMU）采用冗余设计，如力传感器失效时，切换至电流估算力矩，避免系统瘫痪；
   电磁兼容设计：BLDC驱动与传感器（如UWB、IMU）电源隔离，信号线远离动力线，避免电磁干扰导致数据异常，如案例2中UWB模块采用独立电源，防止电机干扰定位数据。
   软件安全逻辑：
   力/位置超限保护：设置力、位置、速度的软限幅，超限时触发减速或急停，如案例1中力超限2倍急停，案例3中轮速偏差过大强制同步；
   通信容错与失步处理：主从通信设置超时机制，通信中断时，从机器人切换至独立安全模式（如减速停车），避免失控；
   看门狗与故障恢复：启用硬件看门狗，程序卡死时自动复位；设置故障恢复逻辑，异常排除后自动恢复正常运行。
5. 参数整定与落地：从理论到实践的闭环优化
   主从协调+位置同步+力矩分配的参数无法仅凭理论公式确定，需结合实际场景与硬件特性，通过闭环调试实现落地：
   参数整定顺序：遵循“从内环到外环、从单机到协同”的顺序：
   先整定BLDC底层参数：包括FOC电流环、速度环的PID参数，确保电机运行平稳，无抖动、无超调；
   再整定单机控制参数：如位置环增益、力矩环增益，确保单机位置跟踪、力矩控制精准；
   最后整定主从协同参数：如位置同步增益、力矩分配比例、通信周期，确保主从协同流畅，无振荡、无失步。
   场景化参数适配：不同场景参数差异极大，需结合场景特性调整：
   精密装配场景：降低位置同步增益，提高力矩环响应速度，确保接触力恒定；
   跟随场景：提高位置同步增益，降低通信延迟，确保队形稳定；
   崎岖地形场景：提高滑移补偿增益，动态调整力矩分配比例，提升地形适应性。
   落地调试闭环：搭建测试环境，记录主从位置误差、力误差、力矩输出等数据，分析误差来源，针对性优化参数；例如案例4中，若力误差大，可调整力控PID的积分项，消除稳态误差；案例6中，若打滑频繁，可提高滑移补偿增益，加快力矩转移速度，形成“测试-记录-分析-优化”的闭环，直至系统稳定运行。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。