“Arduino BLDC之人形机器人足端压力滑移感知与闭环补偿（多传感器融合）”代表了当前仿生机器人领域最前沿的“本体感知（Proprioception）”与底层柔顺力控技术的深度结合。在人形机器人的动态行走中，足端与地面的接触状态是维持全身平衡的基石。该系统旨在解决传统运动学控制在复杂、低摩擦地形下因足端打滑导致状态估计发散和机器人跌倒的痛点。通过多源传感器深度融合与BLDC电机的高频力矩补偿，系统能够实现类似人类神经反射的防滑与姿态恢复机制。

一、 主要特点

1. 多源异构传感器深度融合与状态估计
   足端滑移的精准感知是闭环补偿的前提。系统摒弃了单一传感器的局限，采用多模态融合架构：
   六维力/力矩传感器（F/T）与阵列式压力： 安装在脚踝处，直接测量地面反作用力（GRF）和压力中心（CoP）的移动轨迹。当检测到水平切向力逼近摩擦锥边界或压力中心急剧偏移时，系统可判定为微观滑移。
   IMU与关节编码器融合： 采用接触增强的扩展卡尔曼滤波（Contact-Aided EKF）或不变扩展卡尔曼滤波（InEKF）。当IMU检测到机身异常加速度，而运动学模型（基于编码器）推算足端速度不为零时，系统能精准判定宏观打滑，并利用IMU的高频数据抑制里程计漂移。
2. 前瞻式与反射式双级防滑控制架构
   系统构建了类似人体神经系统的两级响应机制：
   前瞻式控制： 结合视觉或环境模型，提前预判地面摩擦系数变化，从源头调整步态参数（如降低步幅、降低质心高度）。
   反射式控制： 当滑移发生时，系统在极短时间（如10ms内）触发反射机制。基于摩擦锥约束，严格限制足底水平力，通过全身动力学控制（WBC）快速重新分配各关节力矩，确保零力矩点（ZMP）始终落在支撑多边形内，防止倾覆。
3. 基于BLDC FOC的高动态柔顺力控
   防滑补偿对执行器的带宽和力矩密度提出了极致要求。BLDC电机配合FOC（磁场定向控制）算法，在转矩控制模式下运行，具备毫秒级的动态响应。在足端触地瞬间或发生滑移时，BLDC电机能像“弹簧”一样吸收冲击，并根据补偿算法迅速调整输出力矩，实现柔顺且精确的力控。
   二、 典型应用场景
4. 极端非结构化环境下的灾难救援
   在地震废墟、泥石流或冰雪覆盖的山区，地面摩擦系数极低且充满不确定性。人形机器人凭借足端滑移感知与闭环补偿能力，能够在湿滑岩石或松软沙地上维持动态平衡，甚至利用“跨步策略”在单脚支撑于不稳定支点时快速调整姿态，深入危险区域执行搜救任务。
5. 复杂工业与户外巡检作业
   在化工厂、变电站或农业大棚等场景中，地面可能布满油污、积水或泥泞。该系统能确保机器人在这些易滑地面上保持稳定的巡检姿态，避免因打滑导致的设备跌落或任务中断。
6. 医疗康复与外骨骼机器人
   在辅助下肢瘫痪者行走或增强士兵负重能力时，外骨骼需要与使用者的步态意图完美同步。通过感知足底与地面的接触状态，系统能实时计算并提供辅助力矩，在用户踩到湿滑路面时提供额外的支撑与平衡保护，实现“人机一体”的安全行走。
7. 前沿机器人动力学与AI算法验证
   作为高校和科研机构验证强化学习（RL）、全身控制（WBC）及多传感器融合算法的理想平台。研究人员可通过该平台探索高动态跳跃、抗冲击缓冲及复杂地形下的自适应步态生成。
   三、 需要注意的关键事项
8. 算力瓶颈与分布式主从架构
   全身动力学模型（WBC）、状态估计（EKF）以及高频FOC控制对算力要求极高，远超单一Arduino的处理能力。
   对策： 必须采用分布式架构。由高性能上位机（如Jetson Nano、树莓派）负责繁重的模型计算和轨迹规划，通过高速总线（如EtherCAT、CAN FD）将关节力矩指令下发至各个关节的从控制器（如STM32、ESP32等32位MCU），由其专门负责底层的BLDC转矩闭环控制。
9. 机械传动间隙与结构谐振
   齿轮箱的反向间隙（Backlash）是关节力控的“杀手”，会导致控制迟滞和系统振荡，严重影响滑移补偿的精度。
   对策： 腿部关节必须选用低反向间隙的精密减速器（如谐波减速器）。同时，频繁的加减速可能激发机械结构的固有频率，需在控制算法中加入陷波滤波器，或在机械结构上增加阻尼设计。
10. 严苛的电磁兼容（EMC）与传感器同步
    BLDC电机是强电磁干扰源，其高频PWM噪声极易导致IMU数据跳变或编码器丢步，进而引发状态估计灾难性发散。
    对策： 必须进行严格的电源隔离（使用独立DC-DC模块为传感器供电），强弱电走线呈90°垂直交叉，并对IMU和编码器信号线进行屏蔽处理。此外，不同传感器数据的时间戳必须严格同步（硬件触发同步），否则会导致状态估计失真。
11. 动力学模型的不确定性与安全回退机制
    精确的动力学参数（质量、惯性张量）难以获取，且模型误差会严重影响控制性能。
    对策： 需通过系统辨识实验获取参数，并采用鲁棒控制或自适应控制来在线补偿误差。同时，必须设置硬件级安全护栏（如倾角超限急停、过流保护）。当AI或高级算法失效时，底层能自动回退至基础的安全PID模式或紧急制动（如“跪地”模式），确保物理安全。

1、基于IMU的ZMP实时推算与足端滑移补偿
适用场景：双足或四足机器人在动态行走中，通过IMU推算ZMP位置，实时微调关节角度以抑制足端滑移引发的不稳定。

/* ===== 基于IMU的ZMP实时推算与步态微调 =====
 * 硬件：ESP32 + BLDC关节电机 + MPU6050 + 足底力传感器
 * 核心：LIPM模型推算ZMP，ZMP偏离时通过关节微调实现滑移补偿
 * 参考：ZMP优化步态 + 四足BLDC姿态稳定控制方案
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor joints[6];  // 6自由度关节
MPU6050 imu;

// ==================== ZMP参数 ====================
float zmpTargetX = 0;          // 目标ZMP位置
float zmpActualX = 0;          // 实际ZMP（IMU推算）
float comHeight = 0.20;        // 质心高度(m)
float footLength = 0.12;       // 脚掌长度(m)

// ==================== 滑移检测状态 ====================
struct SlipState {
    float slipAmount;          // 估算滑移量
    float slipIntegral;        // 滑移积分（累计偏差）
    bool slipDetected;
};
SlipState slip = {0, 0, false};

// ==================== 步态表（预规划）====================
struct GaitFrame { float angles[6]; float zmpX; };
const int GAIT_FRAMES = 40;
GaitFrame gaitTable[GAIT_FRAMES];
int frameIndex = 0;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    imu.initialize();
    
    // BLDC电机初始化
    for (int i = 0; i < 6; i++) {
        joints[i].controller = MotionControlType::angle;
        joints[i].init(); joints[i].initFOC();
    }
    // 初始化步态表（需预计算逆运动学）
    initGaitTable();
}

void loop() {
    for (int i = 0; i < 6; i++) joints[i].loopFOC();
    
    // ==================== 1. IMU数据采集 ====================
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    float pitch = atan2(ay, az) * 57.3;
    float roll = atan2(ax, az) * 57.3;
    
    // ==================== 2. 步态计时 ====================
    static unsigned long lastStepTime = 0;
    if (millis() - lastStepTime > 50) {
        frameIndex = (frameIndex + 1) % GAIT_FRAMES;
        lastStepTime = millis();
    }
    GaitFrame currentFrame = gaitTable[frameIndex];
    
    // ==================== 3. 【核心】ZMP实时推算（LIPM模型）====================
    float accX = ax / 16384.0 * 9.81;
    // LIPM简化模型: ZMP = -(comHeight / g) * accX
    zmpActualX = -(comHeight / 9.81) * accX;
    
    // ==================== 4. 【核心】ZMP误差检测与滑移判别 ====================
    float zmpError = currentFrame.zmpX - zmpActualX;
    
    // 滑移检测：ZMP持续偏离目标超过阈值
    if (abs(zmpError) > 0.015) {
        slip.slipIntegral += zmpError * 0.02;
        slip.slipIntegral = constrain(slip.slipIntegral, -0.1, 0.1);
        slip.slipAmount = 0.7 * zmpError + 0.3 * slip.slipIntegral;
        slip.slipDetected = true;
    } else {
        slip.slipAmount *= 0.95;  // 衰减
        slip.slipDetected = false;
    }
    
    // ==================== 5. 关节微调补偿 ====================
    float correctedAngles[6];
    for (int i = 0; i < 6; i++) {
        correctedAngles[i] = currentFrame.angles[i];
    }
    
    if (slip.slipDetected) {
        // 通过髋关节/踝关节微调将ZMP拉回目标
        float correction = slip.slipAmount * 2.0;
        correctedAngles[0] += correction;   // 左髋
        correctedAngles[3] -= correction;   // 右髋
        // 踝关节微调辅助
        correctedAngles[2] += correction * 0.3;
        correctedAngles[5] -= correction * 0.3;
        
        Serial.print("⚠️ 滑移检测: ");
        Serial.println(slip.slipAmount);
    }
    
    for (int i = 0; i < 6; i++) {
        joints[i].move(correctedAngles[i]);
    }
    
    delay(20);
}

核心要点：
LIPM模型：线性倒立摆模型用质心加速度反推ZMP位置，公式为ZMP = -(comHeight/g) × accX。ZMP落在支撑多边形内是稳定步态的"黄金判据"。
滑移检测：当实际ZMP持续偏离目标超过阈值时，判定发生足端滑移。用积分项累积偏差，避免单帧噪声误触发。
关节微调补偿：通过髋关节角度修正移动ZMP位置，类似"脚踝策略"。滑移量越大，修正幅度越大。

2、带迟滞状态机的足底接触判别与滑移补偿
适用场景：人形机器人在不同地面条件行走，需要精确判别足底触地/滑移状态，并具备迟滞特性防止状态抖动。

/* ===== 带迟滞状态机的足底接触判别与滑移补偿 =====
 * 硬件：ESP32 + BLDC电机 + 六维力传感器 + IMU + 足底压力阵列
 * 核心：迟滞状态机判别接触状态，多模态特征融合滑移检测
 * 参考：足底接触判别迟滞状态机设计 + IEEE模糊分层触滑检测
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor joints[6];

// ==================== 足底传感器 ====================
#define FOOT_FORCE_PIN A0      // 足底压力（模拟）
#define FOOT_TORQUE_PIN A1     // 足底力矩

// ==================== 接触状态机 ====================
enum ContactState { AIRBORNE, CONTACT_PENDING, STABLE_CONTACT, SLIPPING };
ContactState leftFootState = AIRBORNE;
ContactState rightFootState = AIRBORNE;

// ==================== 迟滞参数 ====================
const float CONTACT_THRESHOLD_ENTER = 3.0;   // 触地进入阈值(N)
const float CONTACT_THRESHOLD_EXIT = 1.5;    // 触地退出阈值(N)
const float SLIP_THRESHOLD = 1.5;            // 滑移力阈值(N)
const unsigned long DEBOUNCE_MS = 30;         // 防抖动延时(ms)

// ==================== 滑移补偿状态 ====================
struct SlipCompensation {
    float forceError;
    float torqueComp;
    float integral;
};
SlipCompensation slipComp = {0, 0, 0};

// ==================== 触地状态更新（带迟滞）====================
ContactState updateContactState(ContactState current, float force, float torque) {
    switch(current) {
        case AIRBORNE:
            // 进入条件：力 > 进入阈值 且 力矩中心在支撑多边形内
            if (force > CONTACT_THRESHOLD_ENTER && abs(torque) < 0.5) {
                return CONTACT_PENDING;
            }
            return AIRBORNE;
            
        case CONTACT_PENDING:
            // 防抖确认
            static unsigned long pendingTime = 0;
            if (force > CONTACT_THRESHOLD_ENTER) {
                if (pendingTime == 0) pendingTime = millis();
                if (millis() - pendingTime > DEBOUNCE_MS) {
                    pendingTime = 0;
                    return STABLE_CONTACT;
                }
            } else {
                pendingTime = 0;
                return AIRBORNE;
            }
            return CONTACT_PENDING;
            
        case STABLE_CONTACT:
            // 退出条件：力 < 退出阈值（与进入阈值形成迟滞）
            if (force < CONTACT_THRESHOLD_EXIT) {
                return AIRBORNE;
            }
            // 滑移检测
            if (force > CONTACT_THRESHOLD_ENTER && 
                abs(torque) > SLIP_THRESHOLD) {
                return SLIPPING;
            }
            return STABLE_CONTACT;
            
        case SLIPPING:
            // 滑移补偿后，力稳定恢复为稳定接触
            if (force > CONTACT_THRESHOLD_ENTER && abs(torque) < 0.3) {
                return STABLE_CONTACT;
            }
            if (force < CONTACT_THRESHOLD_EXIT) {
                return AIRBORNE;
            }
            return SLIPPING;
    }
    return AIRBORNE;
}

void setup() {
    Serial.begin(115200);
    // BLDC电机初始化...
}

void loop() {
    for (int i = 0; i < 6; i++) joints[i].loopFOC();
    
    // ==================== 1. 读取足底传感器 ====================
    float force = analogRead(FOOT_FORCE_PIN) * 0.01;
    float torque = analogRead(FOOT_TORQUE_PIN) * 0.005;
    
    // ==================== 2. 【核心】更新接触状态（迟滞状态机）====================
    leftFootState = updateContactState(leftFootState, force, torque);
    
    // ==================== 3. 滑移状态响应 ====================
    if (leftFootState == SLIPPING) {
        // 滑移补偿：减少该腿负载，增加对侧腿支撑
        slipComp.forceError = force - CONTACT_THRESHOLD_ENTER;
        slipComp.integral += slipComp.forceError * 0.02;
        slipComp.integral = constrain(slipComp.integral, -0.5, 0.5);
        slipComp.torqueComp = 0.5 * slipComp.forceError + 0.2 * slipComp.integral;
        
        // 关节力矩补偿
        joints[0].move(joints[0].target - slipComp.torqueComp);
        joints[3].move(joints[3].target + slipComp.torqueComp * 0.7);
        
        Serial.println("⚠️ 滑移状态 - 执行力补偿");
    }
    
    delay(20);
}

核心要点：
迟滞状态机：触地进入条件与退出条件不对称（进入阈值3.0N，退出阈值1.5N），避免脚掌边缘轻微抬起就触发状态频繁切换。
防抖动延时：CONTACT_PENDING中间态配合30ms防抖延时，过滤冲击尖峰导致的误判。
滑移检测：法向力与切向力矩联合判据。力的峰值高并不必然代表稳定承重，力矩中心持续偏离支撑多边形才是滑移的本质特征。

3、双核架构下足端滑移估计与全状态反馈
适用场景：高性能人形机器人，需要同时运行步态规划、滑移估计、FOC实时控制。利用ESP32双核实现Core0（规划/估计）与Core1（FOC控制）物理隔离。

/* ===== 双核架构下足端滑移估计与FOC实时控制分离 =====
 * 硬件：ESP32 + 12×BLDC关节电机 + 编码器 + IMU + 足底力传感器
 * 核心：Core0运行滑移估计与步态规划，Core1专职FOC实时控制
 * 参考：双核隔离架构 + 足端滑移补偿闭环控制
 */
#include <SimpleFOC.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

// ==================== 双核通信队列 ====================
QueueHandle_t cmdQueue;
struct JointCommand {
    float angle[12];
    float slipComp[12];   // 滑移补偿量
    unsigned long timestamp;
};

// ==================== 电机对象（12路）====================
BLDCMotor joints[12];
// 需为每个关节配置Driver和Encoder

// ==================== 滑移状态 ====================
struct SlipState {
    float actualPos[12];
    float expectedPos[12];
    float footForce[2];      // 左右足力
    float slipAmount[12];
    bool slipDetected[12];
};
SlipState slipState;

// ==================== Core 1任务：FOC实时控制 ====================
void FOC_Task(void * parameter) {
    for (int i = 0; i < 12; i++) {
        joints[i].controller = MotionControlType::angle;
        joints[i].init(); joints[i].initFOC();
    }
    
    JointCommand cmd;
    for (;;) {
        for (int i = 0; i < 12; i++) joints[i].loopFOC();
        
        if (xQueueReceive(cmdQueue, &cmd, 0) == pdTRUE) {
            for (int i = 0; i < 12; i++) {
                // 角度指令 + 滑移补偿前馈
                float compensatedAngle = cmd.angle[i] + cmd.slipComp[i];
                joints[i].move(compensatedAngle);
                slipState.actualPos[i] = joints[i].shaft_angle;
            }
        }
        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
}

// ==================== Core 0任务：滑移估计与规划 ====================
void Planning_Task(void * parameter) {
    JointCommand cmd;
    float targetAngles[12];
    
    for (;;) {
        // ==================== 1. 步态轨迹生成 ====================
        static float t = 0;
        t += 0.02;
        for (int i = 0; i < 12; i++) {
            targetAngles[i] = 0.3 * sin(t * 0.2 + i * 0.5);
        }
        
        // ==================== 2. 足端力感知与滑移估计 ====================
        // 从足底力传感器读取（假设通过ADC）
        float footForceL = analogRead(34) * 0.01;
        float footForceR = analogRead(35) * 0.01;
        slipState.footForce[0] = footForceL;
        slipState.footForce[1] = footForceR;
        
        // 滑移检测：期望角度与实际角度偏差 + 足力异常
        for (int i = 0; i < 12; i++) {
            float error = targetAngles[i] - slipState.actualPos[i];
            
            // 偏差持续增大 + 足力下降 → 滑移
            if (abs(error) > 0.05 && footForceL < 2.0) {
                slipState.slipAmount[i] += error * 0.1;
                slipState.slipAmount[i] = constrain(slipState.slipAmount[i], -0.3, 0.3);
                slipState.slipDetected[i] = true;
                cmd.slipComp[i] = slipState.slipAmount[i] * 0.5;
            } else {
                slipState.slipAmount[i] *= 0.95;
                slipState.slipDetected[i] = false;
                cmd.slipComp[i] = 0;
            }
            cmd.angle[i] = targetAngles[i];
        }
        
        // ==================== 3. 发送到Core 1 ====================
        xQueueSend(cmdQueue, &cmd, portMAX_DELAY);
        vTaskDelay(20 / portTICK_PERIOD_MS);
    }
}

void setup() {
    Serial.begin(115200);
    cmdQueue = xQueueCreate(10, sizeof(JointCommand));
    
    xTaskCreatePinnedToCore(FOC_Task, "FOC", 8192, NULL, 6, NULL, 1);
    xTaskCreatePinnedToCore(Planning_Task, "Planning", 8192, NULL, 4, NULL, 0);
}

void loop() {
    vTaskDelay(1000 / portTICK_PERIOD_MS);
}

核心要点：
双核物理隔离：Core 1专职FOC实时控制（高优先级），Core 0专职步态规划与滑移估计，互不阻塞。
滑移检测融合：关节角度偏差与足底力联合判据。若期望角度与实际角度持续偏离同时足力下降，判定发生滑移。
前馈补偿：滑移量作为前馈叠加到关节角度指令中，在位置环之前修正，实现"预测-检测-补偿"的自适应控制。

要点解读

1. 足底接触判别不能依赖简单的力传感器阈值比较
   脚掌落地时，橡胶垫、地面粗糙度和足底结构柔顺性会将冲击力拉成一段短暂包络。峰值高并不必然代表稳定承重，峰值低也不一定代表脚还悬空。若控制器在冲击尖峰刚出现时就切换支撑相，实际接触点可能仍在滑移，踝关节补偿会进一步放大横摆。

2. 迟滞状态机是工程上最稳妥的解决方案
   进入条件与退出条件不应完全对称，否则脚掌边缘轻微抬起就会触发状态频繁切换。进入条件可综合：法向力上升趋势、足底力矩中心在支撑多边形内、垂向速度接近零；退出条件需设置更低阈值形成迟滞窗口。

3. 多传感器融合的时序一致性至关重要
   六维力传感器、足底压力阵列和IMU的采样周期和滤波延迟各不相同。若直接拼接为同一时刻状态量，接触判据在快速落脚时容易出现错位。小步快走时足端冲击仅持续几十毫秒，时间差就足以让控制器误将旧姿态当新约束使用。

4. 滑移检测需从单脚判断扩展到双足协同
   左右脚的协同会放大单脚判断的误差。若一侧足底可信度虚高，控制器会把更多地面反力分配给该脚，另一侧提前卸载；若可信度虚低，系统又会拖延换脚。实际调参时应同时观察两脚的力中心轨迹，而非仅关注失稳的一侧。

5. 算力瓶颈决定主从架构选型
   12个BLDC关节的FOC控制（电流环20kHz）加上步态规划、滑移估计、状态机决策，远超单核算力。必须采用双核/多MCU架构：高性能上位机运行复杂算法，通过高速总线将指令下发至各关节从控制器；每个关节由ESP32/STM32负责底层力矩闭环。接触判别与相位迟滞对齐后，双足控制才有可靠的支撑前提。

4、基于SimpleFOC的足端位置偏差检测与力矩补偿
适用场景：四足/人形机器人在软质或湿滑地面行走，足端发生滑移时，通过编码器检测位置偏差并即时补偿力矩，抑制滑动累积。
核心逻辑：通过关节编码器实时获取足端实际位置，与步态规划器生成的期望位置对比，计算位置偏差；基于PID算法生成滑移补偿力矩，叠加至基础力矩输出，实现闭环力矩控制，恢复足端抓地力。

/* ===== 基于SimpleFOC的足端位置偏差检测与力矩补偿 =====
 * 硬件：ESP32 + 12×BLDC关节电机 + 编码器(AS5600/MT6701) + IMU
 * 核心：编码器检测实际位置，与期望轨迹比较，计算偏差并补偿力矩
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

// 关节电机定义（以单腿髋关节为例）
BLDCMotor hipMotor(7);
BLDCDriver3PWM hipDriver(25, 26, 27, 14);
MagneticSensorI2C hipSensor(0x36);

// 滑移补偿参数
struct SlipCompensation {
    float expectedPos;
    float actualPos;
    float posError;
    float integral;
    float lastError;
    float Kp_slip = 2.0;
    float Ki_slip = 0.5;
    float Kd_slip = 0.1;
    float compTorque;
};
SlipCompensation slipComp;

MPU6050 imu;
float pitch = 0, roll = 0;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    imu.initialize();
    hipSensor.init();
    hipMotor.linkSensor(&hipSensor);
    hipDriver.voltage_power_supply = 12;
    hipDriver.init();
    hipMotor.linkDriver(&hipDriver);
    hipMotor.controller = MotionControlType::torque;
    hipMotor.init();
    hipMotor.initFOC();
    slipComp.expectedPos = 0;
    slipComp.actualPos = 0;
    slipComp.posError = 0;
    slipComp.integral = 0;
}

void loop() {
    hipMotor.loopFOC();
    float rawAngle = hipSensor.getAngle();
    slipComp.actualPos = rawAngle;
    static float t = 0;
    t += 0.02;
    slipComp.expectedPos = 0.5 * sin(t * 0.5);
    slipComp.posError = slipComp.expectedPos - slipComp.actualPos;
    if (abs(slipComp.posError) < 0.005) slipComp.posError = 0;
    slipComp.integral += slipComp.posError * 0.02;
    slipComp.integral = constrain(slipComp.integral, -1.0, 1.0);
    float derivative = (slipComp.posError - slipComp.lastError) / 0.02;
    slipComp.lastError = slipComp.posError;
    slipComp.compTorque = slipComp.Kp_slip * slipComp.posError
                        + slipComp.Ki_slip * slipComp.integral
                        + slipComp.Kd_slip * derivative;
    slipComp.compTorque = constrain(slipComp.compTorque, -3.0, 3.0);
    float baseTorque = 1.0 * sin(t * 0.5);
    float finalTorque = baseTorque + slipComp.compTorque;
    finalTorque = constrain(finalTorque, -5.0, 5.0);
    hipMotor.move(finalTorque);
    Serial.print("期望:"); Serial.print(slipComp.expectedPos);
    Serial.print(" 实际:"); Serial.print(slipComp.actualPos);
    Serial.print(" 补偿力矩:"); Serial.println(slipComp.compTorque);
    delay(20);
}

5、基于落足点偏差的滑动惩罚机制与质心自适应
适用场景：四足/人形机器人在湿滑或松软地面行走，通过检测足端滑动距离，实时调整质心高度与步态参数，抑制连续滑动。
核心逻辑：融合关节编码器与足端力信息推算实际落足点，与期望落足点对比计算滑动距离；通过双曲正切函数构建连续滑动惩罚系数，动态降低质心高度，提升稳定性；同步调整步态规划参数，减少滑动累积。

/* ===== 基于落足点偏差的滑动惩罚机制 =====
 * 硬件：Arduino+12路BLDC关节（ESP32/Teensy）+ IMU + 足端力/电流检测
 * 核心：检测实际落足点与期望落足点偏差，计算滑动惩罚，动态调整质心高度
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor joints[12];
MPU6050 imu;
float pitch=0, roll=0, yaw=0;

struct SlipState {
    float expectedX, expectedY;
    float actualX, actualY;
    float slipDistance;
    float slipPenalty;
};
SlipState legSlip[4];
float comHeight = 0.20;
const float BASE_COM_HEIGHT = 0.20;
const float COM_ADJUST_RANGE = 0.061;
const float SLIP_THRESHOLD = 0.308;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    imu.initialize();
    for (int i = 0; i < 12; i++) {
        joints[i].controller = MotionControlType::angle;
        joints[i].init(); joints[i].initFOC();
    }
}

void loop() {
    for (int i = 0; i < 12; i++) joints[i].loopFOC();
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    pitch = atan2(ay, az) * 180 / PI;
    roll = atan2(ax, az) * 180 / PI;
    for (int leg = 0; leg < 4; leg++) {
        float dx = legSlip[leg].actualX - legSlip[leg].expectedX;
        float dy = legSlip[leg].actualY - legSlip[leg].expectedY;
        legSlip[leg].slipDistance = sqrt(dx*dx + dy*dy) * 100;
        float slipRatio = legSlip[leg].slipDistance / SLIP_THRESHOLD;
        legSlip[leg].slipPenalty = tanh(slipRatio);
    }
    float maxSlip = 0;
    for (int leg = 0; leg < 4; leg++) {
        if (legSlip[leg].slipPenalty > maxSlip) maxSlip = legSlip[leg].slipPenalty;
    }
    comHeight = BASE_COM_HEIGHT - COM_ADJUST_RANGE * maxSlip;
    comHeight = constrain(comHeight, BASE_COM_HEIGHT - COM_ADJUST_RANGE, BASE_COM_HEIGHT);
    executeGaitWithSlipCompensation(comHeight, legSlip);
    Serial.print("MaxSlip:"); Serial.print(maxSlip);
    Serial.print(" ComH:"); Serial.println(comHeight);
    delay(20);
}

void executeGaitWithSlipCompensation(float comH, SlipState slip[4]) {
    // 带滑动补偿的步态执行逻辑，需结合具体机械结构实现
}

6、基于卡尔曼滤波的轮胎滑移率估计与补偿（基础型）
适用场景：人形机器人足端（类比轮胎）在复杂地面行走时，通过融合编码器转速与IMU加速度数据，实时估算足端滑移率，动态修正驱动力矩，提升复杂地面适应性。
核心逻辑：采用卡尔曼滤波算法融合编码器测得的足端转速与IMU测得的机身加速度，实时估算足端实际滑移率；当滑移率超过阈值时，动态降低驱动力矩，恢复足端抓地力，避免滑移恶化。

/* ===== 基于卡尔曼滤波的足端滑移率估计与补偿 =====
 * 硬件：Arduino + BLDC关节 + 编码器 + IMU
 * 核心：融合编码器转速与IMU加速度，估算滑移率并补偿力矩
 */
#include <KalmanFilter.h>
#include <MotorController.h>

const int ENCODER_PIN[] = {A0, A1, A2, A3};
const int MOTOR_PWM_PIN[] = {9,10,11,12};
MotorController legs[4];
KalmanFilter slipEstimator[4];
const float LEG_RADIUS = 0.05;
const float BODY_MASS = 2.5;
float targetSpeed[4] = {0};

void setup() {
    for(int i=0;i<4;i++){
        legs[i].begin(MOTOR_PWM_PIN[i], ENCODER_PIN[i]);
        slipEstimator[i].setProcessNoise(0.01);
        slipEstimator[i].setMeasurementNoise(0.05);
    }
}

void loop() {
    for(int i=0;i<4;i++){
        float actualSpeed = legs[i].getSpeed();
        float actualAccel = getIMUAcceleration();
        float slipRate = slipEstimator[i].update(actualSpeed, actualAccel);
        if(slipRate > 0.2) {
            targetSpeed[i] *= 0.9;
        }
        float torque = computeTorque(targetSpeed[i], actualSpeed);
        legs[i].setTorque(torque);
    }
    delay(10);
}

float getIMUAcceleration() {
    // 从IMU获取机身加速度，需结合具体IMU驱动实现
    return 0;
}

float computeTorque(float target, float actual) {
    // 根据目标速度与实际速度计算力矩，需结合动力学模型实现
    return 0;
}

要点解读
多传感器融合的滑移感知体系：滑移检测依赖本体感知，融合IMU（姿态、加速度）、关节编码器（位置、速度）、足端力信息，通过扩展卡尔曼滤波等算法解算足端滑移状态，无需外部视觉或六维力传感器，降低成本的同时提升系统鲁棒性。

闭环力矩补偿的动态响应特性：BLDC电机采用FOC磁场定向控制，实现毫秒级力矩响应，结合PID、滑模控制等算法，实时补偿足端位置偏差。滑移发生时，动态调整关节力矩，使足端受力回归摩擦锥，快速恢复抓地力，避免滑移累积导致失衡。

分层控制架构的实时性保障：采用“高层规划-中层解算-底层执行”的分层架构，高层负责步态规划、质心轨迹生成，中层基于动力学模型解算关节目标力矩，底层通过Arduino/ESP32实现高频力矩伺服。借助非阻塞编程、硬件定时器中断，确保控制频率达500Hz以上，满足实时性要求。

参数整定与模型简化的工程平衡：完整动力学模型计算量过大，需简化为单刚体模型、线性倒立摆模型等，降低算力负担。同时，对滑移补偿的PID参数、摩擦系数、MPC预测步长等进行精细整定，平衡响应速度与稳定性，避免参数过大导致振荡、过小导致响应迟缓。

安全冗余与电源管理的可靠性设计：滑移恢复时BLDC电机瞬态电流大，需采用独立DC-DC模块为控制电路供电，与电机电源隔离，防止反向电动势干扰。同时，代码中加入过流保护、倾角超限保护，配备物理急停开关，确保系统失控时快速切断动力，保障设备与环境安全。

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