“Arduino BLDC 之机器人户外高低差视觉识别 + 动态预规划越障”可以理解为：用视觉或深度传感器提前识别前方地形的高度差、台阶、沟壑、斜坡和障碍轮廓，再在触障前动态生成越障策略，最后通过 BLDC 关节或驱动轮执行抬腿、抬升、差速转向、步幅调节和姿态补偿。它的重点不是“撞到再反应”，而是“提前看到、提前算、提前调整步态和力矩”。

一、主要特点

1. 高低差识别从“避障”升级为“地形可通行性评估”
   传统避障只判断“前面有没有东西”；高低差识别需要进一步判断“能不能走、怎么走、哪里能落脚”。视觉或深度传感器可以提取前方地形的坡度、台阶高度、沟壑宽度、边缘位置和落足区域稳定性，形成局部高程地图或代价地图。
2. 动态预规划强调“触障前完成策略切换”
   预规划意味着在机器人真正接触台阶、斜坡或沟壑之前，就完成步态、步幅、抬腿高度、机身姿态和速度曲线的调整。比如四足机器人看到前方有台阶时，可以提前降低速度、增大抬腿高度、调整重心位置，并选择更稳定的落足点。
3. BLDC 执行层提供高动态响应和柔顺力矩输出
   越障过程中，关节需要快速响应高度变化，同时又要避免硬性冲击。BLDC 配合 FOC 可以实现较细的力矩控制，适合用于四足关节抬腿、底盘升降、差速转向和冲击吸收。相比普通有刷电机或开环 ESC，BLDC 更容易实现速度环、位置环和力矩环协同。
4. 感知-规划-控制通常采用分层架构
   户外地形识别和局部地图构建计算量较大，Arduino 级别 MCU 更适合作为底层执行节点。常见做法是：上位机或高性能边缘模块负责视觉处理、地形分割和局部路径生成；Arduino/ESP32/STM32 负责接收目标步态、速度指令和关节角度，再驱动 BLDC 执行。
5. 需要融合 IMU、编码器、足端接触等信息
   仅靠视觉容易受光照、反光、阴影、尘土和运动模糊影响。工程上通常把视觉高低差信息与 IMU 姿态、轮速或关节编码器、足端力/触地信号融合，判断机身是否倾斜、是否打滑、是否已经稳定落足。

二、应用场景

1. 野外巡检与勘探机器人
   在草地、碎石路、田埂、土坡、沟坎等环境中移动。视觉提前识别高低差，可避免机器人误入坑洼或侧翻。
2. 灾后搜救与废墟穿越
   废墟中存在不规则台阶、断裂楼板、碎石堆和狭窄通道。预规划越障能让机器人提前选择更安全的落足路径，减少被动磕碰。
3. 户外物流与物资转运
   园区、工地、山地营地等场景可能存在门槛、坡道、减速带、台阶和临时障碍。背负运输时还需考虑负载变化对重心和越障能力的影响。
4. 农业与林业作业平台
   垄沟、泥地、树根、低矮灌木和土坎都会造成高低差。视觉预规划可以提前降低速度、调整航向，避免底盘托底或陷车。
5. 教学科研与竞赛平台
   适合验证深度视觉、局部高程地图、越障步态、BLDC 力矩控制、IMU 姿态补偿和分层控制架构。

三、需要注意的事项

1. 视觉识别必须考虑户外环境干扰
   强光、逆光、阴影、雨雾、泥水和低照度都会影响深度估计。不能只依赖单一视觉源，建议结合 ToF、结构光、双目、线激光或下视传感器，并加入滤波和置信度判断。
2. 高低差识别要有“可通行性”阈值
   识别到高度差后，不能简单视为障碍，而要判断是否在机器人通过能力范围内。例如台阶高度、沟壑宽度、坡度角、地面摩擦和机身重心位置都会影响是否可越。
3. 预规划必须与步态或底盘动力学匹配
   视觉给出的目标抬腿高度、落足点或转向角，必须经过运动学/动力学约束检查。否则可能出现抬腿不足、落足打滑、重心超出支撑多边形、关节力矩饱和等问题。
4. 切换越障模式时要平滑过渡
   从平地行走到越障模式，速度、加速度、关节角度和力矩都不能突变。应使用插值、斜率限制或 S 曲线过渡，避免 BLDC 电流冲击、齿轮 backlash 放大和机身俯仰震荡。
5. Arduino 平台要合理分配算力
   完整的高程地图构建、地形分割和局部规划通常不适合放在 8 位 Arduino 上。更稳妥的方案是让上位机输出简化指令，例如“前方 60cm 有 12cm 台阶，执行越障步态 A”，Arduino 只负责执行关节轨迹、BLDC 闭环和安全保护。
6. BLDC 驱动层必须做电流、温度和堵转保护
   越障瞬间关节负载可能突然增大，尤其是抬腿、支撑和落地阶段。应设置电流限幅、电压跌落保护、编码器异常检测和堵转降级策略，防止驱动器过热或电机失步。
7. 落足验证和失效保护优先级最高
   视觉预规划只能降低风险，不能保证每次落足都可靠。触地后仍需通过足端力、关节电流、IMU 姿态和编码器位置验证是否稳定。一旦检测到打滑、悬空、侧倾过大或传感器异常，应立即降低刚度、停止前进或进入安全姿态。
   整体来看，这条技术链路可以概括为：视觉/深度感知 → 局部高程或地形代价地图 → 高低差与可通行性评估 → 越障策略预规划 → BLDC 关节/底盘执行 → 触地验证与动态修正。它的工程价值在于把“被动越障”升级为“主动预判越障”，但难点也在于视觉不确定性、地形复杂性和 BLDC 执行安全必须同时闭环。

1、单级台阶检测 + 预规划抬腿高度
场景：固定高度台阶（路缘石、门槛），机器人接近后识别台阶高度，提前调整摆动腿抬腿高度，避免足端撞击台阶立面。

#include <SimpleFOC.h>

// ==================== BLDC 关节（单腿简化） ====================
BLDCMotor hipMotor = BLDCMotor(7);
BLDCMotor kneeMotor = BLDCMotor(7);
BLDCDriver3PWM hipDriver(9, 10, 11, 8);
BLDCDriver3PWM kneeDriver(3, 5, 6, 4);

// ==================== 视觉接口（上位机/OpenMV 串口） ====================
struct VisionData {
    bool detected;
    float stepHeight;     // 台阶高度 (m)
    float stepDistance;   // 首级台阶距离 (m)
    float stepDepth;      // 踏面深度 (m)
};

VisionData vision = {false, 0, 0, 0};

// ==================== 步态预规划参数 ====================
float nominalLiftHeight = 0.08;    // 平地步态抬腿高度
float safetyMargin = 0.03;         // 越障安全余量
float plannedLiftHeight = 0.08;    // 规划后的抬腿高度

// 腿长参数（用于逆运动学）
const float L1 = 0.12;  // 大腿
const float L2 = 0.10;  // 小腿

// ==================== 状态机 ====================
enum GaitState { APPROACH, PRE_PLAN, SWING };
GaitState state = APPROACH;

void setup() {
    Serial.begin(115200);
    Serial1.begin(115200);  // 与上位机/OpenMV 通信
    
    hipMotor.linkDriver(&hipDriver);
    kneeMotor.linkDriver(&kneeDriver);
    hipMotor.controller = MotionControlType::angle;
    kneeMotor.controller = MotionControlType::angle;
    hipMotor.init(); kneeMotor.init();
    hipMotor.initFOC(); kneeMotor.initFOC();
}

void loop() {
    hipMotor.loopFOC();
    kneeMotor.loopFOC();
    
    readVision();
    
    switch (state) {
        case APPROACH:
            // 正常行走，等待视觉触发
            if (vision.detected) {
                state = PRE_PLAN;
                Serial.print("台阶检测: 高度=");
                Serial.print(vision.stepHeight);
                Serial.print("m, 距离=");
                Serial.println(vision.stepDistance);
            }
            break;
            
        case PRE_PLAN:
            // 核心：根据台阶高度预规划抬腿高度
            // 抬腿高度 = 台阶高度 + 安全余量，确保足端高于台阶顶面
            plannedLiftHeight = vision.stepHeight + safetyMargin;
            
            // 限幅：不超过关节运动学极限
            plannedLiftHeight = constrain(plannedLiftHeight, 
                                          nominalLiftHeight, 
                                          0.20);
            
            Serial.print("预规划抬腿高度: ");
            Serial.println(plannedLiftHeight);
            state = SWING;
            break;
            
        case SWING:
            // 执行摆动相（简化：仅展示目标高度计算）
            // 实际需要贝塞尔曲线或多项式插值生成轨迹
            executeSwingWithHeight(plannedLiftHeight);
            // 落地后回到 APPROACH
            state = APPROACH;
            vision.detected = false;
            break;
    }
    
    delay(10);
}

void readVision() {
    if (Serial1.available() >= 5) {
        // 简化协议：'D' + float stepHeight
        if (Serial1.read() == 'D') {
            byte buf[4];
            Serial1.readBytes(buf, 4);
            memcpy(&vision.stepHeight, buf, 4);
            vision.detected = true;
        }
    }
}

// 足端位置 → 关节角度（简化二连杆逆运动学）
void footToJointAngles(float x, float z, float& hip, float& knee) {
    float dist = sqrt(x*x + z*z);
    dist = constrain(dist, 0.01f, L1 + L2 - 0.01f);
    
    float cosKnee = (L1*L1 + L2*L2 - dist*dist) / (2*L1*L2);
    cosKnee = constrain(cosKnee, -1.0f, 1.0f);
    knee = PI - acos(cosKnee);
    
    float alpha = atan2(x, z);
    float cosBeta = (L1*L1 + dist*dist - L2*L2) / (2*L1*dist);
    cosBeta = constrain(cosBeta, -1.0f, 1.0f);
    float beta = acos(cosBeta);
    hip = alpha + beta;
}

void executeSwingWithHeight(float height) {
    // 简化：计算最高点对应的关节角度
    float hipAngle, kneeAngle;
    footToJointAngles(0.0, height, hipAngle, kneeAngle);
    
    hipMotor.move(hipAngle);
    kneeMotor.move(kneeAngle);
    delay(300);
    
    // 放下腿回到标称位置
    footToJointAngles(0.05, 0.12, hipAngle, kneeAngle);
    hipMotor.move(hipAngle);
    kneeMotor.move(kneeAngle);
}

要点：视觉输出的不是“有障碍”，而是显式几何参数（高度、距离、深度）。这些参数直接进入步态规划器，plannedLiftHeight = stepHeight + safetyMargin 是预规划最核心的映射。

2、连续高低差 + 动态落足点可达性校验
场景：台阶高度和深度连续变化（如破损台阶、瓦砾堆），视觉识别后需要校验落足点是否在腿部的运动学可达域内，不可达则调整机身位置。

#include <SimpleFOC.h>

// ==================== 视觉数据结构 ====================
struct StepGeometry {
    float height;      // 台阶高度
    float depth;       // 踏面深度
    float distance;    // 距机器人距离
    float yawAngle;    // 台阶走向角（偏航）
};

StepGeometry step = {0, 0, 0, 0};

// ==================== 腿部运动学 ====================
const float L1 = 0.12, L2 = 0.10;
const float MAX_REACH = 0.21;  // L1 + L2 - 余量
const float MIN_REACH = 0.08;

// 髋关节安装位置（相对于机身中心，前左腿为例）
const float HIP_X = 0.10;
const float HIP_Y = 0.08;
const float HIP_Z = 0.05;  // 髋关节离地高度

// ==================== 可达性校验 ====================
bool checkFootReachability(float targetX, float targetY, float targetZ) {
    // 转换到髋关节坐标系
    float dx = targetX - HIP_X;
    float dy = targetY - HIP_Y;
    float dz = targetZ - HIP_Z;
    
    // 计算足端到髋关节的距离（考虑偏航）
    float horizontalDist = sqrt(dx*dx + dy*dy);
    float totalDist = sqrt(horizontalDist*horizontalDist + dz*dz);
    
    // 检查是否在可达域内
    if (totalDist > MAX_REACH) {
        Serial.println("落足点超出最大伸展");
        return false;
    }
    if (totalDist < MIN_REACH) {
        Serial.println("落足点过近，关节无法收拢");
        return false;
    }
    
    // 检查关节角度是否在安全范围内（简化）
    float hipAngle, kneeAngle;
    float dist = constrain(totalDist, 0.01f, L1 + L2 - 0.01f);
    float cosKnee = (L1*L1 + L2*L2 - dist*dist) / (2*L1*L2);
    cosKnee = constrain(cosKnee, -1.0f, 1.0f);
    kneeAngle = PI - acos(cosKnee);
    
    // 膝关节角度范围检查
    if (kneeAngle < 0.3 || kneeAngle > 2.8) {
        Serial.println("膝关节角度超限");
        return false;
    }
    
    return true;
}

// ==================== 动态调整落足点 ====================
struct FootTarget {
    float x, y, z;
    bool valid;
};

FootTarget planFootTarget(StepGeometry& s) {
    FootTarget target;
    
    // 标称落足点：踏面中央偏前
    target.x = s.distance + s.depth * 0.5;
    target.y = 0.0;
    target.z = s.height;  // 踏面高度
    
    // 校验可达性
    target.valid = checkFootReachability(target.x, target.y, target.z);
    
    if (!target.valid) {
        // 调整策略：缩小落足前伸距离
        target.x = s.distance + s.depth * 0.3;
        target.valid = checkFootReachability(target.x, target.y, target.z);
        Serial.println("落足点调整：前移量缩小");
    }
    
    if (!target.valid) {
        // 进一步调整：机身需要前移，而非足端强行伸展
        Serial.println("需机身前移以覆盖落足点");
        // 实际系统中触发机身位置调整
    }
    
    return target;
}

void setup() {
    Serial.begin(115200);
    Serial1.begin(115200);
    // BLDC 初始化（略）
}

void loop() {
    // 接收台阶几何
    if (Serial1.available()) {
        // 解析协议：高度,深度,距离,偏航
        step.height = 0.10;
        step.depth = 0.15;
        step.distance = 0.35;
        step.yawAngle = 0.0;
        
        FootTarget ft = planFootTarget(step);
        
        Serial.print("落足点: X="); Serial.print(ft.x);
        Serial.print(" Z="); Serial.print(ft.z);
        Serial.print(" 可达:"); Serial.println(ft.valid);
    }
    delay(100);
}

要点：视觉给出的“可落足区域”不一定能被机器人踩到。checkFootReachability 在规划阶段就拦截不可达目标，避免执行时关节到达极限导致堵转或失步。

3、完整越障状态机 + 视觉预规划与触觉确认协同
场景：户外离散地形（石阶、树根、路缘），视觉负责“往哪踩”，触觉负责“踩没踩稳”，状态机协调两者。

#include <SimpleFOC.h>

// ==================== 视觉输入 ====================
struct VisionInput {
    bool detected;
    float height;
    float distance;
    float depth;
};

VisionInput vision = {false, 0, 0, 0};

// ==================== 足端触觉 ====================
const int FORCE_PIN_FL = A0;
float footForceFL = 0;
const float CONTACT_THRESHOLD = 15.0;

// ==================== 步态状态机 ====================
enum GaitState {
    IDLE,
    APPROACH,
    PRE_PLAN,
    LIFT,
    SWING_FORWARD,
    LAND_CHECK,
    BODY_SHIFT
};

GaitState gaitState = IDLE;

// ==================== 预规划参数 ====================
float plannedLiftHeight = 0.08;
float plannedStepLength = 0.15;
float heightCorrection = 0;  // 触觉落点补偿

// ==================== 主循环 ====================
void setup() {
    Serial.begin(115200);
    Serial1.begin(115200);
    pinMode(FORCE_PIN_FL, INPUT);
    // BLDC 初始化（略）
    Serial.println("视觉预规划+触觉确认越障就绪");
}

void loop() {
    // ===== 1. 感知层 =====
    readVision();
    readFootForce();
    
    // ===== 2. 状态机调度 =====
    switch (gaitState) {
        case IDLE:
            if (vision.detected) {
                gaitState = PRE_PLAN;
                Serial.println("检测到台阶，进入预规划");
            }
            break;
            
        case PRE_PLAN:
            // 视觉参数 → 步态参数映射
            plannedLiftHeight = vision.height + 0.03;
            plannedLiftHeight = constrain(plannedLiftHeight, 0.05, 0.20);
            
            plannedStepLength = vision.distance + vision.depth * 0.5;
            
            Serial.print("预规划: 抬腿=");
            Serial.print(plannedLiftHeight);
            Serial.print(" 步长=");
            Serial.println(plannedStepLength);
            
            gaitState = LIFT;
            break;
            
        case LIFT:
            // 垂直抬腿阶段
            executeLift(plannedLiftHeight);
            gaitState = SWING_FORWARD;
            break;
            
        case SWING_FORWARD:
            // 摆动前伸阶段
            executeSwingForward(plannedStepLength, plannedLiftHeight);
            gaitState = LAND_CHECK;
            break;
            
        case LAND_CHECK:
            // 触觉确认：是否触地
            if (footForceFL > CONTACT_THRESHOLD) {
                Serial.print("触地确认, 力: ");
                Serial.println(footForceFL);
                gaitState = BODY_SHIFT;
            } else {
                // 未触地：微调落点高度（继续下降）
                heightCorrection -= 0.005;
                Serial.print("落点补偿: ");
                Serial.println(heightCorrection);
                
                // 安全保护：下降超过一定量仍未触地 → 放弃
                if (heightCorrection < -0.05) {
                    Serial.println("触地失败，回退");
                    gaitState = IDLE;
                    heightCorrection = 0;
                }
            }
            break;
            
        case BODY_SHIFT:
            // 重心转移，准备下一腿
            delay(200);
            vision.detected = false;
            heightCorrection = 0;
            gaitState = IDLE;
            break;
    }
    
    delay(20);
}

void readVision() {
    if (Serial1.available()) {
        // 模拟：实际通过串口接收上位机数据
        vision.detected = true;
        vision.height = 0.15;
        vision.distance = 0.40;
        vision.depth = 0.20;
    }
}

void readFootForce() {
    footForceFL = analogRead(FORCE_PIN_FL) * 0.05;
    footForceFL = constrain(footForceFL, 0, 50);
}

void executeLift(float height) {
    // 简化：控制髋关节抬腿
    Serial.print("执行抬腿: ");
    Serial.println(height);
    delay(200);
}

void executeSwingForward(float length, float height) {
    Serial.print("执行前伸: ");
    Serial.print(length);
    Serial.print(" 高度: ");
    Serial.println(height);
    delay(300);
}

要点：视觉预规划解决“往哪踩”，触觉确认解决“踩没踩稳”。LAND_CHECK 状态中，若足端力未超过阈值，继续微调下降（heightCorrection -= 0.005），直到触地或超时放弃。这是视觉与触觉协同的完整闭环。

要点解读

1. 视觉输出必须是“显式几何参数”而非“有障碍”
   对嵌入式步态规划而言，最有用的不是原始深度图或“前方有障碍”的粗分类，而是台阶高度、深度、距离、走向角这些可直接进入步态规划器的可解释参数。高度决定抬腿量，深度决定落足点前后位置，走向角决定机身偏航补偿。

2. 预规划的核心是“摆动相提前适配”
   平地步态遇到台阶时，摆动腿会撞上台阶立面。预规划的本质是：在看到台阶后、足端接触前，就把摆动轨迹的抬腿高度和落足位置调整到位。plannedLiftHeight = stepHeight + safetyMargin 是最简单的映射，但必须配套可达性校验——视觉认为“有平面”不代表腿能踩到。

3. 上位机负责“看懂”，下位机负责“踩稳”
   Arduino Uno/Nano 的算力不足以同时做台阶识别、逆运动学和多关节FOC。实际工程必须分层：上位机（OpenMV/树莓派）做视觉几何提取和步态预规划，通过串口下发“抬多高、跨多远、往哪落”；下位机（ESP32/Teensy+SimpleFOC）做逆运动学解算和关节闭环执行。

4. 触觉是视觉预规划的“最后一道保险”
   视觉可能误判台阶高度（反光、阴影、遮挡），也可能无法判断踏面是否坚实（松软土壤、湿滑石板）。足端触觉（压力传感器或BLDC关节电流）在落足瞬间确认“是否真正踩稳”。若触地力异常或未触地，立即修正落点或回退。视觉决定往哪踩，触觉决定踩得稳不稳。

5. BLDC 的柔顺执行能力决定了预规划的“落地质量”
   预规划给出的是关节目标角度序列，但足端从空中落到踏面的冲击由执行层决定。SimpleFOC 的力矩限幅或阻抗控制可以让足端触地瞬间“软着陆”——碰到台阶时产生柔顺位移而非硬性撞击。这对背负负载或穿越易碎地形的户外机器人尤为关键。

4、ToF 深度阵列 → 机体/世界坐标高度图与高低差特征提取
这一版把 8×8 ToF 的距离数据，经安装角与机身姿态变换，投影成以机器人脚下水平面为基准的高程栅格，再提取坡度与台阶边缘，前馈给主动悬挂的 BLDC 轴。

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

/* ================= 深度前端：VL53L5CX 8x8 ================= */
SparkFun_VL53L5CX imager;
VL53L5CX_ResultsData data;

const uint8_t  RES       = 8;
const float    FOV_HALF  = 22.5f;       // 视场 ±22.5°（45° 模式）
const float    DEG2RAD   = 0.01745329f;

/* 传感器安装：位于机体前上方，向下俯 12°
   机体系约定：x 前、y 左、z 上，原点在四轮接地面形心 */
const float SENS_X = 260.0f;            // mm 前移
const float SENS_Z = 340.0f;            // mm 离地高
const float SENS_PITCH = 12.0f;         // 向下俯角

/* ================= 高程栅格 ================= */
#define GX 48                           // 横向格数（=2.4 m）
#define GY 56                           // 纵向格数（=2.8 m）
const float CELL     = 50.0f;           // 每格 50 mm
const float ORIG_X   = 0.0f;            // 栅格前缘起点（机体正下方）
const float HALF_W   = GX * CELL * 0.5f;

float   hmap[GY][GX];                   // 高程 mm，NAN 表示未知
uint8_t hconf[GY][GX];                  // 置信度 0..255
uint32_t hstamp[GY][GX];                // 时间戳 ms

/* ================= 机身姿态（来自 IMU / 融合） ================= */
float poseRoll = 0.0f, posePitch = 0.0f, poseZ = 0.0f;   // rad, mm

/* ================= 单点到高程的坐标变换 ================= */
/* 输入：区索引(i=列,j=行)、距离 r(mm)
   输出：机体系坐标 pb[3]，再转到世界系求 z */
void cellToBody(int i, int j, float r, float pb[3]) {
  float a = (-FOV_HALF + (i + 0.5f) * (2 * FOV_HALF / RES)) * DEG2RAD;  // 水平角
  float b = (-FOV_HALF + (j + 0.5f) * (2 * FOV_HALF / RES)) * DEG2RAD;  // 垂直角

  // 传感器坐标系：z 轴沿光轴指向正前，y 轴向下
  float xs = r * sinf(a) * cosf(b);
  float ys = r * sinf(b);
  float zs = r * cosf(a) * cosf(b);

  // 传感器俯角旋转（绕 y 轴）
  float cp = cosf(-SENS_PITCH * DEG2RAD), sp = sinf(-SENS_PITCH * DEG2RAD);
  float xr =  cp * zs + sp * ys;
  float yr =  xs;
  float zr = -sp * zs + cp * ys;

  // 平移到机体系
  pb[0] = xr + SENS_X;          // 前
  pb[1] = yr;                   // 左
  pb[2] = SENS_Z - zr;          // 上（zr 为向下量，取负）
}

/* ================= 投影入栅格（取最高值，凸起更关键） ================= */
void projectToGrid(int i, int j, float dist) {
  float pb[3];
  cellToBody(i, j, dist, pb);

  // 机体系 -> 世界系水平投影（小角度近似，户外大坡需完整旋转）
  float wx = pb[0] * cosf(posePitch) + pb[2] * sinf(posePitch);
  float wy = pb[1] * cosf(poseRoll)  - pb[2] * sinf(poseRoll);
  float wz = poseZ + pb[2];

  int gy = (int)((wx - ORIG_X) / CELL);
  int gx = (int)((wy + HALF_W) / CELL);
  if (gx < 0 || gx >= GX || gy < 0 || gy >= GY) return;

  if (isnan(hmap[gy][gx]) || wz > hmap[gy][gx]) hmap[gy][gx] = wz;
  hconf[gy][gx]  = 255;
  hstamp[gy][gx] = millis();
}

/* ================= 高低差特征：坡度 + 台阶 ================= */
const float STEP_TH = 60.0f;            // 台阶高度阈值 mm
const float SLOPE_TH = 0.35f;           // 约 19° 纵坡上限

bool isStepEdge(uint8_t gy, uint8_t gx) {
  if (gy == 0 || isnan(hmap[gy][gx]) || isnan(hmap[gy-1][gx])) return false;
  return fabsf(hmap[gy][gx] - hmap[gy-1][gx]) > STEP_TH;
}

float localSlope(uint8_t gy, uint8_t gx) {
  if (gy < 2 || gy >= GY || isnan(hmap[gy][gx]) || isnan(hmap[gy-2][gx])) return 0.0f;
  return (hmap[gy][gx] - hmap[gy-2][gx]) / (2 * CELL);
}

/* ================= BLDC 主动悬挂：前馈姿态 ================= */
/* 四路竖直轴或摆臂，接口抽象；姿态前馈由高程图直接生成 */
void legTorque(int i, float tau);
float legAngle(int i);
float legVel(int i);

const float BASE_WB = 480.0f;           // 轴距 mm（前后采样点间距）
float feedPitch = 0.0f;                 // 前馈俯仰指令 rad

void computeFeedforward() {
  // 取机器人前方 0.6m 与后方 0.3m 处的高程均值
  auto sampleRow = [](float x_mm) -> float {
    int gy = (int)((x_mm - ORIG_X) / CELL);
    if (gy < 0 || gy >= GY) return NAN;
    float s = 0; int n = 0;
    for (int gx = GX/2 - 4; gx <= GX/2 + 4; gx++) {
      if (!isnan(hmap[gy][gx])) { s += hmap[gy][gx]; n++; }
    }
    return n ? s / n : NAN;
  };

  float hf = sampleRow(600.0f);
  float hr = sampleRow(-300.0f);
  if (isnan(hf) || isnan(hr)) { feedPitch = 0.0f; return; }

  // 前方高 -> 抬高前腿，即俯仰角向负方向补偿
  feedPitch = -atan2f(hf - hr, 900.0f);
  feedPitch = constrain(feedPitch, -0.30f, 0.30f);
}

/* ================= 主循环 ================= */
void setup() {
  Serial.begin(115200);
  Wire.begin();
  imager.begin();
  imager.setResolution(RES * RES);
  imager.setRangingMode(SF_VL53L5CX_RANGING_MODE::CONTINUOUS, 15);
  imager.startRanging();

  for (int y = 0; y < GY; y++)
    for (int x = 0; x < GX; x++) hmap[y][x] = NAN;
  Serial.println(F("outdoor heightmap ready"));
}

void loop() {
  mL.loopFOC(); mR.loopFOC();           // 底盘
  for (int i = 0; i < 4; i++) { /* 悬挂轴 loopFOC */ }

  static uint32_t tMap = 0;
  if (millis() - tMap >= 66) {           // 15 Hz 更新高程图
    tMap = millis();

    if (imager.isDataReady()) {
      imager.getRangingData(&data);
      for (int j = 0; j < RES; j++) {
        for (int i = 0; i < RES; i++) {
          int k = j * RES + i;
          uint8_t st = data.target_status[k];
          // 太阳光下 status 会大量为 5/255（无效），必须丢弃
          if (st == 5 || st == 255) continue;
          float d = (float)data.distance_mm[k];
          if (d < 100.0f || d > 4000.0f) continue;
          projectToGrid(i, j, d);
        }
      }
    }

    // 过期格清理：超过 800ms 未更新视为失效
    uint32_t now = millis();
    for (int y = 0; y < GY; y++)
      for (int x = 0; x < GX; x++)
        if (hstamp[y][x] && now - hstamp[y][x] > 800) {
          hmap[y][x] = NAN; hconf[y][x] = 0;
        }

    computeFeedforward();
  }
  delayMicroseconds(200);
}

hmap 里存的是"世界系下相对机器人接地面"的高度，而不是传感器距离——这一个区别决定了整条链路是否正确。第 3 行的 target_status 过滤在户外尤其关键：VL53L5CX 用 940nm 红外，强烈日照下无效点比例常常过半，不过滤就会在高度图里凭空造出深坑。

5、基于高程代价图的 D* Lite 增量重规划
静态 A* 每次地形更新都要从头搜，户外根本来不及。D* Lite 的价值在于：地形图局部变化时只更新受影响节点的 g/rhs，把重规划代价压到接近常数。代价函数里必须显式包含执行器的可达性约束。

/* ================= 复用案例一的高程图 ================= */
extern float hmap[GY][GX];
extern float localSlope(uint8_t gy, uint8_t gx);
extern bool  isStepEdge(uint8_t gy, uint8_t gx);

const float EXTERN float MAX_STEP_UP   = 90.0f;    // 单步最大上台阶 mm
const float MAX_STEP_DOWN = 140.0f;                // 下台阶可更大
const float MAX_SLOPE     = 0.35f;
const float MAX_ROUGH     = 70.0f;                 // 局部高差均方根
const float COST_FREE     = 1.0f;
const float COST_UNKNOWN  = 2.5f;                  // 未知区可通行但代价高

/* ================= 代价：越障能力约束 ================= */
float cellCost(uint8_t gy, uint8_t gx) {
  if (gy == 0 || gx == 0 || gy >= GY-1 || gx >= GX-1) return 1e9f;

  float h  = hmap[gy][gx];
  float hf = hmap[gy+1][gx];
  if (isnan(h) || isnan(hf)) return COST_UNKNOWN;

  float dh = hf - h;
  if (dh >  MAX_STEP_UP)   return 1e9f;      // 上不去，硬禁行
  if (dh < -MAX_STEP_DOWN) return 1e9f;      // 悬崖

  if (fabsf(localSlope(gy, gx)) > MAX_SLOPE) return 1e9f;

  // 粗糙度：3x3 窗口标准差
  float s = 0, s2 = 0; int n = 0;
  for (int a = -1; a <= 1; a++)
    for (int b = -1; b <= 1; b++) {
      float v = hmap[gy+a][gx+b];
      if (!isnan(v)) { s += v; s2 += v*v; n++; }
    }
  if (n > 3) {
    float var = s2/n - (s/n)*(s/n);
    if (sqrtf(var) > MAX_ROUGH) return 1e9f;
  }

  // 基础代价：斜坡与台阶边缘附加惩罚，鼓励平坦路线
  float c = COST_FREE;
  c += 3.0f * fabsf(localSlope(gy, gx));
  if (isStepEdge(gy, gx)) c += 1.5f;
  return c;
}

/* ================= D* Lite 核心 ================= */
#define INF 1e18f
float g[GY][GX], rhs[GY][GX];
uint32_t gver[GY][GX];                 // 地形版本，变化时自增

struct PQ {
  uint16_t idx[GY*GX];
  float    k1[GY*GX], k2[GY*GX];
  int      n = 0;
  void clear() { n = 0; }
  void push(uint16_t i, float a, float b) {
    idx[n] = i; k1[n] = a; k2[n] = b; n++;
  }
  bool top(uint16_t &i, float &a, float &b) {
    if (n == 0) return false;
    int m = 0;
    for (int t = 1; t < n; t++)
      if (k1[t] < k1[m] || (k1[t] == k1[m] && k2[t] < k2[m])) m = t;
    i = idx[m]; a = k1[m]; b = k2[m];
    idx[m] = idx[n-1]; k1[m] = k1[n-1]; k2[m] = k2[n-1]; n--;
    return true;
  }
  void remove(uint16_t i) {
    for (int t = 0; t < n; t++)
      if (idx[t] == i) { idx[t]=idx[n-1]; k1[t]=k1[n-1]; k2[t]=k2[n-1]; n--; return; }
  }
};
PQ pq;

const int8_t NX[8] = {0, 1, 0, -1, 1, 1, -1, -1};
const int8_t NY[8] = {-1, 0, 1, 0, -1, 1, 1, -1};

float heuristic(uint16_t a, uint16_t b) {
  int ax = a % GX, ay = a / GX, bx = b % GX, by = b / GX;
  float dx = bx-ax, dy = by-ay;
  return fminf(fabsf(dx), fabsf(dy)) * 1.414f + fabsf(fabsf(dx)-fabsf(dy));
}

// 双向前驱/后继皆使用八邻域，简化处理
float minSucc(uint16_t u, float &bestCost) {
  int ux = u % GX, uy = u / GX;
  float m = INF; bestCost = COST_FREE;
  for (int d = 0; d < 8; d++) {
    int vx = ux + NX[d], vy = uy + NY[d];
    if (vx < 0 || vx >= GX || vy < 0 || vy >= GY) continue;
    uint16_t v = vy * GX + vx;
    float c = cellCost(uy, ux);
    float dist = (d < 4) ? 1.0f : 1.414f;
    float val = g[vy][vx] + c * dist;
    if (val < m) { m = val; bestCost = c; }
  }
  return m;
}

void updateVertex(uint16_t u, uint16_t goal, uint16_t start) {
  int ux = u % GX, uy = u / GX;
  if (u != goal) { float c; rhs[uy][ux] = minSucc(u, c); }

  pq.remove(u);
  if (g[uy][ux] != rhs[uy][ux]) {
    float h = heuristic(u, start);
    pq.push(u, rhs[uy][ux] + h, rhs[uy][ux]);
  }
}

void computeShortestPath(uint16_t start, uint16_t goal) {
  int guard = 0;
  while (pq.n > 0 && guard++ < 20000) {
    uint16_t u; float k1, k2;
    if (!pq.top(u, k1, k2)) break;
    if (k1 >= heuristic(start, start) + rhs[start/GX][start%GX] &&
        rhs[start/GX][start%GX] == g[start/GX][start%GX]) break;

    int ux = u % GX, uy = u / GX;
    if (g[uy][ux] > rhs[uy][ux]) {
      g[uy][ux] = rhs[uy][ux];
      for (int d = 0; d < 8; d++) {
        int vx = ux + NX[d], vy = uy + NY[d];
        if (vx < 0 || vx >= GX || vy < 0 || vy >= GY) continue;
        updateVertex(vy * GX + vx, goal, start);
      }
    } else {
      g[uy][ux] = INF;
      updateVertex(u, goal, start);
      for (int d = 0; d < 8; d++) {
        int vx = ux + NX[d], vy = uy + NY[d];
        if (vx < 0 || vx >= GX || vy < 0 || vy >= GY) continue;
        updateVertex(vy * GX + vx, goal, start);
      }
    }
  }
}

/* ================= 地形变化时的增量更新 ================= */
void onTerrainChanged(uint8_t gy, uint8_t gx, uint16_t start, uint16_t goal) {
  gver[gy][gx]++;
  updateVertex(gy * GX + gx, goal, start);
  // 只影响邻域，不做全局重算——这是 D* Lite 的核心收益
  for (int d = 0; d < 8; d++) {
    int vx = gx + NX[d], vy = gy + NY[d];
    if (vx < 0 || vx >= GX || vy < 0 || vy >= GY) continue;
    updateVertex(vy * GX + vx, goal, start);
  }
}

/* ================= 路径跟随点生成 ================= */
#define MAX_PATH 64
struct Pt { float x, y; };
Pt path[MAX_PATH];
uint8_t nPath = 0;

void extractPath(uint16_t start, uint16_t goal) {
  nPath = 0;
  int ux = start % GX, uy = start / GX;
  int gx = goal % GX,  gy = goal / GY ? goal / GX : goal / GX;

  while (nPath < MAX_PATH) {
    path[nPath].x = uy * CELL;                    // 机体前向 mm
    path[nPath].y = ux * CELL - HALF_W;           // 机体横向 mm
    nPath++;
    if (uy * GX + ux == goal) break;

    int bx = -1, by = -1; float best = INF;
    for (int d = 0; d < 8; d++) {
      int vx = ux + NX[d], vy = uy + NY[d];
      if (vx < 0 || vx >= GX || vy < 0 || vy >= GY) continue;
      if (g[vy][vx] < best) { best = g[vy][vx]; bx = vx; by = vy; }
    }
    if (bx < 0) break;
    ux = bx; uy = by;
  }
}

/* ================= 调度 ================= */
uint16_t gStart = 2 * GX + GX/2;              // 机器人所在格
uint16_t gGoal  = (GY-2) * GX + GX/2;         // 前方目标格

void replanIfNeeded() {
  // 检测本机周边地形版本变化 -> 触发增量更新
  for (int dy = -2; dy <= 4; dy++)
    for (int dx = -3; dx <= 3; dx++) {
      int gx = GX/2 + dx, gy = 2 + dy;
      if (gx < 0 || gx >= GX || gy < 0 || gy >= GY) continue;
      static uint32_t seen[GY][GX];
      if (gver[gy][gx] != seen[gy][gx]) {
        seen[gy][gx] = gver[gy][gx];
        onTerrainChanged(gy, gx, gStart, gGoal);
      }
    }
  computeShortestPath(gStart, gGoal);
  extractPath(gStart, gGoal);
}

cellCost() 里的 MAX_STEP_UP = 90mm 不是一个纯几何阈值，而是机械腿抬升能力与底盘接近角的联合结果——地形识别得再准，如果代价函数里不含这条约束，规划出的路径仍然是机器人走不动的。这一点是户外越障与室内导航最本质的分野。

6、预规划轨迹 → 摆臂/足式 BLDC 越障执行（前馈 + 触觉反馈）
规划给的是"该落在哪"，执行要解决"怎么落得稳"。这一版用高程图生成未来 1.5 秒的足端高度前馈，叠加 IMU 俯仰反馈与足端触觉软化，形成闭环。

/* ================= 复用：高程图、路径点 ================= */
extern float hmap[GY][GX];
extern Pt    path[MAX_PATH];
extern uint8_t nPath;

/* ================= 四腿 BLDC 接口（同系列前文抽象） ================= */
void  initLegs();
float legAngle(int i);
float legVel(int i);
float legFootForce(int i);
void  legTorque(int i, float tau);

/* ================= 摆动腿轨迹前馈 ================= */
const int   N_LEG = 4;
const float PHASE_OFF[4] = {0.0f, 0.5f, 0.5f, 0.0f};   // Trot
float gaitPeriod = 640.0f;
float qDes[4] = {0, 0, 0, 0};

/* 由高程图预测足端落点处的地面高度 */
float predictGroundZ(float forwardMm, float lateralMm) {
  int gy = (int)(forwardMm / CELL);
  int gx = (int)((lateralMm + HALF_W) / CELL);
  if (gy < 0 || gy >= GY || gx < 0 || gx >= GX) return NAN;

  // 3x3 邻域取中位数：抗单点野值
  float buf[9]; int n = 0;
  for (int a = -1; a <= 1; a++)
    for (int b = -1; b <= 1; b++) {
      int yy = gy+a, xx = gx+b;
      if (yy < 0 || yy >= GY || xx < 0 || xx >= GX) continue;
      if (!isnan(hmap[yy][xx])) buf[n++] = hmap[yy][xx];
    }
  if (n == 0) return NAN;
  for (int i = 1; i < n; i++) {           // 插入排序
    float v = buf[i]; int j = i-1;
    while (j >= 0 && buf[j] > v) { buf[j+1] = buf[j]; j--; }
    buf[j+1] = v;
  }
  return buf[n/2];
}

/* 前馈：把落点地形高度写成足端 z 偏移 */
float zFeed[4] = {0, 0, 0, 0};

void updateZFeedforward(float bodySpeed) {
  // 摆动腿在 T_LIFT 之后落地，落点约在前方 0.35m
  const float REACH = 350.0f;
  const float LAT[4] = { 120.0f, -120.0f, 120.0f, -120.0f };

  for (int i = 0; i < N_LEG; i++) {
    float z = predictGroundZ(REACH + bodySpeed * 0.25f, LAT[i]);
    if (isnan(z)) { zFeed[i] = 0.0f; continue; }   // 未知区不冒险前馈
    // 抬腿高度随地形上升而增加，负高度（坑）则减小但仍留安全余量
    zFeed[i] = constrain(z, -40.0f, 110.0f);
  }
}

/* ================= 阻抗：支撑刚性 / 摆动柔顺 / 冲击软化 ================= */
const float K_STANCE = 11.0f, K_SWING = 3.0f, K_SOFT = 2.2f;
const float B_STANCE = 0.45f, B_SWING = 0.22f;
const float IMPACT_TH = 280;
const float ZETA = 1.0f, J_EFF = 0.02f;

float kNow[4], bNow[4];

void slewK(int i, float kTgt, float bTgt) {
  const float KR = 5.0f, BR = 0.4f;
  kNow[i] += constrain(kTgt - kNow[i], -KR, KR);
  bNow[i] += constrain(bTgt - bNow[i], -BR, BR);
}

/* ================= IMU 姿态反馈 ================= */
float posePitch = 0.0f;
float pitchRef = 0.0f;
float pitchInteg = 0.0f;

float pitchCorrection(float dt) {
  float err = pitchRef - posePitch;
  const float KP = 2.5f, KI = 0.8f;
  pitchInteg += KI * err * dt;
  pitchInteg = constrain(pitchInteg, -0.15f, 0.15f);   // 抗积分饱和
  return constrain(KP * err + pitchInteg, -0.20f, 0.20f);
}

/* ================= 视觉失效保护 ================= */
uint32_t tValidMap = 0;
bool visionOk() { return (millis() - tValidMap) < 500; }

/* ================= 主循环 ================= */
void setup() {
  Serial.begin(115200);
  initLegs();
  for (int i = 0; i < N_LEG; i++) { kNow[i] = K_SWING; bNow[i] = B_SWING; }
  Serial.println(F("outdoor obstacle traversal ready"));
}

void loop() {
  for (int i = 0; i < N_LEG; i++) { /* leg.loopFOC(); */ }

  unsigned long now = millis();
  float t = (float)(now % (unsigned long)gaitPeriod) / gaitPeriod;

  static uint32_t tCtrl = 0;
  if (now - tCtrl >= 10) {                 // 100 Hz
    float dt = (now - tCtrl) / 1000.0f;
    tCtrl = now;

    float bodySpeed = 250.0f;              // 由里程计提供 mm/s

    if (visionOk()) {
      updateZFeedforward(bodySpeed);
      tValidMap = now;
    } else {
      // 视觉失效：收起前馈，降速，切保守姿态
      for (int i = 0; i < N_LEG; i++) zFeed[i] = 0.0f;
      bodySpeed = 120.0f;
      Serial.println(F("VISION LOST -> crawl"));
    }

    float pitchCorr = pitchCorrection(dt);

    for (int i = 0; i < N_LEG; i++) {
      float ph = t + PHASE_OFF[i];
      ph -= floor(ph);
      bool stance = (ph < 0.55f);

      float f = legFootForce(i);
      if (f > IMPACT_TH && stance) {        // 硬触地：瞬时软化
        slewK(i, K_SOFT, 0.20f);
      } else {
        float kTgt = stance ? K_STANCE : K_SWING;
        float bTgt = stance ? (2.0f * ZETA * sqrtf(kTgt * J_EFF)) : B_SWING;
        slewK(i, kTgt, bTgt);
      }

      // 目标足端高度 = 摆动轨迹 + 地形前馈 + 姿态反馈
      float lift = stance ? 0.0f : 45.0f * sinf(PI * (ph / 0.45f));
      qDes[i] = lift + zFeed[i] + pitchCorr * 100.0f;   // rad 折算

      float tau = kNow[i] * (qDes[i] - legAngle(i))
                - bNow[i] * legVel(i);
      tau = constrain(tau, -3.0f, 3.0f);
      legTorque(i, tau);
    }
  }
  delay(1);
}

三处设计决定了它在户外能不能活下来：predictGroundZ() 用 3×3 中位数而非均值（单点野值在逆光和反光下极常见）；visionOk() 超时后立即撤掉前馈转保守爬行（视觉掉线时最危险的是"带着过期高度图自信前进"）；冲击软化与地形前馈分层（前馈管"该抬多高"，软化管"落得多重"）。

要点解读
第一，视觉给出的是"距离"，而机器人需要的是"高度"——中间这一步坐标变换才是精度瓶颈。 从传感器距离到脚下水平面高度，要经过传感器安装角、机身姿态（roll/pitch）两次旋转和平移。机身俯仰 5° 时，3 米外一个点的垂直位置误差可以到 26 厘米，足以把一个可跨越的台阶判成悬崖。所以户外方案必须让 IMU 姿态与深度数据在同一时刻同步采样，并按采样时刻做时间对齐——深度帧率只有 15 Hz，而机身俯仰在行走中持续变化，用"上一帧姿态"去解算"这一帧深度"是常见的系统性偏差源。大坡度地形下小角度近似不再成立，需要完整的旋转矩阵。

第二，户外深度传感的可靠性远低于室内，必须按"会失效"来设计。 ToF 用 940nm 红外，强日照下信噪比骤降，有效量程可能从 4 米缩到 1 米以内；玻璃幕墙、积水反射会让深度出现虚假平面；草地和树影的纹理会让立体匹配大面积失效。因此要有三件套：一是逐点状态过滤（案例一里对 target_status 的剔除），二是多源冗余（ToF + 立体视觉 + 超声波，按环境自动加权），三是失效降级策略——视觉掉线时不是简单停车，而是降速 + 依赖触觉爬行。此外，户外还要处理相机标定随温度漂移、镜头沾灰沾水的问题，工程上通常设自动标定检测：当长期无有效深度点时报警。

第三，高程栅格是视觉与规划之间唯一正确的接口，它的分辨率与更新率是一对硬约束。 栅格太粗（如 20cm）会让 9cm 的台阶消失在平均里；太细（如 2cm）则在机械振动下充满噪声。50mm 是常见折中。填充策略要选"取最高值"而不是"取平均"——对越障而言，误判一个凸起为平地会导致碰撞，误判平地为一小凸起只是多抬一次腿，代价不对称。同时必须做过期清理（案例一的 800ms 失效），否则机器人走过一个坑之后，那个坑会永远留在图里，规划器会一直试图绕开一个不存在的地形。另外，栅格坐标是机体固连的，机器人在移动时整张图都要随之平移，这会引入插值误差，所以长距离行进应定期做局部图重建。

第四，动态预规划的核心不是"搜得快"，而是"知道哪些约束是硬的"。 户外地形每帧都在变，从头搜的 A* 每 15Hz 重算一次 3000 格栅格，在单核 MCU 上会挤占 FOC 环的时间片。D* Lite 的增量更新把计算量压到只跟变化区域相关，这是它成为主流的原因。但更关键的是代价函数的设计：坡度上限、单步高差上限、粗糙度上限这些必须作为硬约束（代价设无穷）而不是软惩罚，否则在局部最优的驱使下，规划器一定会在某次迭代中"试探性"地穿过一个走不通的台阶。软硬约束的划分依据来自执行器实测：MAX_STEP_UP 由腿的抬升行程和底盘接近角共同决定，MAX_SLOPE 由防滑与重心高度共同决定，这些值必须现场标定。

第五，视觉是前馈、本体是反馈，且安全保护必须独立于视觉链路。 规划再准也只是"预测该怎么做"，真实落地时的打滑、支撑力不足、地形与预测不符，都只能靠 IMU、关节编码器和足端力来纠正。正确结构是视觉生成前馈轨迹（占主要贡献），IMU 反馈修正姿态残差，触觉触发瞬时软化，三者分层且时间尺度不同：前馈 10–20Hz、姿态反馈 100Hz、力控内环 1kHz。安全链必须独立：视觉超时撤前馈、姿态超限立即停止推进、电流与力矩硬限幅、通信看门狗，以及一个物理急停开关直接切断驱动器使能——这条链路不经过任何软件判断。最后，电池供电的户外机器人还要注意大负载越障时的瞬时电流冲击，母线电压跌落会直接导致 FOC 失步，所以限流阈值要按最差工况（满载爬坡）留足裕度，而不是按平路标定值设定。

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