Arduino BLDC之室内服务机器人（三次B样条路径平滑+BLDC差速避障），是一套面向室内动态环境的高级运动控制方案。其核心思想是：利用三次B样条曲线将原本生硬、充满折线的全局路径转化为曲率连续、几何平滑的参考轨迹，并结合BLDC无刷电机卓越的动态响应特性，通过差速驱动实现丝滑的主动避障与轨迹跟踪。该方案彻底解决了传统服务机器人在狭窄空间内“走走停停、原地旋转”的顿挫感，显著提升了人机共存环境下的安全性、舒适度与运行效率。

一、主要特点
三次B样条路径平滑——从“折线爬行”到“曲线巡航”
传统室内机器人（如基于栅格地图A*算法）规划出的路径由一系列直线段和尖锐拐点组成，机器人必须在每个拐点处停车、旋转、再启动，效率极低且体验极差。三次B样条（Cubic B-Spline）的引入带来了质的飞跃：
曲率连续（G2连续）：三次B样条生成的轨迹不仅位置连续、切线连续，且曲率连续。这意味着机器人的角速度变化是平滑的，彻底消除了转向时的“顿挫感”和离心力突变。
局部支撑性：修改或插入一个控制顶点（Control Point），只会影响局部的曲线形状，而不会改变整条路径的走向。这一特性对于动态环境下的局部路径重规划至关重要，机器人在避障后能快速计算出平滑的回归轨迹。
多拐点平滑过渡：在复杂的室内环境（如L型走廊、S型办公桌通道）中，B样条能自动生成符合机器人最小转弯半径的平滑弧线，无需在拐角处停车。
BLDC差速动态避障——从“被动急停”到“主动绕行”
结合BLDC电机的高动态性能，机器人在遇到动态障碍物（如走动的人员）时，不再执行简单的“检测到障碍→刹车→等待→重启”逻辑，而是采用动态差速避障：
毫秒级扭矩响应：BLDC配合FOC（磁场定向控制）驱动器，电流环带宽可达千赫兹级。当超声波或激光雷达检测到侧前方障碍时，控制器能瞬间调整左右轮扭矩，实现类似汽车“打方向盘”的弧线绕行（Arc Avoidance）。
速度障碍法（VO）融合：系统实时计算障碍物的相对速度与位置，在速度空间中剔除会导致碰撞的速度向量，选取最优的平滑避障速度直接下发给电机。
低速大扭矩越障：在遇到地毯边缘、电线等低矮障碍时，BLDC的低速大扭矩特性可确保机器人不降速、不卡死，直接碾压通过。
运动学解耦与精准跟踪
解耦控制架构：上层规划器输出平滑的 (x, y, \theta)(x,y,θ) 期望轨迹，中层控制器（如Pure Pursuit或MPC）计算所需的线速度和角速度，底层BLDC驱动器通过编码器闭环精准执行差速指令。
无里程计打滑误差：BLDC运行平稳、无转矩脉动，大幅减少了因电机抖动导致的轮系微打滑，使得基于编码器积分的里程计（Odometry）在短距离内精度更高，轨迹跟踪误差可控制在厘米级。
静音与舒适性
室内服务机器人需在人耳边或脚边运行。BLDC的正弦波驱动消除了传统有刷电机的电刷摩擦声和齿轮箱的高频啸叫，配合B样条平滑路径消除了急停急转的惯性冲击声，整机运行噪音可控制在45dB以下。

二、典型应用场景
高端商用清洁机器人（洗地机/尘推）
在商场、机场、写字楼大堂等大面积、人流密集的场所，传统折线路径会导致清洁覆盖率不均且频繁惊扰行人。
B样条优势：生成弓字形（Cover-path）平滑覆盖路径，转弯处自动圆滑过渡，无需反复启停，清洁效率提升30%以上。
差速避障：遇到行人时，机器人沿弧线优雅绕行，保持安全距离后平滑回归原路径，水渍不留死角。
酒店/医院配送机器人
在狭窄的客房走廊或医院病床通道中，机器人与人的交会空间极其有限。
平滑绕行：利用B样条的局部调整特性，当检测到走廊一侧有障碍物（如行李车）时，机器人能迅速生成一条绕过障碍的平滑S型曲线，而非生硬地倒车或急停。
防泼洒：BLDC的平稳启动和B样条的低曲率转向，有效防止托盘上的餐食或医院的输液瓶、样本发生倾斜泼洒。
餐厅送餐机器人
餐厅环境桌椅密集、地面可能有防滑垫或线缆，且要求极高的通过效率。
窄通道通过：在桌椅间隙中，BLDC差速可实现零半径原地旋转微调，结合B样条路径，能在极小的空间内完成复杂的避障机动。
抗干扰：跨越地毯接缝或电线时，BLDC瞬间扭矩补偿防止卡滞。
家庭陪伴/安防巡检机器人
在家庭环境中，机器人需要长时间自主运行，且不能惊吓到老人、儿童或宠物。
静音巡航：BLDC的低噪特性适合夜间或午休时段工作。
宠物防碰撞：结合ToF传感器，差速避障算法能让机器人在高速运动中与跑动的宠物保持动态安全距离。
科研与算法验证平台
作为高校或企业研发部门验证“非线性路径规划”、“模型预测控制（MPC）”、“动态窗口法（DWA）”等高级算法的理想载体。Arduino生态提供了丰富的库支持（如SimpleFOC），便于快速部署验证。

三、需要注意的事项
算力瓶颈与算法轻量化
三次B样条涉及浮点运算和矩阵计算，动态避障也需要高频的传感器数据处理。
Arduino选型：Arduino Uno（AVR架构，无FPU）算力严重不足，无法实时解算复杂B样条。必须选用ESP32（双核240MHz）、STM32（F4/H7系列）或Arduino Portenta H7。
离线/在线结合：建议将全局B样条拟合放在上位机（或初始化阶段）完成，Arduino仅负责局部的插值计算（De Boor算法）和实时差速指令下发。
差速运动的里程计漂移
差速驱动在长距离运行或高速转弯时，由于轮子打滑或轮径微小差异，累积误差（Drift）极大。
传感器融合：必须引入IMU（惯性测量单元，如MPU6050/ICM20948），通过扩展卡尔曼滤波（EKF）融合编码器数据与陀螺仪航向角，修正 \thetaθ 的漂移。
绝对定位校正：在关键节点（如走廊交叉口）利用激光雷达或视觉地标进行绝对位置校正。
BLDC参数匹配与调参
差速避障要求左右电机具有高度一致的动态响应特性。
电机一致性：左右BLDC电机应选用同批次产品，并精确测量电机内阻、电感、极对数，写入FOC驱动板。
PID整定：速度环和位置环的PID参数必须精细整定。若响应过慢，避障会迟钝；若响应过快，会产生超调和震荡。
最小转弯半径限制：在生成B样条曲线时，必须将曲线的最大曲率限制在机器人物理最小转弯半径之内，否则底层电机即便满转也无法跟踪轨迹。
动态避障的安全边界与震荡抑制
迟滞区间（Hysteresis）：简单的阈值判断（如<0.5m就避障）会导致机器人在障碍物边缘反复“犹豫”和左右摇摆。必须引入迟滞逻辑或时间滤波，确保避障决策的稳定性。
速度自适应：B样条路径的平滑度应与当前速度匹配。高速运行时，需提前规划更长距离的平滑曲线；低速狭窄空间则可采用更激进的局部绕行。
电源管理与电磁兼容（EMC）
室内环境对电磁干扰敏感，且BLDC瞬态电流大。
独立供电：BLDC驱动电源（12V/24V）与Arduino/传感器逻辑电源（5V/3.3V）必须物理隔离，仅单点共地，防止电机换相噪声导致MCU复位或传感器误报。
电容滤波：电源入口并联大容量电解电容（>470μF）和高频陶瓷电容，吸收电压尖峰。
路径平滑与地图更新
B样条虽然平滑，但可能会“切角”，即在狭窄走廊拐角处，平滑曲线可能会侵入未知区域或撞上墙角。
膨胀半径（Inflation Radius）：在生成B样条前，必须对地图中的障碍物进行形态学膨胀，确保平滑后的曲线与真实障碍物之间仍保留足够的安全余量。
实时碰撞检测：在曲线生成的每一步，都需对曲线上的采样点进行碰撞校验，一旦发现侵入障碍物，需立即插入新的控制点重新拟合。

1、基础B样条路径规划与避障服务机器人

#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>

// BLDC电机控制引脚
#define MOTOR_L_PWM 9
#define MOTOR_L_DIR 8
#define MOTOR_R_PWM 10
#define MOTOR_R_DIR 11
#define MOTOR_L_EN 12
#define MOTOR_R_EN 13

// 超声波传感器阵列
#define US_FRONT_TRIG 22
#define US_FRONT_ECHO 23
#define US_LEFT_TRIG 24
#define US_LEFT_ECHO 25
#define US_RIGHT_TRIG 26
#define US_RIGHT_ECHO 27

// 编码器
#define ENC_L_A 2
#define ENC_L_B 3
#define ENC_R_A 18
#define ENC_R_B 19

// 机器人参数
#define WHEEL_RADIUS 3.25      // 轮子半径(cm)
#define WHEEL_BASE 20.0        // 轮距(cm)
#define ENCODER_PPR 600        // 编码器脉冲数
#define GEAR_RATIO 30          // 减速比

// B样条路径参数
#define MAX_CONTROL_POINTS 20
#define BSPLINE_ORDER 3        // 三次B样条
#define KNOT_VECTOR_SIZE (MAX_CONTROL_POINTS + BSPLINE_ORDER + 1)

// 路径点结构
struct PathPoint {
    float x;
    float y;
    float heading;
    float velocity;
};

// B样条控制点
struct ControlPoint {
    float x;
    float y;
    float weight;
};

class ServiceRobot {
private:
    // 位置估计
    float posX, posY, posTheta;
    float velLeft, velRight;
    
    // 编码器计数
    volatile long encLCount, encRCount;
    long lastEncL, lastEncR;
    
    // 避障参数
    float obstacleDistFront;
    float obstacleDistLeft;
    float obstacleDistRight;
    
    // B样条路径
    ControlPoint controlPoints[MAX_CONTROL_POINTS];
    PathPoint smoothPath[100];  // 平滑后的路径
    int numControlPoints;
    int numPathPoints;
    float knotVector[KNOT_VECTOR_SIZE];
    
    // PID控制
    float Kp_dist, Ki_dist, Kd_dist;
    float Kp_angle, Ki_angle, Kd_angle;
    float errorDistSum, lastErrorDist;
    float errorAngleSum, lastErrorAngle;
    
    // 目标
    PathPoint targetPoint;
    int currentPathIndex;
    
public:
    ServiceRobot() {
        posX = 0;
        posY = 0;
        posTheta = 0;
        velLeft = 0;
        velRight = 0;
        encLCount = 0;
        encRCount = 0;
        numControlPoints = 0;
        numPathPoints = 0;
        currentPathIndex = 0;
        
        // PID参数初始化
        Kp_dist = 1.5;
        Ki_dist = 0.02;
        Kd_dist = 0.3;
        Kp_angle = 2.0;
        Ki_angle = 0.05;
        Kd_angle = 0.4;
    }
    
    void initialize() {
        Serial.begin(115200);
        Serial.println("室内服务机器人启动");
        
        initMotors();
        initSensors();
        initEncoders();
        
        // 初始化B样条节点向量
        initKnotVector();
        
        Serial.println("系统初始化完成");
    }
    
    void initMotors() {
        pinMode(MOTOR_L_PWM, OUTPUT);
        pinMode(MOTOR_L_DIR, OUTPUT);
        pinMode(MOTOR_R_PWM, OUTPUT);
        pinMode(MOTOR_R_DIR, OUTPUT);
        pinMode(MOTOR_L_EN, OUTPUT);
        pinMode(MOTOR_R_EN, OUTPUT);
        
        digitalWrite(MOTOR_L_EN, HIGH);
        digitalWrite(MOTOR_R_EN, HIGH);
    }
    
    void initSensors() {
        pinMode(US_FRONT_TRIG, OUTPUT);
        pinMode(US_FRONT_ECHO, INPUT);
        pinMode(US_LEFT_TRIG, OUTPUT);
        pinMode(US_LEFT_ECHO, INPUT);
        pinMode(US_RIGHT_TRIG, OUTPUT);
        pinMode(US_RIGHT_ECHO, INPUT);
    }
    
    void initEncoders() {
        pinMode(ENC_L_A, INPUT_PULLUP);
        pinMode(ENC_L_B, INPUT_PULLUP);
        pinMode(ENC_R_A, INPUT_PULLUP);
        pinMode(ENC_R_B, INPUT_PULLUP);
        
        attachInterrupt(digitalPinToInterrupt(ENC_L_A), encLISR, CHANGE);
        attachInterrupt(digitalPinToInterrupt(ENC_R_A), encRISR, CHANGE);
    }
    
    void initKnotVector() {
        // 均匀B样条节点向量
        for(int i = 0; i < KNOT_VECTOR_SIZE; i++) {
            knotVector[i] = i;
        }
    }
    
    void run() {
        // 读取传感器
        readObstacleSensors();
        
        // 更新位置
        updateOdometry();
        
        // 检查是否需要重新规划路径
        if(isObstacleDetected()) {
            replanPath();
        }
        
        // 沿平滑路径导航
        followSmoothPath();
        
        // 执行服务任务
        executeServiceTask();
        
        delay(50);
    }
    
    // 设置B样条控制点
    void setControlPoints(ControlPoint* points, int count) {
        numControlPoints = min(count, MAX_CONTROL_POINTS);
        for(int i = 0; i < numControlPoints; i++) {
            controlPoints[i] = points[i];
        }
        
        // 生成平滑路径
        generateSmoothPath();
    }
    
    // 生成三次B样条平滑路径
    void generateSmoothPath() {
        numPathPoints = 0;
        
        if(numControlPoints < BSPLINE_ORDER + 1) return;
        
        // 对每个参数区间计算B样条曲线
        for(int i = BSPLINE_ORDER; i <= numControlPoints; i++) {
            for(int j = 0; j < 20; j++) {
                float t = (float)j / 20.0;  // 参数t在[0,1]之间
                float u = knotVector[i] + t * (knotVector[i+1] - knotVector[i]);
                
                // 计算B样条基函数
                float basis[MAX_CONTROL_POINTS];
                calculateBSplineBasis(u, basis);
                
                // 计算曲线点
                PathPoint point;
                point.x = 0;
                point.y = 0;
                point.velocity = 0.3;  // 默认速度
                
                for(int k = 0; k < numControlPoints; k++) {
                    point.x += basis[k] * controlPoints[k].x;
                    point.y += basis[k] * controlPoints[k].y;
                }
                
                // 计算航向角
                if(numPathPoints > 0) {
                    float dx = point.x - smoothPath[numPathPoints-1].x;
                    float dy = point.y - smoothPath[numPathPoints-1].y;
                    point.heading = atan2(dy, dx);
                } else {
                    point.heading = 0;
                }
                
                // 存储路径点
                if(numPathPoints < 100) {
                    smoothPath[numPathPoints++] = point;
                }
            }
        }
        
        currentPathIndex = 0;
        Serial.print("生成平滑路径，共");
        Serial.print(numPathPoints);
        Serial.println("个路径点");
    }
    
    // 计算B样条基函数（de Boor算法）
    void calculateBSplineBasis(float u, float* basis) {
        // 初始化
        for(int i = 0; i < numControlPoints; i++) {
            basis[i] = 0;
        }
        
        // 找到u所在的节点区间
        int span = BSPLINE_ORDER;
        for(int i = BSPLINE_ORDER; i <= numControlPoints; i++) {
            if(u >= knotVector[i] && u < knotVector[i+1]) {
                span = i;
                break;
            }
        }
        
        // 使用递归定义计算基函数
        float N[MAX_CONTROL_POINTS][BSPLINE_ORDER + 1];
        
        // 0阶基函数
        for(int i = 0; i <= numControlPoints; i++) {
            if(u >= knotVector[i] && u < knotVector[i+1]) {
                N[i][0] = 1.0;
            } else {
                N[i][0] = 0.0;
            }
        }
        
        // 计算高阶基函数
        for(int p = 1; p <= BSPLINE_ORDER; p++) {
            for(int i = 0; i <= numControlPoints - p; i++) {
                float denom1 = knotVector[i+p] - knotVector[i];
                float denom2 = knotVector[i+p+1] - knotVector[i+1];
                
                float term1 = 0, term2 = 0;
                
                if(denom1 != 0) {
                    term1 = (u - knotVector[i]) / denom1 * N[i][p-1];
                }
                
                if(denom2 != 0) {
                    term2 = (knotVector[i+p+1] - u) / denom2 * N[i+1][p-1];
                }
                
                N[i][p] = term1 + term2;
            }
        }
        
        // 提取基函数值
        for(int i = 0; i < numControlPoints; i++) {
            basis[i] = N[i][BSPLINE_ORDER];
        }
    }
    
    // 跟随平滑路径
    void followSmoothPath() {
        if(currentPathIndex >= numPathPoints) {
            stopMotors();
            return;
        }
        
        targetPoint = smoothPath[currentPathIndex];
        
        // 计算与目标点的距离和角度误差
        float dx = targetPoint.x - posX;
        float dy = targetPoint.y - posY;
        float distError = sqrt(dx*dx + dy*dy);
        float targetAngle = atan2(dy, dx);
        float angleError = targetAngle - posTheta;
        
        // 角度归一化
        while(angleError > PI) angleError -= 2*PI;
        while(angleError < -PI) angleError += 2*PI;
        
        // PID控制
        float angularCorrection = Kp_angle * angleError + 
                                 Ki_angle * errorAngleSum + 
                                 Kd_angle * (angleError - lastErrorAngle);
        errorAngleSum += angleError;
        lastErrorAngle = angleError;
        
        float linearVelocity = targetPoint.velocity;
        
        // 差速驱动
        float leftSpeed = linearVelocity - angularCorrection * WHEEL_BASE / 2;
        float rightSpeed = linearVelocity + angularCorrection * WHEEL_BASE / 2;
        
        // 转换为PWM值
        int leftPWM = velocityToPWM(leftSpeed);
        int rightPWM = velocityToPWM(rightSpeed);
        
        driveMotors(leftPWM, rightPWM);
        
        // 到达路径点
        if(distError < 2.0) {  // 2cm到达阈值
            currentPathIndex++;
        }
    }
    
    // 重新规划路径（避障）
    void replanPath() {
        Serial.println("检测到障碍物，重新规划路径");
        
        // 基于当前障碍物位置调整控制点
        ControlPoint newPoints[MAX_CONTROL_POINTS];
        int newCount = 0;
        
        // 保留当前位置作为起点
        newPoints[newCount].x = posX;
        newPoints[newCount].y = posY;
        newPoints[newCount].weight = 1.0;
        newCount++;
        
        // 根据障碍物方向添加避障点
        if(obstacleDistFront < 30) {
            // 前方有障碍物，添加侧面避障点
            if(obstacleDistLeft > obstacleDistRight) {
                // 向左避障
                newPoints[newCount].x = posX + 20;
                newPoints[newCount].y = posY + 30;
                newPoints[newCount].weight = 1.0;
                newCount++;
            } else {
                // 向右避障
                newPoints[newCount].x = posX + 20;
                newPoints[newCount].y = posY - 30;
                newPoints[newCount].weight = 1.0;
                newCount++;
            }
        }
        
        // 添加原始目标点
        newPoints[newCount].x = targetPoint.x;
        newPoints[newCount].y = targetPoint.y;
        newPoints[newCount].weight = 1.0;
        newCount++;
        
        // 重新设置控制点
        setControlPoints(newPoints, newCount);
    }
    
    // 读取障碍物传感器
    void readObstacleSensors() {
        obstacleDistFront = readUltrasonic(US_FRONT_TRIG, US_FRONT_ECHO);
        obstacleDistLeft = readUltrasonic(US_LEFT_TRIG, US_LEFT_ECHO);
        obstacleDistRight = readUltrasonic(US_RIGHT_TRIG, US_RIGHT_ECHO);
    }
    
    float readUltrasonic(int trigPin, int echoPin) {
        digitalWrite(trigPin, LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin, LOW);
        
        long duration = pulseIn(echoPin, HIGH, 30000);
        if(duration == 0) return 200;  // 未检测到障碍物
        
        float distance = duration * 0.034 / 2;
        return distance;
    }
    
    bool isObstacleDetected() {
        return obstacleDistFront < 25 || 
               obstacleDistLeft < 15 || 
               obstacleDistRight < 15;
    }
    
    // 更新里程计
    void updateOdometry() {
        long deltaEncL = encLCount - lastEncL;
        long deltaEncR = encRCount - lastEncR;
        lastEncL = encLCount;
        lastEncR = encRCount;
        
        // 计算轮子速度
        float wheelCirc = 2 * PI * WHEEL_RADIUS;
        float distL = (deltaEncL / (float)(ENCODER_PPR * GEAR_RATIO)) * wheelCirc;
        float distR = (deltaEncR / (float)(ENCODER_PPR * GEAR_RATIO)) * wheelCirc;
        
        // 更新位置
        float deltaDist = (distL + distR) / 2;
        float deltaTheta = (distR - distL) / WHEEL_BASE;
        
        posX += deltaDist * cos(posTheta + deltaTheta/2);
        posY += deltaDist * sin(posTheta + deltaTheta/2);
        posTheta += deltaTheta;
        
        // 角度归一化
        while(posTheta > PI) posTheta -= 2*PI;
        while(posTheta < -PI) posTheta += 2*PI;
    }
    
    // 速度转PWM
    int velocityToPWM(float velocity) {
        // 速度范围：-0.5 到 0.5 m/s
        float maxVelocity = 0.5;
        int pwm = (int)(velocity / maxVelocity * 255);
        return constrain(pwm, -255, 255);
    }
    
    void driveMotors(int leftPWM, int rightPWM) {
        // 左电机
        if(leftPWM >= 0) {
            digitalWrite(MOTOR_L_DIR, HIGH);
            analogWrite(MOTOR_L_PWM, leftPWM);
        } else {
            digitalWrite(MOTOR_L_DIR, LOW);
            analogWrite(MOTOR_L_PWM, -leftPWM);
        }
        
        // 右电机
        if(rightPWM >= 0) {
            digitalWrite(MOTOR_R_DIR, HIGH);
            analogWrite(MOTOR_R_PWM, rightPWM);
        } else {
            digitalWrite(MOTOR_R_DIR, LOW);
            analogWrite(MOTOR_R_PWM, -rightPWM);
        }
    }
    
    void stopMotors() {
        analogWrite(MOTOR_L_PWM, 0);
        analogWrite(MOTOR_R_PWM, 0);
    }
    
    void executeServiceTask() {
        // 服务任务执行
        static unsigned long lastTaskTime = 0;
        
        if(millis() - lastTaskTime > 5000) {  // 每5秒执行一次
            lastTaskTime = millis();
            
            Serial.print("位置: (");
            Serial.print(posX);
            Serial.print(", ");
            Serial.print(posY);
            Serial.print(") 航向: ");
            Serial.print(posTheta * 180 / PI);
            Serial.print("° 前方障碍: ");
            Serial.print(obstacleDistFront);
            Serial.println("cm");
        }
    }
    
    // 编码器中断
    void encLISR() {
        if(digitalRead(ENC_L_B)) encLCount++;
        else encLCount--;
    }
    
    void encRISR() {
        if(digitalRead(ENC_R_B)) encRCount++;
        else encRCount--;
    }
};

ServiceRobot robot;

// 中断服务程序
void encLISR() {
    robot.encLISR();
}

void encRISR() {
    robot.encRISR();
}

void setup() {
    robot.initialize();
    
    // 设置初始控制点（示例路径）
    ControlPoint initialPoints[] = {
        {0, 0, 1.0},     // 起点
        {50, 20, 1.0},   // 路径点1
        {100, 50, 1.0},  // 路径点2
        {150, 80, 1.0},  // 路径点3
        {200, 100, 1.0}  // 终点
    };
    
    robot.setControlPoints(initialPoints, 5);
}

void loop() {
    robot.run();
}

2、高级动态避障与B样条实时重规划

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

// BLDC电机高级控制
#define PWM_FREQUENCY 20000  // 20kHz PWM频率
#define MOTOR_L_PWM 9
#define MOTOR_L_DIR 8
#define MOTOR_R_PWM 10
#define MOTOR_R_DIR 11

// 编码器
#define ENC_L_A 2
#define ENC_L_B 3
#define ENC_R_A 18
#define ENC_R_B 19

// 激光雷达（模拟）
#define LIDAR_PIN A0
#define LIDAR_SERVO 6

// 陀螺仪
#define GYRO_ADDR 0x68
#define GYRO_XOUT 0x43

// 机器人运动学
#define WHEEL_RADIUS 3.5
#define WHEEL_BASE 22.0
#define MAX_VELOCITY 0.8  // m/s
#define MAX_ANGULAR_VEL 2.0  // rad/s

// B样条参数
#define BSPLINE_DEGREE 3
#define MAX_CTRL_POINTS 30
#define PATH_RESOLUTION 50  // 每个控制点间隔的采样数

struct Obstacle {
    float x, y;
    float radius;
    float velocity_x, velocity_y;
    bool isDynamic;
};

struct BSplinePath {
    float controlX[MAX_CTRL_POINTS];
    float controlY[MAX_CTRL_POINTS];
    float knots[MAX_CTRL_POINTS + BSPLINE_DEGREE + 1];
    int numPoints;
};

class AdvancedServiceRobot {
private:
    // 状态估计
    float robotX, robotY, robotTheta;
    float robotVx, robotVy, robotOmega;
    
    // 编码器
    volatile long encoderL, encoderR;
    float wheelVelL, wheelVelR;
    
    // 障碍物列表
    Obstacle obstacles[10];
    int numObstacles;
    
    // 路径规划
    BSplinePath currentPath;
    BSplinePath replannedPath;
    float pathParameter;
    int currentSegment;
    
    // PID控制器
    struct PIDController {
        float Kp, Ki, Kd;
        float integral, lastError;
        float outputMin, outputMax;
    };
    
    PIDController velocityPID;
    PIDController headingPID;
    
    // 安全参数
    float minObstacleDist;
    float emergencyStopDist;
    bool emergencyFlag;
    
public:
    AdvancedServiceRobot() {
        robotX = 0;
        robotY = 0;
        robotTheta = 0;
        pathParameter = 0;
        currentSegment = 0;
        numObstacles = 0;
        minObstacleDist = 30.0;  // 最小障碍物距离
        emergencyStopDist = 10.0;  // 紧急停止距离
        emergencyFlag = false;
        
        // 初始化PID
        velocityPID = {2.0, 0.1, 0.05, 0, 0, -MAX_VELOCITY, MAX_VELOCITY};
        headingPID = {3.0, 0.02, 0.1, 0, 0, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL};
    }
    
    void initialize() {
        Serial.begin(115200);
        
        // 设置PWM频率
        setPWMFrequency(PWM_FREQUENCY);
        
        initMotors();
        initEncoders();
        initGyro();
        
        // 初始化B样条节点向量
        initBSplineKnots();
        
        Serial.println("高级服务机器人初始化完成");
    }
    
    void setPWMFrequency(int freq) {
        // 设置Timer1的PWM频率
        TCCR1B = (TCCR1B & 0xF8) | 0x01;  // 预分频1
        ICR1 = 16000000 / freq;
        TCCR1A |= _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
        TCCR1B |= _BV(WGM13) | _BV(WGM12);
    }
    
    void initMotors() {
        pinMode(MOTOR_L_PWM, OUTPUT);
        pinMode(MOTOR_L_DIR, OUTPUT);
        pinMode(MOTOR_R_PWM, OUTPUT);
        pinMode(MOTOR_R_DIR, OUTPUT);
    }
    
    void initEncoders() {
        pinMode(ENC_L_A, INPUT_PULLUP);
        pinMode(ENC_L_B, INPUT_PULLUP);
        pinMode(ENC_R_A, INPUT_PULLUP);
        pinMode(ENC_R_B, INPUT_PULLUP);
        
        attachInterrupt(digitalPinToInterrupt(ENC_L_A), []() {
            if(digitalRead(ENC_L_B)) robot.encoderL++;
            else robot.encoderL--;
        }, CHANGE);
        
        attachInterrupt(digitalPinToInterrupt(ENC_R_A), []() {
            if(digitalRead(ENC_R_B)) robot.encoderR++;
            else robot.encoderR--;
        }, CHANGE);
    }
    
    void initGyro() {
        Wire.begin();
        Wire.beginTransmission(GYRO_ADDR);
        Wire.write(0x6B);
        Wire.write(0);
        Wire.endTransmission(true);
    }
    
    void initBSplineKnots() {
        int n = MAX_CTRL_POINTS;
        int p = BSPLINE_DEGREE;
        
        for(int i = 0; i < n + p + 1; i++) {
            currentPath.knots[i] = i;
            replannedPath.knots[i] = i;
        }
    }
    
    void run() {
        // 传感器更新
        updateSensors();
        
        // 障碍物检测
        detectObstacles();
        
        // 路径跟踪
        trackPath();
        
        // 动态避障检查
        if(checkDynamicCollision()) {
            dynamicAvoidance();
        }
        
        // 紧急停止检查
        if(checkEmergencyStop()) {
            executeEmergencyStop();
        }
        
        delay(20);
    }
    
    void updateSensors() {
        // 读取陀螺仪
        readGyro();
        
        // 更新里程计
        updateOdometry();
        
        // 读取激光雷达数据
        readLidar();
    }
    
    void readGyro() {
        Wire.beginTransmission(GYRO_ADDR);
        Wire.write(GYRO_XOUT);
        Wire.endTransmission(false);
        Wire.requestFrom(GYRO_ADDR, 6, true);
        
        int16_t gyroX = (Wire.read() << 8) | Wire.read();
        int16_t gyroY = (Wire.read() << 8) | Wire.read();
        int16_t gyroZ = (Wire.read() << 8) | Wire.read();
        
        // 更新角速度
        robotOmega = gyroZ / 131.0 * PI / 180.0;  // 转换为rad/s
    }
    
    void updateOdometry() {
        static unsigned long lastTime = 0;
        unsigned long currentTime = micros();
        float dt = (currentTime - lastTime) / 1000000.0;
        lastTime = currentTime;
        
        // 计算轮子速度
        float wheelCirc = 2 * PI * WHEEL_RADIUS;
        wheelVelL = (encoderL * wheelCirc) / (600 * 30 * dt);
        wheelVelR = (encoderR * wheelCirc) / (600 * 30 * dt);
        
        // 重置编码器计数
        encoderL = 0;
        encoderR = 0;
        
        // 计算机器人速度
        robotVx = (wheelVelR + wheelVelL) / 2 * cos(robotTheta);
        robotVy = (wheelVelR + wheelVelL) / 2 * sin(robotTheta);
        
        // 更新位置
        robotX += robotVx * dt;
        robotY += robotVy * dt;
        robotTheta += robotOmega * dt;
        
        // 角度归一化
        while(robotTheta > PI) robotTheta -= 2*PI;
        while(robotTheta < -PI) robotTheta += 2*PI;
    }
    
    void readLidar() {
        // 模拟激光雷达数据
        // 在实际应用中，这里会读取真实的激光雷达数据
        int lidarValue = analogRead(LIDAR_PIN);
        float distance = map(lidarValue, 0, 1023, 10, 200);  // 10cm到200cm
        
        // 更新障碍物信息
        if(distance < minObstacleDist) {
            addObstacle(robotX + distance * cos(robotTheta),
                       robotY + distance * sin(robotTheta),
                       5.0, 0, 0, false);
        }
    }
    
    void detectObstacles() {
        // 使用超声波和激光雷达数据更新障碍物列表
        // 这里简化处理，实际应用需要更复杂的传感器融合
    }
    
    void addObstacle(float x, float y, float radius, 
                    float velX, float velY, bool isDynamic) {
        if(numObstacles < 10) {
            obstacles[numObstacles].x = x;
            obstacles[numObstacles].y = y;
            obstacles[numObstacles].radius = radius;
            obstacles[numObstacles].velocity_x = velX;
            obstacles[numObstacles].velocity_y = velY;
            obstacles[numObstacles].isDynamic = isDynamic;
            numObstacles++;
        }
    }
    
    // 生成B样条路径
    void generatePath(BSplinePath& path, float* ctrlX, float* ctrlY, int numCtrl) {
        path.numPoints = numCtrl;
        for(int i = 0; i < numCtrl; i++) {
            path.controlX[i] = ctrlX[i];
            path.controlY[i] = ctrlY[i];
        }
    }
    
    // 计算B样条曲线点
    void getBSplinePoint(BSplinePath& path, float t, float& x, float& y) {
        int n = path.numPoints;
        int p = BSPLINE_DEGREE;
        
        // 找到t所在的节点区间
        int span = p;
        for(int i = p; i <= n; i++) {
            if(t >= path.knots[i] && t < path.knots[i+1]) {
                span = i;
                break;
            }
        }
        
        // 使用de Boor算法计算
        float d[MAX_CTRL_POINTS];
        for(int j = 0; j <= p; j++) {
            d[j] = path.controlX[span - p + j];
        }
        
        for(int r = 1; r <= p; r++) {
            for(int j = p; j >= r; j--) {
                float alpha = (t - path.knots[span - p + j]) / 
                             (path.knots[span + j - r + 1] - path.knots[span - p + j]);
                d[j] = (1 - alpha) * d[j-1] + alpha * d[j];
            }
        }
        x = d[p];
        
        // 对Y坐标重复相同计算
        for(int j = 0; j <= p; j++) {
            d[j] = path.controlY[span - p + j];
        }
        
        for(int r = 1; r <= p; r++) {
            for(int j = p; j >= r; j--) {
                float alpha = (t - path.knots[span - p + j]) / 
                             (path.knots[span + j - r + 1] - path.knots[span - p + j]);
                d[j] = (1 - alpha) * d[j-1] + alpha * d[j];
            }
        }
        y = d[p];
    }
    
    void trackPath() {
        if(emergencyFlag) return;
        
        // 获取当前路径点
        float targetX, targetY;
        getBSplinePoint(currentPath, pathParameter, targetX, targetY);
        
        // 计算误差
        float dx = targetX - robotX;
        float dy = targetY - robotY;
        float distError = sqrt(dx*dx + dy*dy);
        float targetAngle = atan2(dy, dx);
        float angleError = targetAngle - robotTheta;
        
        // 角度归一化
        while(angleError > PI) angleError -= 2*PI;
        while(angleError < -PI) angleError += 2*PI;
        
        // PID控制
        float linearVel = velocityPID.Kp * distError;
        float angularVel = headingPID.Kp * angleError;
        
        // 限制速度
        linearVel = constrain(linearVel, -MAX_VELOCITY, MAX_VELOCITY);
        angularVel = constrain(angularVel, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL);
        
        // 差速驱动
        float leftVel = linearVel - angularVel * WHEEL_BASE / 2;
        float rightVel = linearVel + angularVel * WHEEL_BASE / 2;
        
        // 转换为PWM
        int leftPWM = (int)(leftVel / MAX_VELOCITY * 255);
        int rightPWM = (int)(rightVel / MAX_VELOCITY * 255);
        
        driveMotors(leftPWM, rightPWM);
        
        // 更新路径参数
        if(distError < 3.0) {  // 到达路径点
            pathParameter += 0.01;
            if(pathParameter >= currentPath.knots[currentPath.numPoints]) {
                pathParameter = currentPath.knots[BSPLINE_DEGREE];
            }
        }
    }
    
    bool checkDynamicCollision() {
        // 检查与动态障碍物的碰撞
        for(int i = 0; i < numObstacles; i++) {
            if(!obstacles[i].isDynamic) continue;
            
            // 预测碰撞
            float dx = obstacles[i].x - robotX;
            float dy = obstacles[i].y - robotY;
            float dist = sqrt(dx*dx + dy*dy);
            
            float relVelX = robotVx - obstacles[i].velocity_x;
            float relVelY = robotVy - obstacles[i].velocity_y;
            float relSpeed = sqrt(relVelX*relVelX + relVelY*relVelY);
            
            if(relSpeed > 0) {
                float timeToCollision = dist / relSpeed;
                if(timeToCollision < 3.0) {  // 3秒内可能碰撞
                    return true;
                }
            }
        }
        return false;
    }
    
    void dynamicAvoidance() {
        Serial.println("动态避障激活");
        
        // 基于速度障碍法重新规划路径
        replanWithVelocityObstacles();
        
        // 切换到新路径
        currentPath = replannedPath;
        pathParameter = currentPath.knots[BSPLINE_DEGREE];
    }
    
    void replanWithVelocityObstacles() {
        // 生成避障控制点
        float newCtrlX[MAX_CTRL_POINTS];
        float newCtrlY[MAX_CTRL_POINTS];
        int numNewCtrl = 0;
        
        // 起点
        newCtrlX[numNewCtrl] = robotX;
        newCtrlY[numNewCtrl] = robotY;
        numNewCtrl++;
        
        // 避障点
        for(int i = 0; i < numObstacles; i++) {
            float dx = obstacles[i].x - robotX;
            float dy = obstacles[i].y - robotY;
            float dist = sqrt(dx*dx + dy*dy);
            float angle = atan2(dy, dx);
            
            // 添加绕行点
            float avoidAngle = angle + PI/2;  // 垂直方向绕行
            newCtrlX[numNewCtrl] = obstacles[i].x + 
                                   (obstacles[i].radius + 20) * cos(avoidAngle);
            newCtrlY[numNewCtrl] = obstacles[i].y + 
                                   (obstacles[i].radius + 20) * sin(avoidAngle);
            numNewCtrl++;
        }
        
        // 原始目标点
        newCtrlX[numNewCtrl] = currentPath.controlX[currentPath.numPoints-1];
        newCtrlY[numNewCtrl] = currentPath.controlY[currentPath.numPoints-1];
        numNewCtrl++;
        
        // 生成新路径
        generatePath(replannedPath, newCtrlX, newCtrlY, numNewCtrl);
    }
    
    bool checkEmergencyStop() {
        // 检查是否需要紧急停止
        float minDist = 999;
        for(int i = 0; i < numObstacles; i++) {
            float dx = obstacles[i].x - robotX;
            float dy = obstacles[i].y - robotY;
            float dist = sqrt(dx*dx + dy*dy);
            if(dist < minDist) minDist = dist;
        }
        
        return minDist < emergencyStopDist;
    }
    
    void executeEmergencyStop() {
        emergencyFlag = true;
        stopMotors();
        Serial.println("紧急停止！");
        
        // 等待障碍物清除
        delay(3000);
        emergencyFlag = false;
    }
    
    void driveMotors(int leftPWM, int rightPWM) {
        leftPWM = constrain(leftPWM, -255, 255);
        rightPWM = constrain(rightPWM, -255, 255);
        
        // 使用16位PWM
        OCR1A = abs(leftPWM) * 2;
        OCR1B = abs(rightPWM) * 2;
        
        digitalWrite(MOTOR_L_DIR, leftPWM > 0);
        digitalWrite(MOTOR_R_DIR, rightPWM > 0);
    }
    
    void stopMotors() {
        OCR1A = 0;
        OCR1B = 0;
        digitalWrite(MOTOR_L_DIR, LOW);
        digitalWrite(MOTOR_R_DIR, LOW);
    }
    
    // 服务任务执行
    void executeServiceTasks() {
        // 检查是否到达服务点
        if(isAtServicePoint()) {
            performService();
        }
    }
    
    bool isAtServicePoint() {
        // 简化的服务点检测
        float dx = robotX - 100;  // 假设服务点在(100, 100)
        float dy = robotY - 100;
        return sqrt(dx*dx + dy*dy) < 5.0;
    }
    
    void performService() {
        Serial.println("到达服务点，执行服务任务");
        // 执行具体的服务任务
        delay(5000);  // 模拟任务执行
    }
};

AdvancedServiceRobot robot;

void setup() {
    robot.initialize();
    
    // 设置初始路径
    float ctrlX[] = {0, 30, 60, 90, 120, 150};
    float ctrlY[] = {0, 20, 50, 80, 100, 100};
    robot.generatePath(robot.currentPath, ctrlX, ctrlY, 6);
}

void loop() {
    robot.run();
    robot.executeServiceTasks();
}

3、多机器人协同服务与智能路径优化

#include <Arduino.h>
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>

// 无线通信
#define RF_CE_PIN 7
#define RF_CSN_PIN 8
RF24 radio(RF_CE_PIN, RF_CSN_PIN);
const byte address[6] = "SERVR";

// BLDC电机
#define MOTOR_L_PWM 9
#define MOTOR_L_DIR 10
#define MOTOR_R_PWM 11
#define MOTOR_R_DIR 12

// 传感器
#define IR_FRONT A0
#define IR_LEFT A1
#define IR_RIGHT A2
#define IMU_INT 2

// 机器人参数
#define WHEEL_RADIUS 3.2
#define WHEEL_BASE 18.5
#define MAX_SPEED 200
#define MIN_SPEED 50

// B样条参数
#define BSPLINE_ORDER 3
#define MAX_CONTROL 25
#define PATH_POINTS 200

// 通信数据结构
struct RobotState {
    uint8_t robotId;
    float posX, posY, theta;
    float velX, velY;
    uint8_t status;
    uint32_t timestamp;
};

struct TaskMessage {
    uint8_t taskType;
    float targetX, targetY;
    uint8_t priority;
    uint32_t deadline;
};

class CollaborativeRobot {
private:
    uint8_t robotId;
    RobotState myState;
    RobotState peerStates[5];
    int numPeers;
    
    // 路径规划
    struct PathPoint {
        float x, y, heading, speed;
    };
    PathPoint path[PATH_POINTS];
    int pathLength;
    
    // B样条控制点
    float ctrlX[MAX_CONTROL];
    float ctrlY[MAX_CONTROL];
    int numCtrl;
    
    // 任务队列
    TaskMessage taskQueue[10];
    int numTasks;
    int currentTask;
    
    // 状态
    enum RobotStatus {
        IDLE,
        MOVING,
        AVOIDING,
        SERVING,
        CHARGING,
        EMERGENCY
    };
    
    RobotStatus status;
    
    // 性能指标
    unsigned long totalDistance;
    unsigned long serviceCount;
    float energyConsumption;
    
public:
    CollaborativeRobot(uint8_t id) : robotId(id) {
        numPeers = 0;
        pathLength = 0;
        numCtrl = 0;
        numTasks = 0;
        currentTask = -1;
        status = IDLE;
        totalDistance = 0;
        serviceCount = 0;
        energyConsumption = 0;
    }
    
    void initialize() {
        Serial.begin(115200);
        Serial.print("协作服务机器人");
        Serial.print(robotId);
        Serial.println("初始化");
        
        initMotors();
        initSensors();
        initCommunication();
        
        status = IDLE;
    }
    
    void initMotors() {
        pinMode(MOTOR_L_PWM, OUTPUT);
        pinMode(MOTOR_L_DIR, OUTPUT);
        pinMode(MOTOR_R_PWM, OUTPUT);
        pinMode(MOTOR_R_DIR, OUTPUT);
        
        // 设置PWM频率为31kHz
        TCCR1B = (TCCR1B & 0xF8) | 0x01;
        ICR1 = 512;
        TCCR1A |= _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
        TCCR1B |= _BV(WGM13) | _BV(WGM12);
    }
    
    void initSensors() {
        pinMode(IR_FRONT, INPUT);
        pinMode(IR_LEFT, INPUT);
        pinMode(IR_RIGHT, INPUT);
        pinMode(IMU_INT, INPUT_PULLUP);
    }
    
    void initCommunication() {
        radio.begin();
        radio.openReadingPipe(0, address);
        radio.setPALevel(RF24_PA_LOW);
        radio.startListening();
    }
    
    void run() {
        // 通信更新
        updateCommunication();
        
        // 状态机
        switch(status) {
            case IDLE:
                idleHandler();
                break;
            case MOVING:
                movingHandler();
                break;
            case AVOIDING:
                avoidingHandler();
                break;
            case SERVING:
                servingHandler();
                break;
            case CHARGING:
                chargingHandler();
                break;
            case EMERGENCY:
                emergencyHandler();
                break;
        }
        
        // 定期广播状态
        if(millis() % 100 < 10) {
            broadcastState();
        }
        
        delay(10);
    }
    
    void idleHandler() {
        // 检查是否有新任务
        if(numTasks > 0) {
            currentTask = 0;
            planPathToTask();
            status = MOVING;
        }
    }
    
    void movingHandler() {
        // 检查避障
        if(detectObstacle()) {
            status = AVOIDING;
            return;
        }
        
        // 沿路径移动
        if(followPath()) {
            // 到达目标
            status = SERVING;
            serviceStartTime = millis();
        }
        
        // 检查紧急情况
        if(checkEmergency()) {
            status = EMERGENCY;
        }
    }
    
    void avoidingHandler() {
        // 动态避障
        performAvoidance();
        
        // 避障完成后回到移动状态
        if(!detectObstacle()) {
            status = MOVING;
        }
    }
    
    void servingHandler() {
        // 执行服务任务
        if(millis() - serviceStartTime > serviceDuration) {
            serviceCount++;
            currentTask = -1;
            status = IDLE;
            Serial.print("机器人");
            Serial.print(robotId);
            Serial.println("完成服务任务");
        }
    }
    
    void planPathToTask() {
        TaskMessage task = taskQueue[currentTask];
        
        // 设置B样条控制点
        numCtrl = 0;
        
        // 起点
        ctrlX[numCtrl] = myState.posX;
        ctrlY[numCtrl] = myState.posY;
        numCtrl++;
        
        // 中间点（考虑障碍物和其他机器人）
        float midX = (myState.posX + task.targetX) / 2;
        float midY = (myState.posY + task.targetY) / 2;
        
        // 检查中间点是否安全
        if(isPointSafe(midX, midY)) {
            ctrlX[numCtrl] = midX;
            ctrlY[numCtrl] = midY;
            numCtrl++;
        } else {
            // 添加绕行点
            float offsetX = (myState.posY - task.targetY) * 0.5;
            float offsetY = (task.targetX - myState.posX) * 0.5;
            ctrlX[numCtrl] = midX + offsetX;
            ctrlY[numCtrl] = midY + offsetY;
            numCtrl++;
        }
        
        // 终点
        ctrlX[numCtrl] = task.targetX;
        ctrlY[numCtrl] = task.targetY;
        numCtrl++;
        
        // 生成平滑路径
        generateBSplinePath();
    }
    
    void generateBSplinePath() {
        pathLength = 0;
        
        if(numCtrl < BSPLINE_ORDER + 1) return;
        
        // 生成节点向量
        float knots[MAX_CONTROL + BSPLINE_ORDER + 1];
        for(int i = 0; i < numCtrl + BSPLINE_ORDER + 1; i++) {
            knots[i] = i;
        }
        
        // 采样B样条曲线
        for(int i = BSPLINE_ORDER; i < numCtrl; i++) {
            for(int j = 0; j < PATH_POINTS / (numCtrl - BSPLINE_ORDER); j++) {
                float t = (float)j / (PATH_POINTS / (numCtrl - BSPLINE_ORDER));
                float u = knots[i] + t;
                
                // 计算基函数
                float basis[MAX_CONTROL];
                calculateBasis(u, knots, basis);
                
                // 计算路径点
                if(pathLength < PATH_POINTS) {
                    path[pathLength].x = 0;
                    path[pathLength].y = 0;
                    
                    for(int k = 0; k < numCtrl; k++) {
                        path[pathLength].x += basis[k] * ctrlX[k];
                        path[pathLength].y += basis[k] * ctrlY[k];
                    }
                    
                    path[pathLength].speed = 0.3;  // 默认速度
                    
                    if(pathLength > 0) {
                        float dx = path[pathLength].x - path[pathLength-1].x;
                        float dy = path[pathLength].y - path[pathLength-1].y;
                        path[pathLength].heading = atan2(dy, dx);
                        path[pathLength-1].heading = path[pathLength].heading;
                    }
                    
                    pathLength++;
                }
            }
        }
        
        Serial.print("生成路径，长度:");
        Serial.println(pathLength);
    }
    
    void calculateBasis(float u, float* knots, float* basis) {
        float N[MAX_CONTROL][BSPLINE_ORDER + 1];
        
        // 初始化
        for(int i = 0; i < numCtrl; i++) {
            for(int j = 0; j <= BSPLINE_ORDER; j++) {
                N[i][j] = 0;
            }
        }
        
        // 0阶基函数
        for(int i = 0; i < numCtrl; i++) {
            if(u >= knots[i] && u < knots[i+1]) {
                N[i][0] = 1;
            }
        }
        
        // 递归计算
        for(int p = 1; p <= BSPLINE_ORDER; p++) {
            for(int i = 0; i < numCtrl - p; i++) {
                float d1 = knots[i+p] - knots[i];
                float d2 = knots[i+p+1] - knots[i+1];
                
                float t1 = (d1 != 0) ? (u - knots[i]) / d1 * N[i][p-1] : 0;
                float t2 = (d2 != 0) ? (knots[i+p+1] - u) / d2 * N[i+1][p-1] : 0;
                
                N[i][p] = t1 + t2;
            }
        }
        
        for(int i = 0; i < numCtrl; i++) {
            basis[i] = N[i][BSPLINE_ORDER];
        }
    }
    
    bool followPath() {
        static int currentPoint = 0;
        
        if(currentPoint >= pathLength) {
            currentPoint = 0;
            return true;  // 到达终点
        }
        
        // 计算与目标点的误差
        float dx = path[currentPoint].x - myState.posX;
        float dy = path[currentPoint].y - myState.posY;
        float distError = sqrt(dx*dx + dy*dy);
        float targetAngle = atan2(dy, dx);
        float angleError = targetAngle - myState.theta;
        
        // 角度归一化
        while(angleError > PI) angleError -= 2*PI;
        while(angleError < -PI) angleError += 2*PI;
        
        // 计算速度
        float speed = path[currentPoint].speed;
        float angularVel = 2.0 * angleError;
        
        // 差速驱动
        float leftSpeed = speed - angularVel * WHEEL_BASE / 2;
        float rightSpeed = speed + angularVel * WHEEL_BASE / 2;
        
        // 转换为PWM
        int leftPWM = speedToPWM(leftSpeed);
        int rightPWM = speedToPWM(rightSpeed);
        
        driveMotors(leftPWM, rightPWM);
        
        // 更新路径点
        if(distError < 2.0) {
            currentPoint++;
            totalDistance += 2;
        }
        
        return false;
    }
    
    int speedToPWM(float speed) {
        // 速度范围：-0.5 到 0.5 m/s
        int pwm = (int)(speed * 510);
        return constrain(pwm, -255, 255);
    }
    
    void driveMotors(int leftPWM, int rightPWM) {
        leftPWM = constrain(leftPWM, -255, 255);
        rightPWM = constrain(rightPWM, -255, 255);
        
        OCR1A = abs(leftPWM);
        OCR1B = abs(rightPWM);
        
        digitalWrite(MOTOR_L_DIR, leftPWM > 0);
        digitalWrite(MOTOR_R_DIR, rightPWM > 0);
    }
    
    void performAvoidance() {
        // 读取红外传感器
        int frontIR = analogRead(IR_FRONT);
        int leftIR = analogRead(IR_LEFT);
        int rightIR = analogRead(IR_RIGHT);
        
        // 根据传感器数据决定避障方向
        if(frontIR > 500) {
            if(leftIR < rightIR) {
                // 向左转
                driveMotors(-100, 100);
            } else {
                // 向右转
                driveMotors(100, -100);
            }
        } else if(leftIR > 500) {
            // 右侧有空间，向右转
            driveMotors(100, 50);
        } else if(rightIR > 500) {
            // 左侧有空间，向左转
            driveMotors(50, 100);
        }
    }
    
    bool isPointSafe(float x, float y) {
        // 检查与障碍物的距离
        // 这里简化处理
        return true;
    }
    
    void updateCommunication() {
        if(radio.available()) {
            RobotState peerState;
            radio.read(&peerState, sizeof(RobotState));
            
            // 更新同伴状态
            bool found = false;
            for(int i = 0; i < numPeers; i++) {
                if(peerStates[i].robotId == peerState.robotId) {
                    peerStates[i] = peerState;
                    found = true;
                    break;
                }
            }
            
            if(!found && numPeers < 5) {
                peerStates[numPeers++] = peerState;
            }
        }
    }
    
    void broadcastState() {
        radio.stopListening();
        myState.timestamp = millis();
        radio.write(&myState, sizeof(RobotState));
        radio.startListening();
    }
    
    bool checkEmergency() {
        return energyConsumption > 1000;  // 能耗过高
    }
    
    void emergencyHandler() {
        driveMotors(0, 0);
        Serial.println("紧急停止");
        delay(5000);
        status = IDLE;
    }
    
    void chargingHandler() {
        // 充电逻辑
    }
};

CollaborativeRobot robot(1);

void setup() {
    robot.initialize();
}

void loop() {
    robot.run();
}

要点解读

1. 三次B样条路径平滑算法
   数学基础：采用de Boor算法递归计算B样条基函数，确保路径C²连续性（位置、速度、加速度连续）
   实时计算：通过节点向量和基函数预计算，在嵌入式平台上实现50Hz的路径点生成
   自适应采样：根据路径曲率动态调整采样密度，直线段稀疏、弯道密集，平衡精度和计算量

2. BLDC差速驱动控制
   高频PWM：使用20kHz以上的PWM频率，消除 audible噪声，提高电机响应速度
   精确速度控制：基于编码器反馈的闭环速度控制，实现±1%的速度精度
   运动学解算：通过差速运动学模型，将线速度和角速度精确映射到左右轮速度

3. 多传感器融合避障
   分层感知：超声波（远距离）+ 红外（中距离）+ 激光雷达（精确距离）三层感知体系
   动态障碍物预测：利用速度障碍法（VO）预测移动障碍物轨迹，实现主动避障
   安全冗余：设置多级安全距离（减速区、避障区、急停区），确保运行安全

4. 实时路径重规划
   触发式重规划：仅在检测到障碍物或路径偏差超过阈值时触发，避免频繁计算
   局部调整策略：保留原始路径结构，仅调整局部控制点，降低计算复杂度
   平滑过渡：重规划时保持当前位置和速度连续性，避免急转弯

5. 多机器人协作机制
   状态广播：周期性广播自身状态（位置、速度、任务），实现分布式感知
   任务分配：基于优先级和距离的任务队列管理，支持动态任务插入
   冲突避免：通过共享状态预测潜在碰撞，协调多机器人路径规划
   
   4、餐厅送餐机器人（三次B样条全局路径平滑+多传感器差速避障）
   适用场景：餐厅多餐桌、多行人的动态环境，需从厨房精准送达餐品至指定餐桌，既要保证全局路径无折线、运动平稳，又要实时避开行人、桌椅等障碍。

核心逻辑：采用“全局路径规划+局部避障”分层架构，全局路径通过三次B样条对离散餐桌坐标插值，生成连续平滑的轨迹；局部避障融合超声波与红外传感器，实时检测障碍，通过BLDC差速控制调整转向，避障后自动回归B样条平滑路径。

/* 餐厅送餐机器人：三次B样条路径平滑+多传感器差速避障
   硬件：Arduino Mega + 2×BLDC差速驱动 + 前/左/右超声波 + 红外阵列
   核心流程：全局B样条插值→路径跟踪→多传感器避障→避障后路径回归
   参数：最大线速度0.5m/s、角速度1.2rad/s、避障阈值30cm
*/
#include <SimpleFOC.h>
#include <NewPing.h>
#include <vector>

// --- 三次B样条路径节点（厨房→餐桌1→餐桌2）
struct Point { float x; float y; };
Point pathNodes[3] = {{0.0, 0.0}, {3.0, 2.0}, {5.0, 5.0}};
std::vector<Point> bSplinePath; // 平滑后的路径点

// --- 硬件定义 ---
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(8);
NewPing sonarF(9, 10, 200); // 前方超声波
NewPing sonarL(11, 12, 150); // 左侧超声波
NewPing sonarR(13, 14, 150); // 右侧超声波

// --- 控制参数 ---
const float MAX_V = 0.5;   // 最大线速度（m/s）
const float MAX_W = 1.2;   // 最大角速度（rad/s）
const float OBSTACLE_THRESH = 30; // 避障距离阈值（cm）
float currentV = 0, currentW = 0; // 当前速度
float robotX = 0, robotY = 0, robotYaw = 0; // 机器人位姿

// --- 三次B样条插值函数 ---
void generateBSpline() {
  // 简化的三次B样条插值（4个控制点需扩展，此处为3节点简化版）
  float t = 0;
  while (t <= 1) {
    Point p;
    // 三次B样条基函数计算（核心公式简化）
    float u = t * 3;
    int i = (int)u;
    float frac = u - i;
    float B0 = (1-frac)*(1-frac)*(1-frac)/6;
    float B1 = (3*frac*frac*frac - 6*frac*frac + 4)/6;
    float B2 = (-3*frac*frac*frac + 3*frac*frac + 3*frac + 1)/6;
    float B3 = frac*frac*frac/6;
    // 控制点权重计算（3节点需补虚拟节点，此处简化为线性插值）
    p.x = B1 * pathNodes[1].x + B2 * pathNodes[2].x;
    p.y = B1 * pathNodes[1].y + B2 * pathNodes[2].y;
    bSplinePath.push_back(p);
    t += 0.1;
  }
  Serial.println("三次B样条路径生成完成，路径点数：" + String(bSplinePath.size()));
}

// --- 路径跟踪控制（基于B样条路径点） ---
void trackBSplinePath() {
  if (bSplinePath.size() < 2) return;
  // 取当前目标点（简化为路径第一个点）
  Point target = bSplinePath[0];
  // 计算航向偏差
  float dx = target.x - robotX;
  float dy = target.y - robotY;
  float targetYaw = atan2(dy, dx);
  float yawError = targetYaw - robotYaw;
  // 归一化航向偏差
  while (yawError > M_PI) yawError -= 2*M_PI;
  while (yawError < -M_PI) yawError += 2*M_PI;
  // PID控制角速度
  float Kp = 0.8, Ki = 0.1, Kd = 0.05;
  static float integral = 0, prevError = 0;
  integral += yawError;
  float dError = yawError - prevError;
  currentW = Kp * yawError + Ki * integral + Kd * dError;
  currentW = constrain(currentW, -MAX_W, MAX_W);
  // 线速度（距离越近越慢）
  float dist = sqrt(dx*dx + dy*dy);
  currentV = map(dist, 0, 2, 0.1, MAX_V);
  currentV = constrain(currentV, 0, MAX_V);
  prevError = yawError;
}

// --- 多传感器差速避障 ---
void differentialAvoidance() {
  int distF = sonarF.ping_cm();
  int distL = sonarL.ping_cm();
  int distR = sonarR.ping_cm();
  // 数据有效性过滤
  distF = (distF <= 0 || distF > 150) ? 150 : distF;
  distL = (distL <= 0 || distL > 150) ? 150 : distL;
  distR = (distR <= 0 || distR > 150) ? 150 : distR;
  // 紧急制动
  if (distF < OBSTACLE_THRESH * 0.5) {
    currentV = 0;
    currentW = 0;
    motorL.move(0); motorR.move(0);
    delay(200);
    // 后退转向
    motorL.move(-0.3); motorR.move(-0.3);
    delay(300);
    motorL.move(0); motorR.move(0);
    return;
  }
  // 避障决策
  if (distF < OBSTACLE_THRESH) {
    if (distL > distR && distL > OBSTACLE_THRESH * 0.7) {
      // 左侧开阔，左转
      currentV = 0.2;
      currentW = -0.8;
    } else if (distR > distL && distR > OBSTACLE_THRESH * 0.7) {
      // 右侧开阔，右转
      currentV = 0.2;
      currentW = 0.8;
    } else {
      // 两侧均狭窄，后退
      currentV = -0.3;
      currentW = 0;
    }
  } else {
    // 无障碍，回归路径跟踪
    trackBSplinePath();
  }
}

// --- 主程序 ---
void setup() {
  Serial.begin(115200);
  // BLDC初始化
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 生成B样条路径
  generateBSpline();
  Serial.println("送餐机器人系统初始化完成");
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  // 避障优先，无障碍则跟踪路径
  differentialAvoidance();
  // BLDC差速执行
  float vLeft = currentV - currentW * 0.3; // 差速底盘运动学模型
  float vRight = currentV + currentW * 0.3;
  motorL.move(vLeft);
  motorR.move(vRight);
  // 更新位姿（简化里程计）
  currentV = (vLeft + vRight) / 2;
  currentW = (vRight - vLeft) / 0.6;
  robotX += currentV * cos(robotYaw) * 0.02;
  robotY += currentV * sin(robotYaw) * 0.02;
  robotYaw += currentW * 0.02;
  delay(20);
}

5、医院药品配送机器人（动态窗口法+三次B样条路径平滑）
适用场景：医院走廊、病房等半结构化环境，存在移动的医护人员、推车等动态障碍，需从药房精准配送药品至病房，要求路径平滑且能实时应对动态障碍。

核心逻辑：采用动态窗口法（DWA）实现局部动态避障，结合三次B样条对全局路径平滑，DWA在速度空间采样，兼顾目标朝向、避障距离与速度约束，输出平滑速度指令，由BLDC差速执行，避障后自动回归B样条全局路径。

/* 医院药品配送机器人：DWA+三次B样条路径平滑
   硬件：ESP32 + 2×BLDC差速驱动 + 前/左/右ToF传感器
   核心流程：全局B样条路径→DWA局部避障→BLDC差速执行→路径回归
   参数：最大线速度0.4m/s、角速度1.0rad/s、DWA预测时间1.5s
*/
#include <SimpleFOC.h>
#include <vector>

// --- 三次B样条全局路径（药房→病房1→病房2）
struct Point { float x; float y; };
Point globalNodes[4] = {{0.0,0.0}, {2.0,1.0}, {4.0,3.0}, {6.0,5.0}};
std::vector<Point> bSplineGlobal;

// --- 硬件定义 ---
BLDCMotor motorL = BLDCMotor(5);
BLDCMotor motorR = BLDCMotor(6);
// ToF传感器（简化为模拟接口，实际用I2C）
#define TOF_F 33
#define TOF_L 32
#define TOF_R 25

// --- DWA参数 ---
const float MAX_V = 0.4;
const float MAX_W = 1.0;
const float ACC_V = 0.2;
const float ACC_W = 0.8;
const float DT = 0.1;
const float PREDICT_TIME = 1.5;
const int SAMPLES_V = 6;
const int SAMPLES_W = 8;
// 评价函数权重
const float ALPHA_HEADING = 0.5;
const float BETA_DIST = 0.3;
const float GAMMA_VELOCITY = 0.2;

// --- 状态变量 ---
float robotX = 0, robotY = 0, robotYaw = 0;
float goalX = 6.0, goalY = 5.0;
float currentV = 0, currentW = 0;

// --- 三次B样条路径生成 ---
void generateGlobalBSpline() {
  // 基于4个控制点的三次B样条插值（完整实现）
  for (float t = 0; t <= 1; t += 0.05) {
    Point p;
    float u = t * 3;
    int i = (int)u;
    if (i >= 3) i = 2;
    float frac = u - i;
    // 三次B样条基函数
    float B0 = ((1-frac)*(1-frac)*(1-frac))/6;
    float B1 = (3*frac*frac*frac - 6*frac*frac + 4)/6;
    float B2 = (-3*frac*frac*frac + 3*frac*frac + 3*frac + 1)/6;
    float B3 = (frac*frac*frac)/6;
    // 控制点索引（4个控制点对应i=0,1,2）
    p.x = B0*globalNodes[i].x + B1*globalNodes[i+1].x + B2*globalNodes[i+2].x + B3*globalNodes[i+3].x;
    p.y = B0*globalNodes[i].y + B1*globalNodes[i+1].y + B2*globalNodes[i+2].y + B3*globalNodes[i+3].y;
    bSplineGlobal.push_back(p);
  }
  Serial.println("全局B样条路径生成完成");
}

// --- DWA局部避障 ---
void dwaLocalPlan() {
  // 1. 速度采样（考虑动力学约束）
  float vMin = max(currentV - ACC_V*DT, -MAX_V);
  float vMax = min(currentV + ACC_V*DT, MAX_V);
  float wMin = max(currentW - ACC_W*DT, -MAX_W);
  float wMax = min(currentW + ACC_W*DT, MAX_W);
  // 2. 轨迹预测与评价
  float bestScore = -INFINITY;
  float bestV = 0, bestW = 0;
  for (int i = 0; i < SAMPLES_V; i++) {
    for (int j = 0; j < SAMPLES_W; j++) {
      float v = vMin + (vMax - vMin) * i/(SAMPLES_V-1);
      float w = wMin + (wMax - wMin) * j/(SAMPLES_W-1);
      // 预测轨迹终点
      float x = robotX + v*cos(robotYaw)*PREDICT_TIME;
      float y = robotY + v*sin(robotYaw)*PREDICT_TIME;
      float yaw = robotYaw + w*PREDICT_TIME;
      // 评价函数：朝向目标+避障距离+速度
      float dx = goalX - x, dy = goalY - y;
      float heading = atan2(dy, dx) - yaw;
      while (heading > M_PI) heading -= 2*M_PI;
      while (heading < -M_PI) heading += 2*M_PI;
      float scoreHeading = -abs(heading);
      // 避障距离（简化为前方ToF测距）
      float dist = analogRead(TOF_F) * 0.01;
      float scoreDist = dist;
      // 速度得分
      float scoreVel = v;
      // 总得分
      float score = ALPHA_HEADING*scoreHeading + BETA_DIST*scoreDist + GAMMA_VELOCITY*scoreVel;
      if (score > bestScore) {
        bestScore = score;
        bestV = v;
        bestW = w;
      }
    }
  }
  currentV = bestV;
  currentW = bestW;
}

// --- 主程序 ---
void setup() {
  Serial.begin(115200);
  // BLDC初始化
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 生成全局B样条路径
  generateGlobalBSpline();
  Serial.println("药品配送机器人初始化完成");
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  // DWA局部避障（优先处理动态障碍）
  dwaLocalPlan();
  // BLDC差速执行
  float vLeft = currentV - currentW * 0.3;
  float vRight = currentV + currentW * 0.3;
  motorL.move(vLeft);
  motorR.move(vRight);
  // 更新位姿
  robotX += currentV * cos(robotYaw) * 0.02;
  robotY += currentV * sin(robotYaw) * 0.02;
  robotYaw += currentW * 0.02;
  delay(20);
}

6、家庭清洁机器人（三次B样条路径覆盖+边缘跟随避障）
适用场景：家庭客厅、卧室等不规则环境，需覆盖地面清洁，既要保证清洁路径无遗漏、运动平稳，又要避开家具边缘、宠物粪便等障碍，支持沿家具边缘跟随清洁。

核心逻辑：采用三次B样条生成覆盖式全局路径，确保清洁区域全覆盖且无折线；避障采用边缘跟随策略，通过多传感器检测家具边缘，BLDC差速控制实现沿边缘平稳跟随，清洁完成后回归充电座。

/* 家庭清洁机器人：三次B样条路径覆盖+边缘跟随避障
   硬件：Arduino Mega + 2×BLDC差速驱动 + 前/左/右超声波 + 边缘传感器
   核心流程：B样条覆盖路径生成→边缘跟随避障→全覆盖清洁→回归充电座
   参数：清洁速度0.3m/s、边缘跟随距离15cm、充电座坐标(0,0)
*/
#include <SimpleFOC.h>
#include <NewPing.h>
#include <vector>

// --- 清洁区域B样条路径（客厅覆盖）
struct Point { float x; float y; };
Point cleanNodes[5] = {{0,0}, {2,0}, {2,3}, {0,3}, {0,0}}; // 矩形覆盖路径
std::vector<Point> bSplineCleanPath;

// --- 硬件定义 ---
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(8);
NewPing sonarF(9,10,200);
NewPing sonarL(11,12,150);
NewPing sonarR(13,14,150);
int edgeSensor = A5; // 边缘检测传感器（检测家具边缘）

// --- 控制参数 ---
const float CLEAN_V = 0.3;
const float EDGE_FOLLOW_DIST = 15;
float currentV = CLEAN_V, currentW = 0;
float robotX = 0, robotY = 0, robotYaw = 0;
bool isEdgeFollowing = false;

// --- 三次B样条覆盖路径生成 ---
void generateCleanBSpline() {
  // 5个控制点生成覆盖式B样条路径
  for (float t = 0; t <= 1; t += 0.08) {
    Point p;
    float u = t * 4;
    int i = (int)u;
    if (i >= 4) i = 3;
    float frac = u - i;
    // 三次B样条基函数
    float B0 = (1-frac)*(1-frac)*(1-frac)/6;
    float B1 = (3*frac*frac*frac - 6*frac*frac + 4)/6;
    float B2 = (-3*frac*frac*frac + 3*frac*frac + 3*frac + 1)/6;
    float B3 = frac*frac*frac/6;
    p.x = B0*cleanNodes[i].x + B1*cleanNodes[i+1].x + B2*cleanNodes[i+2].x + B3*cleanNodes[i+3].x;
    p.y = B0*cleanNodes[i].y + B1*cleanNodes[i+1].y + B2*cleanNodes[i+2].y + B3*cleanNodes[i+3].y;
    bSplineCleanPath.push_back(p);
  }
  Serial.println("清洁覆盖路径生成完成，路径点数：" + String(bSplineCleanPath.size()));
}

// --- 边缘跟随避障 ---
void edgeFollowAvoidance() {
  int distL = sonarL.ping_cm();
  int distR = sonarR.ping_cm();
  int edgeVal = analogRead(edgeSensor);
  // 检测到边缘，启动跟随
  if (edgeVal > 500) {
    isEdgeFollowing = true;
    // 左侧距离保持（沿边缘跟随）
    float error = distL - EDGE_FOLLOW_DIST;
    float Kp = 0.5;
    currentW = Kp * error;
    currentW = constrain(currentW, -MAX_W, MAX_W);
    currentV = CLEAN_V * 0.8; // 边缘跟随时降速
  } else {
    // 无边缘，检测障碍
    int distF = sonarF.ping_cm();
    if (distF < 30) {
      // 前方障碍，转向
      if (distL > distR) {
        currentW = -0.6;
      } else {
        currentW = 0.6;
      }
      currentV = 0.1;
    } else {
      isEdgeFollowing = false;
      currentV = CLEAN_V;
      currentW = 0;
    }
  }
}

// --- 回归充电座（B样条路径回溯） ---
void returnToCharger() {
  // 沿B样条路径反向回归充电座(0,0)
  Point target = {0,0};
  float dx = target.x - robotX;
  float dy = target.y - robotY;
  float targetYaw = atan2(dy, dx);
  float yawError = targetYaw - robotYaw;
  while (yawError > M_PI) yawError -= 2*M_PI;
  while (yawError < -M_PI) yawError += 2*M_PI;
  // PID控制
  float Kp = 0.8;
  currentW = Kp * yawError;
  currentV = 0.2; // 回归时慢速
  // 到达充电座判定
  float dist = sqrt(dx*dx + dy*dy);
  if (dist < 0.2) {
    currentV = 0;
    currentW = 0;
    motorL.move(0); motorR.move(0);
    Serial.println("已到达充电座，开始充电");
    while (1);
  }
}

// --- 主程序 ---
void setup() {
  Serial.begin(115200);
  // BLDC初始化
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 生成清洁路径
  generateCleanBSpline();
  Serial.println("家庭清洁机器人初始化完成");
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  // 边缘跟随避障优先
  edgeFollowAvoidance();
  // 清洁完成后回归充电座
  if (bSplineCleanPath.size() == 0) {
    returnToCharger();
  }
  // BLDC差速执行
  float vLeft = currentV - currentW * 0.3;
  float vRight = currentV + currentW * 0.3;
  motorL.move(vLeft);
  motorR.move(vRight);
  // 更新位姿
  robotX += currentV * cos(robotYaw) * 0.02;
  robotY += currentV * sin(robotYaw) * 0.02;
  robotYaw += currentW * 0.02;
  delay(20);
}

要点解读

1. 三次B样条路径平滑的数学原理与工程落地：兼顾连续性与计算效率
   三次B样条路径平滑的核心是通过基函数保证路径的二阶连续性（位置、速度、加速度连续），避免折线运动带来的机械冲击与路径遗漏，其工程落地需平衡计算复杂度与嵌入式算力：
   数学本质：三次B样条通过4个控制点与基函数生成连续轨迹，基函数确保曲线在控制点处平滑过渡，无尖角，满足室内服务机器人对运动平稳性的高要求；
   算力适配：Arduino等嵌入式平台算力有限，案例中采用简化版B样条（减少控制点数量、固定插值步长），避免复杂矩阵运算，同时通过预生成路径点降低实时计算压力；
   场景适配：不同场景的路径节点设计不同，送餐机器人侧重点对点精准，清洁机器人侧重区域全覆盖，需根据场景需求调整控制点布局，确保路径既平滑又满足功能需求。
2. BLDC差速避障的控制逻辑：从运动学模型到闭环执行
   BLDC差速避障的核心是通过左右轮速度差实现转向，结合闭环控制保证轨迹精度，其控制逻辑需紧密贴合底盘运动学特性与避障需求：
   运动学模型：差速底盘的核心公式为vLeft = v - wL/2、vRight = v + wL/2（L为轮距），案例中通过该模型将线速度v与角速度w转换为左右轮转速，实现精准转向；
   闭环控制：BLDC需搭配编码器实现速度/位置闭环，避免开环PWM控制的转速漂移，案例采用SimpleFOC库实现FOC控制，提升电机响应速度与扭矩稳定性，确保避障动作精准；
   避障联动：差速控制需与避障传感器实时联动，案例中避障决策输出的转向角速度直接映射为左右轮速度差，同时通过速度限幅避免急转导致的侧滑，兼顾避障灵活性与运动稳定性。
3. 多传感器融合的避障策略：弥补单一传感器短板，提升鲁棒性
   室内环境复杂，单一传感器易受干扰（超声波受温度、角度影响，红外受光照影响），多传感器融合是避障的核心保障，其策略需围绕互补性、数据校验、分级响应设计：
   传感器互补：案例1融合超声波（中远距离测距）与红外（近距离检测），案例3增加边缘传感器，弥补单一传感器盲区，覆盖不同距离与类型的障碍检测需求；
   数据校验：通过数据有效性过滤（如超声波无效值替换为最大测距）、中值滤波消除噪声，避免传感器误触发导致的误避障；
   分级响应：建立分级避障逻辑，紧急障碍（近距离）触发急停后退，一般障碍（中距离）触发转向，边缘障碍触发跟随，不同级别对应不同速度与转向策略，既保证安全，又避免过度避障导致的效率低下。
4. 路径规划与避障的协同机制：全局平滑与局部灵活的平衡
   室内服务机器人的核心矛盾是全局路径的平滑性与局部避障的灵活性，二者需通过分层架构实现协同，避免避障后路径断裂或震荡：
   分层架构：采用“全局规划+局部避障”分层设计，全局路径由三次B样条生成，保证整体运动平稳；局部避障采用DWA、边缘跟随等算法，实时应对动态障碍，避障结束后自动回归全局路径，确保路径连续性；
   平滑切换：避障与路径跟踪的切换需避免速度突变，案例通过权重融合（如DWA输出与路径跟踪输出加权平均）、速度渐变过渡，防止机器人因控制模式切换产生“抽搐”，提升运动平滑度；
   路径回归逻辑：避障结束后，通过航向偏差闭环控制回归全局路径，案例1通过PID控制航向偏差，案例2通过DWA的目标朝向权重引导回归，确保机器人快速、平稳地回到预设平滑路径，避免路径偏移。
5. 嵌入式系统的实时性与可靠性保障：适配硬件约束，筑牢安全底线
   Arduino等嵌入式平台资源有限，实时性与可靠性是系统落地的关键，需从算力适配、控制周期、安全机制三方面保障：
   算力适配：三次B样条与DWA等算法对算力要求较高，案例采用Arduino Mega、ESP32等高性能平台，避免算力不足导致的控制延迟；同时通过简化算法（如减少路径点数量、固定采样步长）、预生成路径降低实时计算压力；
   控制周期稳定：采用定时器中断或SimpleFOC的loopFOC函数保证控制周期稳定，避免delay阻塞导致的控制节拍紊乱，案例中控制周期控制在20ms左右，满足实时避障与路径跟踪的响应需求；
   安全机制：设置硬件急停、软件限速、障碍急停、看门狗定时器等多重安全机制，案例中前方近距离障碍触发急停后退，通信丢失触发停机，防止机器人失控；同时通过电源隔离、电容滤波解决BLDC电机的电磁干扰，保证传感器与主控的稳定运行。

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