该系统以 Arduino 作为主控，驱动 BLDC 无刷电机底盘，融合多种环境传感器，实现对动态障碍物（移动人员、移动物体） 的感知、预判与自主避障，区别于传统单传感器静态避障，重点解决运动场景下障碍物速度、位置变化带来的误判、碰撞问题。
一、系统主要特点
多传感器信息互补，降低单点失效风险
单一传感器存在固有缺陷：超声波易受角度、软材质物体干扰；红外测距受光照影响；毫米波雷达可测速度但角分辨率差；摄像头易受强光、烟雾干扰；激光雷达精度高但成本高、粉尘环境性能下降。
融合模式下，不同传感器输出距离、障碍物运动速度、角度信息，通过加权滤波 / 简单卡尔曼滤波做数据校验，一个传感器异常时可依靠其余传感器维持基础避障能力，提升鲁棒性。搭配 BLDC 电机，支持快速加减速、急停、柔性调速，可快速响应避障指令。
动态目标感知，不只检测距离，还估算运动趋势
传统避障只判断 “障碍物离我多远”；动态避障额外解析障碍物移动速度、运动方向，预判未来短时间内位置。
系统输出不只是简单 “停下 / 转弯”，会结合自身 BLDC 底盘的速度，判断是减速绕行、原地等待还是变向通过，避免动态目标横穿路径时发生碰撞。
分层决策架构，传感器层‑融合层‑运动控制层解耦
传感器层：采集原始测距、速度数据，做硬件异常校验；
融合层：时间对齐、数据滤波、冲突剔除，输出可靠障碍物列表；
运动控制层：对接 BLDC 驱动器，输出速度、转向指令，设置安全阈值，区分警告区、减速区、禁止进入危险区。
Arduino 资源有限，一般采用轻量化融合算法（加权平均、简单卡尔曼，不跑重型 EKF），保证控制实时性，避免融合计算占用过多 CPU 导致电机控制卡顿。
BLDC 无刷底盘适配，支持柔性安全控制
相比有刷电机，BLDC 响应快、转速闭环可控。避障系统不只有硬急停，支持分级响应：预警→降速→偏移绕行→紧急停机；可设置最大加速度限制，防止机器人高速下急停出现滑移、打滑，提升底盘运动稳定性。
可扩展，兼容人机交互与状态输出
可外接 OLED、蜂鸣器、串口上报障碍物信息；支持切换工作模式：巡逻模式、避障优先模式、远程接管模式；故障时输出传感器异常标记，方便调试。
二、典型应用场景
室内商用巡检机器人（工厂车间、仓储、商场）
环境存在走动人员、转运小车等动态障碍；地面有货架、设备静态障碍。需要机器人一边沿路线巡检，一边躲避走动人流，不能一遇到人就直接卡死不动。
园区 / 半室外安防巡检小车
光照变化大，单纯视觉不可靠；融合毫米波雷达 + 超声，应对阳光、阴天；躲避行人、非机动车，BLDC 底盘适合长时间连续工作。
应急辅助机器人（非防爆简易版本）
有烟雾、粉尘局部干扰，摄像头效果下降，依靠雷达 + 超声做基础动态避障，完成简单环境探查；注意：高温高爆场景需要额外硬件防护。
教育创客竞赛场景
机器人竞赛场地存在移动对手机器人，需要实时规避移动目标，完成任务，该方案是轮式智能机器人竞赛常用技术路线。
限制：Arduino 算力有限，不适合高速、大范围复杂环境；高速室外场景建议升级 ESP32/STM32。
三、开发与工程实践注意事项

1. 传感器层面问题
   1）时间同步是最大坑点
   各个传感器采样速率不一样：雷达 50Hz，超声 20Hz，红外 100Hz。Arduino 串行读取传感器，数据时间戳错位，会造成融合出来障碍物位置漂移。
   对策：给每一组测量打上时间戳，丢弃过期旧数据，不要直接把不同时刻数据直接加权。
   2）传感器视场重叠与盲区
   不同传感器探测角度不一样，存在探测盲区；部分物体对特定传感器反射弱（比如黑色吸光物体对红外，软布料对超声波），不能只依赖某一类传感器读数。
   对策：硬件布局交错布置传感器；软件设置可信度权重，可信度低的测量降低权重。
   3）噪声与误触发
   环境电磁干扰，BLDC 驱动器 PWM 开关噪声容易串入模拟传感器，造成测距乱跳。
   对策：传感器信号线远离 BLDC 功率线；增加滤波电容；软件做跳变值剔除，突变过大的数据直接丢弃。
2. 融合算法层面（Arduino 算力约束）
   1）不要照搬 PC 端复杂融合算法
   完整 EKF、多目标跟踪计算量大，Arduino 内存与算力不足，会造成主线程阻塞，BLDC 电机控制延迟，出现 “感知到障碍但是来不及刹车”。
   工程方案：优先使用加权置信度滤波 + 简单卡尔曼，只跟踪 2‑3 个优先级最高障碍物，舍弃次要目标。
   2）区分静态障碍物和动态障碍物
   全部障碍物统一当成动态目标，会造成机器人不停无意义绕行抖动；需要对连续多帧数据做判断，区分静止物体和移动物体。
3. BLDC 电机运动控制耦合问题
   1）避障逻辑和电机控制不能互相阻塞
   Arduino loop 循环如果传感器处理耗时过长，会造成 BLDC 调速输出卡顿。
   建议：将电机控制放在高优先级，传感器读取做非阻塞模式，不要使用 delay ()。
   2）滑移打滑影响避障效果
   轮子打滑，编码器里程计位置失真，融合出来自身位置不准，导致避障决策出错。
   对策：引入速度上限；打滑严重场景降低机器人行驶速度；依靠外部传感器修正自身位姿。
   3）安全阈值调试
   危险距离阈值不能写死。机器人速度越快，所需要刹车距离越大；要根据当前 BLDC 实际速度动态调整安全距离，低速阈值小，高速阈值放大。
4. 逻辑状态机设计
   需要建立完整状态机：正常巡逻、预警减速、绕行避让、紧急停止、故障降级。
   不能简单 if‑else 判断距离；传感器全部失效时，机器人必须执行安全停机逻辑，防止盲跑。
5. 环境与实测验证
   仿真效果不等于现实效果。仿真下避障流畅，现实中动态行人、反光物体、地面杂物都会导致异常。
   必须做大量实机测试：测试横穿移动目标、斜向靠近目标、弱反射物体场景。

1、超声波+红外分区避障与动态路径修正（三轴机器人）
适用场景：三轴并联机器人在受限空间中作业，需实时感知前/左/右障碍并调整各轴速度。
核心逻辑：超声波传感器覆盖正前方中远距离，两个红外传感器负责左右近距离探测。当某方向检测到障碍，该方向轴的速度按比例抑制，同时向空旷方向补偿速度，实现分区避障。

#include <SimpleFOC.h>
#include <NewPing.h>

// ==================== 三轴BLDC电机 ====================
BLDCMotor motorX(7), motorY(7), motorZ(7);
BLDCDriver3PWM drvX(3, 5, 6, 8), drvY(9, 10, 11, 12), drvZ(22, 23, 24, 25);

// ==================== 传感器阵列 ====================
#define IR_LEFT_PIN A0
#define IR_RIGHT_PIN A1
#define TRIG_F 2
#define ECHO_F 3
NewPing sonarFront(TRIG_F, ECHO_F, 200);

// ==================== 避障参数 ====================
const float SAFE_DIST_CM = 30.0;
const float AVOIDANCE_GAIN = 0.8;
const int IR_THRESHOLD = 400;  // 红外阈值(模拟值)

void setup() {
    Serial.begin(115200);
    
    // 初始化三轴电机
    motorX.linkDriver(&drvX); motorY.linkDriver(&drvY); motorZ.linkDriver(&drvZ);
    motorX.init(); motorY.init(); motorZ.init();
    motorX.initFOC(); motorY.initFOC(); motorZ.initFOC();
    motorX.controller = MotionControlType::velocity;
    motorY.controller = MotionControlType::velocity;
    motorZ.controller = MotionControlType::velocity;

    pinMode(IR_LEFT_PIN, INPUT);
    pinMode(IR_RIGHT_PIN, INPUT);
}

void loop() {
    motorX.loopFOC(); motorY.loopFOC(); motorZ.loopFOC();

    // 1. 传感器读取
    float distFront = sonarFront.ping_cm();
    bool leftObs = analogRead(IR_LEFT_PIN) < IR_THRESHOLD;
    bool rightObs = analogRead(IR_RIGHT_PIN) < IR_THRESHOLD;

    // 2. 目标速度（示例：匀速前进）
    float targetVx = 1.0, targetVy = 0.0, targetVz = 0.0;

    // 3. 分区避障逻辑
    if (distFront > 0 && distFront < SAFE_DIST_CM) {
        // 前方障碍：抑制X轴前进
        targetVx *= AVOIDANCE_GAIN * (distFront / SAFE_DIST_CM);
    }
    if (leftObs) {
        // 左侧障碍：向右补偿（Y轴正向）
        targetVy += 0.3;
    }
    if (rightObs) {
        // 右侧障碍：向左补偿（Y轴负向）
        targetVy -= 0.3;
    }

    // 4. 执行控制
    motorX.move(targetVx);
    motorY.move(targetVy);
    motorZ.move(targetVz);

    Serial.print("F:"); Serial.print(distFront);
    Serial.print(" L:"); Serial.print(leftObs);
    Serial.print(" R:"); Serial.println(rightObs);
    delay(50);
}

该方案参考了多红外阵列分区避障与动态路径修正的典型设计思路。

2、视觉+UWB+IMU多模态融合跟随避障（服务机器人场景）
适用场景：服务机器人在动态环境中跟随目标人员，需融合多传感器以应对光照变化、遮挡等挑战。
核心逻辑：视觉识别目标位置，UWB提供绝对距离/角度，IMU在目标丢失时维持短时预测。系统通过置信度动态评估各传感器权重，避障逻辑拥有硬优先级——前方障碍小于安全距离时无条件挂起跟随。

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

// ==================== BLDC双轮差速 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(3, 5, 6, 8), driverR(9, 10, 11, 12);
Encoder encoderL(18, 19, 2048), encoderR(20, 21, 2048);

// ==================== 传感器 ====================
MPU6050 imu;
NewPing sonarFront(2, 3, 200);

// ==================== 目标状态 ====================
struct FusedTarget {
    float distance;      // 距离(m)
    float angle;         // 相对角度(rad)
    float confidence;    // 置信度 0~1
    bool valid;
};
FusedTarget target = {0, 0, 0, false};

// ==================== 避障与跟随参数 ====================
const float FOLLOW_DIST = 1.0;
const float SAFE_STOP = 0.3;
const float CONFIDENCE_THRESHOLD = 0.15;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    imu.initialize();
    
    // 初始化电机与FOC（省略）
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC(); motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

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

    // ==================== 1. 多传感器数据采集 ====================
    // 视觉数据：假定通过串口/OpenMV接收
    float visualDist = 1.2, visualAngle = 0.3;
    float visualConf = 0.7;  // 光照良好置信度
    
    // UWB数据：距离+角度（实际通过UWB模块读取）
    float uwbDist = 0.9, uwbAngle = 0.25;
    float uwbConf = 0.8;     // 无遮挡时置信度高
    
    // IMU航向
    int16_t gz = 0;
    imu.getRotation(&gz, &gz, &gz);
    static float imuYaw = 0;
    imuYaw += gz * 0.001;

    // ==================== 2. 核心：置信度加权融合 ====================
    // 视觉在光照良好时权重高，UWB在遮挡时权重高
    if (visualConf > 0.6) {
        target.distance = visualDist;
        target.angle = visualAngle;
        target.confidence = visualConf;
        target.valid = true;
    } else if (uwbConf > 0.4) {
        target.distance = uwbDist;
        target.angle = uwbAngle;
        target.confidence = uwbConf * 0.8;
        target.valid = true;
    } else if (target.valid && target.confidence > 0.2) {
        // 传感器退化：IMU航迹推算维持短时预测
        target.distance += 0.05;
        target.angle += imuYaw * 0.02;
        target.confidence *= 0.98;
    } else {
        target.valid = false;
    }

    // ==================== 3. 超声波避障（硬优先级） ====================
    float frontDist = sonarFront.ping_cm() / 100.0;
    if (frontDist > 0 && frontDist < SAFE_STOP) {
        motorL.move(0); motorR.move(0);
        delay(300);
        return;
    }

    // ==================== 4. 跟随控制 ====================
    if (target.valid && target.confidence > CONFIDENCE_THRESHOLD) {
        float distError = target.distance - FOLLOW_DIST;
        float angleError = target.angle;
        
        float linSpeed = constrain(distError * 1.2, -0.8, 1.2);
        float angSpeed = constrain(angleError * 2.5, -0.8, 0.8);
        
        float wheelBase = 0.25;
        // 近距离防撞减速
        if (target.distance < 0.6) linSpeed *= 0.6;
        
        motorL.move(linSpeed - angSpeed * wheelBase / 2);
        motorR.move(linSpeed + angSpeed * wheelBase / 2);
    } else {
        motorL.move(0); motorR.move(0);
    }

    delay(50);
}

本方案融合了视觉-UWB动态置信度权重分配与IMU航迹推算维持的设计思路，确保感知系统的鲁棒性。

3、雷达点云+视觉光流的多目标跟踪避障（GNN数据关联）
适用场景：复杂动态环境中机器人需同时跟踪多个移动目标（人员/车辆），区分主目标与干扰物。
核心逻辑：毫米波雷达提供点云（距离/角度/速度），视觉提供特征点及光流速度。通过GNN（图神经网络）思想构建距离矩阵，将雷达点与视觉特征关联，形成独立跟踪轨迹，避障决策以最近的活跃跟踪目标为参考。

#include <SimpleFOC.h>
#include <math.h>

// ==================== BLDC双轮差速 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(3, 5, 6, 8), driverR(9, 10, 11, 12);
Encoder encoderL(18, 19, 2048), encoderR(20, 21, 2048);

// ==================== 数据结构 ====================
#define MAX_RADAR_POINTS 8
#define MAX_FEATURES 20
#define MAX_TRACKS 5

struct RadarPoint { float x, y, vx, vy; float rssi; };
struct FeaturePoint { float u, v, flowU, flowV; bool active; };
struct Track { float x, y, vx, vy; int id; bool active; };

RadarPoint radarPoints[MAX_RADAR_POINTS];
FeaturePoint features[MAX_FEATURES];
Track tracks[MAX_TRACKS];
int pointCount = 0, featureCount = 0, trackCount = 0;

// 模拟数据（实际从传感器读取）
void readRadarPointCloud() {
    pointCount = 4;
    radarPoints[0] = {1.2, 0.3, 0.2, 0.1, 80};   // 目标1
    radarPoints[1] = {1.5, -0.5, 0.1, -0.1, 75}; // 目标2
    radarPoints[2] = {2.0, 0.1, 0.0, 0.0, 60};   // 静态障碍
    radarPoints[3] = {0.5, 0.0, 0.3, 0.0, 90};   // 近距离目标
}

// ==================== GNN数据关联 ====================
void associateRadarToFeatures() {
    for (int i = 0; i < pointCount; i++) {
        float bestDist = 999;
        int bestIdx = -1;
        for (int j = 0; j < featureCount; j++) {
            if (!features[j].active) continue;
            // 像素坐标转世界坐标（简化标定）
            float fx = (features[j].u - 160) * 0.01;
            float fy = (features[j].v - 120) * 0.01;
            float d = sqrt(pow(radarPoints[i].x - fx, 2) + 
                           pow(radarPoints[i].y - fy, 2));
            if (d < bestDist) {
                bestDist = d;
                bestIdx = j;
            }
        }
        // 距离小于阈值则关联成功
        if (bestDist < 0.3) {
            radarPoints[i].rssi = bestIdx;  // 标记关联的特征索引
        }
    }
}

// ==================== 跟踪更新 ====================
void updateTracks() {
    for (int i = 0; i < pointCount; i++) {
        bool matched = false;
        for (int t = 0; t < trackCount; t++) {
            if (!tracks[t].active) continue;
            float d = sqrt(pow(radarPoints[i].x - tracks[t].x, 2) + 
                           pow(radarPoints[i].y - tracks[t].y, 2));
            if (d < 0.5) {
                // 更新已有轨迹（卡尔曼平滑可进一步优化）
                tracks[t].x = tracks[t].x * 0.6 + radarPoints[i].x * 0.4;
                tracks[t].y = tracks[t].y * 0.6 + radarPoints[i].y * 0.4;
                tracks[t].vx = radarPoints[i].vx;
                tracks[t].vy = radarPoints[i].vy;
                matched = true;
                break;
            }
        }
        if (!matched && trackCount < MAX_TRACKS) {
            // 新增轨迹
            tracks[trackCount].x = radarPoints[i].x;
            tracks[trackCount].y = radarPoints[i].y;
            tracks[trackCount].vx = radarPoints[i].vx;
            tracks[trackCount].vy = radarPoints[i].vy;
            tracks[trackCount].id = trackCount;
            tracks[trackCount].active = true;
            trackCount++;
        }
    }
}

// ==================== 避障决策 ====================
void obstacleAvoidance() {
    // 找最近的活跃目标
    int nearest = -1;
    float minDist = 999;
    for (int i = 0; i < trackCount; i++) {
        if (!tracks[i].active) continue;
        float d = sqrt(tracks[i].x * tracks[i].x + tracks[i].y * tracks[i].y);
        if (d < minDist && d > 0.1) {
            minDist = d;
            nearest = i;
        }
    }

    if (nearest >= 0) {
        float tx = tracks[nearest].x;
        float ty = tracks[nearest].y;
        
        // 基于最近目标避障：远离距离过近的目标
        if (minDist < 0.8) {
            // 逃逸方向：远离目标的方向
            float angle = atan2(ty, tx);
            float speed = constrain(0.3 * (0.8 - minDist) / 0.5, 0.1, 0.6);
            motorL.move(-speed - angle * 0.15);
            motorR.move(-speed + angle * 0.15);
            return;
        }
        
        // 无紧急障碍：跟随最近目标
        float angle = atan2(ty, tx);
        float speed = constrain(minDist * 0.4, 0.1, 0.8);
        motorL.move(speed - angle * 0.25);
        motorR.move(speed + angle * 0.25);
    } else {
        motorL.move(0); motorR.move(0);
    }
}

void setup() {
    Serial.begin(115200);
    // 电机初始化（省略）
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 采集雷达点云与视觉特征
    readRadarPointCloud();
    // readVisualFeatures();  // 实际从摄像头获取
    
    // 2. GNN数据关联
    associateRadarToFeatures();
    
    // 3. 更新多目标跟踪列表
    updateTracks();
    
    // 4. 避障与跟随
    obstacleAvoidance();
    
    delay(50);
}

该方案参考了雷达-视觉数据关联与多目标跟踪的工程化思路，通过点云与特征的空间距离实现多传感器目标级融合。

要点解读
多传感器融合的核心是“互补而非冗余”：不同传感器有各自的物理局限——超声波方向性强但易受吸音材料影响，红外成本低但受颜色和光照干扰严重，UWB抗遮挡但无法识别目标特征，视觉信息丰富但功耗高。工程落地的关键是根据场景特性设计传感器间的“主-辅-备”互补架构，让各传感器在不同条件下互为保障。

置信度动态权重是融合算法工程化落地的关键：固定权重的加权平均无法适应环境变化。案例二展示了基于环境条件动态评估各传感器置信度的方案——光照良好时视觉权重高，UWB信号强时UWB权重高，传感器退化时IMU航迹推算短时维持。这种动态机制使系统在变化环境中保持稳定。

避障逻辑必须拥有硬优先级，独立于跟随/路径规划：在动态环境中，安全高于任务。超声波/雷达检测到障碍物进入紧急距离（如案例一中的30cm安全阈值）时，必须无条件中断当前任务，执行急停或逃逸动作。这一逻辑应在底层实时响应，不应依赖高层决策的周期性调度。

多目标跟踪是复杂动态环境的前置能力：仅依赖单一最近目标易造成判断失误。案例三通过雷达-视觉数据关联建立多目标跟踪列表，将静态障碍物与动态目标区分管理。避障决策以最近的活跃跟踪目标为参考，避免被远处物体或不相关目标干扰。GNN（图神经网络）数据关联是当前多目标跟踪的主流方法，工程上可用距离矩阵匹配实现轻量化替代。

BLDC FOC是避障指令“精准执行”的物理基础：避障输出的速度指令是连续变化的（如案例一中targetVx *= (distFront / SAFE_DIST_CM)产生动态比例减速）。普通有刷电机低速抖动、响应滞后，难以精准执行。SimpleFOC的FOC控制配合编码器闭环可实现毫秒级扭矩响应和低速平稳运行，确保每一次避障指令被平滑执行，减少因速度阶跃导致的碰撞风险。

4、超声波矩阵动态差速跟随与硬避障系统（商场/展厅场景）
适用场景：商场导览、智能行李车等中近距离（0.1-3m）结构化环境，需兼顾目标跟随与障碍规避，成本敏感且对实时性要求高。
核心逻辑：通过5路超声波环形阵列构建360°局部感知场，采用时分复用触发避免传感器串扰；利用加权质心法解算目标方位，结合双PID串级控制（距离环+角度环）生成差速指令；避障逻辑具备硬优先级，当障碍距离小于25cm时，无条件暂停跟随并执行后退转向动作，保障安全。

#include <SimpleFOC.h>
#include <NewPing.h>

// BLDC差速电机配置
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encoderL(18, 19, 2048), encoderR(20, 21, 2048);

// 5路环形超声波阵列
#define SONAR_NUM 5
#define MIN_DIST 25  // 硬避障阈值（cm）
#define TARGET_DIST 50  // 期望跟随距离（cm）
NewPing sonar[SONAR_NUM] = {
    NewPing(2, 3, 200), NewPing(4, 5, 200), NewPing(6, 7, 200),
    NewPing(8, 9, 200), NewPing(10, 11, 200)
};

float distances[SONAR_NUM];
float targetPosition = 0;  // 目标方位（-1左，1右）

void setup() {
    Serial.begin(115200);
    // 初始化电机FOC控制（速度模式）
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 时分复用超声波测距（防串扰）
    for (int i = 0; i < SONAR_NUM; i++) {
        distances[i] = sonar[i].ping_cm();
        delay(30);
    }
    
    // 2. 加权质心法解算目标方位
    float weightedSum = 0, totalWeight = 0;
    for (int i = 0; i < SONAR_NUM; i++) {
        if (distances[i] > 0 && distances[i] < 200) {
            float angle = (i - 2) * 30.0 * PI / 180.0;
            float weight = 1.0 / distances[i];
            weightedSum += angle * weight;
            totalWeight += weight;
        }
    }
    targetPosition = totalWeight > 0 ? weightedSum / totalWeight : 0;
    
    // 3. 硬避障（优先级高于跟随）
    float minDist = 999;
    for (int i = 0; i < SONAR_NUM; i++) {
        if (distances[i] > 0) minDist = min(minDist, distances[i]);
    }
    
    if (minDist < MIN_DIST) {
        // 向开阔方向后退
        if (targetPosition < -0.3) {
            motorL.move(-0.5); motorR.move(0.5);  // 左转后退
        } else if (targetPosition > 0.3) {
            motorL.move(0.5); motorR.move(-0.5);  // 右转后退
        } else {
            motorL.move(-0.5); motorR.move(-0.5);  // 直行后退
        }
        delay(400);
        return;
    }
    
    // 4. 双PID串级跟随控制
    float distError = minDist - TARGET_DIST;
    float linearSpeed = constrain(distError * 0.03, 0.1, 2.0);  // 距离环输出线速度
    float angularSpeed = constrain(targetPosition * 2.0, -1.0, 1.0);  // 角度环输出角速度
    
    // 近距离柔化减速
    if (minDist < 35) linearSpeed *= 0.5;
    
    // 5. 差速驱动执行
    float wheelBase = 0.25;
    float leftSpeed = linearSpeed - angularSpeed * wheelBase / 2;
    float rightSpeed = linearSpeed + angularSpeed * wheelBase / 2;
    motorL.move(leftSpeed);
    motorR.move(rightSpeed);
}

5、红外+超声波混合避障系统（仓储/教育机器人场景）
适用场景：仓储AGV、教育机器人等结构化环境，需低成本实现基础避障，兼顾中远距离障碍检测与近距离精准识别。
核心逻辑：融合超声波（中远距离测距）与红外传感器（近距离、黑色表面检测），通过逻辑判断实现分层决策：超声波检测前方30cm内障碍，红外补充侧方与近距离盲区；结合随机化转向策略避免狭窄通道振荡，同时通过速度分级控制提升避障平稳性。

#include <NewPing.h>

// 硬件：Arduino Mega + L298N驱动 + HC-SR04超声波 + IR红外模块
#define TRIGGER_PIN 7
#define ECHO_PIN 8
#define IR_LEFT 9
#define IR_RIGHT 10

NewPing sonar(TRIGGER_PIN, ECHO_PIN, 200);  // 最大测距200cm

void setup() {
    pinMode(IR_LEFT, INPUT);
    pinMode(IR_RIGHT, INPUT);
    Serial.begin(9600);
    // 初始化电机引脚（示例：L298N连接数字口11、12）
    pinMode(11, OUTPUT);
    pinMode(12, OUTPUT);
}

void loop() {
    int distance = sonar.ping_cm();
    bool leftObstacle = digitalRead(IR_LEFT) == HIGH;
    bool rightObstacle = digitalRead(IR_RIGHT) == HIGH;
    
    if (distance < 30 || leftObstacle || rightObstacle) {
        stopMotors();
        // 随机化转向避免振荡
        if (!leftObstacle && (rightObstacle || random(1))) {
            turnLeft(500);  // 左转500ms
        } else {
            turnRight(500);  // 右转500ms
        }
    } else {
        moveForward(150);  // PWM占空比75%前进
    }
}

void stopMotors() {
    digitalWrite(11, LOW);
    digitalWrite(12, LOW);
}

void turnLeft(int ms) {
    digitalWrite(11, LOW);
    digitalWrite(12, HIGH);
    delay(ms);
    stopMotors();
}

void turnRight(int ms) {
    digitalWrite(11, HIGH);
    digitalWrite(12, LOW);
    delay(ms);
    stopMotors();
}

void moveForward(int speed) {
    analogWrite(11, speed);
    analogWrite(12, speed);
}

6、RS485总线多机器人一致性编队避障系统（应急救援场景）
适用场景：地震废墟、坍塌建筑等极端复杂环境，多机器人需协同避障并动态切换队形（如三角形→纵队），无中心节点，单节点失效不影响整体任务。
核心逻辑：基于RS485总线实现去中心化通信，每台机器人广播自身位置与状态；通过平均一致性算法使编队中心达成共识，当检测到通道变窄时，领头节点发起队形切换指令，所有节点同步更新偏移量，结合BLDC精准执行实现平滑队形变换，同时融合人工势场法规避静态/动态障碍。

#include <SimpleFOC.h>
#include <ModbusRTU.h>
#include <SoftwareSerial.h>

// BLDC差速电机
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(3, 5, 6, 11);
BLDCDriver3PWM driverR(9, 10, 11, 12);
Encoder encoderL(18, 19, 2048), encoderR(20, 21, 2048);

// RS485通信
SoftwareSerial rs485Serial(2, 3);
ModbusRTU modbus;

// 节点参数（3台机器人）
#define NODE_ID 1  // 节点唯一ID（1-3）
#define NUM_NODES 3
struct AgentState {
    float x, y;    // 全局坐标
    float heading; // 航向角
};
AgentState self = {0, 0, 0};
AgentState neighbors[NUM_NODES];

// 编队参数（三角形/纵队）
struct Formation {
    float offsets[NUM_NODES][2];
};
Formation formationTriangle = {{{0.0, 0.0}, {-0.8, 0.6}, {0.8, 0.6}}};
Formation formationColumn = {{{0.0, 0.0}, {0.0, 0.8}, {0.0, 1.6}}};
Formation currentFormation = formationTriangle;
float consensusGain = 0.1;

void setup() {
    Serial.begin(115200);
    rs485Serial.begin(9600);
    modbus.begin(rs485Serial);
    
    // 初始化电机FOC
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    driverL.init(); driverR.init();
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

// 广播自身状态
void broadcastState() {
    modbus.writeSingleRegister(NODE_ID * 10, (int)(self.x * 100));
    modbus.writeSingleRegister(NODE_ID * 10 + 1, (int)(self.y * 100));
    modbus.writeSingleRegister(NODE_ID * 10 + 2, (int)(self.heading * 100));
}

// 读取邻节点状态
void readNeighborStates() {
    for (int i = 1; i <= NUM_NODES; i++) {
        if (i == NODE_ID) continue;
        neighbors[i-1].x = modbus.readHoldingRegisters(i * 10, 1)[0] / 100.0;
        neighbors[i-1].y = modbus.readHoldingRegisters(i * 10 + 1, 1)[0] / 100.0;
        neighbors[i-1].heading = modbus.readHoldingRegisters(i * 10 + 2, 1)[0] / 100.0;
    }
}

// 一致性算法计算编队中心
void consensusCenter() {
    float sumX = self.x, sumY = self.y;
    int count = 1;
    for (int i = 0; i < NUM_NODES; i++) {
        if (neighbors[i].x != 0 || neighbors[i].y != 0) {
            sumX += neighbors[i].x;
            sumY += neighbors[i].y;
            count++;
        }
    }
    float centerX = sumX / count;
    float centerY = sumY / count;
    // 更新编队中心与节点目标位置（此处省略目标速度计算，需结合实际运动学模型）
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    broadcastState();
    readNeighborStates();
    consensusCenter();
    
    // 执行编队控制指令（根据目标位置计算差速速度）
    // 示例：保持编队速度一致
    motorL.move(1.0);
    motorR.move(1.0);
    
    delay(100);
}

要点解读

1. 多传感器互补融合：突破单一传感器局限，提升感知鲁棒性
   多传感器融合的核心是利用不同传感器的优势互补，解决单一传感器的盲区与误差问题，为避障决策提供可靠依据：
   传感器特性互补：超声波传感器擅长中远距离测距，但存在近距离盲区、易受温度与角度影响；红外传感器可精准检测近距离障碍与黑色表面，但检测距离短、易受环境光干扰；IMU可提供姿态与加速度数据，弥补轮式里程计的打滑误差；激光雷达则能提供稠密点云，适用于高精度地图构建。案例中通过“超声波+红外”组合，既覆盖中远距离检测，又弥补近距离盲区，显著提升环境感知的全面性。
   数据融合算法：采用加权质心法、互补滤波或卡尔曼滤波等算法，对多传感器数据进行融合处理，降低噪声与误差。例如超声波矩阵案例中，通过加权质心法将多路超声波数据转化为目标方位，权重与距离成反比，距离越近权重越高，提升方位解算精度。
   盲区互补设计：通过环形、对称等传感器布局，覆盖机器人360°环境，避免单一方向的感知盲区。如超声波矩阵采用5路环形布局，确保前、左、右、后均能检测障碍，为避障决策提供完整环境信息。
2. 分层控制架构：感知-决策-执行闭环，保障系统协同性
   多传感器融合动态避障需构建分层控制架构，实现感知、决策、执行的高效协同，避免功能混乱与控制冲突：
   感知层：负责多传感器数据采集与预处理，通过滤波、校准等操作提升数据质量，为决策层提供准确的环境与自身状态信息。如超声波矩阵案例中，通过时分复用触发超声波传感器，避免串扰，同时采用中值滤波去除噪声，确保距离数据可靠。
   决策层：根据感知层数据，结合路径规划算法（如A*、DWA）或状态机逻辑，生成避障与运动指令。案例4采用状态机区分跟随与避障模式，硬避障逻辑优先于跟随逻辑；案例6通过一致性算法实现编队决策，确保多机器人协同避障。
   执行层：以BLDC电机为核心，通过FOC闭环控制、PID算法等，精准执行决策层指令，同时反馈执行状态形成闭环。如所有案例均采用BLDC电机的编码器反馈实现速度闭环，确保差速控制精度，避免因电机特性差异导致的轨迹偏差。
3. 实时性优化：适配硬件算力，保障避障响应速度
   动态避障的核心要求是低延迟响应，需结合Arduino硬件算力，从算法、代码、硬件三方面优化实时性：
   算法轻量化：针对Arduino算力有限的特点，采用计算量小的算法。如案例2采用基于规则的避障逻辑，避免复杂路径规划算法；案例1通过简化加权质心法减少计算量，确保在毫秒级完成测距、解算与控制指令生成。
   代码非阻塞设计：禁用delay()等阻塞函数，采用millis()实现非阻塞定时，结合状态机拆分任务，避免主循环阻塞。如红外+超声波案例中，通过快速读取传感器数据并立即执行决策，减少等待时间，保障控制频率稳定。
   硬件算力匹配：根据算法复杂度选择适配的主控平台。标准Arduino Uno/Nano算力不足，难以处理多传感器融合与复杂算法，推荐采用ESP32、STM32等32位高性能MCU，或“上位机+下位机”架构，将复杂计算（如SLAM、一致性算法）交由上位机，Arduino仅负责底层电机控制与传感器采集。
4. BLDC电机闭环控制：高动态响应，保障运动精度与稳定性
   BLDC电机是避障执行的核心，其控制精度直接决定避障动作的平稳性与准确性，需实现闭环控制以匹配动态避障需求：
   闭环控制模式：采用速度环、位置环双闭环控制，通过编码器反馈实时修正电机转速，避免开环控制的打滑、失步问题。如所有案例均结合编码器数据实现速度闭环，确保左右轮转速严格匹配指令，保障差速转向精度，避免因转速偏差导致的避障轨迹偏移。
   动态响应匹配：BLDC电机具备高扭矩密度、快速启停与加速能力，可满足动态避障对急停、急转的需求。如超声波矩阵案例中，硬避障触发时，电机能在毫秒级响应后退转向指令，快速规避障碍；编队案例中，电机可平滑跟踪队形切换的速度指令，避免机械冲击。
   控制参数整定：根据机械结构与负载特性，精细整定PID参数。距离环与角度环的P参数过大易导致跟随抖动，过小则响应迟缓；积分项可消除稳态误差，微分项抑制超调。需结合实际场景反复调试，确保电机控制平稳无振荡。
5. 安全冗余设计：软硬件双重防护，筑牢系统安全底线
   动态避障涉及机器人与环境、人员的交互，安全冗余是系统落地的核心保障，需从硬件、软件多维度设计防护机制：
   硬件安全机制：设计物理急停按钮，直连电机驱动器使能端，在软件失控时可瞬间切断动力；配备防撞条与碰撞开关，当机器人发生物理碰撞时触发停机；采用独立电源模块为控制电路供电，避免电机启停时的电流冲击导致主控复位；信号线使用屏蔽线，远离动力线布线，防止电磁干扰导致传感器数据异常。
   软件安全逻辑：设置障碍距离阈值，当障碍距离小于安全值时触发紧急制动；避障逻辑具备硬优先级，如案例1中硬避障优先于跟随，避免盲目跟随导致碰撞；加入状态机容错，当目标丢失或传感器失效时，切换至安全模式（如减速、停止）；对电机速度、电流进行限幅，防止过载与超速。
   故障应对策略：针对传感器失效、通信中断等故障，设计降级处理机制。如视觉目标丢失时，切换至IMU与里程计的惯性导航模式；多机器人通信延迟时，通过状态预测模型补偿延迟，避免编队混乱；当持续避障失败时，触发求助信号或进入待机状态，防止系统陷入死循环。

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