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

仿真主循环与控制逻辑

进入主循环前，先初始化仿真时长、迈步/行走时间节点等参数，并完成 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;
    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 窗口的仿真场景渲染
}

资源释放与退出

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

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

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