QuadPiPS 技术总结：基于感知信息的四足机器人步点规划框架

1. 研究背景与问题定义 (Problem)

随着四足机器人（Legged Robots）在复杂非结构化环境中的应用日益增多，传统的基于高程图（Elevation Map）或体素网格（Voxel Grid）的环境表示方法在**步点规划（Foothold Planning）**方面面临挑战：

• 表示局限：传统方法通常将环境离散化为 2.5D 网格，难以捕捉复杂的几何特征（如法向量）和语义信息（如是否可踩踏），导致在稀疏或狭窄的可行步点区域（如台阶、平衡木）规划失败。
• 计算瓶颈：构建高程图需要昂贵的点云预处理（滤波、射线投射等），增加了系统延迟。
• 规划与感知脱节：现有的规划器往往依赖后处理的几何信息，缺乏对原始传感器数据的直接利用，且难以在安全关键（Safety-critical）场景下实现实时的、动力学可行的步点搜索。

核心问题：如何构建一种感知驱动的框架，能够直接在感知空间（Perception Space）中利用原始深度和语义信息，为四足机器人生成安全、实时且动力学可行的步点规划，特别是在可行步点稀缺的复杂地形中。

2. 方法论 (Methodology)

QuadPiPS (Quad PiPS) 提出了一种感知空间步点规划框架，其核心流程分为感知、规划和控制三个层级：

2.1 感知层：增强型 Legged Egocan

• Egocan 表示：基于“感知空间规划”（PiPS）理念，扩展了原有的 Egocylinder 概念，增加了上下“帽”（Egocaps），形成封闭的 360° 局部环境表示。它直接利用深度相机数据，无需点云预处理。
• 多通道特征融合：
  • 几何特征：通过深度图像梯度实时估计表面法向量（Surface Normals），捕捉地形朝向。
  • 语义特征：利用深度学习模型（DeepLabV3+）预测可踩踏性标签（Steppability Labels），将环境分为“可踩踏（Steppable）”、“可跨越（Passable）”和“不可通行（Non-passable）”。
• 超像素过分割（Superpixels Oversegmentation）：
  • 针对 Egocan 底部图像，提出了一种改进的 SLIC 超像素算法。
  • 将稀疏的多通道图像（深度 + 法向 + 语义）过分割为感知均匀的平面区域（Planar Regions）。
  • 这些区域作为候选步点的搜索空间，既保留了地形的几何细节，又避免了均匀网格的冗余。

2.2 规划层：分层搜索与优化

框架借鉴并扩展了 ALEF (Augmented Leafs with Experience on Foliations) 框架，将规划空间分解为离散和连续子空间：

1. 躯干路径搜索（Coarse Torso Search）：
   • 基于平面区域构建图，使用 A* 算法搜索粗略的躯干路径，作为后续步点搜索的引导。
2. 接触模式族搜索（Contact Mode Family Search）：
   • 离散空间：构建“接触模式族转换图”。节点代表部分支撑状态（部分脚着地），边代表完整的支撑状态转换。
   • 约束：在搜索过程中加入用户定义的步态序列、可达性体积（Reachability Volumes，基于超二次曲面）、支撑稳定性（Stance Stability）和摆动距离约束。
   • 优势：通过显式的图搜索，在稀疏地形（如稀疏的踏脚石）中能找到全局最优的步点序列，而非依赖启发式规则。
3. 摆动轨迹优化（Swing Trajectory Optimization）：
   • 连续空间：利用非线性轨迹优化（基于 OCS2 库）生成动力学可行的全身参考轨迹，连接选定的步点。
   • 优化目标包括最小化能量、满足摩擦锥约束和平面约束。

2.3 控制层：MPC 与 WBC

• 模型预测控制（MPC）：以 50Hz 频率跟踪参考轨迹，具有重新规划（Re-planning）能力，可根据当前状态调整摆动脚落点以维持稳定。
• 全身控制（WBC）：以 >1kHz 频率运行，将 MPC 输出的状态和输入转化为关节力矩指令，优先保证动力学可行性。

3. 关键贡献 (Key Contributions)

1. Legged Egocan 表示：首次将 Egocan 扩展用于四足机器人，融合了表面法向量和语义可踩踏标签，实现了从感知到动作的端到端管道。
2. 合成数据生成：利用基于基本几何形状（Primitive Shapes）的技术生成带有真实步点标签的合成训练数据，解决了语义分割模型在机器人领域缺乏标注数据的问题。
3. 超像素过分割算法：提出了一种针对稀疏多通道图像的 SLIC 改进算法，将 Egocan 地面图像转化为适合步点搜索的凸平面区域集合。
4. 实时 ALEF 扩展：将 ALEF 框架适配为感知驱动、实时且动力学可行的四足运动规划器，支持在未知环境中的在线搜索。
5. 全面的实验验证：
   • 仿真：在 ANYmal C 上进行了 10 种复杂地形（台阶、碎石、平衡木等）的基准测试，对比了 5 种基线方法。
   • 实机：在 Unitree Go2 上部署了自定义计算套件，成功完成了包括斜坡、楼梯和踏脚石在内的 5 种真实地形测试。

4. 实验结果 (Results)

• 仿真基准测试：
  • 在稀疏步点环境（如稀疏踏脚石、带障碍的平衡木）中，QuadPiPS 显著优于基于高程图的基线方法（EM-MPC, EM-RL 等）。高程图方法常因将小石块合并为大平面而导致规划失败。
  • 在语义敏感环境（如带障碍的平衡木）中，QuadPiPS 利用语义标签成功避开了不稳定的障碍物，而纯几何方法则容易尝试踩踏导致翻倒。
  • 在简单环境（如斜坡、碎石）中，基于学习的方法（EM-DTC）表现良好，但在复杂地形下缺乏鲁棒性。
• 实机部署：
  • QuadPiPS 在 Unitree Go2 上成功实现了地形感知 locomotion。
  • 能够处理动态变化的地形（如滑动的木板）和高度变化（如 35cm 高的台阶）。
  • 尽管存在跟踪误差（如脚滑出平台），MPC 层能够有效恢复稳定性。
• 性能指标：感知流水线在硬件上运行频率约为 20Hz（受限于语义分割推理时间，但通过多线程优化未成为瓶颈），规划与控制在 15Hz-50Hz 范围内运行，满足实时性要求。

5. 意义与影响 (Significance)

• 范式转变：证明了“感知空间规划”（PiPS）不仅适用于避障，也能有效解决复杂的四足步点规划问题，减少了中间表示（如点云转高程图）带来的信息丢失和延迟。
• 安全性提升：通过显式的图搜索和语义理解，QuadPiPS 在可行步点稀缺的安全关键场景中表现出极高的鲁棒性，填补了传统启发式规划器在复杂地形下的能力空白。
• 硬件部署可行性：展示了在嵌入式计算平台（Jetson Orin NX + Mini PC）上运行复杂感知 - 规划 - 控制闭环的可行性，为四足机器人在真实世界（如灾难救援、野外勘探）的自主导航提供了技术路径。
• 未来方向：指出了当前框架在超像素时间一致性（减少抖动）和动力学可行性约束（结合 ALEF 经验启发式）方面的改进空间，并建议结合学习策略（Learning-based Policies）以进一步增强底层控制的鲁棒性。

总结：QuadPiPS 通过深度融合感知语义、几何特征与分层规划算法，成功解决四足机器人在复杂、稀疏地形下的步点规划难题，实现了从感知到动作的高效、安全闭环，是四足机器人自主导航领域的一项重要进展。