Arduino BLDC机器人结合红外与IMU的SLAM导航与安全监控系统，是以Arduino/ESP32为主控、BLDC无刷电机为底盘驱动，通过红外传感器阵列提供近距离障碍物检测与边缘感知、IMU提供高频姿态与航向补偿，经互补滤波或轻量级卡尔曼滤波实现多源数据融合，驱动BLDC电机在自主建图、定位导航的同时构建分层安全防护体系的智能移动机器人系统。 该方案具备红外近距感知与IMU姿态补偿的互补融合、BLDC高精度里程计支撑SLAM、分层安全监控与分级响应、轻量化嵌入式SLAM架构四大特点，主要应用于室内服务配送、仓储AGV、安防巡检及科研教学等场景；实际部署时需重点关注红外传感器环境干扰与布局、IMU安装校准与漂移抑制、主控算力瓶颈与主从架构设计、电源隔离与EMC防护及硬件安全冗余。
一、 技术架构与主要特点
红外近距感知与IMU姿态补偿的互补融合：这是该方案的核心感知架构，两种传感器各取所长、互为补充：
红外感知层：通过多路红外传感器（如TCRT5000、E18-D80NK、GP2Y0A21YK等）组成阵列，部署在机器人前/左/右/底部等方向。红外传感器利用红外发射管发射光束、接收管检测反射信号的原理工作，响应时间在毫秒级，检测距离通常为2~80cm（视型号而定），成本极低（单价通常<5元）。红外传感器在系统中承担三重角色：近距离障碍物检测（防止贴边碰撞）、边缘/悬崖检测（底部安装防止跌落）、以及SLAM建图时的特征辅助（如墙壁边缘、门框等反射率突变点）。
IMU姿态层：IMU（如MPU6050、ICM-20948、BMI088等）以200~1000Hz的高频率输出三轴加速度、三轴角速度和三轴磁力计数据，提供机器人本体的姿态（俯仰、横滚、偏航角）和运动状态。IMU不受光照和遮挡影响，响应极快，但存在积分漂移问题。
融合策略：通过互补滤波或轻量级扩展卡尔曼滤波（EKF）将红外的低频近距障碍物信息与IMU的高频姿态信息进行融合。IMU负责在红外帧间提供高频姿态预测，红外负责校正IMU的累积漂移并补充近距安全边界。当红外检测到障碍物时，系统可结合IMU的航向信息判断障碍物的相对方位，为SLAM的局部避障提供精确的空间约束。
BLDC高精度里程计支撑SLAM：BLDC电机配合编码器（霍尔编码器或磁编码器），不仅是执行机构，更是SLAM系统的核心传感器之一。编码器实时采集各轮转速，通过运动学模型推算出机器人的位置和姿态变化（航迹推算/Odometry），为SLAM前端提供高频但存在累积误差的位姿估计。BLDC电机的高扭矩密度和精确可控性确保里程计数据的平滑性和一致性，而里程计数据的质量直接影响SLAM前端位姿估计的准确性。当车轮打滑时，IMU的加速度和角速度数据可短时补偿里程计的失效，维持SLAM的连续性。
分层安全监控与分级响应：安全监控系统采用"远-中-近"三层防御体系，与SLAM导航深度耦合：
远距层（SLAM全局规划）：SLAM构建的占据栅格地图提供全局障碍物信息，A*或Dijkstra算法规划全局最优路径，避免机器人驶入已知危险区域。
中距层（局部动态避障）：当SLAM局部地图检测到动态障碍物（如突然出现的人或物体）时，触发局部重规划（如动态窗口法DWA或人工势场法APF），实时调整运动轨迹。
近距层（红外硬安全）：红外传感器作为最后一道防线，当检测到极近距离障碍（如<15cm）时，直接触发硬件中断，绕过常规导航指令，强制切断BLDC电机PWM输出或施加电子刹车，响应时间<10ms。
分级响应机制：系统根据风险等级执行不同动作——警告区减速、危险区急停、碰撞区断电制动，确保"导航为任务、安全为底线"的优先级调度。
轻量化嵌入式SLAM架构：在Arduino这类资源受限的微控制器上运行完整的SLAM算法几乎不可能，因此通常采用主从架构：
从机（Arduino/ESP32）：负责底层硬件控制，包括BLDC电机的FOC控制和速度闭环、编码器数据读取与里程计计算、IMU数据读取与初步滤波、红外传感器数据采集，并将融合后的数据通过串口/UART上传给上位机。
主机（上位机，如树莓派、Jetson Nano + ROS）：负责运行SLAM算法（如Gmapping、Cartographer等），接收并融合来自Arduino的里程计和IMU数据，处理红外阵列的障碍物信息，执行SLAM的前端扫描匹配和后端图优化，生成并发布导航目标点。
轻量替代方案：在算力极其受限的场景下，可采用"SLAM-lite"思想——基于红外阵列构建低分辨率的局部占据栅格地图，结合IMU航向校正和编码器里程计，实现简化版的定位与建图，适用于走廊、房间等结构化环境。
二、 典型应用场景
室内服务与配送机器人：在办公室、酒店、餐厅等室内环境中，机器人需要自主导航到指定位置递送物品。红外传感器检测桌腿、椅子、行人等近距障碍，IMU补偿机器人在转弯和加减速时的姿态变化，SLAM构建室内地图并实现精确定位。分层安全监控确保机器人在人流密集区域不会碰撞行人，特别是老人和儿童。
仓储物流AGV/AMR：在仓库货架间自主导航搬运货物。红外传感器检测货架立柱、托盘边缘等近距障碍，IMU补偿地面不平导致的姿态偏差，SLAM实现全局路径规划和动态避障。当检测到工人突然进入通道时，安全监控系统立即触发减速或停车，保障人机协作安全。
安防巡检机器人：在园区、厂区或建筑物内自主巡逻。SLAM构建巡检环境地图并按规划路径巡逻，红外传感器检测围栏、障碍物和异常入侵，IMU确保机器人在坡道或不平地面上保持航向稳定。当检测到异常时，机器人可自动报警并记录位置信息。
科研与教育平台：作为SLAM算法学习、多传感器融合、嵌入式安全系统设计的教学实验平台。学生可在Arduino+BLDC硬件上验证里程计推算、IMU姿态融合、红外避障逻辑和分层安全架构等核心概念，为后续开发更复杂的自主移动机器人奠定基础。
三、 关键注意事项
红外传感器环境干扰与布局：
强光干扰：日光和白炽灯含大量红外成分，可能导致误触发。建议使用调制型红外传感器（如带38kHz调制的E18系列），或加装遮光罩，并在软件上做多次采样滤波（如连续3次检测到障碍才判定有效）。
黑色/吸光物体：黑色地毯或深色物体吸光严重，红外可能"看不见"。建议搭配超声波传感器补盲，形成异构感知冗余。
镜面反射：光滑表面可能导致红外误反射，产生虚假障碍物。需合理调整传感器安装角度（向下倾斜10°~15°），避免平行于反射面安装。
阵列布局：建议至少布置前/左/右三路红外传感器形成扇形覆盖，底部安装用于悬崖检测。探测角度通常为±15°，需根据机器人尺寸和运动速度合理配置检测距离阈值。
IMU安装校准与漂移抑制：
IMU必须刚性固定在底盘重心附近，避免柔性连接引入相位滞后。在BLDC电机附近安装时需加硅胶减震垫以隔离高频振动，否则电机振动会严重污染加速度计数据。
需定期校准陀螺仪零偏（静止30秒取均值）和加速度计（六面静置法），并根据实际振动情况微调互补滤波系数，在动态响应与抗漂移之间取得平衡。
IMU坐标系需与机器人机体坐标系对齐，安装时需查阅数据手册，必要时乘以固定旋转矩阵进行校正。
主控算力瓶颈与主从架构设计：
SLAM算法（特别是基于图优化的方法）对计算资源要求极高。标准Arduino Uno（16MHz、2KB SRAM）无法胜任完整SLAM，建议采用ESP32（双核240MHz）或STM32作为从机，配合树莓派/Jetson Nano作为主机运行SLAM。
控制回路必须使用硬件定时器中断或millis()非阻塞定时（建议控制频率≥50Hz），严禁使用delay()函数，以确保红外采样、IMU读取和电机控制的严格同步。
传感器数据上传时需加入时间戳，确保上位机进行多源数据融合时的时间对齐精度。
电源隔离与EMC防护：
BLDC电机启停时电流冲击极大（堵转可达额定3_{5倍），严禁与Arduino及传感器共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容（1000}4700μF）吸收反电动势。
红外传感器和IMU的信号线需使用屏蔽线，并远离动力线布线（间距≥5cm），防止高频PWM噪声干扰传感器数据。
BLDC电机急刹时产生的反向电动势和电磁干扰（EMC）极易导致主控MCU死机或传感器数据错乱，必须做好严格的电源隔离和信号屏蔽。
硬件安全冗余设计：
软件层面的安全逻辑不能作为唯一保障。必须保留独立的物理急停按钮和机械防撞条作为最后防线。
红外安全中断应直接接入BLDC驱动芯片的使能（EN）引脚，实现物理级断电，而非仅依赖软件切断PWM。
启用硬件看门狗定时器，防止程序跑飞导致电机失控。
加入S曲线加减速算法，避免阶跃速度指令导致轮胎打滑或惯性碰撞。

1、编码器+IMU里程计SLAM（差速底盘基础框架）
场景：室内平坦环境下的自主巡逻机器人，通过编码器和IMU进行航位推算定位，构建简易占用栅格地图。

核心逻辑：利用编码器脉冲计算左右轮位移，通过差速运动学模型推算机器人位姿（x, y, θ）；IMU提供陀螺仪角速度数据，用于修正里程计累积的航向角漂移。该方案无需视觉或激光雷达，适合资源受限的Arduino平台。

#include <SimpleFOC.h>
#include <Encoder.h>
#include <MPU6050.h>

// BLDC差速电机
BLDCMotor motorL(7), motorR(8);
Encoder encL(2, 3), encR(4, 5);
MPU6050 imu;

// 位姿变量
float x = 0, y = 0, theta = 0;
const float WHEEL_RADIUS = 0.05;   // 轮半径(m)
const float BASE_WIDTH = 0.30;      // 轮距(m)
const float ENCODER_PPR = 1000.0;   // 编码器每转脉冲数

void loop() {
    // 1. 读取编码器计数
    long leftCount = encL.read();
    long rightCount = encR.read();
    encL.write(0); encR.write(0);  // 清零

    // 2. 计算左右轮位移
    float dLeft = (leftCount / ENCODER_PPR) * 2 * PI * WHEEL_RADIUS;
    float dRight = (rightCount / ENCODER_PPR) * 2 * PI * WHEEL_RADIUS;
    float dCenter = (dLeft + dRight) / 2.0;

    // 3. 差速运动学更新位姿
    float dTheta = (dRight - dLeft) / BASE_WIDTH;
    theta += dTheta;
    x += dCenter * cos(theta);
    y += dCenter * sin(theta);

    // 4. IMU修正航向角（互补滤波或EKF）
    int16_t gz = imu.getGyroZ();  // 陀螺仪Z轴角速度
    theta = 0.98 * (theta + gz * 0.01) + 0.02 * getAccelYaw();

    // 5. 电机速度控制（以目标速度驱动）
    motorL.move(targetSpeedL);
    motorR.move(targetSpeedR);
    motorL.loopFOC(); motorR.loopFOC();

    delay(10);  // 100Hz控制循环
}

2、多传感器融合导航（超声波+红外+IMU）
场景：仓库或办公环境中的自主导航机器人，使用超声波和红外传感器构建局部地图，结合IMU校正位姿，实现基础SLAM导航与安全监控。

核心逻辑：超声波和红外传感器共同检测障碍物，任一传感器触发阈值即判定为不安全；编码器里程计提供位姿基础，IMU通过高频姿态数据补偿里程计漂移。这是一种传感器冗余与优势互补的融合策略。

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

#define SONAR_TRIG 9
#define SONAR_ECHO 10
#define IR_PIN A0
#define SAFE_DIST 30  // 安全距离阈值(cm)

NewPing sonar(SONAR_TRIG, SONAR_ECHO, 200);
MPU6050 imu;
BLDCMotor motorL(7), motorR(8);

float currentX = 0, currentY = 0, currentAngle = 0;
bool obstacleMap[20][20];  // 简易占用栅格

void loop() {
    // 1. 安全监控融合：超声波+红外
    float sonarDist = sonar.ping_cm();
    float irDist = getIrDistance();  // 红外测距
    bool isSafe = (sonarDist > SAFE_DIST || sonarDist == 0) && 
                  (irDist > SAFE_DIST || irDist == 0);

    if (!isSafe) {
        motorL.move(0); motorR.move(0);  // 紧急停车
        return;
    }

    // 2. 更新地图（简化：根据障碍物距离标记栅格）
    if ((sonarDist < 50 && sonarDist > 0) || (irDist < 50 && irDist > 0)) {
        int gridX = (int)(currentX / 10);
        int gridY = (int)(currentY / 10);
        obstacleMap[gridX][gridY + 1] = true;
    }

    // 3. 更新位姿（编码器里程计+IMU修正）
    updatePoseFromEncoders();
    updateAngleFromIMU();

    // 4. 路径规划驱动（示例：简单导航决策）
    if (isSafe) {
        motorL.move(targetSpeed);
        motorR.move(targetSpeed);
    }

    delay(50);
}

float getIrDistance() {
    int val = analogRead(IR_PIN);
    return 1000.0 / (val + 1);  // 近似距离换算
}

3、多级安全防护引擎（三级制动+IMU姿态监测）
场景：需要高安全等级的服务机器人或巡检机器人，通过超声波、红外、IMU三类传感器构建立体安全感知网络，执行分级制动策略。

核心逻辑：将前方及两侧的障碍物距离划分为预警区→制动区→临界区三级，对应不同强度的减速或急停响应。IMU同时监测倾角，防止机器人因地面起伏侧翻。

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

NewPing sonarF(TRIG_F, ECHO_F);
NewPing sonarL(TRIG_L, ECHO_L);
NewPing sonarR(TRIG_R, ECHO_R);
MPU6050 imu;
BLDCMotor motorL(7), motorR(8);

// 三级安全阈值(cm)
const float FRONT_WARN = 50, FRONT_BRAKE = 25, FRONT_CRIT = 10;
const float SIDE_WARN = 30, SIDE_BRAKE = 15, SIDE_CRIT = 8;
const float TILT_THRESHOLD = 15.0;  // 倾角阈值(度)

enum SafetyLevel { SAFE, WARNING, BRAKE, CRITICAL };
SafetyLevel currentLevel = SAFE;

void loop() {
    // 1. 采集传感器数据
    float fDist = sonarF.ping_cm();
    float lDist = sonarL.ping_cm();
    float rDist = sonarR.ping_cm();
    float tilt = getTiltAngle();  // IMU倾角

    // 2. 分级判定
    if (tilt > TILT_THRESHOLD || fDist < FRONT_CRIT || lDist < SIDE_CRIT || rDist < SIDE_CRIT) {
        currentLevel = CRITICAL;
    } else if (fDist < FRONT_BRAKE || lDist < SIDE_BRAKE || rDist < SIDE_BRAKE) {
        currentLevel = BRAKE;
    } else if (fDist < FRONT_WARN || lDist < SIDE_WARN || rDist < SIDE_WARN) {
        currentLevel = WARNING;
    } else {
        currentLevel = SAFE;
    }

    // 3. 分级制动执行
    switch(currentLevel) {
        case SAFE:
            motorL.move(1.0); motorR.move(1.0);  // 正常速度
            break;
        case WARNING:
            motorL.move(0.7); motorR.move(0.7);  // 减速预警
            break;
        case BRAKE:
            motorL.move(0.2); motorR.move(0.2);  // 急减速
            break;
        case CRITICAL:
            motorL.move(0); motorR.move(0);      // 紧急急停
            // 若倾覆则锁定电机
            if (tilt > TILT_THRESHOLD) delay(5000);
            break;
    }

    motorL.loopFOC(); motorR.loopFOC();
    delay(30);
}

float getTiltAngle() {
    int16_t ax, ay, az;
    imu.getAcceleration(&ax, &ay, &az);
    return atan2(ay, az) * 180 / PI;  // 横滚角
}

要点解读
多传感器融合是解决单一传感器局限性的核心手段：超声波在玻璃、镜面等光滑表面反射率差；红外传感器受强光干扰严重；编码器在打滑时累积误差；IMU存在积分漂移。融合不同原理的传感器，通过互补滤波或EKF（扩展卡尔曼滤波）将各自优势结合，才能获得可靠的定位和环境感知。

SLAM的“算力瓶颈”需通过异构架构突破：经典的8位AVR Arduino无法运行完整的SLAM算法。工程实践中推荐采用双MCU异构架构——下位机（Arduino Due/ESP32）负责BLDC电机控制和实时传感器采集，上位机（树莓派/Jetson Nano）运行视觉SLAM或激光SLAM算法，通过串口通信下发控制指令。

安全监控应建立“分级响应+冗余制动”机制：单一传感器的误报可能导致机器人频繁急停，影响任务效率。三级安全模型（预警→制动→急停）通过动态调整制动力矩，避免机械冲击。同时，建议在软件逻辑之外增设独立硬件看门狗监控主控芯片工作状态。

航位推算误差的累积是室内导航的最大挑战：编码器里程计在长距离运行中误差会持续累积，导致地图扭曲。IMU的高频姿态数据可提供短期约束，但仍无法完全消除漂移。引入回环检测（如视觉特征匹配或二维码地标）是校正累积误差的有效手段。

BLDC的FOC驱动是实现平滑安全制动的物理基础：SimpleFOC等库提供的力矩/速度闭环控制，使机器人在安全制动时能够实现柔性减速，而非硬刹车导致的机械冲击。IMU监测到倾覆风险时，BLDC电机可快速执行自锁，防止跌落。

4、家庭服务机器人——红外+IMU融合的轻量SLAM导航与避障监控
适用场景：家庭环境中，机器人自主探索房间、跟随用户，通过红外检测近距离障碍（桌腿、宠物），结合IMU修正SLAM姿态误差，实现稳定建图与导航，同时监控人员靠近触发安全预警。

核心逻辑：
IMU姿态解算：融合加速度计与陀螺仪，实时输出机器人偏航角（航向角），修正SLAM定位漂移；
红外避障与建图辅助：多路红外传感器检测近距离障碍，为SLAM提供障碍物边界信息，同时触发实时避障；
轻量SLAM实现：基于里程计+IMU的扩展卡尔曼滤波（EKF）SLAM，通过编码器+IMU推算位姿，红外补充局部障碍信息，构建栅格地图；
安全监控：红外检测人员靠近，启动声光预警，避免碰撞。

/* ===== 家庭服务机器人：红外+IMU融合SLAM导航与安全监控 =====
 * 核心：IMU姿态修正+红外避障+轻量EKF SLAM，实现自主建图与安全预警
 * 适配：2轮BLDC+全向轮，4路红外传感器，MPU6050 IMU
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include <NewPing.h>

// 硬件定义
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19), encR(20,21);
MPU6050 imu;
// 红外传感器（模拟，实际为红外发射/接收模块）
#define IR_LEFT 4  #define IR_RIGHT 5  #define IR_FRONT 6  #define IR_BACK 7
int irLeft = 0, irRight = 0, irFront = 0, irBack = 0;

// SLAM核心参数
float odometry_x = 0, odometry_y = 0, odometry_theta = 0; // 里程计位姿
float imu_yaw = 0; // IMU偏航角
float slam_x = 0, slam_y = 0, slam_theta = 0; // SLAM输出位姿
float wheel_radius = 0.035; // 轮径（m）
float wheel_base = 0.15; // 轮距（m）

// 安全监控参数
const float SAFE_DIST = 0.3; // 安全距离（m）
bool safety_trigger = false;

void setup() {
  Serial.begin(115200);
  // 初始化BLDC
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 初始化IMU
  Wire.begin();
  imu.initialize();
  // 初始化红外引脚
  pinMode(IR_LEFT, INPUT); pinMode(IR_RIGHT, INPUT);
  pinMode(IR_FRONT, INPUT); pinMode(IR_BACK, INPUT);
}

// IMU姿态解算（互补滤波）
float updateIMUYaw() {
  float accX = imu.getAccelerationX();
  float accY = imu.getAccelerationY();
  float gyroZ = imu.getRotationZ();
  
  // 加速度计计算偏航角（静态时有效）
  float accYaw = atan2(accX, accY) * RAD_TO_DEG;
  // 互补滤波融合陀螺仪（动态修正）
  imu_yaw = 0.98 * (imu_yaw + gyroZ * 0.01) + 0.02 * accYaw;
  return imu_yaw;
}

// 里程计推算位姿
void updateOdometry() {
  // 编码器计算左右轮速度（简化，实际为脉冲数换算）
  float left_vel = motorL.velocity_controller.sensor_vel * wheel_radius * 0.01;
  float right_vel = motorR.velocity_controller.sensor_vel * wheel_radius * 0.01;
  float linear_vel = (left_vel + right_vel) / 2;
  float angular_vel = (right_vel - left_vel) / wheel_base;
  
  // 时间步长（简化为固定周期）
  float dt = 0.1;
  // 更新里程计位姿
  odometry_theta += angular_vel * dt;
  odometry_x += linear_vel * cos(odometry_theta) * dt;
  odometry_y += linear_vel * sin(odometry_theta) * dt;
}

// EKF SLAM（简化版：IMU修正里程计漂移）
void updateSLAM() {
  updateOdometry();
  float imu_yaw = updateIMUYaw();
  // 用IMU修正里程计偏航角漂移
  slam_theta = imu_yaw;
  slam_x = odometry_x * cos(odometry_theta - slam_theta) + odometry_y * sin(odometry_theta - slam_theta);
  slam_y = odometry_y * cos(odometry_theta - slam_theta) - odometry_x * sin(odometry_theta - slam_theta);
}

// 红外避障与安全监控
void irSafetyMonitor() {
  // 读取红外传感器（实际为数字信号，距离简化映射）
  irLeft = analogRead(IR_LEFT) < 200; // 障碍存在=1
  irRight = analogRead(IR_RIGHT) < 200;
  irFront = analogRead(IR_FRONT) < 200;
  irBack = analogRead(IR_BACK) < 200;
  
  // 安全监控：检测人员靠近（红外弱信号，代表距离近）
  if (analogRead(IR_FRONT) < 300) {
    safety_trigger = true;
    Serial.println("&#9888;️ 人员靠近，安全预警！");
    // 声光预警（简化，实际接蜂鸣器/LED）
  } else {
    safety_trigger = false;
  }
  
  // 实时避障
  if (irFront) {
    motorL.move(-0.2); motorR.move(0.2); // 右转避障
    delay(500);
  } else if (irLeft) {
    motorL.move(0.3); motorR.move(0.3);
  } else if (irRight) {
    motorL.move(-0.3); motorR.move(-0.3);
  } else {
    motorL.move(0.5); motorR.move(0.5); // 正常前进
  }
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  updateSLAM();
  irSafetyMonitor();
  
  // 输出SLAM位姿与安全状态
  Serial.printf("SLAM: (%.2f, %.2f, %.1f°) | Safety: %d\n", 
    slam_x, slam_y, slam_theta, safety_trigger);
  delay(100);
}

5、工业巡检机器人——红外+IMU的SLAM路径规划与危险环境监控
适用场景：工厂车间（有高温、漏电风险区域）的自主巡检，机器人通过红外检测地面障碍与危险源，结合IMU+SLAM构建工厂地图，规划巡检路径，同时监控危险区域入侵并触发应急停机。

核心逻辑：
SLAM地图构建与路径规划：基于IMU+编码器的里程计，结合红外补充障碍信息，构建车间栅格地图，实现预设路径的自主导航；
红外危险源检测：红外传感器检测高温设备、漏电区域的热辐射，识别危险区域，在地图上标记；
安全监控与应急：检测到人员闯入危险区域，立即触发声光预警，停止电机并上报位置；
IMU姿态补偿：在车间不平整地面，IMU修正机器人姿态变化，确保导航轨迹稳定。

/* ===== 工业巡检机器人：红外+IMU融合SLAM与危险监控 =====
 * 核心：SLAM建图+路径规划+红外危险检测，实现自主巡检与应急响应
 * 适配：BLDC巡检小车，4路红外热释/热敏传感器，MPU6050 IMU
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

// 硬件定义
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19), encR(20,21);
MPU6050 imu;
// 红外危险检测（热敏/热释模块）
#define IR_HEAT1 8  #define IR_HEAT2 9  #define IR_HEAT3 10  #define IR_HEAT4 11
int heat1=0, heat2=0, heat3=0, heat4=0;

// SLAM与导航参数
float slam_x=0, slam_y=0, slam_theta=0;
float preset_path[5][2] = {{0,0}, {2,0}, {2,3}, {5,3}, {5,0}}; // 预设巡检路径点
int current_point = 0;
bool path_complete = false;

// 安全监控参数
bool danger_detected = false;
const int HEAT_THRESHOLD = 500; // 高温阈值（ADC值）

void setup() {
  Serial.begin(115200);
  // 初始化BLDC
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 初始化IMU
  Wire.begin();
  imu.initialize();
  // 初始化红外引脚
  pinMode(IR_HEAT1, INPUT); pinMode(IR_HEAT2, INPUT);
  pinMode(IR_HEAT3, INPUT); pinMode(IR_HEAT4, INPUT);
}

// IMU+里程计SLAM位姿更新
float updateSlamPose() {
  float imu_yaw = updateIMUYaw(); // 复用案例1的IMU解算函数
  // 里程计推算
  float left_vel = motorL.velocity_controller.sensor_vel * 0.01;
  float right_vel = motorR.velocity_controller.sensor_vel * 0.01;
  float linear_vel = (left_vel + right_vel) / 2;
  float angular_vel = (right_vel - left_vel) / 0.15;
  float dt = 0.1;
  
  static float prev_theta = 0;
  slam_theta = imu_yaw; // IMU修正航向
  slam_x += linear_vel * cos(slam_theta) * dt;
  slam_y += linear_vel * sin(slam_theta) * dt;
  return slam_theta;
}

// 预设路径跟踪
void trackPresetPath() {
  float target_x = preset_path[current_point][0];
  float target_y = preset_path[current_point][1];
  float dx = target_x - slam_x;
  float dy = target_y - slam_y;
  float distance = sqrt(dx*dx + dy*dy);
  
  if (distance < 0.2) { // 到达当前路径点
    current_point++;
    if (current_point >= 5) {
      current_point = 0;
      path_complete = true;
      Serial.println("&#9989; 巡检路径完成，重新开始");
    }
  } else {
    // 计算目标航向，通过PID控制转向
    float target_angle = atan2(dy, dx) * 180 / PI;
    float angle_error = target_angle - slam_theta;
    // 简化PID控制转向
    float turn_cmd = angle_error * 0.02;
    motorL.move(0.3 - turn_cmd);
    motorR.move(0.3 + turn_cmd);
  }
}

// 红外危险检测与安全监控
void dangerMonitor() {
  // 读取高温/漏电危险信号
  heat1 = analogRead(IR_HEAT1);
  heat2 = analogRead(IR_HEAT2);
  heat3 = analogRead(IR_HEAT3);
  heat4 = analogRead(IR_HEAT4);
  
  // 检测高温危险
  if (heat1 > HEAT_THRESHOLD || heat2 > HEAT_THRESHOLD) {
    danger_detected = true;
    Serial.println("&#9888;️ 危险区域：高温检测！位置：(%.2f, %.2f)", slam_x, slam_y);
    // 应急停机
    motorL.move(0); motorR.move(0);
    // 上报位置（实际通过无线模块）
  } else {
    danger_detected = false;
  }
  
  // 路径规划避开危险区域（简化：在地图上标记危险点，路径规划时绕行）
  if (danger_detected) {
    // 此处为简化，实际需调用路径重规划算法，此处直接暂停并等待
    delay(3000);
    motorL.move(0.3); motorR.move(0.3); // 危险解除后继续
  }
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  updateSlamPose();
  trackPresetPath();
  dangerMonitor();
  
  Serial.printf("巡检位姿: (%.2f, %.2f) | 危险状态: %d\n", 
    slam_x, slam_y, danger_detected);
  delay(100);
}

6、废墟救援机器人——红外+IMU的极端环境SLAM与生命监控
适用场景：地震废墟等无GPS、高粉尘、弱光环境，机器人通过红外热释检测生命体征，结合IMU+SLAM构建废墟内部地图，自主避障导航，同时监控自身姿态（倾斜）与生命目标。

核心逻辑：
极端环境SLAM：无GPS时，依赖IMU+编码器的紧耦合SLAM，通过红外辅助修正障碍边界，抵抗粉尘对视觉的干扰；
红外生命探测：热释红外传感器检测废墟下的人体热辐射，识别生命目标位置并标记在SLAM地图上；
姿态安全监控：IMU检测机器人倾斜角度，超过阈值（如30°）时判定被困，启动自救程序（调整姿态、发出求救信号）；
安全避障：红外检测近距离障碍（碎石、钢筋），结合SLAM地图实现实时避障，避免二次坍塌。

/* ===== 废墟救援机器人：红外+IMU融合SLAM与生命监控 =====
 * 核心：极端环境SLAM+红外生命探测+IMU姿态安全，实现废墟自主导航与生命定位
 * 适配：履带式BLDC底盘，热释红外传感器，MPU6050 IMU，粉尘环境专用
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

// 硬件定义
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19), encR(20,21);
MPU6050 imu;
// 热释红外生命检测
#define IR_LIFE 12
int life_detected = 0;

// SLAM与姿态参数
float slam_x=0, slam_y=0, slam_theta=0;
float roll=0, pitch=0; // IMU倾斜角度
float tilt_threshold = 30; // 倾斜安全阈值（度）
bool self_rescue = false;

// 生命目标位置
float life_x=0, life_y=0;
bool life_marked = false;

void setup() {
  Serial.begin(115200);
  // 初始化BLDC（履带底盘）
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 初始化IMU
  Wire.begin();
  imu.initialize();
  // 初始化红外引脚
  pinMode(IR_LIFE, INPUT);
}

// IMU姿态解算（倾斜角：roll/pitch）
void updateIMUTilt() {
  float accX = imu.getAccelerationX();
  float accY = imu.getAccelerationY();
  float accZ = imu.getAccelerationZ();
  
  // 加速度计计算倾斜角（静态下准确）
  roll = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ)) * RAD_TO_DEG;
  
  // 陀螺仪动态修正（简化互补滤波）
  static float prev_roll = 0, prev_pitch = 0;
  float gyroX = imu.getRotationX();
  float gyroY = imu.getRotationY();
  roll = 0.95 * (prev_roll + gyroX * 0.01) + 0.05 * roll;
  pitch = 0.95 * (prev_pitch + gyroY * 0.01) + 0.05 * pitch;
  prev_roll = roll; prev_pitch = pitch;
}

// 极端环境SLAM（IMU+里程计紧耦合）
void updateRescueSLAM() {
  updateIMUTilt();
  float imu_yaw = updateIMUYaw(); // 复用偏航角解算
  // 里程计推算（履带底盘）
  float left_vel = motorL.velocity_controller.sensor_vel * 0.01;
  float right_vel = motorR.velocity_controller.sensor_vel * 0.01;
  float linear_vel = (left_vel + right_vel) / 2;
  float angular_vel = (right_vel - left_vel) / 0.2; // 履带间距0.2m
  float dt = 0.1;
  
  slam_theta = imu_yaw;
  slam_x += linear_vel * cos(slam_theta) * dt;
  slam_y += linear_vel * sin(slam_theta) * dt;
  
  // 红外辅助修正障碍边界（简化：检测到障碍则在地图上标记）
  if (analogRead(IR_LIFE) < 200) { // 近距离障碍
    // 此处简化，实际需记录障碍坐标并更新SLAM地图
  }
}

// 红外生命探测与定位
void lifeDetection() {
  life_detected = digitalRead(IR_LIFE); // 热释红外触发信号
  if (life_detected) {
    life_x = slam_x + 0.5 * cos(slam_theta); // 生命目标相对机器人位置（简化距离0.5m）
    life_y = slam_y + 0.5 * sin(slam_theta);
    life_marked = true;
    Serial.printf("&#9989; 生命目标检测！位置：(%.2f, %.2f)\n", life_x, life_y);
    // 标记在SLAM地图上（实际需调用地图标记函数）
  }
}

// 姿态安全监控与自救
void tiltSafetyMonitor() {
  updateIMUTilt();
  if (abs(roll) > tilt_threshold || abs(pitch) > tilt_threshold) {
    self_rescue = true;
    Serial.println("&#9888;️ 机器人倾斜超阈值，启动自救！");
    // 自救程序：调整姿态（反向运动）
    if (roll > tilt_threshold) {
      motorL.move(-0.2); motorR.move(0.2); // 左侧倾斜，向右调整
    } else if (roll < -tilt_threshold) {
      motorL.move(0.2); motorR.move(-0.2); // 右侧倾斜，向左调整
    }
    delay(1000);
    // 发出求救信号（实际接蜂鸣器/无线模块）
    if (self_rescue) {
      Serial.println("请求外部救援，当前位置：(%.2f, %.2f)", slam_x, slam_y);
    }
  } else {
    self_rescue = false;
    motorL.move(0.3); motorR.move(0.3); // 恢复正常前进
  }
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  updateRescueSLAM();
  lifeDetection();
  tiltSafetyMonitor();
  
  // 输出关键信息
  Serial.printf("SLAM: (%.2f, %.2f) | 倾斜: Roll=%.1f°, Pitch=%.1f° | 生命: %d\n",
    slam_x, slam_y, roll, pitch, life_detected);
  delay(100);
}

要点解读

1. SLAM核心的“传感器互补逻辑”：红外解决近距离感知，IMU保障姿态鲁棒性
   红外与IMU在SLAM中形成“近距离障碍补充+全局姿态保障”的互补体系，是低成本SLAM的核心逻辑：
   红外的核心价值：弥补视觉/激光雷达在弱光、粉尘环境中的失效，近距离（0-2m）检测障碍与目标（生命、人员、障碍物），为SLAM提供局部障碍边界信息，辅助避障；
   IMU的核心价值：解决编码器里程计的累积漂移，通过实时解算偏航角、倾斜角，修正SLAM位姿误差，同时在传感器短暂失效（如红外被遮挡）时，通过IMU+编码器实现短时位姿推算，保障SLAM连续性；
   融合策略：采用紧耦合EKF融合红外、IMU、编码器数据——IMU提供姿态修正，编码器提供位移增量，红外补充障碍约束，三者联合输出高精度位姿与地图，避免单一传感器失效导致的系统崩溃。
2. 导航定位的“低成本SLAM架构”：基于IMU+编码器的轻量方案
   针对Arduino算力限制，采用轻量级里程计+IMU的SLAM架构，是低成本机器人导航的核心路径：
   架构核心：不依赖高算力视觉SLAM，而是通过编码器（位移）+IMU（姿态）构建相对位姿推算系统，结合红外障碍约束，实现轻量SLAM；
   算法选择：采用扩展卡尔曼滤波（EKF）融合多传感器数据——状态变量包含机器人位姿（x,y,θ）、IMU偏置，测量变量包含编码器速度、红外障碍信息，通过迭代更新降低漂移；
   算力优化：Arduino算力有限，需精简状态变量维度，采用固定频率（100Hz）采样，避免复杂的非线性优化（如因子图），同时利用IMU的高频特性（1kHz）插值编码器数据，提升位姿更新精度。
3. 安全监控的“多维度预警机制”：红外检测目标，IMU监控姿态
   安全监控需覆盖环境目标与机器人自身状态，形成“外部目标+内部姿态”的双维度预警：
   红外目标监控：通过热释红外检测人员靠近、生命体征，通过热敏红外检测高温危险源，结合距离阈值触发预警（如人员<0.5m时声光预警，生命目标触发救援标记）；
   IMU姿态监控：实时解算机器人倾斜角（roll/pitch），设定安全阈值（如30°），超过阈值判定机器人被困，启动自救程序（调整姿态、反向运动），同时发出求救信号；
   预警联动机制：安全监控与导航、避障联动——预警触发时，优先保障安全（停机、避障），再恢复导航任务，避免二次风险，例如工业巡检中高温预警触发后，先停机上报，再规划绕行路径。
4. 控制闭环的“BLDC动态响应适配”：支撑SLAM与避障的实时执行
   BLDC电机的动态响应直接决定SLAM导航的精度与安全监控的及时性，核心是构建高精度、低延迟的速度/姿态控制闭环：
   速度闭环控制：采用编码器+FOC（磁场定向控制）实现BLDC速度闭环，响应时间≤10ms，精准跟踪SLAM规划的速度指令，避免因速度波动导致位姿推算误差；
   姿态联动控制：当IMU检测到倾斜超阈值时，BLDC快速执行自救指令（如反向调整姿态），要求电机具备高动态响应，通过PID参数优化（采用位置式PID）实现快速、平稳的姿态调整；
   避障实时执行：红外检测到障碍时，BLDC需在100ms内完成转向/制动，因此需配置编码器高采样频率（≥500Hz），结合PID的微分项抑制转向震荡，确保避障动作的流畅性与准确性。
5. 工程落地的“算力与鲁棒性设计”：适配Arduino平台的轻量化与可靠性
   Arduino算力有限且环境复杂，需通过轻量化算法与鲁棒性设计保障工程落地：
   算力优化策略：精简SLAM状态变量，采用定点运算替代浮点运算，降低计算量；使用非阻塞编程，避免delay()阻塞主循环，确保传感器采样与电机控制的实时性；
   环境鲁棒性：硬件上对红外传感器增加防尘罩、滤波电路，IMU固定在减震支架上，减少粉尘、振动干扰；软件上采用互补滤波、中值滤波处理传感器数据，抑制噪声与异常值；
   故障容错机制：设计多级容错——红外失效时，依赖IMU+编码器SLAM维持导航；IMU失效时，切换为纯编码器里程计+红外避障；导航失控时，触发紧急停机，保障机器人与环境安全；同时预留串口/无线通信接口，实现远程监控与故障排查。

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