（11-4-03）完整人形机器人的设计与实现案例：盲踩障碍物

11.5.3 盲踩障碍物

文件OpenLoong-Dyn-Control/demo/walk_mpc_wbc.cpp为双足机器人 MuJoCo仿真环境中实现盲踩障碍物功能提供支持。通过初始化UI、动力学求解、数据总线等模块，主循环中结合步态调度器规划步态时序，足端放置规划器确定落足位置，MPC和WBC算法实时计算关节力矩，在无环境感知（"盲"）的情况下，依靠预设控制策略与身体状态反馈，实现对障碍物的适应性踩踏，同时记录仿真数据用于分析。

/*
OpenLoong动力学控制模块 - 双足机器人盲踩障碍物仿真
版权所有 (C) 2024-2025 人形机器人（上海）有限公司
开源地址: <https://atomgit.com/openloong/openloong-dyn-control.git>
邮箱: <web@openloong.org.cn>
功能: 实现双足机器人在MuJoCo环境下无视觉感知（盲踩）障碍物的行走控制
*/

// -------------------------- 头文件包含 --------------------------
#include <mujoco/mujoco.h>               // MuJoCo核心仿真库
#include <GLFW/glfw3.h>                  // GLFW图形界面库（用于仿真可视化）
#include "GLFW_callbacks.h"              // GLFW回调函数（处理窗口交互、键盘输入）
#include "MJ_interface.h"                // MuJoCo数据交互接口（传感器/执行器）
#include "PVT_ctrl.h"                    // PVT关节控制器（位置/速度/力矩闭环）
#include "data_logger.h"                 // 仿真数据记录器（保存日志用于分析）
#include "data_bus.h"                    // 全局数据总线（模块间数据共享）
#include "pino_kin_dyn.h"                // 机器人运动学/动力学求解器
#include "useful_math.h"                 // 实用数学工具（欧拉角转旋转矩阵等）
#include "wbc_priority.h"                // 优先级加权WBC控制器（力/运动混合控制）
#include "mpc.h"                         // MPC模型预测控制（轨迹规划与障碍物适应）
#include "gait_scheduler.h"              // 步态调度器（盲踩时步态时序规划）
#include "foot_placement.h"              // 足端放置规划器（盲踩障碍物落足点调整）
#include "joystick_interpreter.h"        // 期望速度解析器（生成行走/避障速度指令）
#include <string>                        // 字符串处理
#include <iostream>                      // 控制台输出
#include <Eigen/Core>                    // Eigen矩阵运算库

// -------------------------- 全局常量/变量定义 --------------------------
const double dt = 0.001;                 // 基础控制时间步长（1ms）
const double dt_200Hz = 0.005;           // MPC控制周期（200Hz）

// MuJoCo模型加载相关
char error[1000] = "无法加载二进制模型";  // 模型加载错误信息缓冲区
// 加载盲踩障碍物仿真场景（包含障碍物的XML模型文件）
mjModel* mj_model = mj_loadXML("../models/scene_obstacle.xml", 0, error, 1000);  
mjData* mj_data = mj_makeData(mj_model);  // 创建MuJoCo仿真数据结构（存储关节位姿/力等动态数据）

// -------------------------- 主函数（程序入口） --------------------------
int main(int argc, char **argv) {
    // ===================== 核心控制模块初始化 =====================
    UIctr uiController(mj_model, mj_data);   // MuJoCo可视化UI控制器（窗口/视角/交互）
    MJ_Interface mj_interface(mj_model, mj_data);  // MuJoCo数据接口（读传感器/写执行器指令）
    // 机器人运动学/动力学求解器（加载机器人URDF模型，用于逆解/动力学计算）
    Pin_KinDyn kinDynSolver("../models/AzureLoong.urdf");  
    DataBus RobotState(kinDynSolver.model_nv);  // 全局数据总线（所有模块共享状态）
    // 优先级WBC控制器初始化（参数：自由度/接触点数/优化维度/权重/仿真步长）
    WBC_priority WBC_solv(kinDynSolver.model_nv, 18, 22, 0.7, mj_model->opt.timestep);
    MPC MPC_solv(dt_200Hz);  // MPC控制器（盲踩障碍物时轨迹预测，控制周期5ms）
    // 步态调度器（盲踩专用，参数：步长0.25m/仿真步长，适配障碍物高度）
    GaitScheduler gaitScheduler(0.25, mj_model->opt.timestep);  
    // PVT关节控制器（参数：仿真步长/关节配置文件，配置PD参数适配障碍物冲击）
    PVT_Ctr pvtCtr(mj_model->opt.timestep, "../common/joint_ctrl_config.json");  
    FootPlacement footPlacement;  // 足端放置规划器（盲踩时动态调整落足高度/位置）
    // 期望速度解析器（生成盲踩时的前进/转向速度指令）
    JoyStickInterpreter jsInterp(mj_model->opt.timestep);  
    DataLogger logger("../record/datalog_obstacle.log");  // 数据记录器（保存盲踩仿真数据）

    // ===================== 可视化窗口初始化 =====================
    uiController.iniGLFW();                          // 初始化GLFW图形库
    uiController.enableTracking();                   // 启用机器人躯干视角跟踪（跟随机器人运动）
    // 创建仿真窗口（参数：窗口名称/是否保存视频；注：保存视频15秒约占2.5GB空间）
    uiController.createWindow("双足机器人盲踩障碍物仿真", false);  

    // ===================== 机器人物理参数初始化 =====================
    double stand_legLength = 1.01;  // 机器人站立时腿长（基链接到地面高度）
    double foot_height = 0.07;      // 脚踝到地面的基础高度（适配障碍物高度）
    double xv_des = 0.8;            // 盲踩时前进期望速度（降低速度提升稳定性）
    int model_nv = kinDynSolver.model_nv;  // 机器人总自由度（含浮动基）

    // 盲踩障碍物关键参数配置
    RobotState.width_hips = 0.229;  // 髋部宽度（步态规划基础参数）
    footPlacement.kp_vx = 0.03;     // 足端x方向速度跟踪比例系数（盲踩时速度修正）
    footPlacement.kp_vy = 0.035;    // 足端y方向速度跟踪比例系数（横向避障修正）
    footPlacement.kp_wz = 0.03;     // 偏航角速度跟踪比例系数（转向避障修正）
    footPlacement.stepHeight = 0.18;// 盲踩时抬脚高度（高于常规0.12m，适配障碍物）
    footPlacement.legLength = stand_legLength;  // 腿长参数传递给足端规划器

    // 初始化MuJoCo初始关节位姿（从模型预设关键位姿读取）
    mju_copy(mj_data->qpos, mj_model->key_qpos, mj_model->nq * 1);

    // ===================== 关节/足端期望状态初始化 =====================
    // 关节指令向量（去除6个浮动基自由度，仅保留驱动关节）
    std::vector<double> motors_pos_des(model_nv - 6, 0);  // 关节期望位置
    std::vector<double> motors_pos_cur(model_nv - 6, 0);  // 关节当前位置
    std::vector<double> motors_vel_des(model_nv - 6, 0);  // 关节期望速度
    std::vector<double> motors_vel_cur(model_nv - 6, 0);  // 关节当前速度
    std::vector<double> motors_tau_des(model_nv - 6, 0);  // 关节期望力矩

    // 足端初始期望位姿（机器人基坐标系下）
    Eigen::Vector3d fe_l_pos_L_des = {-0.018, 0.113, -stand_legLength};  // 左脚期望位置
    Eigen::Vector3d fe_r_pos_L_des = {-0.018, -0.116, -stand_legLength}; // 右脚期望位置
    Eigen::Vector3d fe_l_eul_L_des = {-0.000, -0.008, -0.000};  // 左脚期望欧拉角（滚转/俯仰/偏航）
    Eigen::Vector3d fe_r_eul_L_des = {0.000, -0.008, 0.000};   // 右脚期望欧拉角

    // 欧拉角转旋转矩阵（足端期望姿态）
    Eigen::Matrix3d fe_l_rot_des = eul2Rot(fe_l_eul_L_des(0), fe_l_eul_L_des(1), fe_l_eul_L_des(2));
    Eigen::Matrix3d fe_r_rot_des = eul2Rot(fe_r_eul_L_des(0), fe_r_eul_L_des(1), fe_r_eul_L_des(2));

    // 手部初始期望位姿（基坐标系下）
    Eigen::Vector3d hd_l_pos_L_des = {-0.02, 0.32, -0.159};  // 左手期望位置
    Eigen::Vector3d hd_r_pos_L_des = {-0.02, -0.32, -0.159}; // 右手期望位置
    Eigen::Vector3d hd_l_eul_L_des = {-1.253, 0.122, -1.732};  // 左手期望欧拉角
    Eigen::Vector3d hd_r_eul_L_des = {1.253, 0.122, 1.732};   // 右手期望欧拉角
    Eigen::Matrix3d hd_l_rot_des = eul2Rot(hd_l_eul_L_des(0), hd_l_eul_L_des(1), hd_l_eul_L_des(2));
    Eigen::Matrix3d hd_r_rot_des = eul2Rot(hd_r_eul_L_des(0), hd_r_eul_L_des(1), hd_r_eul_L_des(2));

    // ===================== 逆运动学求解（初始位姿） =====================
    // 求解腿部逆运动学（根据足端期望位姿）
    auto resLeg = kinDynSolver.computeInK_Leg(fe_l_rot_des, fe_l_pos_L_des, fe_r_rot_des, fe_r_pos_L_des);
    // 求解手部逆运动学（根据手部期望位姿）
    auto resHand = kinDynSolver.computeInK_Hand(hd_l_rot_des, hd_l_pos_L_des, hd_r_rot_des, hd_r_pos_L_des);
    
    // 初始化期望关节位置（填充腿+手关节逆解结果）
    Eigen::VectorXd qIniDes = Eigen::VectorXd::Zero(mj_model->nq, 1);
    qIniDes.block(7, 0, mj_model->nq - 7, 1) = resLeg.jointPosRes + resHand.jointPosRes;
    WBC_solv.setQini(qIniDes, RobotState.q);  // 给WBC控制器设置初始关节位姿

    // ===================== 数据记录器配置（盲踩仿真数据） =====================
    logger.addIterm("simTime", 1);                  // 仿真时间（秒）
    logger.addIterm("motor_pos_des", model_nv - 6);  // 关节期望位置
    logger.addIterm("motor_pos_cur", model_nv - 6);  // 关节当前位置
    logger.addIterm("motor_vel_des", model_nv - 6);  // 关节期望速度
    logger.addIterm("motor_vel_cur", model_nv - 6);  // 关节当前速度
    logger.addIterm("motor_tor_des", model_nv - 6);  // 关节期望力矩
    logger.addIterm("rpyVal", 3);                    // 躯干欧拉角（滚转/俯仰/偏航）
    logger.addIterm("base_omega_W", 3);              // 躯干角速度（世界坐标系）
    logger.addIterm("gpsVal", 3);                    // 躯干位置（世界坐标系）
    logger.addIterm("base_vel", 3);                  // 躯干线速度
    logger.addIterm("foot_L_pos", 3);                // 左脚端实际位置（盲踩关键）
    logger.addIterm("foot_R_pos", 3);                // 右脚端实际位置（盲踩关键）
    logger.addIterm("obstacle_contact_force", 3);    // 足端与障碍物接触力（盲踩反馈）
    logger.finishItermAdding();                      // 完成日志项注册

    // ===================== 仿真时序参数（盲踩逻辑） =====================
    int MPC_count = 0;                  // MPC控制器调用计数器（200Hz周期）
    double startSteppingTime = 3;       // 3秒后开始迈步（初始稳定）
    double startWalkingTime = 5;        // 5秒后开始盲踩障碍物行走
    double simEndTime = 40;             // 仿真总时长（40秒）
    mjtNum simstart = mj_data->time;    // 每帧渲染起始时间（控制60Hz帧率）
    double simTime = mj_data->time;     // 当前仿真时间

    // ===================== 主仿真循环（核心控制逻辑） =====================
    while (!glfwWindowShouldClose(uiController.window)) {
        simstart = mj_data->time;
        // 内层循环：保证60Hz可视化帧率（每帧运行1/60秒仿真）
        while (mj_data->time - simstart < 1.0 / 60.0 && uiController.runSim) {
            mj_step(mj_model, mj_data);  // 推进MuJoCo仿真一步
            simTime = mj_data->time;     // 更新当前仿真时间

            // ---------------------- 1. 传感器数据更新 ----------------------
            mj_interface.updateSensorValues();  // 读取MuJoCo传感器（关节位姿/力/接触传感器）
            mj_interface.dataBusWrite(RobotState);  // 传感器数据写入全局数据总线

            // ---------------------- 2. 运动学/动力学计算 ----------------------
            kinDynSolver.dataBusRead(RobotState);  // 从总线读取机器人状态
            kinDynSolver.computeJ_dJ();            // 计算雅可比矩阵及导数（足端速度映射）
            kinDynSolver.computeDyn();             // 计算动力学参数（惯性矩阵/科氏力/重力）
            kinDynSolver.dataBusWrite(RobotState);  // 动力学结果写回总线

            // ---------------------- 3. 盲踩速度指令生成 ----------------------
            if (simTime > startWalkingTime) {
                // 盲踩阶段：设置前进速度（0.8m/s），关闭视觉，仅靠本体感知
                jsInterp.setWzDesLPara(0, 1);        // 偏航角速度期望0（直行）
                jsInterp.setVxDesLPara(xv_des, 2.0); // 前进速度0.8m/s，滤波系数2.0（平滑）
                RobotState.motionState = DataBus::Walk;  // 运动状态标记为"行走（盲踩）"
            } else {
                // 未到行走时间：保持初始位姿（躯干位置/偏航角）
                jsInterp.setIniPos(RobotState.q(0), RobotState.q(1), RobotState.base_rpy(2));
            }
            jsInterp.step();  // 更新期望速度（滤波平滑）
            // 手动设置躯干z方向期望高度（适配障碍物高度）
            RobotState.js_pos_des(2) = stand_legLength + foot_height + 0.05; 
            jsInterp.dataBusWrite(RobotState);  // 期望速度写入总线

            // ---------------------- 4. 盲踩步态调度 ----------------------
            if (simTime >= startSteppingTime) {
                // 步态调度器：盲踩时动态调整支撑腿/摆动腿时序
                gaitScheduler.dataBusRead(RobotState);  
                gaitScheduler.step();  // 更新步态阶段（支撑/摆动/离地/落地）
                gaitScheduler.dataBusWrite(RobotState);  

                // 足端放置规划：盲踩核心逻辑（无视觉，根据步态调整落足高度/位置）
                footPlacement.dataBusRead(RobotState);  
                footPlacement.getSwingPos();  // 计算摆动腿落足点（抬高避开障碍物）
                footPlacement.dataBusWrite(RobotState);  
            }

            // ---------------------- 5. MPC模型预测控制（盲踩轨迹优化） ----------------------
            MPC_count++;
            if (MPC_count % (int)(dt_200Hz / mj_model->opt.timestep) == 0) {
                // 每5ms运行一次MPC（200Hz）：预测未来轨迹，适配障碍物
                MPC_solv.dataBusRead(RobotState);  
                MPC_solv.compute();  // 求解MPC优化问题（调整期望轨迹）
                MPC_solv.dataBusWrite(RobotState);  
                MPC_count = 0;  // 重置计数器
            }

            // ---------------------- 6. WBC优先级控制（力/运动混合） ----------------------
            WBC_solv.dataBusRead(RobotState);  // 读取MPC期望轨迹/足端力
            WBC_solv.computeDdq(kinDynSolver); // 求解期望关节加速度
            WBC_solv.computeTau();             // 求解期望关节力矩（适配障碍物冲击）
            WBC_solv.dataBusWrite(RobotState);  // WBC结果写回总线

            // ---------------------- 7. PVT关节控制器（闭环执行） ----------------------
            if (simTime <= 3) {
                // 前3秒：初始化关节位置（低增益，避免冲击）
                pvtCtr.calMotorsPVT(100.0/1000.0/180.0*3.1415);
            } else {
                // 盲踩阶段：设置关节PD参数（提高刚度，应对障碍物冲击）
                // 左腿PD参数（髋/膝/踝）
                pvtCtr.setJointPD(450, 20, "J_hip_l_roll");    // 左髋滚转（KP=450, KD=20）
                pvtCtr.setJointPD(250, 15, "J_hip_l_yaw");     // 左髋偏航
                pvtCtr.setJointPD(350, 15, "J_hip_l_pitch");   // 左髋俯仰
                pvtCtr.setJointPD(350, 18, "J_knee_l_pitch");  // 左膝俯仰
                pvtCtr.setJointPD(350, 22, "J_ankle_l_pitch"); // 左踝俯仰
                pvtCtr.setJointPD(350, 20, "J_ankle_l_roll");  // 左踝滚转

                // 右腿PD参数（对称配置）
                pvtCtr.setJointPD(450, 20, "J_hip_r_roll");    
                pvtCtr.setJointPD(250, 15, "J_hip_r_yaw");     
                pvtCtr.setJointPD(350, 15, "J_hip_r_pitch");   
                pvtCtr.setJointPD(350, 18, "J_knee_r_pitch");  
                pvtCtr.setJointPD(350, 22, "J_ankle_r_pitch"); 
                pvtCtr.setJointPD(350, 20, "J_ankle_r_roll");  

                pvtCtr.calMotorsPVT();  // 计算关节力矩指令（PD闭环）
            }
            pvtCtr.dataBusRead(RobotState);    // 读取期望关节指令
            pvtCtr.dataBusWrite(RobotState);   // 写入最终力矩指令
            // 将关节力矩指令下发到MuJoCo执行器
            mj_interface.setMotorsTorque(RobotState.motors_tor_out);  

            // ---------------------- 8. 盲踩仿真数据记录 ----------------------
            logger.startNewLine();  // 新建日志行
            logger.recItermData("simTime", simTime);                  // 仿真时间
            logger.recItermData("motor_pos_des", RobotState.motors_pos_des); // 关节期望位置
            logger.recItermData("motor_pos_cur", RobotState.motors_pos_cur); // 关节当前位置
            logger.recItermData("foot_L_pos", RobotState.foot_L_pos); // 左脚位置（盲踩关键）
            logger.recItermData("foot_R_pos", RobotState.foot_R_pos); // 右脚位置（盲踩关键）
            logger.recItermData("obstacle_contact_force", RobotState.contact_force); // 障碍物接触力
            logger.finishLine();  // 完成日志行写入

            // ---------------------- 9. 调试信息输出 ----------------------
            printf("盲踩仿真时间: %.2fs | 躯干高度: %.3fm | 左足接触力: %.1fN\n",
                   simTime, RobotState.basePos[2], RobotState.contact_force[2]);
        }

        // 仿真时长达标后退出
        if (mj_data->time >= simEndTime) {
            break;
        }

        uiController.updateScene();  // 更新GLFW窗口渲染（显示机器人/障碍物/力反馈）
    }

    // ===================== 资源释放 =====================
    mj_deleteData(mj_data);    // 释放MuJoCo仿真数据
    mj_deleteModel(mj_model);  // 释放MuJoCo模型
    glfwTerminate();           // 终止GLFW图形库
    logger.close();            // 关闭数据日志文件

    return 0;
}

机器人盲踩障碍物测试的效果如图11-17所示。

图11-17 机器人盲踩障碍物测试