基于Arduino与BLDC（无刷直流电机）构建的"工业高压气体泄漏巡检机器人（模糊避障+浓度定位采样）"，是一套将模糊逻辑控制、多模态气体感知与BLDC高动态执行深度耦合的特种嵌入式巡检架构。该方案的核心价值在于：通过模糊逻辑处理复杂工业环境中的不确定性与传感器噪声，实现鲁棒的自主避障导航；同时利用多气体传感器阵列构建浓度梯度场，驱动机器人沿浓度递增方向自主逼近泄漏源并完成定点采样，彻底解决传统人工巡检"高危暴露、漏检率高、定位困难"的行业痛点。

主要特点
模糊逻辑避障控制
工业厂区环境复杂多变，存在大量非结构化障碍物（管道支架、临时堆放的物料、移动的工程车辆等），且传感器数据常受电磁干扰和粉尘影响。模糊逻辑控制（FLC）通过引入"隶属度函数"替代传统二值逻辑，将超声波/激光雷达测得的精确距离值转化为"很近"“适中”“较远"等模糊概念，再经由预设的"If-Then"规则库（如"若前方很近且左侧较远，则向左急转”）输出电机PWM指令。这种仿人经验决策机制无需对机器人动力学进行精确数学建模，对传感器噪声具有天然的容忍度，即使测距数据偶尔跳变，也能平滑处理避免机器人剧烈抖动或误判。
多传感器融合的浓度梯度定位
系统通常搭载3个以上呈扇形分布的气体传感器（如TGS2620检测可燃气体、MQ-7检测一氧化碳、电化学传感器检测硫化氢），分别采集左、中、右三个方向的实时浓度值。模糊控制器将三路浓度差值作为输入，通过规则库判断泄漏源的方向偏向，输出差速转向指令驱动机器人向浓度更高的区域移动。这种"仿生嗅觉跟踪"策略模拟了昆虫或哺乳动物循气味溯源的行为模式，无需预先构建环境地图，即可在未知空间中自主逼近泄漏源。
异构气体传感器阵列与交叉干扰抑制
单一气体传感器往往存在交叉敏感问题（如MQ-5对液化气、天然气、丙烷均有响应）。系统通过部署不同原理的传感器（金属氧化物半导体、电化学、NDIR红外吸收、TDLAS激光遥测）形成异构阵列，利用多传感器数据融合算法进行交叉验证和干扰抑制。例如，当TGS2620和MQ-5同时检测到浓度升高而电化学传感器无响应时，可判定为可燃气体泄漏而非有毒气体，从而提高气体类型识别的准确性。
BLDC的高动态柔顺执行与本质安全
BLDC电机无电刷摩擦产生的机械火花，在易燃易爆气体环境中能有效避免成为点火源，满足本质安全要求。配合FOC（磁场定向控制）驱动器，BLDC具备毫秒级转矩响应能力和低速大扭矩特性，能精准执行模糊控制器输出的连续变速指令，实现平滑的差速转向和渐进式加减速，避免传统直流电机的"顿挫感"导致采样探头扰动气流、影响浓度测量精度。此外，BLDC效率高、发热低，有助于将电机表面温度控制在环境气体的引燃温度以下。
分层式硬实时控制架构
系统采用典型的三层架构：感知层（气体传感器阵列+超声波/激光雷达+IMU）负责多模态数据采集；决策层（Arduino Mega 2560或更高性能MCU）运行模糊推理引擎和浓度梯度算法，周期50-100ms；执行层（专用BLDC驱动板）运行FOC电流环和PID速度环，周期1-5ms。这种分层设计确保底层电机控制不被上层模糊推理阻塞，维持系统的硬实时性。

应用场景
化工园区与生产装置巡检
在反应釜、储罐区、管道连接处等高风险区域进行7×24小时不间断自主巡检。机器人沿预设路线移动，当检测到气体浓度异常时，自动切换至"浓度跟踪模式"，沿梯度方向逼近泄漏点并定点采样，同时触发声光报警并将位置坐标和浓度数据回传至监控中心。
地下综合管廊与受限空间探测
进入人员难以进入的地下管廊、阀门井或储罐内部，对甲烷、硫化氢等有毒有害气体进行检测和定位。模糊避障系统使机器人能在狭窄、光线昏暗、障碍物密集的管廊中自主穿行，评估作业环境安全，为后续人工维修提供数据支持。
石油天然气站场与长输管道巡检
在输油站、加气站、阀室等场所，机器人搭载激光甲烷遥测仪（TDLAS）实现50-200米距离的非接触式泄漏检测，结合轮式底盘的近距离采样验证，形成"远距扫描+近距确认"的双重检测机制，大幅降低漏检率。
应急救援与事故现场侦察
在火灾、泄漏、坍塌等事故现场，机器人作为先遣侦察力量进入高危区域，实时回传气体浓度分布、温度场和现场视频，为应急指挥提供决策依据，降低救援人员暴露风险。
教育与科研实验平台
在高校自动化、机器人工程专业中，Arduino+BLDC+气体传感器构成低成本高开放性的模糊控制与气源定位验证平台。学生可编程实现不同模糊规则库设计和浓度跟踪策略，直观理解智能控制在不确定环境中的应用。

注意事项
硬件选型与算力瓶颈
Arduino Uno（ATmega328P，16MHz，2KB SRAM）难以同时运行多路传感器数据采集、模糊推理和BLDC的FOC控制。模糊推理中的隶属度计算和重心法去模糊化需要一定计算时间，若控制周期过长会导致避障响应滞后。建议选用Arduino Mega 2560（256KB Flash，8KB SRAM，4路UART）或更高性能的Teensy 4.0/4.1（Cortex-M7，600MHz）、ESP32-S3（双核，240MHz）作为主控。对于需要运行SLAM或复杂气体扩散模型的场景，应采用"上位机（树莓派/Jetson）做高层决策+下位机做底层BLDC执行"的异构架构。
防爆设计与本质安全认证
工业高压气体环境多为易燃易爆区域（如1区、2区危险场所），机器人必须满足相应的防爆等级标准（如Ex d IIC T6 Gb）。所有电气接口需严格密封，防止气体侵入引发短路或爆炸；电子元器件和电机的表面温度必须控制在环境气体的引燃温度以下，需配备温度传感器和主动散热模块；通信模块建议采用跳频扩频（FHSS）技术，规避化工厂内各种设备产生的窄带电磁干扰。
模糊规则库的设计与优化
规则完备性：模糊规则库必须覆盖所有可能的环境状态组合（如"前方近、左侧远、右侧近""左浓度高、中浓度低、右浓度中"等），规则缺失会导致特定场景下无输出，机器人失控。规则冲突与调整：初始规则通常基于经验试凑，可能不够优化。需要通过大量的实地测试，观察机器人的避障轨迹和浓度跟踪路径，反复调整隶属度函数的形状和规则库的内容，以消除震荡或死锁现象。
气体传感器的预热与校准
金属氧化物半导体传感器（如MQ系列、TGS系列）需要预热24-48小时才能达到稳定状态，刚上电的读数会非常不稳定且偏高，直接使用会导致误报警。此外，传感器存在长期漂移问题，需定期通入洁净空气进行零点校准，或采用温湿度补偿算法（如协同DHT22采集环境参数）提高测量可靠性。
多传感器数据同步与滤波
气体传感器的响应时间通常在10-30秒，而超声波/激光雷达的更新频率可达50-100Hz，二者时间尺度差异巨大。在浓度跟踪过程中，需对气体浓度数据进行滑动平均或中值滤波，消除瞬时波动；同时对测距数据进行预处理，避免模糊控制器因单次跳变产生错误转向指令。建议在硬件上使用同一时钟源触发各传感器，或在软件中进行时间对齐补偿。
通信链路的可靠性与冗余
化工厂区存在大量金属结构和大型设备，对无线信号造成严重遮挡和多径效应。ZigBee通信在复杂环境中可能不稳定，建议采用5G/工业以太网双链路通信或部署多个AP建立无线覆盖，确保控制指令和回传数据的稳定传输。同时应设计断链保护机制：当通信中断超过设定阈值时，机器人自动进入安全模式（原地停止或沿原路返回）。
机械结构与环境适应性
工业厂区地面可能存在油污、积水、碎石等，轮胎需选用高摩擦、耐化学腐蚀的材料；底盘需具备一定离地间隙和越障能力，适应非铺装路面；整机防护等级建议达到IP65以上，防止粉尘和喷水侵入。此外，采样探头的位置设计需考虑气流扰动影响，避免机器人自身运动产生的气流稀释待测气体浓度，影响定位精度。

1、模糊逻辑避障 + 气体梯度追踪（沿墙巡检调制）
场景：化工管廊等强电磁干扰环境，机器人沿管廊壁巡检，模糊逻辑保障稳定沿墙，气体传感器检测到浓度异常时自动调制采样频率与运动方向，实现泄漏源初步定位 。

/* ===== 强电磁环境管廊泄漏源定位巡检机器人 =====
 * 硬件：Arduino ESP32 + BLDC差速底盘 + 侧向红外沿墙 + MQ系列气体传感器 + 卡尔曼滤波 + 航位推算
 * 核心：卡尔曼滤波抗干扰 + 模糊沿墙 + 气体采样调制 + 泄漏源定位
 */
#include <SimpleFOC.h>
#include <Kalman.h>

// --- 硬件驱动定义 ---
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
const int wallSensorPins[3] = {A0, A1, A2}; // 侧向沿墙传感器

// 气体采样传感器
const int gasSensorPin = A3;
const int encoderPinL = 2, encoderPinR = 3;  // 编码器（航位推算）

// --- 卡尔曼滤波与航位推算参数 ---
KalmanFilter kf_gas(0.01, 1.0, 0.001); // 气体信号滤波器
float posX = 0, posY = 0, heading = 0;   // 航位推算坐标
float wheelRadius = 0.05, wheelBase = 0.25; // 车轮半径、轴距
volatile int encLCount = 0, encRCount = 0; // 编码器计数

// --- 模糊沿墙与采样调制参数 ---
float fuzzyWallOut = 0;
float sampleFreq = 100; // 默认采样频率（ms）
float lastSampleTime = 0;
float gasConc = 0, gasGradient = 0;
float lastGasConc = 0;

// --- 模糊沿墙控制（三传感器输入）---
float fuzzyWallControl(int leftDist, int midDist, int rightDist) {
  float idealDist = 300;
  float midError = (float)(midDist - idealDist) / 100.0;
  float angleError = (float)(rightDist - leftDist) / 100.0;

  // 模糊化：将误差映射为语义变量
  float output = 0;

  // 规则1：距离太近 → 向外转
  if (midError < -0.5) {
    output = -0.8 - angleError * 0.5;
  }
  // 规则2：距离太远 → 向内转
  else if (midError > 0.5) {
    output = 0.8 - angleError * 0.5;
  }
  // 规则3：距离合适 → 仅角度修正
  else {
    output = -angleError * 1.2;
  }

  return constrain(output, -1.0, 1.0);
}

// --- 气体梯度追踪调制 ---
void gasGradientTrack(float conc, float &vx, float &wz) {
  float delta = conc - lastGasConc;
  gasGradient = delta;

  // 浓度上升：降低前进速度，增加原地采样频率
  if (delta > 5.0) {
    vx = 0.15;        // 慢速逼近
    sampleFreq = 50;  // 高频采样
  }
  // 浓度下降：恢复巡航，梯度方向转向
  else if (delta < -3.0) {
    vx = 0.4;
    sampleFreq = 200;
    wz = (delta > 0) ? 0.3 : -0.3; // 反向追踪
  }
  // 浓度平稳：正常巡检
  else {
    vx = 0.35;
    sampleFreq = 150;
  }

  lastGasConc = conc;
}

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

  // BLDC 初始化
  driverL.voltage_power_supply = 24; driverL.init();
  motorL.linkDriver(&driverL); motorL.init(); motorL.initFOC();
  motorL.controller = MotionControlType::velocity;

  driverR.voltage_power_supply = 24; driverR.init();
  motorR.linkDriver(&driverR); motorR.init(); motorR.initFOC();
  motorR.controller = MotionControlType::velocity;

  // 编码器中断
  attachInterrupt(digitalPinToInterrupt(encoderPinL), [](){ encLCount++; }, RISING);
  attachInterrupt(digitalPinToInterrupt(encoderPinR), [](){ encRCount++; }, RISING);

  // 卡尔曼滤波初始化
  kf_gas.setAngle(analogRead(gasSensorPin));
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 1. 沿墙传感器与气体数据采集
  int leftDist = analogRead(wallSensorPins[0]);
  int midDist = analogRead(wallSensorPins[1]);
  int rightDist = analogRead(wallSensorPins[2]);
  float rawGas = analogRead(gasSensorPin);

  // 卡尔曼滤波抗电磁干扰
  float filteredGas = kf_gas.updateEstimate(rawGas);

  // 采样频率动态调制
  if (millis() - lastSampleTime > sampleFreq) {
    lastSampleTime = millis();
    gasConc = filteredGas;
    // 可在此处存储浓度场地图数据
    Serial.print("GasConc:");
    Serial.print(gasConc);
    Serial.print(" Gradient:");
    Serial.print(gasGradient);
    Serial.print(" Freq:");
    Serial.println(sampleFreq);
  }

  // 2. 模糊沿墙控制
  fuzzyWallOut = fuzzyWallControl(leftDist, midDist, rightDist);

  // 3. 气体梯度追踪调制
  float vx = 0.35, wz = 0;
  gasGradientTrack(gasConc, vx, wz);

  // 4. 融合控制：沿墙输出叠加梯度追踪的转向
  float wheelBase = 0.3;
  float leftSpd = vx - (wz + fuzzyWallOut) * wheelBase / 2;
  float rightSpd = vx + (wz + fuzzyWallOut) * wheelBase / 2;

  // 5. 执行电机控制
  motorL.move(leftSpd);
  motorR.move(rightSpd);

  // 航位推算更新
  float dL = encLCount * 2 * PI * wheelRadius / 500.0;
  float dR = encRCount * 2 * PI * wheelRadius / 500.0;
  float dCenter = (dL + dR) / 2.0;
  heading += (dR - dL) / wheelBase;
  posX += dCenter * cos(heading);
  posY += dCenter * sin(heading);
  encLCount = 0; encRCount = 0;

  delay(50);
}

核心要点：模糊沿墙将距离误差和角度误差同时模糊化，输出连续的转向修正量，比阈值判断更平滑。气体梯度追踪的关键是采样频率与运动速度的联动调制——浓度上升时主动降速并加密采样，避免高速掠过泄漏点导致漏检。

2、多级安全本能的模糊避障 + 浓度阈值中断采样
场景：机器人巡检中前方突遇障碍物或气体浓度骤升，采用 WARNING→BRAKE→EMERGENCY 三级响应，浓度超阈值时通过中断触发高频采样，兼顾避障安全与泄漏快速响应 。

/* ===== 多级安全本能模糊避障 + 浓度阈值中断采样 =====
 * 硬件：Arduino Mega + BLDC差速 + 超声波×3 + CCS811/MQ气体传感器
 * 核心：分级模糊避障 + 浓度中断触发采样
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// --- BLDC 定义 ---
BLDCMotor motorL(9), motorR(9);
BLDCDriver3PWM driverL(6,7,8,12);
BLDCDriver3PWM driverR(3,4,5,13);

// --- 超声波传感器 ---
#define TRIG_FRONT 22
#define ECHO_FRONT 23
#define TRIG_LEFT 24
#define ECHO_LEFT 25
#define TRIG_RIGHT 26
#define ECHO_RIGHT 27

NewPing sonarFront(TRIG_FRONT, ECHO_FRONT, 200);
NewPing sonarLeft(TRIG_LEFT, ECHO_LEFT, 200);
NewPing sonarRight(TRIG_RIGHT, ECHO_RIGHT, 200);

// --- 气体传感器与中断 ---
const int gasSensorPin = A0;
volatile bool gasAlarmTriggered = false;
volatile float gasConcentration = 0;
float gasThresholdLow = 300;   // 预警阈值
float gasThresholdHigh = 600;  // 危险阈值
float sampleInterval = 1000;   // 默认采样间隔
unsigned long lastSampleTime = 0;

// --- 安全等级 ---
enum SafetyLevel { LEVEL_CRUISE, LEVEL_WARNING, LEVEL_BRAKE, LEVEL_EMERGENCY };
SafetyLevel currentLevel = LEVEL_CRUISE;

// --- 模糊避障参数 ---
float frontDist, leftDist, rightDist;
float brakeStrength = 0;

// 气体中断服务函数
void gasInterrupt() {
  gasAlarmTriggered = true;
}

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

  // BLDC 初始化
  driverL.voltage_power_supply = 24; driverL.init();
  motorL.linkDriver(&driverL); motorL.init(); motorL.initFOC();
  motorL.controller = MotionControlType::velocity;
  driverR.voltage_power_supply = 24; driverR.init();
  motorR.linkDriver(&driverR); motorR.init(); motorR.initFOC();
  motorR.controller = MotionControlType::velocity;

  // 气体传感器中断（模拟阈值触发）
  attachInterrupt(digitalPinToInterrupt(2), gasInterrupt, RISING);
}

// 模糊避障决策
void fuzzyAvoidance() {
  // 读取超声波
  frontDist = sonarFront.ping_cm();
  leftDist = sonarLeft.ping_cm();
  rightDist = sonarRight.ping_cm();

  if (frontDist == 0) frontDist = 200;
  if (leftDist == 0) leftDist = 200;
  if (rightDist == 0) rightDist = 200;

  float SAFE_DIST = 40.0;
  float WARNING_DIST = 80.0;

  // 多级安全判断
  if (frontDist < 15.0) {
    currentLevel = LEVEL_EMERGENCY;
    brakeStrength = 1.0;
  } else if (frontDist < SAFE_DIST) {
    currentLevel = LEVEL_BRAKE;
    brakeStrength = (SAFE_DIST - frontDist) / SAFE_DIST;
  } else if (frontDist < WARNING_DIST) {
    currentLevel = LEVEL_WARNING;
    brakeStrength = 0.3;
  } else {
    currentLevel = LEVEL_CRUISE;
    brakeStrength = 0;
  }

  // 模糊规则计算转向偏差
  int bias = 0;
  float sideDiff = leftDist - rightDist;

  if (frontDist < WARNING_DIST) {
    if (sideDiff > 15) {
      bias = -150 - sideDiff * 0.5; // 左转
    } else if (sideDiff < -15) {
      bias = 150 + abs(sideDiff) * 0.5; // 右转
    } else {
      bias = (random(2) == 0) ? -180 : 180; // 随机转向防死锁
    }
  } else if (frontDist < SAFE_DIST) {
    bias = constrain(sideDiff * 1.5, -100, 100); // 轻微修正
  }

  // 速度控制（带制动强度）
  int baseSpeed = 180;
  int speedFactor = (int)((1.0 - brakeStrength) * 255);
  int leftSpeed = constrain(baseSpeed + bias, 0, speedFactor);
  int rightSpeed = constrain(baseSpeed - bias, 0, speedFactor);

  // 转为 SimpleFOC 速度指令
  float vL = leftSpeed / 255.0 * 10.0;
  float vR = rightSpeed / 255.0 * 10.0;

  if (currentLevel == LEVEL_EMERGENCY) {
    motorL.move(0); motorR.move(0);
  } else {
    motorL.move(vL); motorR.move(vR);
  }
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 气体中断处理：浓度超阈值时加密采样
  if (gasAlarmTriggered) {
    gasAlarmTriggered = false;
    gasConcentration = analogRead(gasSensorPin);

    if (gasConcentration > gasThresholdHigh) {
      sampleInterval = 100;  // 危险：每100ms采样
      Serial.println("!!! HIGH GAS CONCENTRATION !!!");
    } else if (gasConcentration > gasThresholdLow) {
      sampleInterval = 300;  // 预警：每300ms采样
    }
  }

  // 周期性采样
  if (millis() - lastSampleTime > sampleInterval) {
    lastSampleTime = millis();
    float rawGas = analogRead(gasSensorPin);
    // 一阶低通滤波
    static float filteredGas = rawGas;
    filteredGas = filteredGas * 0.7 + rawGas * 0.3;
    gasConcentration = filteredGas;

    Serial.print("F:");
    Serial.print(frontDist);
    Serial.print(" Gas:");
    Serial.print(gasConcentration);
    Serial.print(" Level:");
    Serial.println(currentLevel);
  }

  // 模糊避障执行
  fuzzyAvoidance();

  delay(50);
}

核心要点：多级安全本能的核心是响应强度随距离连续变化，而非简单的“停/不停”二值切换。气体浓度中断机制让采样频率能够动态响应浓度突变——正常巡检时低功耗低频采样，浓度骤升时立即切换到高频采样模式，捕捉泄漏动态特征。

3、外螺旋搜索 + 自适应浓度阈值泄漏源定位
场景：已知大致泄漏区域后，机器人执行外螺旋搜索算法覆盖区域，采用自适应浓度阈值（移动平均）判断是否进入“气味羽流”，切换为浓度质心逼近算法精准定位泄漏点 。

/* ===== 外螺旋搜索 + 自适应阈值泄漏源定位 =====
 * 硬件：Arduino ESP32 + BLDC + 气体传感器 + 风向传感器（可选）
 * 核心：外螺旋覆盖 + 自适应阈值 + 浓度质心逼近
 */
#include <SimpleFOC.h>

// --- BLDC 定义 ---
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);

// --- 气体传感器 ---
const int gasSensorPin = A0;
float currentConc = 0;
float adaptiveThreshold = 0;
float concentrationHistory[20];
int historyIndex = 0;
float lambda = 0.9; // 自适应阈值参数

// --- 搜索算法参数 ---
enum SearchState { SEARCH_SPIRAL, SEARCH_GRADIENT, SOURCE_CONFIRMED };
SearchState searchState = SEARCH_SPIRAL;

float spiralCenterX = 0, spiralCenterY = 0;
float robotX = 0, robotY = 0, robotYaw = 0;
float spiralPitch = 0.3;     // 螺旋间距
float spiralAngle = 0;
float spiralRadius = 0;
float thetaP = 45 * PI / 180; // 每45度一个航点

// 浓度质心算法参数
float highConcPointsX[10], highConcPointsY[10];
int highConcCount = 0;
float estimatedSourceX = 0, estimatedSourceY = 0;

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

  driverL.voltage_power_supply = 24; driverL.init();
  motorL.linkDriver(&driverL); motorL.init(); motorL.initFOC();
  motorL.controller = MotionControlType::velocity;
  driverR.voltage_power_supply = 24; driverR.init();
  motorR.linkDriver(&driverR); motorR.init(); motorR.initFOC();
  motorR.controller = MotionControlType::velocity;

  // 初始化浓度历史
  for (int i = 0; i < 20; i++) concentrationHistory[i] = 0;
}

// 读取并滤波浓度
float readGasConcentration() {
  float raw = analogRead(gasSensorPin);
  static float filtered = raw;
  filtered = filtered * 0.8 + raw * 0.2;
  return filtered;
}

// 自适应阈值更新
void updateAdaptiveThreshold(float conc) {
  concentrationHistory[historyIndex] = conc;
  historyIndex = (historyIndex + 1) % 20;

  // 移动平均作为自适应阈值
  float sum = 0;
  for (int i = 0; i < 20; i++) sum += concentrationHistory[i];
  float movingAvg = sum / 20.0;

  adaptiveThreshold = lambda * adaptiveThreshold + (1 - lambda) * movingAvg;
}

// 外螺旋航点计算
void computeSpiralWaypoint(float &targetX, float &targetY) {
  spiralAngle += thetaP;
  spiralRadius = (spiralAngle / (2 * PI)) * spiralPitch;

  targetX = spiralCenterX + spiralRadius * cos(spiralAngle);
  targetY = spiralCenterY + spiralRadius * sin(spiralAngle);
}

// 浓度质心估算
void estimateSourcePosition() {
  if (highConcCount == 0) return;

  float sumX = 0, sumY = 0, sumW = 0;
  for (int i = 0; i < highConcCount; i++) {
    float weight = 1.0; // 可改为浓度权重
    sumX += highConcPointsX[i] * weight;
    sumY += highConcPointsY[i] * weight;
    sumW += weight;
  }

  estimatedSourceX = sumX / sumW;
  estimatedSourceY = sumY / sumW;
}

// 差速控制到目标点
void goToPoint(float targetX, float targetY) {
  float dx = targetX - robotX;
  float dy = targetY - robotY;
  float targetYaw = atan2(dy, dx);
  float yawErr = targetYaw - robotYaw;
  while (yawErr > PI) yawErr -= 2 * PI;
  while (yawErr < -PI) yawErr += 2 * PI;

  float dist = sqrt(dx * dx + dy * dy);
  float v = constrain(dist * 0.8, 0.05, 0.4);
  float w = constrain(yawErr * 1.5, -1.0, 1.0);

  // 差速运动学
  float wheelBase = 0.3, wheelRadius = 0.05;
  float vL = (v - w * wheelBase / 2.0) / wheelRadius;
  float vR = (v + w * wheelBase / 2.0) / wheelRadius;

  motorL.move(vL);
  motorR.move(vR);

  // 更新位置（简化航位推算）
  robotX += v * cos(robotYaw) * 0.05;
  robotY += v * sin(robotYaw) * 0.05;
  robotYaw += w * 0.05;
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 读取浓度
  currentConc = readGasConcentration();
  updateAdaptiveThreshold(currentConc);

  Serial.print("State:");
  Serial.print(searchState);
  Serial.print(" Conc:");
  Serial.print(currentConc);
  Serial.print(" Thresh:");
  Serial.print(adaptiveThreshold);
  Serial.print(" Pos:");
  Serial.print(robotX);
  Serial.print(",");
  Serial.println(robotY);

  switch (searchState) {
    case SEARCH_SPIRAL: {
      // 判断是否进入气味羽流：浓度显著高于自适应阈值
      if (currentConc > adaptiveThreshold * 1.5 && currentConc > 100) {
        searchState = SEARCH_GRADIENT;
        Serial.println(">>> Entered plume, switching to gradient mode");
        break;
      }

      // 外螺旋搜索
      float targetX, targetY;
      computeSpiralWaypoint(targetX, targetY);
      goToPoint(targetX, targetY);

      // 螺旋半径过大则重置（防止无界搜索）
      if (spiralRadius > 5.0) {
        spiralAngle = 0;
        spiralRadius = 0;
        Serial.println(">>> Spiral reset");
      }
      break;
    }

    case SEARCH_GRADIENT: {
      // 记录高浓度点
      if (currentConc > adaptiveThreshold * 1.5 && highConcCount < 10) {
        highConcPointsX[highConcCount] = robotX;
        highConcPointsY[highConcCount] = robotY;
        highConcCount++;
      }

      // 估算质心并逼近
      estimateSourcePosition();
      goToPoint(estimatedSourceX, estimatedSourceY);

      // 判断是否收敛：距质心很近且浓度高
      float distToCenter = sqrt(pow(robotX - estimatedSourceX, 2) +
                                pow(robotY - estimatedSourceY, 2));
      if (distToCenter < 0.2 && currentConc > 200) {
        searchState = SOURCE_CONFIRMED;
        Serial.println(">>> SOURCE CONFIRMED!");
      }

      // 长时间无高浓度点，退回螺旋搜索
      if (highConcCount == 0 && millis() > 30000) {
        searchState = SEARCH_SPIRAL;
      }
      break;
    }

    case SOURCE_CONFIRMED: {
      // 原地采样，输出结果
      motorL.move(0);
      motorR.move(0);

      static unsigned long lastReport = 0;
      if (millis() - lastReport > 2000) {
        lastReport = millis();
        Serial.print("SOURCE at (");
        Serial.print(estimatedSourceX);
        Serial.print(", ");
        Serial.print(estimatedSourceY);
        Serial.print(") Conc:");
        Serial.println(currentConc);
      }
      break;
    }
  }

  delay(50);
}

核心要点：自适应浓度阈值的核心思想是——泄漏源附近的“背景浓度”会随接近程度上升，固定阈值要么过早触发（噪声导致）要么过晚触发（错过羽流）。用移动平均动态更新阈值，使机器人能够适应环境浓度波动。外螺旋搜索解决了“已知大致区域但不确定精确位置”的覆盖问题，浓度质心算法则在进入羽流后实现精细定位。

要点解读

1. 卡尔曼滤波是工业现场气体采样的“生存底线”
   工业管廊存在强电磁干扰、电机换相噪声、变频器辐射，气体传感器原始信号会出现剧烈跳变。程序案例一使用卡尔曼滤波对浓度信号做状态估计，本质上是用“过程模型+测量更新”抑制瞬时噪声。对于 Arduino 算力，简化的一维卡尔曼（只用角度变量）即可满足需求，不必引入完整的多维滤波 。

2. 模糊避障的核心优势是“连续响应”而非“阈值切换”
   传统避障是“距离<30cm → 停，距离>30cm → 走”，在临界点附近会反复启停。模糊逻辑将距离模糊化为“很近/近/适中/远”，输出连续的转向量和速度衰减系数，使机器人接近障碍物时平滑减速、平滑转向。程序案例二中的三级安全等级（CRUISE→WARNING→BRAKE→EMERGENCY）是模糊响应的离散化实现，制动强度连续可调 。

3. 采样频率必须与浓度状态联动，而非固定周期
   固定 1 秒采样在正常巡检时浪费功耗，在泄漏发生时又可能错过浓度峰值。程序案例一采用梯度调制采样频率（浓度上升→加密），程序案例二采用阈值中断触发（浓度超限→立即采样）。两者的共同逻辑是：让采样率跟随“信息价值”变化——浓度平稳时信息量低，浓度剧变时信息量高 。

4. 泄漏源定位应采用“粗搜索→细逼近”的分阶段策略
   直接用梯度爬升容易陷入局部高浓度点（工业现场的通风死角或残留气体）。外螺旋搜索先覆盖整个怀疑区域，进入羽流后切换到浓度质心算法逼近源点。程序案例三中的“自适应阈值”在这里起到状态切换判据的作用：浓度超过动态基线一定比例，才认为“进入了真正的羽流”，否则继续覆盖搜索 。

5. BLDC + FOC 的力矩平滑性对巡检机器人的“采样稳定性”有隐性贡献
   巡检机器人在慢速逼近泄漏源时，电机需要频繁微调速度和方向。有刷电机的换相顿挫会导致车身抖动，进而使气体传感器的采样位置和姿态不稳定，浓度读数波动加剧。FOC 的正弦驱动使低速运行转矩脉动大幅降低，机器人在“慢速爬行采样”时车身更平稳，传感器读数的一致性更好 。

4、仓储园区高压管道巡检机器人模糊避障程序（核心：模糊控制实现安全绕障）
应用场景：化工仓储园区内的高压气体管道巡检，场景特点：障碍物以管道支架、阀门、低矮设备为主，路径相对狭窄，障碍物类型固定但摆放不规则，需实现平稳避障，避免碰撞高压设备引发安全事故。

核心逻辑
模糊逻辑设计：以机器人与最近障碍物的距离、相对方位为输入，输出避障方向角与速度增益，规避传统阈值式避障的“忽停忽走”问题
模糊规则建立：根据工业场景安全权重，距离越近、障碍优先级越高，避让幅度越大
平稳性优化：避障过程中保持基础巡检速度，仅做平滑转向与减速，保障巡检连续性
安全兜底：近距离触发强制停车，避免碰撞高压设备

// 高压管道巡检机器人模糊避障程序
#include <Wire.h>
#include <Adafruit_SSD1306.h>

// ===== 硬件引脚定义 =====
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_IN1 A2
#define LEFT_MOTOR_IN2 A3
#define RIGHT_MOTOR_PWM 6
#define RIGHT_MOTOR_IN1 A4
#define RIGHT_MOTOR_IN2 A5

// 激光测距（6路：前、左前、右前、左、右、后，覆盖管道巡检场景）
#define LASER_PINS {A0, A1, A2, A3, A4, A5}
#define NUM_LASERS 6
// 红外补盲（2路：左、右近距离）
#define IR_LEFT_PIN 2
#define IR_RIGHT_PIN 3

Adafruit_SSD1306 display(128, 64, &Wire, -1);

// ===== 模糊控制参数 =====
const float BASE_SPEED = 12.0f;   // 基础巡检速度（cm/s，工业场景需平稳）
const uint16_t OBSTACLE_NEAR = 20;// 近距离障碍阈值（cm）
const uint16_t OBSTACLE_MID = 50; // 中距离障碍阈值（cm）
const uint16_t OBSTACLE_FAR = 120;// 远距离障碍阈值（cm）
const float STEERING_MAX = 15.0f; // 最大转向增益（偏移速度）

// ===== 模糊输入输出变量 =====
float input_distance = 0;    // 最近障碍物距离（模糊输入1）
float input_angle = 0;       // 障碍物相对方位（-1=左侧，1=右侧，0=正前方，模糊输入2）
float output_steering = 0;   // 转向增益（模糊输出1）
float output_speed_gain = 1.0f; // 速度增益（模糊输出2）

// ===== 状态变量 =====
uint16_t minDist = 200;      // 最近障碍物距离
int obstacleAngle = 0;       // 障碍物方位（0=前，1=左，2=右，3=后）
unsigned long lastScanTime = 0;
#define SCAN_CYCLE 120        // 扫描周期（ms）

// ===== 函数声明 =====
void scanObstacles();
void fuzzyControl();
void controlMotors();
void displayStatus();
uint16_t readLaser(uint8_t pin);
bool readIR(uint8_t pin);

void setup() {
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 128, 64);

  // 电机与传感器初始化
  pinMode(LEFT_MOTOR_IN1, OUTPUT);
  pinMode(LEFT_MOTOR_IN2, OUTPUT);
  pinMode(RIGHT_MOTOR_IN1, OUTPUT);
  pinMode(RIGHT_MOTOR_IN2, OUTPUT);
  for (int i = 0; i < NUM_LASERS; i++) {
    pinMode(LASER_PINS[i], INPUT);
  }
  pinMode(IR_LEFT_PIN, INPUT);
  pinMode(IR_RIGHT_PIN, INPUT);

  Serial.println("模糊避障系统初始化完成");
}

void loop() {
  // 1. 滚动扫描障碍物
  if (millis() - lastScanTime > SCAN_CYCLE) {
    scanObstacles();
    lastScanTime = millis();
  }

  // 2. 模糊控制决策
  fuzzyControl();

  // 3. 电机执行
  controlMotors();

  // 4. 状态显示与调试
  displayStatus();
  if (millis() % 2000 == 0) {
    Serial.print("最近障碍距离: "); Serial.print(minDist);
    Serial.print("cm 方位: "); Serial.println(obstacleAngle);
    Serial.print("转向增益: "); Serial.print(output_steering, 2);
    Serial.print(" 速度增益: "); Serial.println(output_speed_gain, 2);
  }
  delay(5);
}

// 扫描障碍物，确定最近距离与方位
void scanObstacles() {
  minDist = 200;
  obstacleAngle = 0;
  bool irLeft = readIR(IR_LEFT_PIN);
  bool irRight = readIR(IR_RIGHT_PIN);

  for (int i = 0; i < NUM_LASERS; i++) {
    uint16_t dist = readLaser(LASER_PINS[i]);
    // 方位对应：0=前，1=左前，2=右前，3=左，4=右，5=后
    if (dist < minDist) {
      minDist = dist;
      obstacleAngle = i;
    }
  }
  // 红外补盲：近距离障碍优先
  if (irLeft) {
    if (15 < minDist) { minDist =15; obstacleAngle =1; }
  }
  if (irRight) {
    if (15 < minDist) { minDist = 15; obstacleAngle =2; }
  }
}

// 读取激光测距（模拟量转距离，适配1~200cm工业场景）
uint16_t readLaser(uint8_t pin) {
  int val = analogRead(pin);
  uint16_t dist = map(val, 0, 1023, 200, 10);
  dist = constrain(dist, 5, 200);
  return dist;
}

// 读取红外补盲
bool readIR(uint8_t pin) {
  return digitalRead(pin) == HIGH;
}

// 模糊控制核心算法
void fuzzyControl() {
  // 模糊输入：距离与方位
  input_distance = (float)minDist;
  // 方位转换为方向值：前=0，左=-1，右=1，后=2
  if (obstacleAngle == 0) input_angle = 0;
  else if (obstacleAngle ==1 || obstacleAngle ==3) input_angle =-1;
  else if (obstacleAngle ==2 || obstacleAngle ==4) input_angle =1;
  else input_angle =2;

  // 模糊推理规则（工业安全优先，平缓响应）
  output_steering = 0;
  output_speed_gain = 1.0f;

  if (input_distance < OBSTACLE_NEAR) {
    // 近距离：全速避让+大幅转向
    output_speed_gain = 0.2f;
    if (input_angle == -1) output_steering = -STEERING_MAX;    // 左侧障碍，右转
    else if (input_angle == 1) output_steering = STEERING_MAX; // 右侧障碍，左转
    else if (input_angle == 0) {
      // 正前方近距离，随机转向（或默认左转）
      output_steering = -STEERING_MAX * 0.8f;
    } else {
      // 后方近距离，向前微转（安全优先，避免后退）
      output_steering = 0;
      output_speed_gain = 0.1f;
    }
  } else if (input_distance < OBSTACLE_MID) {
    // 中距离：减速+适度转向
    output_speed_gain = 0.6f;
    if (input_angle == -1) output_steering = -STEERING_MAX * 0.6f;
    else if (input_angle == 1) output_steering = STEERING_MAX * 0.6f;
    else output_steering = 0;
  } else if (input_distance < OBSTACLE_FAR) {
    // 远距离：轻微调整+小幅减速
    output_speed_gain = 0.85f;
    if (input_angle == -1) output_steering = -STEERING_MAX * 0.3f;
    else if (input_angle == 1) output_steering = STEERING_MAX * 0.3f;
    else output_steering = 0;
  }
  // 距离大于FAR：正常巡检，无避让
}

// 电机控制
void controlMotors() {
  float leftSpeed = BASE_SPEED * output_speed_gain - output_steering * 0.5;
  float rightSpeed = BASE_SPEED * output_speed_gain + output_steering * 0.5;

  // 速度限制与平滑
  leftSpeed = constrain(leftSpeed, 0, BASE_SPEED * 1.2f);
  rightSpeed = constrain(rightSpeed, 0, BASE_SPEED * 1.2f);

  int leftPWM = leftSpeed * 5;
  int rightPWM = rightSpeed * 5;

  analogWrite(LEFT_MOTOR_PWM, leftPWM);
  analogWrite(RIGHT_MOTOR_PWM, rightPWM);

  // 前进控制
  digitalWrite(LEFT_MOTOR_IN1, HIGH);
  digitalWrite(LEFT_MOTOR_IN2, LOW);
  digitalWrite(RIGHT_MOTOR_IN1, HIGH);
  digitalWrite(RIGHT_MOTOR_IN2, LOW);
}

// 状态显示
void displayStatus() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("模糊避障巡检");
  display.setCursor(0, 20);
  display.print("障碍距离: "); display.print(minDist); display.print("cm");
  display.setCursor(0, 40);
  display.print("速度增益: "); display.print(output_speed_gain, 2);
  display.setCursor(0, 60);
  display.print("转向增益: "); display.println(output_steering, 2);
  display.display();
}

案例优化方向
加入障碍物类型识别，区分高压管道与普通设备，调整避让优先级
引入距离变化率计算，预判障碍物动态（如移动的设备、人员），提升避让范围
增加避让路径记忆，避免同一位置反复绕行，提升巡检效率

5、化工储罐区气体泄漏浓度定位程序（核心：梯度追踪实现泄漏点定位）
应用场景：化工园区储罐区的高压气体泄漏巡检，场景特点：泄漏气体随风扩散，浓度呈梯度分布，需通过浓度变化追踪，精准定位泄漏点位置，为后续抢修提供依据。

核心逻辑
浓度梯度检测：实时采集机器人周围不同方位的气体浓度，计算浓度梯度方向
风速风向修正：结合环境风速风向，修正浓度扩散方向，排除风的干扰
梯度追踪算法：控制机器人沿浓度升高方向移动，逐步逼近泄漏点
定位记录：实时记录浓度最高点的位置坐标，标记泄漏点

// 化工储罐区气体泄漏浓度定位程序
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_BME280.h> // 风速风向模块+环境传感器（示例，实际需适配风速风向传感器）

// ===== 硬件引脚定义 =====
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_IN1 A2
#define LEFT_MOTOR_IN2 A3
#define RIGHT_MOTOR_PWM 6
#define RIGHT_MOTOR_IN1 A4
#define RIGHT_MOTOR_IN2 A5

// 气体浓度传感器（4路：前、左、右、后，覆盖多方位浓度采集）
#define GAS_SENSOR_PINS {A0, A1, A2, A3}
#define NUM_GAS_SENSORS 4
// 风速风向传感器（I2C接口，适配主流模块）
Adafruit_BME280 bme; // 示例：BME280可扩展为风速风向，实际需匹配具体模块

Adafruit_SSD1306 display(128, 64, &Wire, -1);

// ===== 定位参数 =====
const float BASE_SPEED = 8.0f;   // 定位阶段低速，保证检测精度
const float CONCENTRATION_HIGH = 800; // 高浓度阈值（ppm，需根据气体类型校准）
const float WIND_CORRECT_FACTOR = 0.7f; // 风速修正系数
const uint16_t LOCATION_CYCLE = 150;  // 定位采样周期（ms）

// ===== 状态变量 =====
float gasConcentration[4];  // 四方位浓度值
float maxConcentration = 0; // 最大浓度值
int maxDirection = 0;       // 最大浓度方位（0=前，1=左，2=右，3=后）
float windSpeed = 0;        // 风速（m/s）
float windDirection = 0;    // 风向（°）
float gradientX = 0, gradientY = 0; // 浓度梯度方向
float targetX = 0, targetY = 0;    // 目标点坐标（泄漏点）
bool foundLeak = false;     // 是否找到泄漏点

// ===== 函数声明 =====
void readGasConcentration();
void measureWind();
void calculateGradient();
void trackLeak();
void controlMotors();
void displayInfo();
float readGas(uint8_t pin);

void setup() {
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 128, 64);

  // 电机初始化
  pinMode(LEFT_MOTOR_IN1, OUTPUT);
  pinMode(LEFT_MOTOR_IN2, OUTPUT);
  pinMode(RIGHT_MOTOR_IN1, OUTPUT);
  pinMode(RIGHT_MOTOR_IN2, OUTPUT);

  // 气体传感器初始化
  for (int i = 0; i < NUM_GAS_SENSORS; i++) {
    pinMode(GAS_SENSOR_PINS[i], INPUT);
  }

  // 风速传感器初始化（示例，实际需适配具体模块）
  if (!bme.begin(0x76)) {
    Serial.println("风速传感器初始化失败");
    while(1);
  }

  Serial.println("浓度定位系统初始化完成");
}

void loop() {
  // 1. 采样浓度与风速
  if (millis() - lastLoopTime > LOCATION_CYCLE) {
    readGasConcentration();
    measureWind();
    calculateGradient();
  }

  // 2. 泄漏追踪控制
  if (!foundLeak) {
    trackLeak();
  } else {
    // 找到泄漏点，停止移动并记录
    controlMotors(0, 0);
  }

  // 3. 显示与调试
  displayInfo();
  Serial.print("最大浓度: "); Serial.print(maxConcentration, 1);
  Serial.print("ppm 方位: "); Serial.println(maxDirection);
  Serial.print("梯度方向: "); Serial.print(gradientX, 2); Serial.print(","); Serial.println(gradientY, 2);
  delay(10);
}

uint16_t lastLoopTime = 0;

// 读取四方位气体浓度
void readGasConcentration() {
  maxConcentration = 0;
  for (int i = 0; i < NUM_GAS_SENSORS; i++) {
    gasConcentration[i] = readGas(GAS_SENSOR_PINS[i]);
    if (gasConcentration[i] > maxConcentration) {
      maxConcentration = gasConcentration[i];
      maxDirection = i;
    }
  }
}

// 读取气体浓度（模拟量转浓度，需根据传感器校准）
float readGas(uint8_t pin) {
  int val = analogRead(pin);
  float concentration = map(val, 0, 1023, 0, 1500); // 适配0~1500ppm范围
  concentration = constrain(concentration, 0, 2000);
  return concentration;
}

// 测量风速风向（示例，实际需适配具体传感器接口）
void measureWind() {
  // 从风速传感器读取数据，此处为简化模拟，实际需替换为具体驱动
  windSpeed = bme.readPressure() / 100.0f * 0.5f; // 简化计算，仅作示例
  windDirection = 90; // 简化为东风，实际需根据传感器读取
  windDirection = constrain(windDirection, 0, 359);
}

// 计算浓度梯度（结合风向修正）
void calculateGradient() {
  // 基础梯度：前-后、左-右的浓度差
  float frontBack = gasConcentration[0] - gasConcentration[3];
  float leftRight = gasConcentration[1] - gasConcentration[2];

  // 风速修正：风向与浓度梯度叠加，排除风的扩散影响
  // 简化计算：将风向转换为X/Y分量，叠加到梯度中
  float windX = cos(radians(windDirection)) * WIND_CORRECT_FACTOR;
  float windY = sin(radians(windDirection)) * WIND_CORRECT_FACTOR;

  // 梯度归一化并叠加风修正
  gradientY = (frontBack / maxConcentration) + windY;
  gradientX = (leftRight / maxConcentration) + windX;

  // 梯度阈值过滤，避免噪声干扰
  float threshold = 0.05f;
  if (abs(gradientX) < threshold) gradientX = 0;
  if (abs(gradientY) < threshold) gradientY = 0;
}

// 泄漏追踪控制
void trackLeak() {
  // 达到高浓度阈值，判定找到泄漏点
  if (maxConcentration >= CONCENTRATION_HIGH && abs(gradientX) < 0.1 && abs(gradientY) < 0.1) {
    foundLeak = true;
    targetX = gradientX;
    targetY = gradientY;
    return;
  }

  // 根据梯度方向规划运动：沿梯度升高方向移动
  float leftSpeed = BASE_SPEED;
  float rightSpeed = BASE_SPEED;

  // Y方向（前后）：梯度Y为正，向前移动；为负，向后移动
  if (gradientY > 0) {
    // 向前，保持速度
  } else if (gradientY < 0) {
    // 向后，减速后退
    leftSpeed = -BASE_SPEED * 0.5f;
    rightSpeed = -BASE_SPEED * 0.5f;
  }

  // X方向（左右）：梯度X为正，左转；为负，右转
  if (gradientX > 0) {
    leftSpeed += BASE_SPEED * abs(gradientX) * 2;
  } else if (gradientX < 0) {
    rightSpeed += BASE_SPEED * abs(gradientX) * 2;
  }

  // 速度限制
  leftSpeed = constrain(leftSpeed, -BASE_SPEED * 0.5f, BASE_SPEED * 1.5f);
  rightSpeed = constrain(rightSpeed, -BASE_SPEED * 0.5f, BASE_SPEED * 1.5f);

  controlMotors(leftSpeed, rightSpeed);
}

// 电机控制
void controlMotors(float leftSpeed, float rightSpeed) {
  if (leftSpeed < 0) {
    digitalWrite(LEFT_MOTOR_IN1, LOW);
    digitalWrite(LEFT_MOTOR_IN2, HIGH);
  } else {
    digitalWrite(LEFT_MOTOR_IN1, HIGH);
    digitalWrite(LEFT_MOTOR_IN2, LOW);
  }
  if (rightSpeed < 0) {
    digitalWrite(RIGHT_MOTOR_IN1, LOW);
    digitalWrite(RIGHT_MOTOR_IN2, HIGH);
  } else {
    digitalWrite(RIGHT_MOTOR_IN1, HIGH);
    digitalWrite(RIGHT_MOTOR_IN2, LOW);
  }

  int leftPWM = abs(leftSpeed) * 5;
  int rightPWM = abs(rightSpeed) * 5;
  analogWrite(LEFT_MOTOR_PWM, leftPWM);
  analogWrite(RIGHT_MOTOR_PWM, rightPWM);
}

// 状态显示
void displayInfo() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("泄漏浓度定位");
  display.setCursor(0, 20);
  display.print("最大浓度: "); display.print(maxConcentration, 1); display.print("ppm");
  display.setCursor(0, 40);
  display.print("风速: "); display.print(windSpeed, 1); display.print("m/s");
  display.setCursor(0, 60);
  display.print("泄漏判定: "); display.println(foundLeak ? "是" : "否");
  display.display();
}

案例优化方向
增加多气体传感器适配，检测多种高压气体泄漏，扩展应用场景
优化梯度算法，加入历史浓度数据，降低传感器波动对定位的影响
联动定位模块，保存泄漏点的精确坐标，生成巡检报告

6、高压管网泄漏浓度定位采样一体化程序（核心：定位+采样+避障协同）
应用场景：工业园区高压输气管道巡检，场景特点：管道沿路铺设，路径长、障碍物多为支架、阀门，需兼顾避障与浓度定位，定位到泄漏点后完成定量采样，为后续分析提供样本。

核心逻辑
巡检-定位协同：巡检过程中同时进行浓度检测与避障，既不碰撞障碍，又不遗漏泄漏点
精准采样控制：定位到高浓度区域后，控制机器人靠近泄漏点，启动气泵完成定量采样
安全联动：浓度超标时触发告警，同时加强避障优先级，确保机器人安全
结果记录：记录采样点位置、浓度、采样时间，生成完整巡检数据

// 高压管网泄漏定位采样一体化程序
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_BME280.h>
#include <Servo.h>

// ===== 硬件引脚定义 =====
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_IN1 A2
#define LEFT_MOTOR_IN2 A3
#define RIGHT_MOTOR_PWM 6
#define RIGHT_MOTOR_IN1 A4
#define RIGHT_MOTOR_IN2 A5

#define LASER_PINS {A0, A1, A2, A3, A4, A5} // 6路激光，避障
#define GAS_SENSOR_PINS {A0, A1, A2, A3}   // 4路气体，定位（实际需复用或扩展引脚）
#define PUMP_PIN 8                          // 气泵控制
#define SERVO_PIN 10                        // 采样口转向舵机

Adafruit_SSD1306 display(128, 64, &Wire,-1);
Adafruit_BME280 bme;
Servo sampleServo;

// ===== 控制参数 =====
const float BASE_SPEED = 10.0f;
const float SAMPLE_SPEED = 5.0f;    // 采样阶段低速
const float CONCENTRATION_SAMPLE = 500; // 采样浓度阈值（ppm）
const uint16_t PUMP_SAMPLE_TIME = 3000; // 采样时间（ms）
const uint16_t OBSTACLE_NEAR = 25;  // 避障近距离阈值

// ===== 状态变量 =====
enum SystemState { NORMAL_INSPECT, TRACKING, SAMPLING, ALARM }; // 系统状态
SystemState state = NORMAL_INSPECT;

float maxConcentration = 0;
int maxDirection = 0;
uint16_t minObstacleDist = 200;
bool sampleComplete = false;
unsigned long lastScanTime = 0;

// ===== 函数声明 =====
void scanAll();      // 扫描障碍物+浓度
void fuzzyAvoidance(); // 模糊避障
void concentrationTracking(); // 浓度追踪
void sampleProcess(); // 采样控制
void controlMotors();
void displayAll();

void setup() {
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 128, 64);

  // 电机初始化
  pinMode(LEFT_MOTOR_IN1, OUTPUT);
  pinMode(LEFT_MOTOR_IN2, OUTPUT);
  pinMode(RIGHT_MOTOR_IN1, OUTPUT);
  pinMode(RIGHT_MOTOR_IN2, OUTPUT);

  // 传感器初始化
  for (int i = 0; i < 6; i++) pinMode(LASER_PINS[i], INPUT);
  pinMode(PUMP_PIN, OUTPUT);
  sampleServo.attach(SERVO_PIN);
  sampleServo.write(0); // 采样口关闭

  if (!bme.begin(0x76)) {
    Serial.println("风速传感器初始化失败");
    while(1);
  }

  Serial.println("定位采样一体化系统初始化完成");
}

void loop() {
  // 1. 周期扫描所有数据
  if (millis() - lastScanTime > 100) {
    scanAll();
    lastScanTime = millis();
  }

  // 2. 分状态执行控制
  switch (state) {
    case NORMAL_INSPECT:    // 正常巡检：避障+浓度检测
      fuzzyAvoidance();
      if (maxConcentration > CONCENTRATION_SAMPLE) {
        state = TRACKING;
      }
      break;
    case TRACKING:          // 浓度追踪：低俗追踪+避障
      concentrationTracking();
      if (maxConcentration > CONCENTRATION_SAMPLE * 1.5) {
        state = SAMPLING;
      }
      break;
    case SAMPLING:         // 采样：固定位置+气泵采样
      sampleProcess();
      break;
    case ALARM:            // 告警：浓度超高，安全停止
      controlMotors(0, 0);
      break;
  }

  // 3. 显示与调试
  displayAll();
  Serial.print("状态: ");
  switch(state) {
    case NORMAL_INSPECT: Serial.println("巡检"); break;
    case TRACKING: Serial.println("追踪"); break;
    case SAMPLING: Serial.println("采样"); break;
    case ALARM: Serial.println("告警"); break;
  }
  Serial.print("最大浓度: "); Serial.println(maxConcentration, 1);
  delay(5);
}

// 扫描障碍物+浓度
void scanAll() {
  // 避障扫描
  minObstacleDist = 200;
  for (int i = 0; i < 6; i++) {
    int val = analogRead(LASER_PINS[i]);
    uint16_t dist = map(val, 0, 1023, 200, 10);
    if (dist < minObstacleDist) minObstacleDist = dist;
  }

  // 浓度扫描
  maxConcentration = 0;
  for (int i = 0; i < 4; i++) {
    int val = analogRead(GAS_SENSOR_PINS[i]);
    float concentration = map(val, 0, 1023, 0, 1500);
    if (concentration > maxConcentration) {
      maxConcentration = concentration;
      maxDirection = i;
    }
  }

  // 超高浓度触发告警
  if (maxConcentration > 1200) state = ALARM;
}

// 模糊避障（简化版，与案例1逻辑一致，适配巡检状态）
void fuzzyAvoidance() {
  float leftSpeed = BASE_SPEED;
  float rightSpeed = BASE_SPEED;

  if (minObstacleDist < OBSTACLE_NEAR) {
    // 近距离避让
    leftSpeed = BASE_SPEED * 0.3f;
    rightSpeed = BASE_SPEED * 0.3f;
    // 简单转向：默认左转避让
    rightSpeed -= BASE_SPEED * 0.5f;
  } else if (minObstacleDist < 60) {
    // 中距离减速
    leftSpeed = BASE_SPEED * 0.7f;
    rightSpeed = BASE_SPEED * 0.7f;
  }

  controlMotors(leftSpeed, rightSpeed);
}

// 浓度追踪（与案例2逻辑一致，低速追踪）
void concentrationTracking() {
  float leftSpeed = SAMPLE_SPEED;
  float rightSpeed = SAMPLE_SPEED;

  // 根据最大浓度方位转向
  if (maxDirection == 1 || maxDirection == 3) { // 左侧浓度高，左转
    leftSpeed += SAMPLE_SPEED * 0.4f;
  } else if (maxDirection == 2 || maxDirection ==4) { // 右侧浓度高，右转
    rightSpeed += SAMPLE_SPEED * 0.4f;
  } // 同时保留避障
  if (minObstacleDist < OBSTACLE_NEAR) {
    leftSpeed = SAMPLE_SPEED * 0.5f;
    rightSpeed = SAMPLE_SPEED * 0.5f;
    rightSpeed -= SAMPLE_SPEED * 0.5f;
  }

  controlMotors(leftSpeed, rightSpeed);
}

// 采样控制
void sampleProcess() {
  // 停止移动，确保采样稳定
  controlMotors(0, 0);
  
  if (!sampleComplete) {
    // 打开采样口，启动气泵
    sampleServo.write(90);
    digitalWrite(PUMP_PIN, HIGH);
    
    // 采样时间达到，关闭气泵
    if (millis() - lastScanTime > PUMP_SAMPLE_TIME) {
      digitalWrite(PUMP_PIN, LOW);
      sampleServo.write(0);
      sampleComplete = true;
      Serial.println("采样完成，样本已保存");
    }
  } else {
    // 采样完成，可扩展为返回或继续巡检
  }
}

// 电机控制
void controlMotors(float leftSpeed, float rightSpeed) {
  leftSpeed = constrain(leftSpeed, 0, BASE_SPEED * 1.5f);
  rightSpeed = constrain(rightSpeed, 0, BASE_SPEED * 1.5f);

  int leftPWM = leftSpeed *5;
  int rightPWM = rightSpeed *5;

  analogWrite(LEFT_MOTOR_PWM, leftPWM);
  analogWrite(RIGHT_MOTOR_PWM, rightPWM);

  digitalWrite(LEFT_MOTOR_IN1, HIGH);
  digitalWrite(LEFT_MOTOR_IN2, LOW);
  digitalWrite(RIGHT_MOTOR_IN1, HIGH);
  digitalWrite(RIGHT_MOTOR_IN2, LOW);
}

// 信息显示
void displayAll() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("定位采样巡检");
  display.setCursor(0, 20);
  display.print("最大浓度: "); display.print(maxConcentration,1); display.print("ppm");
  display.setCursor(0, 40);
  display.print("障碍距离: "); display.print(minObstacleDist); display.print("cm");
  display.setCursor(0, 60);
  display.print("采样状态: "); display.println(sampleComplete ? "完成" : "未完成");
  display.display();
}

案例优化方向
增加采样量化控制，精确控制气泵采样体积，提升样本一致性
加入泄漏分级，不同浓度等级采取不同措施（如高浓度禁靠近、低浓度靠近采样）
联动上位机，实时上传巡检数据、采样结果，便于后台分析与决策

要点解读
要点1：模糊避障需贴合工业场景的安全优先级，实现“安全+平稳”的双重要求
工业高压环境对安全的要求远高于普通场景，模糊避障的核心是牺牲部分效率，保障绝对安全。
模糊规则设计：工业场障碍物优先级按“高压设备>普通设备>地面障碍”排序，距离越近，避让响应越强，确保不碰撞高危设备
平稳性优化：避免急停急转，采用平滑的转向与减速，既防止机器人倾倒，也避免采样时的振动影响样本精度
安全兜底：设置多级安全阈值，近距离（<20cm）强制停车、浓度超标时进入告警状态，优先保障机器人与人员安全

要点2：浓度梯度追踪必须结合风速风向修正，排除环境干扰提升定位精度
高压气体泄漏后会随风扩散，单纯的浓度梯度会被风干扰，导致定位偏差，风速风向修正是提升定位精度的核心。
气体扩散规律：泄漏气体会向风向的反方向（上风向为泄漏源）移动，需根据风速大小、风向角度，修正浓度梯度的方向
修正算法：将风的矢量分量叠加到浓度梯度中，过滤无效梯度、抑制噪声，确保机器人始终向泄漏点移动
传感器适配：选用工业级风速风向传感器，确保在化工园区复杂环境下数据采集稳定，避免因传感器波动导致定位错误

要点3：定位与采样的“分阶段协同”是核心，避免同时运行导致精度与效率双降
浓度定位需要低速运动保证检测精度，而巡检需要一定速度保证效率，分阶段切换是平衡二者的关键。
三阶段控制：正常巡检（高速+粗略浓度检测）→泄漏追踪（低速+精准梯度计算）→采样（静止+精准采样），不同阶段切换速度与检测精度
协同逻辑：巡检阶段筛选疑似泄漏点，追踪阶段精准定位，采样阶段固定位置采集，各阶段功能独立、逻辑联动
效率优化：仅在浓度超标时切换到低速追踪，正常路段保持巡检速度，避免全程低速影响巡检效率

要点4：传感器的“工业级适配+融合校准”是检测可靠的基础，恶劣环境下不失效
化工园区存在高温、粉尘、腐蚀气体等恶劣条件，传感器需适配工业环境，且需融合校准降低误差。
传感器选型：激光测距选用工业级、防尘防腐蚀型号；气体传感器需适配目标气体，支持宽量程、抗干扰
融合校准：多路气体传感器交叉比对、激光与红外互补，降低单一传感器失效或误差的影响
滤波优化：对浓度、距离数据进行滑动窗口滤波，压制噪声，避免因传感器抖动导致误判、误动作

要点5：系统需具备“安全联锁”机制，双重保障避免安全事故
工业高压场景一旦发生事故后果严重，需构建多维度安全联锁，实现主动与被动双重防护。
浓度安全联锁：浓度超过安全阈值时，禁止靠近、触发告警，必要时自动撤离
避障安全联锁：近距离障碍强制停车，与运动系统联动，确保不会因控制失效导致碰撞
设备安全联锁：采样装置、电机、传感器异常时，立即进入安全状态，避免设备损坏或采样失效

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