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仿真数据结构，该结构用于存储仿真过程中关节位置、力等动态数据，为后续机器人行走仿真搭建基础的模型和数据载体。
python
// 错误信息缓冲区，初始值为"无法加载二进制模型"
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控制器初始关节位置；为数据日志记录器注册仿真时间、电机状态、基链接位姿等需记录的变量及维度，完成仿真前全量参数与模块的初始化工作。
python
//************************
// 主函数：程序入口
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；记录每帧仿真数据到日志并打印调试信息，仿真时长达标后退出循环，是机器人行走控制的核心执行逻辑。
python
/// ----------------- 仿真主循环 ---------------
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图形库以释放可视化窗口相关资源，避免内存泄漏，确保程序正常退出并完成所有资源清理。
python
// 释放MuJoCo资源
mj_deleteData(mj_data);
mj_deleteModel(mj_model);
// 终止GLFW库
glfwTerminate();
return 0;
}机器人行走测试的效果如图11-16所示。
图11-16 机器人行走测试效果