11.5.3 盲踩障碍物文件OpenLoong-Dyn-Control/demo/walk_mpc_wbc.cpp为双足机器人 MuJoCo仿真环境中实现盲踩障碍物功能提供支持。通过初始化UI、动力学求解、数据总线等模块主循环中结合步态调度器规划步态时序足端放置规划器确定落足位置MPC和WBC算法实时计算关节力矩在无环境感知“盲”的情况下依靠预设控制策略与身体状态反馈实现对障碍物的适应性踩踏同时记录仿真数据用于分析。/* OpenLoong动力学控制模块 - 双足机器人盲踩障碍物仿真 版权所有 (C) 2024-2025 人形机器人上海有限公司 开源地址: https://atomgit.com/openloong/openloong-dyn-control.git 邮箱: webopenloong.org.cn 功能: 实现双足机器人在MuJoCo环境下无视觉感知盲踩障碍物的行走控制 */ // -------------------------- 头文件包含 -------------------------- #include mujoco/mujoco.h // MuJoCo核心仿真库 #include GLFW/glfw3.h // GLFW图形界面库用于仿真可视化 #include GLFW_callbacks.h // GLFW回调函数处理窗口交互、键盘输入 #include MJ_interface.h // MuJoCo数据交互接口传感器/执行器 #include PVT_ctrl.h // PVT关节控制器位置/速度/力矩闭环 #include data_logger.h // 仿真数据记录器保存日志用于分析 #include data_bus.h // 全局数据总线模块间数据共享 #include pino_kin_dyn.h // 机器人运动学/动力学求解器 #include useful_math.h // 实用数学工具欧拉角转旋转矩阵等 #include wbc_priority.h // 优先级加权WBC控制器力/运动混合控制 #include mpc.h // MPC模型预测控制轨迹规划与障碍物适应 #include gait_scheduler.h // 步态调度器盲踩时步态时序规划 #include foot_placement.h // 足端放置规划器盲踩障碍物落足点调整 #include joystick_interpreter.h // 期望速度解析器生成行走/避障速度指令 #include string // 字符串处理 #include iostream // 控制台输出 #include Eigen/Core // Eigen矩阵运算库 // -------------------------- 全局常量/变量定义 -------------------------- const double dt 0.001; // 基础控制时间步长1ms const double dt_200Hz 0.005; // MPC控制周期200Hz // MuJoCo模型加载相关 char error[1000] 无法加载二进制模型; // 模型加载错误信息缓冲区 // 加载盲踩障碍物仿真场景包含障碍物的XML模型文件 mjModel* mj_model mj_loadXML(../models/scene_obstacle.xml, 0, error, 1000); mjData* mj_data mj_makeData(mj_model); // 创建MuJoCo仿真数据结构存储关节位姿/力等动态数据 // -------------------------- 主函数程序入口 -------------------------- int main(int argc, char **argv) { // 核心控制模块初始化 UIctr uiController(mj_model, mj_data); // MuJoCo可视化UI控制器窗口/视角/交互 MJ_Interface mj_interface(mj_model, mj_data); // MuJoCo数据接口读传感器/写执行器指令 // 机器人运动学/动力学求解器加载机器人URDF模型用于逆解/动力学计算 Pin_KinDyn kinDynSolver(../models/AzureLoong.urdf); DataBus RobotState(kinDynSolver.model_nv); // 全局数据总线所有模块共享状态 // 优先级WBC控制器初始化参数自由度/接触点数/优化维度/权重/仿真步长 WBC_priority WBC_solv(kinDynSolver.model_nv, 18, 22, 0.7, mj_model-opt.timestep); MPC MPC_solv(dt_200Hz); // MPC控制器盲踩障碍物时轨迹预测控制周期5ms // 步态调度器盲踩专用参数步长0.25m/仿真步长适配障碍物高度 GaitScheduler gaitScheduler(0.25, mj_model-opt.timestep); // PVT关节控制器参数仿真步长/关节配置文件配置PD参数适配障碍物冲击 PVT_Ctr pvtCtr(mj_model-opt.timestep, ../common/joint_ctrl_config.json); FootPlacement footPlacement; // 足端放置规划器盲踩时动态调整落足高度/位置 // 期望速度解析器生成盲踩时的前进/转向速度指令 JoyStickInterpreter jsInterp(mj_model-opt.timestep); DataLogger logger(../record/datalog_obstacle.log); // 数据记录器保存盲踩仿真数据 // 可视化窗口初始化 uiController.iniGLFW(); // 初始化GLFW图形库 uiController.enableTracking(); // 启用机器人躯干视角跟踪跟随机器人运动 // 创建仿真窗口参数窗口名称/是否保存视频注保存视频15秒约占2.5GB空间 uiController.createWindow(双足机器人盲踩障碍物仿真, false); // 机器人物理参数初始化 double stand_legLength 1.01; // 机器人站立时腿长基链接到地面高度 double foot_height 0.07; // 脚踝到地面的基础高度适配障碍物高度 double xv_des 0.8; // 盲踩时前进期望速度降低速度提升稳定性 int model_nv kinDynSolver.model_nv; // 机器人总自由度含浮动基 // 盲踩障碍物关键参数配置 RobotState.width_hips 0.229; // 髋部宽度步态规划基础参数 footPlacement.kp_vx 0.03; // 足端x方向速度跟踪比例系数盲踩时速度修正 footPlacement.kp_vy 0.035; // 足端y方向速度跟踪比例系数横向避障修正 footPlacement.kp_wz 0.03; // 偏航角速度跟踪比例系数转向避障修正 footPlacement.stepHeight 0.18;// 盲踩时抬脚高度高于常规0.12m适配障碍物 footPlacement.legLength stand_legLength; // 腿长参数传递给足端规划器 // 初始化MuJoCo初始关节位姿从模型预设关键位姿读取 mju_copy(mj_data-qpos, mj_model-key_qpos, mj_model-nq * 1); // 关节/足端期望状态初始化 // 关节指令向量去除6个浮动基自由度仅保留驱动关节 std::vectordouble motors_pos_des(model_nv - 6, 0); // 关节期望位置 std::vectordouble motors_pos_cur(model_nv - 6, 0); // 关节当前位置 std::vectordouble motors_vel_des(model_nv - 6, 0); // 关节期望速度 std::vectordouble motors_vel_cur(model_nv - 6, 0); // 关节当前速度 std::vectordouble motors_tau_des(model_nv - 6, 0); // 关节期望力矩 // 足端初始期望位姿机器人基坐标系下 Eigen::Vector3d fe_l_pos_L_des {-0.018, 0.113, -stand_legLength}; // 左脚期望位置 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)); // 手部初始期望位姿基坐标系下 Eigen::Vector3d hd_l_pos_L_des {-0.02, 0.32, -0.159}; // 左手期望位置 Eigen::Vector3d hd_r_pos_L_des {-0.02, -0.32, -0.159}; // 右手期望位置 Eigen::Vector3d hd_l_eul_L_des {-1.253, 0.122, -1.732}; // 左手期望欧拉角 Eigen::Vector3d hd_r_eul_L_des {1.253, 0.122, 1.732}; // 右手期望欧拉角 Eigen::Matrix3d hd_l_rot_des eul2Rot(hd_l_eul_L_des(0), hd_l_eul_L_des(1), hd_l_eul_L_des(2)); Eigen::Matrix3d hd_r_rot_des eul2Rot(hd_r_eul_L_des(0), hd_r_eul_L_des(1), hd_r_eul_L_des(2)); // 逆运动学求解初始位姿 // 求解腿部逆运动学根据足端期望位姿 auto resLeg kinDynSolver.computeInK_Leg(fe_l_rot_des, fe_l_pos_L_des, fe_r_rot_des, fe_r_pos_L_des); // 求解手部逆运动学根据手部期望位姿 auto resHand kinDynSolver.computeInK_Hand(hd_l_rot_des, hd_l_pos_L_des, hd_r_rot_des, hd_r_pos_L_des); // 初始化期望关节位置填充腿手关节逆解结果 Eigen::VectorXd qIniDes Eigen::VectorXd::Zero(mj_model-nq, 1); qIniDes.block(7, 0, mj_model-nq - 7, 1) resLeg.jointPosRes resHand.jointPosRes; WBC_solv.setQini(qIniDes, RobotState.q); // 给WBC控制器设置初始关节位姿 // 数据记录器配置盲踩仿真数据 logger.addIterm(simTime, 1); // 仿真时间秒 logger.addIterm(motor_pos_des, model_nv - 6); // 关节期望位置 logger.addIterm(motor_pos_cur, model_nv - 6); // 关节当前位置 logger.addIterm(motor_vel_des, model_nv - 6); // 关节期望速度 logger.addIterm(motor_vel_cur, model_nv - 6); // 关节当前速度 logger.addIterm(motor_tor_des, model_nv - 6); // 关节期望力矩 logger.addIterm(rpyVal, 3); // 躯干欧拉角滚转/俯仰/偏航 logger.addIterm(base_omega_W, 3); // 躯干角速度世界坐标系 logger.addIterm(gpsVal, 3); // 躯干位置世界坐标系 logger.addIterm(base_vel, 3); // 躯干线速度 logger.addIterm(foot_L_pos, 3); // 左脚端实际位置盲踩关键 logger.addIterm(foot_R_pos, 3); // 右脚端实际位置盲踩关键 logger.addIterm(obstacle_contact_force, 3); // 足端与障碍物接触力盲踩反馈 logger.finishItermAdding(); // 完成日志项注册 // 仿真时序参数盲踩逻辑 int MPC_count 0; // MPC控制器调用计数器200Hz周期 double startSteppingTime 3; // 3秒后开始迈步初始稳定 double startWalkingTime 5; // 5秒后开始盲踩障碍物行走 double simEndTime 40; // 仿真总时长40秒 mjtNum simstart mj_data-time; // 每帧渲染起始时间控制60Hz帧率 double simTime mj_data-time; // 当前仿真时间 // 主仿真循环核心控制逻辑 while (!glfwWindowShouldClose(uiController.window)) { simstart mj_data-time; // 内层循环保证60Hz可视化帧率每帧运行1/60秒仿真 while (mj_data-time - simstart 1.0 / 60.0 uiController.runSim) { mj_step(mj_model, mj_data); // 推进MuJoCo仿真一步 simTime mj_data-time; // 更新当前仿真时间 // ---------------------- 1. 传感器数据更新 ---------------------- mj_interface.updateSensorValues(); // 读取MuJoCo传感器关节位姿/力/接触传感器 mj_interface.dataBusWrite(RobotState); // 传感器数据写入全局数据总线 // ---------------------- 2. 运动学/动力学计算 ---------------------- kinDynSolver.dataBusRead(RobotState); // 从总线读取机器人状态 kinDynSolver.computeJ_dJ(); // 计算雅可比矩阵及导数足端速度映射 kinDynSolver.computeDyn(); // 计算动力学参数惯性矩阵/科氏力/重力 kinDynSolver.dataBusWrite(RobotState); // 动力学结果写回总线 // ---------------------- 3. 盲踩速度指令生成 ---------------------- if (simTime startWalkingTime) { // 盲踩阶段设置前进速度0.8m/s关闭视觉仅靠本体感知 jsInterp.setWzDesLPara(0, 1); // 偏航角速度期望0直行 jsInterp.setVxDesLPara(xv_des, 2.0); // 前进速度0.8m/s滤波系数2.0平滑 RobotState.motionState DataBus::Walk; // 运动状态标记为行走盲踩 } else { // 未到行走时间保持初始位姿躯干位置/偏航角 jsInterp.setIniPos(RobotState.q(0), RobotState.q(1), RobotState.base_rpy(2)); } jsInterp.step(); // 更新期望速度滤波平滑 // 手动设置躯干z方向期望高度适配障碍物高度 RobotState.js_pos_des(2) stand_legLength foot_height 0.05; jsInterp.dataBusWrite(RobotState); // 期望速度写入总线 // ---------------------- 4. 盲踩步态调度 ---------------------- if (simTime startSteppingTime) { // 步态调度器盲踩时动态调整支撑腿/摆动腿时序 gaitScheduler.dataBusRead(RobotState); gaitScheduler.step(); // 更新步态阶段支撑/摆动/离地/落地 gaitScheduler.dataBusWrite(RobotState); // 足端放置规划盲踩核心逻辑无视觉根据步态调整落足高度/位置 footPlacement.dataBusRead(RobotState); footPlacement.getSwingPos(); // 计算摆动腿落足点抬高避开障碍物 footPlacement.dataBusWrite(RobotState); } // ---------------------- 5. MPC模型预测控制盲踩轨迹优化 ---------------------- MPC_count; if (MPC_count % (int)(dt_200Hz / mj_model-opt.timestep) 0) { // 每5ms运行一次MPC200Hz预测未来轨迹适配障碍物 MPC_solv.dataBusRead(RobotState); MPC_solv.compute(); // 求解MPC优化问题调整期望轨迹 MPC_solv.dataBusWrite(RobotState); MPC_count 0; // 重置计数器 } // ---------------------- 6. WBC优先级控制力/运动混合 ---------------------- WBC_solv.dataBusRead(RobotState); // 读取MPC期望轨迹/足端力 WBC_solv.computeDdq(kinDynSolver); // 求解期望关节加速度 WBC_solv.computeTau(); // 求解期望关节力矩适配障碍物冲击 WBC_solv.dataBusWrite(RobotState); // WBC结果写回总线 // ---------------------- 7. PVT关节控制器闭环执行 ---------------------- if (simTime 3) { // 前3秒初始化关节位置低增益避免冲击 pvtCtr.calMotorsPVT(100.0/1000.0/180.0*3.1415); } else { // 盲踩阶段设置关节PD参数提高刚度应对障碍物冲击 // 左腿PD参数髋/膝/踝 pvtCtr.setJointPD(450, 20, J_hip_l_roll); // 左髋滚转KP450, KD20 pvtCtr.setJointPD(250, 15, J_hip_l_yaw); // 左髋偏航 pvtCtr.setJointPD(350, 15, J_hip_l_pitch); // 左髋俯仰 pvtCtr.setJointPD(350, 18, J_knee_l_pitch); // 左膝俯仰 pvtCtr.setJointPD(350, 22, J_ankle_l_pitch); // 左踝俯仰 pvtCtr.setJointPD(350, 20, J_ankle_l_roll); // 左踝滚转 // 右腿PD参数对称配置 pvtCtr.setJointPD(450, 20, J_hip_r_roll); pvtCtr.setJointPD(250, 15, J_hip_r_yaw); pvtCtr.setJointPD(350, 15, J_hip_r_pitch); pvtCtr.setJointPD(350, 18, J_knee_r_pitch); pvtCtr.setJointPD(350, 22, J_ankle_r_pitch); pvtCtr.setJointPD(350, 20, J_ankle_r_roll); pvtCtr.calMotorsPVT(); // 计算关节力矩指令PD闭环 } pvtCtr.dataBusRead(RobotState); // 读取期望关节指令 pvtCtr.dataBusWrite(RobotState); // 写入最终力矩指令 // 将关节力矩指令下发到MuJoCo执行器 mj_interface.setMotorsTorque(RobotState.motors_tor_out); // ---------------------- 8. 盲踩仿真数据记录 ---------------------- logger.startNewLine(); // 新建日志行 logger.recItermData(simTime, simTime); // 仿真时间 logger.recItermData(motor_pos_des, RobotState.motors_pos_des); // 关节期望位置 logger.recItermData(motor_pos_cur, RobotState.motors_pos_cur); // 关节当前位置 logger.recItermData(foot_L_pos, RobotState.foot_L_pos); // 左脚位置盲踩关键 logger.recItermData(foot_R_pos, RobotState.foot_R_pos); // 右脚位置盲踩关键 logger.recItermData(obstacle_contact_force, RobotState.contact_force); // 障碍物接触力 logger.finishLine(); // 完成日志行写入 // ---------------------- 9. 调试信息输出 ---------------------- printf(盲踩仿真时间: %.2fs | 躯干高度: %.3fm | 左足接触力: %.1fN\n, simTime, RobotState.basePos[2], RobotState.contact_force[2]); } // 仿真时长达标后退出 if (mj_data-time simEndTime) { break; } uiController.updateScene(); // 更新GLFW窗口渲染显示机器人/障碍物/力反馈 } // 资源释放 mj_deleteData(mj_data); // 释放MuJoCo仿真数据 mj_deleteModel(mj_model); // 释放MuJoCo模型 glfwTerminate(); // 终止GLFW图形库 logger.close(); // 关闭数据日志文件 return 0; }机器人盲踩障碍物测试的效果如图11-17所示。图11-17 机器人盲踩障碍物测试