基于Arduino与BLDC（无刷直流电机）构建的“简易360°扫描建图机器人”，是一套将低成本机械式雷达扫描与底层高精度运动控制相结合的轻量级SLAM（同步定位与建图）架构。该方案利用超声波传感器配合舵机实现环境点云采集，并依赖BLDC的高动态响应能力来精准执行差速运动指令，从而在移动中完成栅格地图的构建。

主要特点

1. 机械式极坐标扫描与笛卡尔坐标映射
   系统采用单路超声波传感器（如HC-SR04）搭载在舵机（如SG90）上，通过“步进-停顿-测距”的时序逻辑，实现0°至180°（或360°）的扇形或全向扫描。在Arduino平台上，系统实时采集“角度-距离”的极坐标数据，并利用三角函数将其转换为（X, Y）笛卡尔坐标，进而填充到二维占据栅格地图（Occupancy Grid Map）中，实现低成本的环境感知。
2. 概率化占据栅格与多帧数据融合
   为了克服超声波传感器易受环境噪声干扰的缺陷，系统通常不采用简单的二值判断（有/无障碍），而是引入概率化占据栅格。通过贝叶斯更新或简单的多帧投票机制，只有当某个栅格在连续多次扫描中都被确认为“占用”时，才会被标记为真实障碍物。这种数据融合策略有效剔除了偶发的误检，提升了地图的鲁棒性。
3. BLDC高动态响应与平滑差速执行
   在建图过程中，机器人需要根据未探索区域实时调整航向。BLDC电机配合FOC（磁场定向控制）驱动器，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪路径规划算法输出的高频速度指令。在频繁启停和转向的建图过程中，BLDC能保持运动平滑，避免因机械顿挫导致传感器数据抖动，从而保证建图质量。
4. 停走结合（Stop-and-Go）的扫描策略
   为了消除运动模糊对地图精度的影响，系统通常采用“停走结合”的策略：机器人先移动一段固定距离，然后完全刹停，执行一次完整的360°扫描，更新地图后再进行下一步移动。这种策略虽然牺牲了部分效率，但极大地简化了里程计积分带来的累积误差，是入门级SLAM系统保证地图一致性的关键手段。

应用场景

1. 室内服务与清洁机器人原型验证
   在家庭或办公环境中，机器人需要构建房间的二维轮廓以规划清扫路径。该系统能够以极低的成本验证栅格地图构建、路径规划（如洪水填充算法）等核心逻辑，为后续升级为激光雷达方案提供算法基础。
2. 仓储物流AGV的初级导航
   在结构相对简单的仓库中，机器人可利用超声波建图识别货架通道和墙壁，实现基础的自主巡航与避障。BLDC的高扭矩特性使其能承载一定重量的货物，在平坦地面上稳定运行。
3. 安防巡逻与环境监测
   在园区或特定区域内，机器人可自主巡逻并生成环境地图。当发现新出现的障碍物（如临时堆放的货物）时，系统能实时更新地图并重新规划路径，确保巡逻任务不中断。
4. 嵌入式SLAM算法教学与科研
   该系统是学习SLAM原理、传感器数据融合以及运动控制算法的绝佳硬件平台。通过调整扫描步长、滤波算法和BLDC控制参数，能够直观地观察机器人从环境感知到地图构建的完整过程。

注意事项

1. 超声波传感器的物理局限与抗干扰
   盲区与量程： HC-SR04存在约2cm的近场盲区和约4m的最大量程，超出范围的读数应予以丢弃。
   波束角效应： 超声波具有约15°的宽波束角，在探测墙角或细小物体时会产生“开花”伪影（Blooming Artefacts），导致地图中的障碍物比实际更大。
   抗干扰策略： 必须在软件中加入中值滤波或多次采样均值滤波，剔除因声波串扰或环境噪声产生的野值。
2. 舵机扫描时序与运动模糊
   停顿时间： 每次舵机转动到位后，必须预留足够的稳定时间（通常50-100ms），待机械振动完全消失后再触发超声波测距，否则回波信号会严重失真。
   扫描间隔： 两次超声波触发脉冲的间隔必须大于60ms，避免前一次的回波干扰后一次的测量。
3. 里程计漂移与地图扭曲
   累积误差： 仅依赖轮式里程计（编码器积分）进行定位，会随着移动距离的增加产生严重的累积误差，导致地图出现重影或扭曲。
   闭环校正： 建议引入IMU（如MPU-6050）辅助航向修正，或在算法中加入简单的闭环检测（Loop Closure）逻辑，当机器人识别出曾到过的地点时，校正整体轨迹。
4. 动态障碍物的误判
   数据过滤： 在建图过程中，必须区分静态障碍物（如墙壁）和动态障碍物（如行人）。如果将行人误录入地图，会导致地图迅速被“污染”。
   时序分析： 可通过对比连续几帧的传感器数据，识别并过滤掉移动的物体，仅将长期存在的障碍物更新到地图中。
5. 计算资源与内存管理
   内存限制： 栅格地图的存储和更新非常消耗RAM。在Arduino Uno等资源受限的平台上，必须降低地图分辨率或采用稀疏数据结构（如四叉树）来节省存储空间。
   实时性： 复杂的概率更新算法可能会阻塞主循环，影响BLDC电机的控制周期。建议将地图更新逻辑放在后台任务中，确保电机控制的实时性。

1、伺服云台360°扫描 + 极坐标点云采集
此案例实现最基础的扫描建图功能。伺服电机带动超声波传感器从0°旋转到360°，每个角度触发测距并记录“角度-距离”数据对，形成极坐标点云。数据通过串口输出，可在Processing等上位机中可视化。

#include <Servo.h>
#include <NewPing.h>

// 超声波与伺服引脚
#define TRIG_PIN 9
#define ECHO_PIN 10
#define SERVO_PIN 6
NewPing sonar(TRIG_PIN, ECHO_PIN, 200);
Servo scanServo;

// 扫描参数
const int ANGLE_MIN = 0;
const int ANGLE_MAX = 360;
const int ANGLE_STEP = 5;      // 每5°采样一次
const int SCAN_INTERVAL = 80;  // 采样间隔(ms)

// 点云存储（极坐标）
struct ScanPoint {
    int angle;
    float distance;  // cm
};
#define MAX_POINTS 72  // 360/5
ScanPoint pointCloud[MAX_POINTS];
int pointCount = 0;

void setup() {
    Serial.begin(115200);
    scanServo.attach(SERVO_PIN);
    scanServo.write(ANGLE_MIN);
    delay(500);
}

void loop() {
    pointCount = 0;

    // 正向扫描 0° → 360°
    for (int angle = ANGLE_MIN; angle <= ANGLE_MAX; angle += ANGLE_STEP) {
        scanServo.write(angle);
        delay(SCAN_INTERVAL);  // 等待舵机到位

        int dist = sonar.ping_cm();
        if (dist > 0 && dist < 200) {
            pointCloud[pointCount].angle = angle;
            pointCloud[pointCount].distance = dist;
            pointCount++;
        }
    }

    // 输出点云数据（CSV格式）
    Serial.println("=== SCAN START ===");
    for (int i = 0; i < pointCount; i++) {
        Serial.print(pointCloud[i].angle);
        Serial.print(",");
        Serial.println(pointCloud[i].distance);
    }
    Serial.println("=== SCAN END ===");

    delay(1000);  // 每轮扫描间隔
}

核心逻辑：极坐标点云是建图的基础数据形式。舵机每转动一个步进角，超声波触发一次测距，得到该角度的距离值。pointCloud数组按顺序存储，后续可通过坐标变换转为直角坐标，用于构建栅格地图或直接可视化。

2、增量栅格建图 + 数据滤波
此案例在案例一基础上引入栅格地图增量更新和中值滤波。将极坐标点云转换为直角坐标，标记到栅格地图中。对超声波数据进行中值滤波消除虚假回波，并加入置信度机制减少噪声影响。

#include <Servo.h>
#include <NewPing.h>

// 硬件定义（同案例一）
Servo scanServo;
NewPing sonar(9, 10, 200);

// ===== 栅格地图参数 =====
#define GRID_SIZE 40          // 40x40栅格
#define CELL_SIZE 10          // 每格10cm
#define MAP_ORIGIN_X 20       // 机器人位置在栅格中心
#define MAP_ORIGIN_Y 20

// 栅格状态：0=未知, 1=空闲, 2=障碍
byte gridMap[GRID_SIZE][GRID_SIZE];

// ===== 中值滤波缓存 =====
#define FILTER_WINDOW 5
int distHistory[FILTER_WINDOW];
int historyIdx = 0;

// 读取并滤波
float readFilteredDistance() {
    int raw = sonar.ping_cm();
    if (raw <= 0 || raw > 200) return -1;  // 无效值

    distHistory[historyIdx] = raw;
    historyIdx = (historyIdx + 1) % FILTER_WINDOW;

    // 复制并排序取中值
    int sorted[FILTER_WINDOW];
    memcpy(sorted, distHistory, sizeof(sorted));
    for (int i = 0; i < FILTER_WINDOW - 1; i++) {
        for (int j = i + 1; j < FILTER_WINDOW; j++) {
            if (sorted[j] < sorted[i]) {
                int tmp = sorted[i]; sorted[i] = sorted[j]; sorted[j] = tmp;
            }
        }
    }
    return sorted[FILTER_WINDOW / 2];
}

// 极坐标转栅格坐标并更新地图
void updateGridMap(int angleDeg, float distCm) {
    float rad = angleDeg * PI / 180.0;
    float x = distCm * cos(rad) / CELL_SIZE;  // 转换为栅格单位
    float y = distCm * sin(rad) / CELL_SIZE;

    int gx = MAP_ORIGIN_X + (int)round(x);
    int gy = MAP_ORIGIN_Y + (int)round(y);

    // 标记障碍点
    if (gx >= 0 && gx < GRID_SIZE && gy >= 0 && gy < GRID_SIZE) {
        gridMap[gx][gy] = 2;
    }

    // 标记路径上的空格（从原点到障碍点之间）
    int steps = (int)(distCm / CELL_SIZE);
    for (int s = 1; s < steps; s++) {
        float px = s * cos(rad) / CELL_SIZE;
        float py = s * sin(rad) / CELL_SIZE;
        int mx = MAP_ORIGIN_X + (int)round(px);
        int my = MAP_ORIGIN_Y + (int)round(py);
        if (mx >= 0 && mx < GRID_SIZE && my >= 0 && my < GRID_SIZE) {
            if (gridMap[mx][my] != 2) gridMap[mx][my] = 1;
        }
    }
}

void setup() {
    Serial.begin(115200);
    scanServo.attach(6);
    memset(gridMap, 0, sizeof(gridMap));  // 初始化为未知
}

void loop() {
    for (int angle = 0; angle < 360; angle += 5) {
        scanServo.write(angle);
        delay(80);

        float dist = readFilteredDistance();
        if (dist > 0) {
            updateGridMap(angle, dist);
        }
    }
    delay(500);
}

核心逻辑：中值滤波通过排序取中间值，有效消除超声波偶发的虚假回波（如多径反射）。栅格地图的增量更新只标记障碍点和路径上的空格，未扫描区域保持“未知”状态，便于后续规划算法区分。

3、BLDC差速底盘扫描运动 + FOC闭环执行
此案例将扫描建图与BLDC差速底盘闭环运动控制结合。机器人一边用超声波扫描环境，一边通过SimpleFOC驱动底盘进行“旋转扫描”和“平移扫描”的复合运动，同时利用编码器反馈实现精确定位。

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

// ===== BLDC 差速底盘 =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,7);
Encoder encL(2,3), encR(4,5);

// 扫描云台
Servo scanServo;
NewPing sonar(12, 13, 200);

// 编码器中断函数
void doA_L() { encL.handleA(); }
void doB_L() { encL.handleB(); }
void doA_R() { encR.handleA(); }
void doB_R() { encR.handleB(); }

// 扫描与运动参数
const float SCAN_SPEED = 0.3;    // 扫描时底盘旋转速度(rad/s)
const int SCAN_INTERVAL = 80;    // 测距间隔(ms)

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

    // 编码器初始化
    encL.init(); encL.enableInterrupts(doA_L, doB_L);
    encR.init(); encR.enableInterrupts(doA_R, doB_R);

    // BLDC初始化
    drvL.init(); drvR.init();
    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();

    scanServo.attach(6);
}

void loop() {
    // ===== 阶段1：原地旋转扫描 =====
    // 左右轮反向旋转，机器人原地转动
    motorL.move(-SCAN_SPEED);
    motorR.move(SCAN_SPEED);

    for (int i = 0; i < 72; i++) {  // 旋转一圈，每5°采样
        motorL.loopFOC(); motorR.loopFOC();
        motorL.move(-SCAN_SPEED); motorR.move(SCAN_SPEED);

        int angle = i * 5;
        scanServo.write(angle);
        delay(SCAN_INTERVAL);

        int dist = sonar.ping_cm();
        if (dist > 0 && dist < 200) {
            Serial.print("ROT_SCAN,");
            Serial.print(angle);
            Serial.print(",");
            Serial.println(dist);
        }
    }

    // ===== 阶段2：停止扫描，前进一段距离 =====
    motorL.move(0); motorR.move(0);
    motorL.loopFOC(); motorR.loopFOC();
    delay(500);

    // 前进扫描（边走边扫）
    motorL.move(0.5); motorR.move(0.5);
    for (int i = 0; i < 20; i++) {
        motorL.loopFOC(); motorR.loopFOC();
        motorL.move(0.5); motorR.move(0.5);

        int dist = sonar.ping_cm();
        if (dist > 0) {
            Serial.print("FWD_SCAN,");
            Serial.println(dist);
        }
        delay(50);
    }

    motorL.move(0); motorR.move(0);
    delay(2000);  // 等待下一轮
}

核心逻辑：BLDC配合编码器实现底盘运动的精确闭环控制。原地旋转时，左右轮以相同速度反向旋转，机器人绕自身中心转动，超声波传感器配合云台扫描可获得全向环境数据。FOC算法确保低速旋转时的平稳性和位置精度。

要点解读

1. 伺服云台是360°扫描的物理基础，步进角度需权衡精度与效率

伺服电机带动超声波传感器旋转，每转动一个固定角度（通常5°_{10°）触发一次测距。步进角度越小，点云密度越高，但完整扫描一圈的时间越长（5°步进×80ms间隔≈5.8秒/圈）。在动态环境中，过慢的扫描会导致地图滞后，建议步进角度控制在5°}10°。

2. 超声波数据的滤波处理是建图质量的关键

超声波传感器存在多径反射、软材质误测等问题，原始数据不能直接用于建图。中值滤波（案例二）能有效消除偶发虚假回波。更高级的做法是结合多次测量的一致性检验——同一角度连续测量多次，只有稳定出现的目标才被标记为障碍。

3. 极坐标到直角坐标的转换需处理传感器安装偏差

超声波传感器安装在云台上有一定的高度和偏移量，理想情况下假设传感器与机器人中心重合。实际中需标定传感器的安装位置（相对机器人中心的偏移），在建图时进行坐标补偿，否则近距离扫描会出现系统性偏差。

4. BLDC差速底盘为扫描运动提供精确的运动控制

扫描建图不仅是传感器的事，机器人自身的运动控制同样重要。BLDC配合编码器闭环，可以实现“原地精确旋转90°后扫描”、“直线前进1米后扫描”等复合动作。FOC算法确保低速旋转时的平稳性，避免因速度波动导致扫描角度与预期不符。

5. Arduino算力限制决定了建图的分层架构

完整的SLAM（同时定位与建图）需要维护机器人位姿和地图的联合概率分布，计算量远超Arduino能力。工程上采用分层策略：Arduino负责底层传感器扫描和BLDC运动控制，通过串口或WiFi将点云数据发送至上位机（树莓派/PC）完成建图和定位。Arduino端的核心任务是“精准采集数据”，而非“复杂计算”。

4、基础360°实时扫描程序（核心：角度定位+距离采集+串口绘图）
该程序是建图的基础，实现电机带动超声波模块360°旋转，实时采集角度与距离数据，通过串口发送，配合上位机绘制简易轮廓图，适用于环境初步探测。

代码逻辑
初始化电机驱动引脚（方向+脉冲）和超声波模块引脚；
设定旋转步长（如每次旋转3°，360°需120步），通过脉冲数换算旋转角度；
旋转过程中触发超声波测距，存储角度与距离数据；
通过串口输出数据，供上位机实时绘图。

#include <TimerOne.h> // 用于精准控制电机旋转步长（可选，也可用delay替代）

// 电机驱动引脚定义（TB6600驱动板接口）
#define MOTOR_DIR 3   // 方向控制引脚
#define MOTOR_PUL 4   // 脉冲控制引脚
#define ENC_A 2       // 编码器A相引脚（接外部中断0）
#define ENC_B 5       // 编码器B相引脚（用于判断旋转方向，简化版可省略）

// 超声波模块引脚定义（HC-SR04）
#define TRIG_PIN 8
#define ECHO_PIN 9

// 核心参数设置
const int STEP_PER_ROTATION = 120; // 360°分120步，每步3°（根据电机细分设置调整）
const int MOTOR_SPEED = 100;       // 电机转速（脉冲频率，数值越大转速越快，需匹配驱动板）
float currentAngle = 0.0;          // 当前旋转角度（°）
int stepCount = 0;                 // 脉冲计数

void setup() {
  Serial.begin(115200);           // 串口初始化，用于数据传输到上位机
  
  // 配置电机引脚为输出
  pinMode(MOTOR_DIR, OUTPUT);
  pinMode(MOTOR_PUL, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // 设置编码器外部中断（检测旋转步数）
  attachInterrupt(0, countStep, RISING); // 外部中断0对应Arduino引脚2，触发上升沿中断
  
  // 初始化定时器，控制电机脉冲频率（精准调速）
  Timer1.initialize(1000000 / MOTOR_SPEED); // 计算脉冲周期
  Timer1.attachInterrupt(sendPulse);
  
  // 初始方向设置：顺时针旋转
  digitalWrite(MOTOR_DIR, HIGH);
}

// 编码器中断函数：每接收一个脉冲，更新步数和角度
void countStep() {
  stepCount++;
  if (stepCount >= STEP_PER_ROTATION) {
    stepCount = 0; // 360°循环归零
  }
  currentAngle = stepCount * (360.0 / STEP_PER_ROTATION); // 换算角度
}

// 定时器中断函数：发送电机脉冲，驱动电机旋转
void sendPulse() {
  digitalWrite(MOTOR_PUL, HIGH);
  delayMicroseconds(10); // 脉冲宽度，需匹配驱动板要求
  digitalWrite(MOTOR_PUL, LOW);
}

// 超声波测距函数：返回距离值（单位：cm）
float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * 0.0343) / 2; // 声速343m/s，换算为cm
  
  // 过滤无效数据（超出HC-SR04有效测距范围2-400cm）
  if (distance < 2 || distance > 400) {
    return -1.0; // -1表示无效距离（无障碍物或超出范围）
  }
  return distance;
}

void loop() {
  // 触发一次超声波测距
  float distance = getDistance();
  
  // 串口输出：角度和距离（格式：角度,距离，方便上位机解析）
  Serial.print(currentAngle);
  Serial.print(",");
  Serial.println(distance);
  
  // 等待电机走完一个步长（确保角度与测距同步）
  delay(100);
}

上位机配合说明
将Arduino与PC连接后，打开串口监视器（波特率115200），或使用Processing、Python编写上位机程序，读取串口数据，以极坐标（角度为横轴，距离为纵轴）绘制扫描图，可直观看到360°范围内障碍物的大致位置。

2、局部环境地图构建程序（核心：数据存储+二维坐标转换+本地建图）
该程序在基础扫描基础上，增加数据本地存储功能，并将极坐标（角度+距离）转换为直角坐标（X,Y），构建局部二维地图（以机器人中心为原点），适用于小范围静态环境建图，如房间、货架布局绘制。

代码逻辑
扩展存储：使用Arduino的数组存储单圈扫描数据（或外接EEPROM，需长期存储时推荐）；
坐标转换：将超声波测得的距离（半径r）和角度（θ）转换为直角坐标（X=rsinθ，Y=rcosθ）；
地图输出：通过串口输出坐标数据，或存储到SD卡，供后续导航、避障调用。

#include <TimerOne.h>
#include <SD.h> // 如需存储地图到SD卡，需添加SD卡库

// 电机与超声波引脚定义（同案例1，可复用）
#define MOTOR_DIR 3
#define MOTOR_PUL 4
#define ENC_A 2
#define TRIG_PIN 8
#define ECHO_PIN 9

// SD卡引脚（可选，若不使用可注释相关代码）
#define SD_CS_PIN 10

// 核心参数设置
const int STEP_PER_ROTATION = 120;
const int MOTOR_SPEED = 100;
const int MAX_SCAN_POINTS = 360; // 360个采样点（每度一个点，数据更精细）
float currentAngle = 0.0;
int stepCount = 0;

// 存储扫描数据：角度和距离
float scanAngles[MAX_SCAN_POINTS];
float scanDistances[MAX_SCAN_POINTS];
int dataIndex = 0; // 当前数据索引

// 地图参数（直角坐标系，单位：cm，以机器人中心为原点）
const float MAP_SCALE = 1.0; // 地图比例尺，1cm对应1个坐标单位

void setup() {
  Serial.begin(115200);
  
  pinMode(MOTOR_DIR, OUTPUT);
  pinMode(MOTOR_PUL, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(SD_CS_PIN, OUTPUT); // SD卡片选引脚
  
  attachInterrupt(0, countStep, RISING);
  Timer1.initialize(1000000 / MOTOR_SPEED);
  Timer1.attachInterrupt(sendPulse);
  
  digitalWrite(MOTOR_DIR, HIGH);
  
  // 初始化SD卡（若使用SD卡存储）
  // if (!SD.begin(SD_CS_PIN)) {
  //   Serial.println("SD卡初始化失败！");
  // } else {
  //   Serial.println("SD卡初始化成功！");
  // }
  
  // 初始化扫描数据数组
  for (int i = 0; i < MAX_SCAN_POINTS; i++) {
    scanAngles[i] = 0.0;
    scanDistances[i] = -1.0;
  }
}

void countStep() {
  stepCount++;
  if (stepCount >= MAX_SCAN_POINTS) {
    stepCount = 0;
  }
  currentAngle = stepCount; // 每步对应1°，数据更精细
  
  // 更新扫描数组（仅当触发一次扫描时）
  if (dataIndex < MAX_SCAN_POINTS) {
    scanAngles[dataIndex] = currentAngle;
    dataIndex++;
  } else {
    // 完成一圈扫描，触发地图构建
    buildMap();
    dataIndex = 0; // 重置索引，准备下一圈扫描
  }
}

void sendPulse() {
  digitalWrite(MOTOR_PUL, HIGH);
  delayMicroseconds(10);
  digitalWrite(MOTOR_PUL, LOW);
}

float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * 0.0343) / 2;
  
  if (distance < 2 || distance > 400) {
    return -1.0;
  }
  return distance;
}

// 核心函数：构建二维地图（极坐标转直角坐标）
void buildMap() {
  Serial.println("===== 360°扫描地图构建完成 =====");
  Serial.println("角度(°),距离(cm),X坐标(cm),Y坐标(cm)");
  
  // 遍历扫描数据，转换为直角坐标
  for (int i = 0; i < MAX_SCAN_POINTS; i++) {
    float angleRad = scanAngles[i] * (PI / 180.0); // 角度转弧度
    float distance = scanDistances[i];
    
    // 过滤无效距离
    if (distance < 0) {
      Serial.print(scanAngles[i]);
      Serial.print(",");
      Serial.print("无效,");
      Serial.print("无效,");
      Serial.println("无效");
      continue;
    }
    
    // 极坐标转直角坐标：X = r*sinθ，Y = r*cosθ（角度0°对应X正方向，可根据需求调整坐标系）
    float X = distance * sin(angleRad) * MAP_SCALE;
    float Y = distance * cos(angleRad) * MAP_SCALE;
    
    // 串口输出地图数据
    Serial.print(scanAngles[i]);
    Serial.print(",");
    Serial.print(distance);
    Serial.print(",");
    Serial.print(X, 2); // 保留两位小数
    Serial.print(",");
    Serial.println(Y, 2);
    
    // 若使用SD卡，可写入数据（取消注释以下代码）
    // File mapFile = SD.open("map.txt", FILE_WRITE);
    // if (mapFile) {
    //   mapFile.println(String(scanAngles[i]) + "," + String(distance) + "," + String(X,2) + "," + String(Y,2));
    //   mapFile.close();
    // }
  }
  
  Serial.println("===== 地图构建结束 =====");
}

void loop() {
  // 每走一个步长，触发一次测距
  scanDistances[dataIndex] = getDistance();
  delay(10); // 等待测距稳定
}

地图应用场景
输出的直角坐标数据可直接用于：
PC端可视化：用Python的Matplotlib绘制二维散点图，直观展示障碍物轮廓；
机器人本地导航：结合坐标数据计算障碍物与机器人的距离和方向，为后续避障决策提供基础。

6、自主避障联动程序（核心：实时扫描+路径决策+避障执行）
该程序在扫描基础上增加实时决策逻辑，通过扫描数据判断前方障碍物，控制BLDC电机停止旋转并调整机器人运动方向，实现避障，适用于机器人自主探索未知环境。

代码逻辑
实时扫描：边旋转边采集距离数据，同时设定“安全阈值”（如30cm）；
障碍物检测：当某角度方向的距离小于安全阈值时，判定存在障碍物；
路径决策：选择“无障碍物且距离最远”的角度作为运动方向；
执行避障：控制BLDC电机停止旋转，驱动机器人运动机构（如轮式底盘）转向目标方向，同时继续扫描监测环境变化。
注：为简化程序，本案例假设机器人运动机构由两个轮式电机控制，通过PWM控制左右轮转速实现转向；若使用其他运动结构，可调整控制逻辑。

#include <TimerOne.h>

// 电机与超声波引脚定义
#define MOTOR_DIR 3       // 扫描电机方向引脚
#define MOTOR_PUL 4       // 扫描电机脉冲引脚
#define ENC_A 2           // 扫描电机编码器A相
#define TRIG_PIN 8
#define ECHO_PIN 9

// 运动电机引脚定义（轮式底盘左右轮，简化版为两个PWM控制电机）
#define LEFT_MOTOR_PWM 5
#define RIGHT_MOTOR_PWM 6
#define LEFT_MOTOR_DIR 7
#define RIGHT_MOTOR_DIR 11

// 核心参数设置
const int STEP_PER_ROTATION = 120;
const int MOTOR_SPEED = 100;
const float SAFE_DISTANCE = 30.0; // 安全距离阈值（cm，小于该值判定为障碍物）
const int MAX_SAFE_ANGLES[2] = {0, 0}; // 存储最安全的两个角度（最大距离对应的角度）
float maxSafeDistance = 0.0; // 最大安全距离
int currentAngle = 0;
int stepCount = 0;
bool obstacleDetected = false; // 障碍物检测标志

void setup() {
  Serial.begin(115200);
  
  // 扫描电机引脚初始化
  pinMode(MOTOR_DIR, OUTPUT);
  pinMode(MOTOR_PUL, OUTPUT);
  pinMode(ENC_A, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // 运动电机引脚初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  
  // 中断与定时器初始化
  attachInterrupt(0, countStep, RISING);
  Timer1.initialize(1000000 / MOTOR_SPEED);
  Timer1.attachInterrupt(sendPulse);
  
  // 扫描电机初始方向：顺时针
  digitalWrite(MOTOR_DIR, HIGH);
  
  // 运动电机初始停止
  analogWrite(LEFT_MOTOR_PWM, 0);
  analogWrite(RIGHT_MOTOR_PWM, 0);
  
  Serial.println("避障机器人初始化完成，开始扫描...");
}

void countStep() {
  stepCount++;
  if (stepCount >= STEP_PER_ROTATION) {
    stepCount = 0;
    // 完成一圈扫描后，若未检测到障碍物，重置安全距离
    if (!obstacleDetected) {
      maxSafeDistance = 0.0;
    }
  }
  currentAngle = stepCount * (360.0 / STEP_PER_ROTATION);
}

void sendPulse() {
  digitalWrite(MOTOR_PUL, HIGH);
  delayMicroseconds(10);
  digitalWrite(MOTOR_PUL, LOW);
}

float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * 0.0343) / 2;
  
  if (distance < 2 || distance > 400) {
    return -1.0;
  }
  return distance;
}

// 运动控制函数：控制机器人直行/转向
void moveRobot(int leftSpeed, int rightSpeed) {
  // 左轮控制
  if (leftSpeed > 0) {
    digitalWrite(LEFT_MOTOR_DIR, HIGH);
    analogWrite(LEFT_MOTOR_PWM, leftSpeed);
  } else if (leftSpeed < 0) {
    digitalWrite(LEFT_MOTOR_DIR, LOW);
    analogWrite(LEFT_MOTOR_PWM, abs(leftSpeed));
  } else {
    analogWrite(LEFT_MOTOR_PWM, 0);
  }
  
  // 右轮控制
  if (rightSpeed > 0) {
    digitalWrite(RIGHT_MOTOR_DIR, HIGH);
    analogWrite(RIGHT_MOTOR_PWM, rightSpeed);
  } else if (rightSpeed < 0) {
    digitalWrite(RIGHT_MOTOR_DIR, LOW);
    analogWrite(RIGHT_MOTOR_PWM, abs(rightSpeed));
  } else {
    analogWrite(RIGHT_MOTOR_PWM, 0);
  }
}

// 避障决策函数：根据扫描数据选择最优方向
void avoidObstacle() {
  Serial.println("检测到障碍物，执行避障决策...");
  
  // 假设当前需要向“最大安全距离对应的角度”转向，简化起见，以左右轮差速实现转向
  // 实际可进一步计算最优角度与当前运动方向的夹角，调整转向幅度
  
  // 示例逻辑：若左侧安全，则右转；右侧安全，则左转（需结合角度进一步细化）
  // 这里简化为：当扫描到前方（0°）有障碍物时，选择左侧或右侧转向
  
  // 停止扫描电机
  Timer1.detachInterrupt();
  digitalWrite(MOTOR_PUL, LOW);
  
  // 执行转向（这里假设最大安全距离对应的角度在左侧，向右轮减速实现右转）
  Serial.println("右转避障...");
  moveRobot(100, 50); // 左轮转速100，右轮转速50，实现右转
  
  // 转向3秒后，停止转向，继续扫描
  delay(3000);
  moveRobot(0, 0); // 停止运动
  
  // 恢复扫描电机
  Timer1.attachInterrupt(sendPulse);
  
  // 重置障碍物标志
  obstacleDetected = false;
  maxSafeDistance = 0.0;
  
  Serial.println("避障完成，恢复扫描...");
}

void loop() {
  // 实时扫描测距
  float distance = getDistance();
  
  // 若测得有效距离
  if (distance >= 0) {
    // 检测是否小于安全阈值
    if (distance < SAFE_DISTANCE) {
      obstacleDetected = true;
      Serial.print("检测到障碍物！角度：");
      Serial.print(currentAngle);
      Serial.print("，距离：");
      Serial.println(distance);
      
      // 触发避障动作
      avoidObstacle();
    } else {
      // 更新最大安全距离和对应角度
      if (distance > maxSafeDistance) {
        maxSafeDistance = distance;
        MAX_SAFE_ANGLES[0] = currentAngle;
      }
    }
  }
  
  // 每100ms处理一次，避免卡顿
  delay(100);
}

避障逻辑优化方向
本案例为简化避障决策，采用了基础的差速转向；实际应用中可扩展为：
计算所有安全角度的分布，选择“与当前运动方向夹角最小”的角度作为目标方向；
结合多个角度的安全距离，计算转向幅度，实现更平滑的避障路径；
增加运动反馈（如编码器记录机器人位移），实现动态调整目标方向，避免二次碰撞。

要点解读
要点1：机械结构的精度是建图准确性的前提
超声波模块的旋转精度直接决定角度定位的准确性，进而影响建图质量，需从三方面保障：
电机选型：必须搭配带霍尔传感器的BLDC电机，且驱动板支持细分功能（如8细分、16细分），细分数越高，旋转角度的最小步长越小，角度定位越精准；若使用步进电机，需选择高精度步进电机（如1.8°步距角，可细分至0.09°）。
机械连接：超声波模块与电机轴需通过刚性联轴器连接，避免打滑或角度偏差；转盘安装需保证平衡，防止旋转过程中晃动导致角度测量误差。
角度校准：程序运行前需校准“零点”（如默认0°对应正前方），可通过在电机旋转轴安装机械挡块，触发限位开关，实现旋转归零，确保角度基准统一。
要点2：超声波测距的误差补偿是数据可靠性的关键
超声波测距易受环境干扰（温度、障碍物材质、反射角度等），需通过软件补偿提升数据准确性：
温度补偿：声速受温度影响显著（343m/s为20℃时的声速，温度每变化1℃，声速变化约0.6m/s），可在系统中增加DS18B20温度传感器，实时采集环境温度，修正声速计算公式，公式优化为：声速=331.3+0.606温度，可提升高精度场景的测距精度。
数据滤波：单次测距易出现跳变，可采用中值滤波（连续采集3-5组距离数据，取中间值）或均值滤波（取平均值），过滤随机噪声；对超出HC-SR04有效范围（2-400cm）的数据，直接标记为无效值，避免干扰决策。
盲区处理：超声波模块存在近距盲区（HC-SR04为2cm以内），需在程序中规避盲区，当距离小于2cm时，直接判定为障碍物，避免机器人碰撞。
要点3：角度与距离的数据同步是建图的核心逻辑
360°扫描建图的核心是“角度与距离的一一对应”，需实现采集与存储的精准同步，否则会导致地图错位：
中断与主循环的配合：电机编码器的中断信号触发角度更新，主循环触发超声波测距，需确保角度更新与测距的时序一致，避免因程序执行延迟导致“角度超前于测距”；可通过“中断更新角度，中断内触发测距”的方式，严格绑定角度与距离的同步关系，案例1中采用的编码器中断更新角度、主循环测距，需通过合理的延时控制确保步进与测距的匹配。
脉冲计数与角度换算：角度的计算依赖于电机脉冲计数，需根据驱动板的细分参数准确换算，公式为：角度=脉冲数(360°/(电机步距角驱动细分数))；若出现角度换算错误，会导致地图出现“拉伸”或“旋转偏移”，需严格校准脉冲数与角度的对应关系。
数据存储的连续性：存储扫描数据时，需用数组按角度顺序存储，确保每个角度对应唯一的距离值，避免数据错位；若使用多圈扫描数据，需标记每一圈的数据边界，防止不同圈的数据混淆。
要点4：电机控制的稳定性是实时性的保障
扫描过程需要电机持续稳定旋转，且能快速响应启停指令，需从电机控制逻辑入手保障稳定性：
驱动电路匹配：BLDC电机驱动板的电源需与电机额定电压匹配（如12V电机搭配12V驱动板），Arduino的逻辑电平（5V）与驱动板的控制信号（高电平触发）需兼容，避免信号电压不匹配导致电机失控；驱动板的PWM频率需与电机特性匹配，过高或过低的频率会导致电机抖动或转速不稳定。
启停与调速逻辑：程序中的电机启停需通过驱动板的使能引脚控制，避免直接断电导致的硬件冲击；调速时采用PWM渐变控制（如速度从0缓慢提升到设定值，停车时逐渐降为0），防止电机突然启停造成机械冲击，同时避免大电流冲击电路，延长硬件寿命。
旋转方向控制：需明确旋转方向与角度的对应关系，若顺时针旋转角度递增，程序中需保持角度计算与旋转方向的一致性；若需要正反转，需通过驱动板的方向引脚切换，并校准正反转的角度零点，避免因方向切换导致角度基准混乱。
要点5：系统功能的模块化设计是扩展性的核心
从基础扫描到地图构建、自主避障，程序功能需逐步扩展，模块化设计可提升代码可维护性与扩展性，满足不同场景的需求：
硬件驱动模块化：将电机驱动、超声波测距、传感器数据采集等功能封装为独立函数（如案例中的sendPulse()、getDistance()），后续更换硬件（如更换超声波模块为HC-SR04之外的型号）时，仅需修改对应驱动函数，无需调整主逻辑。
数据算法模块化：将数据滤波、坐标转换、避障决策等算法封装为独立模块，便于升级优化，如坐标转换模块可单独修改比例尺、坐标系原点，避障模块可替换为更复杂的路径规划算法（如A算法），无需改动底层驱动。
存储与通信模块化：如需长期存储地图数据，可将SD卡读写、串口通信封装为独立模块，便于扩展为WiFi、蓝牙等无线传输功能，满足远程监控需求；同时将地图数据格式标准化，方便与其他系统（如机器人导航系统）对接。

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