刚体动力学算法 Chapter 5 逆动力学

Chapter 5 Inverse Dynamics

逆动力学是指在刚体系统中寻找产生给定加速度所需的力。逆动力学计算在运动控制系统、轨迹设计和优化（例如用于机器人和动画角色）、机械设计以及某些正向动力学算法中都有应用。最后一个应用在第6章中介绍。逆动力学计算可以用以下方程表示：
$$\mathbf{\tau }=\mathrm{ID}(\mathrm{model},\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}),$$

其中 $\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}$ 和 $\mathbf{\tau }$ 分别是广义位置、速度、加速度和力的向量，而 model 是特定刚体系统的系统模型。

本章介绍了一种计算运动树逆动力学的算法。闭环系统的算法将在第8章中出现。运动树的逆动力学计算特别容易，这就是我们为什么从这个问题开始的原因。该算法称为递归牛顿-欧拉算法，它是最简单、最高效的算法。它的计算复杂度为 $O(n)$，这意味着计算量与树中的刚体或关节变量数量呈线性增长。首先介绍基本形式的算法，然后添加实现细节，最后将空间向量版本与原始的3D向量版本进行比较。本章还涵盖算法复杂度的主题，并解释了为什么递归算法如此高效。

5.1 算法复杂度

我们将算法的计算成本定义为其操作数量，即在执行算法过程中执行的算术操作次数。如果算法涉及实数运算（如动力学算法），则习惯上仅计算浮点操作。因此，忽略了整数操作，例如计算数组索引或递增循环变量所需的操作。

算法的计算复杂度是衡量成本如何随问题规模增长的一种指标。我们使用“大O”符号来指定这个数量。如果一个算法的复杂度被描述为 $O\left(n^p\right)$，其中 $n$ 是问题的规模，那么当 $n\to \infty$ 时，成本将与 $n^p$ 成比例增长。更准确地说，如果存在正常数$C_{\min }，C_{\max }$ 和 $n_{\min }$，使得计算成本至少为 $C_{\min }f(n)$，但对于所有 $n>n_{\min }$，不超过 $C_{\max }f(n)$ ，则称算法具有 $O(f(n))$ 复杂度。

表达式 $O(f(n))$ 可以解释为所有在 $n\to \infty$ 时与 $f(n)$ 成比例增长的函数集合。因此，像“算法$X$是 $O(f(n))$ ”这样的说法意味着 $X$ 的成本，表示为 $n$ 的函数，是集合 $O(f(n))$ 的元素。类似地，表达式 $O(f(n))=O(g(n))$ 是一个集合相等，意味着 $g(n)\in O(f(n))$ 且反之亦然。在引用复杂度时，我们选择集合中最简单的函数作为 $O$ 的参数。例如，如果算法的精确成本由多项式 $a_0+a_{1}n+\cdots +a_p{n}^p$ 给出，则与该多项式成比例增长的最简单函数是 $n^p$，因此我们引用复杂度为 $O\left(n^p\right)$ 。通常，只有 $f$ 中增长最快的组成部分才重要。如果 $f$ 收敛到常数，则称 $f$ 为 $O(1)$。

算法的复杂度取决于问题规模的衡量方式。例如，矩阵求逆的复杂度被认为是 $O\left(n^3\right)$ ，因为求逆一个 $n\times n$ 矩阵的成本随着 $n\to \infty$ 而以 $n^3$ 成比例增长。这个数字依赖于 $n$ 是矩阵的维度。如果 $n$ 是矩阵中元素的数量，那么复杂度数字将是 $O\left(n^{1.5}\right)$。更一般地，如果 $m$ 和 $n$ 是问题规模的两种不同衡量方式，并且 $m=g(n)$，那么 $O(f(m))=O(f(g(n)))$。

对于动力学算法，问题的规模是刚体系统的规模。如果系统是一个运动链，那么有两种合理的系统规模衡量方式：刚体数量 $N_B$ 和自由度数量 $n$。如果链中的每个关节至少有一个自由度，则这两个数字满足 $N_B\le n\le 6N_B$，这意味着

$$O\left(N_B^p\right)=O\left(n^p\right).$$

因此，在引用复杂度时，无需区分 $N_B$ 和 $n$。

5.2 递归关系

现代动力学算法是递归的，它们比非递归的前身快几个数量级。它们之所以被称为递归，是因为它们利用递归关系来计算中间结果的序列。正是递归关系的使用导致了它们的高效性；因此，让我们花一点时间来看看这些递归关系是如何工作的。递归关系是一个公式，它根据前面的元素定义了序列中的下一个元素。这个公式连同一组初始值提供了序列的递归定义。例如，斐波那契数列$0,1,1,2,3,5,8,13,\dots$可以通过递归关系 $F_i=F_{i-1}+F_{i-2}$ 和初始值 $F_0=0$ 和 $F_1=1$ 来定义。对于刚体系统，在根据 第4.1.2节 的规则编号的情况下，刚体速度序列 ${\mathbf{v}}_0,{\mathbf{v}}_1,{\mathbf{v}}_2,\dots$可以通过递归关系 ${\mathbf{v}}_i={\mathbf{v}}_{\lambda (i)}+{\mathbf{S}}_i{\dot{\mathbf{q}}}_i$ 和给定的 ${\mathbf{v}}_0$ 的初始值来定义。

让我们来看看使用递归关系和不使用递归关系计算刚体速度的任务。为了简化问题，假设系统是一个无分支的运动链，具有1自由度关节。这意味着 $\lambda (i)=i-1$ ，并且关节 $i$ 上的速度是关节轴向量 $s_i$ 与标量关节速度变量 ${\dot{q}}_i$ 的乘积。刚体速度的递归关系如下：

$${\mathbf{v}}_i={\mathbf{v}}_{i-1}+{\mathbf{s}}_i{\dot{q}}_i\text{\hspace{1em}(5.2)}$$

在初始条件 ${\mathbf{v}}_0=0$ 下，刚体速度的非递归公式为：

$${\mathbf{v}}_i=\sum _{j=1}^i{\mathbf{s}}_j{\dot{q}}_j\text{\hspace{1em}(5.3)}$$

设 $\mathrm{m}$ 和 $\mathrm{a}$ 分别表示标量-向量乘法和向量加法的成本。方程5.2 的一次应用成本为 $\mathrm{m}+\mathrm{a}$，方程5.3 的成本为 $i\mathrm{m}+(i-1)$ a。通过方程5.2 计算前 $n$ 个刚体速度的成本为 $n(\mathrm{m}+\mathrm{a})$，通过方程5.3 的成本为 $\frac{1}{2}(n(n+1)\mathrm{m}+n(n-1)\mathrm{a})$。因此，使用方程5.2 可以得到高效的 $O(n)$ 计算，而方程5.3 导致低效的 $O\left(n^2\right)$ 计算。

这种低效的原因并不难理解。如果我们使用方程5.3 进行计算，那么我们最终需要计算的是
v1=s1q˙1v2=s1q˙1+s2q˙2⋮⋮vn=s1q˙1+s2q˙2+⋯+snq˙n. \begin{aligned} \boldsymbol{v}_{1} & =\boldsymbol{s}_{1} \dot{q}_{1} \\ \boldsymbol{v}_{2} & =\boldsymbol{s}_{1} \dot{q}_{1}+\boldsymbol{s}_{2} \dot{q}_{2} \\ & \vdots \\ & \vdots \\ \boldsymbol{v}_{n} & =\boldsymbol{s}_{1} \dot{q}_{1}+\boldsymbol{s}_{2} \dot{q}_{2}+\cdots+\boldsymbol{s}_{n} \dot{q}_{n} . \end{aligned} v1​v2​vn​​=s1​q˙​1​=s1​q˙​1​+s2​q˙​2​⋮⋮=s1​q˙​1​+s2​