足式与人形机器人运动控制

三层结构：
  1. 任务与步态层（~1~10 Hz）
     决定走还是停、往哪走、用什么步态（行走/小跑/跳跃）
     输入：导航目标、地形、操作员指令
     输出：期望速度、步态时序、落脚点序列

  2. 全身控制层 WBC / MPC（~50~500 Hz）
     在动力学与接触约束下求解关节力矩与接触力
     输入：期望躯干轨迹、足端轨迹、接触状态
     输出：关节力矩指令、期望接触力

  3. 关节伺服层（~1~4 kHz）
     关节级的位置/速度/力矩闭环
     输入：关节力矩/位置指令
     输出：电机电流

ZMP（Zero Moment Point）定义：
  地面上使机器人所受重力和惯性力的合力矩水平分量为零的点
  若 ZMP 落在支撑多边形（支撑脚构成的凸多边形）内，机器人不翻倒

支撑多边形：
  单脚支撑：一只脚与地面的接触面
  双脚支撑：两只脚的接触面 + 两脚之间的凸包
  判据：ZMP 越靠近支撑多边形中心越稳，越靠近边缘越危险

简化计算（忽略角动量变化）：
  ZMP_x = (Σ m_i (ẍ_i + g) x_i - Σ m_i ẍ_i z_i) / (Σ m_i (ẍ_i + g))
  常用质心近似：ZMP ≈ 质心在地面的投影 - (z_c/g) · 质心水平加速度

线性倒立摆（LIPM）模型：
  假设质心高度恒定，腿部无质量，接触力可任意分配
  ẍ = ω² (x - p)，其中 ω = √(g/z_c)，p 为 ZMP
  这是一组解耦的线性微分方程，有解析解

捕获点（Capture Point, CP）：
  ξ = x + ẋ / ω
  含义：若把脚落在 ξ 处并保持，机器人最终会停下
  推广为发散分量（DCM）：ξ = x + ẋ/ω，是 LIPM 的「不稳定模态」

N 步捕获点（N-step CP）：
  若当前 CP 在支撑多边形外，需要走 N 步才能停下
  每步的落脚点是下一步的 CP，形成落脚点序列

LIPM 的价值与局限：
  价值：解析解、计算便宜、能在线求解落脚点，是 MPC 的常用模型
  局限：假设质心高度恒定、忽略角动量、忽略腿部动力学
  适用：平地行走、中等速度；不适用：剧烈运动、复杂地形

常见步态（按支撑相数量）：
  行走（Walk）：任意时刻至少三脚着地，始终静态稳定
  小跑（Trot）：对角腿同步，两脚着地，动态稳定，最常用
  踱步（Pace）：同侧腿同步
  奔跑（Bound）：前后腿同步
  疾驰（Gallop）：非对称，最快
  跳跃（Jump）：四脚同时离地，有腾空相

启发式步态生成（早期/简单场景）：
  用正弦曲线或样条指定足端轨迹（贝塞尔曲线最常见）
  用固定的时序切换支撑相
  优点：简单、可预测；缺点：不适应地形、速度受限
  代表：MIT Cheetah 早期、多数四足的基础步态

足端轨迹参数化（摆动相）：
  用三次或五次贝塞尔曲线连接起点与落脚点
  抬脚高度：典型 0.05~0.15 m（地形越复杂越高）
  摆动时间：典型 0.2~0.4 s（速度越快越短）
  落地速度：垂直落地速度不能为零（否则「踩空感」），典型 0.1~0.3 m/s
  贝塞尔控制点决定轨迹形状，可调「抬脚快、落地慢」的非对称性

凸 MPC（基于 LIPM 的简化模型）：
  状态：质心位置与速度（水平面），或加上 DCM
  控制：ZMP 位置、落脚点
  约束：ZMP 在支撑多边形内、步长/步时上限
  目标：跟踪期望速度 + 最小化控制量 + 平滑
  优点：凸问题，求解快（毫秒级），可在 100+ Hz 在线求解
  代表：MIT Cheetah 3 的凸 MPC、多数四足商业产品

全身动力学 MPC（完整模型）：
  状态：浮动基座位姿 + 关节角 + 速度（约 36 维，四足）
  控制：关节力矩 + 接触力
  约束：完整动力学、摩擦锥、接触力上限
  目标：跟踪 + 能耗 + 平滑
  优点：精度高、能利用角动量；缺点：非凸、求解慢（几十毫秒到秒）
  代表：MIT Cheetah 的 kino-dynamic MPC、ANYmal 的 NMPC

MPC 的关键参数：
  预测时域：0.5~1.5 s（覆盖 2~5 步）
  控制步长：0.02~0.05 s
  求解频率：凸 MPC 100~500 Hz，NMPC 20~100 Hz
  热启动：用上一步的解初始化，能大幅加速求解
  摩擦锥约束：μ·f_z ≥ √(f_x² + f_y²)，μ 取 0.5~0.8

WBC 的问题形式（分层 QP）：
  最小化：Σ w_i ||J_i q̈ + J̇_i q̇ - a_i^des||²  （各任务的跟踪误差）
  约束：
    动力学方程：M q̈ + C q̇ + g = S^T τ + Σ J_ci^T f_i
    接触约束：J_ci q̈ + J̇_ci q̇ = 0（支撑脚不滑动）
    摩擦锥：μ f_z ≥ √(f_x² + f_y²)
    力矩上限：τ_min ≤ τ ≤ τ_max

分层（Hierarchical QP）：
  高优先级任务（如保持平衡）的解空间内，再优化低优先级任务（如跟踪速度）
  典型优先级：接触约束 > 躯干姿态 > 速度跟踪 > 手臂动作 > 能耗

// WBC 的求解骨架（伪码，实际用 qpOASES / OSQP / Goldfarb）
// 决策变量：q̈（关节加速度）+ f（接触力）
// 第一层：硬约束下的最优，保证平衡
solve_qp(H1, g1, A_eq, b_eq, A_ineq, b_ineq, x0);
// 第二层：在第一层的零空间内优化次级任务
solve_qp_in_nullspace(H2, g2, x_opt_layer1, ...);
// 最终输出：关节力矩 τ = M q̈ + C q̇ + g - Σ J_ci^T f_i（通过动力学方程反解）

需要估计的量：
  浮动基座位姿（躯干在世界系的位姿）——最关键也最难
  浮动基座速度
  各关节角与角速度（编码器直接给，准）
  接触状态（哪只脚着地）与接触位置
  地面法向与坡度

传感器：IMU（加速度+角速度）、关节编码器、足端力/力矩传感器、
        关节力矩（电流估计）、可选的视觉/激光

状态估计的两条路线：
  1. 基于 IMU + 运动学（无外部传感器）
     用腿部运动学估计躯干速度（支撑脚的关节运动反推）
     用 IMU 积分姿态，用接触足的位置修正漂移
     代表：MIT Cheetah 的状态估计器
     优点：不依赖环境特征；缺点：长期会漂移

  2. 融合 IMU + 腿部运动学 + 外部传感器（视觉/激光）
     用视觉或激光提供绝对位置修正
     代表：ANYmal 的视觉惯性里程计
     优点：无长期漂移；缺点：复杂、依赖环境

接触检测的三种方法：
  1. 阈值法：足端力 > 阈值判定为接触
     简单但需处理噪声与「轻触」
  2. 概率法：用接触概率模型 + 贝叶斯滤波
     更鲁棒，能处理接触的模糊状态
  3. 基于动力学一致性：比较实际关节力矩与「假设无接触」的预测
     差异大说明有接触，不依赖足端传感器

落脚点规划的目标：
  在可行区域（平坦、可支撑）内选择落脚点
  满足运动学约束（腿长、关节限位）
  满足动力学约束（ZMP/CP 可达）
  优化稳定裕度、能耗、步长

地形感知：
  用深度相机/激光建局部高程图（elevation map）
  每个格子存高度与地形粗糙度
  可行区域 = 坡度小 + 粗糙度低 + 无悬空

地形适应策略：
  1. 被动：落脚点选在可行区域，腿部顺应地形
  2. 主动：根据高程图规划足端轨迹，落地后估计接触面法向
  3. 反应式：接触后检测滑移，触发抬脚重落

高程图的参数：
  分辨率：0.02~0.05 m（太粗漏掉小障碍，太细计算量大）
  更新频率：10~30 Hz
  高度估计：融合多帧观测，处理动态障碍（人腿）
  可行坡度：通常 < 30°~40°，取决于足端摩擦与机器人能力

RL 路线的两种范式：
  1. 端到端：策略直接输出关节目标位置/力矩
     输入：本体感受（IMU、关节）+ 可选的感知
     输出：关节位置或力矩指令
     训练：大规模并行仿真（Isaac Gym / Isaac Lab），PPO 为主
     代表：ANYmal 的 RL 控制器、Unitree 的 RL 步态

  2. 分层：高层 RL 出参考，底层传统控制器跟踪
     优点：结合 RL 的适应性与传统控制的稳定性

训练的关键要素：
  奖励设计：跟踪速度 + 姿态 + 能耗 + 平滑 + 足端高度
  域随机化：质量、摩擦、延迟、传感器噪声、地形
  课程学习：从平地到复杂地形逐步加难
  并行：数千个环境同时训练，数小时到数天

RL 与模型控制的取舍：
  模型控制：可解释、可证明稳定、调参有理论依据、依赖模型精度
  RL：适应性强、能处理未建模效应、但难解释、稳定性无保证、需要大量算力

工程现实：多数商业产品用「模型控制打底 + RL 做特定技能」
  行走与平衡用 MPC/WBC，跳跃、跑酷、摔倒起身用 RL

平衡恢复的分级策略：
  1. 踝策略（Ankle Strategy）
     小幅扰动，用踝关节力矩把质心推回支撑区
     适用：扰动小、支撑面大（如双脚站立）
  2. 髋策略（Hip Strategy）
     中等扰动，用髋关节摆动快速调整角动量
  3. 迈步策略（Stepping Strategy）
     大幅扰动，迈出一步重新建立支撑
     用捕获点计算落脚点
  4. 手臂策略（人形）
     摆动手臂产生反作用角动量，辅助平衡

摔倒保护（不可恢复时）：
  1. 判定不可避免（CP 远离支撑区且无法迈步）
  2. 主动收腿、降低重心、保护头部与关键部件
  3. 触地后进入「摔倒起身」流程或安全停机

恢复行为的触发与执行：
  触发：CP 超出支撑多边形 / IMU 检测到异常加速度 / 接触丢失
  执行：切换控制模式（从速度跟踪切到平衡优先）
  关键：模式切换要平滑，避免控制量突变导致的二次扰动

执行器类型的取舍：
  1. 位置控制关节（准直驱/谐波减速）
     优点：控制简单、刚度高、能承受大负载
     缺点：无法直接力控，柔顺性差，碰撞冲击大
     代表：早期双足、部分四足

  2. 串联弹性执行器（SEA）
     电机与负载间有弹性元件，可测力矩
     优点：天然柔顺、力控精确、抗冲击
     缺点：带宽低（弹性限制）、复杂
     代表：ANYmal、早期 Cheetah

  3. 准直驱（Quasi-Direct Drive, QDD）
     低减速比 + 大扭矩电机，电流估计力矩
     优点：高带宽、力控好、抗冲击、成本低
     缺点：需要大电流、散热难
     代表：MIT Mini Cheetah、Unitree、宇树

关节级控制的层次：
  电流环（10~20 kHz）：控制电机电流，对应力矩
  速度环（1~4 kHz）：控制关节速度
  位置环（1~4 kHz）：控制关节位置
  WBC 输出的是关节力矩或位置目标，由关节伺服执行

关键硬件指标：
  力矩密度（Nm/kg）：决定动态能力
  带宽（Hz）：决定力控与顺应能力
  背隙（backlash）：影响精度与接触检测
  关节柔性与共振频率：影响控制带宽上限

差距的主要来源：
  1. 接触模型：仿真的接触是刚性的、无延迟的；真机有柔性、有延迟、有打滑
  2. 执行器模型：仿真的力矩是理想的；真机有力矩常数误差、有摩擦、有带宽限制
  3. 传感器：仿真的 IMU 无噪声无偏置；真机有偏置、有温漂、有量化
  4. 延迟：真机的传感-计算-执行总延迟通常 5~20 ms，仿真里常被忽略
  5. 通信与计算：真机的控制周期有抖动，仿真里是理想的

对策：
  1. 域随机化：训练/调参时随机化摩擦、质量、延迟、噪声
  2. 系统辨识：实测执行器带宽、摩擦、延迟，建立更准的模型
  3. 延迟补偿：把已知的延迟建模进控制器（如预测性补偿）
  4. 鲁棒控制：设计对模型误差不敏感的控制器（如 MPC 加约束裕度）
  5. 渐进迁移：先在仿真调好，再上台架（腿悬空），再上真机

上机调参的顺序：
  1. 单腿台架：验证关节力控、重力补偿、单腿弹跳
  2. 悬挂站立：验证平衡控制（不承重）
  3. 承重站立：验证支撑与接触力分配
  4. 原地踏步：验证接触切换与状态估计
  5. 慢速行走：验证步态与落脚点
  6. 逐步提速与加地形：验证动态能力与鲁棒性
  每步都必须有急停与保护绳，绝不可跳过台架直接上真机