基于 Arduino 平台的 BLDC（无刷直流电机）机器人“自适应负载平衡控制系统”，本质上是一套“基于扰动观测的实时扭矩分配 + 动态参数整定”的嵌入式机电一体化解决方案。它旨在解决机器人面对未知或时变负载时，因惯量、重心偏移导致的运动失稳、效率下降及机械磨损问题。

一、核心概念与主要特点

1. 负载自适应的双重含义
   负载扭矩自适应：通过 FOC（磁场定向控制）的电流环（Iq）实时感知外部阻力矩，自动调整输出扭矩以维持设定速度或加速度，避免“遇阻即停”或“空载振荡”。
   质量-惯量自适应：系统在线估计负载质量及重心位置，动态调整运动控制参数（如 PID 增益、加加速度 Jerk 限制），防止重载时响应迟钝或轻载时超调震荡。
2. 基于扰动观测器（DOB）的力控内核
   这是系统的“神经感知”层。在 Arduino 端通常实现简化版的 Luenberger 观测器​ 或 PI 型扰动观测器：
   原理：利用电机电流（Iq）与编码器加速度（dv/dt）的动力学关系，反推外部负载扰动（F_load）。
   优势：无需昂贵的力传感器，仅凭 BLDC 本身的电流和编码器数据，即可实现“软性力感知”，成本极低且响应快。
3. 分层模糊逻辑与动态分配
   参数自整定：利用模糊规则（Fuzzy Logic）根据观测到的负载大小，在线修正 PID 增益。例如：负载大 → 增大 Kp（刚度）、减小 Kd（阻尼）；负载轻 → 减小 Kp 防振荡。
   多轴协调：在多电机（如四轮、六足）机器人中，根据各关节/车轮的负载差异，动态分配扭矩。例如：爬坡时重心后移，则增大后轮扭矩分配系数，防止前轮打滑。
4. 嵌入式优化实现
   针对 Arduino 有限的算力（如 AVR 或 ESP32），系统通常采用增量式更新与查表法（LUT）：
   避免复杂的矩阵运算，扰动观测器采用一阶简化模型。
   模糊规则表预存储在 Flash 中，运行时直接查表输出修正量，而非在线解模糊运算。

二、典型应用场景

1. 物流搬运 AGV/AMR
   场景痛点：搬运箱体重物从 1kg 到 50kg 变化，导致机器人加速时“翘头”或刹车时“点头”。
   自适应作用：系统观测到负载增大后，自动降低加速度 Jerk 值，并调整前后轮扭矩分配，保持车身俯仰稳定，保护货物安全。
2. 机械臂或关节型机器人
   场景痛点：末端执行器抓取不同质量的工件，导致关节惯量剧变，传统固定 PID 参数下，运动轨迹易产生跟踪误差或机械共振。
   自适应作用：基于电流观测的负载力矩前馈补偿，配合模糊 PID，实现“抓取重物时加大电流推力，空载时减小增益防抖”。
3. 爬坡/越障巡检机器人
   场景痛点：在斜坡或崎岖路面，各轮接地压力不均，固定分配策略易导致低压力轮空转打滑。
   自适应作用：通过电流反馈识别打滑轮，切断其动力并重新分配扭矩至有抓地力的轮子，实现“电子差速锁”功能。
4. 长期运行的能耗敏感型机器人
   场景痛点：电池供电的家用/服务机器人，空载时若仍按重载参数运行，会浪费电能并产生噪音。
   自适应作用：轻载时自动进入“低功耗模式”，降低电流环给定，减小铁损与铜损，显著延长续航。

三、工程实现注意事项（落地关键）

1. 硬件选型与采样精度（不可妥协）
   编码器分辨率：负载观测依赖精确的加速度计算（dv/dt），低分辨率编码器（如 < 1000 PPR）在低速时观测噪声极大。建议使用 AS5600 等磁性编码器或高线数光电编码器。
   电流采样：必须使用高精度低阻采样电阻（如 0.01Ω）配合高速运放。电流采样噪声会直接污染扰动观测器，导致扭矩控制振荡。
   MCU 选型：AVR (Arduino Uno) 仅适合单电机简单观测；多电机或复杂模糊逻辑强烈推荐 ESP32 或 STM32（如 SimpleFOC 官方推荐平台）。
2. 观测器设计的鲁棒性
   模型不确定性：Arduino 难以运行复杂模型。建议采用一阶惯性+滞后的简化电机模型，并通过实验数据拟合时间常数。
   噪声滤波：必须对编码器速度进行低通滤波（如 2阶 Butterworth），但截止频率需高于系统带宽，否则会引入相位滞后，导致观测延迟。
3. 安全与防振荡机制
   输出限幅与抗饱和：自适应 PID 的输出必须严格限幅，防止积分饱和（Integral Windup）导致系统失控。必须实现 Clamping 或 Back-calculation 抗饱和逻辑。
   突变保护：当检测到负载瞬间突变（如碰撞）时，应暂时冻结自适应算法，切换到预设的“安全保守参数”，避免算法在瞬态过程中引入不稳定。
4. 标定与初始化
   空载标定：系统上电后，必须先执行“空载自学习”流程，记录空载电流和转动惯量，作为后续负载观测的基准（Baseline）。
   参数边界：模糊逻辑输出的参数修正量必须有明确的上下界，防止学习过程中参数漂移到危险值（如 Kp 过大导致系统震荡）。
5. 调试与可视化
   串口波形调试：利用 Arduino IDE 的 Serial Plotter 或第三方工具，实时打印“观测负载 vs 实际负载”、“自适应 Kp/Ki”等曲线，这是调参的关键依据。
   状态机设计：将自适应过程分为“观测->决策->执行->验证”四个状态，便于在出现异常时定位故障环节。

1、基于电流反馈的差速动力自适应分配
场景描述：
这是最经典的“电子差速锁”逻辑。当双轮差速机器人在非铺装路面行驶，左侧轮子陷入泥坑（阻力大、转速慢），右侧轮子在平地（阻力小、转速快）。系统通过监测电流（代表负载）和转速，自动将更多扭矩分配给阻力大的一侧，防止机器人原地打转。
核心逻辑：
监测： 实时读取左右电机的相电流（motor.current_sp）和转速。
判断： 计算左右轮的速度差和负载差。
自适应分配： 引入“负载补偿系数”，动态调整左右轮的目标扭矩。

#include <SimpleFOC.h>

// --- 硬件定义 ---
BLDCMotor motor_left(7);
BLDCMotor motor_right(7);
// ... 驱动器与传感器初始化 (省略) ...

// --- 控制参数 ---
float base_torque = 0.5;      // 基础巡航扭矩
float K_load = 0.1;           // 负载补偿增益
float max_torque_limit = 3.0; // 最大扭矩限制

void setup() {
  // 初始化为扭矩模式，因为我们要直接控制力
  motor_left.controller = MotionControlType::torque;
  motor_right.controller = MotionControlType::torque;
  motor_left.init(); motor_left.initFOC();
  motor_right.init(); motor_right.initFOC();
  Serial.begin(115200);
}

void loop() {
  motor_left.loopFOC();
  motor_right.loopFOC();

  // 1. 获取传感器数据
  float i_left = motor_left.current_sp; // 左侧电流 (代表负载)
  float i_right = motor_right.current_sp; // 右侧电流
  float v_left = motor_left.shaft_velocity;
  float v_right = motor_right.shaft_velocity;

  // 2. 自适应负载平衡算法
  // 假设左侧负载大 (电流高)，我们需要给左侧更多扭矩
  // 计算负载差值
  float load_diff = i_left - i_right;
  
  // 计算补偿扭矩：负载大的一侧增加扭矩
  float compensation = load_diff * K_load;

  // 3. 矢量分配
  float target_left = base_torque + compensation;
  float target_right = base_torque - compensation;

  // 4. 安全限制
  target_left = constrain(target_left, 0, max_torque_limit);
  target_right = constrain(target_right, 0, max_torque_limit);

  // 5. 输出
  motor_left.move(target_left);
  motor_right.move(target_right);

  delay(20);
}

2、基于 IMU 姿态反馈的动态重心补偿
场景描述：
机器人搬运货物时，如果货物重心偏向右侧，或者机器人在斜坡上行驶，左右轮的垂直压力（正压力）不同。摩擦力与正压力成正比，压力小的一侧容易打滑。本案例利用 IMU 检测车身倾斜角（Roll），动态调整扭矩，模拟“主动悬挂”的效果。
核心逻辑：
感知： 读取 IMU 的 Roll 角。
物理模型： Roll 角向右倾斜 -> 右轮压力大（抓地力好），左轮压力小（易打滑）。
策略： 为了防止向左侧翻或打滑，增加右侧（低侧）的驱动扭矩，减小左侧（高侧）的扭矩。

#include <SimpleFOC.h>
#include <MPU6050.h> // 需安装 MPU6050 库

BLDCMotor motor_left(7);
BLDCMotor motor_right(7);
MPU6050 imu;

float K_roll = 0.02; // 姿态补偿系数
float base_torque = 0.5;

void setup() {
  motor_left.controller = MotionControlType::torque;
  motor_right.controller = MotionControlType::torque;
  motor_left.init(); motor_left.initFOC();
  motor_right.init(); motor_right.initFOC();
  
  Wire.begin();
  imu.initialize();
  Serial.begin(115200);
}

void loop() {
  motor_left.loopFOC();
  motor_right.loopFOC();

  // 1. 读取姿态
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  // 计算 Roll 角 (弧度)
  float roll_angle = atan2(ay, az); 

  float torque_left = base_torque;
  float torque_right = base_torque;

  // 2. 动态重心补偿
  // 如果 roll > 0 (向右倾)，右轮受压。
  // 策略：增加右侧扭矩以利用抓地力，减小左侧防止翘起
  float compensation = roll_angle * K_roll;

  torque_right += compensation;
  torque_left -= compensation;

  // 3. 输出
  motor_left.move(torque_left);
  motor_right.move(torque_right);

  delay(20);
}

3、基于系统负载的“算力-动力”分级调度
场景描述：
这是一个系统级的“负载平衡”。当 Arduino/ESP32 检测到系统资源紧张（如内存不足、CPU 占用过高，或者电池电压骤降）时，为了保证核心安全（不失控），主动降低电机的控制性能（如降低 FOC 频率或限制最大扭矩）。
核心逻辑：
监测： 模拟监测系统负载（如通过 ESP.getFreeHeap() 或 电池电压）。
分级策略：
绿色状态（正常）： 全功率，高频 FOC。
黄色状态（高负载）： 限制最大扭矩，降低非关键任务频率。
红色状态（危急）： 仅维持基本平衡，强制减速。

#include <SimpleFOC.h>

BLDCMotor motor(7);
// ... 驱动器初始化 ...

// 系统状态定义
enum SystemState { STATE_GREEN, STATE_YELLOW, STATE_RED };
SystemState currentState = STATE_GREEN;

// 动态参数
float current_limit_dynamic = 2.0;
unsigned long control_delay = 10; // 控制周期 (ms)

void setup() {
  motor.controller = MotionControlType::torque;
  motor.init();
  motor.initFOC();
  Serial.begin(115200);
}

void checkSystemLoad() {
  // 模拟读取系统负载 (实际可用 ESP.getFreeHeap() 或 电池电压)
  int freeMemory = 50000; // 假设值
  
  // 简单的状态机逻辑
  if (freeMemory > 40000) {
    currentState = STATE_GREEN;
  } else if (freeMemory > 20000) {
    currentState = STATE_YELLOW;
  } else {
    currentState = STATE_RED;
  }
}

void loop() {
  // 1. 检查系统健康度
  checkSystemLoad();

  // 2. 根据负载调整控制策略
  switch (currentState) {
    case STATE_GREEN:
      // 全性能模式
      current_limit_dynamic = 5.0;
      control_delay = 10; 
      break;
      
    case STATE_YELLOW:
      // 节能/限扭模式
      current_limit_dynamic = 3.0;
      control_delay = 20; // 稍微降低控制频率以释放 CPU
      break;
      
    case STATE_RED:
      // 安全降级模式
      current_limit_dynamic = 1.0; // 强制限制扭矩
      control_delay = 50; // 大幅降低频率
      Serial.println("⚠️ 系统过载，进入安全模式！");
      break;
  }

  // 应用动态限制
  motor.current_limit = current_limit_dynamic;
  
  // 执行 FOC (注意：SimpleFOC 的 loopFOC 通常需要在 loop 中尽可能高频调用，
  // 这里为了演示逻辑简化了延时，实际建议用 millis() 非阻塞计时)
  motor.loopFOC();
  motor.move(0.5); // 基础扭矩

  delay(control_delay);
}

要点解读
扭矩模式是自适应的基础
要实现负载平衡，必须使用 MotionControlType::torque（或电压/电流模式）。速度模式会试图通过不断调整电压来维持速度，这会掩盖负载的变化，导致控制器无法感知哪一侧轮子打滑或受力。
传感器融合决定控制上限
案例一仅靠电流（间接反映负载），案例二引入了 IMU（直接反映姿态）。更高级的系统会结合轮速计、电流计、IMU 甚至视觉数据。数据越丰富，对“负载”的估计就越准确，平衡效果就越好。
正压力与摩擦力的物理关系
在案例二中，核心逻辑基于物理公式 F_{friction} = \mu \cdot NF friction =μ⋅N 。当车身倾斜，低侧轮子的正压力 NN 增大，摩擦力上限提高。此时增加低侧扭矩，既不会打滑，又能提供更大的推进力，这是物理层面的“顺势而为”。
系统级负载平衡（看门狗思维）
案例三展示了一种容易被忽视的“软负载平衡”。在嵌入式系统中，当计算资源或电源资源不足时，主动降级（Degradation）比崩溃要好得多。这种“算力-动力”的耦合设计是高级机器人系统的特征。
防止“正反馈”震荡
在编写案例一（差速补偿）时，补偿系数 K_load 不能设置过大。如果补偿过猛，会导致左右轮扭矩反复剧烈跳变，产生震荡。建议配合低通滤波使用传感器数据，并从小增益开始调试。

1、货物搬运机器人的动态负载自适应平衡系统
场景：仓库搬运机器人承载不同重量的货物（0-50kg）时，实时检测货物重量，动态调整双轮输出扭矩，确保货物重心偏移或重量变化时，机器人平稳行驶不打滑、不侧翻，重点解决“负载突变导致的驱动失衡”问题。

#include <SimpleFOC.h>
#include <HX711_WE.h>  // 称重传感器库
#include <PID_v1.h>

// 硬件配置
BLDCMotor leftMotor = BLDCMotor(7);
BLDCMotor rightMotor = BLDCMotor(8);
HX711_WE loadCell(A3, A4);  // 称重传感器（连接到A3、A4）
float maxPayload = 50.0f;     // 最大负载50kg

// 负载平衡控制参数
float baseTorque = 0.4f;          // 空载基础扭矩
float torquePerKg = 0.01f;        // 每千克负载增加的扭矩
float loadCellOffset = -2000;     // 称重传感器零点偏移（校准值，需根据实际情况调整）
PID loadBalancePID;               // 负载平衡PID（用于校正扭矩偏差）
float loadError, loadOutput;
float leftCurrent, rightCurrent;

// 称重传感器校准与重量检测
float getPayloadWeight() {
  loadCell.tare();  // 去皮（需在机器人空载时调用，实际部署可注释，仅保留偏移校准）
  long rawValue = loadCell.read();
  float weight = (rawValue - loadCellOffset) / 1000.0f;  // 转换为千克
  return constrain(weight, 0.0f, maxPayload);  // 限制在0-50kg
}

// 扭矩自适应分配核心函数
void adaptiveTorqueAllocation(float payload) {
  // 1. 计算总负载扭矩：基础扭矩 + 负载补偿扭矩
  float totalTorque = baseTorque + payload * torquePerKg;
  totalTorque = constrain(totalTorque, 0.3f, 1.5f);  // 限制扭矩范围，防止过载

  // 2. 读取左右电机电流（判断负载是否平衡）
  leftCurrent = leftMotor.driver->getCurrent();
  rightCurrent = rightMotor.driver->getCurrent();
  loadError = leftCurrent - rightCurrent;  // 电流差反映负载偏差（电流大的一侧负载重）

  // 3. PID校正：通过电流偏差动态调整扭矩，实现平衡
  loadBalancePID.Compute();
  totalTorque += loadOutput;  // 叠加校正扭矩

  // 4. 分配扭矩：负载重的一侧扭矩略大（通过电流差调整分配比例）
  if (loadError > 0) {
    // 左侧电流大（左侧负载重），左侧扭矩略高
    leftMotor.move(totalTorque * 1.02f);
    rightMotor.move(totalTorque * 0.98f);
  } else {
    // 右侧电流大（右侧负载重），右侧扭矩略高
    leftMotor.move(totalTorque * 0.98f);
    rightMotor.move(totalTorque * 1.02f);
  }
}

void setup() {
  Serial.begin(115200);
  leftMotor.init();
  rightMotor.init();
  loadCell.begin(128);  // 称重传感器增益设置（128倍，根据传感器规格调整）
  loadCell.tare();      // 初始去皮校准
  loadBalancePID.SetMode(AUTOMATIC);
  loadBalancePID.SetTunings(0.05, 0.01, 0.005);  // PID参数：比例、积分、微分
  loadBalancePID.SetSetpoint(0);  // 目标：左右电机电流差为0（负载平衡）
}

void loop() {
  float payload = getPayloadWeight();
  Serial.print("Payload: "); Serial.print(payload); Serial.print(" kg");
  adaptiveTorqueAllocation(payload);

  // 串口打印调试信息
  Serial.print(" | Left Current: "); Serial.print(leftCurrent);
  Serial.print(" | Right Current: "); Serial.print(rightCurrent);
  Serial.print(" | Torque Correction: "); Serial.println(loadOutput);
  delay(100);
}

5、双机械臂协作搬运机器人的负载动态均衡系统
场景：双机械臂机器人协作搬运异形/偏心货物（如带侧边把手的箱体，重心偏移）时，通过机械臂末端力传感器检测负载差异，动态调整左右电机扭矩，补偿重心偏移导致的不平衡力矩，确保机器人行驶时不因重心偏移打滑或侧翻。

#include <SimpleFOC.h>
#include <ForceSensor.h>  // 力传感器抽象库（实际需按传感器型号选择，如FSR、Load Cell等）
#include <PID_v1.h>

// 硬件配置
BLDCMotor leftMotor = BLDCMotor(7);
BLDCMotor rightMotor = BLDCMotor(8);
ForceSensor leftForceSensor(A1);  // 左机械臂力传感器
ForceSensor rightForceSensor(A2); // 右机械臂力传感器

// 负载均衡参数
float baseTorque = 0.5f;
float maxForceDiff = 50.0f;  // 最大允许力差（N，根据机械臂负载能力设定）
float torqueCompensation = 0.005f;  // 每牛力差对应的扭矩补偿系数
PID forceBalancePID;
float forceError, forceOutput;

// 力传感器读取与负载差异检测
float getForceDifference() {
  float leftForce = leftForceSensor.read();
  float rightForce = rightForceSensor.read();
  return leftForce - rightForce;  // 正为左力大，负为右力大
}

// 动态负载均衡控制核心
void dynamicLoadBalancing() {
  float forceDiff = getForceDifference();  // 获取左右机械臂力差
  forceError = forceDiff;  // 力差作为PID误差（目标力差为0）

  // PID计算扭矩补偿量
  forceBalancePID.Compute();

  // 计算总扭矩：基础扭矩 + 力差补偿扭矩
  float totalTorque = baseTorque + forceOutput * torqueCompensation;
  totalTorque = constrain(totalTorque, 0.3f, 1.5f);

  // 扭矩矢量分配：力大的一侧扭矩适当增加，补偿不平衡力矩
  // 核心逻辑：根据力差调整扭矩分配比例，力差越大，分配差异越明显
  float leftTorque, rightTorque;
  if (forceDiff > 0) {
    // 左侧力大（重心偏左），左侧扭矩增加，右侧减少
    leftTorque = totalTorque * (1 + forceOutput * 0.01f);
    rightTorque = totalTorque * (1 - forceOutput * 0.01f);
  } else {
    // 右侧力大（重心偏右），右侧扭矩增加，左侧减少
    leftTorque = totalTorque * (1 - forceOutput * 0.01f);
    rightTorque = totalTorque * (1 + forceOutput * 0.01f);
  }

  // 限制扭矩范围，防止过载
  leftTorque = constrain(leftTorque, 0.2f, 1.6f);
  rightTorque = constrain(rightTorque, 0.2f, 1.6f);

  // 输出扭矩到电机
  leftMotor.move(leftTorque);
  rightMotor.move(rightTorque);

  // 串口调试
  Serial.print("Force Diff: "); Serial.print(forceDiff);
  Serial.print(" | Compensation: "); Serial.print(forceOutput);
  Serial.print(" | Left Torque: "); Serial.print(leftTorque);
  Serial.print(" | Right Torque: "); Serial.println(rightTorque);
}

void setup() {
  Serial.begin(115200);
  leftMotor.init();
  rightMotor.init();
  leftForceSensor.init();
  rightForceSensor.init();

  forceBalancePID.SetMode(AUTOMATIC);
  forceBalancePID.SetTunings(0.1f, 0.02f, 0.01f);
  forceBalancePID.SetSetpoint(0);
}

void loop() {
  dynamicLoadBalancing();
  delay(50);
}

6、崎岖地形越障的负载自适应平衡与牵引力控制系统
场景：机器人在崎岖地形（斜坡、坑洼、岩石）行驶时，车轮因地形起伏导致接触压力不均（如单侧轮悬空或负载骤增），通过轮速差+电流变化检测负载突变，动态调整左右轮扭矩，确保每个车轮都有足够的牵引力，防止打滑或越障失败，重点解决“地形导致的负载不均”问题。

#include <SimpleFOC.h>
#include <HallSensor.h>
#include <PID_v1.h>

// 硬件配置
BLDCMotor leftMotor = BLDCMotor(7);
BLDCMotor rightMotor = BLDCMotor(8);
HallSensor leftEnc(2, 3);  // 左轮编码器
HallSensor rightEnc(4, 5); // 右轮编码器

// 地形负载自适应参数
float baseSpeed = 0.6f;          // 目标基础速度
float slipThreshold = 0.2f;       // 滑移率阈值（超过判定为打滑/负载突变）
float torqueBoostFactor = 1.5f;   // 打滑时扭矩提升系数
PID terrainBalancePID;            // 地形负载平衡PID
float speedError, speedOutput;
float leftSpeed, rightSpeed;
float leftCurrent, rightCurrent;

// 滑移率计算：反映车轮与地面的附着效率
float calculateSlipRatio(float actualSpeed, float targetSpeed) {
  if (targetSpeed == 0) return 0.0f;
  return (targetSpeed - actualSpeed) / targetSpeed;
}

// 地形负载自适应控制核心
void terrainAdaptiveLoadControl() {
  // 1. 读取轮速和电流
  leftSpeed = leftEnc.getVelocity();
  rightSpeed = rightEnc.getVelocity();
  leftCurrent = leftMotor.driver->getCurrent();
  rightCurrent = rightMotor.driver->getCurrent();

  // 2. 计算左右轮滑移率
  float leftSlip = calculateSlipRatio(leftSpeed, baseSpeed);
  float rightSlip = calculateSlipRatio(rightSpeed, baseSpeed);

  // 3. 负载突变检测：滑移率>阈值或电流差>阈值
  bool leftLoadMutation = (leftSlip > slipThreshold) || (leftCurrent > rightCurrent * 1.2f);
  bool rightLoadMutation = (rightSlip > slipThreshold) || (rightCurrent > leftCurrent * 1.2f);

  // 4. 动态扭矩分配策略
  float leftTorque = baseSpeed;
  float rightTorque = baseSpeed;

  if (leftLoadMutation) {
    // 左侧负载突变（打滑或遇障碍）：提升扭矩，补偿牵引力
    leftTorque = baseSpeed * torqueBoostFactor;
    speedError = leftSlip - slipThreshold;  // 滑移率误差作为PID输入
    terrainBalancePID.Compute();
    leftTorque += speedOutput;  // 叠加PID校正量
  }

  if (rightLoadMutation) {
    // 右侧负载突变：提升扭矩
    rightTorque = baseSpeed * torqueBoostFactor;
    speedError = rightSlip - slipThreshold;
    terrainBalancePID.Compute();
    rightTorque += speedOutput;
  }

  // 5. 扭矩平滑处理：避免频繁突变
  leftTorque = constrain(leftTorque, 0.4f, 1.8f);
  rightTorque = constrain(rightTorque, 0.4f, 1.8f);

  // 6. 输出扭矩
  leftMotor.move(leftTorque);
  rightMotor.move(rightTorque);

  // 串口调试
  Serial.print("Left Speed: "); Serial.print(leftSpeed);
  Serial.print(" | Right Speed: "); Serial.print(rightSpeed);
  Serial.print(" | Left Torque: "); Serial.print(leftTorque);
  Serial.print(" | Right Torque: "); Serial.println(rightTorque);
}

void setup() {
  Serial.begin(115200);
  leftMotor.init();
  rightMotor.init();
  leftEnc.init();
  rightEnc.init();

  terrainBalancePID.SetMode(AUTOMATIC);
  terrainBalancePID.SetTunings(0.08f, 0.01f, 0.005f);
  terrainBalancePID.SetSetpoint(0);
}

void loop() {
  terrainAdaptiveLoadControl();
  delay(20);
}

要点解读

1. 负载状态精准感知：多源传感器融合实现负载动态检测
   核心逻辑：自适应负载平衡的前提是实时、准确感知负载状态，需融合重量传感器、力传感器、电机电流、轮速/编码器等多源数据，通过数据互补消除单一传感器的局限性。
   感知维度与传感器选择：
   重量感知：称重传感器（案例4），直接检测货物重量，适用于静态/准静态负载；
   力差异感知：末端力传感器（案例5），检测机械臂协作时的负载差异，适用于偏心/异形货物；
   动态负载感知：电机电流+轮速（案例6），电流反映电机负载（扭矩需求），轮速反映打滑/悬空，间接检测动态地形导致的负载突变。
   数据融合方式：通过阈值判断、偏差计算将多源数据统一为“负载偏差信号”，如案例4用“电流差”反映负载不平衡，案例6用“滑移率+电流差”判定地形负载突变，避免单一传感器受环境干扰（如轮速受地面摩擦影响，结合电流可准确判断是否打滑）。
   价值：精准的负载感知是平衡控制的基础，多源融合解决了“静态重量检测”与“动态负载突变”的双重需求，避免因感知误差导致的控制失效。
2. 扭矩自适应分配策略：负载-扭矩的动态映射与比例调节
   核心逻辑：根据负载状态建立“负载大小-扭矩需求”“负载偏差-扭矩分配”的动态映射关系，实现扭矩随负载实时调整，核心是基础扭矩+补偿扭矩的双段式分配模型。
   分配模型拆解：
   基础扭矩：空载/轻载时的基准扭矩，保证机器人基本行驶能力（如案例4的baseTorque=0.4f）；
   补偿扭矩：根据负载偏差动态叠加的扭矩，分为两类：
   负载总量补偿：负载越大，总扭矩越大（如案例4的payload*torquePerKg，重量增加时总扭矩线性提升，保证牵引力）；
   负载偏差补偿：根据左右负载差异调整分配比例，偏差越大，分配差异越明显（如案例5的力差补偿，案例6的滑移率补偿，偏差侧扭矩增加，平衡侧扭矩减少）。
   分配原则：负载重的一侧扭矩略大，负载轻的一侧扭矩略小，核心是补偿不平衡力矩，防止机器人因重心偏移侧翻，同时避免轻载侧打滑（轻载侧扭矩过大易打滑）。
   代码体现：案例4的“电流差校正扭矩分配比例”，案例5的“力差决定扭矩分配系数”，案例6的“滑移率高则扭矩提升”，均是这一策略的具体实现。
   价值：避免“固定扭矩”导致的资源浪费（重载时扭矩不足、轻载时扭矩过剩），同时通过动态分配平衡负载，提升行驶稳定性和能量效率。
3. 闭环反馈控制：PID校正实现负载平衡的动态收敛
   核心逻辑：单纯基于负载检测的开环分配无法应对动态扰动（如地面摩擦力变化、传感器噪声），需引入闭环反馈控制器，通过实时监测“平衡偏差”（如电流差、力差、滑移率差），用PID算法动态校正扭矩，使系统稳定收敛至“负载平衡状态”。
   反馈偏差信号的选择：
   案例4：左右电机电流差，直接反映左右轮负载差异（电流与扭矩正相关）；
   案例5：左右机械臂力差，直接反映协作负载的不平衡程度；
   案例6：滑移率差，反映车轮与地面的附着差异，间接反映地形负载突变。
   PID的作用：
   比例环节：快速响应当前偏差，偏差越大，校正力度越大（如案例4的电流差大时，PID输出快速修正扭矩）；
   积分环节：消除稳态误差，确保长时间运行时偏差趋近于零（如地形持续倾斜时，积分累积补偿不平衡力矩）；
   微分环节：抑制超调，避免扭矩突变导致电机震荡，提升系统平稳性。
   价值：闭环反馈解决了开环控制的“静态偏差”和“动态扰动”问题，使系统能自适应环境变化，从不平衡状态快速收敛至平衡，确保动态平衡精度。
4. 动态边界约束：扭矩与转速的实时限幅，保障系统安全
   核心逻辑：自适应控制必须兼顾安全性与稳定性，需对输出扭矩和转速设定严格的动态边界约束，防止电机过载、电池过放、轮子打滑等风险，同时避免控制信号突变导致系统震荡。
   核心约束维度：
   扭矩上限约束：防止电机过载烧毁或电池亏电，如所有案例的constrain(torque, 0.3f, 1.5f)，将扭矩限制在电机额定范围内；
   扭矩变化率约束：通过PID的微分环节或软件限幅，避免扭矩突变（如案例6的扭矩提升是逐步的，而非瞬间跳变），防止轮子与地面的刚性冲击；
   转速/速度约束：通过滑移率检测控制速度，避免因扭矩过大导致轮子打滑（如案例6的滑移率超过阈值时，即使负载突变也不盲目提升速度）；
   传感器信号约束：对传感器数据进行滤波和阈值过滤，避免噪声导致的误控制（如案例4的负载重量通过constrain限制在0-50kg，防止异常数据）。
   代码体现：所有案例均通过constrain()函数限制扭矩范围，PID参数设置抑制超调，实现动态边界约束。
   价值：在保证负载平衡的同时，守护硬件安全底线，避免自适应控制带来的过载风险，确保系统长期稳定运行，尤其适用于未知环境（如崎岖地形）的自主作业。
5. 轻量化算法设计：适配Arduino的资源约束，平衡算力与性能
   核心逻辑：Arduino平台算力有限（8位CPU、RAM通常≤2KB）、存储有限，必须采用轻量化算法架构，简化计算复杂度，避免因资源不足导致控制延迟或程序崩溃，同时保证控制的实时性。
   轻量化实现路径：
   算法选择：优先采用PID、阈值判断等低算力算法，避免复杂的机器学习、多变量优化等高算力算法，如三个案例均用PID+简单逻辑，计算量小，响应快；
   数据类型优化：使用float替代double（节省一半存储空间），减少变量数量，复用全局变量（如leftCurrent、rightSpeed），避免栈溢出；
   控制周期优化：合理设置控制周期（10-100ms），案例4为100ms，案例6为20ms，既保证实时性，又避免高频采样导致的算力紧张；
   代码结构简化：采用函数模块化设计，避免循环嵌套和递归，减少冗余代码（如将扭矩分配逻辑封装为独立函数），提升代码执行效率。
   与高性能平台的平衡：轻量化不代表性能妥协，通过优化PID参数、合理选择反馈信号，在资源受限下实现“够用的控制精度”，满足小型移动机器人的实际需求。
   价值：让自适应负载平衡控制能够在低成本、低算力的Arduino平台落地，大幅降低开发门槛和硬件成本，适用于教学、科研、小型商用机器人等场景，无需依赖高性能控制器即可实现核心功能。

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