“基础增量栅格避障+贝塞尔曲率平滑”是一套专为资源受限嵌入式平台设计的轻量化局部导航架构，核心价值在于：通过增量式栅格更新与局部搜索将避障计算量压缩至Arduino可承受范围，再利用贝塞尔曲线对离散路径点进行曲率连续平滑，最终配合BLDC的FOC矢量控制输出，实现“感知轻量、决策高效、执行柔顺”的工业巡检闭环。

主要特点

1. 增量式栅格地图构建与动态更新
   系统以机器人为中心维护一个固定尺寸的局部栅格窗口（如10×10或20×20），每个栅格单元仅用1bit标记“可通行/障碍”，不存储冗余信息。 当机器人移动时，栅格窗口随位姿更新而滑动，旧区域数据被丢弃，新区域由传感器（超声波/ToF/激光雷达）实时填充。这种“增量式”更新策略避免了全局地图的存储和计算开销，将内存占用压缩至数百字节级别，使Arduino Uno（2KB SRAM）也能运行。
2. 局部A搜索与轻量化避障决策
   避障算法不追求全局最优路径，而是聚焦“当前障碍影响范围”的局部路径调整。当检测到障碍物侵入预设安全距离时，系统仅在障碍周围3×3或5×5的局部栅格内运行简化版A算法，搜索绕行路径。 为适配Arduino算力，算法做了针对性优化：openList用静态数组替代链表以减少内存分配开销；启发式函数采用曼哈顿距离（仅整数加减运算）替代欧氏距离（需浮点开方）；路径搜索深度限制在2-3步内，控制周期可压缩至100ms以内。
3. 贝塞尔曲率平滑与运动学约束
   A*输出的原始路径由一系列离散栅格点连接而成，呈折线状，直接执行会导致BLDC电机频繁启停和急转，产生机械冲击和轨迹跟踪误差。系统引入贝塞尔曲线对离散路径点进行平滑处理：选取路径上的关键航点作为控制点，生成二次或三次贝塞尔曲线，使输出轨迹满足曲率连续性（即加速度连续变化）。 平滑后的路径不仅消除了折线拐角处的速度突变，还能将最大曲率限制在机器人运动学约束范围内（如最小转弯半径），确保BLDC差速底盘能够精准跟踪。
4. BLDC的FOC矢量控制与轨迹精准执行
   平滑后的贝塞尔路径需转化为BLDC电机的左右轮速度指令。系统采用FOC（磁场定向控制）实现三环闭环控制：位置环接收路径点偏差输出目标速度，速度环输出目标转矩，电流环直接控制电机相电流。 相较于传统方波驱动，FOC的转矩脉动<1%，低速运行极其平稳，能精准执行贝塞尔曲线输出的连续变速指令，实现平滑的差速转向和渐进式加减速，避免传统直流电机的“顿挫感”导致轨迹跟踪偏差。
5. 分层式硬实时控制架构
   系统采用典型的三层架构：感知层（超声波/ToF/IMU）负责环境数据采集和位姿估计；决策层（Arduino）运行增量栅格更新、局部A*搜索和贝塞尔平滑，周期100-200ms；执行层（Arduino或专用BLDC驱动板）运行FOC电流环和PID速度环，周期1-5ms。 这种分层设计将计算密集型任务（如SLAM建图）卸载至上位机（若需要），Arduino仅负责局部避障和底层电机控制，充分发挥各自算力优势，确保系统实时性。

应用场景

1. 工业厂房设备巡检
   在制造业车间中，机器人沿预设路线巡检生产设备，检测温度、振动、异响等异常。增量栅格避障使其能灵活避开临时堆放的物料、工具车等动态障碍，贝塞尔平滑确保机器人在狭窄通道中平稳穿行，不产生剧烈晃动影响传感器测量精度。
2. 仓储物流货架巡检
   在立体仓库中，机器人沿货架通道巡检，盘点货物、检测货架变形或货物倾斜。沿墙导航策略将二维平面定位问题降维为单输入单输出系统（维持与货架的恒定距离），极大简化了计算量，使Arduino级平台也能实现稳定运行。 贝塞尔平滑使机器人在货架拐角处平滑转向，避免急转导致货物碰撞。
3. 地下管廊/隧道巡检
   在电力电缆沟道、综合管廊等半结构化环境中，机器人沿管壁或预设路线巡检，检测电缆温度、气体泄漏、结构裂缝等。增量栅格机制使其能适应管廊内临时施工、设备搬运等动态变化，无需预先构建完整地图即可实现自主避障。
4. 园区/厂区周界安防巡逻
   在工业园区、科技园区等室外场景，机器人按预设路线进行24小时巡逻，检测入侵、烟火等异常。增量栅格避障使其能应对行人、车辆等动态障碍，贝塞尔平滑确保机器人在路面不平整时仍能保持平稳运动，避免摄像头画面抖动影响识别效果。
5. 教育与科研实验平台
   在高校机器人课程中，Arduino+BLDC+增量栅格+贝塞尔平滑构成低成本高开放性的局部导航验证平台。学生可编程实现不同栅格更新策略、路径搜索算法和曲线平滑方法，直观理解嵌入式导航系统的设计原则与算力权衡。

需要注意的事项

1. 栅格分辨率与计算量的权衡
   栅格分辨率过高（如<5cm）会导致局部窗口内栅格数量激增，Arduino难以实时处理；分辨率过低（如>30cm）则无法精确描述障碍轮廓，导致避障路径过于保守或发生碰撞。建议根据机器人尺寸和传感器精度选择栅格大小，通常取机器人宽度的1/3-1/2（如机器人宽30cm，栅格取10-15cm）。
2. 贝塞尔曲线控制点选取与曲率约束
   贝塞尔曲线的平滑效果取决于控制点的选取策略。若控制点过密，曲线趋近于原始折线，平滑效果不佳；若控制点过疏，曲线可能偏离原始路径过多，导致机器人与障碍物距离过近甚至碰撞。建议在路径拐点前后各选取1-2个控制点，并验证生成曲线的最大曲率是否小于机器人最小转弯半径的倒数。对于急弯场景，可分段生成贝塞尔曲线并拼接，确保曲率连续。
3. 传感器噪声与栅格误标记
   超声波传感器易受环境干扰（如镜面反射、柔软吸声材料），导致测距跳变，进而使栅格被误标记为障碍或可通行。建议在软件端采用中值滤波或滑动平均滤波处理原始测距数据，并在栅格更新时引入“置信度”机制：同一栅格需连续多次被检测为障碍才标记为不可通行，避免瞬时噪声导致误判。
4. 位姿估计误差与栅格窗口漂移
   增量式栅格地图的更新依赖机器人的位姿估计（x, y, θ），若轮式里程计存在累积误差（如轮子打滑、地面不平），栅格窗口将逐渐偏离真实环境，导致避障失效。建议融合IMU数据（通过互补滤波或简易卡尔曼滤波）补偿航向角漂移，并在条件允许时引入外部定位校正（如UWB、二维码地标）消除累积误差。
5. BLDC驱动与FOC控制的匹配
   普通航模ESC仅支持开环油门控制，无法实现精准的速度/位置跟踪，必须使用支持FOC矢量控制的BLDC驱动器（如SimpleFOC兼容板、ODrive、VESC等），并搭配编码器实现闭环反馈。 若仅使用L298N等简易H桥驱动有刷电机，虽可实现基础避障，但无法发挥贝塞尔平滑的轨迹跟踪优势，运动平稳性大打折扣。
6. 局部最优陷阱与死锁恢复
   局部A*搜索仅在小范围内寻路，当机器人陷入U型障碍或狭窄死胡同时，可能找不到可行路径而陷入死锁。建议设置“死锁检测”机制：若连续多次局部搜索失败，则触发全局重规划（如后退至安全位置后重新搜索）或执行预设的脱困策略（如原地旋转180°后沿原路返回）。
7. 控制频率与实时性保障
   增量栅格更新、A*搜索和贝塞尔平滑的计算需在100-200ms内完成，FOC电流环需达1-5ms控制周期。若控制周期抖动过大，将直接导致轨迹跟踪偏差、避障响应滞后乃至碰撞风险。控制回路必须使用硬件定时器中断或millis()非阻塞定时，严禁使用delay()函数，以确保传感器数据与电机控制的严格同步。

1、增量式栅格地图更新与局部避障
场景：工业厂区巡检机器人通过超声波传感器感知环境，仅对障碍物附近区域进行栅格更新，避免全图重构的算力浪费。在 20×20 栅格（4米×4米）范围内，Arduino 可流畅运行。

/* ===== 增量式栅格地图更新 + 局部避障 =====
 * 硬件：Arduino + BLDC 差速底盘 + 超声波传感器
 * 核心：仅更新障碍物周围栅格，避免全图遍历
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// --- BLDC 驱动 ---
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);

// --- 超声波 ---
#define TRIG_F 22
#define ECHO_F 23
NewPing sonarF(TRIG_F, ECHO_F, 200);

// --- 栅格地图参数 ---
#define GRID_SIZE 20
#define CELL_CM 20
byte grid[GRID_SIZE][GRID_SIZE];  // 0=未知, 1=空闲, 2=障碍
int robotX = 10, robotY = 10;     // 机器人栅格坐标

// --- 增量栅格更新 ---
void incrementalGridUpdate(int obsX, int obsY, int radius) {
  for (int dx = -radius; dx <= radius; dx++) {
    for (int dy = -radius; dy <= radius; dy++) {
      int x = obsX + dx;
      int y = obsY + dy;
      if (x >= 0 && x < GRID_SIZE && y >= 0 && y < GRID_SIZE) {
        if (dx * dx + dy * dy <= radius * radius) {
          grid[x][y] = 2;  // 标记为障碍
        }
      }
    }
  }
}

// 超声波扇形扫描更新地图
void updateGridWithSonar(float distance) {
  int maxCells = floor(distance / CELL_CM);
  for (int angle = -30; angle <= 30; angle += 15) {
    float radAngle = angle * PI / 180.0;
    for (int c = 1; c <= maxCells; c++) {
      int gx = robotX + round(c * cos(radAngle));
      int gy = robotY + round(c * sin(radAngle));
      if (gx >= 0 && gx < GRID_SIZE && gy >= 0 && gy < GRID_SIZE) {
        if (c == maxCells) grid[gx][gy] = 2;      // 边界为障碍
        else if (grid[gx][gy] != 2) grid[gx][gy] = 1;  // 路径为空闲
      }
    }
  }
}

// 基于栅格的避障决策
int getAvoidanceTurn() {
  for (int step = 1; step <= 3; step++) {
    int fx = robotX + step;
    int fy = robotY;
    if (fx < GRID_SIZE && grid[fx][fy] == 2) {
      bool leftFree = (robotY - 1 >= 0 && grid[fx][robotY - 1] != 2);
      bool rightFree = (robotY + 1 < GRID_SIZE && grid[fx][robotY + 1] != 2);
      if (leftFree && !rightFree) return -1;  // 左转
      if (rightFree && !leftFree) return 1;   // 右转
      return 0;  // 两侧都堵，停车
    }
  }
  return 0;
}

void setup() {
  Serial.begin(115200);
  for (int x = 0; x < GRID_SIZE; x++)
    for (int y = 0; y < GRID_SIZE; y++)
      grid[x][y] = 0;

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

  // 超声波测距并增量更新地图
  int dist = sonarF.ping_cm();
  if (dist > 0) {
    updateGridWithSonar(dist / 100.0);
  }

  // 避障决策
  int turn = getAvoidanceTurn();
  float baseSpeed = 1.0;
  motorL.move(baseSpeed - turn * 0.3);
  motorR.move(baseSpeed + turn * 0.3);

  delay(50);
}

核心要点：增量更新的核心在于“只改需要改的地方”。超声波扇形扫描的每个栅格在首次被标记后，后续只有冲突时才覆盖，大幅减少地图维护计算量。对于 Arduino 有限的 SRAM，20×20 的 byte 数组仅占 400 字节，远小于完整概率栅格地图的内存需求。

2、三次贝塞尔曲线路径平滑与轨迹跟踪
场景：将离散的路径点通过三次贝塞尔曲线拟合，生成 C² 连续（加速度连续）的平滑轨迹，消除栅格路径的直角拐点，使 BLDC 差速底盘平稳通过。

/* ===== 三次贝塞尔曲线路径平滑 =====
 * 核心：贝塞尔曲线凸包性与端点插值特性保证路径平滑
 */
#include <SimpleFOC.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);

// --- 贝塞尔曲线结构 ---
struct BezierCurve {
  float P0[2], P1[2], P2[2], P3[2];  // 4个控制点
  float duration;                     // 总时长（秒）
};

BezierCurve currentPath;
float pathProgress = 0;
bool isPathActive = false;
float robotPos[2] = {0, 0};

// 三次贝塞尔曲线计算
void bezierCubic(float t, float P0[2], float P1[2], float P2[2], float P3[2], float out[2]) {
  float u = 1 - t;
  float b0 = u * u * u;
  float b1 = 3 * u * u * t;
  float b2 = 3 * u * t * t;
  float b3 = t * t * t;

  out[0] = b0 * P0[0] + b1 * P1[0] + b2 * P2[0] + b3 * P3[0];
  out[1] = b0 * P0[1] + b1 * P1[1] + b2 * P2[1] + b3 * P3[1];
}

// 设置贝塞尔路径
void setPath(BezierCurve path, float duration) {
  currentPath = path;
  currentPath.duration = duration;
  pathProgress = 0;
  isPathActive = true;
  Serial.println("贝塞尔平滑路径已规划");
}

// 跟踪贝塞尔曲线轨迹
void trackBezierPath() {
  pathProgress += 0.02 / currentPath.duration;
  if (pathProgress >= 1.0) {
    pathProgress = 1.0;
    isPathActive = false;
    robotPos[0] = currentPath.P3[0];
    robotPos[1] = currentPath.P3[1];
    return;
  }

  // 计算曲线上的当前位置
  float pos[2];
  bezierCubic(pathProgress, currentPath.P0, currentPath.P1, currentPath.P2, currentPath.P3, pos);

  // 计算速度与转向
  float dx = pos[0] - robotPos[0];
  float dy = pos[1] - robotPos[1];
  float angle = atan2(dy, dx);
  float speed = sqrt(dx * dx + dy * dy) * 0.3;

  // 差速控制
  float wheelBase = 0.25;
  motorL.move(speed - angle * wheelBase / 2);
  motorR.move(speed + angle * wheelBase / 2);

  // 更新位置
  robotPos[0] = pos[0];
  robotPos[1] = pos[1];
}

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;

  // 定义一条平滑巡检路径
  BezierCurve path = {
    {0.0, 0.0},   // 起点
    {1.0, 1.5},   // 控制点1（决定初始方向）
    {3.0, 1.5},   // 控制点2（决定终点方向）
    {4.0, 0.0}    // 终点
  };
  setPath(path, 5.0);
}

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

  if (isPathActive) {
    trackBezierPath();
  }

  delay(10);
}

核心要点：贝塞尔曲线解决的是“路径连续性”问题。栅格路径存在直角拐点，直接跟踪会导致机器人急停、急转，加剧机械磨损。三次贝塞尔曲线天生具有 C² 连续性（加速度连续），且凸包性保证曲线不会超出控制点范围，适合工业巡检的平稳性要求。路径与速度解耦是关键设计：贝塞尔曲线定义“去哪儿”，速度规划定义“多快去”。

3、增量栅格重规划 + 贝塞尔曲率平滑融合
场景：工业厂区巡检中，超声波检测到新障碍后，在原有路径基础上进行局部增量重规划，绕开障碍后通过贝塞尔曲线平滑衔接，实现“避障不中断、路径不突变”。

/* ===== 增量栅格重规划 + 贝塞尔曲率平滑 =====
 * 核心：D* Lite 思想局部修正 + 贝塞尔平滑衔接
 */
#include <SimpleFOC.h>
#include <NewPing.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);

#define TRIG_F 22
#define ECHO_F 23
NewPing sonarF(TRIG_F, ECHO_F, 200);

// --- 栅格地图 ---
#define GRID_SIZE 30
byte gridMap[GRID_SIZE][GRID_SIZE];

// --- 路径点结构 ---
struct Point { float x, y; };
#define MAX_PATH 20
Point originalPath[MAX_PATH];
Point smoothedPath[MAX_PATH];
int pathCount = 0;

// --- 增量障碍记录 ---
int obstacles[5][2];
int obsCount = 0;

// 初始化栅格与路径
void initGridAndPath() {
  for (int x = 0; x < GRID_SIZE; x++)
    for (int y = 0; y < GRID_SIZE; y++)
      gridMap[x][y] = 0;

  // 预设巡检路径（栅格坐标）
  originalPath[0] = {2, 5};
  originalPath[1] = {5, 5};
  originalPath[2] = {8, 4};
  originalPath[3] = {12, 6};
  originalPath[4] = {15, 5};
  originalPath[5] = {18, 8};
  pathCount = 6;
}

// 增量障碍更新
void dynamicObstacleUpdate(int obs[][2], int cnt) {
  for (int i = 0; i < cnt; i++) {
    int obsX = obs[i][0];
    int obsY = obs[i][1];
    int radius = 2;
    for (int dx = -radius; dx <= radius; dx++) {
      for (int dy = -radius; dy <= radius; dy++) {
        int x = obsX + dx, y = obsY + dy;
        if (x >= 0 && x < GRID_SIZE && y >= 0 && y < GRID_SIZE) {
          if (dx * dx + dy * dy <= radius * radius)
            gridMap[x][y] = 2;
        }
      }
    }
  }
}

// 增量路径重规划：仅修正被障碍覆盖的路径点
int incrementalReplan(Point* origin, int cnt, Point* out) {
  int newCnt = 0;
  for (int i = 0; i < cnt; i++) {
    out[newCnt] = origin[i];
    // 检查当前路径点是否被障碍覆盖
    for (int j = 0; j < obsCount; j++) {
      int dx = out[newCnt].x - obstacles[j][0];
      int dy = out[newCnt].y - obstacles[j][1];
      if (dx * dx + dy * dy <= 4) {  // 障碍半径2格
        out[newCnt].x += 2;  // 向右绕行
        break;
      }
    }
    newCnt++;
  }
  return newCnt;
}

// 贝塞尔曲率平滑：将折线路径转为平滑曲线
void bezierSmooth(Point* route, int cnt, Point* smooth) {
  if (cnt < 3) {
    for (int i = 0; i < cnt; i++) smooth[i] = route[i];
    return;
  }

  // 端点保留
  smooth[0] = route[0];
  smooth[cnt - 1] = route[cnt - 1];

  // 中间点：三次贝塞尔式加权平滑
  for (int i = 1; i < cnt - 1; i++) {
    smooth[i].x = (route[i - 1].x + 2 * route[i].x + route[i + 1].x) / 4.0;
    smooth[i].y = (route[i - 1].y + 2 * route[i].y + route[i + 1].y) / 4.0;

    // 曲率限幅：避免过度平滑偏离原路径
    float dx1 = route[i].x - route[i - 1].x;
    float dy1 = route[i].y - route[i - 1].y;
    float dx2 = route[i + 1].x - route[i].x;
    float dy2 = route[i + 1].y - route[i].y;
    float curvature = fabs(dx1 * dy2 - dy1 * dx2);

    if (curvature > 1.5) {  // 曲率过大，回退部分平滑量
      smooth[i].x = (route[i - 1].x + route[i].x) / 2.0;
      smooth[i].y = (route[i - 1].y + route[i].y) / 2.0;
    }
  }
}

// 跟踪平滑路径（简化：朝向路径点前进）
void trackSmoothedPath(Point* path, int cnt) {
  static int currentIdx = 0;
  if (currentIdx >= cnt) return;

  // 简化的差速控制：朝向目标点
  float dx = path[currentIdx].x - 10;  // 假设机器人当前位置(10,5)
  float dy = path[currentIdx].y - 5;
  float dist = sqrt(dx * dx + dy * dy);

  if (dist < 0.5) {
    currentIdx++;
    if (currentIdx >= cnt) currentIdx = 0;
  }

  float angle = atan2(dy, dx);
  float speed = constrain(dist * 0.2, 0.1, 0.5);

  motorL.move(speed - angle * 0.5);
  motorR.move(speed + angle * 0.5);
}

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

  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. 动态障碍检测（简化：每隔5秒模拟）
  static unsigned long lastObs = 0;
  if (millis() - lastObs > 5000) {
    lastObs = millis();
    int newObs[1][2] = {{8, 4}};
    dynamicObstacleUpdate(newObs, 1);
    obsCount = 1;
    obstacles[0][0] = 8; obstacles[0][1] = 4;
    Serial.println("检测到新障碍，触发增量重规划");
  }

  // 2. 增量路径重规划
  Point newPath[MAX_PATH];
  int newCnt = incrementalReplan(originalPath, pathCount, newPath);

  // 3. 贝塞尔曲率平滑
  bezierSmooth(newPath, newCnt, smoothedPath);

  // 4. 跟踪平滑路径
  trackSmoothedPath(smoothedPath, newCnt);

  delay(50);
}

核心要点：本案例将增量栅格与贝塞尔平滑串联为完整闭环。增量重规划的核心思想是 “仅修正受影响的局部路径”，而非全量重算。贝塞尔平滑则在重规划后的折线路径上施加曲率约束，避免绕行路径出现新的直角拐点。曲率限幅是关键细节——过度平滑会导致路径偏离障碍物安全距离，曲率约束确保平滑后的路径仍保持在安全走廊内。

要点解读

1. 增量栅格的核心是“时间维度的局部更新”，而非空间维度的全图维护
   工业厂区巡检机器人不需要维护完整的全局栅格地图。超声波扇形扫描的每个栅格在首次标记后，后续只有传感器数据冲突时才覆盖。这种“只改需要改的地方”的策略，将 20×20 地图的维护计算量控制在 Arduino 可承受范围内。对于更大的巡检区域，可采用滚动窗口方式，仅维护机器人周围的局部栅格。

2. 贝塞尔曲线解决“路径连续性”，本质是让曲率与执行机构动态特性匹配
   栅格路径的直角拐点会导致 BLDC 差速底盘在跟踪时急停急转，不仅加剧机械磨损，还会造成巡检设备的振动干扰。三次贝塞尔曲线具有 C² 连续性（加速度连续），其凸包性保证曲线不会超出控制点范围。路径平滑的目标不仅是“消除尖角”，更是让路径的曲率、加速度特性与 BLDC 电机的力矩响应带宽相匹配。

3. 增量重规划与路径平滑必须“先重规划、后平滑”，顺序不可颠倒
   如果先平滑再遇到新障碍，平滑后的曲线可能已经偏离了安全走廊，重规划时反而需要更大的修正量。正确顺序是：检测到新障碍 → 在栅格层面增量修正路径点 → 对修正后的折线路径施加贝塞尔平滑。程序案例三中的 incrementalReplan 先运行，bezierSmooth 后运行，且平滑时保留端点、限制中间点曲率，确保平滑后的路径仍能绕开障碍。

4. Arduino 算力约束下，“简化算法 + 参数化”比“完整算法”更可行
   完整的 D* Lite 或 TEB 优化在 Arduino 上难以实时运行。程序案例三采用简化策略：障碍检测后直接在原始路径上做局部偏移（向右绕行2格），而非运行完整的最短路径搜索。贝塞尔平滑也用三点加权平均替代完整的三次贝塞尔控制点计算。这种“够用即可”的工程取舍，是 Arduino 平台实现复杂导航功能的务实路径。

5. 路径平滑的“曲率限幅”是安全底线，防止平滑偏离障碍物安全距离
   贝塞尔平滑的副作用是可能将路径点向障碍物方向“拉近”。程序案例三中的曲率计算与回退机制是关键保障：当平滑后的路径曲率超过阈值时，回退部分平滑量，保持路径点与原折线路径的足够接近。这种“平滑但不过度平滑”的策略，在保证运动平稳性的同时，守住了避障安全距离的底线。

4、基础增量栅格避障系统的底层执行与基础避障逻辑程序（核心：低成本栅格建图+基础避障）
应用场景：工业管廊、车间地面巡检，适用于无GPS、无预载地图的封闭场景，通过激光/超声波传感器增量构建栅格地图，实现基础障碍检测与绕障。

核心逻辑
以机器人为中心，建立固定分辨率增量栅格地图，降低内存占用
增量式更新栅格：根据运动里程与传感器读数，实时更新周围栅格占用状态
栅格碰撞预判：沿运动方向扫描，若前方栅格被占用，触发基础绕障
运动与建图同步：电机运动的同时更新栅格，保证地图与实际环境一致

// 基础增量栅格避障程序
// 硬件：超声波传感器（前/左/右）+ 电机码盘 + BLDC驱动
// 栅格参数：分辨率5cm，内存优化为16×16局部栅格，适配Arduino内存

// ===== 引脚定义 =====
const byte F_TRIG = 8, F_ECHO = 7;   // 前向超声波
const byte L_TRIG = 5, L_ECHO = 4;   // 左向超声波
const byte R_TRIG = 3, R_ECHO = 2;   // 右向超声波const byte IN1 = A2, IN2 = A3;const byte PWM_PIN = 9;
// ===== 栅格参数 =====
#define GRID_SIZE 16        // 16×16局部栅格
#define GRID_RESOLUTION 5   // 每格对应5cm#define GRID_RANGE (40/GRID_RESOLUTION) // 探测范围40cm，对应8格

// ===== 栅格地图（0=空闲，1=障碍，-1=未知）=====
int8_t gridMap[GRID_SIZE][GRID_SIZE];
// 机器人在栅格中的坐标（中心为原点，偏移8格）
int robotGridX = GRID_SIZE/2;
int robotGridY = GRID_SIZE/2;
// ===== 运动与距离变量 =====
float currentSpeed = 8.0;
int gridIndex = 0;
unsigned long lastMoveTime = 0;
float distanceDiff =0; // 用于栅格增量校正

// ===== 函数声明 =====
void initGridMap();
int readUltrasonic(byte trig, byte echo);
void updateGridMap();
bool checkObstacleAhead();
void basicAvoidObstacle();
void moveForward();
void controlMotor(float speed);

void setup() {
  Serial.begin(115200);
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  initGridMap();
  Serial.println("增量栅格避障系统初始化完成");
}

void loop() {
  if (millis() - lastMoveTime > 100) { // 50Hz
    // 1. 增量更新栅格地图
    updateGridMap();
    // 2. 避障检测与控制
    if (checkObstacleAhead()) {
      basicAvoidObstacle();
    } else {
      moveForward();
    }
    lastMoveTime = millis();
  }

Serial.print("栅格坐标:"); Serial.print(robotGridX); Serial.print(","); Serial.println(robotGridY);
  Serial.print("前方障碍:"); Serial.println(checkObstacleAhead());
  delay(20);
}

// 初始化栅格地图，全部为未知
void initGridMap() {
  for (int i=0; i<GRID_SIZE; i++) {
    for (int j=0; j<GRID_SIZE; j++) {
      gridMap[i][j] = -1;
    }
  }
}

// 读取超声波距离（cm）
int readUltrasonic(byte trig, byte echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 30000);
  return (duration * 0.0343) / 2.0;
}

// 增量更新栅格地图
void updateGridMap() {
  // 简化：根据运动距离更新机器人位置（实际可接码盘计算位移）
  float moveSpeed = 8.0; // cm/s
  float time = (millis() - lastMoveTime) / 1000.0;
  distanceDiff += moveSpeed * time * 2; // 来回测距校正（模拟）

  // 更新前方栅格
  int sensorRange = readUltrasonic(F_TRIG, F_ECHO) / GRID_RESOLUTION;
  sensorRange = constrain(sensorRange, 0, GRID_RANGE);
  for (int i=0; i<=sensorRange; i++) {
    int x = robotGridX + i;
    int y = robotGridY;
    x = constrain(x, 0, GRID_SIZE-1);
y = constrain(y, 0, GRID_SIZE-1);
    if (sensorRange <= GRID_RANGE && i >= sensorRange-2) {
      gridMap[x][y] = 1; // 障碍
    } else if (i < sensorRange-2) {
      gridMap[x][y] = 0; // 空闲} else {
      gridMap[x][y] = -1; // 未知
    }
  }
}

// 检测前方是否有障碍
bool checkObstacleAhead() {
  int aheadGrid = robotGridX +3; // 前方3格（15cm）内为危险区
  aheadGrid = constrain(aheadGrid, 0, GRID_SIZE-1);
  return gridMap[aheadGrid][robotGridY] == 1;
}

// 基础避障：左右移动寻找路径
void basicAvoidObstacle() {
  int leftDist = readUltrasonic(L_TRIG, L_ECHO);
  int rightDist = readUltrasonic(R_TRIG, R_ECHO);

  controlMotor(0); // 先停止
  delay(100);

  if (leftDist > rightDist && leftDist > 30) {
    // 左侧空旷，左转
    digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
    controlMotor(5.0);
    Serial.println("左侧空旷，左转避障");
  } else if (rightDist >30) {
    // 右侧空旷，右转
    digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
    controlMotor(5.0);
    Serial.println("右侧空旷，右转避障");
  } else {
    // 左右都有障碍，后退    digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
    controlMotor(3.0);
    Serial.println("左右受限，后退调整");
  }
}

// 前进
void moveForward() {
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  controlMotor(currentSpeed);
}

// 电机速度控制
void controlMotor(float speed) {
  speed = constrain(speed, 0, 15.0);
  analogWrite(PWM_PIN, (int)speed);
}

适用场景优化方向
可增加多组超声波或激光雷达，提升栅格分辨率与探测范围
增加码盘数据，实现精准的栅格增量更新，避免地图漂移
优化栅格存储，采用稀疏矩阵或位图，进一步降低内存占用

5、贝塞尔曲线曲率平滑的路径规划程序（核心：路径平滑与转角优化）
应用场景：工业巡检中，避障后的路径平滑、拐点优化、转角抖动抑制，解决传统栅格避障产生的折线路径曲率突变问题。

核心逻辑
1.从栅格地图中提取路径点序列，作为贝塞尔曲线的控制点
2. 采用三次贝塞尔曲线对路径点进行平滑，保证曲率连续
3. 优化曲线参数，控制曲率最大值，避免急转弯4. 生成平滑轨迹点，输入电机控制，实现平滑运动

// 贝塞尔曲线路径平滑程序
// 硬件：无额外硬件，基于上例栅格地图的路径点
// 核心：三次贝塞尔曲线平滑，实现曲率连续，无转角突变

#include <Arduino.h>
#include <math.h>

// ===== 路径与曲线参数 =====
#define CONTROL_POINT_NUM 8// 控制点数量（最多支持8个，适配Arduino计算能力）
#define BEZIER_POINT_NUM 20       // 每段曲线插值点数
#define MAX_CURVATURE 0.3         // 最大曲率（rad/cm），限制急转弯

// 输入：原始栅格路径点（来自避障算法）
typedef struct {
  float x;  // 单位：cm
  float y;
} Point;

Point rawPath[CONTROL_POINT_NUM];  // 原始路径点
Point smoothPath[CONTROL_POINT_NUM * BEZIER_POINT_NUM]; // 平滑后的路径
int pathPointNum = 5;              // 实际路径点数
int smoothIndex = 0;

// ===== 电机与控制变量 =====
const byte IN1 = A2, IN2 = A3;
const byte PWM_PIN =9;
float currentSpeed = 8.0;
int curPointIndex = 0;
float targetX =0, targetY =0;

// ===== 函数声明 =====
void initPath();
float cubicBezier(float t, float p0, float p1, float p2, float p3);
void smoothPathWithBezier();
float calcCurvature(Point p0, Point p1, Point p2);
void followSmoothPath();
void controlMotorByCurvature(float curvature);

void setup() {
  Serial.begin(115200);
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  initPath();
Serial.println("贝塞尔曲线路径平滑系统初始化完成");
}

void loop() {
  // 1. 生成平滑路径
  smoothPathWithBezier();
  // 2. 沿平滑路径运动
  followSmoothPath();
  delay(20);
}

// 初始化原始路径（模拟栅格避障得到的折线路径）
void initPath() {
  rawPath[0] = {0, 0};
  rawPath[1] = {10, 0};
  rawPath[2] = {20, 15};   // 拐点
  rawPath[3] = {40, 15};
  rawPath[4] = {50, 30};   // 拐点
  pathPointNum =5;
}

// 三次贝塞尔曲线计算：t∈[0,1]，返回插值后的y值（x按t线性映射）
float cubicBezier(float t, float p0, float p1, float p2, float p3) {
  float t2 = t * t;
  float t3 = t2 * t;
  float mt = 1 - t;
  float mt2 = mt * mt;
  float mt3 = mt2 * mt;
  return p0*mt3 + 3*p1*mt2*t + 3*p2*mt*t2 + p3*t3;
}

// 计算三点曲率，用于限制最大曲率
float calcCurvature(Point p0, Point p1, Point p2) {
  float k = 0.0;
  float dx1 = p1.x - p0.x;
  float dy1 = p1.y - p0.y;
  float dx2 = p2.x - p1.x;
  float dy2 = p2.y - p1.y;
  floatdist1 = sqrt(dx1*dx1 + dy1*dy1);
  float dist2 = sqrt(dx2*dx2 + dy2*dy2);
  if (dist1 ==0 || dist2 ==0) return 0.0;
  // 曲率公式：k = |x1y2 - x2y1| / (|v1||v2|(v1·v2))
  float cross = dx1*dy2 - dx2*dy1;
  float dot = dx1*dx2 + dy1*dy2;
  k = abs(cross) / (dist1*dist2*dot);
  k = constrain(k, 0.0, MAX_CURVATURE);
  return k;
}

// 贝塞尔曲线平滑所有路径段
void smoothPathWithBezier() {
  smoothIndex =0;
  // 对每相邻4个控制点（p0,p1,p2,p3）生成一段三次贝塞尔曲线
  for (int i=0; i< pathPointNum -3; i++) {
    Point p0 = rawPath[i];
    Point p1 = rawPath[i+1];   // 实际为辅助控制点，可优化
    Point p2 = rawPath[i+2];   // 实际为辅助控制点
    Point p3 = rawPath[i+3];

    // 控制点优化：限制曲率，调整辅助点位置
    float k = calcCurvature(p0, p1, p2);
    if (k > MAX_CURVATURE) {
      // 曲率超限，向路径内侧收缩辅助点
 float offset = (k - MAX_CURVATURE) * 5;
      p1.x -= offset;
      p2.x += offset;
    }

    // 插值生成平滑点
    for (int t=0; t < BEZIER_POINT_NUM; t++) {
      float tt = t / (BEZIER_POINT_NUM -1);
      smoothPath[smoothIndex].x = p0.x + (p3.x - p0.x) * tt;
      smoothPath[smoothIndex].y = cubicBezier(tt, p0.y, p1.y, p2.y, p3.y);
      smoothIndex++;
    }
  }
}

// 沿平滑路径运动
void followSmoothPath() {
  if (curPointIndex >= smoothIndex) {
    // 路径跟踪完成，停止
    digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
return;
  }

  targetX = smoothPath[curPointIndex].x;
  targetY = smoothPath[curPointIndex].y;

  // 计算当前点曲率，调整速度（曲率大则减速）
  curPointIndex++;
  float curCurvature = 0.0;
  if (curPointIndex >=2) {
    curCurvature = calcCurvature(smoothPath[curPointIndex-2],
     smoothPath[curPointIndex-1],
                                 smoothPath[curPointIndex]); }
  controlMotorByCurvature(curCurvature);
}

// 根据曲率调整电机速度
void controlMotorByCurvature(float curvature) {
  // 曲率越大，速度越低  float speed = map(curvature, 0, MAX_CURVATURE, 10.0, 3.0);
  speed = constrain(speed, 3.0, 10.0);  // 计算航向角，调整转向（简化为直行，实际可结合角度控制）
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(PWM_PIN, (int)speed);
}

适用场景优化方向
可增加路径点简化算法（如RDP算法），减少控制点数量，提升计算效率
可引入曲率连续约束，保证相邻曲线段的曲率平滑过渡，进一步抑制抖动
可结合激光雷达等高精度传感器，提升路径点精度，提升平滑质量

6、栅格避障与贝塞尔平滑联动的完整工业巡检避障程序（核心：避障+平滑一体化）
应用场景：完整工业巡检流程，将增量栅格避障与贝塞尔曲线平滑无缝结合，实现“避障-路径生成-平滑-跟随”全流程自动化。

核心逻辑
避障触发：增量栅格检测到前方障碍，生成绕障路径点
路径平滑：对绕障路径点进行贝塞尔平滑，消除折线与转角突变
平滑跟随：机器人沿平滑路径运动，同时实时更新栅格地图，处理动态障碍
循环巡检：沿预设巡检路线循环，遇障自动避让，保持巡检连续性

// 栅格避障+贝塞尔平滑联动巡检程序
// 融合增量栅格避障与贝塞尔曲线平滑，实现工业巡检全流程避障

#include <Arduino.h>
#include <math.h>

// ===== 硬件引脚 =====
const byte F_TRIG = 8, F_ECHO =7;
const byte L_TRIG = 5, L_ECHO =4;
const byte R_TRIG =3, R_ECHO =2;
const byte IN1 = A2, IN2 = A3;
const byte PWM_PIN =9;

// ===== 栅格参数 =====
#define GRID_SIZE 16
#define GRID_RESOLUTION 5
int8_t gridMap[GRID_SIZE][GRID_SIZE];
int robotGridX = GRID_SIZE/2;
int robotGridY = GRID_SIZE/2;

// ===== 路径与平滑参数 =====
#define CONTROL_POINT_NUM 8
#define MAX_CURVATURE 0.3
typedef struct { float x; float y; } Point;
Point rawPath[CONTROL_POINT_NUM];
Point smoothPath[CONTROL_POINT_NUM * 20];
int rawPathNum =0;
int smoothIndex =0;

// ===== 状态定义 =====
typedef enum {
  STATE_MOVE,        // 沿路径运动
  STATE_AVOID,// 避障
  STATE_SMOOTH,      // 路径平滑
  STATE_HOLD         // 待机
} WorkState;
WorkState state = STATE_MOVE;

int curPointIndex = 0;
unsigned long lastTime = 0;

// ===== 函数声明 =====
void initAll();
int readUltrasonic(byte trig, byte echo);
void updateGridMap();
bool checkObstacle();
void generateAvoidPath();
void smoothPathWithBezier();
void followPath();
void controlMotor(float speed);

void setup() {
  Serial.begin(115200);
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  initAll();
  Serial.println("栅格避障+贝塞尔平滑联动系统初始化完成");
}

void loop() {
  if (millis() - lastTime > 50) { // 20Hz
    switch (state) {
      case STATE_MOVE:
        updateGridMap();
        if (checkObstacle()) {
          state = STATE_AVOID;} else {
          followPath();
        }
        break;

      case STATE_AVOID:
        // 生成避障路径
        generateAvoidPath();
        // 平滑路径
        smoothPathWithBezier();
        // 切换为跟随状态
        state = STATE_MOVE;
        curPointIndex = 0;
        break;

      case STATE_HOLD:
        digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
        break;
    }    lastTime = millis();
  }

  Serial.print("状态:"); Serial.print(state);
  Serial.print(" 路径点:"); Serial.println(rawPathNum);
  delay(10);
}

// 初始化栅格与路径
void initAll() {
  for (int i=0; i<GRID_SIZE; i++)
    for (int j=0; j<GRID_SIZE; j++)
      gridMap[i][j] = -1;  // 初始化巡检主路径（模拟）
  rawPath[0] = {0, 0};
  rawPath[1] = {20, 0};
  rawPath[2] = {40, 0};
  rawPath[3] = {60, 20};
  rawPath[4] = {80, 20};
  rawPathNum =5;
}

// 读取超声波距离
int readUltrasonic(byte trig, byte echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 30000);
  return (duration *0.0343) / 2.0;
}

// 更新栅格地图
void updateGridMap() {
  int frontDist = readUltrasonic(F_TRIG, F_ECHO) / GRID_RESOLUTION;
  frontDist = constrain(frontDist, 0, 8);
  int x = robotGridX + frontDist;
  int y = robotGridY;
  x = constrain(x, 0, GRID_SIZE-1);
  y = constrain(y, 0, GRID_SIZE-1);
  gridMap[x][y] = (frontDist <4) ? 1 : 0;
}

// 检测前方障碍
bool checkObstacle() {
  int aheadGrid = robotGridX +4;
  aheadGrid = constrain(aheadGrid, 0, GRID_SIZE-1);
  return gridMap[aheadGrid][robotGridY] == 1;
}

// 生成避障路径（简化：绕开前方障碍，更新路径点）
void generateAvoidPath() {
  int leftDist = readUltrasonic(L_TRIG, L_ECHO);
  int rightDist = readUltrasonic(R_TRIG, R_ECHO);

  // 找到当前路径终点，在前方添加绕障点
  Point endPoint = rawPath[rawPathNum-1];
  if (leftDist > rightDist && leftDist > 30) {
    // 左侧绕障
    rawPath[rawPathNum-1] = {endPoint.x-10, endPoint.y+15};
    rawPath[rawPathNum] = {endPoint.x, endPoint.y+15};
    rawPath[rawPathNum+1] = {endPoint.x+10, endPoint.y+15};
    rawPathNum +=3;
  } else {
    // 右侧绕障
    rawPath[rawPathNum-1] = {endPoint.x-10, endPoint.y-15};
    rawPath[rawPathNum] = {endPoint.x, endPoint.y-15};
    rawPath[rawPathNum+1] = {endPoint.x+10, endPoint.y-15};
    rawPathNum +=3;
  }
  rawPathNum = constrain(rawPathNum, 0, CONTROL_POINT_NUM);
}

// 贝塞尔曲线平滑路径
void smoothPathWithBezier() {
  smoothIndex =0;
  for (int i=0; i<rawPathNum-3; i++) {
    Point p0 = rawPath[i];
    Point p1 = rawPath[i+1];
    Point p2 = rawPath[i+2];
    Point p3 = rawPath[i+3];

    float k = (p1.x == p0.x || p2.x == p1.x) ? 0.0 :
abs((p1.x-p0.x)*(p2.y-p1.y) - (p1.y-p0.y)*(p2.x-p1.x)) /
              sqrt((p1.x-p0.x)*(p1.x-p0.x) + (p1.y-p0.y)*(p1.y-p0.y));k = constrain(k, 0.0, MAX_CURVATURE);

    for (int t=0; t<20; t++) {
      float tt = t/19.0; smoothPath[smoothIndex].x = p0.x + (p3.x-p0.x)*tt;
      smoothPath[smoothIndex].y = cubicBezier(tt, p0.y, p1.y, p2.y, p3.y);
      smoothIndex++;
    }
  }
}

// 沿平滑路径跟随
void followPath() {
  if (curPointIndex >= smoothIndex) {
    // 路径结束，循环或待机
    state = STATE_HOLD;
    return;
  }

  // 计算曲率调整速度
  float curvature = 0.0;
  if (curPointIndex >=2) {
    Point p0 = smoothPath[curPointIndex-2];
    Point p1 = smoothPath[curPointIndex-1];
    Point p2 = smoothPath[curPointIndex];float dx1 = p1.x-p0.x, dy1 = p1.y-p0.y;
    float dx2 = p2.x-p1.x, dy2 = p2.y-p1.y;
    float dot = dx1*dx2 + dy1*dy2;
    float cross = dx1*dy2 - dx2*dy1;
    float dist1 = sqrt(dx1*dx1+dy1*dy1);
    float dist2 = sqrt(dx2*dx2+dy2*dy2);
    if (dist1>0 && dist2>0 && dot>0) {
      curvature = abs(cross)/(dist1*dist2*dot);
    }
 curvature = constrain(curvature, 0, MAX_CURVATURE);
  }

  // 速度与转向控制
  float speed = map(curvature, 0, MAX_CURVATURE, 10, 4);
  // 简化为直行，实际可根据航向角差值调整差速
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(PWM_PIN, (int)speed);
  curPointIndex++;
}

// 三次贝塞尔曲线计算
float cubicBezier(float t, float p0, float p1, float p2, float p3) {
  float t2 = t*t, t3 = t2*t;
  float mt = 1-t, mt2 = mt*mt, mt3 = mt2*mt;
  return p0*mt3 + 3*p1*mt2*t + 3*p2*mt*t2 + p3*t3;
}

// 电机速度控制
void controlMotor(float speed) {
  speed = constrain(speed, 0, 12.0);
  analogWrite(PWM_PIN, (int)speed);
}

适用场景优化方向
可增加动态障碍检测，实现动态避障，适配车间移动设备等动态场景
可增加路径重规划机制，避障后重新生成平滑路径，保证巡检路线完整
可结合IMU与里程计，提升路径跟随精度，减少累积误差

要点解读
要点1：增量栅格地图需平衡“内存占用”与“探测精度”，适配Arduino算力限制
Arduino的内存与算力有限，工业巡检又要求一定的探测精度，栅格参数的平衡是关键。
分辨率匹配场景：工业管廊等窄空间，栅格分辨率建议5-10cm，兼顾精度与内存；开阔车间可适当降低精度，减少算力消耗
局部栅格优先：采用以机器人为中心的局部移动栅格，而非全局地图，大幅降低内存占用，适配Arduino的SRAM限制
增量更新优化：仅更新传感器探测范围内的栅格，避免全量计算，提升实时性

要点2：基础避障需设置“安全距+多方向探测”，保障工业场景避障可靠性
工业场景障碍复杂（管道、设备、临时堆放物），基础避障必须冗余可靠，避免漏检。
多传感器覆盖：至少配备前、左、右三向传感器，消除视野盲区，管廊等狭长场景可增加前后左右四向探测
安全距离冗余：障碍检测阈值需大于机器人制动距离，避免因响应延迟导致碰撞，移动速度越快，安全距越大
多级避障策略：优先左右绕行，受限则后退调整，避免在复杂环境中卡死，保证巡检连续性

要点3：贝塞尔曲线平滑需控制最大曲率，实现“无突变+无抖动”的平滑运动
平滑的核心不是“曲线美观”，而是曲率连续且不超过安全阈值，避免转角处速度突变与机械抖动。
三次贝塞尔最优：二次贝塞尔曲率不连续，高阶计算量大，三次贝塞尔兼顾曲率连续与计算效率，适配Arduino算力
曲率阈值约束：必须限制曲线最大曲率，且平滑前后曲率平滑过渡，避免因曲率突变导致电机频繁加减速、机械抖动
控制点优化：对原始路径点进行简化与优化，减少不必要的拐点，提升平滑质量，同时降低计算量

要点4：避障与平滑需无缝联动，避免“避障-平滑”脱节导致路径卡顿
两者联动的核心是路径实时更新、状态自动切换、反馈闭环，保证巡检流畅。
触发即平滑：避障生成的绕障路径必须立即进行贝塞尔平滑，不能直接执行折线，否则转角处顿挫严重
运动中更新：机器人沿平滑路径运动的同时，实时更新栅格地图，遇到新障碍立即重新避障、重新平滑，适配动态环境
状态无缝切换：从避障到平滑到跟随的状态切换要平滑，避免速度突变，保证运动连贯

要点5：工业场景需兼顾“路径平滑性”与“安全冗余”，避免过度平滑引发风险
工业巡检的第一优先级是安全，路径平滑不能以牺牲安全为代价，需做好冗余设计。
避障优先：当平滑路径与实际环境产生偏差时，必须优先执行避障，停止平滑，确保安全
速度自适应：曲率越大、障碍越近，速度越低，通过速度闭环保障制动距离与安全
异常兜底：传感器失效、路径规划异常时，立即停车并触发安全模式，避免机器人失控或碰撞

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