11.5 运动控制算法
OpenLoong-Dyn-Control 项目提供了一套基于 MPC（模型预测控制）和 WBC（全身体控制）的仿人机器人运动控制框架，可部署在 MuJoCo 仿真平台上。该框架基于上海人形机器人创新中心的青龙机器人模型，支持行走、跳跃及盲踩障碍物等运动示例，实物样机已验证了相关功能。其设计特点包括易部署（依赖简化）、可扩展（分层模块化）以及易理解（代码逻辑清晰），非常适合二次开发。
11.5.1 机器人的站立与行走
核心文件 walk_wbc.cpp 位于 demo 目录下，负责实现从站立到行走的完整仿真流程。程序加载模型并初始化 UI 控制器、动力学求解器、WBC 优先级控制器及步态调度器等模块，通过仿真循环推进时间步。在循环中，系统更新传感器数据与机器人状态，经状态估计、运动学动力学计算后，由 WBC 求解关节控制量，结合 PVT 控制生成力矩指令。此外，还包含足端放置规划、期望速度生成及仿真数据记录等功能，支持可视化交互以监控行走过程。
模型加载与初始化
首先，我们需要创建错误信息缓冲区并初始化默认提示文本，随后调用 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);
主函数入口与模块实例化
接下来是程序入口 main 函数，这里实例化了机器人行走仿真所需的核心控制模块，包括 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(); // 完成变量注册
}
仿真主循环与控制逻辑
进入主循环前，先初始化仿真时长、迈步/行走时间节点等参数，并完成 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;
}
机器人行走测试的效果如图所示。
