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

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);  // LEFTSpatialInertia<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 XYZfor (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 pointmodel.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 xformMat6<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 transformationsfor (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 itif (!_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 coordsMat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();   //从当前刚体坐标系到世界系的旋转变换矩阵Mat6<T> Xc = createSXform(Rai, _gcLocation.at(k)); //从当前刚体坐标系到世界系的空间变换矩阵  // Bias accelerationSVec<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 worldD3Mat<T> Xout = Xc.template bottomRows<3>();    //3x6// from tips to basewhile (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列描述左后腿。这些列不是在用一个接触雅可比里面；

_Jc[8]表示的是从hip关节刚接连杆末端到浮动基的雅克比（或者说是从knee关节到浮动基的雅可比）

_Jc[9]表示的是从足端到浮动基的雅可比。_Jc[9]是右前腿的接触雅可比，同理_Jc[11]、_Jc[13]、_Jc[15]分别是左前、右后、左后的接触雅可比。

具体推算过程如下：

求解雅克的思想就是牛顿欧拉的内推法，由基座速度，角速度，推出末端的线速度和角速度。