【花雕学编程】Arduino BLDC 之机器人模糊沿墙巡检 + 气体梯度采样调制

“Arduino BLDC之机器人模糊沿墙巡检 + 气体梯度采样调制”是一个典型的软硬件深度耦合系统，专为资源受限的嵌入式平台设计。其核心设计哲学是：在Arduino等算力有限的微控制器上，通过模糊逻辑将复杂的二维沿墙导航降维为可控的单自由度问题，利用BLDC电机的无火花特性满足防爆要求，并引入气体感知调制策略实现泄漏源的自主追踪与梯度采样。以下从主要特点、应用场景及注意事项三个维度进行专业解析。

主要特点

应用场景
该系统凭借其低成本、高鲁棒性和防爆特性，主要应用于以下场景：
智能密集仓储巡检：穿梭于宽度仅60厘米左右的货架巷道，执行库存盘点或环境监测任务。BLDC的大扭矩使其能搭载扫码器或RFID读卡器，模糊逻辑确保在货架轻微变形或地面不平的情况下，始终与货架保持约10厘米的安全距离。
化工厂/实验室泄漏源定位：在化工厂区或实验室中，自主巡检并追踪有毒或易燃气体泄漏源。BLDC的无火花特性满足防爆要求，气体梯度调制策略使其能自主逼近泄漏点，为应急处理提供精准定位。
无人零售/图书馆夜间巡检：在夜间无人时段，沿着冰柜边缘或书架自动巡逻，检查缺货情况或盘点图书。低噪音特性允许其在人员作业时段同时运行，不影响正常工作。
管道/隧道内部探测：在狭窄的管道或隧道内部，沿管壁自主移动，检测内部的可燃气体浓度、温湿度等参数，并通过物联网模块远程回传数据。

需要注意的事项
在实现该系统时，需重点关注以下技术细节与工程红线：

1、三传感器模糊沿墙 + 气体梯度调制基础版
场景：化工管廊巡检，机器人通过侧向红外沿墙，同时根据气体浓度梯度动态调整采样频率与运动速度。

/* ===== 三传感器模糊沿墙 + 气体梯度调制 =====
* 硬件：Arduino + SimpleFOC BLDC + 侧向红外 + 气体传感器
* 核心：模糊沿墙保持稳定距离 + 气体梯度调制采样与速度
*/
#include <SimpleFOC.h>

// — BLDC 差速底盘 —
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;

// — 气体采样调制参数 —
float sampleFreq = 100; // 默认采样间隔 ms
float lastSampleTime = 0;
float gasConc = 0, lastGasConc = 0, gasGradient = 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;

// 模糊规则：距离太近/太远/适中
if (midError < –0.5) {
output = –0.8 – angleError * 0.5; // 太近 → 向外转
} else if (midError > 0.5) {
output = 0.8 – angleError * 0.5; // 太远 → 向内转
} 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);
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;
}

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);

// 2. 采样频率动态调制
if (millis() – lastSampleTime > sampleFreq) {
lastSampleTime = millis();
gasConc = rawGas;
Serial.print("Gas:"); Serial.print(gasConc);
Serial.print(" Grad:"); Serial.print(gasGradient);
Serial.print(" Freq:"); Serial.println(sampleFreq);
}

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

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

// 5. 融合控制：沿墙 + 梯度转向
float wheelBase = 0.3;
float leftSpd = vx – (wz + fuzzyWallOut) * wheelBase / 2;
float rightSpd = vx + (wz + fuzzyWallOut) * wheelBase / 2;

motorL.move(leftSpd);
motorR.move(rightSpd);
delay(20);
}

核心要点：模糊沿墙将距离误差和角度误差同时模糊化，输出连续转向修正，避免强电磁干扰下传感器噪声导致的频繁启停。采样频率与运动速度联动调制是关键——浓度上升时主动降速并加密采样，防止高速掠过泄漏点导致漏检。

2、卡尔曼滤波抗干扰 + 状态机多模式切换
场景：强电磁环境管廊，气体传感器信号受干扰剧烈，通过卡尔曼滤波平滑数据，状态机根据气体浓度和打滑状态切换工作模式。

/* ===== 卡尔曼滤波 + 状态机多模式巡检 =====
* 核心：抗电磁干扰滤波 + 正常/窄通道/打滑/气体报警四模式
*/
#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;

// 卡尔曼滤波
KalmanFilter kf_gas(0.01, 1.0, 0.001);

// 状态模式
enum RobotMode { NORMAL_MODE, NARROW_CHANNEL, SLIP_DETECTED, GAS_ALERT };
RobotMode currentMode = NORMAL_MODE;

// 参数
const float NARROW_DIST = 150;
const float SLIP_THRESHOLD = 0.3;
const float GAS_ALERT_THRESHOLD = 400;

// 模糊沿墙
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;

if (midError < –0.5) output = –0.8 – angleError * 0.5;
else if (midError > 0.5) output = 0.8 – angleError * 0.5;
else output = –angleError * 1.2;

return constrain(output, –1.0, 1.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;

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 gasConc = kf_gas.updateEstimate(rawGas);

// 2. 模式判断
float slipRate = 0; // 简化：实际由编码器与IMU对比得出
if (gasConc >= GAS_ALERT_THRESHOLD * 0.5) {
currentMode = GAS_ALERT;
} else if (slipRate > SLIP_THRESHOLD) {
currentMode = SLIP_DETECTED;
} else if (leftDist < NARROW_DIST && rightDist < NARROW_DIST) {
currentMode = NARROW_CHANNEL;
} else {
currentMode = NORMAL_MODE;
}

// 3. 模式控制
float leftSpeed = 0, rightSpeed = 0;
float vx = 0.35, wz = 0;
float fuzzyWallOut = fuzzyWallControl(leftDist, midDist, rightDist);

switch (currentMode) {
case NORMAL_MODE:
leftSpeed = vx – (wz + fuzzyWallOut) * 0.15;
rightSpeed = vx + (wz + fuzzyWallOut) * 0.15;
break;

case NARROW_CHANNEL:
// 窄通道：减速居中
float centerError = leftDist – rightDist;
leftSpeed = 0.2 + centerError * 0.05;
rightSpeed = 0.2 – centerError * 0.05;
break;

case SLIP_DETECTED:
// 打滑：大幅减速，向墙侧微调
leftSpeed = 0.15 * (leftDist < rightDist ? 0.5 : 0.7);
rightSpeed = 0.15 * (leftDist < rightDist ? 0.7 : 0.5);
Serial.println("SLIP DETECTED! Reducing speed.");
break;

case GAS_ALERT:
// 气体报警：减速避险
float speedScale = 0.5;
leftSpeed = 0.3 * speedScale;
rightSpeed = 0.3 * speedScale;
if (leftDist < 200) {
leftSpeed *= 0.8;
rightSpeed *= 0.6;
}
break;
}

motorL.move(constrain(leftSpeed, –0.5, 0.5));
motorR.move(constrain(rightSpeed, –0.5, 0.5));
delay(30);
}

核心要点：卡尔曼滤波抑制强电磁干扰导致的数据跳变，使气体浓度读数更可靠。状态机将模糊控制与多模式架构结合，在狭窄通道减速居中、打滑时向墙侧微调恢复抓地力、气体报警时优先减速避险，实现分层决策。

3、自适应阈值气体追踪 + 外螺旋泄漏源定位
场景：从沿墙巡检切换到泄漏源搜索，采用外螺旋覆盖搜索，自适应阈值判断是否进入“气味羽流”，切换为浓度质心逼近算法。

/* ===== 自适应阈值追踪 + 外螺旋源定位 =====
* 核心：移动平均自适应阈值 + 外螺旋搜索 + 浓度质心逼近
*/
#include <SimpleFOC.h>

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;

// 搜索状态
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, spiralRadius = 0;
const float thetaP = 45 * PI / 180;

// 浓度质心
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 readGas() {
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 = 0.9 * adaptiveThreshold + 0.1 * movingAvg;
}

// 外螺旋航点
void computeSpiralWaypoint(float &tx, float &ty) {
spiralAngle += thetaP;
spiralRadius = (spiralAngle / (2 * PI)) * spiralPitch;
tx = spiralCenterX + spiralRadius * cos(spiralAngle);
ty = spiralCenterY + spiralRadius * sin(spiralAngle);
}

// 浓度质心估算
void estimateSourcePosition() {
if (highConcCount == 0) return;
float sumX = 0, sumY = 0;
for (int i = 0; i < highConcCount; i++) {
sumX += highConcPointsX[i];
sumY += highConcPointsY[i];
}
estimatedSourceX = sumX / highConcCount;
estimatedSourceY = sumY / highConcCount;
}

// 差速运动到目标点
void goToPoint(float tx, float ty) {
float dx = tx – robotX, dy = ty – 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, wheelR = 0.05;
motorL.move((v – w * wheelBase / 2.0) / wheelR);
motorR.move((v + w * wheelBase / 2.0) / wheelR);

robotX += v * cos(robotYaw) * 0.05;
robotY += v * sin(robotYaw) * 0.05;
robotYaw += w * 0.05;
}

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

currentConc = readGas();
updateAdaptiveThreshold(currentConc);

Serial.print("State:"); Serial.print(searchState);
Serial.print(" Conc:"); Serial.print(currentConc);
Serial.print(" Thresh:"); Serial.println(adaptiveThreshold);

switch (searchState) {
case SEARCH_SPIRAL: {
// 进入羽流判据：浓度显著高于自适应阈值
if (currentConc > adaptiveThreshold * 1.5 && currentConc > 100) {
searchState = SEARCH_GRADIENT;
Serial.println(">>> Entered plume");
break;
}
float tx, ty;
computeSpiralWaypoint(tx, ty);
goToPoint(tx, ty);

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!");
}
break;
}

case SOURCE_CONFIRMED: {
motorL.move(0);
motorR.move(0);
Serial.print("SOURCE at ("); Serial.print(estimatedSourceX);
Serial.print(", "); Serial.print(estimatedSourceY);
Serial.print(") Conc:"); Serial.println(currentConc);
break;
}
}
delay(50);
}

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

要点解读

4、模糊沿墙巡检+基础气体梯度采样程序（核心：沿墙稳定行驶与气体浓度采集）
适用场景：工业管道、储罐外围等规则墙体场景，机器人沿墙行驶完成巡检，同步采集气体浓度，实现基础泄露排查。

核心逻辑
采用模糊控制算法，融合多侧红外测距数据，实现平滑沿墙行驶
结合巡检路径切换采样点位，采集沿途气体浓度并保存
当检测到气体浓度异常时，主动贴近墙体排查泄露源

// 模糊沿墙巡检+基础气体梯度采样程序
// 硬件：4路红外测距、MQ系列气体传感器、BLDC差速驱动
// 功能：规则墙体沿墙行驶，同步采集气体浓度，基础泄露检测

#include <Arduino.h>

// ========== 硬件引脚定义 ==========
const byte IR_LEFT = A0, IR_RIGHT = A1; // 左右沿墙红外传感器
const byte MQ_PIN = A2; // 气体传感器输出引脚
const byte PWM_L = 6, DIR_L = 7, PWM_R = 8, DIR_R = 9;// BLDC驱动

// ========== 模糊沿墙参数 ==========
const float WALL_DIST = 20.0; // 目标沿墙距离（cm）
float speed_left = 180, speed_right = 180; // 初始电机速度（0-255）
int fuzzy_output = 0; // 模糊控制输出

// 模糊控制隶属度函数参数
const int DIST_MIN = 10, DIST_MAX = 50; // 距离输入范围
const int LOW_NP=0, MIDL_NP=1, HIGH_NP=2, MAX_NP=3; // 距离隶属度等级
float membership[4]; // 左右距离隶属度
float speed_adj_L=0, speed_adj_R=0; // 速度调整量

// ========== 气体采样参数 ==========
float gas_concentration = 0.0; // 当前气体浓度
float baseline_gas = 0.0; // 基准浓度
const uint16_t SAMPLE_INTERVAL = 2000; // 采样间隔（ms）
const float GAS_ALARM = 100.0; // 浓度告警阈值（实际需标定）
bool gas_alarm = false;

unsigned long lastSampleTime = 0;
const byte ROBOT_MODE = 0; // 0=沿墙巡检模式

// ========== 函数声明 ==========
void fuzzyWallFollowing();
float getIRDistance(byte pin);
float readGasSensor();
void sampleGas();
void driveMotor(float left, float right);

void setup() {
Serial.begin(115200);
Serial.println("模糊沿墙+气体采样系统初始化完成");

pinMode(IR_LEFT, INPUT);
pinMode(IR_RIGHT, INPUT);
pinMode(MQ_PIN, INPUT);

pinMode(PWM_L, OUTPUT); pinMode(DIR_L, OUTPUT);
pinMode(PWM_R, OUTPUT); pinMode(DIR_R, OUTPUT);

baseline_gas = readGasSensor();
Serial.print("气体基准浓度：");
Serial.println(baseline_gas);
}

void loop() {
fuzzyWallFollowing(); // 1. 模糊沿墙控制
sampleGas(); // 2. 气体采样
delay(10);
}

// 获取红外传感器距离（cm）
float getIRDistance(byte pin) {
int val = analogRead(pin);
return val * 0.08f; // 按实际传感器校准系数调整
}

// 模糊沿墙控制核心
void fuzzyWallFollowing() {
float dist_L = getIRDistance(IR_LEFT);
float dist_R = getIRDistance(IR_RIGHT);

// 1. 隶属度计算：将距离映射为低压、中压、高压、超高压
// 距离越近，隶属度越高
membership[LOW_NP] = (dist_L < DIST_MIN) ? 1.0f : (dist_L < DIST_MAX) ? (DIST_MAX–dist_L)/(DIST_MAX–DIST_MIN) : 0.0f;
membership[MIDL_NP] = (dist_L >= DIST_MIN && dist_L <= DIST_MAX) ?
((dist_L < (DIST_MIN+20)) ? (dist_L–DIST_MIN)/20.0f : (DIST_MAX–dist_L)/30.0f) : 0.0f;
membership[HIGH_NP] = (dist_L >= DIST_MAX–20 && dist_L <= DIST_MAX) ?
(dist_L–(DIST_MAX–20))/20.0f : 0.0f;
membership[MAX_NP] = (dist_L > DIST_MAX) ? 1.0f : 0.0f;

// 对右侧距离执行相同隶属度计算（简化，复制逻辑即可）
float memR[4];
memR[LOW_NP] = (dist_R < DIST_MIN) ? 1.0f : (dist_R < DIST_MAX) ? (DIST_MAX–dist_R)/(DIST_MAX–DIST_MIN) : 0.0f;
memR[MIDL_NP] = (dist_R >= DIST_MIN && dist_R <= DIST_MAX) ?
((dist_R < (DIST_MIN+20)) ? (dist_R–DIST_MIN)/20.0f : (DIST_MAX–dist_R)/30.0f) : 0.0f;
memR[HIGH_NP] = (dist_R >= DIST_MAX–20 && dist_R <= DIST_MAX) ?
(dist_R–(DIST_MAX–20))/20.0f : 0.0f;
memR[MAX_NP] = (dist_R > DIST_MAX) ? 1.0f : 0.0f;

// 2. 模糊规则推理：根据左右距离隶属度调整速度
// 距离越近，减速；距离越远，加速
if (dist_L < WALL_DIST && dist_R > WALL_DIST) {
// 左侧过近，右侧过远：左侧减速，右侧加速，靠近墙体
speed_adj_L = map(dist_L, DIST_MIN, WALL_DIST, –30, 0);
speed_adj_R = map(dist_R, WALL_DIST, DIST_MAX, 0, 20);
} else if (dist_R < WALL_DIST && dist_L > WALL_DIST) {
// 右侧过近，左侧过远：右侧减速，左侧加速
speed_adj_R = map(dist_R, DIST_MIN, WALL_DIST, –30, 0);
speed_adj_L = map(dist_L, WALL_DIST, DIST_MAX, 0, 20);
} else if (dist_L < DIST_MIN || dist_R < DIST_MIN) {
// 两侧过近：双向减速，避免撞墙
speed_adj_L = –20;
speed_adj_R = –20;
} else if (dist_L > DIST_MAX || dist_R > DIST_MAX) {
// 两侧过远：双向加速，靠近墙体
speed_adj_L = 15;
speed_adj_R = 15;
} else {
// 距离适中：保持速度
speed_adj_L = 0;
speed_adj_R = 0;
}

// 3. 速度输出：调整基础速度，限幅
speed_left = constrain(speed_left + speed_adj_L * 0.1f, 80, 220);
speed_right = constrain(speed_right + speed_adj_R * 0.1f, 80, 220);

driveMotor(speed_left, speed_right);
}

// 气体采样：定期采集浓度，异常告警
void sampleGas() {
uint32_t now = millis();
if (now – lastSampleTime > SAMPLE_INTERVAL) {
gas_concentration = readGasSensor();
lastSampleTime = now;

Serial.print("采样浓度：");
Serial.print(gas_concentration);
Serial.print("，基准浓度：");
Serial.println(baseline_gas);

// 浓度异常检测
if (gas_concentration > baseline_gas + GAS_ALARM) {
gas_alarm = true;
Serial.println("警告：气体浓度异常，疑似泄露");
} else {
gas_alarm = false;
}
}
}

// 读取气体传感器
float readGasSensor() {
int val = analogRead(MQ_PIN);
return val * 0.1f; // 按传感器校准系数调整
}

// 电机驱动函数
void driveMotor(float left, float right) {
int l = constrain((int)left, 0, 255);
int r = constrain((int)right, 0, 255);
digitalWrite(DIR_L, HIGH); analogWrite(PWM_L, l);
digitalWrite(DIR_R, HIGH); analogWrite(PWM_R, r);
}

适用场景优化
可增加墙体类型识别，适配混凝土墙、金属围栏等不同材质，调整红外阈值
优化模糊规则库，覆盖墙角、转角等特殊场景，提升沿墙稳定性
扩展采样点位，在转角、阀门等关键位置加密采样，提升泄露排查精度

5、模糊沿墙巡检+气体梯度采样调制程序（核心：浓度梯度分析与动态采样）
适用场景：化工车间、密闭储罐区等场景，机器人沿墙巡检时，根据气体浓度梯度变化调整采样频率与行驶速度，精准定位泄露源。

核心逻辑
沿墙行驶过程中，实时计算气体浓度梯度
依据梯度大小动态调节采样频率与机器人移动速度
梯度增大时降低速度、提升采样频率，实现泄露源精准定位

// 模糊沿墙巡检+气体梯度采样调制程序
// 硬件：红外、MQ气体传感器、BLDC、温湿度传感器（可选，用于修正）
// 功能：沿墙行驶，基于气体梯度调整采样策略，精准定位泄露源

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte IR_LEFT = A0, IR_RIGHT = A1, MQ_PIN = A2;
const byte PWM_L = 6, DIR_L = 7, PWM_R = 8, DIR_R = 9;

// ========== 沿墙与采样参数 ==========
const float WALL_DIST = 22.0;
int base_speed = 170;
float last_gas [3] = {0.0}; // 历史浓度，用于计算梯度
float gas_gradient = 0.0; // 气体浓度梯度（单位距离浓度变化量）
float last_pos_x = 0.0; // 机器人横向位置（用于计算移动距离）
uint16_t sample_interval = 2000; // 动态采样间隔（ms）
const uint16_t MIN_INTERVAL = 500; // 最小采样间隔
const uint16_t MAX_INTERVAL = 3000;// 最大采样间隔
const float GRADIENT_ALARM = 1.5; // 梯度告警阈值

bool locate_source = false; // 是否进入泄露定位模式
unsigned long scanTimer = 0;

// ========== 函数声明 ==========
void adaptiveSampling();
float calculateGradient();
void adjustSpeedByGradient();
void fuzzyWallControl();
void driveMotor(int left, int right);

void setup() {
Serial.begin(115200);
Serial.println("梯度调制沿墙巡检系统初始化完成");

pinMode(IR_LEFT, INPUT); pinMode(IR_RIGHT, INPUT); pinMode(MQ_PIN, INPUT);
pinMode(PWM_L, OUTPUT); pinMode(DIR_L, OUTPUT);
pinMode(PWM_R, OUTPUT); pinMode(DIR_R, OUTPUT);
}

void loop() {
fuzzyWallControl(); // 1. 模糊沿墙控制
adaptiveSampling(); // 2. 梯度调制采样
adjustSpeedByGradient(); // 3. 梯度驱动速度调整
delay(10);
}

// 模糊沿墙控制（简化逻辑，可复用案例1，此处精简）
void fuzzyWallControl() {
float dist_L = analogRead(IR_LEFT) * 0.08f;
float dist_R = analogRead(IR_RIGHT) * 0.08f;

int adj_L = 0, adj_R = 0;
if (dist_L < WALL_DIST && dist_R > WALL_DIST) {
adj_L = map((int)dist_L, 10, (int)WALL_DIST, –2, 0);
adj_R = map((int)dist_R, (int)WALL_DIST, 40, 0, 2);
} else if (dist_R < WALL_DIST && dist_L > WALL_DIST) {
adj_R = map((int)dist_R, 10, (int)WALL_DIST, –2, 0);
adj_L = map((int)dist_L, (int)WALL_DIST, 40, 0, 2);
}

base_speed = constrain(base_speed + adj_L + adj_R, 100, 220);
driveMotor(base_speed, base_speed);
}

// 计算气体梯度：单位距离的浓度变化
float calculateGradient() {
uint32_t now = millis();
if (now – scanTimer > 500) { // 快速采样计算梯度
float current_gas = analogRead(MQ_PIN) * 0.1f;
float last_gas = last_gas[2];
// 估算移动距离（简化，可结合编码器）
float distance_moved = 5.0; // 单位：cm
if (distance_moved > 0.1) {
gas_gradient = (current_gas – last_gas) / distance_moved;
} else {
gas_gradient = 0.0;
}
// 滑动窗口更新历史浓度
last_gas[0] = last_gas[1];
last_gas[1] = last_gas[2];
last_gas[2] = current_gas;
scanTimer = now;
}
return gas_gradient;
}

// 调制采样策略：梯度越大，采样越频繁
void adaptiveSampling() {
gas_gradient = calculateGradient();

// 根据梯度动态调整采样间隔
if (gas_gradient > GRADIENT_ALARM) {
// 梯度高，加密采样
sample_interval = constrain(sample_interval – 200, MIN_INTERVAL, MAX_INTERVAL);
locate_source = true;
} else if (abs(gas_gradient) < 0.3) {
// 梯度低，降低采样频率
sample_interval = constrain(sample_interval + 100, MIN_INTERVAL, MAX_INTERVAL);
locate_source = false;
}

// 执行采样
unsigned long now = millis();
if (now % sample_interval < 20) { // 按动态间隔采样
float current_gas = analogRead(MQ_PIN) * 0.1f;
Serial.print("动态采样浓度：");
Serial.print(current_gas);
Serial.print("，梯度：");
Serial.println(gas_gradient);

if (gas_gradient > GRADIENT_ALARM) {
Serial.println("进入泄露定位模式，逼近排查");
}
}
}

// 依据梯度调整行驶速度：梯度越高，速度越低
void adjustSpeedByGradient() {
if (abs(gas_gradient) > GRADIENT_ALARM) {
// 梯度大，减速逼近
int slow_speed = map((int)(gas_gradient * 100), 0, 300, base_speed, 80);
driveMotor(slow_speed, slow_speed);
} else if (abs(gas_gradient) > 0.5 && !locate_source) {
// 梯度适中，小幅减速
int mid_speed = base_speed – 30;
driveMotor(mid_speed, mid_speed);
}
// 梯度低，保持基础速度（由沿墙控制主导）
}

// 电机驱动
void driveMotor(int left, int right) {
left = constrain(left, 0, 255);
right = constrain(right, 0, 255);
digitalWrite(DIR_L, HIGH); analogWrite(PWM_L, left);
digitalWrite(DIR_R, HIGH); analogWrite(PWM_R, right);
}

适用场景优化
引入多气体传感器，检测多种有害气体，拓宽巡检范围
结合风速风向数据修正梯度判断，减少环境干扰，提升定位精度
增加泄露源识别算法，通过高浓度点聚集判断真实泄露位置

6、模糊沿墙巡检+气体梯度采样+异常安全保护程序（核心：运行监控与应急处置）
适用场景：高危化工区域、受限空间等场景，机器人巡检时需实时监控气体浓度、运行状态，异常时快速启动保护机制，保障设备与人员安全。

核心逻辑
实时监控气体浓度、传感器健康、电机状态与电池电量
当气体浓度超标或系统异常时，触发分级保护策略
异常时自动撤离、上报告警，避免危险扩散

// 模糊沿墙巡检+气体梯度采样+异常安全保护程序
// 硬件：红外、气体传感器、蜂鸣器、急停模块、电压采集
// 功能：巡检过程中状态监控，异常分级保护，安全撤离

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte IR_LEFT = A0, IR_RIGHT = A1, MQ_PIN = A2;
const byte PWM_L = 6, DIR_L = 7, PWM_R = 8, DIR_R = 9;
const byte BUZZER = 10, LED_ERR = 11, LED_OK = 12;
const byte E_STOP_PIN = 2; // 硬件急停引脚

// ========== 保护与监控参数 ==========
float gas_concentration = 0.0;
float gas_gradient = 0.0;
const float MAX_ALLOWED_GAS = 200.0; // 浓度安全阈值
const float DANGER_GRADIENT = 3.0; // 危险梯度阈值
bool systemError = false;
bool dangerous = false;
unsigned long errorFlagTime = 0;

uint16_t batteryVoltage = 0;
const uint16_t LOW_BATTERY = 7000; // 低电量阈值（mV，需校准）
bool isRunning = true;

// ========== 函数声明 ==========
void monitorSystemState();
void gradeProtection();
void evacuateToSafety();
void normalOperation();
void driveMotor(int left, int right);
void alarm(bool danger);

void setup() {
Serial.begin(115200);
Serial.println("巡检+采样+安全保护系统初始化完成");

pinMode(IR_LEFT, INPUT); pinMode(IR_RIGHT, INPUT); pinMode(MQ_PIN, INPUT);
pinMode(PWM_L, OUTPUT); pinMode(DIR_L, OUTPUT);
pinMode(PWM_R, OUTPUT); pinMode(DIR_R, OUTPUT);
pinMode(BUZZER, OUTPUT); pinMode(LED_ERR, OUTPUT); pinMode(LED_OK, OUTPUT);
pinMode(E_STOP_PIN, INPUT_PULLUP);
}

void loop() {
monitorSystemState(); // 1. 全状态监控
if (systemError || dangerous) {
gradeProtection(); // 2. 分级保护
} else {
normalOperation(); // 3. 正常巡检采样
Brooklyn恢复正常逻辑 }
delay(15);
}

// 系统状态监控
void monitorSystemState() {
// 1. 气体浓度与梯度检测
gas_concentration = analogRead(MQ_PIN) * 0.1f;
gas_gradient = calculateGradient(); // 复用案例2梯度计算逻辑
// 2. 传感器异常检测
bool sensorOk = (analogRead(IR_LEFT)!=0 && analogRead(IR_LEFT)!=1023);
sensorOk = sensorOk && (analogRead(IR_RIGHT)!=0 && analogRead(IR_RIGHT)!=1023);
sensorOk = sensorOk && (analogRead(MQ_PIN)!=0 && analogRead(MQ_PIN)!=1023);
// 3. 急停信号检测
bool eStopPressed = !digitalRead(E_STOP_PIN);
// 4. 电池电压检测
batteryVoltage = readBatteryVoltage();
// 5. 电机异常检测（堵转、过流，简化）
bool motorOk = getMotorStatus(); // 需实现电机状态读取

systemError = !sensorOk || eStopPressed || !motorOk;
dangerous = (gas_concentration > MAX_ALLOWED_GAS) ||
(gas_gradient > DANGER_GRADIENT) ||
(batteryVoltage < LOW_BATTERY);

// 异常计时，避免误判
if (systemError || dangerous) {
if (millis() – errorFlagTime > 1000) {
errorFlagTime = millis();
}
}
}

// 分级保护策略
void gradeProtection() {
digitalWrite(LED_OK, LOW);

// 一级保护：传感器异常或急停，立即停机
if (systemError) {
driveMotor(0, 0);
alarm(true);
Serial.println("一级保护：系统异常，紧急停机");
isRunning = false;
return;
}

if (!isRunning) return;

// 二级保护：气体浓度超标或危险梯度，撤离到安全区域
if (dangerous) {
Serial.println("二级保护：检测到危险，启动撤离");
evacuateToSafety();
alarm(true);
}
}

// 撤离到安全区域：反向行驶，远离危险
void evacuateToSafety() {
driveMotor(–120, –120); // 反向行驶，速度可控
// 撤离过程中持续监测浓度，安全后减速
if (gas_concentration < MAX_ALLOWED_GAS * 0.7) {
driveMotor(–80, –80); // 浓度下降后减速
}
}

// 正常运行：沿墙巡检+采样（简化，复用案例2逻辑）
void normalOperation() {
digitalWrite(LED_OK, HIGH); digitalWrite(LED_ERR, LOW);
driveMotor(170, 170); // 基础沿墙速度
// 动态采样逻辑（同案例2）
uint32_t now = millis();
if (now % 1500 < 20) {
Serial.print("正常采样浓度：");
Serial.println(gas_concentration);
}
}

// 告警功能
void alarm(bool danger) {
digitalWrite(LED_ERR, danger ? HIGH : LOW);
for (byte i=0; i<3; i++) {
digitalWrite(BUZZER, HIGH);
delay(150);
digitalWrite(BUZZER, LOW);
delay(150);
}
}

// 电机驱动
void driveMotor(int left, int right) {
left = constrain(left, –255, 255);
right = constrain(right, –255, 255);
if (left >=0) { digitalWrite(DIR_L, HIGH); analogWrite(PWM_L, left); }
else { digitalWrite(DIR_L, LOW); analogWrite(PWM_L, abs(left)); }
if (right >=0) { digitalWrite(DIR_R, HIGH); analogWrite(PWM_R, right); }
else { digitalWrite(DIR_R, LOW); analogWrite(PWM_R, abs(right)); }
}

// 辅助函数：读取电池电压（简化，需校准）
uint16_t readBatteryVoltage() {
return analogRead(A3) * 24;
}
// 辅助函数：获取电机状态（简化，示例返回正常）
bool getMotorStatus() {
return true;
}
// 辅助函数：梯度计算（同案例2，简化）
float calculateGradient() {
return (analogRead(MQ_PIN)*0.1f – 50.0f) / 5.0f;
}

适用场景优化
增加远程通讯模块，异常时实时上报位置与状态，便于后台管控
优化撤离路径，结合SLAM地图选择安全路线，避免撤离时撞墙
加入防护外壳与气体过滤，提升机器人在高危环境下的自身防护能力

要点解读
要点1：模糊沿墙控制核心是“距离隶属度+速度规则”，适配复杂墙体场景
模糊控制能有效应对墙体不规则、传感器波动等问题，是沿墙稳定行驶的关键。
隶属度设计：将红外检测的距离划分为近距离、适中、远距离三个等级，计算隶属度，实现对距离的模糊化处理，避免单一阈值带来的顿挫感
规则库构建：依据左右两侧距离差异制定速度调整规则，距离近的一侧减速、远的一侧加速，实现平滑转向，兼顾沿墙稳定性与行驶速度
自适应调整：可结合环境光照、墙体材质调整隶属度函数参数，适配瓷砖、金属、混凝土等不同反射特性的墙体，提升场景适应性

要点2：气体梯度采样调制需“动态采样+梯度匹配”，提升泄露定位效率
气体梯度是判断泄露方向、定位泄露源的核心依据，动态调制是关键。
梯度计算逻辑：结合历史浓度与移动距离，计算单位距离的浓度变化量，形成梯度数据，避免单次采样的偶然性误差
采样策略调制：梯度越大，采样频率越高、机器人速度越慢；梯度越小，适当降低采样频率，平衡巡检效率与定位精度，实现“梯度驱动采样”
分级响应机制：设置梯度阈值，低于安全梯度正常巡检，中度梯度加密采样排查，重度梯度启动定位模式，匹配不同程度的泄漏风险

要点3：传感器融合需“多源交叉校验”，保障运行可靠性
多传感器融合可弥补单一传感器的不足，降低环境干扰。
红外协同：多路红外传感器交叉检测墙体距离，取平均值或权重计算，减少传感器噪声与盲区的影响
气体传感器滤波：对气体浓度采样数据进行滑动平均、中值滤波，过滤环境波动干扰，提升数据稳定性
状态校验：定期校验传感器输出，出现异常值（如固定值、跳变）时标记传感器故障，避免错误数据影响控制与采样

要点4：巡检与采样需“路径优化+关键点加密”，平衡覆盖与效率
合理的巡检策略能提升隐患发现概率，降低运维成本。
路径规划：沿墙行驶形成闭环或往复路径，覆盖巡检区域的主要点位，避免重复巡检与漏检
关键点加密：在阀门、接口、储罐焊缝等易泄露位置，自动提升采样频率，结合梯度分析，精准排查高风险区域
数据关联：将气体浓度数据与机器人位置绑定，形成带位置标记的浓度地图，便于溯源分析与泄露源定位

要点5：安全保护需“分级触发+应急撤离”，保障高危场景运行
高危场景下，安全保护是机器人可靠运行的底线。
分级响应：将异常分为传感器异常、浓度超标、电池低电量等等级，对应停机、撤离、关机等不同策略，匹配风险程度
应急撤离：检测到危险时，机器人立即沿远离危险源的方向行驶，同时保持数据采集，记录危险区域信息，便于后续处置
冗余设计：配备硬件急停、异常声光报警、通信异常保护等多重机制，单点故障不影响整体安全，确保极端情况下的安全可控

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