在前文中我给出了基于Webots环境下配置和安装qpOASEs非线性优化库的方法:

本文基于该库来实现对三通道降维解耦框架中最核心的力分配环境进行仿真验证并与传统的静力学分配方法对比。由三通道降维解耦控制方法可知通过构建虚拟刚体VMC模型我们可以设计高度、速度与姿态三个通道的虚拟伺服,采用简单的线性PD控制器模拟虚拟弹簧从而模拟与环境柔顺交互所需的不同刚度虚拟弹簧,最终产生虚拟力和虚拟扭矩向关节伺服驱动器发送控制命令,而实现将虚拟力和虚拟扭矩转化为支撑腿足底力的关键环节就是力分配,在下文:

中我给出了基于静力学下的Trot步态力分配方法,由于其仅需要考虑对角腿通过给定一到两个约束就可以获得解析解,而对于站立或Gallop等多腿支撑情况我采用虚拟腿映射的方法仍然转换为对角支撑下的分配,同样可以获取较好的力分配结果,因此力分配实际是解决如下一个类似之前最小二乘法拟合磁场中提到的线性回归问题即,在MIT论文中这样的多点支持平衡问题表示如下:

在MIT开源代码中其采用QP优化的方式来解决上述问题,在前文中我给出了一个简单的侧向平面的简单力分配问题,通过求解Matlab下QP优化所需的H和g矩阵并给出了简单的摩擦锥约束所需的不等式矩阵计算方法,MIT论文中给出的优化问题:

可以看到其相比原始的例子多了几项,首先是权重S矩阵其用于调节虚拟力和虚拟力矩优化结果的权重(注意其为一个对角矩阵,转秩为其自身),第二项为实现最小化满足优化所需要的总合力,最后一项类似一个滤波器通过最小化前后两次力的幅值来平衡优化结果防止足底力的跳变,同理我们还是一步一步推导一下上述QP优化所需的H和g矩阵,参考MIT源码和其引用ETH的参考文献其最终的优化问题形式实际如下:

qpOASES库能求解的QP优化形式如下,即需要整理出所需要的H,g,不等式约束矩阵A和上下限向量ub,lb,ubA,lbA:

参考相同的方法展开F矩阵,并增加S和W的权重矩阵后形式如下

可以看到初步整理后的矩阵大致分为4部分,其中最后的常数部分不影响优化结果可以去除,①部分与QP优化所需H矩阵对应,②和③部分中由于在QP优化形式中其转秩为其本身因此(注意S为对角矩阵,转置同样为其本身):

综上,上面两个公式与QP优化形式对应得到:

与MIT源码BalanceController.cpp中内容对应处H矩阵如下:

void BalanceController::calc_H_qpOASES() {
  // Use the A matrix to compute the QP cost matrix H
  H_eigen = 2 * (A_control.transpose() * S_control * A_control +
                 (alpha_control + 1e-3) * W_control);


g矩阵如下:

void BalanceController::calc_g_qpOASES() {
  g_eigen = -2 * A_control.transpose() * S_control * b_control;
  g_eigen += -2 * xOptPrev.transpose() * alpha_control;

可以看到大致形式一致,只是在工程编程实现时有些许不同,至于S,W,alfa等优化参数除可以参考MIT源码外可以在参考文献《High-slope Terrain Locomotion for Torque-Controlled Quadruped Robots》和《Control of Dynamic Gaits for a Quadrupedal Robot》中找到可参考的相关参数描述:

在求取出H和g矩阵后我们已经可以实现无约束的QP优化产生需要的足力,但是在之前力分配文中提到,采用静力学分析同样可以实现上述目的,但由于在特殊位置下会产生负向力,另外也会出现打滑,因此QP优化的优势除了能限制力赋值外还可以限制摩擦圆锥,首先参考QP库优化形式设定足力的上下限制:

对应MIT源码如下:

void BalanceController::calc_lb_ub_qpOASES() {
  for (int i = 0; i < NUM_CONTACT_POINTS; i++) {
    for (int j = 0; j < NUM_VARIABLES_PER_FOOT; j++) {
      lb_qpOASES[NUM_VARIABLES_PER_FOOT * i + j] =
          contact_state(i) * NEGATIVE_NUMBER;
      ub_qpOASES[NUM_VARIABLES_PER_FOOT * i + j] =
          contact_state(i) * POSITIVE_NUMBER;
    }
  }

对于摩擦圆锥所需的不等式约束,MIT源码参考论文《High-slope Terrain Locomotion for Torque-Controlled Quadruped Robots》中的摩擦圆锥构建方法:

摩擦圆锥定义为:

因此对于腿i的摩擦约束为(可能与MIT代码中不一致):

由于有4条腿,如定义着地状态为gi则不等式约束矩阵为:

因此不等式约束矩阵A对应MIT源码如下:

void BalanceController::calc_A_qpOASES() {
  Eigen::Vector3d t1x;
  t1x << 1, 0, 0;
  Eigen::Vector3d t2y;
  t2y << 0, 1, 0;

  for (int i = 0; i < NUM_CONTACT_POINTS; i++) {
    C_control.block<1, 3>(5 * i + 0, 3 * i)
        << -mu_friction * direction_normal_flatGround.transpose() +
               t1x.transpose();
    C_control.block<1, 3>(5 * i + 1, 3 * i)
        << -mu_friction * direction_normal_flatGround.transpose() +
               t2y.transpose();
    C_control.block<1, 3>(5 * i + 2, 3 * i)
        << mu_friction * direction_normal_flatGround.transpose() +
               t2y.transpose();
    C_control.block<1, 3>(5 * i + 3, 3 * i)
        << mu_friction * direction_normal_flatGround.transpose() +
               t1x.transpose();
    C_control.block<1, 3>(5 * i + 4, 3 * i)
        << direction_normal_flatGround.transpose();
  }

ubA,lbA对应MIT源码如下:

void BalanceController::calc_lbA_ubA_qpOASES() {
  for (int i = 0; i < NUM_CONTACT_POINTS; i++) {
    lbA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i] =
        contact_state(i) * NEGATIVE_NUMBER;
    lbA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 1] =
        contact_state(i) * NEGATIVE_NUMBER;
    lbA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 2] = 0;
    lbA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 3] = 0;
    lbA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 4] =
        contact_state(i) * minNormalForces_feet(i);

    ubA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i] = 0;
    ubA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 1] = 0;
    ubA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 2] =
        contact_state(i) * POSITIVE_NUMBER;
    ubA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 3] =
        contact_state(i) * POSITIVE_NUMBER;
    ubA_qpOASES[NUM_CONSTRAINTS_PER_FOOT * i + 4] =
        contact_state(i) * maxNormalForces_feet(i);
  }
    lbA_qpOASES[NUM_CONSTRAINTS_QP - 1] = NEGATIVE_NUMBER;
    ubA_qpOASES[NUM_CONSTRAINTS_QP - 1] = POSITIVE_NUMBER;
}

综上,将各矩阵带入对应QP优化库接口中既可以得到所需要的优化结果,具体实现方式可以参考MIT代码。本文中我们重新梳理了MIT所采用的QP力分配算法,那它到底和传统静力学分析有啥优势,下面给出一个我在Webots仿真下的对比实验:

实验测试使用两种分配方法(相同虚拟伺服PD参数与关节刚度)上下20°左右的斜坡:

(1)静力学仿真视频如下:

姿态角与地形估计
质心状态
FR关节与足力
HR关节与足力

(2)QP优化力分配上坡仿真视频如下:

实验中仿真数据波形如下:

姿态角与地形估计
质心状态
FR关节与足力
HR关节与足力

从图中可以看到采用静力学分配在上坡后一段时间出现由于没有限制摩擦圆锥的打滑情况,后面绳子出现了足力抖动的情况,而QP优化的足力连续平滑,仿真中上坡迅速,在坡上没有出现打滑情况,效果十分好,可见QP优化在足式机器人控制中还是有非常大的优势,这也是MIT早期样机中能实现多种不同步态切换的原因。

综上,如果你需要移植MIT的优化方法,主要需要完成对输入虚拟控制量、足端位置与MIT机器人坐标系对齐,在输出中也同样对齐即可,当然如果你理解了QP优化的原理依据自己真实的坐标重新写一遍各矩阵也不是很难,当然一定要注意各矩阵维度并正确初始化QP库的类。虽然上述方法效果不错但是我目前还是使用单片机STM32H7系列来构建样机的主控制器,因此无法使用QP优化库后续还是需要基于Linux和RT-thread这样的实时系统搭配,构建高算力控制器才能应用这些先进的优化算法。

备注: MIT代码中的坐标系描述如下: