人形机器人设计与实现：站立与行走控制算法

11.5 运动控制算法

OpenLoong-Dyn-Control 项目提供了一套基于 MPC（模型预测控制）和 WBC（全身体控制）的仿人机器人运动控制框架，可以部署在 Mujoco 仿真平台上。该项目基于上海人形机器人创新中心的青龙'机器人模型，提供了行走、跳跃、盲踩障碍物等运动示例，且实物样机已实现行走和盲踩障碍功能。其具有易部署（包含主要依赖，简化环境配置）、可扩展（分层模块化设计，便于二次开发）、易理解（代码结构简洁，采用读取 - 计算 - 写入逻辑）等特点。

11.5.1 机器人的站立与行走

文件 OpenLoong-Dyn-Control/demo/walk_wbc.cpp 是基于 MuJoCo 的双足机器人仿真控制程序，实现机器人从站立到行走的过程。加载模型并初始化 UI 控制器、动力学求解器、WBC 优先级控制器、步态调度器等模块，通过仿真循环推进时间步。循环中更新传感器数据与机器人状态，经状态估计、运动学动力学计算后，由 WBC 求解关节控制量，结合 PVT 控制生成力矩指令。还包含足端放置规划、期望速度生成，记录仿真数据（如关节状态、基态信息等），并支持可视化交互，实现机器人行走控制与仿真过程监控。

（1）下面代码的功能是创建错误信息缓冲区并初始化默认提示文本，调用 MuJoCo 的 mj_loadXML 函数加载指定路径的仿真场景 XML 模型文件，同时传入错误缓冲区捕获加载异常；基于加载完成的 mjModel 模型实例，调用 mj_makeData 创建对应的 mjData 仿真数据结构，该结构用于存储仿真过程中关节位置、力等动态数据，为后续机器人行走仿真搭建基础的模型和数据载体。

// 错误信息缓冲区，初始值为无法加载二进制模型
char error[1000] = "无法加载二进制模型";
// 加载 MuJoCo 仿真场景的 XML 模型文件，参数：模型路径、虚拟文件系统、错误缓冲区、缓冲区长度
mjModel* mj_model = mj_loadXML("../models/scene_board.xml", 0, error, 1000);
// 创建与模型对应的仿真数据结构，存储仿真过程中的动态数据（如关节位置、力等）
mjData* mj_data = mj_makeData(mj_model);

（2）下面代码的功能是作为程序入口定义主函数，用于实例化机器人行走仿真所需的各类核心控制模块（包括 UI 控制器、数据接口、运动学动力学求解器、WBC 控制器、步态调度器等）；初始化机器人站立腿长、期望前进速度等物理参数与步态规划参数，定义足端/手部期望位姿变量并求解腿部逆运动学，初始化 WBC 控制器初始关节位置；为数据日志记录器注册仿真时间、电机状态、基链接位姿等需记录的变量及维度，完成仿真前全量参数与模块的初始化工作。

//************************
// 主函数：程序入口
int main(int argc, const char** argv) {
    // 初始化各类核心控制类实例
    UIctr uiController(mj_model,mj_data); 
    ; 
    ; 
    ; 
    
    
    ;
    ; 
    
    ;
    FootPlacement footPlacement; 
    ; 
    ; 
    ; 
    
     stand_legLength = ; 
     foot_height = ; 
     xv_des = ; 
    RobotState.width_hips = ; 
    footPlacement.kp_vx = ; 
    footPlacement.kp_vy = ; 
    footPlacement.kp_wz = ; 
    footPlacement.stepHeight = ; 
    footPlacement.legLength=stand_legLength; 
     model_nv=kinDynSolver.model_nv; 
    
    ; 
    ; 
    ; 
    ; 
    ; 
    ; 
    
    Eigen::Vector3d fe_l_pos_L_des={, , -stand_legLength};
    
    Eigen::Vector3d fe_r_pos_L_des={, , -stand_legLength};
    
    Eigen::Vector3d fe_l_eul_L_des={, , };
    
    Eigen::Vector3d fe_r_eul_L_des={, , };
    
    Eigen::Matrix3d fe_l_rot_des= ((),(),());
    
    Eigen::Matrix3d fe_r_rot_des= ((),(),());
    
    Eigen::Vector<, > hd_l_des{, , , , , , };
    
    Eigen::Vector<, > hd_r_des{, , , , , , };
    
     resLeg=kinDynSolver.(fe_l_rot_des,fe_l_pos_L_des,fe_r_rot_des,fe_r_pos_L_des);
    
    Eigen::VectorXd qIniDes=Eigen::VectorXd::(mj_model->nq,);
    
    qIniDes.(, , mj_model->nq - , ) = resLeg.jointPosRes;
    qIniDes.(, , , ) = hd_l_des; 
    qIniDes.(, , , ) = hd_r_des; 
    
    WBC_solv.(qIniDes,RobotState.q);
    
    logger.(, ); 
    logger.(,model_nv); 
    logger.(,model_nv); 
    logger.(,); 
    logger.(,); 
    logger.(,); 
    logger.(,); 
    logger.(,); 
    logger.(,); 
    logger.(,); 
    logger.(); 
}