（11-4-01）完整人形机器人的设计与实现案例：机器人的站立与行走

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); // MuJoCo的UI控制器（负责仿真可视化、交互） MJ_Interface mj_interface(mj_model, mj_data); // MuJoCo数据接口（读取传感器数据、设置执行器指令） Pin_KinDyn kinDynSolver("../models/AzureLoong.urdf"); // 机器人运动学与动力学求解器（加载机器人URDF模型） DataBus RobotState(kinDynSolver.model_nv); // 机器人状态数据总线（统一存储/传递各类状态信息） // 优先级加权优化控制器(WBC)实例化 // 参数：机器人自由度、接触点数量、优化变量维度、权重系数、仿真时间步长 WBC_priority WBC_solv(kinDynSolver.model_nv, 18, 22, 0.7, mj_model->opt.timestep); GaitScheduler gaitScheduler(0.4, mj_model->opt.timestep); // 步态调度器（步长0.4m，仿真时间步长） // PVT关节控制器（基于时间步长初始化，加载关节控制配置文件） PVT_Ctr pvtCtr(mj_model->opt.timestep,"../common/joint_ctrl_config.json"); FootPlacement footPlacement; // 足端放置规划器（计算行走时摆动脚的目标位置） JoyStickInterpreter jsInterp(mj_model->opt.timestep); // 期望基链接速度生成器（解析摇杆指令/预设速度） DataLogger logger("../record/datalog.log"); // 数据日志记录器（指定日志保存路径） StateEst StateModule(mj_model->opt.timestep); // 机器人状态估计模块（基于仿真时间步长初始化） // 变量初始化 double stand_legLength = 1.01; // 机器人站立时的期望基链接高度（腿长） double foot_height = 0.07; // 脚踝关节到地面的距离 double xv_des = 0.7; // 机器人x方向（前进方向）的期望速度 RobotState.width_hips = 0.229; // 机器人髋部宽度（用于步态规划） footPlacement.kp_vx = 0.03; // 足端放置x方向比例系数（速度跟踪） footPlacement.kp_vy = 0.035; // 足端放置y方向比例系数（速度跟踪） footPlacement.kp_wz = 0.03; // 足端放置z轴旋转比例系数（偏航角跟踪） footPlacement.stepHeight = 0.12; // 行走时抬脚高度 footPlacement.legLength=stand_legLength; // 给足端规划器赋值站立腿长 int model_nv=kinDynSolver.model_nv; // 机器人自由度（提取动力学求解器中的自由度参数） // 初始化足端和手部的期望位置、姿态相关变量 std::vector<double> motors_pos_des(model_nv-6,0); // 电机期望位置（去除6个浮动基自由度） 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); // 电机期望力矩 std::vector<double> motors_tau_cur(model_nv-6,0); // 电机当前力矩 // 左足在基坐标系下的期望位置（x,y,z） Eigen::Vector3d fe_l_pos_L_des={-0.018, 0.113, -stand_legLength}; // 右足在基坐标系下的期望位置（x,y,z） 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)); // 左手期望关节角度（7个自由度） Eigen::Vector<double, 7> hd_l_des{0.475, -1.12, 1.9, 0.86, -0.356, 0, 0}; // 右手期望关节角度（7个自由度） Eigen::Vector<double, 7> hd_r_des{-0.475, -1.12, -1.9, 0.86, 0.356, 0, 0}; // 计算腿部逆运动学：根据足端期望姿态和位置，求解腿部关节角度 auto resLeg=kinDynSolver.computeInK_Leg(fe_l_rot_des,fe_l_pos_L_des,fe_r_rot_des,fe_r_pos_L_des); // 初始化期望关节位置向量（全零），维度为MuJoCo模型的关节数量 Eigen::VectorXd qIniDes=Eigen::VectorXd::Zero(mj_model->nq,1); // 将腿部逆运动学求解的关节角度填充到期望关节位置（从第7个关节开始，跳过手臂/头部） qIniDes.block(7, 0, mj_model->nq - 7, 1) = resLeg.jointPosRes; qIniDes.block(7, 0, 7, 1) = hd_l_des; // 填充左手关节期望角度 qIniDes.block(14, 0, 7, 1) = hd_r_des; // 填充右手关节期望角度 // 给WBC控制器设置初始关节位置（用于初始化控制器） WBC_solv.setQini(qIniDes,RobotState.q); // 为数据日志记录器注册需要记录的变量名及维度 logger.addIterm("simTime", 1); // 仿真时间（标量，维度1） logger.addIterm("motors_pos_cur",model_nv-6); // 电机当前位置（维度：自由度-6） logger.addIterm("motors_vel_cur",model_nv-6); // 电机当前速度（维度：自由度-6） logger.addIterm("rpy",3); // 基链接欧拉角（滚转、俯仰、偏航，维度3） logger.addIterm("fL",3); // 左足接触力（维度3） logger.addIterm("fR",3); // 右足接触力（维度3） logger.addIterm("basePos",3); // 基链接位置（x,y,z，维度3） logger.addIterm("baseLinVel",3); // 基链接线速度（x,y,z，维度3） logger.addIterm("baseAcc",3); // 基链接加速度（x,y,z，维度3） logger.addIterm("baseAngVel",3); // 基链接角速度（滚转、俯仰、偏航，维度3） logger.finishItermAdding(); // 完成变量注册 

（3）下面代码的功能是初始化仿真时长、迈步/行走时间节点等参数并完成GLFW可视化窗口创建与视角配置；通过双层循环实现60帧/秒的交互式仿真渲染，在仿真步内完成传感器数据更新、状态估计模块运行、运动学动力学计算；达到指定时间节点后启动步态调度与足端放置规划，设置行走期望速度；执行WBC控制器计算求解关节力矩，结合PD参数完成PVT关节控制器计算并下发力矩指令到MuJoCo；记录每帧仿真数据到日志并打印调试信息，仿真时长达标后退出循环，是机器人行走控制的核心执行逻辑。

 /// ----------------- 仿真主循环 --------------- double simEndTime=30; // 仿真总时长（30秒） mjtNum simstart = mj_data->time; // 记录每次渲染帧的起始仿真时间 double simTime = mj_data->time; // 当前仿真时间 double startSteppingTime=3; // 开始迈步的时间节点（3秒后） double startWalkingTime=5; // 开始行走的时间节点（5秒后） // 初始化UI：基于GLFW创建可视化窗口 uiController.iniGLFW(); // 初始化GLFW库 uiController.enableTracking(); // 启用机器人第1个身体的视角跟踪（跟随机器人） uiController.createWindow("Demo",false); // 创建仿真窗口，标题为"Demo"，非全屏模式 // 窗口未关闭时，持续运行仿真与渲染 while( !glfwWindowShouldClose(uiController.window)) { // 推进交互式仿真，确保每帧渲染间隔为1/60秒（60帧/秒） // 注：假设MuJoCo仿真速度快于实时，此循环能按时完成下一帧渲染； // 若仿真速度慢，需添加CPU计时器，到渲染时间时退出循环 simstart=mj_data->time; // 当当前帧的仿真时长未到1/60秒，且仿真未暂停时（按"1"暂停/继续，"2"单步仿真） while( mj_data->time - simstart < 1.0/60.0 && uiController.runSim) { mj_step(mj_model, mj_data); // 推进一个MuJoCo仿真时间步 simTime=mj_data->time; // 更新当前仿真时间 printf("-------------%.3f 秒------------\n",simTime); // 打印当前仿真时间 mj_interface.updateSensorValues(); // 更新所有传感器数据（读取仿真中的传感器反馈） mj_interface.dataBusWrite(RobotState); // 将传感器数据写入机器人状态数据总线 // 仿真时间超过1秒且状态估计模块未初始化时，执行初始化 if (simTime > 1 && StateModule.flag_init) { std::cout << "初始化状态估计模块" << std::endl; StateModule.init(RobotState); // 用当前机器人状态初始化状态估计模块 } StateModule.set(RobotState); // 将机器人状态传入状态估计模块 StateModule.update(); // 运行状态估计算法（更新位姿、速度等估计值） StateModule.get(RobotState); // 将估计结果写回机器人状态数据总线 // 更新机器人运动学与动力学信息 kinDynSolver.dataBusRead(RobotState); // 从数据总线读取机器人状态 kinDynSolver.computeJ_dJ(); // 计算雅可比矩阵及其导数（用于运动学求解） kinDynSolver.computeDyn(); // 计算动力学参数（如惯性矩阵、科氏力、重力项） kinDynSolver.dataBusWrite(RobotState); // 将动力学计算结果写回数据总线 StateModule.setF(RobotState); // 将足端接触力传入状态估计模块 StateModule.updateF(); // 更新基于接触力的状态估计 StateModule.getF(RobotState); // 将更新后的状态写回数据总线 // 执行器指令发送区域（关节分配： // 左臂：0-6，右臂：7-13，头部：14-15，腰部：16-18，左腿：19-24，右腿：25-30） // 达到行走时间节点后，设置期望速度并切换为行走状态 if (simTime > startWalkingTime) { jsInterp.setWzDesLPara(0, 1); // 设置期望偏航角速度参数（目标值0，滤波系数1） jsInterp.setVxDesLPara(xv_des, 2.0); // 设置x方向期望速度（0.7m/s，滤波系数2.0） RobotState.motionState = DataBus::Walk; // 将机器人运动状态设为"行走" } else { // 未到行走时间时，设置初始位置（基链接x、y位置，偏航角） jsInterp.setIniPos(RobotState.q(0), RobotState.q(1), RobotState.base_rpy(2)); } // 达到迈步时间节点后，启动步态规划流程 if (simTime >= startSteppingTime) { jsInterp.step(); // 更新期望速度生成器（计算滤波后的期望速度） // 设置初始位置（基链接x、y、z位置，偏航角），用于速度生成参考 jsInterp.setIniPos(RobotState.q(0), RobotState.q(1), stand_legLength + foot_height, RobotState.base_rpy(2)); // 将期望速度写入数据总线（仅覆盖x/y/z位置、偏航角、x/y线速度、偏航角速度） jsInterp.dataBusWrite(RobotState); // 步态调度器流程 gaitScheduler.start(); // 启动步态调度器 RobotState.motionState = DataBus::Walk; // 确认机器人运动状态为行走 gaitScheduler.dataBusRead(RobotState); // 从数据总线读取状态（期望速度、足端位置等） gaitScheduler.step(); // 推进步态调度（更新支撑腿/摆动腿状态） gaitScheduler.dataBusWrite(RobotState); // 将步态信息写回数据总线 // 足端放置规划流程 footPlacement.dataBusRead(RobotState); // 从数据总线读取步态、期望速度等信息 footPlacement.getSwingPos(); // 计算摆动脚的目标位置 footPlacement.dataBusWrite(RobotState); // 将摆动脚位置写回数据总线 } // ------------- WBC控制器计算 ------------ // 初始化WBC输入（期望加速度、速度、位置增量全零） RobotState.des_ddq = Eigen::VectorXd::Zero(mj_model->nv); RobotState.des_dq = Eigen::VectorXd::Zero(mj_model->nv); RobotState.des_delta_q = Eigen::VectorXd::Zero(mj_model->nv); // 设置期望接触力前馈值（左右足各370N垂直力，其他方向力为0） RobotState.Fr_ff << 0,0,370,0,0,0, 0,0,370,0,0,0; // 行走时间节点+1秒后，调整期望位置增量、速度、加速度，实现向前行走 if (simTime > startWalkingTime + 1) { // 期望位置增量：x/y方向 = 期望速度 * 仿真时间步长 RobotState.des_delta_q.block<2, 1>(0, 0) << jsInterp.vx_W * mj_model->opt.timestep, jsInterp.vy_W * mj_model->opt.timestep; // 偏航角位置增量 = 期望偏航角速度 * 仿真时间步长 RobotState.des_delta_q(5) = jsInterp.wz_L * mj_model->opt.timestep; // 期望速度：x/y方向 = 期望世界坐标系速度 RobotState.des_dq.block<2, 1>(0, 0) << jsInterp.vx_W, jsInterp.vy_W; // 期望偏航角速度 = 期望基坐标系偏航角速度 RobotState.des_dq(5) = jsInterp.wz_L; double k = 5; // 比例系数（用于速度跟踪） // 期望加速度：x/y方向 = 比例系数*(期望速度 - 实际速度) RobotState.des_ddq.block<2, 1>(0, 0) << k * (jsInterp.vx_W - RobotState.dq(0)), k * (jsInterp.vy_W - RobotState.dq(1)); // 期望偏航角加速度 = 比例系数*(期望偏航角速度 - 实际偏航角速度) RobotState.des_ddq(5) = k * (jsInterp.wz_L - RobotState.dq(5)); } // WBC核心计算流程 WBC_solv.dataBusRead(RobotState); // 从数据总线读取WBC输入（期望运动、接触力等） WBC_solv.computeDdq(kinDynSolver); // 求解期望关节加速度（结合动力学求解器） WBC_solv.computeTau(); // 求解期望关节力矩 WBC_solv.dataBusWrite(RobotState); // 将WBC计算结果（力矩、速度等）写回数据总线 // 获取最终关节控制指令 if (simTime<=startSteppingTime){ // 未迈步时，使用初始逆运动学求解的关节位置作为期望位置 Eigen::VectorXd temp = resLeg.jointPosRes; temp.block(0, 0, 7, 1) = hd_l_des; // 填充左手期望角度 temp.block(7, 0, 7, 1) = hd_r_des; // 填充右手期望角度 RobotState.motors_pos_des = eigen2std(temp); // Eigen向量转std::vector RobotState.motors_vel_des=motors_vel_des; // 期望速度保持全零 RobotState.motors_tor_des=motors_tau_des; // 期望力矩保持全零 } else { // 迈步后，积分WBC求解的位置增量，得到期望关节位置 Eigen::VectorXd pos_des=kinDynSolver.integrateDIY(RobotState.q, RobotState.wbc_delta_q_final); // 提取关节期望位置（跳过前7个关节，取自由度-6维度） RobotState.motors_pos_des = eigen2std(pos_des.block(7,0, model_nv-6,1)); RobotState.motors_vel_des = eigen2std(RobotState.wbc_dq_final); // 期望速度（WBC求解结果） RobotState.motors_tor_des = eigen2std(RobotState.wbc_tauJointRes); // 期望力矩（WBC求解结果） } // PVT关节控制器计算 pvtCtr.dataBusRead(RobotState); // 从数据总线读取期望关节指令 if (simTime<=3) { // 前3秒：计算PVT控制指令（参数为角度转换系数：100/1000/180*π） pvtCtr.calMotorsPVT(100.0/1000.0/180.0*3.1415); } else { // 3秒后：设置关节PD控制参数（比例/微分系数） double kp = 1.; // PD比例系数缩放因子 double kd = 1.; // PD微分系数缩放因子 // 左腿关节PD参数 pvtCtr.setJointPD(400 * kp, 15 * kd, "J_hip_l_roll"); // 左髋滚转关节 pvtCtr.setJointPD(200 * kp, 10 * kd, "J_hip_l_yaw"); // 左髋偏航关节 pvtCtr.setJointPD(300 * kp, 10 * kd, "J_hip_l_pitch"); // 左髋俯仰关节 pvtCtr.setJointPD(300 * kp, 14 * kd, "J_knee_l_pitch"); // 左膝俯仰关节 pvtCtr.setJointPD(300 * kp, 18 * kd, "J_ankle_l_pitch"); // 左踝俯仰关节 pvtCtr.setJointPD(300 * kp, 16 * kd, "J_ankle_l_roll"); // 左踝滚转关节 // 右腿关节PD参数 pvtCtr.setJointPD(400 * kp, 15 * kd, "J_hip_r_roll"); // 右髋滚转关节 pvtCtr.setJointPD(200 * kp, 10 * kd, "J_hip_r_yaw"); // 右髋偏航关节 pvtCtr.setJointPD(300 * kp, 10 * kd, "J_hip_r_pitch"); // 右髋俯仰关节 pvtCtr.setJointPD(300 * kp, 14 * kd, "J_knee_r_pitch"); // 右膝俯仰关节 pvtCtr.setJointPD(300 * kp, 18 * kd, "J_ankle_r_pitch"); // 右踝俯仰关节 pvtCtr.setJointPD(300 * kp, 16 * kd, "J_ankle_r_roll"); // 右踝滚转关节 pvtCtr.calMotorsPVT(); // 计算PVT关节控制指令（结合PD参数） } pvtCtr.dataBusWrite(RobotState); // 将PVT计算的最终力矩指令写回数据总线 mj_interface.setMotorsTorque(RobotState.motors_tor_out); // 将关节力矩指令设置到MuJoCo仿真执行器 // 记录当前帧的仿真数据到日志 logger.startNewLine(); // 开始新一行日志记录 logger.recItermData("simTime", simTime); // 记录仿真时间 logger.recItermData("motors_pos_cur",RobotState.motors_pos_cur); // 记录电机当前位置 logger.recItermData("motors_vel_cur",RobotState.motors_vel_cur); // 记录电机当前速度 logger.recItermData("rpy",RobotState.rpy); // 记录基链接欧拉角 logger.recItermData("fL",RobotState.fL); // 记录左足接触力 logger.recItermData("fR",RobotState.fR); // 记录右足接触力 logger.recItermData("basePos",RobotState.basePos); // 记录基链接位置 logger.recItermData("baseLinVel",RobotState.baseLinVel); // 记录基链接线速度 logger.recItermData("baseAcc",RobotState.baseAcc); // 记录基链接加速度 logger.recItermData("baseAngVel",RobotState.baseAngVel); // 记录基链接角速度 logger.finishLine(); // 完成当前行日志记录 // 打印关键状态信息（调试用） printf("基链接欧拉角=[%.5f, %.5f, %.5f]\n", RobotState.rpy[0], RobotState.rpy[1], RobotState.rpy[2]); printf("基链接位置=[%.5f, %.5f, %.5f]\n", RobotState.basePos[0], RobotState.basePos[1], RobotState.basePos[2]); printf("基链接线速度=[%.5f, %.5f, %.5f]\n", RobotState.baseLinVel[0], RobotState.baseLinVel[1], RobotState.baseLinVel[2]); } // 仿真时间达到总时长后，退出循环 if (mj_data->time>=simEndTime) { break; } uiController.updateScene(); // 更新GLFW窗口的仿真场景渲染 } 

（4）下面代码的功能是调用MuJoCo的资源释放函数，释放仿真数据结构（mjData）与模型（mjModel）资源，调用glfwTerminate终止GLFW图形库以释放可视化窗口相关资源，避免内存泄漏，确保程序正常退出并完成所有资源清理。

 // 释放MuJoCo资源 mj_deleteData(mj_data); mj_deleteModel(mj_model); // 终止GLFW库 glfwTerminate(); return 0; } 

机器人行走测试的效果如图11-16所示。

图11-16  机器人行走测试效果