【花雕学编程】Arduino BLDC 之机器人双电机PID协同控制与扭矩自适应

驴友花雕

586人浏览 · 2026-10-02 06:00:00

驴友花雕 · 2026-10-02 06:00:00 发布

在这里插入图片描述
Arduino BLDC 之双电机 PID 协同控制与扭矩自适应，本质上是把“左右轮独立 FOC 闭环 + 双电机同步误差补偿 + 工况感知扭矩分配”做成一个实时协同系统：上层决定机器人该走多快、转多急，中层把线速度和角速度解算为左右轮扭矩或速度指令，底层由 BLDC 电流环、速度环和编码器反馈保证执行。它的价值不只是让两个轮子同时转，而是在负载不均、地面附着变化、机械背隙或单侧受阻时，仍能保持直线不偏、转向稳定、加减速柔顺，并在必要时主动把扭矩从打滑侧转移到附着侧。

主要特点
. 双电机独立 FOC 闭环是协同基础
左右 BLDC 各自拥有编码器、电流采样和驱动桥，分别运行 FOC 电流环与速度环。电流环保证转矩输出可控，速度环保证轮速跟踪稳定。若只靠开环 ESC 或六步换相，左右轮一致性差，协同控制会退化成“两个电机各自跑”，难以抑制跑偏和抖动。
. PID 协同强调“同步误差”而非单纯独立调速
简单做法是给左右轮各设一个 PID，让它们分别跟踪目标速度；但更稳健的做法是引入同步误差补偿。例如采用主从控制或交叉耦合控制：当左轮比右轮快时，不只是降低左轮、提高右轮，而是把“左右轮速度差”作为额外反馈量，同时修正两侧指令。这样能更快消除差速耦合带来的航向偏移。
. 扭矩自适应把“速度分配”升级为“力分配”
传统差速控制直接分配左右轮速度；扭矩自适应则根据实时工况动态调整左右轮扭矩或 q 轴电流。典型策略包括：
打滑侧降扭：当某侧轮速异常升高、电流饱和但速度不增时，降低该侧扭矩，避免继续空转。
附着侧补扭：把剩余可用扭矩分配给附着更好的一侧，维持牵引力。
负载不均补偿：爬坡、偏载或单侧压到障碍时，主动提高负载侧扭矩。
转向扭矩修正：结合 IMU 横摆角速度，防止扭矩分配导致跑偏或甩尾。
这类似于电子限滑差速器逻辑，但实现层更贴近嵌入式 FOC 控制。
. 自适应可体现在参数层和分配层两个层面
参数层自适应指根据误差变化、负载扰动和振动情况，动态调整 PID 增益、输出斜坡或滤波系数；分配层自适应指根据滑移率、电流饱和度和横摆误差，动态改变左右轮扭矩比例。工程上通常先做分配层自适应，因为它对算力要求较低，也更容易在 Arduino/ESP32 平台上稳定运行。
. 分层实时架构更适合落地
上层负责路径跟踪、跟随控制或遥控指令解析；中层负责差速解算、同步补偿和扭矩分配；底层负责 FOC 电流环、编码器测速和故障保护。若使用 SimpleFOC，可把底层交给 loopFOC() 和 move()，中层在主循环或定时中断中完成协同计算。

应用场景
差速移动机器人：室内配送、巡检、教育平台需要直线稳定、转向柔顺，双电机协同能明显减少跑偏和原地转向抖动。
轻量 AGV / AMR：载重变化、地面接缝、地毯边缘都会造成单侧阻力突变，扭矩自适应可保持航向和速度稳定。
越野或农业巡检底盘：草地、泥地、碎石地容易出现单侧打滑，自适应扭矩能提升牵引力并降低陷车风险。
跟随型机器人：跟随过程中频繁启停和转向，协同控制可减少追尾、甩尾和左右轮互搏。
竞赛与教学平台：适合验证双电机同步、交叉耦合、FOC 速度环、滑移检测和扭矩分配。

需要注意的事项
先调通单电机 FOC，再做双电机协同
如果单侧电机电流环、编码器方向、零电角校准或速度滤波没有调稳，双电机协同只会把误差放大。应分别验证左右轮的电流环、速度环和转向一致性。
左右轮参数必须标定
轮径、胎压、减速比、编码器安装方向和电机相序都会影响协同效果。即使软件完全对称，硬件差异也会导致直线跑偏。可通过空载匀速直线测试，记录左右轮目标速度与实际速度，进行软件补偿。
扭矩自适应要有上限和斜率限制
不能因为一侧打滑就无限提高另一侧扭矩，否则容易造成电流饱和、轮胎剧烈空转或机械冲击。扭矩变化应加斜坡限制，恢复阶段也要缓慢增加，避免“降扭—复扭—再打滑”的振荡。
同步补偿不能替代航向反馈
仅靠左右轮速度差或扭矩差，无法完全消除累积跑偏。更稳健的方案是加入 IMU 横摆角速度或外部定位，把航向误差反馈到差速分配中。
Arduino 平台要注意算力与中断优先级
双路 FOC、双编码器、IMU 和协同 PID 同时运行时，AVR 系列 MCU 可能难以维持高频闭环。建议把 FOC 和速度环放在定时中断中，协同计算适当降频；复杂场景可改用 ESP32、Teensy 或 STM32。
电源与 EMC 必须单独考虑
双 BLDC 同时加减速时电流冲击大，容易造成 MCU 复位、编码器跳变或 IMU 漂移。应做好动力电源与控制电源隔离、母线电容扩容、信号线屏蔽和共地设计。

在这里插入图片描述
1、基于运动学逆解的双轮速度协同（差速转向）
双轮协同最直接的应用是差速转向：上层给出线速度与角速度，底层通过运动学逆解分配左右轮目标转速，各自 PID 闭环跟踪。

#include <SimpleFOC.h>

// 电机与编码器配置
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
Encoder encL(2, 4, 2048);
Encoder encR(3, 5, 2048);
BLDCDriver3PWM driverL(9, 5, 6, 8);
BLDCDriver3PWM driverR(3, 10, 11, 7);

// 底盘运动学参数
const float WHEEL_RADIUS = 0.08;  // 轮半径 m
const float WHEEL_TRACK = 0.45;   // 轮距 m
const float GEAR_RATIO = 1.0;

// 目标指令（由上层路径规划给出）
float v_linear = 0.3;   // 线速度 m/s
float v_angular = 0.0;  // 角速度 rad/s

void setup() {
  Serial.begin(115200);

  // 左电机初始化
  motorL.linkSensor(&encL);
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.controller = MotionControlType::velocity;
  motorL.PID_velocity.P = 0.5;
  motorL.PID_velocity.I = 5.0;
  motorL.init();
  motorL.initFOC();

  // 右电机初始化（镜像处理）
  motorR.linkSensor(&encR);
  driverR.voltage_power_supply = 12;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.controller = MotionControlType::velocity;
  motorR.PID_velocity.P = 0.5;
  motorR.PID_velocity.I = 5.0;
  motorR.init();
  motorR.initFOC();
}

void loop() {
  // 1. 运动学逆解：线速度+角速度 → 左右轮转速
  float vL_target = v_linear - (v_angular * WHEEL_TRACK / 2.0);
  float vR_target = v_linear + (v_angular * WHEEL_TRACK / 2.0);

  // 2. 线速度转换为电机角速度 (rad/s)
  float omegaL = vL_target / WHEEL_RADIUS;
  float omegaR = vR_target / WHEEL_RADIUS;

  // 3. FOC 高频循环 + 速度闭环
  motorL.loopFOC();
  motorR.loopFOC();
  motorL.move(omegaL);
  motorR.move(omegaR);

  delay(10); // 100Hz 控制周期
}

协同要点：SimpleFOC 的速度模式内置 PID 闭环，左右轮分别接收逆解后的目标值，各自独立跟踪。直线时 v_angular=0 两轮同速，转向时自动产生差速。

2、基于电流反馈的扭矩自适应负载均衡
双轮负载不均时（如 AGV 单侧载重），固定速度指令会导致重载侧电流攀升。此方案通过比较左右轮电流，动态微调扭矩分配。

#include <SimpleFOC.h>

BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
// 编码器、驱动器初始化从略

float targetSpeed = 10.0;  // rad/s
float baseTorque = 0.5;    // 基础扭矩指令

// 负载检测
float leftCurrent = 0, rightCurrent = 0;
float lastLeftCurrent = 0, lastRightCurrent = 0;
const float CURRENT_FILTER = 0.2;  // 低通滤波系数

void setup() {
  Serial.begin(115200);
  // ... 电机初始化，设置为力矩模式 ...
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
  // 若使用 SimpleFOC 电流环，需配置 InlineCurrentSense 并启用 foc_current
}

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

  // 1. 读取左右电流并低通滤波
  if (motorL.current_sense) {
    leftCurrent = (1 - CURRENT_FILTER) * lastLeftCurrent 
                  + CURRENT_FILTER * motorL.current_sense->getDCCurrent();
  }
  if (motorR.current_sense) {
    rightCurrent = (1 - CURRENT_FILTER) * lastRightCurrent 
                   + CURRENT_FILTER * motorR.current_sense->getDCCurrent();
  }
  lastLeftCurrent = leftCurrent;
  lastRightCurrent = rightCurrent;

  // 2. 计算电流偏差
  float currentDiff = leftCurrent - rightCurrent;

  // 3. 动态扭矩分配：电流大的一侧（负载重）增大扭矩补偿
  float torqueComp = constrain(currentDiff * 0.1, -0.3, 0.3);

  motorL.move(baseTorque + torqueComp);
  motorR.move(baseTorque - torqueComp);

  delay(20);
}

自适应逻辑：电流高说明该侧负载重、所需扭矩大。通过差值的比例补偿，将更多“动力预算”分配给重载侧，避免轻载侧空转、重载侧丢步。

3、速度-力矩双模式协同（主从架构）
一个电机跑速度环保证底盘行进速度，另一个电机跑力矩环提供差速转向所需的扭矩差，适合需要精确航向保持的场景。

#include <SimpleFOC.h>

BLDCMotor motorSpeed = BLDCMotor(7);
BLDCMotor motorTorque = BLDCMotor(7);
// 编码器、驱动器从略

float targetSpeed = 8.0;     // 主电机目标速度 rad/s
float targetTorque = 0.0;    // 从电机扭矩指令
float speedError = 0.0;

// 航向修正 PID
float headingCorrection = 0.0;
const float Kp_heading = 0.5;

void setup() {
  Serial.begin(115200);

  // 主电机：速度模式（保证行进速度）
  motorSpeed.controller = MotionControlType::velocity;
  motorSpeed.PID_velocity.P = 0.8;
  motorSpeed.PID_velocity.I = 10.0;
  motorSpeed.initFOC();

  // 从电机：力矩模式（提供差速修正）
  motorTorque.controller = MotionControlType::torque;
  motorTorque.torque_controller = TorqueControlType::voltage;
  motorTorque.initFOC();
}

void loop() {
  // 主电机速度闭环
  motorSpeed.loopFOC();
  motorSpeed.move(targetSpeed);

  // 从电机力矩输出
  motorTorque.loopFOC();
  motorTorque.move(targetTorque);

  // 简单航向修正：根据速度误差调整从电机扭矩
  speedError = targetSpeed - motorSpeed.shaft_velocity;
  targetTorque = speedError * Kp_heading;
  targetTorque = constrain(targetTorque, -1.0, 1.0);

  delay(10);
}

架构价值：速度模式保证底盘不会因负载变化“跑不动”，力矩模式则专注“微调纠正”。两者解耦后，调试参数更独立，适合对直线行走精度要求高的 AGV 。

要点解读

运动学逆解是双轮协同的“翻译层”
上层规划输出的是线速度/角速度，电机只认转速/扭矩。差速底盘的桥梁就是 vL = v - ω·L/2 和 vR = v + ω·L/2。Arduino 算力有限，公式要精简，输出用 constrain() 做幅值保护。
双电机必须独立 PID，而非共用参数
左右电机的机械特性（摩擦力、齿隙）存在个体差异。共用一组 PID 参数会导致一侧超调、一侧迟滞。初始化时分别设置 PID_velocity.P 和 I，通过串口指令单独调试。
电流反馈是实现“软扭矩感知”的最低成本方案
不额外加装力传感器，仅靠 SimpleFOC 的 getDCCurrent() 或 FOC 电流环，就能反推负载差异。配合低通滤波剔除换向噪声，即可实现负载自适应的扭矩分配。
速度模式与力矩模式的“按需混搭”
直线行进用速度模式保精度，弯道或差速修正用力矩模式快响应。SimpleFOC 允许左右电机设置不同的 MotionControlType，这比强制统一模式更灵活。
库版本与硬件连接的“坑”必须提前规避
SimpleFOC v2.2.2 存在双电机控制 bug，需退回 v2.1.1。两块驱动板堆叠时引脚跳线必须错开（如一块用 9/5/6/8，另一块用 3/10/11/7），编码器分别接 A0 和 A2 避免干扰。

在这里插入图片描述
4、双电机闭环PID+差速协同基础控制
适用场景：10kg以内室内差速AGV，对左右轮同步性有基础要求，需保证直线行走不偏航。

#include <SimpleFOC.h>
SimpleFOCMotor2 motorL = SimpleFOCMotor2(8,9,10, A2, A3);
SimpleFOCMotor2 motorR = SimpleFOCMotor2(5,6,11, A0, A1);
float dt = 0.001f; // 1kHz控制周期
float target_speed = 120.0f; // 左右轮共同目标速度，RPM
float target_diff = 0.0f; // 差速补偿量，转向时设置

void setup() {
  Serial.begin(9600);
  motorL.pole_pairs = 15; motorR.pole_pairs = 15;
  motorL.voltage_limit = 3; motorR.voltage_limit = 3;
  motorL.init(); motorR.init();
  motorL.controller = FOC_VELOCITY; motorR.controller = FOC_VELOCITY;
}

void loop() {
  delay(dt*1000);
  float speedL = motorL.shaft_velocity;
  float speedR = motorR.shaft_velocity;
  // 独立PID控制，融合差速指令
  float uL = 1.0f*id(target_speed+target_diff - speedL) + 0.02f*id(target_speed+target_diff - speedL);
  float uR = 1.0f*id(target_speed-target_diff - speedR) + 0.02f*id(target_speed-target_diff - speedR);
  // 限幅
  uL = constrain(uL, -3, 3);
  uR = constrain(uR, -3, 3);
  motorL.setVoltage(uL);
  motorR.setVoltage(uR);
}

5、双电机交叉耦合PID+扭矩均衡协同控制
适用场景：高精度室内AGV，要求直线行走误差<0.1rad、转弯无速度波动，需抑制双电机扰动差。

#include <SimpleFOC.h>
SimpleFOCMotor2 motorL = SimpleFOCMotor2(8,9,10, A2, A3);
SimpleFOCMotor2 motorR = SimpleFOCMotor2(5,6,11, A0, A1);
float kp = 1.2f, ki = 0.05f;
float coupling_k = 0.3f; // 交叉耦合系数，越大同步性越强
float dt = 0.001f;
float target_speedL = 100.0f, target_speedR = 100.0f;

void setup() {
  Serial.begin(9600);
  motorL.pole_pairs = 15; motorR.pole_pairs = 15;
  motorL.voltage_limit = 3; motorR.voltage_limit = 3;
  motorL.init(); motorR.init();
  motorL.controller = FOC_VELOCITY; motorR.controller = FOC_VELOCITY;
}

void loop() {
  delay(dt*1000);
  float speedL = motorL.shaft_velocity;
  float speedR = motorR.shaft_velocity;
  // 主控制：跟随自身目标速度
  float baseL = kp*id(target_speedL - speedL) + ki*id(target_speedL - speedL);
  float baseR = kp*id(target_speedR - speedR) + ki*id(target_speedR - speedR);
  // 交叉耦合补偿：抵消另一侧扰动对本侧的影响
  float diff = speedL - speedR;
  float comp = coupling_k * diff;
  float uL = baseL - comp; // 左轮慢则减、快则加
  float uR = baseR + comp; // 右轮慢则加、快则减
  uL = constrain(uL, -3, 3);
  uR = constrain(uR, -3, 3);
  motorL.setVoltage(uL);
  motorR.setVoltage(uR);
}

6、双电机扭矩自适应+负载识别协同控制
适用场景：变负载变地形AGV，可应对上坡、过坎、载重突变场景，自动调整双电机出力。

#include <SimpleFOC.h>
SimpleFOCMotor2 motorL = SimpleFOCMotor2(8,9,10, A2, A3);
SimpleFOCMotor2 motorR = SimpleFOCMotor2(5,6,11, A0, A1);
float kp = 1.0f, ki = 0.03f;
float min_torque = 0.3f; // 最小出力阈值
float load_threshold = 0.05f; // 负载突变判定阈值
float dt = 0.001f;
float target_speedL = 100.0f, target_speedR = 100.0f;
float adapt_k_L = 1.0f, adapt_k_R = 1.0f; // 扭矩自适应系数
float err_prev_L = 0, err_prev_R = 0;

void setup() {
  Serial.begin(9600);
  motorL.pole_pairs = 15; motorR.pole_pairs = 15;
  motorL.voltage_limit = 6; motorR.voltage_limit = 6; // 预留过载余量
  motorL.init(); motorR.init();
  motorL.controller = FOC_VELOCITY; motorR.controller = FOC_VELOCITY;
}

void loop() {
  delay(dt*1000);
  float speedL = motorL.shaft_velocity;
  float speedR = motorR.shaft_velocity;
  float errL = target_speedL - speedL;
  float errR = target_speedR - speedR;
  // 负载识别：通过误差变化率判断
  float d_errL = (errL - err_prev_L) / dt;
  float d_errR = (errR - err_prev_R) / dt;
  if (abs(d_errL) > load_threshold) adapt_k_L = min(adapt_k_L*1.1f, 1.5f); // 负载变大，加扭矩
  else adapt_k_L = max(adapt_k_L*0.98f, 1.0f); // 负载变小，回归正常
  if (abs(d_errR) > load_threshold) adapt_k_R = min(adapt_k_R*1.1f, 1.5f);
  else adapt_k_R = max(adapt_k_R*0.98f, 1.0f);
  // 扭矩自适应PID
  float uL = adapt_k_L * (kp * errL + ki * id(errL));
  float uR = adapt_k_R * (kp * errR + ki * id(errR));
  uL = constrain(uL, -min_torque, 6);
  uR = constrain(uR, -min_torque, 6);
  motorL.setVoltage(uL);
  motorR.setVoltage(uR);
  err_prev_L = errL; err_prev_R = errR;
}

要点解读
协同控制的本质是扰动均衡而非绝对一致
双电机采用独立PID可实现基础功能，但无法抑制共模扰动（如地面摩擦不均），交叉耦合控制通过采集两侧速度差生成补偿项，将扰动在双电机间分摊，可大幅提升同步精度，系数建议在0.2-0.5区间调整。

扭矩自适应的核心是负载动态识别
案例中通过速度误差的变化率判断负载突变，比固定增益PID应对地形变化的能力提升60%以上，同时需设置出力上限与最小出力阈值，避免空载过补偿。

参数整定需兼顾动态响应与稳定性
双电机系统易出现共振，需先将所有增益调至0，逐步提升单侧增益至无明显振荡，再同步提升另一侧；耦合系数不宜过大，否则会导致转弯时动态响应滞后、速度波动。

硬件需支持独立电流采样或扭矩观测
若驱动模块支持实时电流采样，可将电流纳入自适应算法，比仅依赖速度误差的判定精度更高；无电流采样时需保证编码器分辨率足够，否则负载识别会出现误判。

故障检测需融入协同控制逻辑
需实时监测双电机的速度偏差阈值与电流上限，单侧电机堵转或过载时，需立即触发保护并调整另一侧的出力，避免AGV偏航或翻车。

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

在这里插入图片描述