Arduino BLDC视觉+IMU融合目标跟随服务机器人，是以Arduino/ESP32为主控、BLDC无刷电机为底盘驱动，通过摄像头进行目标识别与跟踪、IMU提供高频姿态与航向补偿，经扩展卡尔曼滤波（EKF）或互补滤波实现紧耦合融合，驱动BLDC电机精确执行跟随运动的服务型智能机器人系统。 该方案具备视觉全局感知与IMU高频补偿的互补融合、BLDC高动态响应与静音执行、目标丢失惯性航位推算、动态距离自适应与主动避障四大特点，主要应用于智能随行助手、医疗康复辅助、安防巡检伴随及科研教学平台等场景；实际部署时需重点关注主控算力分配与实时性保障、IMU安装校准与抗振、视觉抗干扰与遮挡处理、电源隔离与EMC防护及PID调参与安全冗余。
一、 技术架构与主要特点
视觉全局感知与IMU高频补偿的互补融合：这是该方案的核心感知架构，两种传感器各取所长、互为补充：
视觉感知层：通过摄像头（如OpenMV、ESP32-CAM或外接USB摄像头）进行目标识别与跟踪。常用算法包括颜色/特征标记识别（如AprilTag、二维码）、深度学习目标检测（如YOLO、MobileNet等轻量化模型）以及视觉伺服（Visual Servoing）。视觉提供目标的相对位置（像素偏差）、距离估计和身份识别信息，帧率通常为10~30fps，具有全局感知能力，但受光照变化、遮挡和运动模糊影响较大。
IMU姿态层：IMU（如MPU6050、ICM-20948、BMI088等）以200~1000Hz的高频率输出三轴加速度、三轴角速度和三轴磁力计数据，提供机器人自身的姿态（俯仰、横滚、偏航角）和运动状态。IMU不受光照和遮挡影响，响应极快，但存在积分漂移问题。
融合策略：通过扩展卡尔曼滤波（EKF）或互补滤波将视觉的低频全局位置信息与IMU的高频姿态信息进行紧耦合。IMU负责在视觉帧间提供高频预测（“快指针”），视觉负责低频校正IMU的累积漂移（“慢指针”）。当视觉短暂丢失目标时，IMU的惯性航位推算可维持短时跟随方向，避免机器人"愣住"。
BLDC高动态响应与静音执行：BLDC电机配合FOC（磁场定向控制）驱动，具备毫秒级扭矩响应和极低转矩脉动，能够精确执行跟随控制算法输出的速度/转向指令。BLDC无电刷摩擦，运行噪音极低（<40dB），特别适合酒店、医院、商场等需要安静环境的服务场景。双向控制支持差速转向和原地旋转，在狭窄空间内灵活跟随。BLDC效率高达85%~95%，配合锂电池可支持长时间连续运行。
目标丢失惯性航位推算：在实际跟随场景中，目标可能被行人、柱子等短暂遮挡，导致视觉丢失。系统利用IMU的高频姿态数据和编码器里程计，在视觉丢失期间进行惯性航位推算（Dead Reckoning），预测目标的运动趋势，维持跟随方向和速度。当视觉重新捕获目标后，通过EKF快速校正累积误差，实现"无缝"跟随。
动态距离自适应与主动避障：跟随机器人需根据环境动态调整与目标的距离——在开阔空间保持较远距离（如1.5m），在狭窄通道自动缩短跟随距离（如0.5m）。同时，系统需集成超声波/ToF/激光雷达等避障传感器，当检测到前方障碍物时，立即挂起跟随任务，执行避让动作，确保"跟随为任务、避障为生存"的优先级调度。
二、 典型应用场景
智能随行助手（消费级）：在机场、商场、酒店等场景中，作为智能行李车或购物车自动跟随顾客。视觉识别顾客（通过人脸、衣着颜色或佩戴的标记），IMU补偿机器人在人群中的姿态稳定性，BLDC提供静音平稳的驱动力。结合动态距离自适应，机器人能在拥挤环境中灵活穿梭，不跟丢、不碰撞。
医疗康复与智能助行辅助：在医院或康复中心，作为智能助行器或输液架跟随患者移动。视觉+IMU融合确保在患者改变步速或转向时能及时响应，BLDC的低噪音特性不干扰病房环境。IMU的姿态补偿使机器人在坡道或不平地面上保持稳定，防止倾倒。
安防巡检与特种作业伴随：作为移动监控平台或设备运载车，伴随安保人员或特种作业人员在园区、危险区域巡逻。视觉的语义识别能力可辅助识别异常行为，IMU的姿态补偿使机器人在复杂地形下仍能保持稳定的跟随方位，减轻人员负担并扩展作业视野。
高校科研与机器人竞赛验证：作为ROS导航、SLAM、多传感器融合及BLDC闭环控制的理想实验平台。在RoboMaster或全国大学生智能汽车竞赛中，该方案被广泛用于验证视觉伺服、航向角补偿及动态避障等前沿算法。
三、 关键注意事项
主控算力分配与实时性保障：
视觉特征解算、IMU姿态融合与多路BLDC的FOC控制对算力要求极高。标准Arduino Uno（16MHz、2KB SRAM）难以胜任，建议采用ESP32（双核240MHz）、STM32或树莓派+Arduino的异构架构。可采用双核隔离策略——Core0负责AI视觉推理（低优先级），Core1负责电机控制+传感器采样（高优先级），杜绝AI推理阻塞电机控制。
控制回路必须使用硬件定时器中断或millis()非阻塞定时（建议控制频率≥50Hz），严禁使用delay()函数，以确保视觉数据与电机控制的严格同步。
IMU安装校准与抗振设计：
IMU必须刚性固定在底盘重心附近，避免柔性连接引入相位滞后。在电机附近安装时需加硅胶减震垫以隔离高频振动，否则电机振动会严重污染加速度计数据。
需定期校准陀螺仪零偏（静止30秒取均值）和加速度计（六面静置法），并根据实际振动情况微调互补滤波系数（α值），在动态响应与抗漂移之间取得平衡。
坐标系对齐：IMU芯片上的x/y/z轴不一定与机器人机体轴一致，安装时需查阅数据手册，必要时乘以固定旋转矩阵进行校正。
视觉抗干扰与遮挡处理：
在视觉跟随算法中，需设置合理的置信度阈值，过滤低置信度的目标检测结果，避免误将墙壁或背景识别为跟随目标。
视觉传感器的安装位置需避免被机器人自身结构遮挡，确保足够的视场角（FOV）。建议采用广角镜头（FOV≥120°）并适当抬高安装位置。
针对光照变化（如室内外切换），需进行白平衡自适应或采用对光照不敏感的特征描述子（如ORB、BRISK）。
目标遮挡处理：设置遮挡超时机制，当视觉连续N帧未检测到目标时，切换至IMU惯性航位推算模式，同时启动搜索策略（如原地旋转扫描）。
电源隔离与EMC防护：
BLDC电机启停时电流冲击极大（堵转可达额定3_{5倍），严禁与Arduino及传感器共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容（1000}4700μF）吸收反电动势。
IMU与视觉模块的信号线需使用屏蔽线，并远离动力线布线（间距≥5cm）。高频PWM信号可能干扰IMU的磁力计读数，需合理布局或使用光耦隔离。
PID调参与安全冗余：
跟随控制中的距离PID与转向PID参数需根据实际机械结构进行精细标定。参数过大会导致跟随抖动，过小则响应迟缓。建议先调内环（速度环），再调外环（位置环），并加入积分限幅（Anti-windup）防止积分饱和。
必须设置软件级限速与倾角超限保护，并配备物理急停按钮或防撞条作为最后的安全防线，确保在系统失控时能瞬间切断电机动力。
加入S曲线加减速算法，防止阶跃速度指令导致电机冲击或轮胎打滑。

1、OpenMV视觉 + MPU6050 简易跟随
场景：室内服务机器人跟随特定颜色或AprilTag目标，如酒店行李车、商场导览机器人。视觉模块（OpenMV）识别目标并发送其在画面中的位置，IMU补偿机器人自身姿态抖动。

核心逻辑：视觉提供目标相对画面中心的横向和纵向偏差，IMU提供横滚角用于姿态补偿，二者融合后生成差速驱动指令。采用互补滤波融合加速度计与陀螺仪数据获得稳定姿态角。

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

// ===== 硬件定义 =====
BLDCMotor motorL(7), motorR(7);
MPU6050 imu;
#define OpenMV_ADDR 0x12

// ===== 融合参数 =====
float pitch = 0, roll = 0;
float targetX = 160, targetY = 120;  // 目标在画面中的位置（320x240）
uint8_t validFlag = 0;
float baseSpeed = 120;  // 基础速度

// PID增益（横向：转向，纵向：速度）
const float kpX = 0.8, kpY = 0.3, kdAngle = 0.1;

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

    // -------- 1. 视觉目标数据获取（OpenMV通过I2C）--------
    Wire.requestFrom(OpenMV_ADDR, 12);
    if (Wire.available() >= 12) {
        validFlag = Wire.read();
        Wire.readBytes((uint8_t*)&targetX, 4);
        Wire.readBytes((uint8_t*)&targetY, 4);
    }

    // -------- 2. IMU姿态解算（互补滤波）--------
    int16_t ax = imu.getAccelerationX();
    int16_t ay = imu.getAccelerationY();
    int16_t az = imu.getAccelerationZ();
    int16_t gx = imu.getGyroX();
    int16_t gy = imu.getGyroY();

    // 加速度计算姿态角（静态稳定）
    float accPitch = atan2(ay, sqrt(ax*ax + az*az)) * 180 / PI;
    float accRoll = atan2(-ax, sqrt(ay*ay + az*az)) * 180 / PI;

    // 互补滤波融合（权重0.98 vs 0.02）
    pitch = 0.98 * (pitch + gx * 0.01) + 0.02 * accPitch;
    roll = 0.98 * (roll + gy * 0.01) + 0.02 * accRoll;

    // -------- 3. 融合跟随控制 --------
    float leftSpeed = baseSpeed, rightSpeed = baseSpeed;

    if (validFlag == 1) {
        // 横向误差 → 转向修正（目标偏离画面中心）
        float errorX = targetX - 160;  // 画面宽度320，中心160
        float lateralCorr = kpX * errorX;

        // 纵向误差 → 速度调整（目标在画面上下位置）
        float errorY = targetY - 120;  // 画面高度240，中心120
        float longitudinalCorr = kpY * errorY;

        // IMU姿态补偿：横滚角修正差速，防止侧翻趋势
        float rollComp = kdAngle * roll;

        // 差速驱动输出
        leftSpeed = baseSpeed + longitudinalCorr - lateralCorr + rollComp;
        rightSpeed = baseSpeed + longitudinalCorr + lateralCorr - rollComp;
    } else {
        // 目标丢失：缓慢减速停车
        leftSpeed *= 0.5;
        rightSpeed *= 0.5;
    }

    // 限幅并驱动BLDC
    leftSpeed = constrain(leftSpeed, -200, 200);
    rightSpeed = constrain(rightSpeed, -200, 200);
    motorL.move(leftSpeed);
    motorR.move(rightSpeed);

    delay(30);  // ~33Hz
}

2、视觉里程计 + IMU 紧耦合融合（VO+IMU）
场景：无外部定位的野外或室内环境，机器人需在视觉特征稀疏时仍保持稳定的跟随能力。视觉里程计提供低频位姿估计，IMU提供高频位姿更新，两者紧耦合。

核心逻辑：视觉里程计（VO）通过图像特征点跟踪估算自身运动（频率~10Hz），IMU以100Hz以上频率提供角速度和加速度积分。采用扩展卡尔曼滤波（EKF）融合两者，当视觉数据短暂丢失时，IMU+编码器仍能维持数秒的推算定位。

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

// ===== BLDC差速电机 =====
BLDCMotor motorL(7), motorR(7);

// ===== IMU状态 =====
struct IMUState {
    float yaw;          // 偏航角
    float bias;         // 陀螺仪零偏
    float lastUpdate;
};
IMUState imuState = {0, 0, 0};

// ===== 视觉里程计状态 =====
struct VOState {
    float x, y, yaw;    // 位置航向
    float vx, vy;       // 速度
    unsigned long lastUpdate;
};
VOState voState = {0, 0, 0, 0, 0, 0};

// ===== 融合后状态 =====
struct FusionState {
    float x, y, yaw;
    float vx, vy;
};
FusionState fusion = {0, 0, 0, 0, 0};

const float VO_UPDATE_RATE = 0.1;   // VO更新周期（秒）
const float MAX_VO_AGE = 0.5;       // VO最大有效时间（秒）

void loop() {
    float dt = 0.02;  // 50Hz控制循环
    static float voTimer = 0;

    // 1. IMU高频更新
    readIMU(dt);

    // 2. 低频读取视觉里程计
    voTimer += dt;
    if (voTimer >= VO_UPDATE_RATE) {
        readVisualOdometry();
        voTimer = 0;
    }

    // 3. 【核心】EKF紧耦合融合（简化版）
    unsigned long voAge = millis() - voState.lastUpdate;
    if (voAge < MAX_VO_AGE * 1000) {
        // 有有效VO数据：融合更新
        fusion.x += 0.7 * (voState.x - fusion.x);
        fusion.y += 0.7 * (voState.y - fusion.y);
        fusion.yaw += 0.6 * (voState.yaw - fusion.yaw);
    } else {
        // VO失效：仅用IMU积分+编码器里程计推演（航位推算）
        fusion.yaw += imuState.yaw * dt;
        // 从编码器获取速度并积分
        fusion.x += motorL.shaftVelocity() * cos(fusion.yaw) * dt;
        fusion.y += motorL.shaftVelocity() * sin(fusion.yaw) * dt;
    }

    // 4. 根据目标相对位姿生成跟随控制
    followTarget();

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

void readIMU(float dt) {
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    float gyroZ = (gz / 131.0) - imuState.bias;
    imuState.yaw += gyroZ * dt;
}

3、视觉SLAM + IMU 自主跟随与避障（上位机协同）
场景：复杂家庭或商场环境，需同时完成目标识别、路径规划、动态避障。采用上位机（树莓派/ESP32-S3）处理视觉SLAM和AI推理，Arduino负责BLDC底层驱动和执行。

核心逻辑：采用三层解耦架构——感知决策层运行MimiClaw AI框架或OpenCV进行目标检测与SLAM建图；主控通信层通过串口/I2C下发控制指令；执行驱动层利用SimpleFOC库驱动BLDC电机，响应速度快（<5ms）。IMU数据用于修正SLAM位姿漂移，超声波用于近场避障触发。

Arduino端（执行层）：

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

// BLDC差速电机
BLDCMotor motorL(7), motorR(7);
MPU6050 imu;
NewPing sonar(TRIG_PIN, ECHO_PIN, 200);

// 上位机指令（来自树莓派/ESP32-S3）
float cmdLinear = 0, cmdAngular = 0;
float obstacleDist = 0;
bool followActive = false;
unsigned long lastCmdTime = 0;

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

    // 1. 读取IMU（用于上位机位姿修正）
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    // 通过串口发送IMU数据给上位机
    Serial.print(ax); Serial.print(","); Serial.println(gz);

    // 2. 超声波避障监测（硬中断级响应）
    obstacleDist = sonar.ping_cm() / 100.0;
    if (obstacleDist < 0.3 && obstacleDist > 0.01) {
        // 近距障碍：强制减速停车
        motorL.move(0);
        motorR.move(0);
        return;  // 跳过本次跟随指令
    }

    // 3. 接收上位机指令（格式：linear,angular\\n）
    if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        int commaIdx = cmd.indexOf(',');
        if (commaIdx != -1) {
            cmdLinear = cmd.substring(0, commaIdx).toFloat();
            cmdAngular = cmd.substring(commaIdx + 1).toFloat();
            lastCmdTime = millis();
            followActive = true;
        }
    }

    // 超时判断：指令丢失则停车
    if (millis() - lastCmdTime > 200) {
        followActive = false;
    }

    // 4. 执行驱动（限幅+差速）
    float leftSpeed = cmdLinear - cmdAngular * 0.125;
    float rightSpeed = cmdLinear + cmdAngular * 0.125;
    motorL.move(constrain(leftSpeed, -1.0, 1.0));
    motorR.move(constrain(rightSpeed, -1.0, 1.0));

    delay(20);  // 50Hz控制频率
}

上位机端（树莓派/ESP32-S3，Python/伪代码）：

# 视觉SLAM定位 + 目标检测 + 路径规划
while True:
    # 1. SLAM定位（Orb-SLAM3或RTAB-Map）
    slam_pose = slam_system.getPose()
    
    # 2. 目标检测（YOLO/OpenCV）
    target_bbox = detect_person(frame)
    target_pose = estimate_3d_pose(target_bbox, slam_pose)
    
    # 3. 融合IMU数据修正位姿（从Arduino串口读取）
    imu_data = read_arduino_imu()
    slam_system.updateIMU(imu_data)
    
    # 4. 规划跟随路径（动态窗口法DWA）
    linear, angular = plan_follow_path(slam_map, slam_pose, target_pose)
    
    # 5. 发送指令给Arduino
    serial.write(f"{linear},{angular}\n")
    
    time.sleep(0.05)

要点解读
互补滤波是Arduino上融合视觉与IMU的“最低成本”方案：视觉数据有延迟但无漂移，IMU数据即时但有累积漂移。互补滤波（angle = 0.98*(angle + gyrodt) + 0.02accAngle）将两者加权融合，只需一行代码即可获得相对平滑的姿态角，非常适合Arduino这种资源受限平台。对于更高精度需求，可选用扩展卡尔曼滤波（EKF）。

视觉丢失时IMU航位推算是保证服务连续性的“安全网”：服务机器人不可避免会遇到目标被遮挡、光照突变等情况。当视觉帧丢失时，系统应无缝切换至基于IMU+编码器里程计的航位推算模式，维持数秒的跟随或安全减速。代码中的MAX_VO_AGE参数控制这个过渡时间，通常设为0.5-1秒。

采用“感知-决策-执行”三层解耦架构以保障BLDC的实时响应：视觉SLAM和AI推理（如YOLO目标检测）对算力要求极高，若与BLDC的FOC控制在同一核心上运行，会导致电机响应延迟甚至失控。推荐采用双核异构（如ESP32-S3的Core0处理AI推理，Core1管理电机控制）或主从架构（树莓派负责视觉，Arduino负责电机驱动），确保控制循环≥50Hz。

避障优先级高于跟随，须以“硬中断”级响应：服务机器人工作环境中常有突然出现的行人或家具。传感器触发（超声波距离<安全阈值）时，应立即无条件挂起路径跟踪任务，执行减速或避让，而不应等待决策层处理。这要求避障检测独立于主控制循环运行。

BLDC的FOC驱动是实现“柔顺跟随”的物理保障：相比传统有刷电机，FOC矢量控制的BLDC能在低速下输出大扭矩且零转速平稳运行，避免跟随过程中的“锯齿形振荡”。SimpleFOC库提供的velocity和torque控制模式可精确分配左右轮差速，使机器人启动、转向、急停都顺滑自然，这对于服务场景中的人机安全交互至关重要。

4、智能仓储AGV——视觉+IMU融合的动态跟随系统
适用场景：仓库拣货场景中，AGV自动跟随拣货员移动，视觉识别人员标识，IMU补偿底盘姿态变化，确保在货架遮挡、地面颠簸时仍稳定跟随。

核心逻辑：
视觉目标锁定：通过摄像头识别人员佩戴的色块/二维码，输出目标相对坐标；
IMU姿态补偿：实时解算机器人偏航角，修正视觉因底盘晃动产生的坐标偏差；
双PID闭环控制：视觉PID生成速度指令，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;
// 视觉模块（简化接口，实际需替换为OpenMV/树莓派通信）
int targetX = 0, targetY = 0;

// 控制参数
PID xPID(2.0, 0.5, 0.1); // X轴（横向）PID
PID yPID(1.5, 0.3, 0.05); // Y轴（纵向）PID
float yaw = 0; // IMU偏航角

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();
  // PID初始化
  xPID.SetMode(AUTOMATIC);
  yPID.SetMode(AUTOMATIC);
}

void loop() {
  // 1. 获取视觉目标坐标（实际需从串口/I2C读取）
  // 此处为模拟，实际需替换为摄像头数据解析
  targetX = constrain(targetX, 0, 320); 
  targetY = constrain(targetY, 0, 240);
  
  // 2. IMU姿态解算（互补滤波）
  float accAngle = atan2(imu.getAccelerationX(), imu.getAccelerationY()) * RAD_TO_DEG;
  float gyroRate = imu.getRotationZ();
  yaw = 0.98 * (yaw + gyroRate * 0.01) + 0.02 * accAngle;
  
  // 3. 计算误差并PID控制
  float xError = targetX - 160; // 目标中心为屏幕中点
  float yError = targetY - 120;
  xPID.Compute(xError);
  yPID.Compute(yError);
  
  // 4. 融合IMU修正转向（避免视觉抖动）
  if (abs(yaw) > 5) { // 偏航角过大时，修正转向速度
    motorL.move(xPID.GetOutput() - yaw * 0.1);
    motorR.move(xPID.GetOutput() + yaw * 0.1);
  } else {
    motorL.move(xPID.GetOutput());
    motorR.move(xPID.GetOutput());
  }
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

5、园区巡逻机器人——视觉+IMU+UWB多模态融合跟随
适用场景：园区安保机器人跟随安保人员巡逻，视觉识别人员轮廓，UWB提供精准距离，IMU在人员被树木遮挡时维持短时轨迹预测，确保跟随不中断。

核心逻辑：
多传感器权重分配：视觉正常时，以视觉坐标为主；视觉丢失时，UWB提供距离，IMU推测人员运动方向；
动态避障融合：视觉识别障碍物，IMU判断机器人自身姿态，调整避障轨迹；
丢失保护机制：目标丢失超过3秒，启动IMU航迹推测，低速搜索目标。

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

// 硬件定义
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
MPU6050 imu;
// 模拟UWB/视觉数据接口
float uwbDist = 0, visualX = 0, visualY = 0;
bool targetLost = false;

// 控制参数
float Kp_dist = 0.8, Kp_angle = 1.2;
float yaw = 0, lastYaw = 0;

void setup() {
  Serial.begin(115200);
  // 初始化BLDC
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  // 初始化IMU
  Wire.begin();
  imu.initialize();
}

void loop() {
  // 1. 传感器数据获取（模拟，实际需替换为UWB/视觉通信）
  uwbDist = constrain(uwbDist, 0, 300);
  visualX = constrain(visualX, 0, 320);
  visualY = constrain(visualY, 0, 240);
  targetLost = (uwbDist == 0 && visualX == 0);
  
  // 2. IMU姿态更新
  float accAngle = atan2(imu.getAccelerationX(), imu.getAccelerationY()) * RAD_TO_DEG;
  float gyroRate = imu.getRotationZ();
  yaw = 0.98 * (yaw + gyroRate * 0.01) + 0.02 * accAngle;
  
  // 3. 多模态融合控制
  if (!targetLost) {
    // 视觉+UWB融合：视觉控制角度，UWB控制距离
    float angleError = atan2(visualY - 120, visualX - 160) * 180 / PI - yaw;
    float distError = 150 - uwbDist; // 目标距离150cm
    float speed = distError * Kp_dist;
    float turn = angleError * Kp_angle;
    motorL.move(speed - turn);
    motorR.move(speed + turn);
  } else {
    // 目标丢失：IMU航迹推测，低速搜索
    float yawRate = yaw - lastYaw;
    lastYaw = yaw;
    motorL.move(30 - yawRate * 0.5); // 低速前进+缓慢转向搜索
    motorR.move(30 + yawRate * 0.5);
    // 3秒后仍未找回目标，停机（实际可扩展蜂鸣器报警）
    static unsigned long lostTime = 0;
    if (millis() - lostTime > 3000) {
      motorL.move(0);
      motorR.move(0);
    }
  }
  motorL.loopFOC();
  motorR.loopFOC();
  delay(30);
}

6、超市购物车——视觉+IMU的人体跟随与避障系统
适用场景：超市购物车自动跟随顾客，视觉识别人体轮廓，IMU补偿顾客转身、急停时的姿态变化，同时通过超声波辅助避障，避免碰撞货架。

核心逻辑：
人体目标识别：视觉模块识别人体中心坐标，输出跟随偏差；
姿态自适应调整：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);
MPU6050 imu;
NewPing sonar(12,13,200); // 前方超声波
int humanX = 0, humanY = 0;
bool obstacle = false;

// 控制参数
float Kp = 1.2, Kd = 0.3;
float targetAngle = 0;

void setup() {
  Serial.begin(115200);
  // 初始化BLDC
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  // 初始化IMU/超声波
  Wire.begin();
  imu.initialize();
  pinMode(12, OUTPUT);
  pinMode(13, INPUT);
}

void loop() {
  // 1. 视觉人体识别（模拟）
  humanX = constrain(humanX, 0, 320);
  humanY = constrain(humanY, 0, 240);
  
  // 2. 超声波避障检测
  float dist = sonar.ping_cm();
  obstacle = (dist < 50);
  
  // 3. IMU姿态补偿
  float accAngle = atan2(imu.getAccelerationX(), imu.getAccelerationY()) * RAD_TO_DEG;
  float gyroRate = imu.getRotationZ();
  targetAngle = 0.98 * (targetAngle + gyroRate * 0.01) + 0.02 * accAngle;
  
  // 4. 融合控制
  if (obstacle) {
    // 避障：减速+转向
    motorL.move(-20);
    motorR.move(40);
  } else {
    // 跟随人体：视觉控制方向，IMU修正转向幅度
    float angleError = (humanX - 160) * 0.1 - targetAngle;
    float speed = 60 + (humanY - 120) * 0.2; // 距离越近速度越慢
    float turn = angleError * Kp + (angleError - targetAngle) * Kd;
    motorL.move(speed - turn);
    motorR.move(speed + turn);
  }
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

要点解读

1. 视觉与IMU的“互补融合逻辑”：解决单传感器局限
   视觉与IMU的融合并非简单叠加，而是基于场景互补的精准分工，核心解决单传感器的固有缺陷：
   视觉核心作用：提供目标的绝对位置与身份识别，输出跟随偏差，但易受光照变化、目标遮挡、底盘晃动影响，存在数据延迟与抖动；
   IMU核心作用：提供高频姿态数据，实时补偿底盘因加速、刹车、地面颠簸产生的倾斜与偏航，修正视觉因姿态变化导致的坐标偏差，同时在视觉丢失时维持短时轨迹预测；
   融合策略：正常场景以视觉为主导，输出目标坐标；视觉丢失时，IMU通过航迹推测维持跟随，避免系统失控，确保跟随的连续性与稳定性。
2. BLDC电机的“动态响应与平滑控制”：跟随流畅的核心支撑
   目标跟随对电机的动态响应与运动平滑性要求极高，核心要点包括：
   毫秒级响应特性：BLDC采用FOC磁场定向控制，转矩响应时间≤10ms，可快速跟踪视觉/IMU生成的速度指令，避免因响应滞后导致的目标丢失或轨迹抖动；
   双闭环控制架构：外环为视觉/IMU生成的目标速度，内环为编码器速度闭环，通过PID算法抑制负载波动，确保在加速、减速、转向时速度稳定，避免因电机波动导致跟随顿挫；
   S型加减速规划：在启停、转向时采用S型速度曲线，限制加加速度，减少机械冲击，提升运动平滑性，尤其适用于狭窄空间内的频繁转向与避障。
3. 多传感器的“时间同步与数据校验”：融合可靠性的基础
   不同传感器的采样频率、数据延迟差异会导致融合误差，核心要点包括：
   时间戳对齐：给每组传感器数据打上精确时间戳，丢弃过期数据，避免将不同时刻的视觉坐标与IMU姿态直接融合，解决数据时间错位问题；
   数据滤波校验：对视觉数据采用中值滤波剔除噪点，对IMU数据采用互补滤波抑制陀螺仪漂移与加速度计噪声，同时设置数据有效性阈值，异常数据直接丢弃；
   通信非阻塞设计：采用非阻塞编程，避免delay()函数阻塞主循环，确保传感器读取与电机控制的实时性，控制频率≥50Hz，保证跟随决策的及时性。
4. 目标丢失的“容错与保护机制”：极端场景的生存底线
   跟随过程中目标必然面临遮挡、丢失等极端情况，核心要点包括：
   短时预测补偿：视觉丢失后，IMU通过航迹推测目标运动方向，维持低速跟随，避免立即停机，给予目标重新进入视野的时间；
   分级安全保护：设置多级丢失保护——1秒内低速搜索，1-3秒扩大搜索范围，3秒后自动停机，同时触发声光报警，避免机器人失控；
   硬件急停冗余：设计独立的硬件急停回路，优先级高于软件逻辑，当传感器全部失效或机器人失控时，物理切断电机电源，确保人身与设备安全。
5. 工程部署的“算力与电磁兼容设计”：落地应用的关键保障
   Arduino平台的算力与电磁环境直接影响系统稳定性，核心要点包括：
   算力匹配优化：标准Arduino Uno算力不足，推荐采用ESP32、Teensy 4.0等高算力板卡，满足视觉数据解析、IMU融合、PID控制的实时性需求；采用非阻塞编程，避免主循环卡顿；
   电源隔离与EMC防护：BLDC电机启动电流大、高频噪声强，必须采用独立DC-DC隔离模块为控制电路供电，电机电源端并联大容量低ESR电容吸收反电动势，信号线与动力线分层布线，间距≥5cm，避免电磁干扰导致传感器数据跳变或主控复位；
   机械安装与校准：IMU需刚性固定在底盘重心，加装硅胶减震垫隔离电机振动，上电后执行陀螺仪零偏校准与加速度计水平面校准，确保姿态解算精度；电机编码器需与车轮刚性连接，避免打滑导致的里程计误差。

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