Mini Cheetah 代码分析（九）加速度的零空间任务分级与WBIC解析

一、原理部分

二、公式部分

三、代码注释

一、原理部分

整体代码分为两步

第一步：通过零空间投影法的任务分级，计算出各个关节的加速度。

第二步：将关节加速度通过QP优化，计算关节扭矩。QP中以加速度修正项和地面反作用力的修正项，也就是所谓的松弛量。以整个动力学作为约束进行求出修正项，最后与MPC结果一起带入动力学方程，求出关节扭矩。

二、公式部分

第一步：

a.初始化，从接触雅可比Jc开始，首先计算得到（a）接触雅可比Jc的动力学伪逆，权重矩阵使用惯性矩阵，

b.然后，同时计算接触任务的零空间，此处的Jc*为上一步的J_bar

同时得到，第零次任务的加速度，此处的Jc__dyn_bar为(a)得到的接触雅可比Jc的动力学伪逆

第二步：

执行第1层级的任务，利用第一步中的结果开始迭代

a. 计算第一层级任务的雅可比矩阵J1在接触任务零空间的投影J1|0，并计算投影对应的动力学伪逆矩阵J1_bar

b.同时使用a步中，当前的第一层任务在第0级任务零空间投影J1|0，及其伪逆J1_bar，计算的得到当前第一层任务的零空间

同时，得到第1次任务的加速度，此处的Jc__dyn_bar为(a)得到的接触雅可比Jc的动力学伪逆

第三步，再次进行重复第二步中迭代，此处示例第二层级任务

a.计算第二层级任务的，雅可比矩阵J2在第一层任务零空间Ni|i-1的投影J2|1，并计算投影对应的动力学伪逆矩阵J2_bar

b.同时计算当前，第二层任务的零空间N2|1，

同时，得到当前第二层级任务的加速度，

第四步，重复第三步中的a，b步骤。

简单概括，主要迭代步骤为，

利用上一层任务的零空间Ni-1，计算当前Ji在零空间Ni-1中的投影，并以投影计算得到当前零空间Ni。

同时，利用投影的伪逆，获取任务的加速度。

三、加速度任务分级部分的代码注释

主要在WBIC.cpp文件的函数void WBIC<T>::MakeTorque(DVec<T> &cmd, void *extra_input)中。

template <typename T>
void WBIC<T>::MakeTorque(DVec<T> &cmd, void *extra_input)
{

  DVec<T> qddot_pre;
  DMat<T> JcBar;
  DMat<T> Npre;

  if (_dim_rf > 0) // 有接触
  {
    // Contact Setting
    _ContactBuilding();

    // Set inequality constraints
    _SetInEqualityConstraint();
    //从第0级接触任务开始 对应公式23，求解动力学一致的雅克比矩阵伪逆
    WB::_WeightedInverse(_Jc, WB::Ainv_, JcBar); // dynamic sonsistent inverse
    qddot_pre = JcBar * (-_JcDotQdot);           // 对应公式21，得到qddot_0__cmd
    Npre = _eye - JcBar * _Jc;                   // null space projection matrix得到零空间矩阵，N

  }
  else // 无接触
  {
    qddot_pre = DVec<T>::Zero(WB::num_qdot_); //
    Npre = _eye;
  }

  // Task
  Task<T> *task;
  DMat<T> Jt, JtBar, JtPre;
  DVec<T> JtDotQdot, xddot;

  for (size_t i(0); i < (*_task_list).size(); ++i)
  { // 开始按照任务分级，进行迭代
    task = (*_task_list)[i];

    task->getTaskJacobian(Jt);
    task->getTaskJacobianDotQdot(JtDotQdot);
    task->getCommand(xddot);

    JtPre = Jt * Npre;                             // 对应公式19，由当前任务Ji和上一次任务零空间Ni-1得到Ji在零空间的投影JtPre
    WB::_WeightedInverse(JtPre, WB::Ainv_, JtBar); // 将JtPre转换为动力学一致的雅克比伪逆
    // 对应公式18，由伪逆，雅可比，和一阶雅可比，得到当前任务的加速度
    qddot_pre += JtBar * (xddot - JtDotQdot - Jt * qddot_pre);

    // 由当前任务在零空间中投影的雅克比伪逆，得到当前任务的零空间
    Npre = Npre * (_eye - JtBar * JtPre);
  }

******************************************************************************

优化求解部分

一、原理部分

solve_quadprog求解器带入的标准形式

二、公式部分

(a) solve_quadprog求解器带入的标准形式

T f = solve_quadprog(G, g0, CE, ce0, CI, ci0, z); // 求解优化问题

// CE,ce0等式约束条件，CI, ci0,不等式约束条件

// z为求解的结果，前6项的加速度松弛变量，后12项为地反力松弛变量

对应文章中的公式：

待求解的自变量，前6项的加速度松弛变量delt_qddot，后12项为地反力松弛变量delt_fr

主要的约束是等式约束，等式约束其实只有一项，即：

使用前一部分任务分级的得到的12维qddot带入动力学方程，Sf为筛选矩阵，取前六项

等式的左侧为松弛变量，右侧为残差

展开形式即为

不等式约束也只有一项，即接触的摩擦锥约束

(b)后处理，在函数void WBIC<T>::_GetSolution(const DVec<T> &qddot, DVec<T> &cmd)中，利用公式25，利用acceleration这一项计算得到加速度，利用reaction forces这一项计算得到接触力

最后带入动力学方程得到关节扭矩，下发给电机执行，完成整个过程。

三、优化部分代码注释

(a)完成优化，同样在WBIC.cpp的函数void WBIC<T>::MakeTorque(DVec<T> &cmd, void *extra_input)中

void WBIC<T>::MakeTorque(DVec<T> &cmd, void *extra_input)
{
  if (!WB::b_updatesetting_)
  {
    printf("[Wanning] WBIC setting is not done\n");
  }
  if (extra_input)
    _data = static_cast<WBIC_ExtraData<T> *>(extra_input);

  // resize G, g0, CE, ce0, CI, ci0
  _SetOptimizationSize();
  _SetCost();

 if (_dim_rf > 0) // 有接触，设置接触的不等式约束
  {
    // Contact Setting
    _ContactBuilding();

    // Set inequality constraints
    _SetInEqualityConstraint();
    // 对应公式23，求解动力学一致的雅克比矩阵伪逆
    WB::_WeightedInverse(_Jc, WB::Ainv_, JcBar); // dynamic sonsistent inverse
    qddot_pre = JcBar * (-_JcDotQdot);           // 对应公式21，得到qddot_0__cmd
    Npre = _eye - JcBar * _Jc;                   // null space projection matrix得到零空间矩阵，N
    // pretty_print(JcBar, std::cout, "JcBar");
    // pretty_print(_JcDotQdot, std::cout, "JcDotQdot");
    // pretty_print(qddot_pre, std::cout, "qddot 1");
  }
  else // 无接触
  {
    qddot_pre = DVec<T>::Zero(WB::num_qdot_); //
    Npre = _eye;
  }
///
此处为任务分级部分
///
  // qddot_pre：对应公式18,由任务分级，计算得到的的加速度
//等式约束
  _SetEqualityConstraint(qddot_pre); // 对应公式25中的floating base dyn.项，
  T f = solve_quadprog(G, g0, CE, ce0, CI, ci0, z); // 求解优化问题
  // CE,ce0等式约束条件，CI, ci0,不等式约束条件
  // z为求解的结果，前6项的加速度松弛变量，后12项为地反力松弛变量
  (void)f;//变量f没有使用


//后处理
  for (size_t i(0); i < _dim_floating; ++i)
    qddot_pre[i] += z[i]; // 对应公式25,acceleration这一项,前6项的加速度松弛变量

  if (fabs(qddot_pre[0]) > 99999.) // 发现状态切换第一个时刻会存在加速度爆炸，踢出这个时刻到数据
  {
    qddot_pre = DVec<T>::Zero(WB::num_qdot_);
    printf("qddot_pre = DVec<T>::Zero(WB::num_qdot_);  !!!!!!!!!!!!\n");
  }
  _GetSolution(qddot_pre, cmd); // 对应公式26,带入动力学方程,由qddot计算tot_tau,即,所有的关节扭矩数值

  _data->_opt_result = DVec<T>(_dim_opt);
  for (size_t i(0); i < _dim_opt; ++i)
  {
    _data->_opt_result[i] = z[i]; // 获取到所有的优化结果，前6项的加速度松弛变量，后12项为地反力松弛变量
  }

等式约束的构造

// 等式约束，动力学方程约束
template <typename T>
void WBIC<T>::_SetEqualityConstraint(const DVec<T> &qddot)
{
  if (_dim_rf > 0)
  {
    // _dyn_CE为等式左侧,前6行列的惯性矩阵的，右下角为S*Jc^T，松弛变量顺序为
    _dyn_CE.block(0, 0, _dim_eq_cstr, _dim_floating) =
        WB::A_.block(0, 0, _dim_floating, _dim_floating);
    _dyn_CE.block(0, _dim_floating, _dim_eq_cstr, _dim_rf) =
        -WB::Sv_ * _Jc.transpose();
    // _dyn_ce0为等式右侧，对应公式25中的floating base dyn.项，选择矩阵Sf为Sv_
    _dyn_ce0 = -WB::Sv_ * (WB::A_ * qddot + WB::cori_ + WB::grav_ -
                           _Jc.transpose() * _Fr_des);
  }
  else // 没有接触力情况的约束
  {
    _dyn_CE.block(0, 0, _dim_eq_cstr, _dim_floating) =
        WB::A_.block(0, 0, _dim_floating, _dim_floating);
    _dyn_ce0 = -WB::Sv_ * (WB::A_ * qddot + WB::cori_ + WB::grav_);
  }

  for (size_t i(0); i < _dim_eq_cstr; ++i)
  {
    for (size_t j(0); j < _dim_opt; ++j)
    {
      CE[j][i] = _dyn_CE(i, j); // 求了个转置
    }
    ce0[i] = -_dyn_ce0[i];
  }
  // pretty_print(_dyn_CE, std::cout, "WBIC: CE");
  // pretty_print(_dyn_ce0, std::cout, "WBIC: ce0");
}

不等式约束的构造

void WBIC<T>::_SetInEqualityConstraint() // 不等式约束条件，对应公式25中的contact force constraints
{
  _dyn_CI.block(0, _dim_floating, _dim_Uf, _dim_rf) = _Uf;
  _dyn_ci0 = _Uf_ieq_vec - _Uf * _Fr_des;

  for (size_t i(0); i < _dim_Uf; ++i)
  {
    for (size_t j(0); j < _dim_opt; ++j)
    {
      CI[j][i] = _dyn_CI(i, j);
    }
    ci0[i] = -_dyn_ci0[i];
  }
}