这是一个面向高可靠性、高机动性、任务关键型机器人的先进控制架构。其核心在于超越传统的、均等地将扭矩指令分配给各电机的模式，而是根据机器人实时状态、任务目标和各执行器的健康状态，智能、动态地分配扭矩，并结合主动容错机制，在部分执行器性能退化或失效时，维持系统的基本功能。这是从“控制单个电机”到“管理一个动力系统”的思维跃升。

一、 主要特点
核心：扭矩动态分配
原理：基于优化理论，将上层控制器（如运动控制、平衡控制）计算出的期望整车合力/合力矩（纵向力、横向力、横摆力矩），在多个驱动轮（通常是四轮或全向轮）之间进行最优分配。
优化目标：通常包括：
最小化轮胎附着裕度：避免任一车轮打滑，最大化整体稳定性。
最小化能量消耗：提高效率，延长续航。
最小化执行器损耗：平衡各电机负载，防止单一电机过热。
实现方法：建立轮胎力与电机扭矩的数学模型，构造一个带有约束（如电机扭矩上限、轮胎摩擦圆）的二次规划问题，实时求解最优分配矩阵。在嵌入式系统中，常采用计算量较小的加权最小二乘法。
关键：容错控制机制
分层容错：
执行器级：电机/ESC内置的过流、过温、欠压保护。这是硬件基础。
控制级：通过状态观测器（如龙伯格观测器）或卡尔曼滤波器，实时估计每个电机的“健康度”（如扭矩输出能力、效率）。当检测到某电机实际扭矩与指令存在持续偏差、温度异常升高或电流饱和时，标记其性能下降。
决策级：基于健康度信息，动态调整扭矩分配优化问题的约束和权重。例如：
对于过热电机，降低其扭矩输出上限。
对于响应迟缓的电机，在其控制环路中增加前馈补偿。
对于完全失效的电机，将其扭矩能力设为0，重新分配其他电机的扭矩，以“拖拽”故障轮继续完成任务。
系统集成与闭环
这是一个感知-决策-执行的闭环。它需要持续输入各电机状态（电流、转速、温度）和整车状态（IMU、轮速），经过优化算法计算，输出差异化的扭矩指令给各个ESC，形成一个能主动适应内外部变化的鲁棒控制系统。

二、 应用场景
军用/救援/探险机器人：在极端或危险环境中，系统必须能在车轮损毁、电机进水、齿轮卡滞等部分失效情况下，仍保有移动能力，以返回安全区或完成关键任务。容错机制是生存保障。
高性能全向移动平台：如使用麦克纳姆轮或全向轮的AGV。其运动高度依赖于各轮速度的精确配合。扭矩动态分配能优化各轮负载，防止在急转弯或侧向移动时因某个轮子打滑而导致运动精度崩溃。
高负载/不平衡负载AGV：搬运大型不规则重物时，重心偏移导致各轮负载不均。动态分配可主动为负载更大的轮子分配更多扭矩，补偿因正压力增大而增加的滚动阻力，保持车体直线行驶，避免“跑偏”。
无人驾驶车辆：在车辆稳定性控制中，通过对四个车轮实施差异化的驱动/制动扭矩，来产生纠正车辆横摆、侧滑的力矩，这是现代ESP/TCS系统的核心思想。在机器人平台上可作为先进控制算法的试验床。

三、 注意事项与挑战
模型依赖性与参数准确性
精准的轮胎-地面模型是关键瓶颈。优化分配依赖于对每个轮胎最大可用附着力（摩擦圆）的估计。这受到地面材质、湿度、轮胎磨损的极大影响，且难以实时精确测量。
解决方案：采用保守估计和自适应更新。初始使用一个较小的、安全的摩擦圆估计。同时，利用IMU和轮速数据在线估计轮胎力，并反向更新对地面附着系数的估计。
计算复杂性与实时性
在线求解优化问题（即使是QP问题）对嵌入式处理器是沉重负担，尤其在电机数量多、更新频率高（>100Hz）时。
解决方案：
简化模型：在满足精度前提下，使用线性轮胎模型代替复杂的非线性模型。
查表法与离线计算：针对常见工况（不同速度、曲率）预计算最优分配矩阵，在线查表加插值。
使用高性能硬件：必须采用带有硬件FPU和DSP指令集的MCU，如STM32F4/H7, ESP32-S3，或使用专门的控制计算库。
执行器性能的精确感知
容错控制的前提是能准确、快速地诊断出执行器的性能衰减或故障。这需要高精度的传感器和观测算法。
挑战：电机温度、绕组电阻变化、电池电压跌落都会影响其扭矩输出能力，与机械故障（如齿轮损坏）的表现类似，难以区分。
应对：建立电机的灰箱或黑箱模型，输入电压、PWM指令，观测输出转速和电流。通过比较模型预测输出与实际输出的残差，结合温度等信息，进行故障诊断。
与底层驱动器的深度集成
传统PWM/模拟信号控制的ESC是一个“黑盒”，无法获取内部电流环、温度等信息，也无法高带宽地接收扭矩指令。
必须升级：需要采用支持高级通信协议（如CAN FD, EtherCAT）的智能驱动器。这些驱动器能：
接收直接的扭矩/电流指令。
回报精确的相电流、母线电流、转子位置、温度。
实现本地的电流环、速度环闭环，为主控减负。
安全与稳定性边界
重新分配扭矩可能使健康电机过载。必须有全局的总功率/总电流限制，并在优化问题中作为硬约束。
在容错模式下，机器人的运动性能必然下降（如最大速度降低、转弯半径增大）。上层路径规划器必须能感知到底层动力系统的“健康状态”，并相应地进行降级规划，例如选择更平缓的路径或降低全局速度。

总结：该方案代表了轮式机器人动力控制的最高层次之一——协同优化与主动容错。它不再将多个驱动单元视为独立个体，而是作为一个可重构、可优化的整体动力系统进行管理。其实施是算法、硬件、传感深度融合的复杂系统工程，核心价值在于在不确定性（地面变化、负载变化、自身损耗）和部分失效的情况下，最大化系统的任务完成能力和生存概率。这不仅是提升“控制精度”，更是从根本上提升系统的可靠性与智能水平。

1、四轮差速机器人扭矩动态分配（带滑移检测补偿）

#include <SimpleFOC.h>
BLDCMotor motor[4];  // 4个BLDC电机
BLDCDriver3PWM driver[4];  // 4路驱动
Encoder enc[4];  // 4个编码器

// 扭矩分配参数
float base_torque = 0.5;  // 基础扭矩
float slip_threshold = 0.3;  // 滑移阈值
float max_torque = 1.0;  // 最大扭矩限制

void setup() {
  // 初始化电机、编码器、驱动器（略）
  for (int i = 0; i < 4; i++) {
    motor[i].linkDriver(&driver[i]);
    motor[i].linkSensor(&enc[i]);
    motor[i].initFOC();
  }
}

void loop() {
  float target_vel = 0.5;  // 目标线速度
  float wheel_vel[4];  // 实际轮速
  float torque[4];  // 分配扭矩

  // 1. 读取编码器速度
  for (int i = 0; i < 4; i++) {
    wheel_vel[i] = enc[i].getVelocity();
  }

  // 2. 滑移检测（左右轮速度差）
  float vel_diff = abs(wheel_vel[0] - wheel_vel[1]);  // 假设0/1为左右轮
  if (vel_diff > slip_threshold) {
    // 滑移补偿：减少高速轮扭矩，增加低速轮扭矩
    if (wheel_vel[0] > wheel_vel[1]) {
      torque[0] = base_torque * 0.7;
      torque[1] = base_torque * 1.3;
    } else {
      torque[0] = base_torque * 1.3;
      torque[1] = base_torque * 0.7;
    }
  } else {
    // 正常分配（差速转向）
    for (int i = 0; i < 4; i++) {
      torque[i] = base_torque;  // 简化示例，实际需根据转向计算
    }
  }

  // 3. 扭矩限制与输出
  for (int i = 0; i < 4; i++) {
    torque[i] = constrain(torque[i], 0, max_torque);
    motor[i].move(torque[i]);  // FOC扭矩控制
  }
  delay(10);  // 控制周期10ms
}

2、六轴机械臂关节容错控制（三模冗余表决）

// 假设3个冗余控制模块（A/B/C）独立运行
float module_output[3];  // 三个模块的输出扭矩
float final_torque;  // 最终输出

void redundant_control() {
  // 1. 读取三个模块的输出（示例值）
  module_output[0] = read_module_A();  // 模块A输出
  module_output[1] = read_module_B();  // 模块B输出
  module_output[2] = read_module_C();  // 模块C输出

  // 2. 三取二表决逻辑
  int valid_count = 0;
  float sum = 0;
  for (int i = 0; i < 3; i++) {
    if (abs(module_output[i] - module_output[(i+1)%3]) < 0.1) {  // 容差范围
      sum += module_output[i];
      valid_count++;
    }
  }

  // 3. 输出有效结果或故障隔离
  if (valid_count >= 2) {
    final_torque = sum / valid_count;  // 平均值
  } else {
    // 故障处理：切换至降级模式或紧急停止
    final_torque = 0;
    trigger_fault_recovery();
  }

  // 4. 应用扭矩到电机
  apply_torque(final_torque);
}

3、全向移动机器人动态负载自适应（模型参考自适应控制）

#include <PID_v1.h>
float target_load = 2.0;  // 目标负载扭矩（N·m）
float actual_load = 0;  // 实际负载（通过电流估算）
float Kt = 0.05;  // 电机扭矩常数
float adaptive_gain = 0.1;  // 自适应增益

PID loadPID(&actual_load, &output, &target_load, 2.0, 0.5, 0.1, DIRECT);

void setup() {
  loadPID.SetMode(AUTOMATIC);
  loadPID.SetSampleTime(20);  // 20ms控制周期
}

void loop() {
  // 1. 读取电流并估算负载扭矩
  float current = read_motor_current();  // 通过ADC或传感器
  actual_load = current * Kt;

  // 2. 模型参考自适应（调整PID参数）
  float error = target_load - actual_load;
  if (abs(error) > 0.5) {  // 负载突变检测
    loadPID.SetTunings(2.5, 0.8, 0.2);  // 增强响应
  } else {
    loadPID.SetTunings(2.0, 0.5, 0.1);  // 恢复稳态参数
  }

  // 3. PID输出扭矩
  loadPID.Compute();
  float final_torque = output + adaptive_gain * error;  // 前馈补偿

  // 4. 应用扭矩
  apply_torque(final_torque);
  delay(20);
}

要点解读
扭矩动态分配策略
差速补偿：通过检测轮速差异动态调整扭矩（如案例1），解决滑移问题。
负载均衡：在多轴系统中（如机械臂），根据负载变化重新分配扭矩，避免单轴过载。
前馈补偿：结合目标速度/加速度提前计算扭矩需求（如案例3），提升动态响应。
容错机制设计
三模冗余：通过硬件或时间冗余实现“三取二”表决（如案例2），屏蔽单点故障。
故障隔离：检测到异常时立即切断故障通道电源，防止故障扩散。
降级模式：冗余失效后切换至保守控制策略（如限速、限载），确保系统安全。
实时性与算力优化
高性能MCU：使用Teensy 4.0/ESP32等32位处理器，满足FOC、自适应算法的高计算需求。
任务分层：将实时性要求高的扭矩控制放在中断服务程序（ISR），编队算法放在主循环。
硬件加速：利用专用驱动芯片（如ODrive）分担PWM生成和电流环计算。
传感器融合与状态估计
多源数据：结合编码器、电流传感器、IMU数据，提升扭矩估算精度（如案例3）。
卡尔曼滤波：对噪声较大的传感器信号进行滤波，避免误触发容错机制。
机械模型：在线辨识负载惯量、摩擦系数等参数，优化自适应控制律。
安全与可靠性保障
电流限制：防止电机过流损坏，同时避免电源电压跌落导致系统复位。
热管理：监测电机温度，动态调整扭矩输出以防止过热。
通信冗余：在编队控制中采用TDMA协议，避免无线丢包导致队形发散。

4、基于差速模型的扭矩动态分配
功能描述：这是扭矩分配的基础。在双轮差速驱动中，不直接控制速度，而是通过控制左右轮的扭矩（电流）来实现转向和直行。当一侧负载变大（如爬坡）时，动态增加该侧扭矩。

#include <SimpleFOC.h>

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

// --- 扭矩分配参数 ---
float target_total_torque = 0.5; // 目标总扭矩 (归一化 0-1)
float steering_ratio = 0.0;      // 转向比例 (-1.0 左转, 1.0 右转)

void setup() {
  // 初始化电机为扭矩模式
  motor_left.controller = MotionControlType::torque;
  motor_left.init();
  motor_left.initFOC();
  
  motor_right.controller = MotionControlType::torque;
  motor_right.init();
  motor_right.initFOC();
}

void loop() {
  // 1. 动态扭矩计算
  // 左轮扭矩 = 基础扭矩 - 转向分量
  // 右轮扭矩 = 基础扭矩 + 转向分量
  float torque_left = target_total_torque * (1.0 - steering_ratio);
  float torque_right = target_total_torque * (1.0 + steering_ratio);

  // 2. 负载补偿逻辑 (模拟)
  // 如果检测到左侧编码器速度低于右侧，说明左侧阻力大
  // 这里增加一个简单的比例补偿
  float load_comp = 0.1; // 假设左侧需要额外补偿
  if (motor_left.shaft_velocity < motor_right.shaft_velocity * 0.9) {
      torque_left += load_comp;
  }

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

  // 4. 执行 FOC 算法
  motor_left.loopFOC();
  motor_right.loopFOC();

  delay(10);
}

5、基于电流监测的电机故障容错
功能描述：利用电流传感器（如 INA219）实时监测电机电流。当检测到某电机过流（卡死）或断路（无电流）时，触发容错机制，调整剩余正常电机的输出，维持机器人基本运动或安全停机。

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

// --- 硬件定义 ---
Adafruit_INA219 ina_left; // 左电机电流传感器
Adafruit_INA219 ina_right; // 右电机电流传感器
BLDCMotor motor_left(7), motor_right(7);

// --- 容错状态 ---
bool left_motor_fault = false;
bool right_motor_fault = false;
const float MAX_CURRENT = 5.0; // 最大安全电流 (A)

void setup() {
  Wire.begin();
  ina_left.begin();
  ina_right.begin();
  
  // 初始化电机...
  motor_left.init(); motor_left.initFOC();
  motor_right.init(); motor_right.initFOC();
}

void loop() {
  // 1. 读取电流
  float current_L = ina_left.getCurrent_mA() / 1000.0;
  float current_R = ina_right.getCurrent_mA() / 1000.0;

  // 2. 故障诊断
  // 过流检测
  if (current_L > MAX_CURRENT) left_motor_fault = true;
  if (current_R > MAX_CURRENT) right_motor_fault = true;
  
  // 断路检测 (有速度指令但无电流)
  // if (target_torque > 0.1 && current_L < 0.1) left_motor_fault = true;

  // 3. 容错控制策略
  float torque_cmd = 0.5; // 基础扭矩指令
  
  if (left_motor_fault && !right_motor_fault) {
    // 左电机故障：右电机接管，尝试维持直线 (需加大扭矩或调整角度)
    // 简单策略：右电机全速，左电机断电
    motor_left.move(0);
    motor_right.move(torque_cmd * 1.5); // 补偿扭矩
    Serial.println("⚠️ 左电机故障，右电机补偿运行");
  } 
  else if (!left_motor_fault && !right_motor_fault) {
    // 正常运行
    motor_left.move(torque_cmd);
    motor_right.move(torque_cmd);
  }
  else {
    // 双电机故障：紧急停机
    motor_left.move(0);
    motor_right.move(0);
  }

  motor_left.loopFOC();
  motor_right.loopFOC();
  delay(10);
}

6、多轴协同的动态扭矩补偿（主从模式）
功能描述：在四轮或多关节机器人中，由于机械结构误差，各轴很难完全同步。本案例采用“主从模式”，以主轴（如左前轮）为基准，从轴（如左后轮）根据位置误差动态调整扭矩，实现“软同步”，防止电机互搏。

#include <SimpleFOC.h>

// --- 硬件定义 ---
BLDCMotor motor_master(7); // 主轴
BLDCMotor motor_slave(7);  // 从轴

// --- 协同参数 ---
float sync_gain = 0.2; // 同步增益

void loop() {
  // 1. 获取主轴状态
  float master_angle = motor_master.shaft_angle;
  float master_velocity = motor_master.shaft_velocity;

  // 2. 计算从轴误差
  float slave_angle = motor_slave.shaft_angle;
  float angle_error = master_angle - slave_angle; // 角度差

  // 3. 动态扭矩补偿
  // 从轴的基础扭矩 + 同步修正扭矩
  // 如果从轴落后 (error > 0)，增加正向扭矩追赶
  float base_torque = 0.3;
  float sync_torque = angle_error * sync_gain;
  
  // 限制补偿量，防止震荡
  sync_torque = constrain(sync_torque, -0.2, 0.2);

  float final_slave_torque = base_torque + sync_torque;

  // 4. 输出
  motor_master.move(base_torque);
  motor_slave.move(final_slave_torque);

  motor_master.loopFOC();
  motor_slave.loopFOC();
  
  delay(10);
}

要点解读
扭矩控制是容错的基础
只有工作在扭矩模式（电流模式），才能真正实现对机器人受力状态的精确控制。速度模式下，电机会为了维持转速而无限增加电流，这在发生卡死时极易烧毁硬件。扭矩模式允许我们设定“最大出力上限”，从物理层面保护了系统。
差速分配的数学本质
就是分配逻辑，这种线性分配方式比单纯的速度差速更柔和。在越野或复杂地形中，通过独立控制左右扭矩，可以实现类似汽车“电子限滑差速器”的功能，将动力传递给有附着力的车轮。
故障诊断的多维性
案例5中的故障检测不仅依赖电流大小（过流），还可以结合断路检测（有指令无电流）。在实际工程中，还可以加入温度传感器（如案例中提到的 DS18B20）数据，形成“电流+温度+速度”的多维故障判断矩阵，避免单一参数误判。
主从协同防止“互搏”
在刚性连接的多电机系统（如双驱 AGV）中，如果两个电机都强行控制速度，极易产生“互搏”（一个推一个拉），导致电流激增。案例6的主从扭矩补偿策略，本质上是让从轴“柔性跟随”主轴，利用扭矩差来消除位置误差，这是解决机械装配误差的黄金法则。
实时性与安全停机
容错机制的核心是快。当检测到故障（如电流突增）时，必须在毫秒级时间内切断或调整输出。Arduino 的单线程特性要求我们在 loop 中优先处理故障标志位，或者利用硬件比较器直接关断 PWM，确保在软件死机时也能物理断电。

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