基于Arduino与BLDC（无刷直流电机）构建的机器人系统，其“三冗余传感器投票 + 电流环碰撞检测”是一套将高可靠性感知与底层力矩监控深度融合的主动安全架构。该方案通过“三取二”（2oo3）投票逻辑消除单一传感器的误报与盲区，并利用BLDC电流环的实时扰动观测，实现对意外碰撞的毫秒级响应与柔顺退让。

主要特点

1. 基于“三取二”逻辑的容错感知与动态仲裁
   系统部署三组异构或同构传感器（如超声波、红外或激光雷达）监测同一物理量。在Arduino平台上，通过“三取二”（2oo3）投票算法进行数据仲裁：仅当至少两个传感器读数一致时，才判定为有效信号。这种机制能有效剔除因电磁干扰、环境噪声或单点故障导致的野值，确保在复杂环境中感知数据的绝对可靠性，避免因误判导致的系统急停或碰撞。
2. 基于FOC电流环的扰动观测与碰撞推断
   系统利用BLDC电机的FOC（磁场定向控制）架构，实时采集q轴电流（Iq，即转矩电流）。通过构建电机动力学模型，计算出当前运动状态下的“预期力矩”，并将其与“实际力矩”进行比对。当机器人发生碰撞时，外部阻力会导致实际电流瞬间偏离预期值。系统通过监测这一残差，无需额外安装昂贵的六维力传感器，即可实现“无接触式”的碰撞检测。
3. 毫秒级安全响应与柔顺退让策略
   一旦电流环检测到碰撞特征（如电流残差超过动态阈值），系统会在毫秒级内触发安全策略。除了常规的紧急制动（E-Stop），系统还可执行“柔顺反转”或“重力补偿”模式，驱动BLDC电机沿碰撞反方向输出微小力矩，模拟“缩回”动作。这种机制能有效释放夹持力，避免对人员或设备造成二次伤害，特别适用于人机协作场景。
4. 动态阈值与多源信息融合
   为了适应机器人不同运动状态下的负载变化，碰撞检测的阈值并非固定值，而是根据当前的速度、加速度和关节角度进行动态调整。同时，系统会将电流环的碰撞信号与IMU的加速度突变、编码器的位置偏差进行多源融合，进一步降低误报率，确保只有在真正发生物理接触时才触发保护。

应用场景

1. 人机协作轻型机械臂（Cobot）
   在小型装配、分拣或3D打印辅助任务中，机械臂需与人共享工作空间。该系统能确保机械臂在意外触碰人体时瞬间停止或退让，满足ISO/TS 15066对协作机器人的部分安全要求，保障操作人员安全。
2. 桌面级自动化与精密装配
   在自动拧螺丝、PCB插件或样品搬运等场景中，机器人需防止因工件错位、夹具失效导致的机械过载。三冗余传感器确保定位精准，电流环碰撞检测则能在卡死瞬间保护昂贵的工件与工具。
3. 教育与科研实验平台
   在高校机器人实验室中，该系统是演示安全控制策略、验证扰动观测器算法的绝佳平台。学生可通过编程实现不同碰撞检测算法（如滑模观测器），直观理解力矩控制与安全逻辑。
4. 家庭/创客级服务机器人
   在智能取物臂或宠物喂食机械手等非结构化环境中，机器人需具备基本防撞能力。该方案以低成本实现了高可靠性的安全防护，避免机器人在运行中因碰撞损坏家具或伤及用户。

注意事项

1. 传感器同步与信号不同步处理
   三冗余传感器的响应时间、采样时刻及数据传输延迟可能存在差异。必须在软件中设计有效的不同步报警逻辑，通过计算传感器间的差值并引入持续时间阈值，避免因短暂波动或噪声干扰触发错误报警。
2. 电流检测精度与带宽要求
   碰撞检测的可靠性直接依赖于Iq的测量精度。必须选用高精度、高带宽的电流传感器（如ACS712、INA240），并合理设计模拟前端（如差分放大、RC滤波），以抑制BLDC高速运行时的反电动势干扰。
3. 动态阈值整定与模型准确性
   固定电流阈值极易在加速或减速阶段导致误触发。必须根据当前运动状态动态调整碰撞判定阈值，或引入滑动窗口标准差分析。同时，扰动观测器的核心是准确的电机模型，需进行严谨的系统辨识实验，确保模型参数（如惯量、摩擦系数）的准确性。
4. Arduino实时性与算力瓶颈
   Arduino Uno等8位MCU难以胜任高频FOC（>10kHz）与实时碰撞检测的并行运算。建议优先选用Arduino Due（Cortex-M3）、Teensy 4.0/4.1（Cortex-M7）或ESP32等具备硬件FPU的高性能微控制器，以保证控制周期的实时性。
5. 硬件级安全冗余与法规符合性
   基于软件和模型的安全策略不能替代硬件的安全回路。在涉及人身安全的关键应用中，必须配备独立的安全继电器、硬件扭矩限制器或光栅作为最高等级的保护。此外，Arduino系统不具备工业安全认证（如SIL、PL等级），不可直接用于高风险工业产线。

1、三路IMU姿态冗余投票（横滚角三取二）
此案例聚焦于姿态传感器的三模冗余投票。三路MPU6050以不同I2C地址挂载同一总线，读取横滚角（Roll）后通过中值投票输出最终姿态角。当某一路IMU因振动或电磁干扰产生异常值时，投票机制将其“投票出局”。

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

// 三路IMU配置（不同I2C地址）
const uint8_t imuAddr[3] = {0x68, 0x69, 0x6A};
MPU6050 imu[3];

// 姿态数据结构
struct AttitudeData {
  float roll;        // 横滚角（度）
  bool valid;        // 有效性标记
} attitude[3];

// 投票结果
float votedRoll = 0.0;
int validCount = 0;

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

  // 初始化三路IMU
  for (int i = 0; i < 3; i++) {
    imu[i].initialize();
    imu[i].setFullScaleGyroRange(MPU6050_GYRO_FS_250);
    imu[i].setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    attitude[i].valid = true;
  }
  Serial.println("三路IMU初始化完成");
}

// 读取三路IMU横滚角
void readAllIMU() {
  for (int i = 0; i < 3; i++) {
    int16_t ax, ay, az;
    imu[i].getAcceleration(&ax, &ay, &az);

    // 横滚角 = atan2(ay, az)
    attitude[i].roll = atan2(ay, az) * 180.0 / PI;

    // 有效性检查：角度超出合理范围则标记无效
    if (attitude[i].roll < -90 || attitude[i].roll > 90) {
      attitude[i].valid = false;
    }
  }
}

// 三取二中值投票
float majorityVote() {
  // 收集有效数据
  float validRolls[3];
  int count = 0;

  for (int i = 0; i < 3; i++) {
    if (attitude[i].valid) {
      validRolls[count++] = attitude[i].roll;
    }
  }

  validCount = count;

  if (count == 0) {
    // 全部失效：返回安全值
    return 0.0;
  } else if (count == 1) {
    // 仅一路有效：直接采用（降级运行）
    return validRolls[0];
  } else if (count == 2) {
    // 两路有效：取平均
    return (validRolls[0] + validRolls[1]) / 2.0;
  } else {
    // 三路有效：中值投票（取排序后的中间值）
    // 手动三值排序
    float a = validRolls[0], b = validRolls[1], c = validRolls[2];
    if ((a >= b && a <= c) || (a <= b && a >= c)) return a;
    if ((b >= a && b <= c) || (b <= a && b >= c)) return b;
    return c;
  }
}

void loop() {
  // 1. 读取三路IMU
  readAllIMU();

  // 2. 中值投票
  votedRoll = majorityVote();

  // 3. 串口调试
  Serial.print("Rolls: ");
  for (int i = 0; i < 3; i++) {
    Serial.print(attitude[i].roll); Serial.print("\t");
  }
  Serial.print("| Voted: "); Serial.print(votedRoll);
  Serial.print("\t| Valid: "); Serial.println(validCount);

  // 4. 基于投票结果的姿态控制（此处为占位）
  // motorL.move(votedRoll * 0.01); ...

  delay(50);
}

核心逻辑：三路IMU的横滚角通过中值投票输出。中值投票的优势在于：即使一路IMU完全失效（输出0或90°），投票结果仍由另外两路正常IMU决定，不受异常值影响。valid标记进一步排除明显超出量程的读数。

2、三冗余温度传感器投票 + 指令仲裁
此案例将投票机制扩展到温度传感器和指令源。三路温度传感器通过中值投票消除单点故障，三路指令（手动/遥控/自动）通过优先级仲裁决定最终执行指令。

#include <SimpleFOC.h>

// 三路温度传感器（模拟输入）
#define TEMP_A A0
#define TEMP_B A1
#define TEMP_C A2

// 三路指令源优先级（0最高）
enum CmdSource { CMD_AUTO = 2, CMD_RADIO = 1, CMD_MANUAL = 0 };

// 温度阈值
const float TEMP_WARNING = 60.0;
const float TEMP_CRITICAL = 85.0;

// 指令结构
struct Command {
  float speed;       // 目标速度
  float turn;        // 转向
  int priority;      // 优先级
  bool valid;
};

Command cmds[3];  // 三路指令
float votedTemp = 0.0;

// 三取二中值投票
float medianOfThree(float a, float b, float c) {
  if ((a >= b && a <= c) || (a <= b && a >= c)) return a;
  if ((b >= a && b <= c) || (b <= a && b >= c)) return b;
  return c;
}

// 读取三路温度并投票
float voteTemperature() {
  float tA = analogRead(TEMP_A) * 5.0 / 1023.0 * 100.0;
  float tB = analogRead(TEMP_B) * 5.0 / 1023.0 * 100.0;
  float tC = analogRead(TEMP_C) * 5.0 / 1023.0 * 100.0;

  // 一致性检查：与平均值差异过大的传感器标记为故障
  float avg = (tA + tB + tC) / 3.0;
  int faultCount = 0;
  if (abs(tA - avg) > 25) { faultCount++; tA = avg; }
  if (abs(tB - avg) > 25) { faultCount++; tB = avg; }
  if (abs(tC - avg) > 25) { faultCount++; tC = avg; }

  if (faultCount >= 2) {
    Serial.println("温度传感器故障：两路以上异常！");
    return -1.0;  // 标记为故障
  }

  return medianOfThree(tA, tB, tC);
}

// 指令仲裁：优先级最高的有效指令胜出
Command arbitrateCommands() {
  Command winner = {0, 0, 999, false};

  for (int i = 0; i < 3; i++) {
    if (!cmds[i].valid) continue;

    // 优先级数值越小越优先
    if (cmds[i].priority < winner.priority) {
      winner = cmds[i];
    }
  }

  return winner;
}

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

  // 初始化三路指令为默认值
  cmds[0] = {1.0, 0.0, CMD_MANUAL, true};   // 手动（最高优先级）
  cmds[1] = {0.5, 0.0, CMD_RADIO, true};    // 遥控
  cmds[2] = {0.8, 0.1, CMD_AUTO, true};     // 自动（最低优先级）
}

void loop() {
  // ===== 1. 三冗余温度投票 =====
  votedTemp = voteTemperature();

  // ===== 2. 指令仲裁 =====
  Command finalCmd = arbitrateCommands();

  // ===== 3. 热安全决策（最高优先级，覆盖指令）=====
  if (votedTemp > TEMP_CRITICAL) {
    // 极端过热：强制停机
    Serial.println("CRITICAL TEMP! Emergency stop.");
    motorL.move(0);
    motorR.move(0);
    motorL.loopFOC();
    motorR.loopFOC();
    delay(1000);
    return;
  } else if (votedTemp > TEMP_WARNING) {
    // 预警：限制功率输出
    finalCmd.speed *= 0.5;
    Serial.println("Temp warning: power limited.");
  }

  // ===== 4. 执行最终指令 =====
  if (finalCmd.valid) {
    motorL.target = finalCmd.speed - finalCmd.turn;
    motorR.target = finalCmd.speed + finalCmd.turn;
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();
  }

  // 调试输出
  Serial.print("Temp: "); Serial.print(votedTemp);
  Serial.print(" | Cmd: "); Serial.println(finalCmd.priority);

  delay(50);
}

核心逻辑：温度投票和指令仲裁共享同一套“三取二”哲学。温度投票通过中值法排除异常传感器；指令仲裁通过优先级选择最终指令。热安全决策拥有最高优先级，一旦温度超过临界值，直接覆盖所有指令执行停机。

3、电流环碰撞检测 + 硬件急停融合
此案例聚焦于BLDC电流环碰撞检测。利用FOC的q轴电流（Iq）作为碰撞传感器——当机器人遇到异常阻力时，q轴电流突增，系统立即触发碰撞保护。硬件急停按钮作为最高优先级，通过中断直接切断电机动力。

#include <SimpleFOC.h>

// BLDC电机（带FOC电流环）
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);

// 碰撞检测参数
const float CURRENT_THRESHOLD = 2.5;   // 碰撞电流阈值(A)
const float CURRENT_RECOVERY = 1.0;    // 恢复电流阈值(A)
const unsigned long COLLISION_COOLDOWN = 2000;  // 碰撞冷却(ms)

// 碰撞状态
bool collisionDetected = false;
unsigned long collisionTime = 0;

// 硬件急停引脚
#define EMG_STOP_PIN 2
volatile bool emgStopTriggered = false;

// 硬件急停ISR（最高优先级）
void emgStopISR() {
  emgStopTriggered = true;
}

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

  // BLDC初始化
  driver.voltage_power_supply = 12;
  driver.init();
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::torque;  // 扭矩模式（电流环直接可用）
  motor.init();
  motor.initFOC();

  // 硬件急停配置
  pinMode(EMG_STOP_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(EMG_STOP_PIN), emgStopISR, FALLING);

  Serial.println("电流环碰撞检测系统就绪");
}

void loop() {
  // ===== 1. 硬件急停（最高优先级）=====
  if (emgStopTriggered) {
    motor.move(0);
    motor.disable();
    Serial.println("HARDWARE E-STOP! Motor disabled.");
    // 等待按钮释放
    if (digitalRead(EMG_STOP_PIN) == HIGH) {
      emgStopTriggered = false;
      motor.enable();
      Serial.println("System reset.");
    }
    delay(100);
    return;
  }

  // ===== 2. 读取q轴电流（FOC电流环输出）=====
  // SimpleFOC的current.q即为q轴电流
  float iq = abs(motor.current.q);

  // ===== 3. 碰撞检测 =====
  if (iq > CURRENT_THRESHOLD && !collisionDetected) {
    collisionDetected = true;
    collisionTime = millis();

    // 立即停止电机（软件级急停）
    motor.move(0);

    Serial.print("COLLISION! iq = ");
    Serial.print(iq);
    Serial.println(" A");
  }

  // ===== 4. 碰撞恢复逻辑 =====
  if (collisionDetected) {
    // 冷却时间到达后检查电流是否回落
    if (millis() - collisionTime > COLLISION_COOLDOWN) {
      if (iq < CURRENT_RECOVERY) {
        collisionDetected = false;
        Serial.println("Collision cleared, resuming.");
      }
    }
    // 碰撞状态下保持停止
    motor.move(0);
    motor.loopFOC();
    delay(20);
    return;
  }

  // ===== 5. 正常运动控制 =====
  motor.move(0.5);  // 目标扭矩
  motor.loopFOC();

  // 调试输出
  if (millis() % 200 < 10) {
    Serial.print("iq: "); Serial.println(iq);
  }

  delay(20);
}

核心逻辑：BLDC在扭矩控制模式下，q轴电流与输出转矩成正比。当机器人遇到碰撞或堵转时，负载转矩突增，q轴电流迅速上升。通过监测motor.current.q，可在毫秒级检测到碰撞事件，比外部碰撞传感器更直接、更快速。硬件急停中断则作为不可屏蔽的最终防线。

要点解读

1. 三取二投票的核心是“中值”，而非“平均”

三冗余投票的目的是消除单点故障。如果采用平均值，一个失效传感器（输出0或满量程）会严重拉偏结果。中值投票则天然排除异常值——三个数排序后取中间值，异常值要么在最大值位置，要么在最小值位置，不影响中间值。案例一和案例二的温度投票均采用此策略。

2. 冗余投票需要“有效性检查”作为前置过滤

投票的前提是数据本身“可用”。案例一中通过valid标记排除超出量程的读数，案例二中通过“与平均值差异过大”标记故障传感器。没有前置过滤的投票，在传感器完全失效（如输出固定值）时可能产生错误结果。有效性检查与投票机制是互补的。

3. BLDC的FOC电流环是天然的“碰撞传感器”

在FOC扭矩模式下，q轴电流（Iq）与电机输出转矩成正比。当机器人遇到碰撞或堵转时，负载转矩突增，Iq迅速上升。这一信号比任何外部碰撞传感器都更直接——它反映的是电机“真实感受到的阻力”，响应速度在毫秒级。通过设置Iq阈值，即可实现无额外硬件的碰撞检测。

4. 硬件急停必须独立于软件逻辑，拥有不可屏蔽的最高优先级

软件碰撞检测可能因MCU死机或程序卡死而失效。硬件急停按钮通过外部中断直接触发ISR，设置volatile标志位后由主循环执行制动。中断响应是微秒级的，不依赖主程序状态。案例三中，硬件急停标志的检查位于主循环最前端，优先于任何其他逻辑。

5. 控制权交接需要平滑，避免安全恢复时的二次冲击

当系统从“碰撞停止”恢复到“正常运动”时，如果直接交还控制权，速度或扭矩指令的突变可能导致二次冲击。工程上需要设计恢复斜坡——确认安全后，扭矩/速度目标值从0逐步上升至正常水平，而非阶跃恢复。这一细节在安全系统中容易被忽视，但对机械寿命至关重要。

4、三冗余传感器投票核心程序（核心：故障识别+数据融合）
该程序聚焦三冗余传感器的投票逻辑，通过3个同类型传感器采集同一目标数据，按投票规则筛选有效值、识别传感器故障，适用于机器人避障、位姿检测等需要高可靠感知的场景，是三冗余机制的基础实现。

核心逻辑
数据采集：同步读取3路冗余传感器，获取原始数据；
故障识别：剔除明显异常值（超量程、跳变、断线）；
投票融合：按“少数服从多数”规则筛选有效值，输出可信结果。

// 三冗余传感器投票核心程序——感知层容错基础
#include <Filter.h> // 可选：用于滤波，简化可采用简单平均
// 硬件引脚定义（以HC-SR04超声波传感器为例，3路冗余检测前侧障碍物）
#define SENSOR1_TRIG 22
#define SENSOR1_ECHO 23
#define SENSOR2_TRIG 24
#define SENSOR2_ECHO 25
#define SENSOR3_TRIG 26
#define SENSOR3_ECHO 27
// 电机控制引脚（可简化，案例聚焦传感器投票）
#define LEFT_MOTOR_PWM 9
#define RIGHT_MOTOR_PWM 11

// 投票规则参数
const float SENSOR_DANGER_DIST = 0.3; // 危险距离阈值（m，低于此值需刹车）
const float SENSOR_MAX_DIST = 2.0;    // 传感器最大测距（m，超出判定为故障）
const float SENSOR_MIN_DIST = 0.05;   // 传感器最小测距（m，低于此值判定为故障）
const float SENSOR_ERROR_DELTA = 0.4; // 传感器数值偏差阈值（m，超出判定为异常）

// 传感器数据变量
float sensorVal[3] = {0.0}; // 存储3路传感器原始数据（单位：m）
bool sensorValid[3] = {false}; // 存储3路传感器有效性float votedValue = 0.0;      // 投票输出的有效值
bool collisionWarning = false; // 碰撞预警信号

// 超声波测距函数：返回距离（m），异常返回MAX_DIST+1标记故障
float getUltrasonicDistance(int trigPin, int echoPin) {
  // 触发测距
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // 读取回波时间
  long duration = pulseIn(echoPin, HIGH, 30000); // 超时时间30ms，对应最大测距约5m
  float distanceCM = (duration * 0.0343) / 2; // 声速343m/s，时间转距离
  
  // 故障判定：超出量程或数值异常
  if (distanceCM <= SENSOR_MIN_DIST * 100 || distanceCM > SENSOR_MAX_DIST * 100) {
    return SENSOR_MAX_DIST + 1; // 故障标记
  }
  return distanceCM / 100.0;}

// 传感器有效性检测：剔除明显故障
void checkSensorValidity() {
  for (int i = 0; i < 3; i++) {
    // 故障判定：数值超出量程范围
    if (sensorVal[i] > SENSOR_MAX_DIST || sensorVal[i] < SENSOR_MIN_DIST) {
      sensorValid[i] = false;
    } else {
      sensorValid[i] = true;
    }
  }
}

// 三冗余投票融合函数：筛选有效值，输出可信距离
void threeVoteFusion() {
  checkSensorValidity(); // 先剔除故障传感器
  
  // 统计有效传感器数量
  int validCount = 0;
  for (int i = 0; i < 3; i++) {
    if (sensorValid[i]) validCount++;
  }
  
  // 情况1：无有效传感器 → 取最大安全距离（保守策略）
  if (validCount == 0) {
    votedValue = SENSOR_MAX_DIST;
    collisionWarning = true; // 感知失效，预警停车
    return;
  }
  
  // 情况2：1个有效传感器 → 取该值（需结合电流环双重验证，案例2补充）
  if (validCount == 1) {
    for (int i = 0; i < 3; i++) {
      if (sensorValid[i]) {
        votedValue = sensorVal[i];
        break;
      }
    }
  }
  
  // 情况3：2个及以上有效传感器 → 投票筛选
  if (validCount >= 2) {
    // 计算3路传感器数据两两之间的偏差
    float delta12 = abs(sensorVal[0] - sensorVal[1]);
    float delta13 = abs(sensorVal[0] - sensorVal[2]);
    float delta23 = abs(sensorVal[1] - sensorVal[2]);
    
    // 情况3a：两两偏差均在阈值内 → 投票一致，取平均值
    if (delta12 <= SENSOR_ERROR_DELTA && delta13 <= SENSOR_ERROR_DELTA && delta23 <= SENSOR_ERROR_DELTA) {
      votedValue = (sensorVal[0] + sensorVal[1] + sensorVal[2]) / 3;// 单独标记：若偏差小但数值低于危险阈值，仍判定预警
      collisionWarning = (votedValue <= SENSOR_DANGER_DIST);
    }
    // 情况3b：某一传感器与其他两个偏差过大 → 判定该传感器故障，取多数    else {
      // 找出偏差最大的传感器索引
      int faultIndex = -1;
      if (delta12 > SENSOR_ERROR_DELTA && delta13 > SENSOR_ERROR_DELTA) {
        faultIndex = 0; // sensor1与其他两个偏差大
      } else if (delta12 > SENSOR_ERROR_DELTA && delta23 > SENSOR_ERROR_DELTA) {
        faultIndex = 1; // sensor2与其他两个偏差大
      } else if (delta13 > SENSOR_ERROR_DELTA && delta23 > SENSOR_ERROR_DELTA) {
        faultIndex = 2; // sensor3与其他两个偏差大
      }
      
 if (faultIndex != -1) {
        sensorValid[faultIndex] = false; // 标记故障传感器
        // 取另外两个有效传感器的平均值
        votedValue = (sensorVal[(faultIndex + 1) % 3] + sensorVal[(faultIndex + 2) % 3]) / 2;
      } else {
        // 极端情况：两两偏差均超过阈值，保守取最大距离        votedValue = SENSOR_MAX_DIST;
        collisionWarning = true;
      }
    }
  }
}

// 碰撞预警控制：根据投票结果控制电机
void collisionControl() {
  if (collisionWarning) {
    // 预警触发：急停电机
    analogWrite(LEFT_MOTOR_PWM, 0);
    analogWrite(RIGHT_MOTOR_PWM, 0);
  } else {
    // 正常行驶：根据距离调整速度（距离越近，速度越低）
    int baseSpeed = map(votedValue, SENSOR_DANGER_DIST, SENSOR_MAX_DIST, 0, 150);
    baseSpeed = constrain(baseSpeed, 0, 150);
    analogWrite(LEFT_MOTOR_PWM, baseSpeed);
    analogWrite(RIGHT_MOTOR_PWM, baseSpeed);
  }
}

void setup() {
  Serial.begin(115200);
  
  // 传感器引脚初始化
  pinMode(SENSOR1_TRIG, OUTPUT);
  pinMode(SENSOR1_ECHO, INPUT);
  pinMode(SENSOR2_TRIG, OUTPUT);
  pinMode(SENSOR2_ECHO, INPUT);
  pinMode(SENSOR3_TRIG, OUTPUT);
  pinMode(SENSOR3_ECHO, INPUT);
  
  // 电机引脚初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT); Serial.println("三冗余传感器投票系统初始化完成...");
}

void loop() {
  // 1. 采集3路传感器数据
  sensorVal[0] = getUltrasonicDistance(SENSOR1_TRIG, SENSOR1_ECHO);
  sensorVal[1] = getUltrasonicDistance(SENSOR2_TRIG, SENSOR2_ECHO);
  sensorVal[2] = getUltrasonicDistance(SENSOR3_TRIG, SENSOR3_ECHO);
  
  // 2. 三冗余投票融合
  threeVoteFusion(); // 3. 碰撞控制
  collisionControl();
  
  // 4. 串口输出调试信息
  Serial.print("传感器数值[m]:");
  Serial.print(sensorVal[0], 2);
  Serial.print(" / ");
  Serial.print(sensorVal[1], 2);
  Serial.print(" / ");
  Serial.print(sensorVal[2], 2);
  Serial.print(" | 有效标记:");
  Serial.print(sensorValid[0]);
  Serial.print(" / ");
  Serial.print(sensorValid[1]);
  Serial.print(" / ");
  Serial.print(sensorValid[2]);
  Serial.print(" | 投票值[m]:");
  Serial.print(votedValue, 2);
  Serial.print(" | 预警:");
  Serial.println(collisionWarning ? "是" : "否");
  
  delay(100); // 循环周期100ms，适配传感器响应速度
}

程序优化方向

• 可加入滑动平均滤波，滤除传感器随机噪声，提升数据稳定性；
• 支持多组冗余传感器（如前、左、右各3个），构建全方位感知冗余；
• 投票规则可扩展为“权重投票”：精度高的传感器权重更高，适配不同量程、精度的传感器组合。

5、电流环碰撞检测程序（核心：电流采样+阈值判定）
该程序聚焦电流环碰撞检测的核心逻辑，通过采集BLDC电机的工作电流，当碰撞导致电机堵转、电流骤增时，快速识别碰撞并触发保护，无需额外碰撞传感器，适用于所有带电流采样的BLDC机器人，是机械碰撞的最后一道防护。

核心逻辑

1. 电流采集：通过ADC读取驱动板的电流采样端，转换为实际电流值；
2. 阈值判定：对比实际电流与“碰撞电流阈值”，识别碰撞；
3. 保护动作：触发电机急停、报警，避免硬件损坏或人员伤害。

// 电流环碰撞检测程序——无额外硬件的碰撞保护
#include <Filter.h>

// 硬件引脚定义
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
#define LEFT_CURRENT_PIN A0 // 左电机电流采样引脚（驱动板电流输出端）
#define RIGHT_CURRENT_PIN A1 // 右电机电流采样引脚
#define BUZZER_PIN 8        // 报警蜂鸣器引脚

// 电流环参数
const int ADC_RESOLUTION = 1024;    // Arduino ADC分辨率（10位）
const float REFERENCE_VOLTAGE = 5.0; // 参考电压（V）
const float SHUNT_RESISTOR = 0.1;    // 采样电阻（Ω，驱动板内置参数，需查询手册）
const float CURRENT_GAIN = 1.0;      // 电流增益（驱动板放大倍数，需校准）
const float COLLISION_CURRENT_THRESHOLD = 2.5; // 碰撞电流阈值（A，堵转电流阈值）
const float SMOOTH_FACTOR = 0.6;     // 电流滤波平滑因子（0~1，越小越平滑）
const int DEBOUNCE_CYCLES = 3;       // 去抖采样次数（连续3次超阈值判定碰撞）

// 电流与状态变量
float leftCurrent = 0.0;       // 左电机电流（A）
float rightCurrent = 0.0;      // 右电机电流（A）
float leftCurrentSmooth = 0.0; // 滤波后的左电流
float rightCurrentSmooth = 0.0;// 滤波后的右电流
bool leftCollision = false;    // 左电机碰撞标记
bool rightCollision = false;   // 右电机碰撞标记
int collisionDebounce = 0;     // 去抖计数器

// 电机基础速度（正常行驶）
int normalSpeed = 100;

// 电流采样函数：ADC值转实际电流（A）
float adcToCurrent(int adcValue) {
  const float voltage = (adcValue / (ADC_RESOLUTION * 1.0)) * REFERENCE_VOLTAGE;  const float current = (voltage / SHUNT_RESISTOR) / CURRENT_GAIN;
  return current;
}

// 电流滤波函数：平滑随机噪声
float filterCurrent(float current, float lastSmooth) {
  float smooth = lastSmooth * SMOOTH_FACTOR + current * (1 - SMOOTH_FACTOR);
  return smooth;
}

// 碰撞检测函数：对比阈值+去抖判定
void detectCollision() {
  // 电流阈值判定
  bool leftOverCurrent = (leftCurrentSmooth > COLLISION_CURRENT_THRESHOLD);
  bool rightOverCurrent = (rightCurrentSmooth > COLLISION_CURRENT_THRESHOLD);
  
  // 去抖计数：连续多次超阈值才判定碰撞，避免误触发
  if (leftOverCurrent && rightOverCurrent) {
    collisionDebounce++;
  } else {
    collisionDebounce = 0;
  }
  
  // 判定碰撞（去抖通过后）
  if (collisionDebounce >= DEBOUNCE_CYCLES) {
    leftCollision = true;
    rightCollision = true;
  } else { leftCollision = false;
    rightCollision = false;
  }
}

// 碰撞保护控制：急停+报警
void collisionProtection() {
  if (leftCollision || rightCollision) {
    // 急停电机
    analogWrite(LEFT_MOTOR_PWM, 0);
    analogWrite(RIGHT_MOTOR_PWM, 0);
    // 触发报警
    digitalWrite(BUZZER_PIN, HIGH);
  } else {
    // 正常行驶
    analogWrite(LEFT_MOTOR_PWM, normalSpeed);
    analogWrite(RIGHT_MOTOR_PWM, normalSpeed);
    digitalWrite(BUZZER_PIN, LOW);
  }
}

void setup() {
  Serial.begin(115200);
  
  // 电机引脚初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  
  // 电流采样引脚初始化
  pinMode(LEFT_CURRENT_PIN, INPUT);
  pinMode(RIGHT_CURRENT_PIN, INPUT);
  
  // 报警引脚初始化
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, HIGH);
  
  // 电机方向初始化（前进）
  digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, HIGH);
  
  Serial.println("电流环碰撞检测系统初始化完成...");
}

void loop() {
  // 1. 采集电流ADC值并转换为实际电流
  int leftAdc = analogRead(LEFT_CURRENT_PIN);
  int rightAdc = analogRead(RIGHT_CURRENT_PIN);
  leftCurrent = adcToCurrent(leftAdc);
  rightCurrent = adcToCurrent(rightAdc);
  
  // 2. 电流滤波
  leftCurrentSmooth = filterCurrent(leftCurrent, leftCurrentSmooth);
  rightCurrentSmooth = filterCurrent(rightCurrent, rightCurrentSmooth);
  
  // 3. 碰撞检测
  detectCollision();
  
  // 4. 碰撞保护  collisionProtection();
  
  // 5. 串口输出调试信息
  Serial.print("左电流[A]:");
  Serial.print(leftCurrentSmooth, 2);
  Serial.print(" 右电流[A]:");
  Serial.print(rightCurrentSmooth, 2);
  Serial.print(" 碰撞电流阈值[A]:");
  Serial.print(COLLISION_CURRENT_THRESHOLD, 2);
  Serial.print(" 去抖计数:");
  Serial.print(collisionDebounce);
  Serial.print(" 碰撞状态:");
  Serial.println((leftCollision || rightCollision) ? "是" : "否"); 
  delay(50); // 循环周期50ms，保证碰撞检测的实时性
}

程序优化方向
碰撞阈值可动态调整：结合电机转速，高速时阈值适当提高（惯性大、电流自然升高），低速时降低（堵转特征更明显）；
可加入电流变化率检测，识别“急停导致的电流尖峰”与“真实碰撞”的差异，进一步降低误触发；
多电机系统可独立检测每个电机的电流，精准定位碰撞位置（如仅左电机碰撞，可判定为左侧撞击）。

6、三冗余投票+电流环检测协同程序（核心：双重验证+安全保护）
该程序集成前两个案例的功能，将三冗余传感器的碰撞预警与电流环的碰撞检测结合，形成“感知预警→物理验证→分级保护”的双重安全机制，适用于高风险场景（如带载搬运、危险环境作业），避免单一保护机制失效导致事故。

核心逻辑
预警触发：三冗余传感器检测到障碍物，触发预警；
双重验证：当传感器预警时，通过电流环检测是否真的发生碰撞（避免传感器误判）；
分级保护：预警告警减速，确认碰撞立即急停，兼顾安全性与运行效率。

// 三冗余投票+电流环检测协同程序——双重安全保护
#include <Filter.h>

// ========== 硬件引脚定义 ==========
// 冗余传感器（前侧3路超声波）
#define SENSOR1_TRIG 22
#define SENSOR1_ECHO 23
#define SENSOR2_TRIG 24
#define SENSOR2_ECHO 25
#define SENSOR3_TRIG 26
#define SENSOR3_ECHO 27
// 电机控制
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
// 电流检测
#define LEFT_CURRENT_PIN A0
#define RIGHT_CURRENT_PIN A1
// 报警与指示
#define BUZZER_PIN 8
#define LED_WARNING_PIN 7#define LED_STOP_PIN 13

// ========== 三冗余传感器参数 ==========
const float SENSOR_DANGER_DIST = 0.3;
const float SENSOR_MAX_DIST = 2.0;
const float SENSOR_MIN_DIST = 0.05;
const float SENSOR_ERROR_DELTA = 0.4;
float sensorVal[3] = {0}, votedValue = 0;
bool sensorValid[3] = {false}, sensorWarning = false;

// ========== 电流环参数 ==========
const int ADC_RESOLUTION = 1024;
const float REFERENCE_VOLTAGE = 5.0;
const float SHUNT_RESISTOR = 0.1;
const float CURRENT_GAIN = 1.0;
const float COLLISION_CURRENT_THRESHOLD = 2.5;
const float SMOOTH_FACTOR = 0.6;
const int DEBOUNCE_CYCLES = 3;
float leftCurrentSmooth = 0, rightCurrentSmooth = 0;
bool currentCollision = false;
int collisionDebounce = 0;

// ========== 协同控制参数 ==========
const int SLOW_SPEED = 50;  // 预警减速速度
int normalSpeed = 100;     // 正常行驶速度

// ========== 函数声明 ==========
float getUltrasonicDistance(int trig, int echo);
void threeVoteFusion();
float adcToCurrent(int adc);
void detectCurrentCollision();
void collaborativeControl();

void setup() {
  Serial.begin(115200);
  Serial.println("三冗余+电流环协同保护系统初始化完成..."); // 传感器初始化
  for (int i = 0; i < 3; i++) {
    int trigPin = SENSOR1_TRIG +2*i;
    int echoPin = SENSOR1_ECHO + 2*i;
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);
  }
  // 电机初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, HIGH); // 电流采样初始化
  pinMode(LEFT_CURRENT_PIN, INPUT);
  pinMode(RIGHT_CURRENT_PIN, INPUT); // 指示与报警初始化
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_WARNING_PIN, OUTPUT);
  pinMode(LED_STOP_PIN, OUTPUT);
  digitalWrite(LED_WARNING_PIN, LOW);
  digitalWrite(LED_STOP_PIN, LOW);
}

// 超声波测距
float getUltrasonicDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 30000);
  float distanceCM = (duration * 0.0343) / 2;
  if (distanceCM <= SENSOR_MIN_DIST * 100 || distanceCM > SENSOR_MAX_DIST * 100) {
    return SENSOR_MAX_DIST + 1;
  }
  return distanceCM / 100.0;
}

// 三冗余投票融合（与案例1一致）
void threeVoteFusion() {
  // 采集数据
  sensorVal[0] = getUltrasonicDistance(SENSOR1_TRIG, SENSOR1_ECHO);
  sensorVal[1] = getUltrasonicDistance(SENSOR2_TRIG, SENSOR2_ECHO);
  sensorVal[2] = getUltrasonicDistance(SENSOR3_TRIG, SENSOR3_ECHO); // 有效性检测
  for (int i = 0; i < 3; i++) {
    sensorValid[i] = !(sensorVal[i] > SENSOR_MAX_DIST || sensorVal[i] < SENSOR_MIN_DIST);
  }
  
  // 投票逻辑
  int validCount = sensorValid[0] + sensorValid[1] + sensorValid[2];
  if (validCount == 0) {
    votedValue = SENSOR_MAX_DIST;
    sensorWarning = true;
return;
  }
  if (validCount == 1) {
    for (int i = 0; i < 3; i++) {
      if (sensorValid[i]) {
        votedValue = sensorVal[i];
        break;
      }
    }
  }
  if (validCount >= 2) {
    float delta12 = abs(sensorVal[0] - sensorVal[1]);
    float delta13 = abs(sensorVal[0] - sensorVal[2]);
    float delta23 = abs(sensorVal[1] - sensorVal[2]);
    if (delta12 <= SENSOR_ERROR_DELTA && delta13 <= SENSOR_ERROR_DELTA && delta23 <= SENSOR_ERROR_DELTA) {
      votedValue = (sensorVal[0] + sensorVal[1] + sensorVal[2]) / 3;} else {
      int faultIndex = -1;
      if (delta12 > SENSOR_ERROR_DELTA && delta13 > SENSOR_ERROR_DELTA) faultIndex = 0;
      else if (delta12 > SENSOR_ERROR_DELTA && delta23 > SENSOR_ERROR_DELTA) faultIndex = 1;
      else if (delta13 > SENSOR_ERROR_DELTA && delta23 > SENSOR_ERROR_DELTA) faultIndex = 2;
      if (faultIndex != -1) {
        sensorValid[faultIndex] = false;
        votedValue = (sensorVal[(faultIndex + 1) % 3] + sensorVal[(faultIndex + 2) % 3]) / 2;
      } else {
        votedValue = SENSOR_MAX_DIST;
      }
    }
  }
  
  // 传感器预警：距离低于危险阈值
  sensorWarning = (votedValue <= SENSOR_DANGER_DIST);
}

// ADC转电流
float adcToCurrent(int adc) {
  float voltage = (adc / (ADC_RESOLUTION * 1.0)) * REFERENCE_VOLTAGE;
  return (voltage / SHUNT_RESISTOR) / CURRENT_GAIN;
}

// 电流环碰撞检测
void detectCurrentCollision() {
  // 采集电流
  float leftCurrent = adcToCurrent(analogRead(LEFT_CURRENT_PIN));
  float rightCurrent = adcToCurrent(analogRead(RIGHT_CURRENT_PIN));
  
  // 滤波
  leftCurrentSmooth = leftCurrentSmooth * SMOOTH_FACTOR + leftCurrent * (1 - SMOOTH_FACTOR);
  rightCurrentSmooth = rightCurrentSmooth * SMOOTH_FACTOR + rightCurrent * (1 - SMOOTH_FACTOR);
  
  // 阈值判定与去抖
  bool overCurrent = (leftCurrentSmooth >= COLLISION_CURRENT_THRESHOLD) || (rightCurrentSmooth >= COLLISION_CURRENT_THRESHOLD);
  if (overCurrent) {
    collisionDebounce++;
  } else {
    collisionDebounce = 0;
  }
  
  currentCollision = (collisionDebounce >= DEBOUNCE_CYCLES);
}

// 协同控制：分级保护
void collaborativeControl() {
  if (currentCollision) {
    // 优先级1：电流环确认碰撞，立即急停
    analogWrite(LEFT_MOTOR_PWM, 0);
    analogWrite(RIGHT_MOTOR_PWM, 0);
    digitalWrite(LED_STOP_PIN, HIGH);
    digitalWrite(LED_WARNING_PIN, LOW);
    digitalWrite(BUZZER_PIN, HIGH);
  } else if (sensorWarning) {
    // 优先级2：传感器预警，减速行驶+告警
    analogWrite(LEFT_MOTOR_PWM, SLOW_SPEED);
    analogWrite(RIGHT_MOTOR_PWM, SLOW_SPEED);
    digitalWrite(LED_WARNING_PIN, HIGH);
    digitalWrite(LED_STOP_PIN, LOW);
    digitalWrite(BUZZER_PIN, LOW); // 蜂鸣器间歇告警（可扩展）
  } else {
    // 优先级3：无风险，正常行驶
    analogWrite(LEFT_MOTOR_PWM, normalSpeed);
    analogWrite(RIGHT_MOTOR_PWM, normalSpeed);
    digitalWrite(LED_WARNING_PIN, LOW);
    digitalWrite(LED_STOP_PIN, LOW);
    digitalWrite(BUZZER_PIN, LOW);
  }
}

void loop() {
  // 1. 三冗余传感器投票，输出预警
  threeVoteFusion();
  
  // 2. 电流环检测，输出碰撞确认
  detectCurrentCollision();
  
  // 3. 协同控制，分级保护
  collaborativeControl();
  
  // 4. 调试输出
  Serial.print("投票值[m]:");
  Serial.print(votedValue, 2);
  Serial.print(" 传感器预警:");
  Serial.print(sensorWarning?'是':'否');
  Serial.print(" 电流碰撞:");
  Serial.print(currentCollision?'是':'否');
  Serial.print(" 平均电流[A]:");
  Serial.println((leftCurrentSmooth + rightCurrentSmooth)/2, 2);
  
 delay(50);
}

程序优化方向
可加入“传感器失效时，电流环阈值降低”逻辑：传感器失效后，依赖电流环更早触发保护，弥补感知盲区；
支持远程报警：碰撞触发后，通过串口或WiFi向上位机发送警报，实现远程监控；
记录碰撞数据：存储碰撞时的电流、传感器数值，便于后续故障分析与参数优化。

要点解读
要点1：冗余传感器的故障识别准则——三冗余的基础
三冗余的核心是“识别故障而非仅融合数据”，需先剔除异常传感器，再做数据融合，关键把握三点：
量程校验：传感器数值超出自身量程（如超声波测距超2m、低于5cm）直接判定故障，避免无效数据参与投票；
一致性校验：通过两两偏差判断传感器是否一致，偏差超过阈值时，认定少数传感器为故障，采用多数的平均值；
保守容错原则：当多个传感器故障或一致性极差时，采用保守策略（取最大距离、触发预警），确保安全优先，而非追求最优行驶。
要点2：电流环碰撞检测的阈值校准——检测准确的关键
电流阈值是碰撞检测的核心参数，校准精度直接影响碰撞识别的准确性，需分两步校准：
基准电流校准：采集机器人正常行驶（不同速度）下的电流，取最大值作为“正常电流上限”，碰撞阈值需高于该上限，避免误触发；
堵转电流校准：人为触发电机堵转（模拟碰撞），采集稳定后的电流，将阈值设为“堵转电流的70%~80%”，确保碰撞时能快速响应；
动态调整：碰撞阈值可与速度联动，高速时适当提高（惯性导致电流自然升高），低速时降低（堵转特征更明显），案例2中默认固定阈值，可扩展为动态参数。
要点3：协同机制的优先级设计——双重保护的核心
三冗余与电流环协同的核心是“分级保护，安全落地”，需明确优先级，避免逻辑冲突：
电流环优先级最高：电流检测反映真实的机械碰撞，是物理层面的硬保护，一旦触发必须立即急停，无需额外验证；
传感器预警优先级次之：传感器预警是提前量，只需减速告警，避免直接停车导致运行效率大幅降低；
互补性要求：传感器失效时，电流环可独立工作；电流环阈值设置不合理时，传感器可提前预警，两者互补覆盖所有故障场景。
要点4：数据采样的同步与滤波——稳定性的保障
采样的同步性与数据滤波是减少误判的关键，需重点关注：
三冗余传感器同步采样：3路传感器需在同一时间窗口采集，避免因时间差导致的数据偏差（如机器人移动中，采样时间不同会导致距离差异）；
电流采样滤波：采用滑动平均或指数滤波，滤除ADC采样的随机噪声与电机电流波动，避免误判；
去抖机制：碰撞检测需连续多次超阈值才判定，传感器投票可加入多次采样平均，减少瞬时干扰的影响。
要点5：安全机制的落地边界——实用性的前提
任何安全机制都需考虑实际落地的边界条件，避免逻辑脱离硬件能力：
硬件能力匹配：电流采样需驱动板支持电流输出，若为简易驱动，需外接电流传感器，不可强行接线；
检测延迟约束：电流环与传感器的响应延迟需满足安全要求，案例中电流检测周期50ms、传感器100ms，可确保碰撞发生后快速响应；
场景适配：高风险场景（带载/危险环境）可提高传感器危险阈值（提前减速）、降低电流阈值（更早急停），普通场景可适当放宽，平衡安全性与效率。

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