Chapter 3
Modeling
第三章 建模

This chapter provides the underlying models for deriving the Whole-Body Control framework in Chaps. 6 and 7. Section 3.1 gives a brief overview on rigid body transformations in order to introduce the equations of motion of humanoid robots in Sect. 3.2. In multi-contact balancing, the distribution of forces and torques among the contacts is a challenging aspect of the problem, which arises from the closed kinematic chain given by the robot and its environment. Section 3.3 provides a mathematical description of this so-called Wrench Distribution Problem. The physical properties of the involved contacts are modeled and studied in Sects.3.4 to 3.6.
本章为推导第 6 章和第 7 章中的全身控制框架提供了基础模型。第 3.1 节简要介绍了刚体变换，以便在第 3.2 节介绍仿人机器人的运动方程。第 3.1 节简要介绍了刚体变换，以便在第 3.2 节中介绍仿人机器人的运动方程。3.2.在多触点平衡中，触点之间的力和力矩分布是一个具有挑战性的问题，这源于机器人及其环境所提供的封闭运动链。第 3.3 节对这一所谓的 "扳手分布问题 "进行了数学描述。第 3.4 节至第 3.6 节对相关触点的物理特性进行了建模和研究。

This section provides a brief summary on rigid body transformations, which will be used later to discuss the so-called wrench distribution problem (see Sect.3.3). Let us consider two arbitrary frames ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} and ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} in six-dimensional Cartesian space attached to a rigid body (see Fig. 3.1). The position and orientation of both frames relative to a common world frame $\mathcal{W}$$\mathcal{W}$W\mathcal{W} are denoted by ${\mathit{x}}_A\in {\mathbb{R}}^3,{\mathit{R}}_A\in \mathcal{S}\mathcal{O}(3)$${\mathit{x}}_A\in {\mathbb{R}}^3,{\mathit{R}}_A\in \mathcal{S}\mathcal{O}(3)$x_(A)inR^(3),R_(A)inSO(3)\boldsymbol{x}_{A} \in \mathbb{R}^{3}, \boldsymbol{R}_{A} \in \mathcal{S O}(3) and ${\mathit{x}}_B\in {\mathbb{R}}^3,{\mathit{R}}_B\in \mathcal{S}\mathcal{O}(3)$${\mathit{x}}_B\in {\mathbb{R}}^3,{\mathit{R}}_B\in \mathcal{S}\mathcal{O}(3)$x_(B)inR^(3),R_(B)inSO(3)\boldsymbol{x}_{B} \in \mathbb{R}^{3}, \boldsymbol{R}_{B} \in \mathcal{S O}(3), respectively. The linear and angular velocities of frame ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} and ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} relative to the world frame $\mathcal{W}$$\mathcal{W}$W\mathcal{W} are given by ${\chi }_A\in {\mathbb{R}}^3,{\omega }_A\in {\mathbb{R}}^3$${\chi }_A\in {\mathbb{R}}^3,{\omega }_A\in {\mathbb{R}}^3$chi_(A)inR^(3),omega_(A)inR^(3)\chi_{A} \in \mathbb{R}^{3}, \omega_{A} \in \mathbb{R}^{3} and ${\chi }_B\in {\mathbb{R}}^3$${\chi }_B\in {\mathbb{R}}^3$chi_(B)inR^(3)\chi_{B} \in \mathbb{R}^{3}, ${\omega }_B\in {\mathbb{R}}^3$${\omega }_B\in {\mathbb{R}}^3$omega_(B)inR^(3)\omega_{B} \in \mathbb{R}^{3}. Both can be stacked into the six-dimensional Cartesian velocities
本节简要介绍了刚体变换，稍后将用于讨论所谓的扳手分布问题（见第 3.3 节）。让我们考虑六维笛卡尔空间中与刚体相连的两个任意框架 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 和 ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} （见图 3.1）。两个框架相对于共同世界框架 $\mathcal{W}$$\mathcal{W}$W\mathcal{W} 的位置和方向分别用 ${\mathit{x}}_A\in {\mathbb{R}}^3,{\mathit{R}}_A\in \mathcal{S}\mathcal{O}(3)$${\mathit{x}}_A\in {\mathbb{R}}^3,{\mathit{R}}_A\in \mathcal{S}\mathcal{O}(3)$x_(A)inR^(3),R_(A)inSO(3)\boldsymbol{x}_{A} \in \mathbb{R}^{3}, \boldsymbol{R}_{A} \in \mathcal{S O}(3) 和 ${\mathit{x}}_B\in {\mathbb{R}}^3,{\mathit{R}}_B\in \mathcal{S}\mathcal{O}(3)$${\mathit{x}}_B\in {\mathbb{R}}^3,{\mathit{R}}_B\in \mathcal{S}\mathcal{O}(3)$x_(B)inR^(3),R_(B)inSO(3)\boldsymbol{x}_{B} \in \mathbb{R}^{3}, \boldsymbol{R}_{B} \in \mathcal{S O}(3) 表示。帧 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 和 ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} 相对于世界帧 $\mathcal{W}$$\mathcal{W}$W\mathcal{W} 的线速度和角速度分别用 ${\chi }_A\in {\mathbb{R}}^3,{\omega }_A\in {\mathbb{R}}^3$${\chi }_A\in {\mathbb{R}}^3,{\omega }_A\in {\mathbb{R}}^3$chi_(A)inR^(3),omega_(A)inR^(3)\chi_{A} \in \mathbb{R}^{3}, \omega_{A} \in \mathbb{R}^{3} 和 ${\chi }_B\in {\mathbb{R}}^3$${\chi }_B\in {\mathbb{R}}^3$chi_(B)inR^(3)\chi_{B} \in \mathbb{R}^{3} , ${\omega }_B\in {\mathbb{R}}^3$${\omega }_B\in {\mathbb{R}}^3$omega_(B)inR^(3)\omega_{B} \in \mathbb{R}^{3} 表示。两者可以叠加成六维笛卡尔速度

The forces and torques acting on ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} and ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} are given by ${\mathit{f}}_A\in {\mathbb{R}}^3,{\mathit{\tau }}_A\in {\mathbb{R}}^3$${\mathit{f}}_A\in {\mathbb{R}}^3,{\mathit{\tau }}_A\in {\mathbb{R}}^3$f_(A)inR^(3),tau_(A)inR^(3)\boldsymbol{f}_{A} \in \mathbb{R}^{3}, \boldsymbol{\tau}_{A} \in \mathbb{R}^{3} and ${\mathit{f}}_B\in {\mathbb{R}}^3,{\mathit{\tau }}_B\in {\mathbb{R}}^3$${\mathit{f}}_B\in {\mathbb{R}}^3,{\mathit{\tau }}_B\in {\mathbb{R}}^3$f_(B)inR^(3),tau_(B)inR^(3)\boldsymbol{f}_{B} \in \mathbb{R}^{3}, \boldsymbol{\tau}_{B} \in \mathbb{R}^{3}, respectively. Analogously to the velocities, the forces and
作用在 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 和 ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} 上的力和力矩分别由 ${\mathit{f}}_A\in {\mathbb{R}}^3,{\mathit{\tau }}_A\in {\mathbb{R}}^3$${\mathit{f}}_A\in {\mathbb{R}}^3,{\mathit{\tau }}_A\in {\mathbb{R}}^3$f_(A)inR^(3),tau_(A)inR^(3)\boldsymbol{f}_{A} \in \mathbb{R}^{3}, \boldsymbol{\tau}_{A} \in \mathbb{R}^{3} 和 ${\mathit{f}}_B\in {\mathbb{R}}^3,{\mathit{\tau }}_B\in {\mathbb{R}}^3$${\mathit{f}}_B\in {\mathbb{R}}^3,{\mathit{\tau }}_B\in {\mathbb{R}}^3$f_(B)inR^(3),tau_(B)inR^(3)\boldsymbol{f}_{B} \in \mathbb{R}^{3}, \boldsymbol{\tau}_{B} \in \mathbb{R}^{3} 给出。与速度类似，作用在${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A}和${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B}上的力和力矩分别为

Fig. 3.1 Frames ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} and ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} located on a rigid body
图 3.1 位于刚体上的框架 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 和 ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B}

torques can be stacked into the wrenches
扭矩可叠加到扳手中

If the translational and rotational velocities are expressed in body frame, which means with respect to the base vectors of ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} and ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B}, then they can be computed via
如果平移速度和旋转速度以主体框架表示，也就是相对于 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 和 ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} 的基向量，那么它们可以通过以下方式进行计算

Here, the operator $\mathit{S}(\bullet )$$\mathit{S}(\bullet )$S(∙)\boldsymbol{S}(\bullet) denotes the cross product matrix according to Definition 3.1.
这里，运算符 $\mathit{S}(\bullet )$$\mathit{S}(\bullet )$S(∙)\boldsymbol{S}(\bullet) 表示定义 3.1 中的交叉积矩阵。

Definition 3.1 (Cross product matrix $\mathit{S}(\mathit{\lambda })$$\mathit{S}(\mathit{\lambda })$S(lambda)\boldsymbol{S}(\boldsymbol{\lambda}) ) Let $\mathit{\lambda }={\left({\lambda }_x^T{\lambda }_y^T{\lambda }_z^T\right)}^T$$\mathit{\lambda }={\left({\lambda }_x^T{\lambda }_y^T{\lambda }_z^T\right)}^T$lambda=(lambda_(x)^(T)lambda_(y)^(T)lambda_(z)^(T))^(T)\boldsymbol{\lambda}=\left(\lambda_{x}^{T} \lambda_{y}^{T} \lambda_{z}^{T}\right)^{T} be an arbitrary vector in ${\mathbb{R}}^3$${\mathbb{R}}^3$R^(3)\mathbb{R}^{3}. The cross product matrix $S(\lambda )\in {\mathbb{R}}^{3\times 3}$$S(\lambda )\in {\mathbb{R}}^{3\times 3}$S(lambda)inR^(3xx3)S(\lambda) \in \mathbb{R}^{3 \times 3} is defined as
定义 3.1（交叉积矩阵 $\mathit{S}(\mathit{\lambda })$$\mathit{S}(\mathit{\lambda })$S(lambda)\boldsymbol{S}(\boldsymbol{\lambda}) ）设 $\mathit{\lambda }={\left({\lambda }_x^T{\lambda }_y^T{\lambda }_z^T\right)}^T$$\mathit{\lambda }={\left({\lambda }_x^T{\lambda }_y^T{\lambda }_z^T\right)}^T$lambda=(lambda_(x)^(T)lambda_(y)^(T)lambda_(z)^(T))^(T)\boldsymbol{\lambda}=\left(\lambda_{x}^{T} \lambda_{y}^{T} \lambda_{z}^{T}\right)^{T} 是 ${\mathbb{R}}^3$${\mathbb{R}}^3$R^(3)\mathbb{R}^{3} 中的任意向量。交叉积矩阵 $S(\lambda )\in {\mathbb{R}}^{3\times 3}$$S(\lambda )\in {\mathbb{R}}^{3\times 3}$S(lambda)inR^(3xx3)S(\lambda) \in \mathbb{R}^{3 \times 3} 的定义为

Note that the cross product matrix $\mathit{S}(\mathit{\lambda })$$\mathit{S}(\mathit{\lambda })$S(lambda)\boldsymbol{S}(\boldsymbol{\lambda}) is skew-symmetric, that is $\mathit{S}(\mathit{\lambda })=-\mathit{S}(\mathit{\lambda }{)}^T$$\mathit{S}(\mathit{\lambda })=-\mathit{S}(\mathit{\lambda }{)}^T$S(lambda)=-S(lambda)^(T)\boldsymbol{S}(\boldsymbol{\lambda})=-\boldsymbol{S}(\boldsymbol{\lambda})^{T} and $\mathit{S}(-\lambda )=\mathit{S}(\lambda {)}^T$$\mathit{S}(-\lambda )=\mathit{S}(\lambda {)}^T$S(-lambda)=S(lambda)^(T)\boldsymbol{S}(-\lambda)=\boldsymbol{S}(\lambda)^{T} hold.
请注意，交叉积矩阵 $\mathit{S}(\mathit{\lambda })$$\mathit{S}(\mathit{\lambda })$S(lambda)\boldsymbol{S}(\boldsymbol{\lambda}) 是倾斜对称的，即 $\mathit{S}(\mathit{\lambda })=-\mathit{S}(\mathit{\lambda }{)}^T$$\mathit{S}(\mathit{\lambda })=-\mathit{S}(\mathit{\lambda }{)}^T$S(lambda)=-S(lambda)^(T)\boldsymbol{S}(\boldsymbol{\lambda})=-\boldsymbol{S}(\boldsymbol{\lambda})^{T} 和 $\mathit{S}(-\lambda )=\mathit{S}(\lambda {)}^T$$\mathit{S}(-\lambda )=\mathit{S}(\lambda {)}^T$S(-lambda)=S(lambda)^(T)\boldsymbol{S}(-\lambda)=\boldsymbol{S}(\lambda)^{T} 成立。

The velocities of frame ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} can be mapped to frame ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} via
帧 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 的速度可通过以下方式映射到帧 ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B}

with $\mathit{A}\mathit{d}(\bullet ,\star )$$\mathit{A}\mathit{d}(\bullet ,\star )$Ad(∙,***)\boldsymbol{A d}(\bullet, \star) denoting the adjoint matrix according to Definition 3.2. The matrix ${\mathit{R}}_{AB}={\mathit{R}}_A^T{\mathit{R}}_B$${\mathit{R}}_{AB}={\mathit{R}}_A^T{\mathit{R}}_B$R_(AB)=R_(A)^(T)R_(B)\boldsymbol{R}_{A B}=\boldsymbol{R}_{A}^{T} \boldsymbol{R}_{B} and the vector ${}^A{\mathit{x}}_{AB}={\mathit{R}}_A^T({}^{\mathcal{W}}{\mathit{x}}_B-{}^{\mathit{W}}{\mathit{x}}_A)$${}^A{\mathit{x}}_{AB}={\mathit{R}}_A^T\left({}^{\mathcal{W}}{\mathit{x}}_B-{}^{\mathit{W}}{\mathit{x}}_A\right)$^(A)x_(AB)=R_(A)^(T)(^(W)x_(B)-^(W)x_(A)){ }^{A} \boldsymbol{x}_{A B}=\boldsymbol{R}_{A}^{T}\left({ }^{\mathcal{W}} \boldsymbol{x}_{B}-{ }^{\boldsymbol{W}} \boldsymbol{x}_{A}\right) describe the orientation and position of ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} relative to ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A}, expressed using the base vectors of ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A}.
根据定义 3.2， $\mathit{A}\mathit{d}(\bullet ,\star )$$\mathit{A}\mathit{d}(\bullet ,\star )$Ad(∙,***)\boldsymbol{A d}(\bullet, \star) 表示邻接矩阵。矩阵 ${\mathit{R}}_{AB}={\mathit{R}}_A^T{\mathit{R}}_B$${\mathit{R}}_{AB}={\mathit{R}}_A^T{\mathit{R}}_B$R_(AB)=R_(A)^(T)R_(B)\boldsymbol{R}_{A B}=\boldsymbol{R}_{A}^{T} \boldsymbol{R}_{B} 和向量 ${}^A{\mathit{x}}_{AB}={\mathit{R}}_A^T({}^{\mathcal{W}}{\mathit{x}}_B-{}^{\mathit{W}}{\mathit{x}}_A)$${}^A{\mathit{x}}_{AB}={\mathit{R}}_A^T\left({}^{\mathcal{W}}{\mathit{x}}_B-{}^{\mathit{W}}{\mathit{x}}_A\right)$^(A)x_(AB)=R_(A)^(T)(^(W)x_(B)-^(W)x_(A)){ }^{A} \boldsymbol{x}_{A B}=\boldsymbol{R}_{A}^{T}\left({ }^{\mathcal{W}} \boldsymbol{x}_{B}-{ }^{\boldsymbol{W}} \boldsymbol{x}_{A}\right) 描述了 ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} 相对于 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 的方向和位置，使用基向量 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} 表示。

Definition 3.2 (Adjoint matrix $\mathit{A}\mathit{d}(\mathit{R},\mathit{x})$$\mathit{A}\mathit{d}(\mathit{R},\mathit{x})$Ad(R,x)\boldsymbol{A d}(\boldsymbol{R}, \boldsymbol{x}) ) Let $\mathit{R}\in \mathcal{S}\mathcal{O}(3)$$\mathit{R}\in \mathcal{S}\mathcal{O}(3)$R inSO(3)\boldsymbol{R} \in \mathcal{S O}(3) and $\mathit{x}\in {\mathbb{R}}^3$$\mathit{x}\in {\mathbb{R}}^3$x inR^(3)\boldsymbol{x} \in \mathbb{R}^{3} be an arbitrary rotation matrix and an arbitrary position vector. Then, the adjoint matrix is defined as
定义 3.2（邻接矩阵 $\mathit{A}\mathit{d}(\mathit{R},\mathit{x})$$\mathit{A}\mathit{d}(\mathit{R},\mathit{x})$Ad(R,x)\boldsymbol{A d}(\boldsymbol{R}, \boldsymbol{x}) ）假设 $\mathit{R}\in \mathcal{S}\mathcal{O}(3)$$\mathit{R}\in \mathcal{S}\mathcal{O}(3)$R inSO(3)\boldsymbol{R} \in \mathcal{S O}(3) 和 $\mathit{x}\in {\mathbb{R}}^3$$\mathit{x}\in {\mathbb{R}}^3$x inR^(3)\boldsymbol{x} \in \mathbb{R}^{3} 是任意旋转矩阵和任意位置矢量。那么，邻接矩阵的定义为

based on the cross product matrix $\mathit{S}(\bullet )$$\mathit{S}(\bullet )$S(∙)\boldsymbol{S}(\bullet) according to Definition 3.1. The inverse is given by $\mathit{A}\mathit{d}(\mathit{R},\mathit{x}{)}^{-1}=\mathit{A}\mathit{d}({\mathit{R}}^T,-{\mathit{R}}^T\mathit{x})$$\mathit{A}\mathit{d}(\mathit{R},\mathit{x}{)}^{-1}=\mathit{A}\mathit{d}\left({\mathit{R}}^T,-{\mathit{R}}^T\mathit{x}\right)$Ad(R,x)^(-1)=Ad(R^(T),-R^(T)x)\boldsymbol{A d}(\boldsymbol{R}, \boldsymbol{x})^{-1}=\boldsymbol{A d}\left(\boldsymbol{R}^{T},-\boldsymbol{R}^{T} \boldsymbol{x}\right).
根据定义 3.1，以交叉积矩阵 $\mathit{S}(\bullet )$$\mathit{S}(\bullet )$S(∙)\boldsymbol{S}(\bullet) 为基础。逆矩阵由 $\mathit{A}\mathit{d}(\mathit{R},\mathit{x}{)}^{-1}=\mathit{A}\mathit{d}({\mathit{R}}^T,-{\mathit{R}}^T\mathit{x})$$\mathit{A}\mathit{d}(\mathit{R},\mathit{x}{)}^{-1}=\mathit{A}\mathit{d}\left({\mathit{R}}^T,-{\mathit{R}}^T\mathit{x}\right)$Ad(R,x)^(-1)=Ad(R^(T),-R^(T)x)\boldsymbol{A d}(\boldsymbol{R}, \boldsymbol{x})^{-1}=\boldsymbol{A d}\left(\boldsymbol{R}^{T},-\boldsymbol{R}^{T} \boldsymbol{x}\right) 给出。

The transpose of the adjoint matrix can be used to map forces and torques from frame ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A} to frame ${\mathcal{F}}_B$${\mathcal{F}}_B$F_(B)\mathcal{F}_{B} via
可以通过邻接矩阵的转置将力和力矩从框架 ${\mathcal{F}}_A$${\mathcal{F}}_A$F_(A)\mathcal{F}_{A}<