WBC 在 MIT Cheetah 的实现分析1. WBC 概述1.1 核心组件1.2 运行频率2. WBC 架构3. Task 系统3.1 任务基类3.2 任务类型3.3 机体位置任务4. Contact 系统4.1 接触约束基类4.2 单点接触实现4.3 摩擦锥可视化5. WBIC 算法5.1 WBIC QP 形式化5.2 WBIC 实现6. KinWBC - 运动学全身控制7. 任务优先级8. 使用的开源库8.1 核心库8.2 Goldfarb Optimizer8.3 CMake 依赖9. MPC 与 WBC 集成9.1 数据流9.2 任务分配10. 增益配置11. 文件结构12. 构建说明12.1 依赖安装12.2 编译12.3 运行13. 性能特性14. 参考本文档详细分析MIT Cheetah四足机器人中全身控制 (Whole Body Control, WBC)的实现方式及使用的开源库。1. WBC 概述WBC (Whole Body Control)是底层控制器负责根据 MPC 的输出计算关节力矩。它使用优先级任务分解和whole-body impulse control (WBIC)来求解优化问题。1.1 核心组件组件用途WBICWhole Body Impulse Control - 力矩优化求解器KinWBC运动学 WBC - 计算期望关节位置/速度Task 系统任务定义 (机体位置、机体姿态、足部位置)Contact 系统接触约束 (摩擦锥、力限制)1.2 运行频率WBC: 1 kHz (全控制循环速率)MPC: ~33 Hz (每 27-33 次控制迭代)2. WBC 架构┌─────────────────────────────────────────────────────────────────────────────────┐ │ WBC_Ctrl 架构 │ ├─────────────────────────────────────────────────────────────────────────────────┤ │ │ │ ┌───────────────────────────────────────────────────────────────────────────┐ │ │ │ 输入 │ │ │ │ │ │ │ │ ┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ │ │ │ │ │ MPC 输出 │ │ 状态估计 │ │ 腿部数据 │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ pBody_des │ │ position │ │ q[12] │ │ │ │ │ │ vBody_des │ │ orientation │ │ qd[12] │ │ │ │ │ │ pFoot_des[4] │ │ velocity │ │ p[12] │ │ │ │ │ │ Fr_des[4] │ │ omega │ │ v[12] │ │ │ │ │ │ contact_state │ │ │ │ J[36] │ │ │ │ │ └─────────────────┘ └─────────────────┘ └─────────────────┘ │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ ▼ │ │ ┌───────────────────────────────────────────────────────────────────────────┐ │ │ │ WBC_Ctrl::run() │ │ │ │ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 1. 更新模型 │ │ │ │ │ │ │ │ │ │ │ │ _UpdateModel(state_est, leg_data) │ │ │ │ │ │ { │ │ │ │ │ │ _state.bodyOrientation state_est.orientation; │ │ │ │ │ │ _state.bodyPosition state_est.position; │ │ │ │ │ │ _state.bodyVelocity [omega, v]; │ │ │ │ │ │ _state.q joint_positions; │ │ │ │ │ │ _state.qd joint_velocities; │ │ │ │ │ │ │ │ │ │ │ │ _model.setState(_state); │ │ │ │ │ │ _model.contactJacobians(); │ │ │ │ │ │ _model.massMatrix(); → _A │ │ │ │ │ │ _model.generalizedGravityForce(); → _grav │ │ │ │ │ │ _model.generalizedCoriolisForce(); → _coriolis │ │ │ │ │ │ _Ainv _A.inverse(); │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ │ │ ▼ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 2. 接触和任务更新 │ │ │ │ │ │ │ │ │ │ │ │ _ContactTaskUpdate(input, data) │ │ │ │ │ │ { │ │ │ │ │ │ // 机体姿态任务 │ │ │ │ │ │ _body_ori_task-UpdateTask(quat_des, vBody_Ori_des, zero); │ │ │ │ │ │ _task_list.push_back(_body_ori_task); │ │ │ │ │ │ │ │ │ │ │ │ // 机体位置任务 │ │ │ │ │ │ _body_pos_task-UpdateTask(pBody_des, vBody_des, aBody_des); │ │ │ │ │ │ _task_list.push_back(_body_pos_task); │ │ │ │ │ │ │ │ │ │ │ │ for (leg 0; leg 4; leg) { │ │ │ │ │ │ if (contact_state[leg] 0) { // 支撑腿 │ │ │ │ │ │ _foot_contact[leg]-setRFDesired(Fr_des[leg]); │ │ │ │ │ │ _contact_list.push_back(_foot_contact[leg]); │ │ │ │ │ │ } else { // 摆动腿 │ │ │ │ │ │ _foot_task[leg]-UpdateTask(pFoot_des, ...); │ │ │ │ │ │ _task_list.push_back(_foot_task[leg]); │ │ │ │ │ │ } │ │ │ │ │ │ } │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ │ │ ▼ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 3. 计算 WBC │ │ │ │ │ │ │ │ │ │ │ │ _ComputeWBC() │ │ │ │ │ │ { │ │ │ │ │ │ // KinWBC - 找到期望配置 │ │ │ │ │ │ _kin_wbc-FindConfiguration(_full_config, _task_list, │ │ │ │ │ │ _contact_list, │ │ │ │ │ │ _des_jpos, _des_jvel); │ │ │ │ │ │ │ │ │ │ │ │ // WBIC - 计算力矩 │ │ │ │ │ │ _wbic-UpdateSetting(_A, _Ainv, _coriolis, _grav); │ │ │ │ │ │ _wbic-MakeTorque(_tau_ff, _wbic_data); │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ │ │ ▼ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 4. 更新腿部命令 │ │ │ │ │ │ │ │ │ │ │ │ _UpdateLegCMD(data) │ │ │ │ │ │ { │ │ │ │ │ │ for (leg 0; leg 4; leg) { │ │ │ │ │ │ cmd[leg].tauFeedForward[jidx] _tau_ff[leg*3 jidx]; │ │ │ │ │ │ cmd[leg].qDes[jidx] _des_jpos[leg*3 jidx]; │ │ │ │ │ │ cmd[leg].qdDes[jidx] _des_jvel[leg*3 jidx]; │ │ │ │ │ │ cmd[leg].kpJoint(jidx,jidx) _Kp_joint[jidx]; │ │ │ │ │ │ cmd[leg].kdJoint(jidx,jidx) _Kd_joint[jidx]; │ │ │ │ │ │ } │ │ │ │ │ │ │ │ │ │ │ │ // 膝关节防翻转限制 │ │ │ │ │ │ if (qDes[2] 0.3) qDes[2] 0.3; │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ ▼ │ │ ┌───────────────────────────────────────────────────────────────────────────┐ │ │ │ 输出 │ │ │ │ │ │ │ │ 每条腿 (FR, FL, HR, HL): │ │ │ │ ┌───────────────────────────────────────────────────────────────────┐ │ │ │ │ │ tauFeedForward[3] - 关节力矩 (N·m) │ │ │ │ │ │ qDes[3] - 期望关节位置 (rad) │ │ │ │ │ │ qdDes[3] - 期望关节速度 (rad/s) │ │ │ │ │ │ kpJoint[3x3] - 位置增益 (N·m/rad) │ │ │ │ │ │ kdJoint[3x3] - 速度增益 (N·m·s/rad) │ │ │ │ │ └───────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────────┘ │ │ │ └─────────────────────────────────────────────────────────────────────────────────┘3. Task 系统3.1 任务基类templatetypenameTclassTask{public:Task(size_t dim):dim_task_(dim),op_cmd_(dim),pos_err_(dim),vel_des_(dim),acc_des_(dim){}// 主接口boolUpdateTask(constvoid*pos_des,constDVecTvel_des,constDVecTacc_des){_UpdateTaskJacobian();// 计算 Jt_UpdateTaskJDotQdot();// 计算 Jt·q̇_UpdateCommand(pos_des,vel_des,acc_des);// 计算命令_AdditionalUpdate();returntrue;}// 获取器voidgetCommand(DVecTop_cmd){op_cmdop_cmd_;}voidgetTaskJacobian(DMatTJt){JtJt_;}voidgetTaskJacobianDotQdot(DVecTJtDotQdot){JtDotQdotJtDotQdot_;}protected:// 虚函数 (必须实现)virtualbool_UpdateCommand(...)0;virtualbool_UpdateTaskJacobian()0;virtualbool_UpdateTaskJDotQdot()0;// 任务数据size_t dim_task_;DVecTop_cmd_;// 操作命令 (加速度)DMatTJt_;// 任务 JacobianDVecTJtDotQdot_;// Jt·q̇DVecTpos_err_;// 位置误差DVecTvel_des_;// 期望速度DVecTacc_des_;// 期望加速度// 增益DVecT_Kp;// 位置增益DVecT_Kd;// 速度增益};3.2 任务类型任务类型描述优先级BodyPosTask机体位置控制3BodyOriTask机体姿态控制2LinkPosTask(FootTask)足部位置控制4 (最低)3.3 机体位置任务templatetypenameTclassBodyPosTask:publicTaskT{public:BodyPosTask(constFloatingBaseModelT*robot):TaskT(3),_robot_sys(robot){// Jacobian: 从广义速度到机体速度的映射Jt_.block(0,3,3,3).setIdentity();// 机体速度部分// 增益_Kp_kinDVecT::Constant(3,1.0);_KpDVecT::Constant(3,50.0);_KdDVecT::Constant(3,1.0);}protected:bool_UpdateCommand(constvoid*pos_des,constDVecTvel_des,constDVecTacc_des)override{Vec3T*pos_cmd(Vec3T*)pos_des;Vec3Tlink_pos_robot_sys-_state.bodyPosition;// 位置误差pos_err__Kp_kin*(pos_cmd-link_pos);// 世界坐标系中的速度Vec3Tcurr_velRot.transpose()*_robot_sys-_state.bodyVelocity.tail(3);// PD 控制 前馈op_cmd__Kp*(pos_cmd-link_pos)_Kd*(vel_des-curr_vel)acc_des;returntrue;}};4. Contact 系统4.1 接触约束基类templatetypenameTclassContactSpec{public:ContactSpec(size_t dim):dim_contact_(dim){}virtualbool_UpdateJc()0;// 接触 Jacobianvirtualbool_UpdateJcDotQdot()0;// Jc·q̇virtualbool_UpdateUf()0;// 力约束矩阵virtualbool_UpdateInequalityVector()0;voidsetRFDesired(constDVecTFr_des){Fr_des_Fr_des;}DMatTJc_;// 接触 Jacobian (dim x num_qdot)DVecTJcDotQdot_;// Jc·q̇DMatTUf_;// 力约束矩阵 (摩擦锥)DVecTieq_vec_;// 不等式向量DVecTFr_des_;// 期望反作用力};4.2 单点接触实现templatetypenameTclassSingleContact:publicContactSpecT{public:SingleContact(constFloatingBaseModelT*robot,intcontact_pt):ContactSpecT(3),_contact_pt(contact_pt),_dim_U(6){// 摩擦锥约束 (μ 0.4)// 标准摩擦金字塔:// fz 0// |fx| μ*fz// |fy| μ*fz// fz max_fzUf_(0,2)1.;// fz 0Uf_(1,0)1.;// fx μ*fz 0Uf_(1,2)mu;Uf_(2,0)-1.;// -fx μ*fz 0Uf_(2,2)mu;Uf_(3,1)1.;// fy μ*fz 0Uf_(3,2)mu;Uf_(4,1)-1.;// -fy μ*fz 0Uf_(4,2)mu;Uf_(5,2)-1.;// -fz -max_fz}};4.3 摩擦锥可视化力空间 (单接触) Fz (法向) │ │ ┌─────────┐ │ │╲ ╱│ │ │ ╲ ╱ │ │ │ ╲ ╱ │ │ │ ╲ ╱ │ │ │ V │ ← 力必须在摩擦金字塔内 │ │ │ │ └─────────┘ └────────────────► Fy ╱ ╱ Fx 约束条件: ┌────────────────────────────────────────┐ │ Fz 0 (单边) │ │ |Fx| μ * Fz (摩擦 X) │ │ |Fy| μ * Fz (摩擦 Y) │ │ Fz Fz_max (力限制) │ └────────────────────────────────────────┘5. WBIC 算法5.1 WBIC QP 形式化最小化: J ||Δq̈_floating||² ||ΔF||² 约束条件: A_full * q̈ b Jc^T * F (浮基动力学) Uf * F 0 (摩擦锥) Uf * (Fr_des ΔF) 0 (力约束) 其中: Δq̈_floating 浮基加速度修正量 ΔF 反作用力修正量 Fr_des 来自 MPC 的期望力 Jc 接触 Jacobian A 质量矩阵 b 科里奥利力 重力5.2 WBIC 实现templatetypenameTvoidWBICT::MakeTorque(DVecTcmd,void*extra_input){// 1. 接触构建_ContactBuilding();// 组装 Jc, JcDotQdot, Uf, Fr_des// 2. 从接触约束计算初始 qddotDMatTJcBar;_WeightedInverse(_Jc,Ainv_,JcBar);qddot_preJcBar*(-_JcDotQdot);NpreI-JcBar*_Jc;// 零空间投影// 3. 层级任务分解for(task:_task_list){task-getTaskJacobian(Jt);task-getTaskJacobianDotQdot(JtDotQdot);task-getCommand(xddot);// 投影到零空间JtPreJt*Npre;_WeightedInverse(JtPre,Ainv_,JtBar);// 更新 qddotqddot_preJtBar*(xddot-JtDotQdot-Jt*qddot_pre);// 更新零空间NpreNpre*(I-JtBar*JtPre);}// 4. 设置 QP_SetEqualityConstraint(qddot_pre);// 动力学约束_SetInEqualityConstraint();// 摩擦锥_SetCost();// 最小化修正量// 5. 求解 QPsolve_quadprog(G,g0,CE,ce0,CI,ci0,z);// 6. 获取解for(i0;i_dim_floating;i)qddot_pre[i]z[i];// 添加修正量_GetSolution(qddot_pre,cmd);// 提取力矩}6. KinWBC - 运动学全身控制KinWBC 用于计算期望的关节位置和速度不涉及力矩优化。templatetypenameTboolKinWBCT::FindConfiguration(constDVecTcurr_config,conststd::vectorTaskT*task_list,conststd::vectorContactSpecT*contact_list,DVecTjpos_cmd,DVecTjvel_cmd){// 1. 构建接触零空间DMatTNcI;if(contact_list.size()0){DMatTJc;for(contact:contact_list){contact-getContactJacobian(Jc_i);Jc.append(Jc_i);}// 零空间: Nc I - Jc^ * Jc_BuildProjectionMatrix(Jc,Nc);}// 2. 按优先级处理任务DMatTN_preNc;DVecTdelta_qDVecT::Zero(num_qdot_);DVecTqdotDVecT::Zero(num_qdot_);for(task:task_list){task-getTaskJacobian(Jt);// 投影到零空间JtPreJt*N_pre;_PseudoInverse(JtPre,JtPre_pinv);// 计算配置变化delta_qJtPre_pinv*(task-getPosError()-Jt*delta_q);qdotJtPre_pinv*(task-getDesVel()-Jt*qdot);// 更新零空间_BuildProjectionMatrix(JtPre,N_nx);N_pre*N_nx;}// 3. 提取关节命令for(i0;inum_act_joint_;i){jpos_cmd[i]curr_config[i6]delta_q[i6];jvel_cmd[i]qdot[i6];}returntrue;}7. 任务优先级优先级 1 (最高): 接触约束 - 支撑腿必须保持在地面上 - 通过零空间投影实现 优先级 2: 机体姿态任务 - 保持机体直立 - 跟踪期望的 roll, pitch, yaw 优先级 3: 机体位置任务 - 跟踪期望轨迹 - 跟随速度命令 优先级 4 (最低): 摆动足部任务 - 跟踪摆动轨迹 - 实现落脚点放置8. 使用的开源库8.1 核心库库用途位置Eigen线性代数、矩阵运算、SVD系统库/usr/include/eigen3/Goldfarb Optimizer (QuadProg)QP 求解器third-party/Goldfarb_Optimizer/8.2 Goldfarb OptimizerGoldfarb Optimizer 是用于求解二次规划的 C 库特别适用于 WBC 中的力矩优化。特点基于有效集算法支持等式和不等式约束轻量级实现代码调用#includeGoldfarb_Optimizer/QuadProg.hhsolve_quadprog(G,g0,CE,ce0,CI,ci0,z);其中G- 二次目标矩阵 (Hessian)g0- 线性目标向量CE- 等式约束矩阵ce0- 等式约束向量CI- 不等式约束矩阵ci0- 不等式约束向量z- 解向量8.3 CMake 依赖# user/MIT_Controller/Controllers/WBC/WBIC/CMakeLists.txt add_library(WBIC SHARED ${sources} ${headers}) target_link_libraries(WBIC Goldfarb_Optimizer)9. MPC 与 WBC 集成9.1 数据流┌─────────────────────────────────────────────────────────────────────────────┐ │ MPC → WBC 数据流 │ ├─────────────────────────────────────────────────────────────────────────────┤ │ │ │ LocomotionCtrlData (接口结构): │ │ ┌───────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ │ │ // 机体命令 (来自 MPC) │ │ │ │ pBody_des[3] cMPCOld-pBody_des │ │ │ │ vBody_des[3] cMPCOld-vBody_des │ │ │ │ aBody_des[3] cMPCOld-aBody_des │ │ │ │ pBody_RPY_des[3] cMPCOld-pBody_RPY_des │ │ │ │ vBody_Ori_des[3] cMPCOld-vBody_Ori_des │ │ │ │ │ │ │ │ // 足部命令 (来自 MPC) │ │ │ │ pFoot_des[4][3] cMPCOld-pFoot_des │ │ │ │ vFoot_des[4][3] cMPCOld-vFoot_des │ │ │ │ aFoot_des[4][3] cMPCOld-aFoot_des │ │ │ │ │ │ │ │ // 力 (来自 MPC) │ │ │ │ Fr_des[4][3] cMPCOld-Fr_des │ │ │ │ │ │ │ │ // 接触状态 (来自 MPC) │ │ │ │ contact_state[4] cMPCOld-contact_state │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────┘ │ │ │ └──────────────────────────────────────────────────────────────────────────────┘9.2 任务分配for(leg0;leg4;leg){if(contact_state[leg]0){// 支撑腿// 接触约束_foot_contact[leg]-setRFDesired(Fr_des[leg]);_contact_list.push_back(_foot_contact[leg]);}else{// 摆动腿// 任务_foot_task[leg]-UpdateTask(pFoot_des[leg],vFoot_des[leg],aFoot_des[leg]);_task_list.push_back(_foot_task[leg]);}}10. 增益配置// WBC 增益Kp_body[50,50,50]// 机体位置 P 增益Kd_body[1,1,1]// 机体位置 D 增益Kp_ori[50,50,50]// 机体姿态 P 增益Kd_ori[1,1,1]// 机体姿态 D 增益Kp_foot[70,70,70]// 足部位置 P 增益Kd_foot[3,3,3]// 足部位置 D 增益Kp_joint[5,5,5]// 关节 P 增益Kd_joint[1.5,1.5,1.5]// 关节 D 增益11. 文件结构user/MIT_Controller/Controllers/WBC/ ├── WBC_Ctrl/ │ ├── WBC_Ctrl.hpp │ ├── WBC_Ctrl.cpp │ └── CMakeLists.txt │ ├── WBC/ │ ├── Task.hpp ← 任务基类 │ ├── ContactSpec.hpp ← 接触约束基类 │ ├── WBC.hpp │ ├── WBC.cpp │ └── CMakeLists.txt │ ├── WBIC/ ← Whole Body Impulse Control │ ├── WBIC.hpp │ ├── WBIC.cpp │ ├── KinWBC.hpp ← 运动学 WBC │ ├── KinWBC.cpp │ └── CMakeLists.txt ← 链接 Goldfarb_Optimizer │ └── TaskSet/ ← 任务实现 ├── BodyPosTask.hpp ├── BodyOriTask.hpp └── LinkPosTask.hpp third-party/ ├── Goldfarb_Optimizer/ ← QP 求解器 │ ├── QuadProg.hh │ ├── QuadProg.cc │ └── CMakeLists.txt │ └── ...12. 构建说明12.1 依赖安装# Ubuntu/Debiansudoapt-getinstallcmake build-essential libeigen3-dev libblas-dev liblapack-dev12.2 编译# 创建构建目录mkdirbuildcdbuild cmake..make-j412.3 运行# 运行仿真器./sim/sim# 运行 WBC 控制器 (另一个终端)./user/MIT_Controller/mit_ctrl3s13. 性能特性指标值执行时间0.1-0.3 ms频率1 kHz内存使用较低 (任务 Jacobian)数值稳定性优秀 (零空间投影)实时性能即时响应14. 参考Goldfarb Optimizer:third-party/Goldfarb_Optimizer/QuadProg.hhEigen: http://eigen.tuxfamily.org原始论文: “Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control”WBC 论文: “Whole-Body Control of Torque-Controlled Humanoid Robots” WBC 在 MIT Cheetah 的实现分析
WBC 在 MIT Cheetah 的实现分析1. WBC 概述1.1 核心组件1.2 运行频率2. WBC 架构3. Task 系统3.1 任务基类3.2 任务类型3.3 机体位置任务4. Contact 系统4.1 接触约束基类4.2 单点接触实现4.3 摩擦锥可视化5. WBIC 算法5.1 WBIC QP 形式化5.2 WBIC 实现6. KinWBC - 运动学全身控制7. 任务优先级8. 使用的开源库8.1 核心库8.2 Goldfarb Optimizer8.3 CMake 依赖9. MPC 与 WBC 集成9.1 数据流9.2 任务分配10. 增益配置11. 文件结构12. 构建说明12.1 依赖安装12.2 编译12.3 运行13. 性能特性14. 参考本文档详细分析MIT Cheetah四足机器人中全身控制 (Whole Body Control, WBC)的实现方式及使用的开源库。1. WBC 概述WBC (Whole Body Control)是底层控制器负责根据 MPC 的输出计算关节力矩。它使用优先级任务分解和whole-body impulse control (WBIC)来求解优化问题。1.1 核心组件组件用途WBICWhole Body Impulse Control - 力矩优化求解器KinWBC运动学 WBC - 计算期望关节位置/速度Task 系统任务定义 (机体位置、机体姿态、足部位置)Contact 系统接触约束 (摩擦锥、力限制)1.2 运行频率WBC: 1 kHz (全控制循环速率)MPC: ~33 Hz (每 27-33 次控制迭代)2. WBC 架构┌─────────────────────────────────────────────────────────────────────────────────┐ │ WBC_Ctrl 架构 │ ├─────────────────────────────────────────────────────────────────────────────────┤ │ │ │ ┌───────────────────────────────────────────────────────────────────────────┐ │ │ │ 输入 │ │ │ │ │ │ │ │ ┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ │ │ │ │ │ MPC 输出 │ │ 状态估计 │ │ 腿部数据 │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ pBody_des │ │ position │ │ q[12] │ │ │ │ │ │ vBody_des │ │ orientation │ │ qd[12] │ │ │ │ │ │ pFoot_des[4] │ │ velocity │ │ p[12] │ │ │ │ │ │ Fr_des[4] │ │ omega │ │ v[12] │ │ │ │ │ │ contact_state │ │ │ │ J[36] │ │ │ │ │ └─────────────────┘ └─────────────────┘ └─────────────────┘ │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ ▼ │ │ ┌───────────────────────────────────────────────────────────────────────────┐ │ │ │ WBC_Ctrl::run() │ │ │ │ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 1. 更新模型 │ │ │ │ │ │ │ │ │ │ │ │ _UpdateModel(state_est, leg_data) │ │ │ │ │ │ { │ │ │ │ │ │ _state.bodyOrientation state_est.orientation; │ │ │ │ │ │ _state.bodyPosition state_est.position; │ │ │ │ │ │ _state.bodyVelocity [omega, v]; │ │ │ │ │ │ _state.q joint_positions; │ │ │ │ │ │ _state.qd joint_velocities; │ │ │ │ │ │ │ │ │ │ │ │ _model.setState(_state); │ │ │ │ │ │ _model.contactJacobians(); │ │ │ │ │ │ _model.massMatrix(); → _A │ │ │ │ │ │ _model.generalizedGravityForce(); → _grav │ │ │ │ │ │ _model.generalizedCoriolisForce(); → _coriolis │ │ │ │ │ │ _Ainv _A.inverse(); │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ │ │ ▼ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 2. 接触和任务更新 │ │ │ │ │ │ │ │ │ │ │ │ _ContactTaskUpdate(input, data) │ │ │ │ │ │ { │ │ │ │ │ │ // 机体姿态任务 │ │ │ │ │ │ _body_ori_task-UpdateTask(quat_des, vBody_Ori_des, zero); │ │ │ │ │ │ _task_list.push_back(_body_ori_task); │ │ │ │ │ │ │ │ │ │ │ │ // 机体位置任务 │ │ │ │ │ │ _body_pos_task-UpdateTask(pBody_des, vBody_des, aBody_des); │ │ │ │ │ │ _task_list.push_back(_body_pos_task); │ │ │ │ │ │ │ │ │ │ │ │ for (leg 0; leg 4; leg) { │ │ │ │ │ │ if (contact_state[leg] 0) { // 支撑腿 │ │ │ │ │ │ _foot_contact[leg]-setRFDesired(Fr_des[leg]); │ │ │ │ │ │ _contact_list.push_back(_foot_contact[leg]); │ │ │ │ │ │ } else { // 摆动腿 │ │ │ │ │ │ _foot_task[leg]-UpdateTask(pFoot_des, ...); │ │ │ │ │ │ _task_list.push_back(_foot_task[leg]); │ │ │ │ │ │ } │ │ │ │ │ │ } │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ │ │ ▼ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 3. 计算 WBC │ │ │ │ │ │ │ │ │ │ │ │ _ComputeWBC() │ │ │ │ │ │ { │ │ │ │ │ │ // KinWBC - 找到期望配置 │ │ │ │ │ │ _kin_wbc-FindConfiguration(_full_config, _task_list, │ │ │ │ │ │ _contact_list, │ │ │ │ │ │ _des_jpos, _des_jvel); │ │ │ │ │ │ │ │ │ │ │ │ // WBIC - 计算力矩 │ │ │ │ │ │ _wbic-UpdateSetting(_A, _Ainv, _coriolis, _grav); │ │ │ │ │ │ _wbic-MakeTorque(_tau_ff, _wbic_data); │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ │ │ ▼ │ │ │ │ ┌─────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ 4. 更新腿部命令 │ │ │ │ │ │ │ │ │ │ │ │ _UpdateLegCMD(data) │ │ │ │ │ │ { │ │ │ │ │ │ for (leg 0; leg 4; leg) { │ │ │ │ │ │ cmd[leg].tauFeedForward[jidx] _tau_ff[leg*3 jidx]; │ │ │ │ │ │ cmd[leg].qDes[jidx] _des_jpos[leg*3 jidx]; │ │ │ │ │ │ cmd[leg].qdDes[jidx] _des_jvel[leg*3 jidx]; │ │ │ │ │ │ cmd[leg].kpJoint(jidx,jidx) _Kp_joint[jidx]; │ │ │ │ │ │ cmd[leg].kdJoint(jidx,jidx) _Kd_joint[jidx]; │ │ │ │ │ │ } │ │ │ │ │ │ │ │ │ │ │ │ // 膝关节防翻转限制 │ │ │ │ │ │ if (qDes[2] 0.3) qDes[2] 0.3; │ │ │ │ │ │ } │ │ │ │ │ └─────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────────┘ │ │ │ │ │ ▼ │ │ ┌───────────────────────────────────────────────────────────────────────────┐ │ │ │ 输出 │ │ │ │ │ │ │ │ 每条腿 (FR, FL, HR, HL): │ │ │ │ ┌───────────────────────────────────────────────────────────────────┐ │ │ │ │ │ tauFeedForward[3] - 关节力矩 (N·m) │ │ │ │ │ │ qDes[3] - 期望关节位置 (rad) │ │ │ │ │ │ qdDes[3] - 期望关节速度 (rad/s) │ │ │ │ │ │ kpJoint[3x3] - 位置增益 (N·m/rad) │ │ │ │ │ │ kdJoint[3x3] - 速度增益 (N·m·s/rad) │ │ │ │ │ └───────────────────────────────────────────────────────────────────┘ │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────────┘ │ │ │ └─────────────────────────────────────────────────────────────────────────────────┘3. Task 系统3.1 任务基类templatetypenameTclassTask{public:Task(size_t dim):dim_task_(dim),op_cmd_(dim),pos_err_(dim),vel_des_(dim),acc_des_(dim){}// 主接口boolUpdateTask(constvoid*pos_des,constDVecTvel_des,constDVecTacc_des){_UpdateTaskJacobian();// 计算 Jt_UpdateTaskJDotQdot();// 计算 Jt·q̇_UpdateCommand(pos_des,vel_des,acc_des);// 计算命令_AdditionalUpdate();returntrue;}// 获取器voidgetCommand(DVecTop_cmd){op_cmdop_cmd_;}voidgetTaskJacobian(DMatTJt){JtJt_;}voidgetTaskJacobianDotQdot(DVecTJtDotQdot){JtDotQdotJtDotQdot_;}protected:// 虚函数 (必须实现)virtualbool_UpdateCommand(...)0;virtualbool_UpdateTaskJacobian()0;virtualbool_UpdateTaskJDotQdot()0;// 任务数据size_t dim_task_;DVecTop_cmd_;// 操作命令 (加速度)DMatTJt_;// 任务 JacobianDVecTJtDotQdot_;// Jt·q̇DVecTpos_err_;// 位置误差DVecTvel_des_;// 期望速度DVecTacc_des_;// 期望加速度// 增益DVecT_Kp;// 位置增益DVecT_Kd;// 速度增益};3.2 任务类型任务类型描述优先级BodyPosTask机体位置控制3BodyOriTask机体姿态控制2LinkPosTask(FootTask)足部位置控制4 (最低)3.3 机体位置任务templatetypenameTclassBodyPosTask:publicTaskT{public:BodyPosTask(constFloatingBaseModelT*robot):TaskT(3),_robot_sys(robot){// Jacobian: 从广义速度到机体速度的映射Jt_.block(0,3,3,3).setIdentity();// 机体速度部分// 增益_Kp_kinDVecT::Constant(3,1.0);_KpDVecT::Constant(3,50.0);_KdDVecT::Constant(3,1.0);}protected:bool_UpdateCommand(constvoid*pos_des,constDVecTvel_des,constDVecTacc_des)override{Vec3T*pos_cmd(Vec3T*)pos_des;Vec3Tlink_pos_robot_sys-_state.bodyPosition;// 位置误差pos_err__Kp_kin*(pos_cmd-link_pos);// 世界坐标系中的速度Vec3Tcurr_velRot.transpose()*_robot_sys-_state.bodyVelocity.tail(3);// PD 控制 前馈op_cmd__Kp*(pos_cmd-link_pos)_Kd*(vel_des-curr_vel)acc_des;returntrue;}};4. Contact 系统4.1 接触约束基类templatetypenameTclassContactSpec{public:ContactSpec(size_t dim):dim_contact_(dim){}virtualbool_UpdateJc()0;// 接触 Jacobianvirtualbool_UpdateJcDotQdot()0;// Jc·q̇virtualbool_UpdateUf()0;// 力约束矩阵virtualbool_UpdateInequalityVector()0;voidsetRFDesired(constDVecTFr_des){Fr_des_Fr_des;}DMatTJc_;// 接触 Jacobian (dim x num_qdot)DVecTJcDotQdot_;// Jc·q̇DMatTUf_;// 力约束矩阵 (摩擦锥)DVecTieq_vec_;// 不等式向量DVecTFr_des_;// 期望反作用力};4.2 单点接触实现templatetypenameTclassSingleContact:publicContactSpecT{public:SingleContact(constFloatingBaseModelT*robot,intcontact_pt):ContactSpecT(3),_contact_pt(contact_pt),_dim_U(6){// 摩擦锥约束 (μ 0.4)// 标准摩擦金字塔:// fz 0// |fx| μ*fz// |fy| μ*fz// fz max_fzUf_(0,2)1.;// fz 0Uf_(1,0)1.;// fx μ*fz 0Uf_(1,2)mu;Uf_(2,0)-1.;// -fx μ*fz 0Uf_(2,2)mu;Uf_(3,1)1.;// fy μ*fz 0Uf_(3,2)mu;Uf_(4,1)-1.;// -fy μ*fz 0Uf_(4,2)mu;Uf_(5,2)-1.;// -fz -max_fz}};4.3 摩擦锥可视化力空间 (单接触) Fz (法向) │ │ ┌─────────┐ │ │╲ ╱│ │ │ ╲ ╱ │ │ │ ╲ ╱ │ │ │ ╲ ╱ │ │ │ V │ ← 力必须在摩擦金字塔内 │ │ │ │ └─────────┘ └────────────────► Fy ╱ ╱ Fx 约束条件: ┌────────────────────────────────────────┐ │ Fz 0 (单边) │ │ |Fx| μ * Fz (摩擦 X) │ │ |Fy| μ * Fz (摩擦 Y) │ │ Fz Fz_max (力限制) │ └────────────────────────────────────────┘5. WBIC 算法5.1 WBIC QP 形式化最小化: J ||Δq̈_floating||² ||ΔF||² 约束条件: A_full * q̈ b Jc^T * F (浮基动力学) Uf * F 0 (摩擦锥) Uf * (Fr_des ΔF) 0 (力约束) 其中: Δq̈_floating 浮基加速度修正量 ΔF 反作用力修正量 Fr_des 来自 MPC 的期望力 Jc 接触 Jacobian A 质量矩阵 b 科里奥利力 重力5.2 WBIC 实现templatetypenameTvoidWBICT::MakeTorque(DVecTcmd,void*extra_input){// 1. 接触构建_ContactBuilding();// 组装 Jc, JcDotQdot, Uf, Fr_des// 2. 从接触约束计算初始 qddotDMatTJcBar;_WeightedInverse(_Jc,Ainv_,JcBar);qddot_preJcBar*(-_JcDotQdot);NpreI-JcBar*_Jc;// 零空间投影// 3. 层级任务分解for(task:_task_list){task-getTaskJacobian(Jt);task-getTaskJacobianDotQdot(JtDotQdot);task-getCommand(xddot);// 投影到零空间JtPreJt*Npre;_WeightedInverse(JtPre,Ainv_,JtBar);// 更新 qddotqddot_preJtBar*(xddot-JtDotQdot-Jt*qddot_pre);// 更新零空间NpreNpre*(I-JtBar*JtPre);}// 4. 设置 QP_SetEqualityConstraint(qddot_pre);// 动力学约束_SetInEqualityConstraint();// 摩擦锥_SetCost();// 最小化修正量// 5. 求解 QPsolve_quadprog(G,g0,CE,ce0,CI,ci0,z);// 6. 获取解for(i0;i_dim_floating;i)qddot_pre[i]z[i];// 添加修正量_GetSolution(qddot_pre,cmd);// 提取力矩}6. KinWBC - 运动学全身控制KinWBC 用于计算期望的关节位置和速度不涉及力矩优化。templatetypenameTboolKinWBCT::FindConfiguration(constDVecTcurr_config,conststd::vectorTaskT*task_list,conststd::vectorContactSpecT*contact_list,DVecTjpos_cmd,DVecTjvel_cmd){// 1. 构建接触零空间DMatTNcI;if(contact_list.size()0){DMatTJc;for(contact:contact_list){contact-getContactJacobian(Jc_i);Jc.append(Jc_i);}// 零空间: Nc I - Jc^ * Jc_BuildProjectionMatrix(Jc,Nc);}// 2. 按优先级处理任务DMatTN_preNc;DVecTdelta_qDVecT::Zero(num_qdot_);DVecTqdotDVecT::Zero(num_qdot_);for(task:task_list){task-getTaskJacobian(Jt);// 投影到零空间JtPreJt*N_pre;_PseudoInverse(JtPre,JtPre_pinv);// 计算配置变化delta_qJtPre_pinv*(task-getPosError()-Jt*delta_q);qdotJtPre_pinv*(task-getDesVel()-Jt*qdot);// 更新零空间_BuildProjectionMatrix(JtPre,N_nx);N_pre*N_nx;}// 3. 提取关节命令for(i0;inum_act_joint_;i){jpos_cmd[i]curr_config[i6]delta_q[i6];jvel_cmd[i]qdot[i6];}returntrue;}7. 任务优先级优先级 1 (最高): 接触约束 - 支撑腿必须保持在地面上 - 通过零空间投影实现 优先级 2: 机体姿态任务 - 保持机体直立 - 跟踪期望的 roll, pitch, yaw 优先级 3: 机体位置任务 - 跟踪期望轨迹 - 跟随速度命令 优先级 4 (最低): 摆动足部任务 - 跟踪摆动轨迹 - 实现落脚点放置8. 使用的开源库8.1 核心库库用途位置Eigen线性代数、矩阵运算、SVD系统库/usr/include/eigen3/Goldfarb Optimizer (QuadProg)QP 求解器third-party/Goldfarb_Optimizer/8.2 Goldfarb OptimizerGoldfarb Optimizer 是用于求解二次规划的 C 库特别适用于 WBC 中的力矩优化。特点基于有效集算法支持等式和不等式约束轻量级实现代码调用#includeGoldfarb_Optimizer/QuadProg.hhsolve_quadprog(G,g0,CE,ce0,CI,ci0,z);其中G- 二次目标矩阵 (Hessian)g0- 线性目标向量CE- 等式约束矩阵ce0- 等式约束向量CI- 不等式约束矩阵ci0- 不等式约束向量z- 解向量8.3 CMake 依赖# user/MIT_Controller/Controllers/WBC/WBIC/CMakeLists.txt add_library(WBIC SHARED ${sources} ${headers}) target_link_libraries(WBIC Goldfarb_Optimizer)9. MPC 与 WBC 集成9.1 数据流┌─────────────────────────────────────────────────────────────────────────────┐ │ MPC → WBC 数据流 │ ├─────────────────────────────────────────────────────────────────────────────┤ │ │ │ LocomotionCtrlData (接口结构): │ │ ┌───────────────────────────────────────────────────────────────────────┐ │ │ │ │ │ │ │ // 机体命令 (来自 MPC) │ │ │ │ pBody_des[3] cMPCOld-pBody_des │ │ │ │ vBody_des[3] cMPCOld-vBody_des │ │ │ │ aBody_des[3] cMPCOld-aBody_des │ │ │ │ pBody_RPY_des[3] cMPCOld-pBody_RPY_des │ │ │ │ vBody_Ori_des[3] cMPCOld-vBody_Ori_des │ │ │ │ │ │ │ │ // 足部命令 (来自 MPC) │ │ │ │ pFoot_des[4][3] cMPCOld-pFoot_des │ │ │ │ vFoot_des[4][3] cMPCOld-vFoot_des │ │ │ │ aFoot_des[4][3] cMPCOld-aFoot_des │ │ │ │ │ │ │ │ // 力 (来自 MPC) │ │ │ │ Fr_des[4][3] cMPCOld-Fr_des │ │ │ │ │ │ │ │ // 接触状态 (来自 MPC) │ │ │ │ contact_state[4] cMPCOld-contact_state │ │ │ │ │ │ │ └───────────────────────────────────────────────────────────────────────┘ │ │ │ └──────────────────────────────────────────────────────────────────────────────┘9.2 任务分配for(leg0;leg4;leg){if(contact_state[leg]0){// 支撑腿// 接触约束_foot_contact[leg]-setRFDesired(Fr_des[leg]);_contact_list.push_back(_foot_contact[leg]);}else{// 摆动腿// 任务_foot_task[leg]-UpdateTask(pFoot_des[leg],vFoot_des[leg],aFoot_des[leg]);_task_list.push_back(_foot_task[leg]);}}10. 增益配置// WBC 增益Kp_body[50,50,50]// 机体位置 P 增益Kd_body[1,1,1]// 机体位置 D 增益Kp_ori[50,50,50]// 机体姿态 P 增益Kd_ori[1,1,1]// 机体姿态 D 增益Kp_foot[70,70,70]// 足部位置 P 增益Kd_foot[3,3,3]// 足部位置 D 增益Kp_joint[5,5,5]// 关节 P 增益Kd_joint[1.5,1.5,1.5]// 关节 D 增益11. 文件结构user/MIT_Controller/Controllers/WBC/ ├── WBC_Ctrl/ │ ├── WBC_Ctrl.hpp │ ├── WBC_Ctrl.cpp │ └── CMakeLists.txt │ ├── WBC/ │ ├── Task.hpp ← 任务基类 │ ├── ContactSpec.hpp ← 接触约束基类 │ ├── WBC.hpp │ ├── WBC.cpp │ └── CMakeLists.txt │ ├── WBIC/ ← Whole Body Impulse Control │ ├── WBIC.hpp │ ├── WBIC.cpp │ ├── KinWBC.hpp ← 运动学 WBC │ ├── KinWBC.cpp │ └── CMakeLists.txt ← 链接 Goldfarb_Optimizer │ └── TaskSet/ ← 任务实现 ├── BodyPosTask.hpp ├── BodyOriTask.hpp └── LinkPosTask.hpp third-party/ ├── Goldfarb_Optimizer/ ← QP 求解器 │ ├── QuadProg.hh │ ├── QuadProg.cc │ └── CMakeLists.txt │ └── ...12. 构建说明12.1 依赖安装# Ubuntu/Debiansudoapt-getinstallcmake build-essential libeigen3-dev libblas-dev liblapack-dev12.2 编译# 创建构建目录mkdirbuildcdbuild cmake..make-j412.3 运行# 运行仿真器./sim/sim# 运行 WBC 控制器 (另一个终端)./user/MIT_Controller/mit_ctrl3s13. 性能特性指标值执行时间0.1-0.3 ms频率1 kHz内存使用较低 (任务 Jacobian)数值稳定性优秀 (零空间投影)实时性能即时响应14. 参考Goldfarb Optimizer:third-party/Goldfarb_Optimizer/QuadProg.hhEigen: http://eigen.tuxfamily.org原始论文: “Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control”WBC 论文: “Whole-Body Control of Torque-Controlled Humanoid Robots”