刚体动力学算法Chapter 4 刚体系统建模 Modelling Rigid Body Systems

Chapter 4 刚体系统建模 Modelling Rigid Body Systems

刚体系统是由组件部分组成的。这些组件包括刚体、关节和各种力元素。关节负责系统中的运动约束，因此我们广义地将术语“关节”解释为两个刚体之间任何可能的运动关系。在本章中，我们将从组件部分的角度来描述刚体系统的方法。特别是，我们试图描述仅由刚体和关节组成的系统。这样的描述可以称为系统模型，因为它描述的是系统本身，而不是其行为的某个方面。

系统模型的作用可以如下理解。假设我们希望在计算机上计算给定刚体系统的动力学。基本上有两种方法：

1. 推导给定系统的符号运动方程，并将这些方程转化为计算机代码；或者
2. 构建一个描述给定系统的系统模型，并将其作为参数提供给

(a) 基于模型的动力学计算程序；或者

(b) 基于模型的自动方程（和代码）生成器。

第二种方法更可取，部分原因是构建和修改系统模型比手工推导运动方程要简单得多，部分原因是基于模型的程序可以在被大量使用之前进行彻底的调试和优化。本书中的动力学算法是基于模型的，并且期望得到一个类似于本章描述的系统模型。方法2(b)在第10.4节中进行了讨论。

图4.1：两个刚体系统的连通性图

4.1 连通性

系统模型必须包含以下数据：组件部分的描述以及它们之间如何连接的描述。后者被称为系统的拓扑结构或连通性，可以通过连通性图的形式来表示，具有以下属性：

1. 节点代表刚体。
2. 弧代表关节。
3. 恰好有一个节点代表固定刚体，称为基座。所有其他节点代表移动刚体。
4. 图是无向的。
5. 图是连通的（即，任意两个节点之间存在路径）。

术语“移动刚体”包括任何能够移动的刚体，无论实际上是否移动。因此，通过运动约束被固定的刚体也算作是移动刚体。除非另有说明，“图”和“连通图”应被理解为“无向图”。

图4.1 中显示了一些连通性图的例子。第一个是一个简单的平面机构，称为曲柄滑块连杆机构。它由三个移动刚体（B1到B3）、一个固定基座、三个旋转关节（J1 到 J3）和一个平移关节（J4）组成。刚体 B1（曲柄）的旋转使得 B3 相对于基座来回滑动。在连通性图中，每个节点代表一个刚体，每个弧代表一个关节。通常需要对至少一些节点和弧进行标记，以便清楚地显示图与原始机构之间的对应关系。此外，有时为了方便视觉识别，我们还会将基座节点与其他节点区分开来，通过给基座节点标记一个白色圆点来实现。请注意，机构中的刚体通常称为连杆。

第二个例子是一颗轨道上的卫星。这个卫星本身由两个移动刚体和一个关节组成。但是，我们不能直接从这个双刚体系统构建连通性图，因为 规则3 要求必须有一个节点表示固定基座。因此，我们添加了第三个刚体-在这种情况下是下方的行星作为固定基座。添加了第三个刚体后，规则5 要求我们还必须添加第二个关节，以便将行星连接到其他两个刚体之一。由于卫星和行星之间没有物理连接，我们使用了一种特殊的关节，它允许六个自由度运动-六自由度关节。该关节不施加运动约束，因此对系统没有物理影响。它的作用是定义额外的关节变量，以便系统整体的位置和速度变量可以从它所包含的关节的位置和速度变量中提取出来。在这种情况下，卫星具有总共 $6+n_s$ 个自由度，其中 $n_s$ 是卫星上关节允许的自由度。因此，我们需要 $6+n_s$ 个速度变量，而这正好是两个关节定义的数量。

通过一个六自由度关节连接到固定基座的刚体称为浮动基座。在这个例子中，卫星的刚体被选择作为浮动基座，但我们也可以选择天线作为浮动基座。

4.1.1 运动树和回路 Kinematic Trees and Loops

如果一个图中任意两个节点之间存在唯一的路径，那么该图就是一个拓扑树。如果刚体系统的连通图是一个拓扑树，那么我们称该系统为运动树。图4.1中的卫星就是一个运动树，但曲柄滑块连杆机构不是。运动树具有特殊的性质，可以方便地高效计算它们的动力学。

设 $G$ 是任意的连通图。$G$ 的一个生成树，记为 $G_t$，是 $G$ 的一个子图，包含 $G$ 中所有的节点，并包含 $G$ 中的任意子集的弧，使得 $G_t$ 是一个拓扑树。每个连通图至少有一个生成树。如果 $G$ 本身就是一棵树，那么 $G_t=G$；否则，$G_t$ 是 $G$ 的一个真子图，并且不唯一。图4.2 展示了一些生成树的例子。

一个有 $n$ 个节点的拓扑树有 $n-1$ 条弧。因此，如果 $G$ 包含 $n_n$ 个节点和 $n_a$ 条弧，那么 $G_t$ 将包含 $n_n$ 个节点和 $n_n-1$ 弧，意味着在 $G$ 中将会有 $n_a-n_n+1$ 条弧不在 $G_t$ 中。这些不在拓扑树中的非树弧被称为弦。尽管 $G$ 中弦的数量是固定的，但在生成树 $G_t$ 的选择下，作为弦的弧的集合会有所不同。这种变化在 图4.2 中有所体现。（我们通过虚线来区分弦和树弧。）

一个回路是图中的闭合路径。树中没有回路，所以图中的每个回路都必须经过至少一条弦。我们特别关注那些只经过一条弦的回路。对于图中的每条弦，总有且只有一个这样的回路，但它的路径取决于生成树的选择。例如，图4.2中的图形有两条弦和三个回路。对于任何生成树的选择，其中两个回路是单弦回路，第三个回路（未显示）经过两条弦。单弦回路的路径由它经过的一条弦以及生成树中连接该弦两个节点之间的唯一路径组成。

连通图中的每个回路都定义了一个运动回路。然而，我们只对独立的运动回路感兴趣，而不是所有的运动回路。这是在制定运动方程时必须考虑的最小回路集合。单弦回路的重要性在于它们确定了独立运动回路的集合。如果我们将刚体系统描述为具有$n$个运动回路，那么实际上意味着它具有$n$个独立的运动回路。

考虑一个由固定基座、$N_B$ 个运动刚体和 $N_J$ 个关节组成的刚体系统，其中后者包括为了确保连通图连通而添加的任何 6自由度关节。设 $G$ 是该系统的连通图，$G_t$ 是 $G$ 的一棵生成树。这两个图都包含 $N_B+1$ 个节点。然而，$G$ 将包含 $N_J$ 条弧，而$G_t$ 只包含 $N_B$ 条，因此系统中弦的数量为 $N_J-N_B$。系统中独立运动回路的数量 $N_L$ 等于 $G$ 中的弦的数量，因此由下式给出：

$$N_L=N_J-N_B\text{\hspace{1em}(4.1)}$$

将该公式应用于 图4.1 中的曲柄滑块连杆机构可以发现它有一个运动回路。

如果一个系统具有运动回路，则被称为闭环系统，否则为运动树。与弦相关联的关节被称为回路关节或闭环关节，而那些属于生成树的关节被称为树关节。因此，刚体系统具有$N_B$个树关节和$N_L$个回路关节。

4.1.2 编号

刚体和关节可以用各种方式进行标识，但最方便的方法是进行编号。我们将使用一种适合描述和实施动力学算法的编号方案。根据这个方案，刚体从0到$N_B$编号，关节从1到$N_J$编号，并且根据以下规则分配编号。

1.选择一个生成树 $G_t$ 。
2.将代表固定基座的节点赋予编号 0，并将该节点定义为 $G_t$ 的根节点。
3.以任意顺序将剩余节点从 1 到 $N_B$ 编号，使得每个节点的编号都比它在 $G_t$ 中的父节点高。
4.将 $G_t$ 中的弧从 1 到 $N_B$ 编号，使得弧$i$连接节点$i$和其父节点。
5.以任意顺序将所有剩余的弧从 $N_B+1$ 到 $N_J$ 编号。
6.每个刚体得到与其节点相同的编号，每个关节得到与其弧相同的编号。

这个方案称为常规编号。如果这些规则应用于无分支的运动学树（树中没有节点有多于一个子节点），那么刚体和关节将从基座开始按顺序编号，如图4.3所示。在所有其他情况下，编号不唯一。例如，图4.4显示了图4.1中曲柄滑块连杆机构的所有可能编号。这八种可能性来自于四个可能的生成树。两棵树没有分支，因此只有一种编号方式。这两种方式出现在图的左上角和右下角。另外两棵树在根节点处有分支，它们各自有三种编号方式。

4.1.3 Joint Polarity

每个关节连接两个刚体。我们将其中一个刚体称为关节的前驱，另一个称为关节的后继。关节本身被认为是从前驱到后继的连接。这个术语的目的有两个：定义关节和刚体变量之间的关系，并指定关节在系统中连接的方式。例如，关节上的速度定义为后继相对于前驱的速度，关节上传递的力定义为从前驱传递到后继的力。

确定关节在两个刚体之间连接的方式被称为它的极性。我们可以通过在连接图上的弧上放置箭头来确定关节的极性，箭头指向从前驱到后继。图4.5和图4.6显示了一些示例。为方便起见，我们采用以下约定关于树关节的极性：树关节的正常极性是从父节点连接到子节点。除非另有说明，所有树关节都被认为具有这种极性。

一些关节在物理上是对称的，意味着后继相对于前驱的运动与前驱相对于后继的运动在性质上是相同的。没有这个性质的关节是不对称的。改变对称关节的极性对刚体系统没有实质性影响，但改变不对称关节的极性会产生影响。例如，如果两个刚体通过球接头连接，它是对称的，那么无论哪个刚体有球，哪个有座，实际上并没有什么区别——如果我们交换它们的位置，运动仍然是相同的。相反，如果两个刚体由齿轮齿条连接，它是不对称的，那么哪个刚体具有齿条哪个具有齿轮就很重要。因此，不对称关节的极性由物理系统决定，但对称关节的极性不受限制，可以在系统模型中自由选择。

4.1.4 表示方法 Representation

在计算机中，有许多方法可以表示连接图。其中最简单的一种方法是使用一对数组 $p$ 和 $s$ 记录关节的前驱和后继刚体编号，其中 $p(i)$ 和 $s(i)$ 分别表示第 $i$ 个关节的前驱和后继刚体编号。这种方法在 图4.5 中进行了说明，图中重现了 图4.4 中的两个连接图，并添加了箭头以显示每个关节的极性。

通过 $p$ 和 $s$ ，我们可以完整描述连接图，并可以计算其他有用的量。其中之一是父节点数组 $\lambda$，用于标识跨度树中每个刚体的父节点：$\lambda (i)$ 表示刚体 $i$ 的父节点。根据编号方案的规则3和规则4，树关节 $i$ 连接在刚体 $i$ 和 $\lambda (i)$ 之间，且$\lambda (i)<i$，因此有

$$\lambda (i)=\min (p(i),s(i)).\left(1\le i\le N_B\right)\text{\hspace{1em}(4.2)}$$

如果树关节满足 $s(i)=i$ ，则它连接从父节点到子节点，并称为树中的正向关节。否则，它连接从子节点到父节点，并称为树中的反向关节。图4.5 中的树关节都是正向的（箭头指向远离根节点）。图4.6中的树关节也都是正向的，除了关节4。此图中省略了树关节编号和正向箭头：前者可以从编号方案中推导出来，后者是因为我们将“正向”视为树关节的标准方向。

还有三个量可以提供关于运动学树连接性的有用信息：对于除了基底之外的任何刚体 $i$ ，$\kappa (i)$ 是连接刚体 $i$ 和基底之间的所有树关节的集合；对于任何刚体 $i$（包括基底），$\mu (i)$ 是刚体 $i$ 的子节点集合，$\nu (i)$ 是以刚体 $i$ 为起点的子树中的刚体集合。它们分别被称为支撑集、子节点集和子树集。（如果一棵树关节位于某个刚体与基底之间的路径上，则称该关节支撑该刚体。）$\nu (i)$ 也可以视为由关节$i$支撑的所有刚体的集合。对于 图4.6 中的跨度树，支撑集、子节点集和子树集如下：

κ ( 1 ) = { 1 } μ ( 0 ) = { 1 , 4 } ν ( 0 ) = { 0 , 1 , 2 , … , 8 } κ ( 2 ) = { 1 , 2 } μ ( 1 ) = { 2 , 3 } ν ( 1 ) = { 1 , 2 , 3 , 5 } κ ( 3 ) = { 1 , 3 } μ ( 2 ) = { } ν ( 2 ) = { 2 } ⋮ ⋮ ⋮ κ ( 8 ) = { 4 , 8 } μ ( 8 ) = { } ν ( 8 ) = { 8 } \begin{array}{lll} \kappa(1)=\{1\} & \mu(0)=\{1,4\} & \nu(0)=\{0,1,2, \ldots, 8\} \\ \kappa(2)=\{1,2\} & \mu(1)=\{2,3\} & \nu(1)=\{1,2,3,5\} \\ \kappa(3)=\{1,3\} & \mu(2)=\{\} & \nu(2)=\{2\} \\ \vdots & \vdots & \vdots \\ \kappa(8)=\{4,8\} & \mu(8)=\{\} & \nu(8)=\{8\} \end{array} κ(1)={1}κ(2)={1,2}κ(3)={1,3}⋮κ(8)={4,8}​μ(0)={1,4}μ(1)={2,3}μ(2)={}⋮μ(8)={}​ν(0)={0,1,2,…,8}ν(1)={1,2,3,5}ν(2)={2}⋮ν(8)={8}​

显然，$\kappa ,\lambda ,\mu$ 和 $\nu$ 都具有许多简单的性质，这些性质可以从它们的定义中得出。例如，$\mu (i)\subseteq \nu (i),j\in \kappa (i)\Rightarrow i\in \nu (j)$，等等。一个不太显然但非常有用的性质是下面的恒等式：

$$\sum _{i=1}^{N_B}\sum _{j\in \kappa (i)}(\cdots )=\sum _{j=1}^{N_B}\sum _{i\in \nu (j)}(\cdots ).\text{\hspace{1em}(4.3)}$$

左边是对所有满足关节$j$支撑刚体$i$的$i,j$对的求和，而右边也是对所有满足关节$j$支撑刚体$i$的$i,j$对的求和，因此两者相等。

例4.1 展示了$\lambda$ 在动力学算法中的作用，以下是一个简单的例子。假设我们希望计算刚体系统中每个刚体的速度，已知树关节间的速度。设 ${\mathbf{v}}_i$ 和 ${\mathbf{v}}_{\mathrm{J}i}$ 分别为刚体 $i$ 的速度和过关节 $i$ 的速度，后者是相对于父节点的速度（即，关节是正向的）。现在，刚体 $i$ 的速度可以递归地定义为其父节点的速度与连接它到父节点的关节上的速度之和；因此，计算刚体速度的公式为：

$${\mathbf{v}}_i={\mathbf{v}}_{\lambda (i)}+{\mathbf{v}}_{\mathrm{J}i}.\left({\mathbf{v}}_0=0\right)$$

计算这些速度的算法如下：

$$\begin{array}{rl}& {\mathbf{v}}_0=0\\ & \text{ for }i=1\text{ to }{N}_B\text{ do }\\ & {\mathbf{v}}_i={\mathbf{v}}_{\lambda (i)}+{\mathbf{v}}_{\mathrm{J}i}\\ & \text{ end }\end{array}$$

性质 $\lambda (i)<i$ 保证了在使用 ${\mathbf{v}}_{\lambda (i)}$ 之前它总是被计算出来的。另一种表达刚体速度的方式是：

$${\mathbf{v}}_i=\sum _{j\in \kappa (i)}{\mathbf{v}}_{\mathrm{J}j}\text{\hspace{1em}(4.4)}$$

例4.2 例子2.3 介绍了一个称为刚体雅可比矩阵的矩阵，它将每个刚体的空间速度表示为关节速度变量的函数。如果 ${\mathbf{J}}_i$ 是刚体 $i$ 的雅可比矩阵，则有：
$${\mathbf{v}}_i={\mathbf{J}}_i\dot{\mathbf{q}}$$

方程2.18给出了 ${\mathbf{J}}_i$ 的一个公式，适用于非分支的运动链的特殊情况，现在我们来得到一般的公式。设关节 $j$ 上的速度为 ${\mathbf{S}}_j{\dot{\mathbf{q}}}_j$，并定义 ${ϵ}_{ij}$ 如下：

$${ϵ}_{ij}=\{\begin{array}{ll}1& \text{ if }j\in \kappa (i)\\ 0& \text{ otherwise. }\end{array}\text{\hspace{1em}(4.5)}$$

因此，如果关节 $j$ 支撑刚体 $i$，则 ${ϵ}_{ij}$ 非零。刚体 $i$ 的速度可以表示如下：

$${\mathbf{v}}_i=\sum _{j\in \kappa (i)}{\mathbf{S}}_j{\dot{\mathbf{q}}}_j=\sum _{j=1}^{N_B}{ϵ}_{ij}{\mathbf{S}}_j{\dot{\mathbf{q}}}_j=\left[\begin{array}{lll}{ϵ}_{i1}{\mathbf{S}}_1& \cdots & {ϵ}_{i{N}_B}{\mathbf{S}}_{N_B}\end{array}\right]\left[\begin{array}{c}{\dot{\mathbf{q}}}_1\\ ⋮\\ {\dot{\mathbf{q}}}_{N_B}\end{array}\right].$$

因此，刚体雅可比矩阵的一般公式为：

$${\mathbf{J}}_i=\left[\begin{array}{llll}{ϵ}_{i1}{\mathbf{S}}_1& {ϵ}_{i2}{\mathbf{S}}_2& \cdots & {ϵ}_{i{N}_B}{\mathbf{S}}_{N_B}\end{array}\right]\text{\hspace{1em}(4.6)}$$

4.2 几何性质 Geometry

如果两个刚体通过关节连接在一起，那么它们的相对运动是受限制的。完整描述约束需要了解两个方面：关节允许的运动以及关节相对于每个刚体的位置。前者由关节模型描述，后者由系统的几何性质描述。

刚体系统的几何模型描述了连接的位置。特别是，它描述了每个刚体上每个关节的相对位置。图4.7 显示了一个刚体系统的几何模型，包括一个固定基座 $B_0$，三个移动刚体 $B_1$ 到 $B_3$ ，三个树状关节 $J_1$ 到 $J_3$，以及一个回路关节 $J_4$。图中的虚线仅表示每个关节的连接性，而不表示其确切物理位置。

每个移动刚体内嵌有一个坐标系，为该刚体定义了一个局部坐标系统。这些坐标系标记为 $F_1$ 到 $F_3$，它们定义的坐标系称为刚体坐标或链接坐标。还有一个嵌入在基座中的坐标系 $F_0$，它为整个刚体系统定义了一个绝对、参考或基座坐标系。刚体 $B_i$ 的位置定义为 $F_i$ 相对于 $F_0$ 的位置，可以用坐标变换 ${}^i{\mathbf{X}}_0$ 来表示。

这幅图还展示了几个坐标系，命名形式为 $F_{i,j}$，其中 $i$ 指代一个刚体，$j$ 指代一个关节，描述了关节 $j$ 在刚体 $i$ 中的位置。每个树状关节 (tree joint) $i$连接从坐标系 $F_{\lambda (i),i}$ 到坐标系 $F_i$（如果关节是反向的，则相反），而每个回路关节 $i$ 连接从 $F_{p(i),i}$ 到 $F_{s(i),i}$ 。因此，总共有 $N_B+2N_L$ 个定位坐标系：每个树状关节一个，每个回路关节两个。坐标系 $F_{i,j}$ 相对于 $F_i$ 的位置由变换 ${}^{i,j}{\mathbf{X}}_i$ 给出，它是一个常数。这些变换，或者可以计算它们的数据集，定义了刚体系统的几何模型。

为了方便起见，我们定义了一个树状变换(tree transforms)数组 ${\mathbf{X}}_{\mathrm{T}}$，使得 ${\mathbf{X}}_{\mathrm{T}}(i)={}^{\lambda (i),i}{\mathbf{X}}_{\lambda (i)}$。这些是树状关节的定位变换，该数组提供了跨度树的完整几何描述。我们还定义了数组 ${\mathbf{X}}_{\mathrm{P}}$ 和 ${\mathbf{X}}_{\mathrm{S}}$，每个数组从 1 到 $N_L$ 进行索引，使得对于每个回路关节 $i$，${\mathbf{X}}_{\mathrm{P}}\left(i-N_B\right)={}^{p(i),i}{\mathbf{X}}_{p(i)}$，且 ${\mathbf{X}}_{\mathrm{S}}\left(i-N_B\right)={}^{s(i),i}{\mathbf{X}}_{s(i)}$。这些是回路关节的定位变换。

最后，每个关节定义了一个关节变换 ${\mathbf{X}}_{\mathrm{J}i}$，它是关节位置变量的函数。这些变换将在第4.4节中讨论。如图所示，如果 $i$ 是树状关节，则 $\boldsymbol{X}_{\mathrm{J} i} $ 等于 ${}^i{\mathbf{X}}_{\lambda (i),i}$，如果 $i$ 是回路关节，则 ${\mathbf{X}}_{\mathrm{J}i}$ 等于 ${}^{s(i),i}{\mathbf{X}}_{p(i),i}$。例如，假设我们希望计算运动树中每个刚体的位置；也就是说，我们希望计算每个刚体的 ${}^i{\mathbf{X}}_0$。完成此计算的算法如下所示：

在上述代码片段中，${\mathbf{X}}_{\mathrm{J}}$ 是一个局部变量，jtype $(i)$是描述关节$i$的数据结构，jcalc 是一个从关节描述和关节位置变量向量计算关节变换的函数。关于 jtype $(i)$ 和 jcalc 的详细信息可参考第4.4节。

4.3 Denavit-Hartenberg Parameters

通常情况下，为了指定一个坐标系相对于另一个坐标系的位置，需要六个参数。然而，对于某些刚体系统，可以以一种特定的方式定位刚体坐标系，从而只需要更少的参数。Denavit-Hartenberg（DH）方法是其中一种方法，最初由Denavit和Hartenberg（Denavit和Hartenberg，1955）描述，每个关节只需要四个参数。该方法利用了某些常见关节类型可以用空间中的一条直线来描述，并且只需要四个参数来定位该直线。

图4.8展示了如何将Denavit-Hartenberg（DH）方法应用于表示机器人手臂或类似装置的非分支运动树。该系统的重要几何特征有：

• 基坐标系
• $n$个关节轴
• 嵌入在末端链接中的末端执行器坐标系

末端执行器坐标系用于机器人正确执行定位命令，但在动力学计算中并不需要。根据以下规则放置DH坐标系。

1. 轴$z_0$和$z_{n+1}$与基坐标系和末端执行器坐标系的$z$轴对齐。

2. 轴$z_1$到$z_n$与$n$个关节轴对齐，使得$z_i$与关节$i$的轴对齐。（如果关节$i$是移动关节，则$z_i$可以是任何具有正确方向的轴；但在可行的情况下，选择与$z_{i-1}$相交的轴是合理的。）

3. 轴$x_i$是$z_i$和$z_{i+1}$之间的公共垂线，从$z_i$指向$z_{i+1}$。如果$z_i$和$z_{i+1}$相交，则$x_i$的方向存在$180^{\circ }$的歧义。如果$z_i$和$z_{i+1}$平行，则公共垂线的位置不确定，合理的选择是与$x_{i-1}$相交。

4. 刚体坐标系$F_i$由轴$x_i$、$y_i$和$z_i$定义，其中$y_i$从$x_i$和$z_i$派生，形成一个右手坐标系。

放置了坐标系之后，它们的相对位置由以下DH参数描述：

• $d_i$ 是从$x_{i-1}$沿$z_i$测量到$x_i$的距离。
• ${\theta }_i$ 是从$x_{i-1}$到$x_i$绕$z_i$测量的角度。
• $a_i$ 是从$z_{i-1}$沿$x_{i-1}$测量到$z_i$的距离。定义$x_{i-1}$暗示着$a_i\ge 0$。
• ${\alpha }_i$ 是从$z_{i-1}$到$z_i$绕$x_{i-1}$测量的角度。

所有参数$a_i$和${\alpha }_i$都是常数，但一些参数$d_i$和${\theta }_i$用作关节变量。具体来说，如果关节$i$是旋转关节，则${\theta }_i$是其关节变量；如果关节$i$是移动关节，则$d_i$是其关节变量；如果关节$i$是圆柱关节，则${\theta }_i$和$d_i$都是关节变量；如果关节$i$是螺旋关节，且螺距为$h_i$，则${\theta }_i$是关节变量，并且从$x_{i-1}$到$x_i$的距离定义为$d_i+h_i{\theta }_i$。如果每个关节都是旋转关节，则DH参数为图4.8中非分支树的以下几何模型定义：

$$\begin{array}{ll}i=1:& {\mathbf{X}}_{\mathrm{T}}(1)=\mathrm{xlt}\left({\left[\begin{array}{lll}{a}_1& 0& d_1\end{array}\right]}^{\mathrm{T}}\right)\mathrm{rotx}\left({\alpha }_1\right)\mathrm{rotz}\left({\theta }_0\right)\mathrm{xlt}\left({\left[\begin{array}{lll}0& 0& d_0\end{array}\right]}^{\mathrm{T}}\right)\\ i>1:& {\mathbf{X}}_{\mathrm{T}}(i)=\mathrm{xlt}\left({\left[\begin{array}{lll}{a}_i& 0& d_i\end{array}\right]}^{\mathrm{T}}\right)\mathrm{rotx}\left({\alpha }_i\right)\end{array}$$

请参考表2.2中有关平移和旋转函数的定义。此外，请参阅附录中的$\S$ A.4，了解螺旋变换的详细信息。

对于具有$n$个关节的系统，需要$4n+6$个DH参数来描述，其中至少有$n$个关节变量而非参数。然而，最后四个参数仅用于定位末端执行器的坐标系，在不需要这些信息的情况下可以省略。如果基坐标系可以选择与$z_1$对齐，则还可以省略多达五个参数。

Denavit-Hartenberg方法的基本形式适用于任何刚体系统，其中每个关节可以由关节轴来描述，并且拓扑结构可以是非分支的运动树或单个封闭的运动回路。然而，该方法可以根据Khalil和Dombre（2002）的描述适应更广泛的系统类别。有关DH参数的描述可在各种教材中找到，例如Angeles（2003）和Craig（2005）。符号细节在不同的作者之间可能有所不同，特别是关于$x_i$、$a_i$和${\alpha }_i$的编号。这里使用的符号遵循Springer机器人手册（Siciliano和Khatib，2008）。

4.4 Joint Models 关节模型

关节可以被看作是两个笛卡尔坐标系之间的运动约束，一个嵌入在关节的后继体中，另一个嵌入在其前驱中。我们将分别称这些坐标系为 $F_s$ 和 $F_p$ 。关节模型的目的是提供计算 $F_s$ 相对于 $F_p$ 的运动所需的信息，作为关节变量的函数。

关节允许的运动类型取决于其关节类型。例如，旋转关节允许围绕单个轴进行纯旋转，而球面关节允许围绕特定点进行任意旋转。因此，我们需要为每种类型的关节设计一个关节模型。关节模型的内容包括用于计算特定量的数据和公式的集合，例如：关节变换$\left({}^s{\mathbf{X}}_p\right)$、关节速度$\left({\mathbf{v}}_{\mathrm{J}}\right)$、关节的运动子空间$(\mathbf{S})$等等。表4.1显示了几种常见关节类型的公式，图4.9显示了其中一些关节的几何形状。

让我们更详细地研究一个模型。表4.1 中的第一个条目描述了一个允许 $F_s$ 相对于 $F_p$ 围绕它们的公共 $z$ 轴旋转的旋转关节，并定义关节变量为旋转角度。标记为 $\mathbf{E}$ 和 $\mathbf{r}$ 的两列描述了关节变换的旋转和平移分量，而变换本身由方程 ${}^s{\mathbf{X}}_p=\mathrm{rot}(\mathbf{E})\mathrm{xlt}(\mathbf{r})$ 给出。（有关rot、xlt和rz的定义，请参见表2.2。）注意，当关节变量 $\left(q_1\right)$ 为零时，$F_s$ 与 $F_p$ 重合。我们尽可能地将此约定应用于所有关节模型。我们对所有关节模型应用的另一个约定是 $\mathbf{S}、\mathbf{T}$ 及相关量都是以 $F_s$ 坐标表示的。在旋转关节的情况下，这并不重要，因为 ${}^s\mathbf{S}={}^p\mathbf{S}$ ，但对于其他关节来说这是重要的。

在设计良好的动力学模拟器中，很可能会有一个关节模型库，其中包含软件支持的每种关节类型的模型。然后，系统模型将包含系统中每个关节的关节类型描述符。我们将使用表达式 jtype $(i)$ 来表示关节$i$的关节类型描述符。最简单的情况下，关节类型描述符只是一个标识库中特定模型的类型代码。例如，类型代码 ‘$\mathrm{R}$’ 可以表示旋转关节，‘$\mathrm{P}$’ 表示平移关节。然而，某些关节既由类型又由一组参数来表征。例如，螺旋关节除了是螺旋形状外，还通过一个参数指定了螺旋线的螺距。刚体系统中的每个个体螺旋关节的螺距可能不同；因此，这种参数是关节的属性，而不是关节模型的属性。为了适应关节参数，我们定义关节类型描述符为一个数据结构，其中包含类型代码和所需的一组参数值（如果有）。

Notes: $c_1=\cos \left(q_1\right),s_1=\sin \left(q_1\right)$, and ${}^s{\mathbf{X}}_p=\mathrm{rot}(\mathbf{E})\mathrm{xlt}(\mathbf{r})$ (see Table 2.2).

Table 4.1: Joint Model Formulae

Joint Model Calculation Routine 关节模型计算例程

第四章后面的章节描述了几种动力学算法，这些算法都使用关节模型数据。为了保持算法描述的简洁，我们假设存在一个名为jcalc 的单一函数，它可以在一次调用中计算所有请求的关节相关数据。该函数的示例调用如下所示：

$$\begin{array}{ll}\left[{\mathbf{X}}_{\mathrm{J}},\mathbf{S},{\mathbf{v}}_{\mathrm{J}},{\mathbf{c}}_{\mathrm{J}}\right]=\mathrm{jcalc}(\text{ type, }\mathbf{q},\mathbf{\alpha })& \text{ (explicit joint constraint) }\\ [\mathbf{\delta },\mathbf{T}]=\mathrm{jcalc}\left(\text{ type },{}^s{\mathbf{X}}_p\right)& \text{ (implicit joint constraint) }\\ [\dot{\mathbf{q}}]=\text{ jcalc }(\text{ type },\mathbf{q},\mathbf{\alpha })& \text{ (for qdfn (Eq. }3.7))\end{array}$$

在每种情况下，左侧的表达式是请求返回值的列表，参数类型指的是关节类型描述符（例如 jtype $(i)$ ）。关于使用 jcalc 计算隐式关节约束的方法将在第8章中介绍，而使用它计算 $\dot{\mathbf{q}}$ 的方法将在本章后面介绍。在算法列表中，我们通常假设 $\mathbf{\alpha }=\dot{\mathbf{q}}$ ，并在参数列表中使用 $\dot{\mathbf{q}}$ 代替 $\mathbf{\alpha }$。如果一个关节明确依赖于时间，那么时间必须作为列表中的最后一个参数添加进去。注意，${\mathbf{v}}_{\mathrm{J}},{\mathbf{c}}_{\mathrm{J}},\mathbf{S}$ 和 $\mathbf{T}$ 都用 $F_s$ 坐标表示。${\mathbf{v}}_{\mathrm{J}}$ 和 ${\mathbf{c}}_{\mathrm{J}}$ 的表达式在第3.5节中给出。

示例4.4 示例4.3展示了如何计算运动树中每个刚体的坐标变换 ${}^i{\mathbf{X}}_0$。现在我们扩展这个示例，计算 ${}^i{\mathbf{X}}_0$ 和 ${}^0{\mathbf{v}}_i$（刚体 $i$ 在基座标系中的速度）。实现这一目标的算法如下所示：
$$\begin{array}{rl}& {}^0{\mathbf{v}}_0=0\\ & \text{ for }i=1\text{ to }{N}_B\text{ do }\\ & \left[{\mathbf{X}}_{\mathrm{J}},{\mathbf{v}}_{\mathrm{J}}\right]=\mathrm{jcalc}\left(\mathrm{jtype}(i),{\mathbf{q}}_i,{\dot{\mathbf{q}}}_i\right)\\ & {}^i{\mathbf{X}}_{\lambda (i)}={\mathbf{X}}_{\mathrm{J}}{\mathbf{X}}_{\mathrm{T}}(i)\\ & \text{ if }\lambda (i)\ne 0\text{ then }\\ & {}^i{\mathbf{X}}_0={}^i{\mathbf{X}}_{\lambda (i)}{}^{\lambda (i)}{\mathbf{X}}_0\\ & \text{ end }\\ & {}^0{\mathbf{v}}_i={}^0{\mathbf{v}}_{\lambda (i)}+{}^0{\mathbf{X}}_i{\mathbf{v}}_{\mathrm{J}}\\ & \text{ end }\end{array}$$

这段代码片段包含一个新特性：它计算 ${}^i{\mathbf{X}}_0$，然后使用 ${}^0{\mathbf{X}}_i$ 来变换 ${\mathbf{v}}_{\mathrm{J}}$。关于这种做法的解释可以在 附录$A$ 中找到，在那里说明了如何直接从 $\mathbf{X}$ 和 $\mathbf{v}$ 计算${\mathbf{X}}^{-1}\mathbf{v}$，而不必先计算 ${\mathbf{X}}^{-1}$。也可以直接从 $\mathbf{X}$ 和 $\mathbf{f}$ 计算 ${\mathbf{X}}^{\ast }\mathbf{f}$和 ${\left({\mathbf{X}}^{\ast }\right)}^{-1}\mathbf{f}$，而不必先计算 ${\mathbf{X}}^{\ast }$ 或 ${\left({\mathbf{X}}^{\ast }\right)}^{-1}$。我们在几个动力学算法列表中利用了这个事实。

示例4.5 表4.1中平面关节和 6-DoF关节 的一个特点是，平移关节变量以 $F_s$ 坐标而不是 $F_p$坐标表示。这样做的原因是为了使运动子空间矩阵成为一个简单的常数，以便 $\stackrel{\circ }{\mathbf{S}}=0,{\mathbf{c}}_{\mathrm{J}}=0$ ，并且 $\mathbf{S}$ 的简单形式允许将各种优化应用于动力学算法。然而，这种安排可能存在一个很大的缺点：如果后继刚体与前驱相距很远，并开始旋转，则旋转将导致位置变量的变化速率很大（就像一个翻滚的卫星或飞行器）。在这种情况下，可以通过使用一组位置变量来表示以 $F_p$ 坐标为基础的平移，并结合一组速度变量，其中线速度以 $F_s$ 坐标表示，来兼顾两者。

设 ${}^p{q}_1,{}^p{q}_2$ 和 ${}^p{q}_3$ 是平面关节的一组位置变量，其中 ${}^p{q}_2$ 和 ${}^p{q}_3$ 是 $F_p$ 坐标系中 $x$ 和 $y$ 方向的平移；设 ${}^s{q}_1,{}^s{q}_2$ 和 ${}^s{q}_3$ 是同一个平面关节的第二组位置变量，其中 ${}^s{q}_2$ 和 ${}^s{q}_3$ 是 $F_s$ 坐标系中 $x$ 和 $y$ 方向的平移。第二组变量是 表4.1 中使用的变量并在 图4.9 中显示。这两组变量之间的关系是

$$\left[\begin{array}{l}{}^p{q}_1\\ {}^p{q}_2\\ {}^p{q}_3\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& c_1& -s_1\\ 0& s_1& c_1\end{array}\right]\left[\begin{array}{c}{}^s{q}_1\\ {}^s{q}_2\\ {}^s{q}_3\end{array}\right].$$

我们希望使用${}^p{q}_i$作为位置变量，但使用${}^s{\dot{q}}_i$作为速度变量。为了满足这一要求，我们需要一个公式来根据位置和速度变量计算位置变量的导数。这样的公式可以通过对上述方程进行求导获得，得到

$$\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ -q_3& c_1& -s_1\\ q_2& s_1& c_1\end{array}\right]\left[\begin{array}{c}{\alpha }_1\\ {\alpha }_2\\ {\alpha }_3\end{array}\right],$$

其中我们将 ${}^p{q}_i$ 和 ${}^p{\dot{q}}_i$ 的上标 $p$ 省略，并用 ${\alpha }_i$ 代替 ${}^s{\dot{q}}_i$。这就是如果要求 jcalc 根据这个平面关节模型从 $\mathbf{q}$ 和 $\mathbf{\alpha }$ 计算 $\dot{\mathbf{q}}$ 时所执行的计算。

例4.6 我们来构建一个示例，展示在图4.10中所示的齿轮齿条关节的关节模型。该关节允许继承体中的齿轮沿着前导体中的齿条滚动。齿条与$F_p$坐标系的$x$轴平行，而齿轮位于$F_s$坐标系的$z$轴上。齿轮具有半径$r$，这是关节模型的一个参数。该关节只允许单个自由度运动，因此只有一个关节变量，记作$q_1$，并且选择它作为齿轮的旋转角度。

如果当 $q_1=0$ 时 $F_p$ 和 $F_s$ 重合，则从 $F_p$ 到 $F_s$ 的变换包括沿 $x$ 方向平移 $r{q}_1$，然后绕 $z$ 轴旋转 $q_1$。因此，关节变换为：

$${\mathbf{X}}_{\mathrm{J}}={}^s{\mathbf{X}}_p=\left[\begin{array}{cc}\mathbf{E}& 0\\ 0& \mathbf{E}\end{array}\right]\left[\begin{array}{cc}1& 0\\ -\mathbf{p}\times & 1\end{array}\right]$$

其中，

$$\mathbf{E}=\mathrm{rz}\left(q_1\right)=\left[\begin{array}{ccc}{c}_1& s_1& 0\\ -s_1& c_1& 0\\ 0& 0& 1\end{array}\right]\text{ and }\mathbf{p}=\left[\begin{array}{c}r{q}_1\\ 0\\ 0\end{array}\right]$$

下一步是找到 ${\mathbf{v}}_{\mathrm{J}}$ 和 $\mathbf{S}$ 的公式。我们首先将关节速度表示为关节位置和速度变量的函数。相对于 $F_p$，齿轮以角速度$\mathbf{\omega }={\left[\begin{array}{lll}0& 0& {\dot{q}}_1\end{array}\right]}^{\mathrm{T}}$绕通过 $\mathbf{p}$ 的轴旋转，并且以线速度 $\dot{\mathbf{p}}={\left[\begin{array}{lll}r{\dot{q}}_1& 0& 0\end{array}\right]}^{\mathrm{T}}$ 平移。这相当于在 $F_p$ 坐标系中表示的空间速度，即

由于只有一个关节变量，运动子空间矩阵的公式为 $\mathbf{S}={\mathbf{v}}_{\mathrm{J}}/{\dot{q}}_1$。因此，

$${\mathbf{v}}_{\mathrm{J}}={}^s{\mathbf{X}}_p{}^p\mathbf{v}=\left[\begin{array}{c}0\\ 0\\ {\dot{q}}_1\\ r{c}_1{\dot{q}}_1\\ -r{s}_1{\dot{q}}_1\\ 0\end{array}\right]\text{ and }\mathbf{S}=\frac{{\mathbf{v}}_{\mathrm{J}}}{{\dot{q}}_1}=\left[\begin{array}{c}0\\ 0\\ 1\\ r{c}_1\\ -r{s}_1\\ 0\end{array}\right]\text{. }$$

最后，由于 $\mathbf{S}$ 不是恒定的，我们需要一个计算速度乘积项 ${\mathbf{c}}_{\mathrm{J}}$ 的公式。它可以表示为

$${\mathbf{c}}_{\mathrm{J}}=\stackrel{\circ }{S}{\dot{q}}_1=\frac{\partial \mathbf{S}}{\partial q_1}{\dot{q}}_1^2=\left[\begin{array}{c}0\\ 0\\ 0\\ -r{s}_1{\dot{q}}_1^2\\ -r{c}_1{\dot{q}}_1^2\\ 0\end{array}\right]$$

（参见 方程3.42 中的 $\stackrel{\circ }{\mathbf{S}}$ ）。注意，如果 $r=0$，齿轮齿条关节简化为旋转关节。

Polarity Reversal 极性反转

如第4.1.4节所述，运动树中的关节在前向方向上意味着前驱是后继的父节点，否则就在反向方向上。如果一个关节是对称的，那么可以在定义系统模型时选择其极性，使其处于前向方向；但如果它是非对称的，则其极性由正在建模的刚体系统决定。因此，无法保证每个树关节都处于前向方向。

处理反向关节的最简单方法是将其转换为前向关节。我们可以按以下方式进行操作。首先，在关节描述符数据结构中添加一个布尔变量，如果关节是树关节且处于反向方向，则取值为true，否则为false。在给定这个额外的信息后，当这个布尔变量为true时，jcalc可以被修改为模拟相反极性的关节。实际上，jcalc 会颠倒前驱和后继的角色。为了完成这个任务，jcalc 只需要在返回之前修改一些计算得到的值。例如，它不再返回根据给定关节类型的公式计算得出的矩阵 ${}^s{\mathbf{X}}_p$，而是返回该矩阵的逆；而且，它不再返回计算得到的 ${\mathbf{v}}_{\mathrm{J}}$ 的值，而是返回 ${}^p{\mathbf{X}}_s{\mathbf{v}}_{\mathrm{J}}$。修改后的返回值的完整集合如表4.2所示。这种方法的优点是，它允许动力学算法假定所有的树关节都处于前向方向。反过来，这使得算法更简单、更易于实现。

$\begin{array}{l}\text{ normal }\\ \text{ return value }\end{array}$	$\begin{array}{c}\text{ simulating }\\ \text{ opposite polarity }\end{array}$
${}^s{\mathbf{X}}_p$	${}^p{\mathbf{X}}_s$
$S$	$-{}^p{\mathbf{X}}_s\mathbf{S}$
$T$	$-{}^p{\mathbf{X}}_s\mathbf{T}$
${\mathbf{v}}_{\mathrm{J}}$	$-{}^p{\mathbf{X}}_s{\mathbf{v}}_{\mathrm{J}}$
$c_J$	$-{}^p{\mathbf{X}}_s{\mathbf{c}}_{\mathrm{J}}$
$\dot{\mathbf{q}}$	$\dot{\mathbf{q}}$

Table 4.2: How jcalc simulates an opposite-polarity joint

4.5 Spherical Motion 球面运动

表示一般的3D旋转有许多方法（例如，参见Rooney，1977年），但主要的选择是欧拉角和欧拉参数。后者也被称为单位四元数。欧拉角的优点是位置变量是三个独立角度的集合，速度变量是它们的导数。这使得它们相对容易实现。它们的缺点是它们会出现奇点：随着方向接近奇点，表示变得越来越病态，并最终完全崩溃。欧拉参数的主要优点是它们不会出现奇点；但它们的缺点是它们是一组四个非独立的位置变量，这意味着速度变量不能是位置变量的导数。使用欧拉参数的软件必须编写成允许位置变量的数量与速度变量的数量不同。

欧拉角

我们将“欧拉角”一词分为广义和狭义两种意义。在广义上，它指的是通过一系列围绕指定坐标轴的三次旋转来表示一般旋转的任何方法。在狭义上，它指的是特殊情况，即第一次和第三次旋转围绕$z$轴进行，而第二次旋转围绕$x$轴或$y$轴进行。

每个版本的欧拉角都会出现奇点。当第一次和第三次轴对齐时，奇点出现。如果第一次和第三次旋转使用相同的坐标轴，则奇点出现在第二个角度为$0^{\circ }$或$180^{\circ }$时；否则，在第二个角度为$\pm 90^{\circ }$时出现奇点。在接近奇点的附近，所表示的旋转的微小变化可以引起第一和第三角度的大变化。基于欧拉角的关节模型在奇点处无效。

让我们使用偏航角、俯仰角和滚转角构建一个球面关节模型。这些是围绕$z$轴、$y$轴和$x$轴的一系列旋转，按顺序执行。因此，相对于$F_p$，$F_s$的正确方向是先将$F_s$与$F_p$重合，然后围绕其$z$轴旋转一个量$q_1$（偏航角），然后围绕其$y$轴旋转一个量$q_2$（俯仰角），最后围绕其$x$轴旋转一个量$q_3$（滚转角）。因此，$\mathbf{E}$的公式为

$$\begin{array}{rl}\mathbf{E}& =\mathrm{rx}\left(q_3\right)\mathrm{ry}\left(q_2\right)\mathrm{rz}\left(q_1\right)\\ & =\left[\begin{array}{ccc}1& 0& 0\\ 0& c_3& s_3\\ 0& -s_3& c_3\end{array}\right]\left[\begin{array}{ccc}{c}_2& 0& -s_2\\ 0& 1& 0\\ s_2& 0& c_2\end{array}\right]\left[\begin{array}{ccc}{c}_1& s_1& 0\\ -s_1& c_1& 0\\ 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{ccc}{c}_1{c}_2& s_1{c}_2& -s_2\\ c_1{s}_2{s}_3-s_1{c}_3& s_1{s}_2{s}_3+c_1{c}_3& c_2{s}_3\\ c_1{s}_2{c}_3+s_1{s}_3& s_1{s}_2{c}_3-c_1{s}_3& c_2{c}_3\end{array}\right].\end{array}$$

$\mathbf{S}$ 的公式可以通过将关节速度表示为 $\dot{\mathbf{q}}$ 的函数，并使用$\mathbf{S}=\partial {\mathbf{v}}_{\mathrm{J}}/\partial \dot{\mathbf{q}}$ 来获得。我们将跳过细节，只呈现最终结果：

$$\mathbf{S}=\left[\begin{array}{ccc}-s_2& 0& 1\\ c_2{s}_3& c_3& 0\\ c_2{c}_3& -s_3& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right].$$

$\mathbf{S}$的列包含相对于$F_s$的坐标系，围绕其进行旋转的轴：第一列是$F_p$的$z$轴；第二列是第一次旋转后$y$轴所在的位置；第三列是前两次旋转后$x$轴所在的位置。由于$\mathbf{S}$不是常数，我们还需要一个关于${\mathbf{c}}_{\mathrm{J}}$的公式。它可以表示为

$$\begin{array}{rl}{\mathbf{c}}_{\mathrm{J}}=\stackrel{\circ }{S}\dot{\mathbf{q}}& =\left[\begin{array}{ccc}-c_2{\dot{q}}_2& 0& 0\\ -s_2{s}_3{\dot{q}}_2+c_2{c}_3{\dot{q}}_3& -s_3{\dot{q}}_3& 0\\ -s_2{c}_3{\dot{q}}_2-c_2{s}_3{\dot{q}}_3& -c_3{\dot{q}}_3& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]\\ =& \left[\begin{array}{c}-c_2{\dot{q}}_1{\dot{q}}_2\\ -s_2{s}_3{\dot{q}}_1{\dot{q}}_2+c_2{c}_3{\dot{q}}_1{\dot{q}}_3-s_3{\dot{q}}_2{\dot{q}}_3\\ -s_2{c}_3{\dot{q}}_1{\dot{q}}_2-c_2{s}_3{\dot{q}}_1{\dot{q}}_3-c_3{\dot{q}}_2{\dot{q}}_3\\ 0\\ 0\\ 0\end{array}\right].\end{array}$$

方程4.7和4.8替代了表4.1中$\mathbf{E}$和$\mathbf{S}$的公式，但$\mathbf{T}$的公式保持不变。因此，这是一个关节模型的例子，其中$\mathbf{S}$变化而$\mathbf{T}$保持不变。这种情况是可能的，因为范围$(\mathbf{S})$是一个常数，并且只需要$\mathbf{T}$满足范围$(\mathbf{T})=\mathrm{range}(\mathbf{S}{)}^{\perp }$。

欧拉参数 （四元数）

一般的旋转也可以通过单位向量和角度来描述。单位向量指定旋转轴，而角度指定旋转大小。欧拉参数与这种表示密切相关。如果$\mathbf{u}$和$\theta$是单位向量和角度，那么描述相同旋转的欧拉参数为

$$\begin{array}{rl}& p_0=\cos (\theta /2)\\ & p_1=\sin (\theta /2)u_x\\ & p_2=\sin (\theta /2)u_y\\ & p_3=\sin (\theta /2)u_z.\end{array}$$

They are not independent, but must satisfy the equation

$$p_0^2+p_1^2+p_2^2+p_3^2=1.$$

球形关节必须具有三个独立的速度变量。因此，如果我们希望将四个非独立的欧拉参数用作位置变量，那么速度变量不能是位置变量的导数。速度变量的最佳选择是使用关节角速度矢量$\mathbf{\omega }$在$F_s$中的笛卡尔坐标。这个选择导致$\mathbf{S}$采用了表4.1中所示的简单且恒定的值。因此，我们只需要一个关于$\mathbf{E}$的公式和一个关于位置变量的导数的公式。它们为

$$\mathbf{E}=2\left[\begin{array}{ccc}{p}_0^2+p_1^2-1/2& p_1{p}_2+p_0{p}_3& p_1{p}_3-p_0{p}_2\\ p_1{p}_2-p_0{p}_3& p_0^2+p_2^2-1/2& p_2{p}_3+p_0{p}_1\\ p_1{p}_3+p_0{p}_2& p_2{p}_3-p_0{p}_1& p_0^2+p_3^2-1/2\end{array}\right]$$

and

$$\left[\begin{array}{c}{\dot{p}}_0\\ {\dot{p}}_1\\ {\dot{p}}_2\\ {\dot{p}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{ccc}-p_1& -p_2& -p_3\\ p_0& -p_3& p_2\\ p_3& p_0& -p_1\\ -p_2& p_1& p_0\end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right].$$

A sp基于这些方程的球形关节模型在数值上优于基于方程4.7、4.8和4.9的模型。然而，这确实给周围的软件带来了负担，因为需要处理一个位置变量数量与速度变量数量不同的关节模型。还有三个复杂之处：位置变量的值必须通过积分方程4.13获得，而不是直接积分速度变量；零旋转由非零的欧拉参数集定义；在积分过程中需要频繁地对欧拉参数进行重新归一化。（积分过程中的截断误差导致参数不能完全满足方程4.11，从而导致线性缩放误差影响到由关节支持的整个子树。理想情况下，应该在每个积分步骤后重新归一化它们。）

$\begin{array}{l}\text{ data for a }\\ \text{ kinematic tree }\end{array}$	$\begin{array}{l}\text{ extra data for a }\\ \text{ closed-loop system }\end{array}$
$N_B$	$N_L$
$\lambda (1)\cdots \lambda \left(N_B\right)$	$p\left(N_B+1\right)\cdots p\left(N_B+N_L\right)$
jtype(1) $\cdots$ jtype $\left(N_B\right)$	$s\left(N_B+1\right)\cdots s\left(N_B+N_L\right)$
${\mathbf{X}}_{\mathrm{T}}(1)\cdots {\mathbf{X}}_{\mathrm{T}}\left(N_B\right)$	$\mathrm{jtype}\left(N_B+1\right)\cdots \mathrm{jtype}\left(N_B+N_L\right)$
${\mathbf{I}}_1\cdots {\mathbf{I}}_{N_B}$	${\mathbf{X}}_{\mathrm{P}}(1)\cdots {\mathbf{X}}_{\mathrm{P}}\left(N_L\right)$
	${\mathbf{X}}_{\mathrm{S}}(1)\cdots {\mathbf{X}}_{\mathrm{S}}\left(N_L\right)$

Table 4.3: Complete system model

完整的系统模型

表4.3展示了一个刚体系统的完整系统模型。对于一个运动链，所需的数据包括：$N_B$，父节点数组，关节类型描述数组，树形变换数组和刚体惯性数组。刚体的惯性以刚体坐标系表示，并且在该坐标系下是常数。对于闭环系统，额外需要的数据是：$N_L$，以及对于每个闭环关节，前驱和后继刚体编号、关节类型描述符、前驱和后继变换。

在最简单的情况下，软件包可能只针对一组受限制的关节类型进行设计，例如仅旋转关节和平移关节。在这种情况下，关节类型 $j$ 可以简单地是一个数字类型码，我们可以将${\mathbf{X}}_{\mathrm{J}}$、${\mathbf{v}}_{\mathrm{J}}$ 和 $\mathbf{S}$ 的适当值直接硬编码到 jcalc 的源代码中，甚至直接硬编码到动力学算法的源代码中。另一个简化涉及访问关节变量向量（如 $\mathbf{q}$ 和 $\dot{\mathbf{q}}$）中的元素。在一般情况下，${\mathbf{q}}_i$ 是 $\mathbf{q}$ 的一个子向量，并且其在 $\mathbf{q}$ 中的位置取决于所有前面子向量的大小。因此，访问和更新 ${\mathbf{q}}_i$ 的代码应该具有一个偏移数组，每个关节一个，指示 $\mathbf{q}$ 中的哪个元素是 ${\mathbf{q}}_i$ 的第一个元素。然而，如果每个关节恰好有一个自由度，就像旋转关节和平移关节一样，那么 ${\mathbf{q}}_i$ 就是 $\mathbf{q}$ 的第 $i$ 个元素。

如果目标是开发一个通用的软件包，那么创建一个关节模型库，并让 jcalc 访问这个库会更有意义。其中一种特别有用的关节类型是用户自定义关节。通过使库具有可扩展性，可以提供这样的功能。或者，可以要求用户自定义关节的“参数”之一是该关节对应的用户提供的 jcalc 的地址，而内置的 jcalc 可以编程将控制权传递给用户提供的函数。

rbdl 代码注释

结构体 Joint

私有变量

• mJointAxes ： 关节的轴向
  • 这是一个数组，数组里的每一个元素为空间矢量
  • 数组里的元素个数等于关节的自由度 mDoFCount
• mJointType：关节的类型
  • 旋转关节 Revolute、平移关节 Prismatic、绕 x 轴旋转的关节RevoluteX 等等
• mDoFCount：关节的自由度
• q_index：
  • 默认值为 0
• custom_joint_index：
  • 默认值为 0

函数 jcalc

输入：机器人模型 model 、关节编号 joint_id、关节的位置 q、关节的速度 qdot

作用：计算出机器人模型里的 X_lambda[joint_id] 的 E 和 r ，以及计算 关节速度 v_J[joint_id][2]

计算方式

一堆 if else 实现根据不同的关节类型实现计算目标

以 绕z轴旋转为例子

	Scalar s, c;
	// 计算关节旋转角度的正弦值（s）和余弦值（c）。  
	// q[model.mJoints[joint_id].q_index]是关节的旋转角度。
    sincosp (q[model.mJoints[joint_id].q_index], &s, &c);

	// 计算关节的旋转矩阵。
    model.X_lambda[joint_id].E = Matrix3d (
         c * model.X_T[joint_id].E(0, 0) + s * model.X_T[joint_id].E(1, 0),
         c * model.X_T[joint_id].E(0, 1) + s * model.X_T[joint_id].E(1, 1),
         c * model.X_T[joint_id].E(0, 2) + s * model.X_T[joint_id].E(1, 2),

        -s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(1, 0),
        -s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(1, 1),
        -s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(1, 2),

        model.X_T[joint_id].E(2, 0),
        model.X_T[joint_id].E(2, 1),
        model.X_T[joint_id].E(2, 2));
	// 将关节的初始位置赋值给关节的新位置。
	// 因为这是一个旋转关节，所以关节的位置没有改变，只是方向改变了。
    model.X_lambda[joint_id].r = model.X_T[joint_id].r;
	// 计算关节的角速度
    model.v_J[joint_id][2] = qdot[model.mJoints[joint_id].q_index];

绕 z 轴旋转，
$${\mathbf{X}}_j=\mathrm{rot}(\mathbf{E})\mathrm{xlt}(\mathbf{r})$$
其中

$$\mathbf{E}=\mathrm{rz}(\theta )=\left[\begin{array}{ccc}c& s& 0\\ -s& c& 0\\ 0& 0& 1\end{array}\right]$$
（r 表示位置，旋转关节只有旋转，没有位置变化）

因此，这里只计算 $E$ 的变换，计算 第 $i$ 个关节的 旋转变化公式
$$E_{\lambda }=E_J{E}_T$$
最终计算出
$${}^i{\mathbf{X}}_{\lambda (i)}={\mathbf{X}}_{\mathrm{J}}{\mathbf{X}}_{\mathrm{T}}(i)$$
最后一行，是补充计算
$${}^0{\mathbf{v}}_i={}^0{\mathbf{v}}_{\lambda (i)}+{}^0{\mathbf{X}}_i{\mathbf{v}}_{\mathrm{J}}$$

函数 jcalc_XJ

返回值：${\mathbf{X}}_j$

计算方式：根据不同的关节类型，用公式
$${\mathbf{X}}_j=\mathrm{rot}(\mathbf{E})\mathrm{xlt}(\mathbf{r})$$
可以理解为是 函数 jcalc 的补充

函数 jcalc_X_lambda_S

作用：计算 ${}^i{\mathbf{X}}_{\lambda (i)}={\mathbf{X}}_{\mathrm{J}}{\mathbf{X}}_{\mathrm{T}}(i)$ 以及 第 $i$ 个关节运动子空间 ${\mathbf{S}}_i$ 。

结构体Body

成员变量：

• 质量大小 mMass
• 描述刚体质心相对于刚体坐标系的三维向量 mCenterOfMass
• 相对于刚体质心的惯性矩阵 mInertia

模型 Model

成员变量

• lambda：vector 类型，存储了每个刚体的父刚体 id。
• lambda_q：vector 类型，存储了直接影响当前自由度的父自由度的索引。
• mu：这是一个二维向量，vector的vector类型，存储了给定刚体的所有子刚体的 id 。
• dof_count：表示模型的自由度数量。
• q_size：表示 $\mathbf{q}$ 向量的大小。
• qdot_size：表示$\dot{\mathbf{q}}$ ， $\ddot{\mathbf{q}}$ 和 $\tau$ 向量的大小。
• gravity：3维向量，使用笛卡尔坐标系表示重力的方向和大小。
• v：SpatialVector 类型的vector，存储了刚体的空间速度
• a：SpatialVector 类型的vector，存储了刚体的空间加速度。

关节相关：

• mJoints ：Joint 类型的 vector，存储了这个机器人模型的所有关节
• S：SpatialVector 类型的vector，表示关节 i 的轴（关节运动子空间）。
• v_J & c_J ：SpatialVector 类型的vector，存储了关节的速度和加速度。
• X_T：SpatialTransform 类型的 vector，存储了从父刚体坐标系到关节坐标系的变换。 它的表达基于父坐标系。

动力学相关：

• c：速度相关的空间加速度

• IA：空间惯量

• pA：空间偏置力。

• U、d、u ：用于计算的临时变量

• f：刚体的力，（只在 函数 InverseDynamics 使用）

• X_lambda和X_base：这些是向量，分别存储了从父刚体到当前刚体的变换和从基座到刚体参考框架的变换。

• mBodies：这是一个向量，存储了所有的刚体。

刚体相关：

• X_lambda：SpatialTransform 的 vector 类型，存储 从父刚体到当前刚体的变换 。$X_{\lambda (i)}={}^i{X}_{\lambda (i)}$
• X_base：存储从基座标系到刚体坐标系的变换
• mBodies：Body 类型的vector，存储所有刚体，包含 base 刚体在内。 0 … N_B 。
  • mBodies[0] - base body
  • mBodies[1] - 1st moveable body
  • …
  • mBodies[N_B] - N_Bth moveable body

成员函数

• Model()：构造函数
• AddBody()：这个函数用于将一个给定的刚体连接到模型中。
  • 添加方法：首先是要添加的父（或后继）刚体，其次是连接两个刚体的关节到原点的变换。