载荷转移感知的力矩动态分配，本质上是把“整车要多少力”变成“每个轮子该出多少力”的实时优化问题：加速/上坡时后轴加载、制动/下坡时前轴加载、转弯时外侧加载，分配器按各轮法向载荷与附着能力动态分配，避免轻载轮打滑、重载轮过载。

主要特点

1. 控制层次分层
   上层是整车速度/姿态环，输出总力矩需求；中层是载荷估计器，用 IMU 加速度、俯仰/横滚角、质心高度、轴距、轮距实时算各轮法向载荷；下层是分配器，按载荷占比与附着极限把总力矩拆成四个轮端指令；最底层是 SimpleFOC 的电流/力矩环执行。
2. 载荷不是静态均分
   静态时四轮均分 mg/4；但加速时后轴加载、前轴卸载，转弯时外侧加载、内侧卸载，上坡时前轴卸载、后轴加载。分配器必须用 IMU 的 ax、ay、pitch、roll 实时修正。
3. 分配带附着约束
   每轮最大可用力矩 ≈ μ × N_i × R。总需求超过总附着能力时，应整体等比缩小，而不是让某一轮硬顶到打滑。
4. 滑移率反馈形成闭环
   轮速显著高于整车速度且电流偏低，说明该轮打滑或悬空，应降低其分配权重，把力矩转移给仍有附着的轮子。
5. 从“控制电机”转向“管理动力”
   传统做法是四轮给相同 PWM；动态分配是把整车当作一个动力系统，按附着能力、能耗、健康度做约束优化。

典型应用场景
重载/偏载 AGV：货物偏置导致左右轮荷差 30%~50%，不补偿会跑偏、单侧电机过热。
越野巡检机器人：过减速带、台阶、坡道时载荷剧烈转移，前轮可能短暂悬空，需自动卸载悬空轮、向接地轮转移力矩。
全向移动平台（麦克纳姆轮）：侧向移动时对角轮载荷差异大，动态分配可提升轨迹精度、减少打滑。
高速差速转向：转弯时外侧轮加载，内侧轮易打滑，通过扭矩矢量产生辅助横摆力矩，提升循迹性。

需要注意的事项

1. 质心参数必须实测
   质心高度、前后/左右位置决定载荷转移量，估算偏差会导致分配失准。建议用称重法或倾斜台标定。
2. IMU 安装与标定
   IMU 必须装在本体刚性中心附近，并做加速度零偏、轴对齐标定。IMU 偏了，载荷估计就系统性偏。
3. 分配频率要低于 FOC 环
   FOC 电流环需 kHz 级更新，载荷估计与分配跑 50~100Hz 即可。把分配塞进 FOC 环会引发定时抖动、电流环失稳。
4. 分配结果必须限幅与平滑
   扭矩指令要做斜率限制（如 5~10Hz 低通），避免突变冲击机械结构；同时严格约束在 μ·N·R 与电机额定力矩内。
5. 悬空/打滑要有恢复迟滞
   悬空轮判定需要“持续 N 毫秒”才生效，恢复也需要“持续 M 毫秒”，否则颠簸时权重反复跳变，车体会抖动。
6. Arduino 算力边界
   Arduino Uno/Nano 跑四轮分配 + 四路 FOC 已接近极限，建议用 ESP32 或 STM32。分配算法优先用加权比例法，避免在线解 QP。
7. 安全兜底
   总需求超附着时等比缩、单轮异常时卸载、通信丢失时归零、看门狗兜底。水下/野外/重载场景一旦失控，回收成本远高于调试成本。

一句话总结：FOC 解决“每个轮子出力准不准”，载荷转移感知解决“每个轮子该出多少力”。 前者是执行层，后者是决策层；两者结合，四轮独立驱动才真正从“四个电机各转各的”变成“一个整车动力系统”。

1、基于 MRAC 误差项的“负载感知”扭矩分配（阻力大者多出力）
适用场景：四轮独立驱动机器人在搬运不规则重物或行驶于软硬不均地面时，各轮滚动阻力差异巨大。传统均分扭矩会导致阻力小的轮子空转，阻力大的轮子拖后腿。核心逻辑是：为每个轮子运行独立的 MRAC 速度跟踪器，将 MRAC 输出中“为了维持目标速度而额外付出的努力”（即误差补偿量）作为该轮的负载系数，负载越重，分配扭矩越大。

#include <SimpleFOC.h>

// 四轮 BLDC
BLDCMotor motor[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM driver[4] = {
  BLDCDriver3PWM(9, 10, 11, 8),    // 左前
  BLDCDriver3PWM(3, 5, 6, 4),      // 右前
  BLDCDriver3PWM(44, 45, 46, 43),  // 左后
  BLDCDriver3PWM(22, 23, 24, 21)   // 右后
};
Encoder enc[4] = {
  Encoder(18, 19, 2048), Encoder(20, 21, 2048),
  Encoder(30, 31, 2048), Encoder(32, 33, 2048)
};

// MRAC 状态（简化：用 PI 误差累积模拟“努力程度”）
float loadFactor[4] = {1.0, 1.0, 1.0, 1.0};
float effortIntegral[4] = {0, 0, 0, 0};

// 基础目标速度
float targetSpeed[4] = {2.0, 2.0, 2.0, 2.0};

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 4; i++) {
    enc[i].init();
    motor[i].linkSensor(&enc[i]);
    driver[i].voltage_power_supply = 24;
    driver[i].init();
    motor[i].linkDriver(&driver[i]);
    // 速度模式：MRAC 在速度环外层工作
    motor[i].controller = MotionControlType::velocity;
    motor[i].init();
    motor[i].initFOC();
  }
}

void loop() {
  for (int i = 0; i < 4; i++) motor[i].loopFOC();

  // ===== 负载感知：提取每个轮的“努力程度” =====
  for (int i = 0; i < 4; i++) {
    float actualVel = motor[i].shaftVelocity();
    float error = targetSpeed[i] - actualVel;

    // 误差持续为正（实际速度低于目标）说明该轮阻力大
    // 累积这个误差，作为负载指标
    effortIntegral[i] += error * 0.01; // 积分项
    effortIntegral[i] = constrain(effortIntegral[i], -2.0, 2.0);

    // 负载系数：努力程度越高，系数越大（1.0 为基准）
    loadFactor[i] = 1.0 + (effortIntegral[i] * 0.5);
    loadFactor[i] = constrain(loadFactor[i], 0.5, 2.0);
  }

  // ===== 动态扭矩分配（“谁阻力大谁出力大”） =====
  // 计算平均负载，用于归一化
  float avgLoad = 0;
  for (int i = 0; i < 4; i++) avgLoad += loadFactor[i];
  avgLoad /= 4.0;

  // 总扭矩需求（由上层速度 PID 给出，此处简化为固定值）
  float totalTorque = 2.0;

  for (int i = 0; i < 4; i++) {
    // 按负载系数比例分配扭矩
    float torque = totalTorque * (loadFactor[i] / avgLoad) / 4.0;
    // 限幅后执行
    torque = constrain(torque, 0, 1.2);
    motor[i].move(torque);
  }

  // 调试输出
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 300) {
    lastPrint = millis();
    Serial.print("Load: ");
    for (int i = 0; i < 4; i++) { Serial.print(loadFactor[i], 2); Serial.print(" "); }
    Serial.println();
  }

  delay(10);
}

解读：该案例直接映射了搜索结果中“提取 MRAC 的 theta 变化率作为负载因子”的思路。effortIntegral 的物理含义是“该轮为了跟上目标速度，比别的轮多付出了多少积分补偿”。在软地面或重载下，轮子持续跟不上速度，积分累积，loadFactor 升高，分配到的扭矩增大。这实现了无需力传感器的负载感知——用控制器的“挣扎程度”反推负载。

2、基于 IMU 姿态的静态载荷转移补偿（坡道/倾斜场景）
场景：机器人在斜坡上行驶或单侧负载偏重时，车身俯仰/横滚导致各轮法向压力重新分布。上坡时后轮载荷增加，下坡时前轮载荷增加；右倾时左侧轮载荷增加。本案例利用 IMU 解算姿态角，直接计算各轮的理论载荷比例，并作为扭矩分配的前馈系数，与案例一的 MRAC 反馈叠加。

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

BLDCMotor motor[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM driver[4] = {
  BLDCDriver3PWM(9, 10, 11, 8), BLDCDriver3PWM(3, 5, 6, 4),
  BLDCDriver3PWM(44, 45, 46, 43), BLDCDriver3PWM(22, 23, 24, 21)
};
Encoder enc[4] = {
  Encoder(18, 19, 2048), Encoder(20, 21, 2048),
  Encoder(30, 31, 2048), Encoder(32, 33, 2048)
};

MPU6050 imu;
float pitch = 0, roll = 0;

// 底盘几何参数
const float WHEELBASE = 0.35;   // 轴距 (m)
const float TRACK = 0.30;       // 轮距 (m)
const float CG_HEIGHT = 0.15;   // 重心高度 (m)
const float MASS = 8.0;         // 整车质量 (kg)

// 各轮静态载荷系数 (初始均分)
float staticLoad[4] = {0.25, 0.25, 0.25, 0.25};

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);

  for (int i = 0; i < 4; i++) {
    enc[i].init();
    motor[i].linkSensor(&enc[i]);
    driver[i].voltage_power_supply = 24;
    driver[i].init();
    motor[i].linkDriver(&driver[i]);
    motor[i].controller = MotionControlType::torque;
    motor[i].init();
    motor[i].initFOC();
  }
}

void loop() {
  for (int i = 0; i < 4; i++) motor[i].loopFOC();

  // 读取姿态
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  pitch = atan2(ay, az) * 180.0 / PI;  // 俯仰
  roll  = atan2(-ax, az) * 180.0 / PI; // 横滚

  // ===== 静态载荷转移计算 =====
  // 俯仰导致纵向转移：上坡 (pitch>0) 后轮增载
  float pitchRad = pitch * PI / 180.0;
  float deltaLong = (CG_HEIGHT / WHEELBASE) * sin(pitchRad);
  // 横滚导致侧向转移：右倾 (roll>0) 左侧增载
  float rollRad = roll * PI / 180.0;
  float deltaLat = (CG_HEIGHT / TRACK) * sin(rollRad);

  // 索引：0=左前, 1=右前, 2=左后, 3=右后
  // 纵向：前轮减载 (-deltaLong)，后轮增载 (+deltaLong)
  // 侧向：左轮增载 (+deltaLat)，右轮减载 (-deltaLat)
  staticLoad[0] = 0.25 - deltaLong + deltaLat; // 左前
  staticLoad[1] = 0.25 - deltaLong - deltaLat; // 右前
  staticLoad[2] = 0.25 + deltaLong + deltaLat; // 左后
  staticLoad[3] = 0.25 + deltaLong - deltaLat; // 右后

  // 限幅（保证不为负）
  for (int i = 0; i < 4; i++) {
    staticLoad[i] = constrain(staticLoad[i], 0.05, 0.45);
  }

  // ===== 扭矩分配 =====
  float totalTorque = 2.0;
  for (int i = 0; i < 4; i++) {
    // 按载荷比例分配：载荷越大的轮子需要更多扭矩来克服滚动阻力
    float torque = totalTorque * (staticLoad[i] / 0.25) / 4.0;
    torque = constrain(torque, 0, 1.2);
    motor[i].move(torque);
  }

  delay(10);
}

解读：该案例实现了搜索结果中提到的“基于姿态的负载转移补偿”。deltaLong 和 deltaLat 利用重心高度与轴距/轮距的比值，估算出俯仰和横滚引起的法向力转移量。staticLoad[i] 是每个轮子理论上的载荷占比（总和为 1）。扭矩按 staticLoad 比例分配，使载荷重的轮子获得更大驱动力。这属于前馈补偿，不依赖误差累积，响应比案例一的 MRAC 更快，但需要准确的几何参数。

3、MRAC 反馈 + 姿态前馈的融合分配（综合场景）
场景：机器人既在斜坡上行驶（静态载荷转移），又在软硬不均地面（动态阻力差异）。单独用前馈（案例二）无法应对地面变化，单独用反馈（案例一）响应滞后。本案例将两者融合：静态载荷比例作为基准分配系数，MRAC 误差项作为动态修正。

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

BLDCMotor motor[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM driver[4] = {
  BLDCDriver3PWM(9, 10, 11, 8), BLDCDriver3PWM(3, 5, 6, 4),
  BLDCDriver3PWM(44, 45, 46, 43), BLDCDriver3PWM(22, 23, 24, 21)
};
Encoder enc[4] = {
  Encoder(18, 19, 2048), Encoder(20, 21, 2048),
  Encoder(30, 31, 2048), Encoder(32, 33, 2048)
};

MPU6050 imu;
const float WHEELBASE = 0.35, TRACK = 0.30, CG_HEIGHT = 0.15;
const float R_W = 0.05; // 轮半径

// 融合系数
const float FEEDFORWARD_WEIGHT = 0.6;  // 静态载荷前馈权重
const float FEEDBACK_WEIGHT = 0.4;     // MRAC 反馈权重

float baseSpeed = 3.0; // 基础目标轮速 (rad/s)
float effort[4] = {0, 0, 0, 0};

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  for (int i = 0; i < 4; i++) {
    enc[i].init();
    motor[i].linkSensor(&enc[i]);
    driver[i].voltage_power_supply = 24;
    driver[i].init();
    motor[i].linkDriver(&driver[i]);
    motor[i].controller = MotionControlType::velocity;
    motor[i].init();
    motor[i].initFOC();
  }
}

void loop() {
  for (int i = 0; i < 4; i++) motor[i].loopFOC();

  // 读取姿态
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  float pitch = atan2(ay, az) * 180.0 / PI;
  float roll  = atan2(-ax, az) * 180.0 / PI;

  // ===== 1. 静态载荷前馈系数 =====
  float deltaLong = (CG_HEIGHT / WHEELBASE) * sin(pitch * PI / 180.0);
  float deltaLat  = (CG_HEIGHT / TRACK) * sin(roll * PI / 180.0);
  float staticLoad[4] = {
    0.25 - deltaLong + deltaLat,  // 左前
    0.25 - deltaLong - deltaLat,  // 右前
    0.25 + deltaLong + deltaLat,  // 左后
    0.25 + deltaLong - deltaLat   // 右后
  };

  // ===== 2. MRAC 动态负载反馈 =====
  for (int i = 0; i < 4; i++) {
    float err = baseSpeed - motor[i].shaftVelocity();
    effort[i] += err * 0.01;
    effort[i] = constrain(effort[i], -1.5, 1.5);
  }

  // ===== 3. 融合分配 =====
  float totalTorque = 2.5;
  for (int i = 0; i < 4; i++) {
    // 前馈部分：按静态载荷比例
    float ffTorque = totalTorque * (staticLoad[i] / 0.25) / 4.0 * FEEDFORWARD_WEIGHT;
    // 反馈部分：按 MRAC 努力程度比例
    float effortNorm = effort[i] / 1.5; // 归一化到 -1~1
    float fbTorque = totalTorque / 4.0 * (1.0 + effortNorm * 0.5) * FEEDBACK_WEIGHT;

    float torque = ffTorque + fbTorque;
    torque = constrain(torque, 0, 1.5);
    motor[i].move(torque);
  }

  delay(10);
}

解读：该案例是前两个案例的工程化融合。ffTorque 利用几何前馈在坡道上“预判”载荷分布，fbTorque 利用 MRAC 反馈在软地面上“修正”阻力差异。权重 0.6/0.4 是一个起点，实际调参时：地面均匀但坡度大 → 提高前馈权重；地面软硬不均但平坦 → 提高反馈权重。搜索结果中“扭矩动态分配与容错”的讨论也强调，单一分配策略无法覆盖所有工况，需要根据场景调整权重。

要点解读

1. “载荷转移感知”在无传感器条件下，本质上是对“控制努力”的解读
   四轮独立驱动系统通常不安装轮上力传感器（成本高、易损坏）。案例一和案例三的核心技巧是：利用速度控制器的误差累积量作为负载的代理指标。轮子越“挣扎”（误差持续存在），说明阻力越大、载荷越重。搜索结果中 MRAC 案例明确写道“如果误差持续为正，说明该轮子负载重或打滑，MRAC 的 theta 会自动增大以补偿误差，我们显式地提取这个努力程度作为负载因子”。这是无传感器负载感知的最务实路径。

2. 静态前馈与动态反馈必须融合，单独使用都有致命缺陷
   案例二的前馈分配在坡道上响应极快（IMU 直接给出姿态角），但无法应对地面摩擦系数的突然变化——从水泥地开上泥地时，IMU 姿态没变，但某轮开始打滑，前馈分配完全不知情。案例一的 MRAC 反馈能感知打滑，但响应滞后——需要累积几个控制周期的误差才能识别出负载变化。融合方案（案例三）让前馈负责“已知的、可预测的”载荷转移（坡度、侧倾），反馈负责“未知的、突发的”阻力变化（软地、打滑）。搜索结果中关于“模型依赖性与参数准确性”的讨论也指出，纯模型前馈在参数不准时反而有害。

3. 扭矩分配后必须限幅，且限幅策略决定安全边界
   三个案例都使用了 constrain(torque, 0, 1.2) 或类似限幅。这不是可选的。搜索结果明确指出：“重新分配扭矩可能使健康电机过载。必须有全局的总功率/总电流限制，并在优化问题中作为硬约束”。如果某个轮子被分配了超过其物理极限的扭矩，它会过流保护或烧毁，导致分配逻辑崩溃。更安全的策略是：先按比例缩放所有轮的扭矩，使总和不超过总功率上限，再对单轮做上限约束，而不是让某一轮“硬顶”然后触发保护。

4. FOC 力矩/电流模式是动态分配的执行前提，速度模式会干扰分配逻辑
   案例一使用速度模式（MotionControlType::velocity），其 MRAC 的“努力程度”来自速度环的误差；案例二和三使用力矩模式（MotionControlType::torque），直接下发扭矩。在需要精确扭矩分配的场合，力矩模式更合适，因为速度环的积分作用会“掩盖”负载差异——速度环会持续增扭直到轮子达到目标速度，此时你无法从速度环输出中读出真实的负载信息。搜索结果中滑移自适应扭矩控制的文章也强调：“扭矩模式比纯速度模式更适合越野场景，因为速度环在打滑时会持续加大输出，反而加剧空转”。

5. Arduino 算力边界下，四轮独立力矩分配需要分频与简化
   案例中的 delay(10) 对应 100Hz 的分配频率。四路 BLDC 的 loopFOC() 已占用大量 CPU 时间，IMU 姿态解算和分配算法如果也以 100Hz 运行，在 Arduino Uno 上会导致 FOC 环抖动。合理的分层是：FOC 环（loopFOC()）以最大频率运行，IMU 读取和分配计算降频到 50-100Hz。搜索结果建议使用带 FPU 的 MCU（ESP32-S3、Teensy 4.x、STM32F4）来承载四轮 FOC + 姿态解算 + 分配逻辑，Arduino Uno 的 8 位架构无法同时处理这四路电机和浮点运算。如果必须用 Uno，应将四轮分为两组，每组由独立从控 MCU 执行 FOC，主控只做分配决策。

4、载荷转移感知的驱动力矩比例分配
适用于直线或缓弯工况的 AGV/巡检车，按四轮实时法向力分配总驱动转矩，并对每轮做轮胎附着上限限幅。

/* ============================================================
   平台：ESP32-S3 + 4×BLDC（SimpleFOC）+ MPU6050
   功能：Fz 估计 → 比例分配 → 摩擦圆限幅
   轮序：0=FL 1=FR 2=RL 3=RR
   ============================================================ */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

/* ---------- 整车参数（必须实车标定） ---------- */
const float MASS   = 12.5f;    // kg
const float L      = 0.36f;    // 轴距 m
const float TRACK  = 0.28f;    // 轮距 m
const float H_CG   = 0.075f;   // 质心高度 m
const float A_CG   = 0.20f;    // 质心到前轴距离 m
const float R_W    = 0.065f;   // 车轮半径 m
const float MU     = 0.85f;    // 路面附着系数（可在线估计）
const float G      = 9.80665f;
const float KT_EFF = 0.018f;   // N·m/V，力矩-电压模式台架标定值

/* ---------- 电机 / 驱动 / 编码器 ---------- */
BLDCMotor   M_FL(7), M_FR(7), M_RL(7), M_RR(7);
BLDCMotor*  M[4] = {&M_FL, &M_FR, &M_RL, &M_RR};

BLDCDriver3PWM D_FL(32,33,25), D_FR(26,27,14), D_RL(15,2,4), D_RR(16,17,5);
BLDCDriver3PWM* D[4] = {&D_FL, &D_FR, &D_RL, &D_RR};

Encoder E_FL(34,35,2048), E_FR(36,39,2048), E_RL(18,19,2048), E_RR(21,23,2048);
Encoder* E[4] = {&E_FL, &E_FR, &E_RL, &E_RR};

MPU6050 imu;

/* ---------- 状态量 ---------- */
float ax_b = 0, ay_b = 0, yawRate = 0;   // 车体系加速度 m/s²、横摆角速度 rad/s
float roll = 0, pitch = 0;
float Fz[4] = {0};
unsigned long tIMU = 0;

/* ---------- IMU：姿态解算 + 重力分量剥离 ---------- */
void imuUpdate() {
  int16_t a[3], g[3];
  imu.getMotion6(&a[0],&a[1],&a[2], &g[0],&g[1],&g[2]);

  float axr = a[0]/16384.0f*G, ayr = a[1]/16384.0f*G, azr = a[2]/16384.0f*G;
  float gx  = g[0]/131.0f*DEG_TO_RAD;
  float gy  = g[1]/131.0f*DEG_TO_RAD;
  float gz  = g[2]/131.0f*DEG_TO_RAD;

  unsigned long now = micros();
  float dt = (now - tIMU) * 1e-6f;
  if (dt <= 0 || dt > 0.05f) dt = 2e-3f;
  tIMU = now;

  /* 简化互补滤波；工程上建议换成 Mahony / Madgwick */
  float roll_a  = atan2f(ayr, azr);
  float pitch_a = atan2f(-axr, sqrtf(ayr*ayr + azr*azr));
  roll  = 0.995f*(roll  + gx*dt) + 0.005f*roll_a;
  pitch = 0.995f*(pitch + gy*dt) + 0.005f*pitch_a;

  /* 投影到车体水平面：前向 + 左向 */
  ax_b = axr*cosf(pitch) - azr*sinf(pitch);
  ay_b = ayr*cosf(roll)  + azr*sinf(roll);
  yawRate = gz;
}

/* ---------- 法向力估算：静态分配 + 动载荷转移 ---------- */
void updateNormalForces() {
  const float Fzf = MASS*G*(L - A_CG)/L * 0.5f;   // 单前轮静态法向力
  const float Fzr = MASS*G* A_CG    /L * 0.5f;   // 单后轮静态法向力

  float dFzL = MASS * ax_b * H_CG / L;       // 纵向转移量（加速→后轴加）
  float dFzT = MASS * ay_b * H_CG / TRACK;   // 横向转移量（左向加速度→右侧加）

  Fz[0] = Fzf - 0.5f*dFzL - 0.5f*dFzT;   // FL
  Fz[1] = Fzf - 0.5f*dFzL + 0.5f*dFzT;   // FR
  Fz[2] = Fzr + 0.5f*dFzL - 0.5f*dFzT;   // RL
  Fz[3] = Fzr + 0.5f*dFzL + 0.5f*dFzT;   // RR

  const float Fz_min = 0.05f*MASS*G/4.0f;   // 防除零、防负值
  for (int i = 0; i < 4; i++) if (Fz[i] < Fz_min) Fz[i] = Fz_min;
}

/* ---------- 按 Fz 比例分配总力矩 + 摩擦圆限幅 ---------- */
void allocateTorque(float T_total) {
  float sumFz = Fz[0] + Fz[1] + Fz[2] + Fz[3];

  for (int i = 0; i < 4; i++) {
    float Ti   = T_total * Fz[i] / sumFz;   // 载荷转移比例分配
    float Tcap = MU * Fz[i] * R_W;          // 轮胎可传递力矩上限
    Ti = constrain(Ti, -Tcap, Tcap);
    M[i]->target = Ti / KT_EFF;             // 电压模式下的等效力矩指令
  }
}

void setup() {
  Wire.begin();
  imu.initialize();
  for (int i = 0; i < 4; i++) {
    M[i]->linkDriver(*D[i]);
    M[i]->linkSensor(*E[i]);
    D[i]->voltage_power_supply = 12.0f;
    D[i]->init();
    M[i]->init();
    M[i]->initFOC();
    M[i]->torque_controller = TorqueControlType::voltage;
  }
  tIMU = micros();
}

void loop() {
  imuUpdate();
  updateNormalForces();

  /* T_total 来自速度环：T = Kp*(v_ref - v_meas) + Ki*∫…，或由上位机直接下发 */
  static float v_ref = 0.8f;
  float v_meas = 0.25f*(E_FL.getVelocity() + E_FR.getVelocity()
                      + E_RL.getVelocity() + E_RR.getVelocity()) * R_W;
  float T_total = 0.6f*(v_ref - v_meas);   // 简化速度环

  allocateTorque(T_total);
  for (int i = 0; i < 4; i++) { M[i]->loopFOC(); M[i]->move(M[i]->target); }
}

5、滑移率感知的力矩降额与余量再分配
案例一的纯模型分配在路面附着突变时会失效。本案例用编码器轮速反算滑移率，对打滑轮降矩，把释放的力矩按其余轮的"附着余量"重新分配。

/* ============================================================
   在案例4基础上增加：滑移率判别 → 降额 → 余量再分配
   参考轮速按车体参考速度 + 横摆差速项生成
   ============================================================ */
const float SLIP_TH   = 0.15f;   // 滑移率阈值
const float SLIP_GAIN = 2.0f;    // 降额增益

/* 生成各轮参考线速度：v_ref[i] = v_body ± yawRate * TRACK/2 */
void buildWheelRefs(float v_body, float v_ref[4]) {
  float half = yawRate * TRACK * 0.5f;
  v_ref[0] = v_body - half;   // FL
  v_ref[1] = v_body + half;   // FR
  v_ref[2] = v_body - half;   // RL
  v_ref[3] = v_body + half;   // RR
}

void slipAwareAllocate(float T_total, const float v_ref[4]) {
  float sumFz = Fz[0] + Fz[1] + Fz[2] + Fz[3];
  float T[4], cap[4];
  float surplus = 0.0f;

  /* 第一遍：比例分配 + 滑移降额 */
  for (int i = 0; i < 4; i++) {
    T[i]   = T_total * Fz[i] / sumFz;
    cap[i] = MU * Fz[i] * R_W;

    float v_w = E[i]->getVelocity() * R_W;
    float den = fmaxf(fabsf(v_ref[i]), 0.20f);   // 低速时抑制噪声
    float s   = (v_w - v_ref[i]) / den;          // 驱动为正滑移，制动为负

    if (fabsf(s) > SLIP_TH) {
      float scale = fmaxf(0.0f, 1.0f - SLIP_GAIN*(fabsf(s) - SLIP_TH));
      float Tn = T[i] * scale;
      surplus += T[i] - Tn;      // 本周期被削掉的力矩
      T[i] = Tn;
    }
    T[i] = constrain(T[i], -cap[i], cap[i]);
  }

  /* 第二遍：余量按"剩余附着能力"加权再分配 */
  if (fabsf(surplus) > 1e-3f) {
    float avail[4], sumAv = 0.0f;
    for (int i = 0; i < 4; i++) {
      avail[i] = fmaxf(0.0f, cap[i] - fabsf(T[i]));
      sumAv += avail[i];
    }
    if (sumAv > 1e-4f)
      for (int i = 0; i < 4; i++) T[i] += surplus * avail[i] / sumAv;
  }

  for (int i = 0; i < 4; i++)
    M[i]->target = constrain(T[i], -cap[i], cap[i]) / KT_EFF;
}

void loop() {
  imuUpdate();
  updateNormalForces();

  static float v_body = 0.8f;
  float v_ref[4];
  buildWheelRefs(v_body, v_ref);

  float v_meas = 0.25f*(E_FL.getVelocity()+E_FR.getVelocity()
                      + E_RL.getVelocity()+E_RR.getVelocity()) * R_W;
  float T_total = 0.6f*(v_body - v_meas);

  slipAwareAllocate(T_total, v_ref);
  for (int i = 0; i < 4; i++) { M[i]->loopFOC(); M[i]->move(M[i]->target); }
}

6、横摆率闭环的扭矩矢量分配
纵向分配解决"不打滑"，横摆分配解决"走得正"。本案例用横摆角速度闭环产生附加横摆力矩，再按前后轴法向力权重拆成左右轮的力矩差。

/* ============================================================
   横摆率闭环 → 附加横摆力矩 Mz → 前后/左右二级分配
   ============================================================ */
const float KP = 1.8f, KI = 0.6f, KD = 0.05f;
const float DT = 0.001f;           // 1 kHz 控制周期

float yawPID(float r_ref, float r_meas) {
  static float I = 0, ePrev = 0;
  float e = r_ref - r_meas;
  I += e * DT;
  I  = constrain(I, -0.5f, 0.5f);          // 抗积分饱和
  float d = (e - ePrev) / DT;
  ePrev = e;
  return KP*e + KI*I + KD*d;               // 输出单位：N·m
}

void vectorAllocate(float T_total, float Mz) {
  float sumFz = Fz[0] + Fz[1] + Fz[2] + Fz[3];

  /* 1) 纵向基础分配（含载荷转移） */
  float T[4], cap[4];
  for (int i = 0; i < 4; i++) {
    T[i]   = T_total * Fz[i] / sumFz;
    cap[i] = MU * Fz[i] * R_W;
  }

  /* 2) 附加横摆力矩按前后轴法向力比例拆分 */
  float kf  = (Fz[0] + Fz[1]) / sumFz;          // 前轴承担比例
  float dTf = Mz * kf       * R_W / TRACK;      // 前轴左右力矩差
  float dTr = Mz * (1 - kf) * R_W / TRACK;      // 后轴左右力矩差

  T[0] -= dTf;  T[1] += dTf;   // FL 减 / FR 加
  T[2] -= dTr;  T[3] += dTr;   // RL 减 / RR 加

  /* 3) 摩擦圆限幅：先保横摆，再保纵向 */
  for (int i = 0; i < 4; i++)
    T[i] = constrain(T[i], -cap[i], cap[i]);

  for (int i = 0; i < 4; i++)
    M[i]->target = T[i] / KT_EFF;
}

void loop() {
  imuUpdate();
  updateNormalForces();

  static float v_body = 0.8f;
  static float yaw_ref = 0.0f;          // 上位机给定，或由 v*tan(δ)/L 生成
  float v_meas = 0.25f*(E_FL.getVelocity()+E_FR.getVelocity()
                      + E_RL.getVelocity()+E_RR.getVelocity()) * R_W;
  float T_total = 0.6f*(v_body - v_meas);

  float Mz = yawPID(yaw_ref, yawRate);  // 横摆力矩需求
  vectorAllocate(T_total, Mz);

  for (int i = 0; i < 4; i++) { M[i]->loopFOC(); M[i]->move(M[i]->target); }
}

要点解读
载荷转移模型必须与姿态解算绑定。 法向力的动态项来自车体纵向、横向加速度，而加速度计测的是比力，含重力投影分量。坡道或侧倾路面上不做重力补偿，ax_b、ay_b 会产生与真实加速度同量级的系统性偏差，分配权重随之整体偏移。H_CG（质心高度）是最敏感的标定参数，它直接放大动态项，建议用斜坡静止法或配重法实测，而非按几何尺寸估算。

比例分配只是加权最小二乘的特例。 严格的最优分配应求解 min Σ (Fx_i / (μ·Fz_i))²，即最小化轮胎附着利用率，其解为按 μ·Fz 加权的伪逆；当各轮 μ 相同时退化为按 Fz 比例分配。案例一采用比例式是为了实时性与可解释性，但在前后轴驱动能力不对称（如前轴电机功率更小）时，需要引入每轮力矩上限作为约束，否则出现"分配出去但电机给不出"的饱和。

滑移率闭环是模型失效时的兜底。 法向力估算是前馈，路面附着系数突变（油污、环氧地坪、地毯边缘）时前馈必然失准，必须用轮速做反馈。这里有两个易错点：低速段滑移率分母极小会导致判别抖动，故分母设下限（案例中取 0.20 m/s）；降额后的余量若不回代再分配，总驱动力会凭空损失，速度环积分项会持续累积直至饱和。

横摆与纵向需要优先级仲裁。 横摆力矩需求通过左右轮力矩差实现，会消耗轮胎的纵向附着余量。当某个轮长时间处于限幅边缘，说明纵向和横摆在争夺同一份附着，此时应优先保证横摆（车辆姿态与稳定性），必要时主动降低总驱动力矩，而不是让限幅把横摆需求悄悄吃掉——后者会让横摆闭环失去实际执行能力而自己不知道。

实现层面有四个硬约束。 一是控制频率：力矩分配与 loopFOC 应在 1 kHz 以上，而法向力估算 200~500 Hz 足够，二者可分频；二是四轮一致性：CPR、安装方向、KT_EFF 必须逐轮标定，否则会出现车辆静止时四轮互相顶牛的隐性偏航；三是运行模式：建议在 foc_current 电流环下工作并叠加电流限幅与温度降额，纯电压模式在母线电压波动时力矩不可控；四是验证顺序：先单轮台架标定 KT_EFF 与编码器，再做四轮同步空载测试，最后分 μ 档（干沥青/瓷砖/湿滑）做实车阶梯加速与定圆试验，记录各轮滑移率与横摆跟踪误差来定阈值。

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