“Arduino BLDC 之动态边界与运动约束：机器人底盘的高阶导航”可以理解为：在差速/全向底盘上，把环境安全边界、底盘运动学边界和执行器动力学边界统一纳入导航控制，使机器人在动态环境中既能贴近目标运动，又不会因为速度过高、转向过猛或边界判断过粗而发生碰撞、打滑或失控。
这里的“高阶”主要体现在三层约束协同：
环境边界：障碍物、走廊、门框、动态行人形成的安全通行区域；
运动学边界：差速底盘的非完整约束、最大线速度、最大角速度、最小转弯半径；
动力学/执行器边界：BLDC 最大电流、加速度、加加速度、摩擦极限和制动距离。

一、主要特点

1. 从“几何避障”升级为“可达安全集”
   传统避障常把机器人简化为质点或圆，只判断距离是否小于阈值。高阶导航会把机器人外形、轮距、制动能力和当前速度一起考虑，形成“当前速度下仍能安全停止或转向”的可达集合。这样在高速接近障碍时会自动减速，而不是等到距离很近才急刹。
2. 动态边界会随速度、负载和环境变化
   安全边界不是固定值。速度越高、负载越大、地面越滑，制动距离和转向半径都会变大，安全边界应随之外扩。相反，在低速、开阔、摩擦良好的区域，可以适当放宽边界，提高通行效率。
3. 运动约束保证指令可执行
   差速底盘不能横向平移，角速度受轮距和轮速限制；全向底盘虽然能横移，但也受电机功率和摩擦约束。高阶导航会在生成速度指令时检查这些约束，避免输出“理论上安全但底盘无法执行”的轨迹。
4. BLDC 执行层提供高带宽力矩响应
   BLDC 配合 FOC 或闭环驱动器，可以较快响应速度、加速度和扭矩变化，适合执行动态避障中的连续速度修正。相比开环 ESC 或有刷电机，BLDC 更容易实现平滑加减速、差速转向和打滑抑制。
5. 通常采用分层架构
   完整系统一般分为：
   感知层：LiDAR、超声波、ToF、相机等获取局部障碍；
   边界层：构建局部安全走廊、代价地图或控制屏障函数；
   规划层：在动态窗口、向量场或滚动窗口内选择安全速度；
   执行层：Arduino/ESP32/STM32 进行运动学解算和 BLDC 速度/扭矩闭环。
   Arduino 更适合承担底层执行和轻量局部决策；复杂边界优化、CBF/QP 或 NMPC 通常放在上位机或更高性能 MCU 上。

二、应用场景

1. 仓储 AMR / AGV
   在货架通道、装卸口和人流交叉区域运行时，动态边界可防止高速接近转角盲区，运动约束可避免重载启停时货物晃动或车轮打滑。
2. 室内服务机器人
   扫地机、送餐机器人、巡检机器人在狭窄走廊和门框附近需要精确考虑机身外形，不能只按圆形半径避障。
3. 户外差速/履带平台
   草地、碎石、坡道等地形会改变摩擦边界。动态边界可以根据 IMU 倾角、轮速差和电流变化判断是否接近打滑，从而降低速度或增大转向半径。
4. 人机协作环境
   当行人、叉车或其他机器人动态穿过路径时，系统需要根据相对速度和制动能力调整安全边界，而不是简单停止。
5. 教学科研平台
   适合验证 DWA、向量场引导、CBF 安全约束、差速运动学解算、BLDC 扭矩分配和滚动窗口重规划。

三、需要注意的事项

1. 安全边界不能只依赖距离阈值
   固定距离阈值无法反映速度、加速度和制动能力。更合理的方式是根据当前速度、最大减速度和传感器不确定性动态计算安全边界。
2. 差速底盘必须遵守非完整约束
   差速底盘不能直接执行横向速度指令，规划出的轨迹必须能分解为左右轮可达的线速度和角速度。否则会出现路径跟踪失败、原地抖动或轮速饱和。
3. BLDC 电流和扭矩限幅要前置
   动态避障可能频繁要求加减速和差速转向。如果只在上层规划安全边界，而底层电流、温度和堵转保护不足，容易出现驱动器过热、轮速跟踪失真或突然失力。
4. 局部重规划要平滑过渡
   避障触发时，速度指令不应突变。应通过插值、斜率限制或 S 曲线过渡，让 BLDC 从当前实际速度平滑切换到新指令，避免车身冲击和里程计跳变。
5. 传感器噪声会影响边界判断
   超声波、ToF 和 LiDAR 都可能存在多径反射、镜面反射和遮挡。动态边界需要滤波、置信度评估和冗余判断，避免因为单次异常测距误触发紧急转向。
6. Arduino 平台要控制计算复杂度
   完整 CBF、QP、NMPC 或高密度栅格地图在 8 位 Arduino 上很难稳定运行。工程上更适合把复杂边界计算放到上位机，Arduino 负责接收安全速度指令、执行运动学解算和 BLDC 闭环。
7. 失效保护优先级最高
   当编码器失联、IMU 异常、通信丢包或传感器被遮挡时，应优先降级到安全模式：降低速度、停止转向、原地停车或等待人工接管，而不是继续按错误边界高速运行。

整体来看，这条技术链路可以概括为：感知局部环境 → 构建动态安全边界 → 叠加底盘运动学与 BLDC 动力学约束 → 生成可达安全速度 → 差速/全向解算 → BLDC 闭环执行 → 持续验证边界与状态。它的核心价值是让导航不再只看“目标在哪、障碍在哪”，而是同时回答“当前速度下是否还能安全停住、转向和执行”。

1、DWA动态窗口——三重运动约束下的速度筛选
场景：室内动态环境，机器人需在速度边界、加速度能力、安全制动距离三重约束交集内选择最优速度指令。

#include <SimpleFOC.h>

// ==================== 底盘与电机 ====================
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 4);

// ==================== 运动约束参数 ====================
const float MAX_V = 1.2f;        // 最大线速度 (m/s)
const float MIN_V = -0.3f;       // 最小线速度（允许慢速后退）
const float MAX_W = 2.0f;        // 最大角速度 (rad/s)
const float MAX_ACC = 1.5f;      // 最大加速度 (m/s²)
const float MAX_DEC = 2.0f;      // 最大减速度（制动能力）
const float WHEEL_BASE = 0.25f;  // 轮距

// ==================== DWA 采样参数 ====================
const int V_SAMPLES = 15;        // 线速度采样数
const int W_SAMPLES = 15;        // 角速度采样数
const float DT = 0.1f;           // 控制周期 (s)
const float PREDICT_TIME = 1.5f; // 轨迹预测时长

// ==================== 当前状态 ====================
float currentV = 0, currentW = 0;

// ==================== DWA 核心：动态窗口计算 ====================
struct VelocityWindow {
    float vMin, vMax;
    float wMin, wMax;
};

VelocityWindow computeDynamicWindow(float obstacleDist) {
    VelocityWindow window;
    
    // 1. 速度边界约束：硬件极限
    window.vMin = MIN_V;
    window.vMax = MAX_V;
    window.wMin = -MAX_W;
    window.wMax = MAX_W;
    
    // 2. 加速度约束：一个控制周期内可达范围
    float vReachMax = currentV + MAX_ACC * DT;
    float vReachMin = currentV - MAX_DEC * DT;
    window.vMax = min(window.vMax, vReachMax);
    window.vMin = max(window.vMin, vReachMin);
    
    // 3. 安全约束：必须在障碍物前刹停
    // v ≤ √(2 · dist · dec)
    if (obstacleDist < 999.0f) {
        float vSafe = sqrt(2.0f * obstacleDist * MAX_DEC);
        window.vMax = min(window.vMax, vSafe);
    }
    
    return window;
}

// ==================== 轨迹预测与评价 ====================
struct Trajectory {
    float v, w;
    float endX, endY, endTheta;
    float minObstacleDist;
    float score;
};

Trajectory predictTrajectory(float v, float w, 
                              float obstacleDist, 
                              float goalX, float goalY) {
    Trajectory traj;
    traj.v = v;
    traj.w = w;
    
    // 简化运动学：假设预测时间内速度恒定
    float x = 0, y = 0, theta = 0;
    float minDist = 999.0f;
    
    int steps = (int)(PREDICT_TIME / DT);
    for (int i = 0; i < steps; i++) {
        x += v * cos(theta) * DT;
        y += v * sin(theta) * DT;
        theta += w * DT;
        
        // 简化：障碍物距离按直线衰减估计
        float dist = obstacleDist - sqrt(x*x + y*y);
        if (dist < minDist) minDist = dist;
    }
    
    traj.endX = x;
    traj.endY = y;
    traj.endTheta = theta;
    traj.minObstacleDist = minDist;
    
    // 多目标评价函数
    float headingScore = cos(theta) * 0.5f + 0.5f;  // 朝向目标程度
    float distScore = min(minDist / 2.0f, 1.0f);     // 障碍物距离
    float velScore = v / MAX_V;                      // 速度效率
    
    traj.score = 0.4f * headingScore + 0.4f * distScore + 0.2f * velScore;
    
    return traj;
}

// ==================== DWA 主循环 ====================
void dwaSelectVelocity(float obstacleDist, float goalX, float goalY,
                        float& bestV, float& bestW) {
    VelocityWindow window = computeDynamicWindow(obstacleDist);
    
    bestV = 0; bestW = 0;
    float bestScore = -1.0f;
    
    for (int i = 0; i < V_SAMPLES; i++) {
        float v = window.vMin + (window.vMax - window.vMin) * i / (V_SAMPLES - 1);
        
        for (int j = 0; j < W_SAMPLES; j++) {
            float w = window.wMin + (window.wMax - window.wMin) * j / (W_SAMPLES - 1);
            
            Trajectory traj = predictTrajectory(v, w, obstacleDist, goalX, goalY);
            
            // 淘汰不可行轨迹
            if (traj.minObstacleDist < 0.05f) continue;
            
            if (traj.score > bestScore) {
                bestScore = traj.score;
                bestV = v;
                bestW = w;
            }
        }
    }
}

// ==================== 主程序 ====================
void setup() {
    Serial.begin(115200);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 模拟传感器输入
    float obstacleDist = 2.0f;  // 超声波/激光测距
    float goalX = 3.0f, goalY = 1.0f;
    
    float bestV, bestW;
    dwaSelectVelocity(obstacleDist, goalX, goalY, bestV, bestW);
    
    // 差速运动学：速度 → 左右轮
    float vL = bestV - bestW * WHEEL_BASE / 2.0f;
    float vR = bestV + bestW * WHEEL_BASE / 2.0f;
    
    motorL.move(vL / 0.05f);  // 轮半径缩放
    motorR.move(vR / 0.05f);
    
    currentV = bestV;
    currentW = bestW;
    
    delay(50);
}

要点：DWA 的核心是三重约束求交集——速度边界（硬件极限）∩ 加速度边界（物理可达）∩ 安全制动边界（碰撞前能停）。只有落在交集内的速度才是“合法”的，评价函数再从合法速度中挑最优的。

2、加加速度（Jerk）约束的S曲线运动平滑
场景：配送机器人载有易洒货物，启停冲击会导致液体倾洒。需要将DWA输出的速度指令通过加加速度限制进一步平滑。

#include <SimpleFOC.h>

// ==================== 电机 ====================
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);

// ==================== S曲线规划器 ====================
struct JerkLimitedPlanner {
    float pos = 0;
    float vel = 0;
    float acc = 0;
    
    float vMax, aMax, jMax;   // 运动约束上限
    float targetPos;
    bool active = false;
    
    void init(float v, float a, float j) {
        vMax = v; aMax = a; jMax = j;
    }
    
    void setTarget(float target) {
        targetPos = target;
        active = true;
    }
    
    // 核心：基于加加速度限制的七段式或简化三段式更新
    void update(float dt) {
        if (!active) return;
        
        float distToGoal = targetPos - pos;
        float distAbs = fabs(distToGoal);
        float sign = (distToGoal > 0) ? 1.0f : -1.0f;
        
        // 制动距离判断
        float stopDist = (vel * vel) / (2.0f * aMax);
        
        if (distAbs <= stopDist && fabs(vel) > 0.001f) {
            // 减速区：施加负 jerk
            acc -= jMax * dt * sign;
        } else if (fabs(vel) < vMax) {
            // 加速区：施加正 jerk
            acc += jMax * dt * sign;
        } else {
            // 匀速区：加速度趋向零
            acc *= 0.9f;
        }
        
        // 约束传播
        acc = constrain(acc, -aMax, aMax);
        vel += acc * dt;
        vel = constrain(vel, -vMax, vMax);
        pos += vel * dt;
        
        if (distAbs < 0.001f && fabs(vel) < 0.01f) {
            vel = 0; acc = 0; active = false;
        }
    }
};

// 左右轮各一个规划器（差速运动解耦）
JerkLimitedPlanner plannerL, plannerR;

// ==================== 约束参数 ====================
const float WHEEL_BASE = 0.25f;
const float WHEEL_RADIUS = 0.05f;

// 对配送机器人而言，加加速度限制直接影响货物安全
const float J_MAX = 2.0f;   // 保守值：2 m/s³，避免急加速
const float A_MAX = 1.0f;   // 最大加速度
const float V_MAX = 1.0f;   // 最大速度

// ==================== 主程序 ====================
void setup() {
    Serial.begin(115200);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    plannerL.init(V_MAX, A_MAX, J_MAX);
    plannerR.init(V_MAX, A_MAX, J_MAX);
    
    Serial.println("Jerk-limited planner ready");
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    static unsigned long lastTime = micros();
    float dt = (micros() - lastTime) / 1e6f;
    if (dt < 0.001f) return;
    lastTime = micros();
    
    // 模拟高层速度指令（来自DWA或路径跟踪器）
    float cmdV = 0.6f;   // 目标线速度
    float cmdW = 0.3f;   // 目标角速度
    
    // 差速分解
    float targetVL = cmdV - cmdW * WHEEL_BASE / 2.0f;
    float targetVR = cmdV + cmdW * WHEEL_BASE / 2.0f;
    
    // 设置目标（累积位置目标，简化处理）
    plannerL.setTarget(plannerL.pos + targetVL * 0.1f);
    plannerR.setTarget(plannerR.pos + targetVR * 0.1f);
    
    // 加加速度约束平滑
    plannerL.update(dt);
    plannerR.update(dt);
    
    // 输出到BLDC（速度模式）
    motorL.move(plannerL.vel / WHEEL_RADIUS);
    motorR.move(plannerR.vel / WHEEL_RADIUS);
    
    delay(5);
}

要点：S曲线规划器在制动距离判断处决定何时开始减速，而非简单的“距离小于阈值就刹车”。jMax 是配送机器人货物安全的关键参数——限制加加速度变化率，消除了梯形速度曲线的“加速度阶跃”。

3、全向底盘运动学逆解 + MRAC自适应分配
场景：麦克纳姆轮/全向底盘在负载不均或打滑时，需要运动学逆解 + 自适应修正，将全局速度指令分解为各轮速度，并动态补偿。

#include <SimpleFOC.h>

// ==================== 四轮全向底盘 ====================
BLDCMotor motors[4];  // 0:FL, 1:FR, 2:BL, 3:BR
Encoder encoders[4];

// ==================== 底盘几何参数 ====================
const float WHEEL_RADIUS = 0.05f;
const float WHEEL_BASE_X = 0.20f;  // 轴距一半
const float WHEEL_BASE_Y = 0.20f;  // 轮距一半

// ==================== MRAC 自适应参数 ====================
struct WheelMRAC {
    float theta = 1.0f;
    float gamma = 0.15f;
    float refModelVal = 0.0f;
    
    // 参考模型 + 自适应律
    float update(float target, float actual) {
        // 一阶参考模型
        refModelVal = refModelVal * 0.9f + target * 0.1f;
        
        float error = actual - refModelVal;
        
        // 自适应律：误差驱动 theta 更新
        theta += gamma * error * target * 0.01f;
        theta = constrain(theta, 0.3f, 2.5f);
        
        return theta * target;
    }
};

WheelMRAC mrac[4];

// ==================== 运动学逆解 ====================
// 全向底盘：全局速度 (Vx, Vy, W) → 四轮速度
void omniInverseKinematics(float Vx, float Vy, float W, 
                            float wheelSpeeds[4]) {
    float k = WHEEL_BASE_X + WHEEL_BASE_Y;
    
    // 麦克纳姆轮运动学
    wheelSpeeds[0] = Vx - Vy - W * k;  // FL
    wheelSpeeds[1] = Vx + Vy + W * k;  // FR
    wheelSpeeds[2] = Vx + Vy - W * k;  // BL
    wheelSpeeds[3] = Vx - Vy + W * k;  // BR
    
    // 约束到电机能力范围
    for (int i = 0; i < 4; i++) {
        wheelSpeeds[i] = constrain(wheelSpeeds[i], -2.0f, 2.0f);
    }
}

// ==================== 运动约束检查 ====================
float computeMaxReachableSpeed(float Vx, float Vy, float W) {
    float wheelSpeeds[4];
    omniInverseKinematics(Vx, Vy, W, wheelSpeeds);
    
    float maxWheelSpeed = 0;
    for (int i = 0; i < 4; i++) {
        maxWheelSpeed = max(maxWheelSpeed, fabs(wheelSpeeds[i]));
    }
    
    // 电机最大轮速限制
    const float MAX_WHEEL_SPEED = 2.0f;
    
    if (maxWheelSpeed > MAX_WHEEL_SPEED) {
        float scale = MAX_WHEEL_SPEED / maxWheelSpeed;
        return scale;  // 需要缩放的系数
    }
    return 1.0f;
}

// ==================== 主程序 ====================
void setup() {
    Serial.begin(115200);
    
    for (int i = 0; i < 4; i++) {
        motors[i].linkDriver(&driver);
        motors[i].controller = MotionControlType::velocity;
        motors[i].init();
        motors[i].initFOC();
    }
}

void loop() {
    for (int i = 0; i < 4; i++) {
        motors[i].loopFOC();
    }
    
    // 高层指令（来自路径跟踪器）
    float Vx = 0.8f;
    float Vy = 0.3f;
    float W = 0.2f;
    
    // 1. 运动学逆解
    float wheelSpeeds[4];
    omniInverseKinematics(Vx, Vy, W, wheelSpeeds);
    
    // 2. 约束检查与缩放（如果轮速超限）
    float scale = computeMaxReachableSpeed(Vx, Vy, W);
    for (int i = 0; i < 4; i++) {
        wheelSpeeds[i] *= scale;
    }
    
    // 3. MRAC 自适应修正 + 执行
    for (int i = 0; i < 4; i++) {
        // 读取实际轮速（编码器微分）
        float actualSpeed = encoders[i].getVelocity() * WHEEL_RADIUS;
        
        // 自适应修正（谁阻力大谁出力大）
        float corrected = mrac[i].update(wheelSpeeds[i], actualSpeed);
        
        motors[i].move(corrected / WHEEL_RADIUS);
    }
    
    delay(10);
}

要点：全向底盘的运动约束体现在逆解后的轮速缩放——当某轮速度超过电机能力时，按比例缩放所有轮速，保证运动方向不变但整体减速。MRAC 的 theta 参数变化率间接反映负载分布，实现“重载轮自动加大出力”的智能分配。

要点解读

1. 运动约束是“规划-执行”之间的翻译层
   栅格路径只回答“走哪里”，不回答“怎么走”。DWA的三重约束求交集是这一翻译层的核心：速度边界（电机极限）∩ 加速度边界（物理可达）∩ 安全制动边界（能刹停）。三者的交集才是机器人真正能执行的速度空间。

2. 加加速度约束决定负载安全
   对背负货物、液体的机器人而言，加速度的变化率（jerk）比加速度本身更危险。梯形速度曲线的加速度是阶跃的，意味着 jerk 在切换点无穷大，货物会因“顿挫”而倾洒。S曲线通过限制 jMax 让加速度连续变化，这是配送机器人“不洒货”的数学保障。

3. 逆解后的约束检查比逆解本身更重要
   全向底盘运动学逆解是线性代数，但逆解结果可能超出电机能力。computeMaxReachableSpeed 在逆解后检查最大轮速，超限则按比例缩放全部轮速。这保证了底盘不会因为“要求它做不到的动作”而堵转或失步。

4. 自适应修正应对“模型不准”
   运动学逆解假设轮子完美抓地、无打滑。实际中负载偏移、地面摩擦差异会导致某些轮“出力大但跑得慢”。MRAC的 theta 参数随误差自适应调整，间接感知负载分布，实现“谁阻力大谁出力大”的动态分配。

5. BLDC的闭环带宽决定了约束层的有效性
   DWA输出的速度指令是高频变化的（10-20Hz），S曲线的加加速度限制要求电机能快速跟踪加速度变化。BLDC配合FOC的电流环带宽远高于有刷电机或步进电机，是运动约束层“想得出来就能执行出来”的物理基础。开环ESC会因响应滞后破坏约束层精心计算的平滑性。

4、动态制动包络与旋转扫掠 —— 速度相关的虚拟边界
静态膨胀半径的错误在于它假设"边界与速度无关"。但速度 0.8 m/s、延迟 80ms 时，光通信与执行延迟就吃掉 64mm，加上制动距离 v²/(2a) 共 864mm——远超任何静态半径。

#include <SimpleFOC.h>

/* ================= 底盘几何 ================= */
const float L_FRONT = 300.0f;      // 前悬 mm
const float L_BACK  = 280.0f;
const float W_HALF  = 200.0f;
const float TRACK   = 400.0f;
// 旋转扫掠半径：最远角点到中心的距离
const float FOOT_R  = sqrtf(L_FRONT * L_FRONT + W_HALF * W_HALF);

/* ================= 运动约束寄存器（全部现场标定） ================= */
struct MotionLimits {
  float vMax;       // mm/s
  float aMax;       // mm/s²  纵向加减速
  float aLatMax;    // mm/s²  侧向（防滑防倾覆）
  float wMax;       // rad/s
  float alphaMax;   // rad/s² 角加速度
  float jMax;       // mm/s³  jerk
  float tReact;     // s  传感器+通信+执行总延迟
  float mu;         // 地面摩擦系数
};
MotionLimits lim = { 800.0f, 400.0f, 250.0f, 1.6f, 6.0f, 1500.0f, 0.08f, 0.7f };

/* ================= 动态包络：平移制动 + 旋转扫掠 ================= */
float brakingMargin(float v, float a) {
  float va = fabsf(v);
  return va * lim.tReact + (va * va) / (2.0f * fmaxf(a, 1.0f));
}

float dynamicInflation(float v, float w) {
  float trans = brakingMargin(v, lim.aMax);
  // 旋转期间角速度对应的扫掠附加量
  float rot = FOOT_R * fabsf(w) * lim.tReact
            + 0.5f * FOOT_R * w * w / fmaxf(lim.alphaMax, 0.1f);
  // 平移项权重 <1：包络沿运动方向而非全向展开
  return FOOT_R + 0.6f * trans + rot;
}

边界判定用连续距离场（电子围栏地图、代价图的欧氏距离变换都可提供），而不是逐像素踩点：

/* ================= 边界距离查询 ================= */
extern float sdfQuery(float x, float y);   // 返回最近边界距离 mm，负=已在界外

const float HARD_MARGIN = 60.0f;           // 物理硬余量，不可压缩

struct EnvelopeResult {
  float vScale;      // 0..1 速度缩放
  bool  hardStop;    // 已触及硬边界
  float infl;        // 本次使用的膨胀半径（调试用）
};

EnvelopeResult checkEnvelope(float vCmd, float wCmd, float x, float y) {
  EnvelopeResult r;
  r.infl = dynamicInflation(vCmd, wCmd);
  float d = sdfQuery(x, y);

  float hardLimit = FOOT_R + HARD_MARGIN;
  if (d <= hardLimit) {                 // 硬边界：立即停
    r.vScale = 0.0f; r.hardStop = true;
    return r;
  }
  float span = r.infl - hardLimit;      // 软约束带宽
  if (span <= 1.0f || d >= r.infl) {
    r.vScale = 1.0f;                    // 余量充足
  } else {
    r.vScale = constrain((d - hardLimit) / span, 0.0f, 1.0f);
  }
  r.hardStop = false;
  return r;
}

/* ================= 应用：缩放而非截断 ================= */
float vCmd = 0.0f, wCmd = 0.0f;

void applyEnvelope(float x, float y, float dt) {
  EnvelopeResult e = checkEnvelope(vCmd, wCmd, x, y);

  // 关键：显式引入减速迟滞，避免临界点抖动
  const float SCALE_RATE_DOWN = 3.0f;   // 每秒允许的缩放下降率
  static float scaleNow = 1.0f;
  if (e.vScale < scaleNow)
    scaleNow = fmaxf(e.vScale, scaleNow - SCALE_RATE_DOWN * dt);
  else
    scaleNow = fminf(e.vScale, scaleNow + 0.5f * dt);

  if (e.hardStop) {
    emergencyStop();
    return;
  }
  float v = vCmd * scaleNow;
  float w = wCmd * scaleNow;            // 角速度同步缩放，保住转弯几何
  driveDiff(v, w);                       // v/w 映射到双轮
}

scaleNow 的迟滞是整个案例里最容易被省掉、又最不该省的部分。没有它，机器人会在"软约束区间内→减速→距离变大→重新加速"之间形成极限环，表现为边界附近的周期性顿挫。

5、速度空间可行域求解 —— 摩擦圆、侧向加速度与坡度约束
底盘是非完整约束系统：约束不出现在位置空间，而出现在速度空间。所以所有硬约束必须在 (v, ω) 平面上裁剪，而不是在路径上打补丁。

/* ================= 物理参数 ================= */
const float G      = 9810.0f;    // mm/s²
const float MASS   = 35.0f;      // kg
const float MU     = 0.7f;       // 基准摩擦系数
const float CG_H   = 180.0f;     // 重心高 mm
const float K_TILT = 0.6f;       // 抗倾覆安全系数

/* 电机侧牵引力上限：由电流限幅与减速比换算 */
float wheelForceMax() {
  const float T_MAX = 3.5f;      // N·m 单关节峰值力矩（含限流）
  const float R_W   = 75.0f;     // 轮半径 mm
  return T_MAX / (R_W / 1000.0f);  // N
}

/* ================= 摩擦圆与倾覆限速 ================= */
struct Feasibility {
  bool  ok;
  float vMaxHere;
  float wMaxHere;      // 在给定 v 下允许的角速度
  float reason;
};

Feasibility probe(float v, float w, float slopeRad, float latOffset) {
  Feasibility f = { true, 0, 0, 0 };

  // ---- 1. 静态限幅 ----
  if (fabsf(v) > lim.vMax || fabsf(w) > lim.wMax) { f.ok = false; f.reason = 1; return f; }

  // ---- 2. 侧向加速度：a_lat = v·ω ----
  float aLat = fabsf(v * w);
  if (aLat > lim.aLatMax) { f.ok = false; f.reason = 2; return f; }

  // ---- 3. 摩擦圆：sqrt(a_long² + a_lat²) ≤ μ·g·cosθ ----
  float aLong = fabsf((v - lastV) / DT);
  float aCap  = MU * G * cosf(slopeRad) / 1000.0f;   // 单位统一为 m/s² 系
  float aLat_m = aLat / 1000.0f;
  float aLong_m = aLong / 1000.0f;
  float need  = sqrtf(aLong_m*aLong_m + aLat_m*aLat_m);
  if (need > aCap) { f.ok = false; f.reason = 3; return f; }

  // ---- 4. 坡度：驱动力需求不应超过牵引力上限 ----
  float fSlope = MASS * G * sinf(slopeRad) / 1000.0f;
  float fAccel = MASS * fabsf(aLong_m);
  if (fSlope + fAccel > wheelForceMax() * 2.0f) {   // 双驱动轮
    f.ok = false; f.reason = 4; return f;
  }

  // ---- 5. 抗倾覆：高速过弯禁止 ----
  float vTilt = sqrtf(K_TILT * G * (TRACK * 0.5f) / fmaxf(aLat / fmaxf(fabsf(v), 1.0f), 1e-3f));
  if (v > vTilt) { f.ok = false; f.reason = 5; return f; }

  // ---- 6. 载荷偏移：外侧轮是否会被抬起 ----
  float latShift = latOffset;                        // 重心横向偏移 mm（估得）
  float aLatCap  = K_TILT * G * (TRACK * 0.5f - fabsf(latShift)) / fmaxf(CG_H, 1.0f);
  if (aLat > aLatCap) { f.ok = false; f.reason = 6; return f; }

  // 通过：反算该 v 下可用的最大角速度
  f.vMaxHere = lim.vMax;
  f.wMaxHere = fminf(lim.wMax, lim.aLatMax / fmaxf(fabsf(v), 1.0f));
  return f;
}

/* ================= 动态窗口采样：只在增量可行域内搜 ================= */
const float DT = 0.02f;        // 50 Hz
float lastV = 0.0f, lastW = 0.0f;

struct Cmd { float v, w, cost; };

Cmd solveVelocitySpace(float vWant, float wWant, float slopeRad,
                       float latOffset, float goalDirErr,
                       float clearFront, float clearLeft, float clearRight) {
  // 增量窗口：一个周期内加速度可达范围
  float vLo = lastV - lim.aMax * DT, vHi = lastV + lim.aMax * DT;
  vLo = fmaxf(vLo, -lim.vMax); vHi = fminf(vHi, lim.vMax);

  Cmd best = { 0, 0, 1e18f };
  const int NV = 15, NW = 21;

  for (int i = 0; i < NV; i++) {
    float v = vLo + (vHi - vLo) * i / (NV - 1);
    for (int j = 0; j < NW; j++) {
      float wLo = lastW - lim.alphaMax * DT;
      float wHi = lastW + lim.alphaMax * DT;
      wLo = fmaxf(wLo, -lim.wMax); wHi = fminf(wHi, lim.wMax);
      float w = wLo + (wHi - wLo) * j / (NW - 1);

      Feasibility f = probe(v, w, slopeRad, latOffset);
      if (!f.ok) continue;                    // 硬约束：不可行直接剪掉

      // 代价：接近期望 + 朝向对齐 + 余量奖励 + 速度跟踪
      float c = 0.0f;
      c += 1.6f * fabsf(v - vWant) / lim.vMax;
      c += 1.2f * fabsf(w - wWant) / lim.wMax;
      c += 2.0f * fabsf(goalDirErr);
      // 前瞻净空越小，越惩罚高速
      float tStop = fmaxf(v, 1.0f) / lim.aMax;
      float reach = v * lim.tReact + 0.5f * lim.aMax * tStop * tStop;
      if (reach > clearFront) c += 8.0f * (reach - clearFront) / 200.0f;
      if (w > 0 && reach > clearLeft)  c += 4.0f;
      if (w < 0 && reach > clearRight) c += 4.0f;
      // 平滑性：抑制抖动
      c += 0.4f * (fabsf(v - lastV) + 200.0f * fabsf(w - lastW)) / lim.vMax;

      if (c < best.cost) { best.v = v; best.w = w; best.cost = c; }
    }
  }

  // 无可行解 → 保守刹车，而不是沿用上周期指令
  if (best.cost > 1e17f) { best.v = 0; best.w = 0; }
  return best;
}

关键在于 probe() 里的六条判据全部返回布尔可行/不可行，不做加权折中。摩擦圆和抗倾覆一旦以软惩罚形式进入代价函数，求解器就一定会在极端工况下"试探性"地突破它——而这类约束被突破的后果是侧滑和翻车，不是绕远路。

6、曲率自适应时间参数化 —— 高阶导航的轨迹生成与在线重规划
高阶导航的结构是两段式：先在空间上算曲率受限速度上限，再做时间维度上的 jerk 受限积分。这个解耦让"路径好不好"和"开得顺不顺"可以分别调优。

/* ================= 路径：世界系折线 ================= */
struct P2 { float x, y; };
#define NP 40
P2      path[NP];
uint8_t nPath = 0;

/* ================= 三点曲率（有符号：正=左转） ================= */
float curvatureAt(int i) {
  if (i <= 0 || i >= nPath - 1) return 0.0f;
  P2 a = path[i-1], b = path[i], c = path[i+1];
  float abx = b.x - a.x, aby = b.y - a.y;
  float bcx = c.x - b.x, bcy = c.y - b.y;
  float cross = abx * bcy - aby * bcx;
  float ab = sqrtf(abx*abx + aby*aby);
  float bc = sqrtf(bcx*bcx + bcy*bcy);
  float ca = sqrtf((c.x-a.x)*(c.x-a.x) + (c.y-a.y)*(c.y-a.y));
  float denom = ab * bc * ca;
  if (denom < 1e-3f) return 0.0f;
  return 2.0f * cross / denom;        // 单位 1/mm
}

/* ================= 空间限速：曲率 + 侧向加速度 + 角速度上限 ================= */
float vLimitAt(int i) {
  float k = fabsf(curvatureAt(i));
  float vK = (k < 1e-6f) ? lim.vMax : sqrtf(lim.aLatMax / k);
  // 差速转向还需要 |v·κ| ≤ wMax
  float vW = (k < 1e-6f) ? lim.vMax : lim.wMax / k;
  // 摩擦圆预留：过弯时保留一部分纵向加速度能力
  float vMu = sqrtf(MU * G * 0.8f / fmaxf(k, 1e-6f));
  return fminf(lim.vMax, fminf(vK, fminf(vW, vMu)));
}

/* ================= 时间参数化：先反算制动，再 jerk 受限前向积分 ================= */
float vProf[NP];         // 每点速度上限
float aProf[NP];         // 每点加速度状态

void buildSpeedProfile() {
  // ---- 1. 前向：曲率与边界限速取小 ----
  for (int i = 0; i < nPath; i++)
    vProf[i] = vLimitAt(i);

  // ---- 2. 边界项：动态包络的距离场限速 ----
  for (int i = 0; i < nPath; i++) {
    float d = sdfQuery(path[i].x, path[i].y);
    float hardLimit = FOOT_R + HARD_MARGIN;
    if (d <= hardLimit) { vProf[i] = 0.0f; continue; }
    // 由余量反算该点允许的最高进近速度
    float allowance = d - hardLimit;
    float vMaxBound = sqrtf(2.0f * lim.aMax * allowance);
    vProf[i] = fminf(vProf[i], vMaxBound);
  }

  // ---- 3. 反向传播：从终点往回做制动可达性检查 ----
  vProf[nPath-1] = 0.0f;                    // 终点必停
  for (int i = nPath - 2; i >= 0; i--) {
    float ds = hypotf(path[i+1].x - path[i].x, path[i+1].y - path[i].y);
    float vAllow = sqrtf(vProf[i+1] * vProf[i+1] + 2.0f * lim.aMax * ds);
    vProf[i] = fminf(vProf[i], vAllow);
  }

  // ---- 4. 前向传播：起步端不能超限（防起步即超速） ----
  for (int i = 1; i < nPath; i++) {
    float ds = hypotf(path[i].x - path[i-1].x, path[i].y - path[i-1].y);
    float vAllow = sqrtf(vProf[i-1] * vProf[i-1] + 2.0f * lim.aMax * ds);
    vProf[i] = fminf(vProf[i], vAllow);
  }
}

/* ================= 在线执行：jerk 受限 + 重规划 ================= */
float sNow = 0.0f;              // 沿路径的弧长进度 mm
float vNow = 0.0f;
float aNow = 0.0f;

/* 弧长反查当前点索引 */
int locateIndex(float s) {
  float acc = 0.0f;
  for (int i = 1; i < nPath; i++) {
    acc += hypotf(path[i].x - path[i-1].x, path[i].y - path[i-1].y);
    if (acc >= s) return i;
  }
  return nPath - 1;
}

void runTrajectory(float dt) {
  int idx = locateIndex(sNow);
  int idxNext = min(idx + 1, nPath - 1);
  float ds = hypotf(path[idxNext].x - path[idx].x, path[idxNext].y - path[idx].y);
  if (ds < 1e-3f) { ds = 1.0f; }

  // 目标速度取当前点与前视点的较小值：提前减速
  float vTgt = fminf(vProf[idx], vProf[min(idx + 3, nPath - 1)]);

  // 加速度需求 + 限幅
  float aTgt = (vTgt - vNow) / dt;
  aTgt = constrain(aTgt, -lim.aMax, lim.aMax);

  // jerk 限幅
  aNow += constrain(aTgt - aNow, -lim.jMax * dt, lim.jMax * dt);

  vNow += aNow * dt;
  if (vNow < 0) { vNow = 0; aNow = 0; }
  sNow += vNow * dt;

  // 前轮偏航率：由曲率与纵向速度给出
  float k = curvatureAt(idx);
  float wCmd = vNow * k;

  // 再叠加案例二的可行域检查（曲率限速已含，但载荷与坡度偏移在此修正）
  float slope = slopeEstimate(idx);
  float latOff = comLateralOffset();
  Feasibility f = probe(vNow, wCmd, slope, latOff);
  if (!f.ok) {
    vNow *= 0.7f;                 // 约束不可行：先降速而非停车
    wCmd = constrain(wCmd, -f.wMaxHere, f.wMaxHere);
  }

  // 边界包络最终仲裁
  EnvelopeResult e = checkEnvelope(vNow, wCmd, path[idx].x, path[idx].y);
  if (e.hardStop) { emergencyStop(); return; }
  driveDiff(vNow * e.vScale, wCmd * e.vScale);
}

/* ================= 在线重规划：滚动时域 ================= */
void replanCycle() {
  static uint32_t tPlan = 0;
  if (millis() - tPlan < 200) return;      // 5 Hz 重规划，与外环错开
  tPlan = millis();

  // 1) 重新截取局部路径（从当前位形到目标）
  reseatLocalPath(sNow);
  // 2) 若边界或约束发生显著变化，重算速度剖面
  buildSpeedProfile();
  // 3) 若全局路径被新障碍切断，触发上层重规划
  if (!isPathFeasible()) requestGlobalReplan();
}

void loop() {
  // FOC 环由中断/高频任务处理
  static uint32_t tTraj = 0;
  if (millis() - tTraj >= 20) {            // 50 Hz 轨迹环
    float dt = (millis() - tTraj) / 1000.0f;
    tTraj = millis();
    runTrajectory(dt);
    replanCycle();
  }
  delay(1);
}

第 2 步的边界限速与第 3 步的反向制动传播在数学上是同一个约束的两种表达——前者是空间约束（离边界多远就得多慢），后者是时间约束（终点必须能停住）。两者必须在同一遍计算中取最小值，分开处理会导致"曲率允许全速但边界不允许"的矛盾被忽略。

要点解读
第一，动态边界的本质是速度的函数，静态膨胀半径是一个错误的抽象。 制动距离按 v² 增长，通信与执行延迟按 v 线性增长，所以余量需求随速度超线性上升：0.3 m/s 可能只需 15cm，0.8 m/s 就需要近 90cm。更棘手的是旋转——底盘在转向时最远角点的扫掠半径可达 √(L²+W²)，远大于车体半长，这就是"直行没事、转弯撞墙"的成因。所以包络必须拆成"平移制动项 + 旋转扫掠项"，且旋转项要含 αmax（角加速度）。此外，包络的缩放必须引入迟滞（案例一的 SCALE_RATE_DOWN），否则在软约束带内会形成减速—加速的极限环，表现为边界附近的周期性顿挫。

第二，运动约束必须在速度空间裁剪，且硬约束绝不能用软惩罚代替。 差速底盘是非完整约束系统，它的物理约束（侧向加速度、摩擦圆、抗倾覆）都表达在 (v, ω) 或 (v, ω, a) 上，不在位置空间。工程上最常见的错误是把"过弯太快"写成代价函数里的一项，指望求解器自己权衡——但代价优化只保证"总体最优"，不保证"这条底线不被突破"，在极端工况下必然给出一个略微越界的解。正确做法是六条判据全部返回布尔可行性（案例二的 probe()），不可行就剪枝；只有平滑性、进度、净空这类可折中项才进代价函数。另外要注意单位统一：μ·g 是 m/s²，而底盘速度常用 mm/s，混算造成十倍二十倍偏差是这类代码的经典 bug。

第三，高阶导航的核心是把"空间路径"与"时间参数化"解耦，曲率限速是这个解耦的枢纽。 空间上，路径的优劣由曲率、净空、坡度共同决定；时间上，流畅与否由 jerk、加速度传播、制动可达性共同决定。两者的桥梁是 v_lim = min(v_max, √(a_lat/κ), ω_max/κ)——曲率越大，允许的速度越低，且这个关系是开方反比，非常陡峭。案例三的做法是先算逐点限速，再反向传播制动约束、前向传播起步约束，最后用 jerk 受限积分推进。这个结构的好处是"改路径"和"改驾驶风格"互不干扰，调参时能快速定位问题层。

第四，动态环境下的导航必须滚动时域执行，且每一层都要有各自的兜底。 边界会变（人走过来）、约束会变（载荷移位、地面变湿导致 μ 下降）、路径会失效（临时障碍封路）。所以轨迹生成必须按 50Hz 量级滚动，速度剖面按 5Hz 重算，全局路径按需重规划——三层时间尺度分开，不能挤在同一个循环里。更重要的是每层都要有独立的降级路径：约束不可行时先降速而非停车（案例三的 vNow *= 0.7f），速度空间无解时输出零指令而非沿用上周期（案例二的 best.cost > 1e17f），边界触发硬限时直接急停。这与"轨迹规划失败就停车"的思路相反——行进中突然停车本身就可能被后车追尾或被行人撞上，所以"可控降速"往往比"立即停止"更安全。

第五，所有约束参数都必须实测标定，且随工况在线修正，绝不能写死。 μ 会从干燥水泥的 0.4–0.7 变到积水地面的 0.2 以下；a_lat_max 取决于重心高度与轮距，装载后会显著下降；a_max 受限于电机可用力矩与减速比，电池电压跌落时也会缩水；t_react 包含传感器处理、通信与执行器延迟，必须实测而不是估算。这些参数的标定方法应写进自检流程：直线加速测 a_max、定半径圆周逐步提速找侧滑临界测 μ 与 a_lat_max、突然松油门测延迟。同时在线层面要加参数观测器——相电流与编码器加速度的偏差可以反推 μ 的下降，重心偏移可由四轮支撑力估计（如案例三用到的 comLateralOffset()）。归根到底，这套导航的可信度来自"边界随速度自适应、约束在速度空间硬裁剪、轨迹空间时间解耦、三层滚动时域各有兜底、参数实测并在线观测"这五件事同时成立。

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