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列描述左后腿。这些列不是在用一个接触雅可比里面;
_Jc[8]表示的是从hip关节刚接连杆末端到浮动基的雅克比(或者说是从knee关节到浮动基的雅可比)
_Jc[9]表示的是从足端到浮动基的雅可比。_Jc[9]是右前腿的接触雅可比,同理_Jc[11]、_Jc[13]、_Jc[15]分别是左前、右后、左后的接触雅可比。
具体推算过程如下:
求解雅克的思想就是牛顿欧拉的内推法,由基座速度,角速度,推出末端的线速度和角速度。