研究方案 ¶

传感器感知（LiDAR/深度相机）
        │
        ▼
地形点云 → 平面分割（convex_plane_decomposition）
        │
        ▼
SegmentedPlanesTerrainModel（terrain接口）
        │
        ▼
SwingTrajectoryPlanner（感知版）落脚点规划 + 摆腿轨迹
        │
        ▼
OCS2 MPC（SQP求解器）—— 质心动力学 + 约束优化
        │
        ▼
关节力矩指令 → 机器人执行器

# Ubuntu 22.04 + ROS2 Humble
sudo apt install ros-humble-pinocchio
sudo apt install ros-humble-grid-map
pip install convex_plane_decomposition  # 或从源码编译

# 子模块初始化（重要！）
cd F:/ROS/ocs2_ros2/ocs2_ros2
git submodule update --init --recursive

cd ~/ros2_ws
# 将 ocs2_ros2 软链接或复制到 src/
colcon build --packages-up-to ocs2_legged_robot_ros --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo

# 终端1：启动MPC节点（SQP求解器）
ros2 launch ocs2_legged_robot_ros legged_robot_sqp.launch.py

# 终端2：查看话题
ros2 topic list
# 应该看到 /legged_robot_mpc_observation, /legged_robot_mpc_policy 等

swing_trajectory_config {
  swingHeight  0.1      ← 摆腿高度，上楼梯需要增大至 0.15~0.20
  liftOffVelocity   0.2  ← 抬腿初速度
  touchDownVelocity -0.4 ← 落脚速度
}
mpc {
  timeHorizon  1.0      ← 预测时域，楼梯场景建议增到 1.5~2.0s
  mpcDesiredFrequency  50  ← MPC频率
  mrtDesiredFrequency  400 ← 控制频率
}

include/ocs2_legged_robot/LeggedRobotInterface.h

include/ocs2_legged_robot/gait/
├── Gait.h            ← 步态结构体（modeSequence + switchingTimes）
├── GaitSchedule.h    ← 步态调度器，管理步态切换
├── LegLogic.h        ← 从mode解析各腿接触状态（核心！）
└── MotionPhaseDefinition.h  ← STANCE/LF_RH等枚举定义

include/ocs2_legged_robot/foot_planner/
├── SwingTrajectoryPlanner.h  ← 摆腿轨迹（Z方向位置+速度约束）
├── SplineCpg.h               ← 样条曲线中央模式生成器
└── CubicSpline.h             ← 三次样条

include/ocs2_legged_robot/constraint/
├── FrictionConeConstraint.h         ← 摩擦锥（接触力必须在摩擦锥内）
├── ZeroForceConstraint.h            ← 摆动腿接触力为零
├── ZeroVelocityConstraintCppAd.h    ← 站立腿速度为零
└── NormalVelocityConstraintCppAd.h  ← 法向速度约束

ocs2_switched_model_interface/include/.../terrain/TerrainModel.h

// 关键接口方法：
TerrainPlane getLocalTerrainAtPositionInWorldAlongGravity(positionInWorld);
// 给定世界坐标，返回该点下方的局部地形平面（法向量+位置）

ConvexTerrain getConvexTerrainAtPositionInWorld(positionInWorld);
// 返回凸多边形地形区域（用于落脚点约束）

vector3_t getHighestObstacleAlongLine(pos1, pos2);
// 返回摆腿路径上最高障碍点（楼梯台阶关键！）

std::vector<vector2_t> getHeightProfileAlongLine(pos1, pos2);
// 返回两点间高度剖面（用于调整摆腿高度）

segmented_planes_terrain_model/include/
├── SegmentedPlanesTerrainModel.h    ← TerrainModel的具体实现
├── SegmentedPlanesTerrainModelRos.h ← ROS2订阅接口（订阅planar_terrain话题）
└── SegmentedPlanesSignedDistanceField.h ← SDF距离场

ocs2_switched_model_interface/include/.../foot_planner/SwingTrajectoryPlanner.h

# Step 1: 编译
colcon build --packages-up-to ocs2_legged_robot_ros

# Step 2: 启动
ros2 launch ocs2_legged_robot_ros legged_robot_sqp.launch.py

# Step 3: 发送步态指令（在gait_command终端）
# 输入: trot

# Step 4: 发送目标位置（在robot_target终端）
# 输入: 1.0 0.0 0.0 0.0 (x y z yaw)

# convex_plane_decomposition（ETH开源）
cd ~/ros2_ws/src
git clone https://github.com/leggedrobotics/convex_plane_decomposition.git
git clone https://github.com/leggedrobotics/grid_map.git
colcon build --packages-up-to convex_plane_decomposition

colcon build --packages-up-to segmented_planes_terrain_model ocs2_anymal_mpc

LiDAR/深度相机
    │  PointCloud2
    ▼
elevation_mapping（高程图）
    │  GridMap(elevation layer)
    ▼
convex_plane_decomposition_ros
    │  PlanarTerrain.msg
    ▼
SegmentedPlanesTerrainModelRos::callback()
    │  SegmentedPlanesTerrainModel
    ▼
SwingTrajectoryPlanner::updateTerrain()
    │  感知落脚点选择
    ▼
MPC优化求解

// 添加地形接口方法：
void updateTerrain(std::unique_ptr<TerrainModel> terrainModel);

// 修改 update() 方法签名：
void update(const ModeSchedule& modeSchedule,
            const feet_array_t<scalar_array_t>& liftOffHeightSequence,
            const feet_array_t<scalar_array_t>& touchDownHeightSequence);
            // 改为从地形模型动态获取高度

// 核心改动：在计算摆腿高度时，查询地形模型
scalar_t getAdaptiveSwingHeight(size_t leg, scalar_t liftOffX, scalar_t touchDownX) {
    // 1. 获取摆腿路径上的最高障碍
    vector3_t liftOffPos = ...; // 从当前状态获取
    vector3_t touchDownPos = ...; // 从规划落脚点获取
    vector3_t highestObstacle = terrainModel_->getHighestObstacleAlongLine(liftOffPos, touchDownPos);
    
    // 2. 自适应摆腿高度 = max(默认高度, 障碍高度 + 安全余量)
    scalar_t safetyMargin = 0.05; // 5cm 安全余量
    return std::max(config_.swingHeight,
                    highestObstacle.z() - liftOffPos.z() + safetyMargin);
}

; === 楼梯场景参数修改 ===

swing_trajectory_config
{
  liftOffVelocity     0.3      ; 原0.2，增大抬腿速度
  touchDownVelocity  -0.3      ; 原-0.4，减小落脚冲击
  swingHeight         0.15     ; 原0.1，增大摆腿高度（楼梯台阶约0.18m）
  touchdownAfterHorizon  0.2
  swingTimeScale      0.15
}

mpc
{
  timeHorizon         1.5      ; 原1.0，增大预测时域覆盖至少一个完整步态
  mpcDesiredFrequency 50
  mrtDesiredFrequency 400
}

; 摩擦锥约束：楼梯表面可能不如平地，适当降低摩擦系数
frictionConeSoftConstraint
{
  frictionCoefficient  0.4    ; 原0.5，楼梯更保守
  mu                   0.1
  delta                5.0
}

; 状态权重调整：加强高度和姿态保持
Q
{
  scaling 1e+0
  (8,8)   800.0    ; p_base_z 原500，上楼梯高度追踪更重要
  (10,10) 300.0    ; theta_base_y 原200，俯仰角保持更重要
  (11,11) 300.0    ; theta_base_x 原200，滚转角保持更重要
}

; 为楼梯添加"慢速静态步行"步态
stair_walk
{
  modeSequence
  {
    [0]  LF_RF_RH   ; 抬左前腿，三腿支撑
    [1]  RF_LH_RH   ; 抬左后腿，三腿支撑
    [2]  LF_RF_LH   ; 抬右前腿，三腿支撑
    [3]  LF_LH_RH   ; 抬右后腿，三腿支撑
  }
  switchingTimes
  {
    [0]  0.0
    [1]  0.4         ; 每腿支撑0.4s（比static_walk的0.3s更慢，更稳）
    [2]  0.8
    [3]  1.2
    [4]  1.6
  }
}

; 机器人重心高度随楼梯台阶动态调整
; 需要在代码中实现动态comHeight更新（不是静态配置）
comHeight  0.575    ; 平地默认值，上楼梯时由reference_manager动态修改

namespace ocs2::legged_robot {

class StairFootholdPlanner {
public:
    struct Config {
        scalar_t stepWidth = 0.12;       // 足端横向间距
        scalar_t stepLength = 0.28;      // 步长（小于台阶深度）
        scalar_t safetyMarginFromEdge = 0.03; // 离台阶边缘安全距离
        scalar_t minStepHeight = 0.005;  // 低于此值认为是平地
    };

    // 核心方法：给定期望落脚位置和地形，返回安全落脚点
    vector3_t selectSafeFoothold(
        const vector3_t& nominalFoothold,
        const TerrainModel& terrain,
        size_t legIndex) const;

private:
    // 检查落脚点是否在平面区域内（不在台阶边缘）
    bool isFootholdSafe(const vector3_t& foothold,
                        const ConvexTerrain& localTerrain) const;

    Config config_;
};

} // namespace

选项A（仿真）：Gazebo深度相机 → depth_image_proc → PointCloud2
选项B（实物）：Intel RealSense D435i → realsense2_driver → PointCloud2
选项C（实物）：Velodyne VLP-16 LiDAR → velodyne_driver → PointCloud2

# 安装高程图包
sudo apt install ros-humble-elevation-mapping

# 安装平面分割包（从源码）
cd ~/ros2_ws/src
git clone https://github.com/leggedrobotics/convex_plane_decomposition_ros.git
colcon build --packages-up-to convex_plane_decomposition_ros

def generate_launch_description():
    return LaunchDescription([
        # 1. 机器人状态发布
        Node(package='robot_state_publisher', ...),

        # 2. 高程图构建
        Node(package='elevation_mapping',
             executable='elevation_mapping',
             parameters=[{'sensor_processor/type': 'structured_light',
                          'robot_base_frame_id': 'base'}]),

        # 3. 平面分割
        Node(package='convex_plane_decomposition_ros',
             executable='convex_plane_decomposition_node',
             parameters=[{'input_topic': '/elevation_map'}]),

        # 4. OCS2 MPC（感知版）
        Node(package='ocs2_legged_robot_ros',
             executable='legged_robot_sqp_mpc',
             parameters=[{'taskFile': task_file,
                          'enable_terrain': True}]),  # 新增地形开关

        # 5. 仿真/可视化
        Node(package='rviz2', ...),
    ])

<!-- 在 Gazebo world 中添加楼梯模型 -->
<model name="staircase">
  <static>true</static>
  <!-- 5级台阶，每级高0.15m，深0.30m，宽1.2m -->
  <link name="step_0">
    <collision><geometry><box><size>1.2 0.30 0.15</size></box></geometry></collision>
    <visual>...</visual>
    <pose>0 0 0.075 0 0 0</pose>
  </link>
  <!-- ... step_1 到 step_4 ... -->
</model>

core/ocs2_core/                    ← 理解数学基础
├── integration/                    ← ODE积分器
└── automatic_differentiation/      ← CppAD原理

mpc/ocs2_sqp/ocs2_sqp/            ← 理解SQP求解流程
robotics/ocs2_pinocchio/           ← 理解Pinocchio动力学

basic examples/ocs2_legged_robot/
├── include/ocs2_legged_robot/dynamics/     ← SRBD动力学（不改）
├── include/ocs2_legged_robot/gait/         ← 步态逻辑（精读）
└── include/ocs2_legged_robot/constraint/   ← 约束定义（理解）

advance examples/ocs2_perceptive_anymal/
├── ocs2_switched_model_interface/terrain/  ← TerrainModel接口（精读）
└── segmented_planes_terrain_model/         ← 具体实现（精读）

basic examples/ocs2_legged_robot/config/
├── mpc/task.info          ⭐⭐⭐ 最重要！修改swingHeight/timeHorizon/Q权重
├── command/gait.info      ⭐⭐  添加 stair_walk 步态
└── command/reference.info ⭐   修改comHeight（或动态调整）

advance examples/ocs2_perceptive_anymal/
├── segmented_planes_terrain_model/   ← 整个包直接复用，只修改CMakeLists依赖
└── ocs2_switched_model_interface/
    └── terrain/                       ← TerrainModel接口整体复用

; task.info 中降低求解精度
sqp {
  sqpIteration  1       ; 只迭代1次
  nThreads      4       ; 增加线程数
  dt            0.02    ; 增大时间步长（降低精度但加速）
}

; 增大摆腿高度和降低速度
swing_trajectory_config {
  swingHeight     0.20   ; 增大
  swingTimeScale  0.20   ; 增大（减慢）
}
; 同时增大俯仰角权重
Q { (10,10) 400.0 }

CppAD 代码生成加速：
  task.info: recompileLibrariesCppAd  false  (第一次设true生成，之后设false跳过)
  modelFolderCppAd  /tmp/ocs2  (确保路径可写)

多线程：
  sqp { nThreads 6 }  (根据CPU核数调整)

理论学习 + 环境搭建
跑通基础 legged_robot_sqp（平地trot）
阅读感知模块代码，理解TerrainModel接口
接入 convex_plane_decomposition 感知流水线
修改 SwingTrajectoryPlanner 支持地形自适应
调整 task.info 参数，实现楼梯步态
Gazebo楼梯仿真验证（上楼）
Gazebo仿真验证（下楼 + 连续上下）
性能优化 + 实验数据整理