惯性矩阵分解 mj_factorM
在上一节中， 我们参考《刚体动力学算法》中提到的CRB 算法分析了函数 mj_crb，得到了仿真系统的惯性矩阵 \(\boldsymbol{H}\)。 接下来还需要根据作用力 \(\boldsymbol{\tau}\) 和 \(\boldsymbol{C}\) 来求得关节加速度 \(\ddot{\boldsymbol{q}}\)， 即求解方程 \(\boldsymbol{H}\ddot{\boldsymbol{q}} = \boldsymbol{\tau} - \boldsymbol{C}\)。 我们在线性代数中介绍了一些线性方程组的求解方法， 由于惯性矩阵 \(\boldsymbol{H}\) 是对称正定矩阵， 所以我们可以将其分解为\(\boldsymbol{L}\boldsymbol{D}\boldsymbol{L}^T\)的形式。 其中 \(\boldsymbol{L}\) 是一个对角线全为 1 的下三角矩阵，\(\boldsymbol{D}\) 则是一个对角矩阵。
实际上仿真系统中并不是任意两个刚体之间都有连接关系，根据拓扑形式的不同，我们很大概率上能够得到一个稀疏的惯性矩阵。 为了充分利用这种稀疏特性，mujoco 采用了一种 \(\boldsymbol{L}^T\boldsymbol{D}\boldsymbol{L}\) 的分解形式。 这里的 \(\boldsymbol{L}, \boldsymbol{D}\) 仍然是一个下三角矩阵和一个对角矩阵。这种分解形式与 \(\boldsymbol{L}\boldsymbol{D}\boldsymbol{L}^T\) 类似， 但是分解出来的 \(\boldsymbol{L}\) 仍然保持了原矩阵的稀疏特性。
本文，我们将结合刚体动力学算法分析 \(\boldsymbol{L}^T\boldsymbol{DL}\) 分解和函数 mj_factorM 的实现。
1. 稀疏分解算法 LTDL
与 Cholesky 分解 类似的，我们也可以将惯性矩阵分解成 \(\boldsymbol{L}^T\boldsymbol{L}\) 的形式， 其中 \(\boldsymbol{L}\) 是一个下三角矩阵。下式描述的是将一个 \(4 \times 4\) 的对称正定矩阵的分解成\(\boldsymbol{L}^T\boldsymbol{L}\) 的形式。
$$ \underbrace{\begin{bmatrix} a_{11} & a_{21} & a_{31} & a_{41} \\ a_{21} & a_{22} & a_{32} & a_{42} \\ a_{31} & a_{32} & a_{33} & a_{43} \\ a_{41} & a_{42} & a_{43} & a_{44} \end{bmatrix}}_{\boldsymbol{A}} = \underbrace{\begin{bmatrix} l_{11} & l_{21} & l_{31} & l_{41} \\ 0 & l_{22} & l_{32} & l_{42} \\ 0 & 0 & l_{33} & l_{43} \\ 0 & 0 & 0 & l_{44} \end{bmatrix}}_{\boldsymbol{L^T}} \underbrace{\begin{bmatrix} l_{11} & 0 & 0 & 0 \\ l_{21} & l_{22} & 0 & 0 \\ l_{31} & l_{32} & l_{33} & 0 \\ l_{41} & l_{42} & l_{43} & l_{44} \end{bmatrix}}_{\boldsymbol{L}} $$我们将其展开，就得到了如下的形式，由于矩阵 \(\boldsymbol{A}\) 是对称的，所以这里省略了右上角的元素。
$$ \left[ \begin{array}{l} l_{11}l_{11} + l_{21}l_{21} + l_{31}l_{31} + l_{41}l_{41} & & & \\ l_{22}l_{21} + l_{32}l_{31} + l_{42}l_{41} & l_{22}l_{22} + l_{32}l_{32} + l_{42}l_{42} & & \\ l_{33}l_{31} + l_{43}l_{41} & l_{33}l_{32} + l_{43}l_{42} & l_{33}l_{33} + l_{43}l_{43} & \\ l_{44}l_{41} & l_{44}l_{42} & l_{44}l_{43} & l_{44}l_{44} \end{array} \right] = \begin{bmatrix} a_{11} & & & \\ a_{21} & a_{22} & & \\ a_{31} & a_{32} & a_{33} & \\ a_{41} & a_{42} & a_{43} & a_{44} \end{bmatrix} $$显然，按照 LU 和 Cholesky 分解的套路， 我们也可以按照特定的顺序依次求解出矩阵 \(\boldsymbol{L}\) 中的所有元素，只是这里需要总最后一行开始逐行向上求解。 对于最后一行有 \(l_{44} = \sqrt{a_{44}}\)，因此 \(l_{4j} = a_{4j} / l_{44}\)。在此基础上对于倒数第二行有，\(l_{33} = \sqrt{a_{33} - l_{43}l_{43}}\)， \(l_{3j} = \frac{1}{l_{33}}\left(a_{3j} - l_{4j}l_{43}\right)\)。可以从中归纳出如下的两式，用于求解 \(\boldsymbol{L}\)。
$$ l_{ii} = \left(a_{ii} - \sum_{k = n}^{i}l_{ki}^2 \right)^{\frac{1}{2}}, \qquad l_{ij} = \frac{1}{l_{ii}} \left(a_{ij} - \sum_{k = n}^{i}l_{kj}l_{ki} \right) $$求解 \(\boldsymbol{L}\) 对角元素的时候，需要进行一次开方的操作，有损计算效率。所以人们在此基础上又提出了改进的 LTDL 分解，如下的形式， 将矩阵 \(\boldsymbol{A}\) 分解成对角元素全为 1 的下三角矩阵 \(\boldsymbol{L}\) 及其转置和对角矩阵 \(\boldsymbol{D}\) 相乘的形式。
$$ \underbrace{\begin{bmatrix} 1 & l_{21} & l_{31} & l_{41} \\ 0 & 1 & l_{32} & l_{42} \\ 0 & 0 & 1 & l_{43} \\ 0 & 0 & 0 & 1 \end{bmatrix}}_{\boldsymbol{L^T}} \underbrace{\begin{bmatrix} d_1 & & & \\ & d_2 & & \\ & & d_3 & \\ & & & d_4 \end{bmatrix}}_{\boldsymbol{D}} \underbrace{\begin{bmatrix} 1 & 0 & 0 & 0 \\ l_{21} & 1 & 0 & 0 \\ l_{31} & l_{32} & 1 & 0 \\ l_{41} & l_{42} & l_{43} & 1 \end{bmatrix}}_{\boldsymbol{L}} = \underbrace{\begin{bmatrix} a_{11} & a_{21} & a_{31} & a_{41} \\ a_{21} & a_{22} & a_{32} & a_{42} \\ a_{31} & a_{32} & a_{33} & a_{43} \\ a_{41} & a_{42} & a_{43} & a_{44} \end{bmatrix}}_{\boldsymbol{A}} $$整理一下，有:
$$ \begin{equation}\label{ltdt_a} \left[ \begin{array}{l} d_1 + l_{21}l_{21}d_2 + l_{31}l_{31}d_3 + l_{41}l_{41}d_4 & & & \\ l_{21}d_2 + l_{32}l_{31}d_3 + l_{42}l_{41}d_4 & d_2 + l_{32}l_{32}d_3 + l_{42}l_{42}d_4 & & \\ l_{31}d_3 + l_{43}l_{41}d_4 & l_{32}d_3 + l_{43}l_{42}d_4 & d_3 + l_{43}l_{43}d_4 & \\ l_{41}d_4 & l_{42}d_4 & l_{43}d_4 & d_4 \end{array} \right] = \underbrace{\begin{bmatrix} a_{11} & a_{21} & a_{31} & a_{41} \\ a_{21} & a_{22} & a_{32} & a_{42} \\ a_{31} & a_{32} & a_{33} & a_{43} \\ a_{41} & a_{42} & a_{43} & a_{44} \end{bmatrix}}_{\boldsymbol{A}} \end{equation} $$按照特定的顺序依次求解，可以写出 \(\boldsymbol{L}\) 和 \(\boldsymbol{D}\) 中的元素:
$$ \begin{equation}\label{dilij} d_{i} = a_{ii} - \sum_{k = n}^{i}l_{ki}^2 d_k, \qquad l_{ij} = \frac{1}{d_i} \left(a_{ij} - \sum_{k = n}^{i}l_{ki} d_k l_{kj} \right) \Longrightarrow d_{i}l_{ij} = a_{ij} - \sum_{k = n}^{i}l_{ki} d_k l_{kj} \end{equation} $$2. LTDL 的实现逻辑
在 mujoco 中对稀疏惯性矩阵进行 LTDL 分解之后得到的\(\boldsymbol{L}\) 和 \(\boldsymbol{D}\) 都被保存在同一个稀疏矩阵结构中。 因为下三角矩阵 \(\boldsymbol{L}\) 的对角线元素全为 1，所以用稀疏矩阵结构的对角线元素保存 \(\boldsymbol{D}\) 并不会有什么歧义。
mujoco 所采用的分解算法实际上是一个原地分解的方法。右图以一个 \(7 \times 7\) 的惯性矩阵为例，描述了 LTDL 的分解方法。整个计算过程是从最后一行开始向上迭代的， 对于每个 \(k\) 都会分三个区域对原惯性矩阵更新。
区域 1: 第 k 行的对角线元素。它并不需要特别的计算。对于一个\(n \times n\) 的惯性矩阵而言，当 \(k = n\) 时，根据式(\(\ref{ltdt_a}\)) 的关系，有 \(d_n = a_{nn}\)。 当 \(k < n\) 时，由于区域3的累加项已经提前更新，迭代到第 k 行时，对应元素中保存的值正是满足式 (\(\ref{dilij}\)) 的对角元素 \(d_k\)。
区域 2: 第 k 行非对角线的非零元素的更新。它们只需要除以区域1的值。这也是得益于区域3的累加项提前更新，使得迭代到第 k 行时， 对应元素中保存的值正是满足式 (\(\ref{dilij}\)) 的 \(d_kl_{kj}\)。为了支持区域3的原地更新，右图中将该区域的计算拆成了 2a 和 2b 两部分，先在 2a 中用一个临时变量保存结果， 完成区域3的更新之后，由 2b 将临时变量写到原矩阵中。
区域 3: 更新其它\(< k\)的行的累加项。需要按照式(\(\ref{dilij}\))将\(l_{ki}d_kl_{kj}\)累加到其它于第\(k\)行有连接关系的行列上， 可以理解为共同支撑了刚体 \(k\) 的关节\(i,j\)的累加项。
3. mj_factorM 的实现
函数 mj_fwdPosition 通过调用 mj_factorM 完成惯性矩阵的稀疏分解。该函数实际只是一层封装，具体的分解任务是由 mj_factorI 完成的。 所传递的参数中，mjData.qM 是保存惯性矩阵的稀疏数据结构，mjData.qLD 则是于 qM 一样结构的矩阵，用于保存分解后的下三角和对角矩阵。 mjData.qLDiagInv 和 mjData.qLDiagSqrtInv 则分别记录了对角矩阵各元素的倒数和倒数的开方。
void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv, d->qLDiagSqrtInv);
TM_ADD(mjTIMER_POS_INERTIA);
}
下面是函数 mj_factorI 的代码片段，在该函数的一开始，主要是第 6 行将 mjData.qM 中的惯性矩阵拷贝到 mjData.qLD 中。
mjModel.nM 记录了惯性矩阵中非零元素数量，即数组 mjData.qM 和 mjData.qLD 的大小。
mjModel.dof_Madr对应惯性矩阵中的每一行，记录着各行中非零元素在mjData.qM中的位置索引。
mjModel.dof_parentid描述的是运动树中各个关节的父关节索引。mjModel.nV则记录了模型中自由度的数量。
void mj_factorI(const mjModel* m, mjData* d,
const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv, mjtNum* qLDiagSqrtInv) {
// local copies of key variables
int* dof_Madr = m->dof_Madr; int* dof_parentid = m->dof_parentid;
int nv = m->nv;
mju_copy(qLD, M, m->nM);
接下来，在一个 for 循环中从最后一行开始，逐行向上遍历 qLD。首先根据 dof_Madr 获取第 k 行的起始元素索引，保存在局部变量 Madr_kk 中，它实际就是惯性矩阵中第k行的对角元素， 即区域1。之后的各个元素对应的就是区域2，第k行中非对角线的非零元素，所以这里用局部变量 Madr_ki 记录。i 则是对应 Madr_ki 的关节索引。
for (int k=nv-1; k >= 0; k--) {
// 省略对角元素非正数的检查
int Madr_kk = dof_Madr[k]; int Madr_ki = Madr_kk + 1; int i = dof_parentid[k];
然后就在一个 while 循环中完成三个区域的更新。首先用一个局部变量 tmp 记录区域2的更新值。然后通过函数 mju_addToScl 完成区域3的累加。最后将 tmp 写到区域2中完成更新。 第15行中的两条语句是在查询关节 i 的父关节，用来驱动完成第k行的遍历
while (i >= 0) {
mjtNum tmp = qLD[Madr_ki] / qLD[Madr_kk]; // tmp = M(k,i) / M(k,k)
// 省略 cnt 的计算，cnt 表示关节 k 的祖先数量
mju_addToScl(qLD+dof_Madr[i], qLD+Madr_ki, -tmp, cnt); // M(i,j) -= M(k,j) * tmp
qLD[Madr_ki] = tmp;
i = dof_parentid[i]; Madr_ki++;
} }
上述的 for 循环结束实际就已经完成了惯性矩阵的稀疏分解。下面的 for 循环只是用来记录对角矩阵元素的倒数和开方的倒数，用来方便后续计算。
for (int i=0; i < nv; i++) {
mjtNum qLDi = qLD[dof_Madr[i]];
qLDiagInv[i] = 1.0/qLDi;
if (qLDiagSqrtInv)
qLDiagSqrtInv[i] = 1.0/mju_sqrt(qLDi);
} }