四足机器人动力学建模（一）

MIT开源的四足机器人动力学建模主要是牛顿欧拉法式（6.45）下一个关节的角速度=两个关节间的旋转变换矩阵*上一个关节的角速度+下一个关节的角速度牛顿欧拉法通过外推来计算各个连杆的质心受到的力和力矩。计算过程式（6.49）计算连杆质心受到的力。式（6.50）计算连杆质心收到的力矩；连杆质心受到的力和力矩由重力加非惯性力组成。外推就是从基座向机械臂末端递推，各个关节的角速度，线速度，角加速度，线加速

文章共3,510字 · 阅读需要大约12分钟

一键AI生成摘要，助你高效阅读

问答

重八的小弟

8596人浏览 · 2022-07-22 20:10:38

重八的小弟 · 2022-07-22 20:10:38 发布

MIT开源的四足机器人动力学建模主要是牛顿欧拉法

式（6.45）下一个关节的角速度=两个关节间的旋转变换矩阵*上一个关节的角速度+下一个关节的角速度

牛顿欧拉法通过外推来计算各个连杆的质心受到的力和力矩。计算过程式（6.49）计算连杆质心受到的力。

式（6.50）计算连杆质心收到的力矩；连杆质心受到的力和力矩由重力加非惯性力组成。

外推就是从基座向机械臂末端递推，各个关节的角速度，线速度，角加速度，线加速度，以及连杆质心的线加速度。

内推就是机械臂的末端的力和力矩向基座递推，求各个关节的力和力矩。

然后由牛顿欧拉方程整理得到机器人的动力学方程：

这个动力方程是机械臂末端没有与外界接触，不含接触力。

带接触力的动力学方程：

其中的第二项由上一个式子的后三项组成，这个式子的右边式关节力矩和接触力产生的力矩。

二、在代码中实现

FloatingBaseModel<float> _model; //定义浮动基模型变量
_model = _quadruped.buildModel();  //初始化浮动基模型

1.确定机机器人的质量长度信息

  cheetah._bodyMass = 3;  机身质量
  cheetah._bodyLength = 0.185 * 2;  身体的长  前后两个hip电机的中心距离
  cheetah._bodyWidth = 0.05025 * 2;  宽       左右两个abad电机的中心距离
  cheetah._bodyHeight = 0.05 * 2;    高
  cheetah._abadGearRatio = 6;        电机减速比
  cheetah._hipGearRatio = 6;         电机减速比
  cheetah._kneeGearRatio = 9.33;     电机和连杆的减速比
  cheetah._abadLinkLength = 0.072;    abad电机中心到hip电机的中心距离
  cheetah._hipLinkLength = 0.211;    大腿连杆长度

  cheetah._kneeLinkY_offset = 0;
  cheetah._kneeLinkLength = 0.20;    小腿连杆长
  cheetah._maxLegLength = 0.409;     大腿加小腿的总长

2.从solidworks中查看关节的惯性张量

  Mat3<T> abadRotationalInertia;
  abadRotationalInertia << 381, 58, 0.45, 58, 560, 0.95, 0.45, 0.95, 444;   //旋转轴DH系的Z轴
  abadRotationalInertia = abadRotationalInertia * 1e-6;
  Vec3<T> abadCOM(0, 0.036, 0);  // LEFT
  SpatialInertia<T> abadInertia(0.54, abadCOM, abadRotationalInertia);

髋关节hip的惯性张量

  Mat3<T> hipRotationalInertia;
  hipRotationalInertia << 1983, 245, 13, 245, 2103, 1.5, 13, 1.5, 408;
  hipRotationalInertia = hipRotationalInertia * 1e-6;
  Vec3<T> hipCOM(0, 0.016, -0.02);
  SpatialInertia<T> hipInertia(0.634, hipCOM, hipRotationalInertia);

SpatialInertia<T> 是将三维的惯性张量转换为空间六维的惯性张量

膝盖关节的惯性张量

 Mat3<T> kneeRotationalInertia, kneeRotationalInertiaRotated;
  kneeRotationalInertiaRotated << 6, 0, 0, 0, 248, 0, 0, 0, 245;
  kneeRotationalInertiaRotated = kneeRotationalInertiaRotated * 1e-6;
  kneeRotationalInertia = RY * kneeRotationalInertiaRotated * RY.transpose();
  Vec3<T> kneeCOM(0, 0, -0.061);    //当前关节在当前关节坐标系下的质心位置
  SpatialInertia<T> kneeInertia(0.064, kneeCOM, kneeRotationalInertia);

身体的惯性张量

  Mat3<T> bodyRotationalInertia;
  bodyRotationalInertia << 11253, 0, 0, 0, 36203, 0, 0, 0, 42673;
  bodyRotationalInertia = bodyRotationalInertia * 1e-6;
  Vec3<T> bodyCOM(0, 0, 0);
  SpatialInertia<T> bodyInertia(cheetah._bodyMass, bodyCOM, bodyRotationalInertia);

确定关节转子和关节刚接连杆的末端位置，在关节坐标系下

  cheetah._abadRotorLocation = Vec3<T>(0.125, 0.049, 0);
  cheetah._abadLocation =
      Vec3<T>(cheetah._bodyLength, cheetah._bodyWidth, 0) * 0.5;
  cheetah._hipLocation = Vec3<T>(0, cheetah._abadLinkLength, 0);
  cheetah._hipRotorLocation = Vec3<T>(0, 0.04, 0);
  cheetah._kneeLocation = Vec3<T>(0, 0, -cheetah._hipLinkLength);
  cheetah._kneeRotorLocation = Vec3<T>(0, 0, 0);

关节坐标系的建立图示：

本体系的坐标轴方向 x轴朝机身正前方，y轴朝机身左侧，z轴竖直向上，初始时各个关节与本体系的方向相同

3.构建浮动基模型

创建基座的物理模型：浮动基座的编号为baseID = 5;

model.addBase(_bodyInertia);  //创建基座

  _nDof = 6;   // 基座的自由度  RPY XYZ
  for (size_t i = 0; i < 6; i++) {
    _parents.push_back(0);      //给基座创建刚体编号 0 0 0 0 0 0 6个0
    _gearRatios.push_back(0);
    _jointTypes.push_back(JointType::Nothing);  // doesn't actually matter
    _jointAxes.push_back(CoordinateAxis::X);    // doesn't actually matter
    _Xtree.push_back(eye6);     //刚体内部的空间变换矩阵
    _Ibody.push_back(zeroInertia);  //刚体的惯性张量
    _Xrot.push_back(eye6);
    _Irot.push_back(zeroInertia);
    _bodyNames.push_back("N/A");
  }
   //基座有用的参数 
  _jointTypes[5] = JointType::FloatingBase;    基座的关节类型
  _Ibody[5] = inertia;                         基座的惯性张量
  _gearRatios[5] = 1;                          基座的减速比
  _bodyNames[5] = "Floating Base";             基座的名称

给基座添加动力学参数

    _v.push_back(zero6);     //关节的空间速度
    _vrot.push_back(zero6);  //转子的空间速度
    _a.push_back(zero6);     //关节的空间加速度
    _arot.push_back(zero6);
    _avp.push_back(zero6);   //连杆质心加速度
    _avprot.push_back(zero6); //连杆转子质心加速度
    _c.push_back(zero6);        //非惯性力 包含科氏力与离心力
    _crot.push_back(zero6); 
    _S.push_back(zero6);         //关节空间到运动空间的速度映射向量
    _Srot.push_back(zero6);
    _f.push_back(zero6);         //关节受到的空间力
    _frot.push_back(zero6);
    _fvp.push_back(zero6);      //连杆质心的空间力
    _fvprot.push_back(zero6);
    _ag.push_back(zero6);     //连杆所受到的重力
    _agrot.push_back(zero6);
    _IC.push_back(zeroInertia); //多刚体的惯性张量
    _Xup.push_back(eye6);      //从父刚体到子刚体的空间变换矩阵
    _Xuprot.push_back(eye6);
    _Xa.push_back(eye6);       //从世界系到当前关节坐标系的空间变换矩阵

给浮动基座添加8个点或者说叫做8个坐标如下图

model.addGroundContactBoxPoints(5, bodyDims);

  addGroundContactPoint(bodyId, Vec3<T>( dims(0),  dims(1),  dims(2))/2);
  addGroundContactPoint(bodyId, Vec3<T>(-dims(0),  dims(1),  dims(2))/2);
  addGroundContactPoint(bodyId, Vec3<T>( dims(0), -dims(1),  dims(2))/2);
  addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1),  dims(2))/2);

  addGroundContactPoint(bodyId, Vec3<T>(dims(0), dims(1), -dims(2)) / 2);
  addGroundContactPoint(bodyId, Vec3<T>(-dims(0), dims(1), -dims(2)) / 2);
  addGroundContactPoint(bodyId, Vec3<T>(dims(0), -dims(1), -dims(2)) / 2);
  addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1), -dims(2)) / 2);

添加关节和连杆：一个电机及其固连的连杆视作一个刚体。

_parents[18]= 0 0 0 0 0 0 5 6 7 5 9 10 5 12 13 5 15 16   自由度与刚体编号的对应关系
前六个零代表浮动基座  后面12个每三个一组代表一条腿的三个刚体的编号

    if (sideSign < 0) {
      model.addBody(_abadInertia.flipAlongAxis(CoordinateAxis::Y),
                    _abadRotorInertia.flipAlongAxis(CoordinateAxis::Y),
                    _abadGearRatio, baseID, JointType::Revolute,
                    CoordinateAxis::X, xtreeAbad, xtreeAbadRotor);     //右侧
    } else {
      model.addBody(_abadInertia, _abadRotorInertia, _abadGearRatio, baseID,
                    JointType::Revolute, CoordinateAxis::X, xtreeAbad,
                    xtreeAbadRotor);                                   //左侧
    }

sideSign表示这个关节是在身体的右侧还是在身体的左侧，-1表示右侧，+1表示左侧，由于abad这个电机与身体刚接，没有相对位移，因此它的编号为_parents[6]=5; 同理还有_parents[9]=_parents[12]=_parents[15]=5;

  _parents.push_back(parent);    //添加刚体编号

  
  _gearRatios.push_back(gearRatio);   //添加刚体关节电机的减速比
  _jointTypes.push_back(jointType);   //添加关节的类型
  _jointAxes.push_back(jointAxis);    //添加关节绕本体坐标系的旋转轴
  _Xtree.push_back(Xtree);            //添加刚体内部的常量空间变换
  _Xrot.push_back(Xrot);              //电机转子的
  _Ibody.push_back(inertia);          //刚体的惯性张量
  _Irot.push_back(rotorInertia);      
  _nDof++;                            //每创建一个刚体就加一个自由度

Mat6<T> xtreeAbad = createSXform(I3, withLegSigns<T>(_abadLocation, legID)); 
abad关节的内部的常量空间变换矩阵

添加hip髋关节和连杆：

    Mat6<T> xtreeHip =
        createSXform(coordinateRotation<T>(CoordinateAxis::Z, T(M_PI)),
                     withLegSigns<T>(_hipLocation, legID));    //刚体内的常量变换矩阵
    Mat6<T> xtreeHipRotor =
        createSXform(coordinateRotation<T>(CoordinateAxis::Z, T(M_PI)),
                     withLegSigns<T>(_hipRotorLocation, legID));    //对应电机转子的常量变换矩阵
    if (sideSign < 0) {
      model.addBody(_hipInertia.flipAlongAxis(CoordinateAxis::Y),
                    _hipRotorInertia.flipAlongAxis(CoordinateAxis::Y),
                    _hipGearRatio, bodyID - 1, JointType::Revolute,
                    CoordinateAxis::Y, xtreeHip, xtreeHipRotor);    //右侧
    } else {
      model.addBody(_hipInertia, _hipRotorInertia, _hipGearRatio, bodyID - 1,
                    JointType::Revolute, CoordinateAxis::Y, xtreeHip,
                    xtreeHipRotor);                左侧
    }
    // add knee ground contact point
    model.addGroundContactPoint(bodyID, Vec3<T>(0, 0, -_hipLinkLength));  //初始时hip关节刚接连杆的末端在hip关节坐标系下的位置。

对应的刚体编号 _parents[7]=6 _parents[10]=9 _parents[13]=12 _parents[16]=15;

添加knee膝盖关节及其连杆：

Mat6<T> xtreeKnee = createSXform(I3, _kneeLocation);
    Mat6<T> xtreeKneeRotor = createSXform(I3, _kneeRotorLocation);
    if (sideSign < 0) {
      model.addBody(_kneeInertia.flipAlongAxis(CoordinateAxis::Y),
                    _kneeRotorInertia.flipAlongAxis(CoordinateAxis::Y),
                    _kneeGearRatio, bodyID - 1, JointType::Revolute,
                    CoordinateAxis::Y, xtreeKnee, xtreeKneeRotor);

      model.addGroundContactPoint(bodyID, Vec3<T>(0, _kneeLinkY_offset, -_kneeLinkLength), true);   //右侧，同理是膝盖刚接连杆末端的坐标 在膝盖关节坐标系下的描述
    } else {
      model.addBody(_kneeInertia, _kneeRotorInertia, _kneeGearRatio, bodyID - 1,
                    JointType::Revolute, CoordinateAxis::Y, xtreeKnee,
                    xtreeKneeRotor);

      model.addGroundContactPoint(bodyID, Vec3<T>(0, -_kneeLinkY_offset, -_kneeLinkLength), true);  //坐标系为当前刚体坐标系
    }

添加重力参数

  Vec3<T> g(0, 0, -9.81);
  model.setGravity(g);

构建浮动基模型的目的是为了求解动力学方程的质量矩阵M, 非惯性和惯性力矩阵，以及接触雅可比矩阵。

利用forwardKinematics()这个函数求出父刚体到子刚体的空间变换矩阵、科氏力加速度，刚体i到世界系的空间变换矩阵这样就可以求得足端在世界坐标系下的位置和速度

   _Xup[5] = createSXform(quaternionToRotationMatrix(_state.bodyOrientation),
                         _state.bodyPosition);   //计算身体从世界系到本体系的运动状态的变换矩阵
  _v[5] = _state.bodyVelocity;   //身体在空间中的六维速度 由角速度和平动速度组成
  for (size_t i = 6; i < _nDof; i++) 
  {
    // joint xform
    Mat6<T> XJ = jointXform(_jointTypes[i], _jointAxes[i], _state.q[i - 6]);    //纯旋转矩阵  刚体的坐标系在关节处
    _Xup[i] = XJ * _Xtree[i];//计算从父刚体到子刚体的运动状态变换矩阵
    _S[i] = jointMotionSubspace<T>(_jointTypes[i], _jointAxes[i]);//关节运动子空间到空间速度的映射向量
    SVec<T> vJ = _S[i] * _state.qd[i - 6];  
    // total velocity of body i
    _v[i] = _Xup[i] * _v[_parents[i]] + vJ;  //子刚体的空间速度

    // Coriolis accelerations
    _c[i] = motionCrossProduct(_v[i], vJ);//子刚体的科氏力加速度
 

  // calculate from absolute transformations
  for (size_t i = 5; i < _nDof; i++) {
    if (_parents[i] == 0) {
      _Xa[i] = _Xup[i];  // float base   世界系到身体的空间变换矩阵
    } else {
      _Xa[i] = _Xup[i] * _Xa[_parents[i]];//世界系到刚体i的空间变换矩阵
    }
  }

  // ground contact points
  //  // TODO : we end up inverting the same Xa a few times (like for the 8
  //  points on the body). this isn't super efficient.
  for (size_t j = 0; j < _nGroundContact; j++) {
    if (!_compute_contact_info[j]) continue;
    size_t i = _gcParent.at(j);   
    Mat6<T> Xai = invertSXform(_Xa[i]);  // from link to absolute  从刚体i到世界系的空间变换矩阵
    SVec<T> vSpatial = Xai * _v[i];//在世界系下的空间速度

    // foot position in world
    _pGC.at(j) = sXFormPoint(Xai, _gcLocation.at(j));    //16个点在世界坐标系下的位置
    _vGC.at(j) = spatialToLinearVelocity(vSpatial, _pGC.at(j)); //16个点在世界系下的速度
  }

计算刚体质心的空间加速度:

由biasAccelerations()计算由重力和非惯性力产生

  _avp[5] << 0, 0, 0, 0, 0, 0;  //基座的空间加速度

  for (size_t i = 6; i < _nDof; i++) {
    // Outward kinamtic propagtion
    _avp[i] = _Xup[i] * _avp[_parents[i]] + _c[i];    //刚体i质心的空间加速度
    
  }

求解接触雅可比

size_t i = _gcParent.at(k);   //i= 5 5 5 5 5 5 5 5 7 8 10 11 13 14 16 17

前面8个点表示的是四个hip电机的中心位置上下对称的点后面八个点分别是四条腿的hip和knee关节刚接连杆的末端位置

 for (size_t k = 0; k < _nGroundContact; k++) {
    _Jc[k].setZero();   //每个都是3x18
    _Jcdqd[k].setZero();  //3x1

    // Skip it if we don't care about it
    if (!_compute_contact_info[k]) continue;

    size_t i = _gcParent.at(k);   //i= 5 5 5 5 5 5 5 5 7 8 10 11 13 14 16 17
    // Rotation to absolute coords
    Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();   //从当前刚体坐标系到世界系的旋转变换矩阵
    Mat6<T> Xc = createSXform(Rai, _gcLocation.at(k)); //从当前刚体坐标系到世界系的空间变换矩阵  

    // Bias acceleration
    SVec<T> ac = Xc * _avp[i];   //刚体i在世界系下的空间加速度
    SVec<T> vc = Xc * _v[i];     //刚体i在世界系下空间速度

    // Correct to classical
    _Jcdqd[k] = spatialToLinearAcceleration(ac, vc);   //工作空间加速度

    // rows for linear velcoity in the world
    D3Mat<T> Xout = Xc.template bottomRows<3>();    //3x6

    // from tips to base
    while (i > 5) {   //i的取值为  7 8 10 11 13 14 16 17 
      _Jc[k].col(i) = Xout * _S[i];
      Xout = Xout * _Xup[i];
      i = _parents[i];
    }
    _Jc[k].template leftCols<6>() = Xout;         //leftcols(6) 前六列 
  }

接触雅可比矩阵3x18的含义雅可比矩阵的行是每条腿在空间中的自由度，hip和knee在空间中有两个自由度，abad电机垂直与hip电机，增加一个维度。雅可比矩阵的前六列是描述浮动基座的，

第6、7、8列是描述右前腿的，第9、10、11l列是描述左前腿的，第12、13、14列是描述右后腿的

第15、16、17列描述左后腿。这些列不是在用一个接触雅可比里面；