Mini Cheetah 代码分析（五）腿部控制器算法

阿狸OKay

已于 2022-05-05 17:46:41 修改

阅读量1.9k

点赞数 2

分类专栏：四足机器人文章标签：人工智能

于 2022-04-17 00:56:41 首次发布

本文链接：https://blog.csdn.net/weixin_50084948/article/details/124222623

版权

四足机器人专栏收录该内容

12 篇文章 40 订阅

订阅专栏

首先建立腿部运动学

坐标系定义

坐标系的建立方法比较简单，并没有使用DH等建系法，只有髋关节有一个旋转，剩下的都是坐标系平移。

因此根据上图中腿部各连杆坐标系的关系，可以得到腿部末端的坐标系的变换方程为

右：平移*旋转*平移*旋转

Tr = rotx(q1+pi)*trany(L1)*roty(q2)*tranz(L2)*trany(L4)*roty(q3)*tranz(L3)

Tr= transl(0,0,0)*trotx(q1+pi)*transl(0,L1,0)*troty(q2)...
    *transl(0,0,L2)*transl(0,L4,0)*troty(q3)*transl(0,0,L3)
simplify(Tr)

左：平移*旋转*平移*旋转

Tl = rotx(q1+pi)*trany(-L1)*roty(q2)*tranz(L2)*trany(-L4)*roty(q3)*tranz(L3)

Tl= transl(0,0,0)*trotx(q1+pi)*transl(0,-L1,0)*troty(q2)...
    *transl(0,0,L2)*transl(0,-L4,0)*troty(q3)*transl(0,0,L3)

simplify(Tl)

得到结果：

Tr = [ cos(q2 + q3), 0, sin(q2 + q3), L3*sin(q2 + q3) + L2*sin(q2)]
[-sin(q2 + q3)*sin(q1), -cos(q1), cos(q2 + q3)*sin(q1), L2*cos(q2)*sin(q1) - L1*cos(q1) - L4*cos(q1) - L3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))]
[ sin(q2 + q3)*cos(q1), -sin(q1), -cos(q2 + q3)*cos(q1), - L1*sin(q1) - L4*sin(q1) - L3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - L2*cos(q1)*cos(q2)]
[ 0, 0, 0, 1]

Tl =[ cos(q2 + q3), 0, sin(q2 + q3), L3*sin(q2 + q3) + L2*sin(q2)]
[-sin(q2 + q3)*sin(q1), -cos(q1), cos(q2 + q3)*sin(q1), L1*cos(q1) - L3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + L4*cos(q1) + L2*cos(q2)*sin(q1)]
[ sin(q2 + q3)*cos(q1), -sin(q1), -cos(q2 + q3)*cos(q1), L1*sin(q1) + L4*sin(q1) - L3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - L2*cos(q1)*cos(q2)]
[ 0, 0, 0, 1]

腿部控制器的内容主要在\common\include\Controllers\LegController.h和\common\src\Controllers\LegController.cpp文件中。

下发的指令变量主要包含了前馈关节力大小tauFeedForward，足端力大小forceFeedForward, 期望关节位置qDes, 期望的关节速度qdDes, 期望的质心位置pDes, 期望的质心速度vDes，以及质心和关节的PD控制器的参数。

template <typename T>
struct LegControllerCommand {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  LegControllerCommand() { zero(); }

  void zero();

  Vec3<T> tauFeedForward, forceFeedForward, qDes, qdDes, pDes, vDes;
  Mat3<T> kpCartesian, kdCartesian, kpJoint, kdJoint;
};

控制器数据结构体包含了计算所用到的变量，具体的含义写在注释中。

template <typename T>
struct LegControllerData {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  LegControllerData() { zero(); }

  void setQuadruped(Quadruped<T>& quad) { quadruped = &quad; }

  void zero();

  Vec3<T> q, qd, p, v,q_buffer,qd_buffer,qd_buffer_10[10],v_buffer,qd_numeric,qd_numeric_buffer;
//关节位置q, 关节角速度qd, 质心位置p, 质心速度v,缓存q_buffer,qd_buffer,qd_buffer_10[10],v_buffer,qd_numeric,qd_numeric_buffer;
  Mat3<T> J;//足端到髋关节位置的雅克比矩阵J
  Vec3<T> tauEstimate;//估计的关节力
  Vec3<T> tauActuatual;//实际的关节力
  Vec3<T> footforceActuatual;//实际足端力
  Vec3<T> footforceDesired;//期望足端力
  Quadruped<T>* quadruped;//机器人模型对象

  Vec3<T> tau_out;  //输出的关节力
};

函数computeLegJacobianAndPosition用于计算腿部雅克比矩阵和位置

template <typename T>
void computeLegJacobianAndPosition(Quadruped<T>& quad, Vec3<T>& q, Mat3<T>* J,
                                   Vec3<T>* p, int leg);

J和p的具体实现在LegController.cpp文件中，所有变量都相对于髋关节坐标系，它与身体坐标系具有相同的方向，但被平移到了ab/ad 关节处。

l1 =0.072 从abad髋关节到伸展屈曲髋关节的连杆长度; l2 = 0.211髋关节连杆长度(大腿长); l3 =0.208 小腿长度; l4 =0 膝关节偏移值，具体数值定义在MiniCheetah.h文件中。sideSign是区分左右的正负号。该构型的腿的正运动学和雅克比矩阵J可由经典的机械臂正运动学导出，需要注意关节坐标系的定义需与软件中定义的相一致。其运动学是有几何解析解的，一下提供一个相关的参考文档，目录为guyueclass/simulation&control/quadruped_robot_simulation/单腿逆解.pdf，还有其他文档可以学习。

https://github.com/guyuehome/guyueclass.git

雅克比矩阵的定义为，\dot{x}为足端坐标的速度，\dot{q}为关节角速度，

J的求解可以通过正运动学求导

如果熟练的话，使用J的Explicit form求解J更加，\dot{J_i}为J中的第i列，e-i为齐次矩阵第三列

ri的定义如下，表示从i关节坐标原点Oi指向末端坐标的向量

总之，MIT miniCheetah的雅克比矩阵结果如下

    J->operator()(0, 0) = 0;
    J->operator()(0, 1) = l3 * c23 + l2 * c2;
    J->operator()(0, 2) = l3 * c23;
    J->operator()(1, 0) = l3 * c1 * c23 + l2 * c1 * c2 - (l1+l4) * sideSign * s1;
    J->operator()(1, 1) = -l3 * s1 * s23 - l2 * s1 * s2;
    J->operator()(1, 2) = -l3 * s1 * s23;
    J->operator()(2, 0) = l3 * s1 * c23 + l2 * c2 * s1 + (l1+l4) * sideSign * c1;
    J->operator()(2, 1) = l3 * c1 * s23 + l2 * c1 * s2;
    J->operator()(2, 2) = l3 * c1 * s23;

足端位置使用正运动学求得，如果不会求解，需要补一下机器人学的基础知识。

  if (p) {
    p->operator()(0) = l3 * s23 + l2 * s2;
    p->operator()(1) = (l1+l4) * sideSign * c1 + l3 * (s1 * c23) + l2 * c2 * s1;
    p->operator()(2) = (l1+l4) * sideSign * s1 - l3 * (c1 * c23) - l2 * c1 * c2;
  }
}

关节力的计算未完待续

下面继续LegController.cpp中的具体函数实现

edampCommand用于紧急停止，实际并使用的edamp，也没有关闭腿的使能。不用管。

LegControllerCommand是控制器算法下发的执行腿部控制的指令。LegControllerData是从腿部获取到的实际数据。LegController<T>::updateData主要负责通信用于将数据送至控制器。

LegController<T>::updateCommand用于指令的计算。对于SPIne的主板的代码，tauFeedForward和forceFeedForward初始都设为0。

足端力f=kp*(pDes-p)+kd*(vDes-v)；

关节扭矩tau = J^T*f，求出tau之后，发送出去

然后获取关节空间的PD参数，和关节位置速度，发送出去。

关节力的估计值tauEstimate = tau +kp*(qDes-q)+kd*(qdDes-qd)；等于足端受到的外界力产生的关节力加上关节模拟的关节刚度产生的关节力。

LegController<T>::setLcm函数负责从LegControllerCommand和LegControllerData获取数据通过 LCM 通信用于调试数据。这个函数定义了从“Mini Cheetah 代码分析（三）快速从仿真中获取数据”中的LCM的输出内容，表达式的含义放在了注释中，可以看到，其实也可以从这个接口将数据以printf打印在终端上，一部分数据，作者也是注释掉了。datas代表实际值，是从updateData中获取的。commands是命令值，是从LegControllerCommand中获取的。

            lcmData->q[idx] = datas[leg].q[axis]; //关节角度
            lcmData->qd[idx] = datas[leg].qd[axis];//关节角速度
            lcmData->p[idx] = datas[leg].p[axis];//足端位置
            lcmData->v[idx] = datas[leg].v[axis];//足端速度
//            lcmData->tau_est[idx] = datas[leg].tauEstimate[axis];//估计的关节扭矩
            lcmData->tau_est[idx] = datas[leg].tauActuatual[axis];//实际的关节扭矩

//            if(leg == 3 && axis==2)
//printf("datas[leg].tauActuatual[axis] %.2f\n",lcmData->tau_est[idx]);

            lcmCommand->tau_ff[idx] = commands[leg].tauFeedForward[axis];//计算的前馈扭矩
            lcmCommand->f_ff[idx] = commands[leg].forceFeedForward[axis];//计算的前馈足端力
            lcmCommand->q_des[idx] = commands[leg].qDes[axis];//期望的关节角度
            lcmCommand->qd_des[idx] = commands[leg].qdDes[axis];//期望的关节角速度
            lcmCommand->p_des[idx] = commands[leg].pDes[axis];//期望的足端位置
            lcmCommand->v_des[idx] = commands[leg].vDes[axis];//期望的足端速度
            lcmCommand->kp_cartesian[idx] = commands[leg].kpCartesian(axis, axis);//足端与外界的Kp，用于推算足端力和关节力
            lcmCommand->kd_cartesian[idx] = commands[leg].kdCartesian(axis, axis);//足端与外界的Kd
            lcmCommand->kp_joint[idx] = commands[leg].kpJoint(axis, axis);//关节空间的Kp
            lcmCommand->kd_joint[idx] = commands[leg].kdJoint(axis, axis);//关节空间的Kd

阿狸OKay

关注

2
点赞
踩
19

收藏

觉得还不错? 一键收藏
0
评论
Mini Cheetah 代码分析（五）腿部控制器算法

腿部控制器的内容主要在LegController.h和LegController.cpp文件中。下发的指令变量主要包含了前馈关节力大小tauFeedForward，足端力大小forceFeedForward, 期望关节位置qDes, 期望的关节速度qdDes, 期望的质心位置pDes, 期望的质心速度vDes，以及质心和关节的PD控制器的参数。template <typename T>struct LegControllerCommand { EIGEN_MAKE_ALIGNE
复制链接

扫一扫