这是一个刚体动力学中的问题，内容参考Abhinandan Jain的Robot and Multibody Dynamics

空间惯量（ The Spatial Inertia）

单个刚体的动能可表示为以下体积积分：

$E = \frac{1}{2}\int _\Omega v^T(x)\rho (x)v(x)d \textrm{v}(x)$

式中， $v(x)$为点x的线速度， $\rho(x)$为在x点处的质量密度， $d \textrm{v}(x)$为包含点x的体积微分。

现在定义刚体上一点 $k$，让 $v(K)$表示该点的空间速度。 刚体上任何其他点x的空间速度 $v(X)$与 $v(K)$有关：

$v(x) = Rv(k)$

其中， $R$为 $k$点到x点的变换矩阵。此时，刚体的动能可以表示为：

$\begin{matrix} E &=& \frac{1}{2} v^T(k) \begin{Bmatrix} R\int _\Omega \rho (x)Rd \textrm{v}(x) \end{Bmatrix}v(k) \\ \\ &=& \frac{1}{2}v^T(k)M(k)v(k) \end{matrix}$

这里的 $M(k)$就是点k的空间惯性，可以写成如下形式：

$M(k)=\begin{pmatrix} J(k) & mp(k)\\ -mp(k) & mI_3 \end{pmatrix} \in \mathbb{R}^{6\times6}$

式中，m为刚体质量， $J(k)$为k点的转动惯量， $p(k)$是一个三维向量

如果k点为质心，则：

$M(k)=\begin{pmatrix} J(k) & 0 \\ 0 & mI_3 \end{pmatrix}$

二、bodyInertia计算（c++实现）

我们首先确定刚体的空间惯性和空间动量，以及它们与刚体上各点的关系

a、(quadruped.cpp)质量，质心，转动惯量

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

$M^{body}_{Rot} = \begin{bmatrix} 0.011253 & 0 & 0\\ 0& 0.036203 & 0 \\ 0 & 0 &0.042673 \end{bmatrix}$

b、从质量，质心，转动惯量计算空间惯量

（MiniCheetah.h /91行）调用：

SpatialInertia<T> bodyInertia(cheetah._bodyMass, bodyCOM,
                                bodyRotationalInertia);

(spatiallnertia.h/30-37行)实现：

  SpatialInertia(T mass, const Vec3<T>& com, const Mat3<T>& inertia) {
    Mat3<T> cSkew = vectorToSkewMat(com);
    _inertia.template topLeftCorner<3, 3>() =
        inertia + mass * cSkew * cSkew.transpose();
    _inertia.template topRightCorner<3, 3>() = mass * cSkew;
    _inertia.template bottomLeftCorner<3, 3>() = mass * cSkew.transpose();
    _inertia.template bottomRightCorner<3, 3>() = mass * Mat3<T>::Identity();
  }

（orientation_tool.h）将质心向量转换为矩阵，

/*!
 * Convert a 3x1 vector to a skew-symmetric 3x3 matrix
 */
template <typename T>
Mat3<typename T::Scalar> vectorToSkewMat(const Eigen::MatrixBase<T>& v) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 3,
                "Must have 3x1 matrix");
  Mat3<typename T::Scalar> m;
  m << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0;
  return m;
}

计算方式，假设传入 $3\times1$向量为 $v = [v_0, v_1, v_2]$，则：

$m = \begin{bmatrix} 0 & -v_2 & v_1\\ v_2 & 0 & -v_0\\ -v_1 & v_0 & 0 \end{bmatrix}$

由于bodyCOM = [0, 0, 0]，最终cSkew形式如下：
$C_{skew} = \begin{bmatrix} 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 &0 \end{bmatrix}$

最后我们来计算惯量矩阵bodyInertia，他是一个 $6\times6$矩阵。

$bodyInertia = \begin{bmatrix} M_{lup} & M_{rup}\\ M_{ldown} & M_{rdown} \end{bmatrix}$

$\begin{matrix} M_{lup} =& M_{int} + m C_{skew}C^T_{skew}\\ M_{rup} =& mC_{skew}\\ M_{ldown}=&mC^T_{skew}\\ M_{rdown}=& mI_3 \end{matrix}$

结果：
$bodyInertia=\begin{bmatrix} 0.011253 & 0 & 0 & 0 & 0 & 0 \\ 0& 0.036203 & 0 & 0 & 0 & 0 \\ 0 & 0& 0.042673 & 0 & 0 & 0\\ 0 & 0 & 0 & 3.3 & 0 & 0\\ 0 & 0 & 0 & 0 & 3.3 & 0\\ 0 & 0 & 0 & 0 & 0 & 3.3 \end{bmatrix}$

测试用代码：

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <vector>

using namespace std;
using Eigen::Matrix;

Matrix<float, 6, 6> calInertia(float mass,
	Matrix<float, 3, 1> bodyCOM, 
	Matrix<float, 3, 3> inertia);

int main()
{	
	float _bodyLength = 0.19 * 2;
	float _bodyWidth = 0.049 * 2;
	float _bodyHeight = 0.05 * 2;
	float mass = 3.3;
	Matrix<float, 3, 1> bodyCOM(0, 0, 0);
	Matrix<float, 3, 1>bodyDims(_bodyLength, _bodyWidth, _bodyHeight);

	
	Matrix<float, 3, 3> bodyRotationalInertia;
	bodyRotationalInertia << 11253, 0, 0, 0, 36203, 0, 0, 0, 42673;
	bodyRotationalInertia = bodyRotationalInertia * 1e-6;

	Matrix<float, 6, 6> bodyInertia = calInertia(mass, bodyCOM, bodyRotationalInertia);

	cout << "bodyDims:\n\n" << bodyDims << "\n\n------------------------------\n";
	cout << "bodyCOM:\n\n" << bodyCOM << "\n\n------------------------------\n";
	cout << "bodyRotationalInertia:\n\n" << bodyRotationalInertia << "\n\n------------------------------\n";
	cout << "bodyInertia:\n\n" << bodyInertia << "\n\n------------------------------\n";

	getchar();
	return 0;
}

Matrix<float, 6, 6> calInertia(float mass, Matrix<float, 3, 1> bodyCOM, Matrix<float, 3, 3> inertia)
{
	Matrix<float, 3, 3> cSkew;
	Matrix<float, 6, 6> _inertia;
	cSkew << 0, -bodyCOM[2], bodyCOM[1], bodyCOM[2], 0, -bodyCOM[0], -bodyCOM[1], bodyCOM[0], 0;
	_inertia.topLeftCorner<3, 3>() = inertia + mass * cSkew * cSkew.transpose();
	_inertia.topRightCorner<3, 3>() = mass * cSkew;
	_inertia.bottomLeftCorner<3, 3>() = mass * cSkew.transpose();
	_inertia.bottomRightCorner<3, 3>() = mass * Matrix<float, 3, 3>::Identity();
	return _inertia;

}