在机器人控制算法与底层硬件协同的视角下，Arduino BLDC（无刷直流电机）在强电磁环境中的“泄漏源自主定位与精准采样”是一个典型的软硬件深度耦合课题。这不仅要求电机具备高动态响应，更考验系统在极端电磁噪声下的信号完整性与闭环控制鲁棒性。
以下是针对该系统的专业解析：

主要特点
底层驱动的抗干扰闭环控制
在强电磁环境下，BLDC的换相极易受到干扰导致失步。系统通常采用FOC（磁场定向控制）或六步换相法，结合高共模抑制比（CMRR）的差分运放进行电流采样。在算法层面，通过中心对齐PWM模式，在PWM计数器峰值（避开开关瞬态和死区时间）触发ADC采样，从控制时序上规避dv/dt峰值干扰。
多模态感知与三维浓度场重构
机器人搭载异构传感器（如漏磁信号阵列、电化学或红外吸收光谱传感器），在自主导航（SLAM）的过程中，将采集到的微弱磁场或气体浓度数据与实时三维空间坐标绑定。通过后端算法构建泄漏源的三维分布图，实现从“发现异常”到“精准定位”的跨越。
非侵入式与自适应采样策略
针对漏磁等物理场，采用外部传感器阵列进行“无创”检测。为了应对动态环境，系统往往采用分层控制架构：底层MCU负责高频的电机PID闭环与紧急避障，上层负责路径规划与数据融合；在低采样率下，还会引入预测控制器（Predictive Controller）进行开环运动预判，确保采样过程的连续性。

应用场景
化工园区与地下管廊巡检
在充满变频器、大功率继电器等强电干扰的化工厂或地下管廊，机器人可替代人工进入受限空间，对甲烷、硫化氢等有毒有害气体或管道微小泄漏点进行7×24小时自主侦测与三维建模。
核设施与高辐射环境应急响应
在核事故废墟或反应堆厂房内，机器人利用BLDC底盘的高扭矩密度与长寿命特性，搭载辐射探测器自主绘制辐射剂量热力图，精准定位放射源，避免人员受到高剂量辐射。
工业电机与电力设备预测性维护
在大型工业现场，利用搭载高精度磁传感器的巡检机器人，对运行中的大功率电机、变压器进行外部漏磁信号阵列检测，通过AI模型识别定子绕组匝间短路等早期故障，实现非接触式的“健康体检”。

需要注意的关键事项
严苛的电源隔离与EMC（电磁兼容）设计
BLDC电机在启停和高频PWM换向时会产生巨大的电流冲击和宽频谱噪声。必须为Arduino主控及敏感传感器提供独立的LDO或DC-DC隔离电源，严禁与电机共用；信号线需远离动力线，并在电源端加装π型滤波电路或磁珠，防止地环路和共模干扰导致数据跳变。
PCB布局与信号完整性
在硬件设计上，应全面采用贴片封装（SMT）以消除长引脚带来的“天线效应”。模拟地、数字地、功率地必须严格分割并单点接地；微弱的采样线路需进行精准的阻抗匹配与等长布线，从物理源头切断噪声耦合路径。
算力瓶颈与分布式架构
多路传感器阵列的数据解算、EKF（扩展卡尔曼滤波）融合以及复杂的运动学逆解对算力要求极高。标准Arduino极易因内存溢出或浮点运算过载而崩溃。建议采用“上位机（如树莓派/工控机）负责解算与调度 + Arduino底层负责BLDC高频控制”的分布式异构架构。
安全机制与容错设计
在强干扰下，传感器数据可能会出现突变或完全丢失。系统必须设计完善的容错逻辑：当检测到定位数据异常（如距离突变超出物理极限）时，底层控制算法需立即接管，自动触发减速或硬件急停机制，防止机器人失控造成二次破坏。

1、卡尔曼滤波 + 气体浓度梯度追踪
此案例聚焦于抗干扰感知层。在强电磁环境下，气体传感器的模拟信号极易被电机换相产生的噪声污染。通过卡尔曼滤波对原始信号进行降噪，并结合航位推算实现浓度梯度追踪，是构建稳定“嗅觉”的基础。

#include <SimpleFOC.h>
#include <Wire.h>
#include <Kalman.h> // 使用简单卡尔曼滤波库

// BLDC电机定义 (差速底盘)
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM drvL = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM drvR = BLDCDriver3PWM(5, 6, 8);

// 气体传感器引脚 (模拟输入)
#define GAS_SENSOR_PIN A0
#define ENC_LEFT_PIN 2   // 编码器用于里程计
#define ENC_RIGHT_PIN 3

// 卡尔曼滤波器实例
KalmanFilter kf_gas(0.01, 1.0, 0.001); // Q, R, P初始值

// 系统状态
float gasConcentration = 0.0;
float prevConcentration = 0.0;
float posX = 0.0, posY = 0.0;
float heading = 0.0;

void setup() {
  Serial.begin(115200);
  
  // 初始化电机及FOC
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  
  // 初始化编码器
  pinMode(ENC_LEFT_PIN, INPUT_PULLUP);
  pinMode(ENC_RIGHT_PIN, INPUT_PULLUP);
}

void loop() {
  // 1. 传感器数据采集与卡尔曼滤波 (抗电磁干扰)
  float rawGas = analogRead(GAS_SENSOR_PIN);
  gasConcentration = kf_gas.update(rawGas); // 滤波后的浓度值
  
  // 2. 简单航位推算 (编码器累计里程)
  // 读取左右编码器脉冲数，换算为距离增量
  float leftDist = getEncoderDistance(LEFT); 
  float rightDist = getEncoderDistance(RIGHT);
  float deltaDist = (leftDist + rightDist) / 2.0;
  float deltaTheta = (rightDist - leftDist) / WHEEL_BASE;
  
  posX += deltaDist * cos(heading);
  posY += deltaDist * sin(heading);
  heading += deltaTheta;

  // 3. 浓度梯度追踪算法 (核心)
  float concentrationGradient = gasConcentration - prevConcentration;
  prevConcentration = gasConcentration;
  
  // 若浓度上升，则继续当前方向；否则随机转向或按梯度方向调整
  if (concentrationGradient > 0.2) {
    // 浓度上升，保持或微调方向
    motorL.target = BASE_SPEED;
    motorR.target = BASE_SPEED;
  } else {
    // 浓度下降，原地旋转搜寻 (模拟“嗅探”行为)
    motorL.target = -BASE_SPEED * 0.5;
    motorR.target = BASE_SPEED * 0.5;
    delay(200); // 旋转并重新采样
  }

  // 4. 更新FOC
  motorL.move(motorL.target);
  motorR.move(motorR.target);
  motorL.loopFOC();
  motorR.loopFOC();

  // 5. 数据上传至上位机 (用于建图)
  Serial.print(gasConcentration); Serial.print(",");
  Serial.print(posX); Serial.print(","); Serial.println(posY);
  
  delay(50);
}

要点：卡尔曼滤波能有效抑制电机PWM换相产生的高频噪声，同时将滤波后的浓度值与位置坐标绑定，为上位机绘制气体分布热力图提供干净的数据源。

2、自适应中值滤波 + 安全隔离控制
此案例在感知端采用三冗余传感器和自适应中值滤波以增强系统容错；在运动控制端，当检测到泄漏浓度超标时，自动降低电机功率并切换至安全模式，防止电机产生电火花引爆可燃气体。

#include <SimpleFOC.h>

// 定义三个冗余的气体传感器
#define SENSOR_1 A0
#define SENSOR_2 A1
#define SENSOR_3 A2

// BLDC电机及隔离驱动 (假设驱动自带隔离)
IsolatedBLDC motorL = IsolatedBLDC(7);
IsolatedBLDC motorR = IsolatedBLDC(8);
IsolatedDriver drvL = IsolatedDriver(9, 10, 11, 12); // 带隔离引脚
IsolatedDriver drvR = IsolatedDriver(5, 6, 7, 13);

// 历史数据窗口 (用于中值滤波)
#define WINDOW_SIZE 20
float history[WINDOW_SIZE];

void setup() {
  Serial.begin(115200);
  
  // 初始化隔离驱动 (重点关注安全)
  drvL.initIsolated();
  drvR.initIsolated();
  motorL.linkDriver(&drvL);
  motorR.linkDriver(&drvR);
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  
  // 安全优先的PID参数 (弱刚性，避免过冲)
  motorL.PID_velocity.P = 0.3;
  motorL.PID_velocity.I = 3.0;
  motorR.PID_velocity.P = 0.3;
  motorR.PID_velocity.I = 3.0;
}

void loop() {
  // 1. 三冗余传感器投票 & 自适应中值滤波
  float s1 = analogRead(SENSOR_1);
  float s2 = analogRead(SENSOR_2);
  float s3 = analogRead(SENSOR_3);
  
  // 三值投票：取中位值作为有效读数 (剔除异常)
  float validRaw = median(s1, s2, s3);
  
  // 更新滑动窗口并计算中值
  for (int i = WINDOW_SIZE-1; i > 0; i--) {
    history[i] = history[i-1];
  }
  history[0] = validRaw;
  float filteredConcentration = median(history, WINDOW_SIZE);

  // 2. 安全状态机 (防爆控制)
  if (filteredConcentration > LEAK_THRESHOLD_HIGH) {
    // 高浓度泄漏: 立即急停并切换至防爆模式
    motorL.setPowerLimit(0.3);  // 限制功率，防电火花
    motorR.setPowerLimit(0.3);
    motorL.switchToSafeMode();
    motorR.switchToSafeMode();
    // 发送警报并开始撤离
    Serial.println("CRITICAL LEAK DETECTED! EVACUATING!");
    motorL.target = -50; 
    motorR.target = -50; // 后退撤离
  } else {
    // 正常浓度: 恢复正常工作模式
    motorL.setPowerLimit(1.0);
    motorR.setPowerLimit(1.0);
    // 执行正常的追踪或巡检逻辑
    // ...
  }

  // 3. 更新FOC (带隔离保护)
  motorL.move(motorL.target);
  motorR.move(motorR.target);
  motorL.loopFOC();
  motorR.loopFOC();

  delay(50);
}

// 取中位数的辅助函数
float median(float arr[], int n) { /* 实现排序取中值... */ }

要点：自适应中值滤波能去除传感器由于电磁干扰产生的尖峰脉冲；而隔离驱动和功率限制是在易燃易爆环境中保障系统本质安全的关键措施。

3、多传感器互补滤波 + 自主路径规划
此案例融合多传感器定位（里程计+IMU）和避障导航，使机器人在追踪泄漏源的同时避开废墟或障碍，实现“边排雷、边前进”的智能侦查。

#include <SimpleFOC.h>
#include <MPU6050.h>
#include <NewPing.h> // 超声波避障

MPU6050 mpu;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DIST);

// 互补滤波融合航向角 (IMU+磁力计或陀螺仪积分)
float fusionHeading = 0.0;
unsigned long lastTime = 0;

void setup() {
  // ... 初始化电机、IMU、超声波 ...
  Wire.begin();
  mpu.initialize();
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
}

void loop() {
  // 1. 多传感器数据融合 (互补滤波)
  // 读取IMU角速度
  int16_t gx, gy, gz;
  mpu.getRotation(&gx, &gy, &gz);
  float dt = (micros() - lastTime) / 1000000.0;
  lastTime = micros();
  
  // 陀螺仪积分角度
  float gyroAngle = fusionHeading + gz * dt / 131.0; // 灵敏度系数
  
  // 加速度计计算角度 (只能测倾角，这里仅为示例)
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  float accelAngle = atan2(ay, az) * 180 / PI;
  
  // 互补滤波 (抗电磁干扰强)
  fusionHeading = 0.98 * gyroAngle + 0.02 * accelAngle;

  // 2. 自主导航逻辑 (泄漏源追踪 + 避障)
  // 2.1 超声波检测前方障碍
  int obstacleDist = sonar.ping_cm();
  
  // 2.2 决策: 避障优先
  if (obstacleDist > 0 && obstacleDist < 30) {
    // 有障碍: 执行绕行 (右转)
    motorL.target = 80;
    motorR.target = -80;
  } else {
    // 无障碍: 执行浓度梯度追踪 (复用案例一逻辑)
    // 但此处用融合后的fusionHeading更新航位推算
    // ...
  }

  // 3. 更新FOC
  motorL.move(motorL.target);
  motorR.move(motorR.target);
  motorL.loopFOC();
  motorR.loopFOC();

  delay(20);
}

要点：互补滤波通过融合陀螺仪的短期高精度和加速度计的长期稳定性，能在电机强磁干扰下提供稳健的姿态角；而避障与追踪的优先级策略保证了机器人在复杂废墟环境中的生存能力。

要点解读
电磁兼容（EMC）设计决定系统生死：BLDC电机的PWM驱动是强干扰源，必须进行严格的信号与功率隔离。建议传感器与主控共地需谨慎，可采用磁耦隔离芯片，并将传感器供电与电机供电完全分离。

感知滤波是“抗干扰”的核心：在强电磁环境下，单一传感器极易饱和或数据跳变。必须构建多层级滤波：底层用硬件RC滤波抑制高频噪声；软件层面使用卡尔曼滤波或滑动平均/中值滤波，并根据电机转速动态调整滤波器参数。

泄漏源定位需“动态重规划”：气体泄漏方向受气流影响多变。单纯的静态浓度梯度容易失效。工程实践建议采用A*全局规划 + DWA局部重规划的架构：当浓度梯度丢失时，触发局部重规划模块，生成“蜿蜒搜索”轨迹，避免陷入局部极值。

安全机制应独立于主控逻辑：在化学泄漏现场，防爆是底线。程序架构上，浓度阈值中断的优先级应高于路径规划和跟随控制。一旦检测到超标，可直接通过硬件中断或独立看门狗触发电机功率封顶（Power Limit），强制降低输出力矩以杜绝电火花风险。

分布式架构是算力富余的保障：强电磁环境下的SLAM建图、浓度热力图绘制和路径重规划计算量极大。标准Arduino仅适合执行底层FOC电机控制与传感器滤波。强烈建议采用 “树莓派/Jetson(规划+感知) + Arduino(执行)” 的异构架构，利用ESP32的双核亦可实现任务分离（Core 0处理传感器，Core 1控制电机）。

4、工业高压气体泄漏巡检机器人——模糊抗干扰避障+泄漏源定位采样
适用场景：变电站、工业气体输送站等强电磁环境，核心需求是机器人在高压电磁干扰下，自主规避高压设备、气体管道等障碍物，精准追踪气体泄漏源，完成泄漏气体采样（如SF₆、天然气）。

核心逻辑：采用模糊逻辑抗干扰避障+气体浓度梯度定位双闭环策略——外环用模糊逻辑融合多传感器信号，在强电磁干扰下维持路径稳定性，规避障碍物；内环通过气体传感器检测浓度梯度，输出泄漏源方向偏差，结合模糊控制调节BLDC电机差速，实现泄漏源精准逼近与采样。传感器采用硬件滤波与软件均值滤波，抑制电磁干扰。

/* ===== 工业高压气体泄漏巡检机器人：模糊避障+浓度定位采样 =====
 * 硬件：Arduino Mega + BLDC差速底盘 + 气体传感器 + 超声波避障 + 电磁屏蔽电路
 * 核心：模糊抗干扰避障 + 气体浓度梯度定位 + 采样臂闭环控制
 */
#include <SimpleFOC.h>

// ==================== 硬件驱动初始化 ====================
BLDCMotor motorL(7), motorR(7);       // 左右驱动轮
BLDCDriver3PWM driverL(9,10,11,8);    // 左电机驱动
BLDCDriver3PWM driverR(3,5,6,7);      // 右电机驱动
Encoder encL(18,19,2048), encR(20,21,2048); // 轮速编码器

// 传感器接口（含电磁防护滤波电路）
const int gasPin = A0;    // 气体传感器（模拟输出，浓度越高电压越高）
const int sonarPin = 2;   // 超声波避障（前端）
const int leftSonar = 3;  // 左侧超声波
const int rightSonar = 4; // 右侧超声波
const int samplerPin = 5; // 采样臂舵机控制

// ==================== 模糊控制参数 ====================
#define MAX_SPEED 1.0      // BLDC最大转速（rad/s）
#define OBSTACLE_SAFE 15   // 避障安全距离（cm）
#define LEAK_CONC 300      // 泄漏触发浓度阈值

// 模糊变量域：避障距离（近/中/远）、浓度梯度（负/零/正）
// 输出：左轮速、右轮速

void setup() {
    Serial.begin(115200);
    // BLDC电机初始化
    motorL.linkSensor(&encL); motorL.linkDriver(&driverL);
    motorR.linkSensor(&encR); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 采样臂初始化
    pinMode(samplerPin, OUTPUT);
    digitalWrite(samplerPin, LOW);
}

// ==================== 抗干扰传感器滤波（均值+硬件滤波后处理） ====================
float filteredSensor(int pin, int sampleCnt=5) {
    int vals[sampleCnt];
    int sum = 0;
    for (int i=0; i<sampleCnt; i++) {
        vals[i] = analogRead(pin);
        delay(2);
    }
    for (int i=1; i<sampleCnt-1; i++) { // 剔除异常值
        if (vals[i] > vals[i-1] && vals[i] > vals[i+1]) continue;
        sum += vals[i];
    }
    return (float)sum / (sampleCnt-2);
}

// ==================== 模糊抗干扰避障 + 浓度定位控制核心 ====================
void fuzzyLeakTracking(float &leftSpd, float &rightSpd) {
    // 抗干扰传感器采样
    float frontDist = filteredSensor(sonarPin);
    float leftDist = filteredSensor(leftSonar);
    float rightDist = filteredSensor(rightSonar);
    float conc = filteredSensor(gasPin);
    
    // 浓度梯度计算（泄漏源在浓度快速升高的方向）
    static float lastConc = 0;
    float concGrad = conc - lastConc;
    lastConc = conc;
    
    // 模糊输入：避障距离（近/中/远）、浓度梯度（负/零/正）
    int obsState;
    if (frontDist < OBSTACLE_SAFE*3) {
        obsState = (frontDist < OBSTACLE_SAFE ? 0 : (frontDist < OBSTACLE_SAFE*2 ? 1 : 2)); // 0=近,1=中,2=远
    } else {
        obsState = 2; // 无障碍
    }
    int gradState = (concGrad > 50 ? 2 : (concGrad < -50 ? 0 : 1)); // 0=负梯度,1=零,2=正梯度
    
    // 模糊规则库（优先级：避障>定位）
    if (obsState == 0) { // 近障碍：优先避障
        if (leftDist < rightDist) {
            leftSpd = 0.3; rightSpd = 0.8; // 右转避障
        } else {
            leftSpd = 0.8; rightSpd = 0.3; // 左转避障
        }
        return;
    }
    
    // 无障碍：浓度梯度定位
    if (gradState == 2) { // 正梯度（向泄漏源靠近）
        leftSpd = MAX_SPEED; rightSpd = MAX_SPEED; // 直行逼近
    } else if (gradState == 0) { // 负梯度（远离泄漏源）
        if (leftDist < rightDist) {
            leftSpd = 0.4; rightSpd = 0.9; // 右转扫描
        } else {
            leftSpd = 0.9; rightSpd = 0.4; // 左转扫描
        }
    } else { // 零梯度：调整方向
        if (frontDist < OBSTACLE_SAFE*2.5) {
            leftSpd = 0.6; rightSpd = 0.6; // 减速试探
        } else {
            leftSpd = 0.7; rightSpd = 0.7; // 恒速探索
        }
    }
}

// ==================== 精准采样控制 ====================
void executeSampling() {
    static bool sampling = false;
    if (analogRead(gasPin) > LEAK_CONC && !sampling) {
        digitalWrite(samplerPin, HIGH); // 触发采样
        delay(1000);
        digitalWrite(samplerPin, LOW);  // 收回采样臂
        sampling = true;
        delay(5000); // 采样间隔
        sampling = false;
    }
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 模糊避障+泄漏源定位
    float leftSpd, rightSpd;
    fuzzyLeakTracking(leftSpd, rightSpd);
    
    // 2. BLDC电机执行
    motorL.move(leftSpd);
    motorR.move(rightSpd);
    
    // 3. 精准采样
    executeSampling();
    
    // 调试输出
    Serial.print("Conc:"); Serial.print(analogRead(gasPin));
    Serial.print(" LSpd:"); Serial.print(leftSpd);
    Serial.print(" RSpd:"); Serial.println(rightSpd);
    
    delay(30);
}

5、变电站SF₆泄漏巡检机器人——强电磁屏蔽+自适应路径规划采样
适用场景：变电站配电房、GIS设备区等强电磁环境，泄漏源多为高压设备密封失效导致的SF₆气体泄漏，机器人需在强电磁辐射下稳定导航，精准定位泄漏点并完成采样。

核心逻辑：采用电磁屏蔽硬件+自适应路径规划——硬件层通过电源隔离、信号滤波、电磁屏蔽壳，降低电磁干扰对传感器与主控的影响；算法层通过自适应卡尔曼滤波融合轮式里程计与泄漏源方位角，实时修正定位误差，结合动态窗口法（DWA）规划最优路径，调节BLDC电机速度与转向，实现泄漏源的快速锁定与精准采样。

/* ===== 变电站SF₆泄漏巡检机器人：强电磁屏蔽+自适应路径规划 =====
 * 硬件：Arduino Due（主控）+ BLDC底盘 + 电磁屏蔽壳 + SF₆传感器 + 角度传感器
 * 核心：自适应卡尔曼滤波抗漂移 + DWA路径规划 + BLDC精准跟随
 */
#include <SimpleFOC.h>

// ==================== 硬件初始化 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// 传感器接口（电磁屏蔽设计）
const int sf6Pin = A0;       // SF₆传感器（电磁屏蔽罩内安装）
const int anglePin = A1;     // 泄漏源方位角传感器
const int compassPin = A2;   // 电磁兼容磁罗盘

// ==================== 自适应卡尔曼滤波抗定位漂移 ====================
struct KalmanFilter {
    float x, y, theta; // 位置与航向角
    float P[3][3];     // 协方差矩阵
    float Q, R;        // 过程/观测噪声
    
    KalmanFilter() {
        x=y=theta=0;
        P = {{0.1,0,0},{0,0.1,0},{0,0,0.1}};
        Q = 0.01; R = 0.1;
    }
    
    // 预测（基于轮式里程计）
    void predict(float dx, float dy, float dTheta) {
        x += dx; y += dy; theta += dTheta;
        // 更新协方差
        P[0][0] += Q; P[1][1] += Q; P[2][2] += Q;
    }
    
    // 更新（融合方位角观测）
    void update(float obsTheta) {
        float y = obsTheta - theta; // 观测残差
        float K = P[2][2] / (P[2][2] + R); // 卡尔曼增益
        theta += K * y;
        P[2][2] *= (1 - K);
    }
};

KalmanFilter kf;
float odoX=0, odoY=0, odoTheta=0; // 里程计推算

// ==================== DWA路径规划（抗干扰动态避障） ====================
void dwaPlanning(float &vx, float &wz, float targetTheta, float targetConc) {
    // 状态变量：位置(x,y)、航向角theta、线速度vx、角速度wz
    static float vx_cmd=0, wz_cmd=0;
    float maxV = 0.8, maxW = 1.0; // BLDC运动限制
    
    // 1. 采样候选速度（自适应步长，强电磁下简化）
    float vCandidates[] = {-0.2, 0, 0.2, 0.5, maxV};
    float wCandidates[] = {-maxW, -0.5, 0, 0.5, maxW};
    
    // 2. 评估函数：朝向目标角度+靠近高浓度
    float bestScore = -1e9;
    for (auto v : vCandidates) {
        for (auto w : wCandidates) {
            // 模拟下一步航向：theta + w*dt
            float nextTheta = odoTheta + w*0.1;
            float thetaErr = fabs(nextTheta - targetTheta);
            // 模拟下一步浓度：基于距离的近似（简化）
            float dist = fabs(odoX - targetConc/10);
            float score = -thetaErr - dist;
            if (score > bestScore) {
                bestScore = score;
                vx_cmd = v; wz_cmd = w;
            }
        }
    }
    
    // 3. 输出控制量
    vx = constrain(vx_cmd, -maxV, maxV);
    wz = constrain(wz_cmd, -maxW, maxW);
}

// ==================== 自适应控制执行 ====================
void adaptiveControl() {
    // 1. 抗干扰传感器采样
    float conc = analogRead(sf6Pin) * 0.01;
    float targetTheta = analogRead(anglePin) * 3.1416 / 180.0; // 方位角转弧度
    
    // 2. 卡尔曼滤波更新定位
    float dx = 0.1 * cos(odoTheta);
    float dy = 0.1 * sin(odoTheta);
    kf.predict(dx, dy, 0.01);
    kf.update(targetTheta);
    odoX += dx; odoY += dy; odoTheta = kf.theta;
    
    // 3. DWA规划速度
    float vx, wz;
    dwaPlanning(vx, wz, targetTheta, conc);
    
    // 4. 差速BLDC控制解算
    float wheelBase = 0.25;
    float leftSpd = vx - wz * wheelBase/2;
    float rightSpd = vx + wz * wheelBase/2;
    
    // 限幅（强电磁下抑制速度跳变）
    leftSpd = constrain(leftSpd, -MAX_SPEED, MAX_SPEED);
    rightSpd = constrain(rightSpd, -MAX_SPEED, MAX_SPEED);
    
    // 执行
    motorL.move(leftSpd);
    motorR.move(rightSpd);
    
    // 浓度达标触发采样
    if (conc > 3.0) {
        // 扩展口触发采样装置（示例）
        digitalWrite(6, HIGH);
        delay(500);
        digitalWrite(6, LOW);
    }
}

void setup() {
    Serial.begin(115200);
    // BLDC初始化
    motorL.linkSensor(&encL); motorL.linkDriver(&driverL);
    motorR.linkSensor(&encR); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 采样口初始化
    pinMode(6, OUTPUT);
    digitalWrite(6, LOW);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    adaptiveControl();
    
    // 调试输出
    Serial.print("X:");Serial.print(odoX);
    Serial.print(" Theta:");Serial.print(odoTheta);
    Serial.print(" Conc:");Serial.println(analogRead(sf6Pin));
    
    delay(50);
}

6、输油管道泄漏水下巡检机器人——强电磁+复杂流场抗干扰采样
适用场景：输油管道水下泄漏检测，机器人需在水下强电磁（管道杂散电流）与复杂流场（涡流、暗流）中，通过声呐与姿态传感器定位泄漏点，对抗水流干扰，实现精准采样。

核心逻辑：采用抗干扰传感器融合+闭环姿态控制——传感器通过硬件滤波抑制管道杂散电流干扰，姿态与深度传感器实时修正机器人姿态；控制层基于模糊自适应PID调节BLDC推进器推力，抵消流场干扰，结合泄漏源方位角与距离，实现稳定接近与精准采样，采样完成后通过闭环控制回归回收路径。

/* ===== 输油管道泄漏水下巡检机器人：强电磁+流场抗干扰采样 =====
 * 硬件：Arduino Mega + BLDC水下推进器 + 声呐定位 + 姿态传感器 + 水下采样器
 * 核心：抗干扰滤波 + 模糊自适应PID姿态控制 + 闭环路径跟踪
 */
#include <SimpleFOC.h>

// ==================== 硬件初始化 ====================
BLDCMotor propL(7), propR(7);       // 水下推进器（左右）
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// 传感器接口（水下强电磁防护）
const int sonarPin = A0;    // 声呐距离传感器
const int rollPin = A1;     // 横滚角传感器
const int pitchPin = A2;    // 俯仰角传感器
const int samplerPin = 3;   // 水下采样器电磁触发

// ==================== 抗干扰数字滤波（IIR低通，抑制电磁尖峰） ====================
class IIRFilter {
    float a, b;
    float lastOut = 0;
public:
    IIRFilter(float alpha=0.3) { a = alpha; b = 1-alpha; } // 一阶低通
    float filter(float input) {
        float out = a*input + b*lastOut;
        lastOut = out;
        return out;
    }
};

IIRFilter sonarFilt(0.2), attitudeFilt(0.15);

// ==================== 模糊自适应PID姿态控制（抗流场干扰） ====================
class FuzzyPID {
    float Kp, Ki, Kd;
    float integral, lastErr;
public:
    FuzzyPID() { Kp=0.8; Ki=0.01; Kd=0.2; integral=0; lastErr=0; }
    
    // 模糊推理修正PID参数（简化规则：误差大→Kp增大，误差变化率大→Kd增大）
    void adaptParams(float err, float dErr) {
        if (abs(err) > 10) Kp = 1.2;
        else if (abs(err) > 5) Kp = 0.9;
        else Kp = 0.7;
        
        if (abs(dErr) > 50) Kd = 0.4;
        else if (abs(dErr) > 20) Kd = 0.3;
        else Kd = 0.2;
    }
    
    float compute(float setpoint, float feedback, float dt) {
        float err = setpoint - feedback;
        float dErr = (err - lastErr) / dt;
        adaptParams(err, dErr);
        
        integral += err * dt;
        integral = constrain(integral, -1, 1);
        float output = Kp*err + Ki*integral + Kd*dErr;
        lastErr = err;
        return constrain(output, -MAX_SPEED, MAX_SPEED);
    }
};

FuzzyPID pidPitch, pidRoll; // 俯仰/横滚控制

// ==================== 水下闭环定位采样控制 ====================
void underwaterLeakSampling() {
    // 1. 抗干扰传感器采样
    float targetDist = sonarFilt.filter(analogRead(sonarPin)); // 泄漏点距离（模拟值转cm）
    float pitch = attitudeFilt.filter(analogRead(pitchPin));    // 俯仰角
    float roll = attitudeFilt.filter(analogRead(rollPin));      // 横滚角
    
    // 2. 目标参数计算（泄漏点位置估算）
    float targetPitch = 5.0; // 期望俯仰角（朝向泄漏点）
    float targetDistSet = 20.0; // 期望采样距离
    
    // 3. 模糊PID控制俯仰/横滚姿态
    float pitchOut = pidPitch.compute(targetPitch, pitch, 0.1);
    float rollOut = pidPitch.compute(0, roll, 0.1);
    
    // 4. 距离闭环控制（推进器速度调节）
    float distErr = targetDist - targetDistSet;
    float distCtrl = constrain(distErr * 0.05, -0.8, 0.8); // 距离环输出
    
    // 5. 推进器差速控制（抵消流场+姿态修正）
    float leftSpd = distCtrl + pitchOut - rollOut;
    float rightSpd = distCtrl - pitchOut + rollOut;
    
    // 限幅输出
    leftSpd = constrain(leftSpd, -MAX_SPEED, MAX_SPEED);
    rightSpd = constrain(rightSpd, -MAX_SPEED, MAX_SPEED);
    
    // 执行推进
    propL.move(leftSpd);
    propR.move(rightSpd);
    
    // 6. 精准采样触发
    if (abs(targetDist - targetDistSet) < 5 && abs(pitch) < 3 && abs(roll) < 3) {
        digitalWrite(samplerPin, HIGH); // 触发采样
        delay(800);
        digitalWrite(samplerPin, LOW);
    }
}

// ==================== 回收路径闭环控制 ====================
void returnPathControl() {
    static bool returning = false;
    static float returnDist = 0;
    
    // 判断是否开始回收（采样完成后触发）
    if (analogRead(sonarPin) < 10 && !returning) { // 模拟采样完成标志
        returning = true;
        returnDist = 0;
    }
    
    if (returning) {
        // 沿预设方向回收，速度闭环
        returnDist += 0.05;
        float leftSpd = 0.5;
        float rightSpd = 0.5;
        propL.move(leftSpd);
        propR.move(rightSpd);
        
        // 回收完成判断（模拟距离阈值）
        if (returnDist > 100) {
            returning = false;
            propL.move(0);
            propR.move(0);
        }
    } else {
        underwaterLeakSampling(); // 未回收时执行采样任务
    }
}

void setup() {
    Serial.begin(115200);
    // BLDC推进器初始化
    propL.linkSensor(&encL); propL.linkDriver(&driverL);
    propR.linkSensor(&encR); propR.linkDriver(&driverR);
    propL.controller = MotionControlType::velocity;
    propR.controller = MotionControlType::velocity;
    propL.init(); propL.initFOC();
    propR.init(); propR.initFOC();
    // 采样器初始化
    pinMode(samplerPin, OUTPUT);
    digitalWrite(samplerPin, LOW);
}

void loop() {
    propL.loopFOC(); propR.loopFOC();
    returnPathControl();
    
    // 调试输出
    Serial.print("Dist:");Serial.print(analogRead(sonarPin));
    Serial.print(" Pitch:");Serial.print(analogRead(pitchPin));
    Serial.print(" Roll:");Serial.print(analogRead(rollPin));
    Serial.println();
    
    delay(40);
}

要点解读

1. 强电磁干扰下的传感器抗干扰设计：硬件+软件双防护
   强电磁环境会通过空间辐射、传导耦合干扰传感器信号，导致数据跳变、漂移，硬件滤波+软件算法是抗干扰的核心：
   硬件防护层：传感器供电采用DC-DC隔离模块，避免共地引入干扰；信号输入端并联TVS管、磁珠，抑制电磁尖峰；关键传感器（如气体、姿态）置于电磁屏蔽壳内，减少空间辐射干扰；布线时模拟信号与数字信号分离，降低串扰。
   软件滤波层：采用均值滤波、IIR低通滤波等数字滤波算法，进一步抑制高频干扰；对关键传感器进行异常值剔除，避免干扰信号触发错误控制；对采样信号进行归一化处理，降低电磁噪声对浓度、距离计算的影响。
   冗余校验：针对关键定位信息（如航向角、距离），采用多传感器冗余校验，当多传感器偏差超过阈值时，自动切换至备份数据，保障定位连续性。

2. 泄漏源定位的多源信息融合策略：提升定位精度与鲁棒性
   单一传感器无法在强电磁环境下实现高精度泄漏源定位，多源信息融合是关键：
   传感器互补设计：气体泄漏定位融合气体浓度传感器与方位角传感器——气体传感器通过浓度梯度确定泄漏源大致方向，方位角传感器精准锁定目标方位，形成“梯度初筛+角度精锁”的互补；水下场景则融合声呐（距离）与姿态传感器（方位），应对流场干扰。
   融合算法适配：采用自适应卡尔曼滤波（案例5）、模糊推理（案例4）等算法，动态调整各传感器权重——强干扰下，降低易受干扰传感器的权重，提升高可靠性传感器的占比；目标特征明显时，提升主定位传感器的权重，加快收敛速度。
   定位结果闭环修正：将定位结果反馈至控制层，通过机器人运动状态与定位偏差的闭环调节，修正定位误差，如案例5中卡尔曼滤波融合里程计与方位角，实时修正航向角，避免定位漂移。

3. BLDC电机的抗干扰闭环控制：保障运动稳定性
   强电磁环境会干扰BLDC电机的驱动信号，导致电流波动、转速失稳，FOC闭环+干扰抑制是核心：
   FOC闭环控制：采用磁场定向控制（FOC）实现BLDC电机的转矩、速度双闭环控制，案例中所有电机均采用FOC，通过实时检测电机转速与电流，动态调节驱动信号，抑制电磁干扰引起的电流波动，确保电机转速稳定。
   驱动电路防护：BLDC驱动电路采用隔离栅极驱动芯片，隔离主控与功率电路的电磁耦合；电源端并联电解电容与陶瓷电容，滤除电源纹波；驱动信号采用屏蔽线传输，减少信号辐射干扰。
   控制参数自适应：针对电磁干扰导致电机负载波动，采用模糊自适应PID（案例6）动态调整控制参数，当负载突变（如流场干扰）时，增大比例系数快速响应，增大微分系数抑制振荡，确保电机在干扰下稳定跟踪目标速度。

4. 路径规划与采样的协同控制：平衡安全与精准
   泄漏源定位与采样需兼顾路径安全与采样精准，分层规划+闭环执行是关键：
   分层路径规划：高层采用全局路径规划确定泄漏源搜寻大致方向，低层采用局部路径规划（如DWA、模糊避障）应对动态障碍物与干扰，案例2采用DWA动态规划局部路径，案例4采用模糊避障应对随机障碍，确保路径安全。
   采样闭环控制：采样前通过距离、姿态闭环控制将机器人调整至最佳采样位置（如案例6中距离误差<5cm、姿态误差<3°），采样执行器采用闭环控制，确保采样动作精准执行，避免因干扰导致采样失败。
   状态切换逻辑：明确“搜寻→逼近→采样→回收”的状态切换逻辑，案例6中通过传感器阈值判断状态切换时机，采样完成后自动进入回收路径，确保任务流程连贯，避免状态混乱。

5. 硬件系统与控制架构的电磁兼容性设计：系统稳定的基础
   强电磁环境下，系统稳定依赖从硬件到软件的全栈电磁兼容设计：
   硬件架构设计：主控、传感器、驱动电路分模块布局，采用独立屏蔽壳体，减少模块间电磁耦合；电源系统采用分级滤波，主控电源与功率电源完全隔离，避免功率回路干扰传导至控制回路；接口采用光电隔离，切断干扰传导路径。
   控制架构适配：采用“感知-决策-执行”的闭环控制架构，将高算力任务（如复杂融合算法）下放至主控，BLDC电机控制采用独立循环，确保控制实时性；关键控制指令采用冗余校验，避免电磁干扰导致指令错误。
   故障自诊断与容错：系统内置自诊断机制，实时监测传感器、电机、采样器状态，当检测到故障（如信号丢失、电机堵转）时，自动切换至安全模式（如减速停车），案例2中通信超时后自动减速，保障系统安全。

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