刚体动力学算法第二章 空间向量代数（2）

2.8 坐标变换(Coordinate Transforms)

运动矢量从 $A$ 坐标系到 $B$ 坐标系的坐标变换通常写为 ${}^B{\mathbf{X}}_A$，力矢量的相同变换为 ${}^B{\mathbf{X}}_A^{\ast }$。这两个方程的关系式为 ${}^B{\mathbf{X}}_A^{\ast }={}^B{\mathbf{X}}_A^{-\mathrm{T}}$（参见方程 2.13）。在本节中，我们将建立普吕克坐标系之间的坐标变换公式。

设 $A$ 和 $B$ 表示两个普吕克坐标系。每个坐标系都由笛卡尔坐标系的位置和方向定义，并且每个坐标系还定义一个笛卡尔坐标系。为了方便起见，我们将对所有三个 entities 使用相同的名称；因此坐标系 $A$ 定义了普吕克坐标系 $A$ 和笛卡尔坐标系 $A$，$B$ 也类似。 ${}^B{\mathbf{X}}_A$ 的公式仅取决于坐标系 $B$ 相对于坐标系 $A$ 的位置和方向，因此可以表示为两个更简单变换的乘积：一个取决于坐标系 $B$ 的相对位置，另一个取决于其相对方向。

旋转(Rotation)

设 $A$ 和 $B$ 是两个原点都在点 $O$ 的笛卡尔坐标系。给定点 $O$，$\hat{\mathbf{m}}\in {\mathbf{M}}^6$ 的任一运动矢量可以表示为三维矢量$\mathbf{m}$ 和 ${\mathbf{m}}_O$，且 $\hat{\mathbf{m}}$ 的普吕克坐标系是 矢量$\mathbf{m}$ 和 ${\mathbf{m}}_O$ 的笛卡尔坐标系。因此，令 ${}^A\hat{\mathbf{m}},{}^A\mathbf{m},{}^A{\mathbf{m}}_O,{}^B\hat{\mathbf{m}},{}^B\mathbf{m}$ 和 ${}^B{\mathbf{m}}_O$ 表示 $A$ 和 $B$ 坐标系中 $\hat{\mathbf{m}},\mathbf{m}$ 和 ${\mathbf{m}}_O$ 的坐标向量，设 $\mathbf{E}$ 是将 3D 矢量从 A 坐标系转换为 B 坐标系（即 $\mathbf{E}={}^B{\mathbf{E}}_A$）的 $3\times 3$ 旋转矩阵，可得出：
$${}^B\hat{\mathbf{m}}=\left[\begin{array}{c}{}^B\mathbf{m}\\ {}^B{\mathbf{m}}_O\end{array}\right]=\left[\begin{array}{c}{\mathbf{E}}^A\mathbf{m}\\ {\mathbf{E}}^A{\mathbf{m}}_O\end{array}\right]=\left[\begin{array}{cc}\mathbf{E}& 0\\ 0& \mathbf{E}\end{array}\right]{}^A\hat{\mathbf{m}},$$

因此

$${}^B{\mathbf{X}}_A=\left[\begin{array}{cc}\mathbf{E}& 0\\ 0& \mathbf{E}\end{array}\right]\text{\hspace{1em}(2.19)}$$

力矢量的等效变换完全相同：

$${}^B{\mathbf{X}}_A^{\ast }={}^B{\mathbf{X}}_A^{-\mathrm{T}}=\left[\begin{array}{ll}\mathbf{E}& 0\\ 0& \mathbf{E}\end{array}\right]\text{\hspace{1em}(2.20)}$$

平移(Translation)

假设 $O$ 和 $P$ 是空间中的两个点，且每个点对应一个笛卡尔坐标系，两个坐标系的方向相同。任一运动矢量 $\hat{\mathbf{m}}\in {\mathbf{M}}^6$ 在点 $O$ 处可以用 $3\mathrm{D}$ 矢量 $\mathbf{m}$ 和 ${\mathbf{m}}_O$ 表示, 且在点 $P$ 处可以用向量 $\mathbf{m}$ 和 ${\mathbf{m}}_P$ 表示，其中 ${\mathbf{m}}_P={\mathbf{m}}_O-\overrightarrow{OP}\times \mathbf{m}$ (如公式 2.1 所示)。在这两种情况下，$\hat{\mathbf{m}}$ 的普吕克坐标是相应的一对 $3\mathrm{D}$ 向量的笛卡尔坐标，因此我们有

$${\hat{\mathbf{m}}}_P=\left[\begin{array}{c}\mathbf{m}\\ {\mathbf{m}}_P\end{array}\right]=\left[\begin{array}{c}\mathbf{m}\\ {\mathbf{m}}_O-\mathbf{r}\times \mathbf{m}\end{array}\right]=\left[\begin{array}{cc}1& 0\\ -\mathbf{r}\times & 1\end{array}\right]{\hat{\mathbf{m}}}_O,$$

其中 $\mathbf{r}=\overrightarrow{OP}$ 。因此

$${}^P{\mathbf{X}}_O=\left[\begin{array}{cc}1& 0\\ -\mathbf{r}\times & 1\end{array}\right]$$

力矢量的等效变换为

$${}^P{\mathbf{X}}_O^{\ast }={}^P{\mathbf{X}}_O^{-\mathrm{T}}=\left[\begin{array}{cc}1& -\mathbf{r}\times \\ 0& 1\end{array}\right].$$

形式为 $\mathbf{a}\times$（发音为 ‘a-cross’）的表达式表示将 $\mathbf{b}$ 映射到 $\mathbf{a}\times \mathbf{b}$ 的运算符。如果 $\mathbf{a}$ 是一个 $3\mathrm{D}$ 坐标向量，则 $\mathbf{a}\times$ 是公式给定 $3\times 3$ 矩阵
$$\left[\begin{array}{l}x\\ y\\ z\end{array}\right]\times =\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]$$

该运算符具有几个数学性质，如表 2.1 中列出。我们将把形式为 $\mathbf{a}\times \mathbf{b}\times \mathbf{c}$ 的表达式解释为表示 $（\mathbf{a}\times ）（\mathbf{b}\times ）\mathbf{c}$，这与 $\mathbf{a}\times （\mathbf{b}\times \mathbf{c}）$ 相同。任何其他解释都需要括号。同样，表达式 $\mathbf{a}+\mathbf{b}\times \mathbf{c}$ 和 $\mathbf{E}\mathbf{a}\times$ 分别表示 $\mathbf{a}+（\mathbf{b}\times \mathbf{c}）$ 和 $\mathbf{E}（\mathbf{a}\times ）$，以及任何其他解释都需要括号。

rbdl 代码

在 rbdl 库里，对应 $\mathbf{a}\times$ 的对应代码为

inline Matrix3d VectorCrossMatrix (const Vector3d &vector) {
  return Matrix3d (
      0., -vector[2], vector[1],
      vector[2], 0., -vector[0],
      -vector[1], vector[0], 0.
      );
}

注：这也是一个三维向量的反对称阵

广义变换(General Transforms)

设 $A$ 和 $B$ 是笛卡尔坐标系，原点分别为 $O$ 和 $P$；设 $\mathbf{r}$ 表示在 $A$ 坐标系的 $\overrightarrow{OP}$ 坐标向量；并让 $\mathbf{E}$ 表示将 $3\mathrm{D}$ 向量从 $A$ 坐标此转换为 $B$ 坐标系的旋转矩阵。从 $A$ 坐标系到 $B$ 坐标系的 Plücker 变换是 $\mathbf{r}$ 平移和 $\mathbf{E}$ 旋转的乘积。运动矢量变换的公式为
$${}^B{\mathbf{X}}_A=\left[\begin{array}{cc}\mathbf{E}& 0\\ 0& \mathbf{E}\end{array}\right]\left[\begin{array}{cc}1& 0\\ -\mathbf{r}\times & 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{E}& 0\\ -\mathbf{E}\mathbf{r}\times & \mathbf{E}\end{array}\right]\text{\hspace{1em}(2.24)}$$

力矢量的等价变换为

$${}^B{\mathbf{X}}_A^{\ast }=\left[\begin{array}{cc}\mathbf{E}& 0\\ 0& \mathbf{E}\end{array}\right]\left[\begin{array}{cc}1& -\mathbf{r}\times \\ 0& 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{E}& -\mathbf{E}\mathbf{r}\times \\ 0& \mathbf{E}\end{array}\right]\text{\hspace{1em}(2.25)}$$

这两个变换的逆为

$${}^A{\mathbf{X}}_B=\left[\begin{array}{cc}1& 0\\ \mathbf{r}\times & 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{E}}^{\mathrm{T}}& 0\\ 0& {\mathbf{E}}^{\mathrm{T}}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{E}}^{\mathrm{T}}& 0\\ \mathbf{r}\times {\mathbf{E}}^{\mathrm{T}}& {\mathbf{E}}^{\mathrm{T}}\end{array}\right]\text{\hspace{1em}(2.26)}$$

和

$${}^A{\mathbf{X}}_B^{\ast }=\left[\begin{array}{cc}1& \mathbf{r}\times \\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{E}}^{\mathrm{T}}& 0\\ 0& {\mathbf{E}}^{\mathrm{T}}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{E}}^{\mathrm{T}}& \mathbf{r}\times {\mathbf{E}}^{\mathrm{T}}\\ 0& {\mathbf{E}}^{\mathrm{T}}\end{array}\right]\text{\hspace{1em}(2.27)}$$

这个有点奇怪，看起来像少了一个 负号

为了进行比较，从 $A$ 到 $B$（齐次）坐标的 $4\times 4$ 齐次坐标变换矩阵的公式为
$${}^B{\mathbf{T}}_A=\left[\begin{array}{cc}\mathbf{E}& -\mathbf{E}\mathbf{r}\\ 0& 1\end{array}\right]\text{\hspace{1em}(2.28)}$$

$$\begin{array}{rl}& \mathrm{rx}(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& c& s\\ 0& -s& c\end{array}\right]\mathrm{ry}(\theta )=\left[\begin{array}{ccc}c& 0& -s\\ 0& 1& 0\\ s& 0& c\end{array}\right]\mathrm{rz}(\theta )=\left[\begin{array}{ccc}c& s& 0\\ -s& c& 0\\ 0& 0& 1\end{array}\right]\\ & (c=\cos (\theta ),s=\sin (\theta ))\\ & \mathrm{rot}(\mathbf{E})=\left[\begin{array}{cc}\mathbf{E}& 0\\ 0& \mathbf{E}\end{array}\right]\mathrm{xlt}(\mathbf{r})=\left[\begin{array}{cc}1& 0\\ -\mathbf{r}\times & 1\end{array}\right]\begin{array}{l}\mathrm{rotx}(\theta )=\mathrm{rot}(\mathrm{rx}(\theta ))\\ \mathrm{roty}(\theta )=\mathrm{rot}(\mathrm{ry}(\theta ))\\ \mathrm{rotz}(\theta )=\mathrm{rot}(\mathrm{rz}(\theta ))\end{array}\end{array}$$

Table 2.2: 旋转和普吕克变换函数

为了便于描述 Plücker 变换，我们偶尔会使用表 2.2 中列出的函数。就这些功能而言，等式 2.24 可以写成 ${}^B{\mathbf{X}}_A=\mathrm{rot}(\mathbf{E})\mathrm{xlt}(\mathbf{r})$，等等。

最后，请注意，我们采用了以下约定来描述坐标变换：${}^B{\mathbf{X}}_A$ 的公式（或 ${}^B{\mathbf{E}}_A$ ) 与如何移动坐标系 $A$ 以便使其与坐标系 $B$ 重合的描述相关联。例如，表达式“$\mathrm{rotx}(\theta )$”读起来就像语句“围绕$x$轴旋转$\theta$”语句的缩写版本，其值是坐标变换${}^B{\mathbf{X}}_A$ 表示坐标系 $B$ 相对于坐标系 $A$ 绕公共 $x$ 轴旋转 $\theta$ 的情况。读者应该记住，有些作者使用不同的约定。

rbdl 代码

在 rbdl 的代码里，

• 使用空间变换作为结构体 SpatialTransform 描述 X 。

• 使用 Xrot 函数描述绕给定方向的轴的旋转。这里选取《现代机器人学》里关于旋转某一向量或坐标系的计算方法。（详细推导可以看罗德里得斯公式。）

$$\begin{array}{rl}& \mathrm{Rot}(\hat{\omega },\theta )=\\ & \left[\begin{array}{ccc}{c}_{\theta }+{\hat{\omega }}_1^2\left(1-c_{\theta }\right)& {\hat{\omega }}_1{\hat{\omega }}_2\left(1-c_{\theta }\right)-{\hat{\omega }}_3{s}_{\theta }& {\hat{\omega }}_1{\hat{\omega }}_3\left(1-c_{\theta }\right)+{\hat{\omega }}_2{s}_{\theta }\\ {\hat{\omega }}_1{\hat{\omega }}_2\left(1-c_{\theta }\right)+{\hat{\omega }}_3{s}_{\theta }& c_{\theta }+{\hat{\omega }}_2^2\left(1-c_{\theta }\right)& {\hat{\omega }}_2{\hat{\omega }}_3\left(1-c_{\theta }\right)-{\hat{\omega }}_1{s}_{\theta }\\ {\hat{\omega }}_1{\hat{\omega }}_3\left(1-c_{\theta }\right)-{\hat{\omega }}_2{\mathrm{s}}_{\theta }& {\hat{\omega }}_2{\hat{\omega }}_3\left(1-c_{\theta }\right)+{\hat{\omega }}_1{\mathrm{s}}_{\theta }& c_{\theta }+{\hat{\omega }}_3^2\left(1-c_{\theta }\right)\end{array}\right]\end{array}$$

• Xrotx、Xroty、Xrotz、 Xtrans 都对应上述的旋转和普吕克变换函数

---

以下摘抄自《现代机器人学》的书

齐次变换矩阵是一个 $4\times 4$ 的矩阵，其表示方法为
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{p}\\ 0& 1\end{array}\right]$$
其逆矩阵
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{p}\\ 0& 1\end{array}\right]$$
定义 3.20. 给定 $T=(R,p)\in SE(3)$， 其伴随变换矩阵 (adjoint representation) $\left[{\mathrm{Ad}}_T\right]$ 为
$$\left[{\mathrm{Ad}}_T\right]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\in {\mathbb{R}}^{6\times 6}.$$
命题 3.21 对任一 $T\in SE(3)$ ，总是满足:
$${\left[{\mathrm{Ad}}_T\right]}^{-1}=\left[{\mathrm{Ad}}_{T^{-1}}\right],$$
对运动旋量的结果小结

命题 3.22. 给定一个（空间）固定坐标系 { s } \{\mathrm{s}\} {s}，和物体坐标系 { b } \{\mathrm{b}\} {b}，和一个可微的齐次变换 $T_{sb}(t)\in SE(3)$，其中
$$T_{sb}(t)=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right],$$
那么
$$T_{sb}^{-1}{\dot{T}}_{sb}=\left[{\mathcal{V}}_b\right]=\left[\begin{array}{cc}\left[{\omega }_b\right]& v_b\\ 0& 0\end{array}\right]\in \mathrm{se}(3)$$
是物体运动旋量的矩阵表示，而
$${\dot{T}}_{sb}{T}_{sb}^{-1}=\left[{\mathcal{V}}_s\right]=\left[\begin{array}{cc}\left[{\omega }_s\right]& v_s\\ 0& 0\end{array}\right]\in \mathrm{se}(3)$$
是空间运动旋量的矩阵表示。运动旋量 ${\mathcal{V}}_s$ 和 ${\mathcal{V}}_b$ 之间的关系表达式为
$$\begin{array}{rl}& {\mathcal{V}}_s=\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]=\left[{\mathrm{Ad}}_{T_{sb}}\right]{\mathcal{V}}_b\\ & {\mathcal{V}}_b=\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]=\left[\begin{array}{cc}{R}^{\mathrm{T}}& 0\\ -R^{\mathrm{T}}[p]& R^{\mathrm{T}}\end{array}\right]\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]=\left[{\mathrm{Ad}}_{T_{bs}}\right]{\mathcal{V}}_s.\end{array}$$
写为更为通用的形式，对于任意的两个坐标系 { c } \{\mathrm{c}\} {c}和 { d } \{\mathrm{d}\} {d}，在 { c } \{\mathrm{c}\} {c}中表示的运动旋量 ${\mathcal{V}}_c$与在 { d } \{\mathrm{d}\} {d}中表示的运动旋量 ${\mathcal{V}}_d$之间有关系
$${\mathcal{V}}_c=\left[{\mathrm{Ad}}_{T_{cd}}\right]{\mathcal{V}}_d,{\mathcal{V}}_d=\left[{\mathrm{Ad}}_{T_{dc}}\right]{\mathcal{V}}_c$$

两本书的公式符号区别：

$E=R$ 且 $p=-Er$ 。因为在《现代机器人学》里，假设 R 是 A 坐标系变换到 B 坐标系，p 是 A 坐标系的原点在 B 坐标系中的表示；而 《刚体动力学算法》里，r 是 B 坐标系的原点在 A 坐标系中的表示。

例如伴随变换中的第二行第一列，在 《刚体动力学算法》里: $-\mathbf{E}\mathbf{r}\times$ 。根据 《现代机器人学》里的命题 3.8 $R[\omega ]R^T=[R\omega ]$ 。 因此 ， $-Er\times =-E[r]E^{T}E=-[-p]E=[p]E$ 。到这一步把公式里的 E 全部换成 R ，就能和 《现代机器人学》里的伴随变换矩阵一样了。

---

齐次变换矩阵和伴随变换矩阵的区别

齐次变换矩阵和伴随变换矩阵在机器人学里的物理意义的区别

• 齐次变换矩阵（Homogeneous Transformation Matrix）:
  • 齐次变换矩阵描述了从一个参考坐标系到另一个参考坐标系的刚体变换，即姿态和位置的变换关系。
  • 对于机器人来说，它用于将机械臂末端执行器相对于机器人基座的位姿（平移和旋转）表示为一个矩阵形式。
  • 具体而言，对于一个机器人的末端执行器，其位姿（包括位置和姿态）可以通过齐次变换矩阵 T 表示为一个 4×4 的矩阵。
  • 齐次变换矩阵在机器人学中广泛应用于描述机器人末端执行器的位姿变换，可以用于控制机器人的运动、路径规划和逆运动学求解等问题。
• 伴随变换矩阵（Adjoint Transformation Matrix）:
  • 伴随变换矩阵是齐次变换矩阵在刚体运动过程中的衍生概念。它描述了刚体坐标系的变换对于刚体上一点速度和力矩的影响。
  • 对于一个机器人的末端执行器来说，伴随变换矩阵用于将机器人基座坐标系下的速度和力矩转换到末端执行器坐标系下。这样可以方便地进行力矩控制、扭矩传递和动力学计算等任务。
  • 伴随变换矩阵通常表示为一个 6×6 的矩阵形式。
  • 伴随变换矩阵在机器人学中被广泛应用于运动学和动力学计算，通过将力矩和速度在不同坐标系间进行转换，实现了机器人末端执行器的运动规划和控制。

---

《机器人手册》里的相关内容

另外，在 《机器人手册》 的书里，也有与《刚体动力学算法》空间向量代数理论上的对应关系。

反对称阵

$S(\underline{p})$ 为斜对角阵, 对于任意的 3 维矢量 $\underline{v}$, 满 足 $S(\underline{p})\underline{v}=\underline{p}\times \underline{v}$, 用公式表示为。（反对称阵，类似于前面讲述的 $\mathbf{a}\times$ ）
$$\mathbf{S}(\underline{\mathbf{p}})=\left(\begin{array}{ccc}0& -p_z& p_y\\ p_z& 0& -p_x\\ -p_y& p_x& 0\end{array}\right)$$

变换矩阵求逆

(注：这里的 X 对应的是 《现代机器人学》里的伴随变换矩阵)

设构型 $A$ 相对于构型 $B$ 的位置和姿态通过一个 位置矢量 ${}^B{\underline{p}}_A$ 和一个 $4\times 4$ 的旋转矩阵 ${}^B{\mathbf{R}}_A$ (见 1.2 节) 来描述, 则 ${}^B{\mathbf{X}}_A$ 为
$$\begin{array}{rl}{}^B{\mathbf{X}}_A& =\left(\begin{array}{cc}1& 0\\ \mathbf{S}\left({}^B{\mathbf{p}}_A\right)& 1\end{array}\right)\left(\begin{array}{cc}{}^B{\mathbf{R}}_A& 0\\ 0& {}^B{\mathbf{R}}_A\end{array}\right)\\ & =\left(\begin{array}{cc}{}^B{\mathbf{R}}_A& 0\\ \mathbf{S}\left({}^B{\mathbf{p}}_A\right){}^B{\mathbf{R}}_A& {}^B{\mathbf{R}}_A\end{array}\right)\end{array}$$
它的逆为
$${}^A{\mathbf{X}}_B=\left(\begin{array}{cc}{}^A{\mathbf{R}}_B& 0\\ 0& {}^A{\mathbf{R}}_B\end{array}\right)\left(\begin{array}{cc}1& 0\\ -\mathbf{S}\left({}^B{\mathbf{p}}_A\right)& 1\end{array}\right)$$
（内心os：它怎么也不化简一下。其实相乘一下，跟《现代机器人学》里的伴随变换矩阵的逆变换是一样的。）

直接引用结论：
$$\begin{array}{rl}& \begin{array}{cc}\text{ 有效空间算术公式 }& \\ \text{ 表 达 式 }& \text{ 计 算 }\\ X\mathbf{v}& \left(\mathbf{R}\mathbf{\omega };\mathbf{R}\left({\mathbf{v}}_0-\mathbf{p}\times \mathbf{\omega }\right)\right)\\ {\mathbf{X}}^F\mathbf{f}& \left(\mathbf{R}\left({\mathbf{n}}_o-\mathbf{p}\times \mathbf{f}\right);\mathbf{R}\mathbf{f}\right)\\ {\mathbf{X}}^{-1}& \left({\mathbf{R}}^{\mathrm{T}};-\mathbf{R}p\right)\\ {\mathbf{X}}^{-1}\mathbf{v}& \left({\mathbf{R}}^{\mathrm{T}}\mathbf{\omega };{\mathbf{R}}^{\mathrm{T}}{\mathbf{v}}_O+\mathbf{p}\times {\mathbf{R}}^{\mathrm{T}}\mathbf{\omega }\right)\\ {\left({\mathbf{X}}^F\right)}^{-1}\mathbf{f}& \left({\mathbf{R}}^{\mathrm{T}}{\mathbf{n}}_o+\mathbf{p}\times {\mathbf{R}}^{\mathrm{T}}\mathbf{f};{\mathbf{R}}^{\mathrm{T}}\mathbf{f}\right)\\ {\mathbf{X}}_1{\mathbf{X}}_2& \left({\mathbf{R}}_1{\mathbf{R}}_2;{\mathbf{p}}_2+{\mathbf{R}}_2^{\mathrm{T}}{\mathbf{p}}_1\right)\\ {\mathbf{I}}_1+{\mathbf{I}}_2& \left(m_1+m_2;{\mathbf{h}}_1+{\mathbf{h}}_2;{\overline{\mathbf{I}}}_1+{\overline{\mathbf{I}}}_2\right)\\ \mathbf{I}\mathbf{v}& \left(\overline{\mathbf{I}}\mathbf{\omega }+\mathbf{h}\times {\mathbf{v}}_O;m{\mathbf{v}}_O-\mathbf{h}\times \mathbf{\omega }\right)\\ & (m;{\mathbf{R}}^{\mathrm{T}}\mathbf{h}+m\mathbf{p};{\mathbf{R}}^{\mathrm{T}}\overline{\mathbf{I}}\mathbf{R}-\\ {\mathbf{X}}^{\mathrm{T}}\mathbf{I}\mathbf{X}& \mathbf{S}(\mathbf{p})\mathbf{S}\left({\mathbf{R}}^{\mathrm{T}}\mathbf{h}\right)-\mathbf{S}\left({\mathbf{R}}^{\mathrm{T}}\mathbf{h}+m\mathbf{p}\right)\mathbf{S}(\mathbf{p}))\end{array}\\ & \text{ 注: }{\mathbf{X}}^{\mathrm{T}}\mathbf{I}\mathbf{X}\text{ 的含义参见式 (2.32)。 }\end{array}$$

---

代码解释

成员变量

• E ：一个 $3\times 3$ 的旋转矩阵，对应《现代机器人学》和 《机器人手册》里理论公式的 $\mathbf{R}$ ，对应《刚体动力学算法》里的 $\mathbf{E}$ 。
• r ：三维向量，对应 《现代机器人学》和 《机器人手册》里理论公式的 $ p $ 。对应《刚体动力学算法》里的 $\mathbf{r}$ 。

构造函数

• 默认构造函数里，旋转是单位阵，平移是 0 矢量
• 拷贝构造可以传入 E 和 r

函数 apply

• 计算的表达式 是 $ X * v$ ，计算结果为 (E * w, - E * rxw + E * v)$

• v_rxw 计算的是 $\mathbf{v}-\mathbf{r}\times \mathbf{\omega }$ 。（具体原因参考有效空间算术公式表，或者运动旋量与伴随变换的关系）

• 旋转矩阵与六维向量相乘。

• 输入是一个六维空间矢量，输出也是六维空间矢量

函数 applyTranspose

• 输入是一个六维空间矢量，输出也是六维空间矢量
• 计算的表达式为 $X^T\ast f$ ，返回结果 $ (E^T * n + rx * E^T * f, E^T * f)$

函数 apply

• $X^{\ast }I{X}^{-1}$

函数 applyTranspose

• $X^{T}IX$

函数 applyAdjoint

矩阵构造对应于力矢量的等价变换

给定空间向量
$${\mathbf{f}}_{sp}=\left[\begin{array}{c}\mathbf{n}\\ \mathbf{f}\end{array}\right]$$
该伴随运算的应用定义为
$$
\begin{aligned}
& Adjoint(\boldsymbol{X}) \boldsymbol{f}_{sp} =\left[\begin{array}{cc}
\boldsymbol{E} & -\boldsymbol{E} \boldsymbol{r} \times \
\mathbf{0}& \boldsymbol{E}
\end{array}\right]\left[\begin{array}{c}
\boldsymbol{n}\
\boldsymbol{f}
\end{array}\right]=\left[\begin{array}{c}
\boldsymbol{E}（\boldsymbol{n} - \boldsymbol{r} \times \boldsymbol{f} ）\
\boldsymbol{E}\boldsymbol{f} \
\end{array}\right]

\
\end{aligned}
$$

函数 toMatrix

返回值为运动矢量的变换矩阵
$$\left[\begin{array}{cc}\mathbf{E}& 0\\ -\mathbf{E}\mathbf{r}\times & \mathbf{E}\end{array}\right]$$

函数 toMatrixAdjoint

返回值为力矢量的等价变换
$$\left[\begin{array}{cc}\mathbf{E}& -\mathbf{E}\mathbf{r}\times \\ 0& \mathbf{E}\end{array}\right]$$

函数toMatrixTranspose

返回值为
$$\left[\begin{array}{cc}{\mathbf{E}}^T& -(\mathbf{E}\mathbf{r}\times {)}^T\\ 0& {\mathbf{E}}^T\end{array}\right]$$

函数 inverse

返回值为 （ E 的逆， - E * r ）

运算符重载 *

返回值

$$\begin{gathered} \mathbf{E}{\mathbf{X}}_E \\ {\mathbf{X}}_r+{\mathbf{X}}_E^T\mathbf{r} \end{gathered}$$
对应《现代机器人学》里的公式

Whether we pre-multiply or post-multiply $T_{sb}$ by $T=(R,p)$ determines whether the $\hat{\omega }$-axis and $p$ are interpreted as in the fixed frame { s } \{\mathrm{s}\} {s} or in the body frame { b } \{b\} {b} :
$$\begin{array}{rl}{T}_{s{b}^{\prime }}& =T{T}_{sb}=\mathrm{Trans}(p)\mathrm{Rot}(\hat{\omega },\theta )T_{sb}\\ & =\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_{sb}& p_{sb}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}R{R}_{sb}& R{p}_{sb}+p\\ 0& 1\end{array}\right]\\ T_{s{b}^{\prime \prime }}& =T_{sb}T=T_{sb}\mathrm{Trans}(p)\mathrm{Rot}(\hat{\omega },\theta )\\ & =\left[\begin{array}{cc}{R}_{sb}& p_{sb}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_{sb}R& R_{sb}p+p_{sb}\\ 0& 1\end{array}\right].\end{array}\text{\hspace{1em}(3.66 \& 3.67)}$$

运算符重载 * =

和上述乘号运算符重载基本一致

2.9 空间叉积(Spatial Cross Products)

设 $\mathbf{r}\in {\mathrm{E}}^3$ 是一个欧几里得向量，它以角速度 $\mathbf{\omega }$ 旋转，但没有其他变化。这个向量的导数由众所周知的公式 $\dot{\mathbf{r}}=\mathbf{\omega }\times \mathbf{r}$ 给出。在这种情况下，$\mathbf{\omega }\times$ 类似微分算子，将 $\mathbf{r}$ 映射到 $\dot{\mathbf{r}}$。

事实证明，这个想法可以扩展到空间向量，但我们需要有两个运算符：一个用于运动，一个用于力。因此，设 $\hat{\mathbf{m}}\in {\mathbf{M}}^6$ 和 $\hat{\mathbf{f}}\in {\mathbf{F}}^6$ 是两个以速度 $\hat{\mathbf{v}}\in {\mathbf{M}}^6$ 移动的空间向量（例如， because they are fixed in a rigid body having that velocity），但不会以其他方式改变。我们现在定义了两个新的运算符，$\times$ 和 $x^{\ast }$，它们是满足以下方程：
$$\dot{\hat{\mathbf{m}}}=\hat{\mathbf{v}}\times \hat{\mathbf{m}}\text{\hspace{1em}(2.29)}$$

和

$$\dot{\hat{\mathbf{f}}}=\hat{\mathbf{v}}\times {}^{\ast }\hat{\mathbf{f}}\text{\hspace{1em}(2.30)}$$

运算符 $x^{\ast }$ 可以看作是 $\times$ 的对偶。这两个算子之间的关系类似于坐标变换矩阵 $\mathbf{X}$ 和 ${\mathbf{X}}^{\ast }$ 之间的关系。

Figure 2.5: Spatial cross-product tables

这些算子可以通过图 2.5 所示的乘法表明确定义。这些表格中的每个 条目都是最上面一行所列基础矢量的时间导数，假定其速度等于左边一列所列基矢量的速度。例如，图 2.5(a) 所示坐标系的运动速度为 ${\mathbf{d}}_{Ox}$ ，即绕直线 $Ox$ 以单位角速度旋转。在 δt 时间单位内，该运动对直线 $Ox$ 没有影响，但却使 $Oy$ 在 $y-z$ 平面上旋转 $\delta t$ 弧度。因此，我们有 ${\mathbf{d}}_{Ox}(t+\delta t)={\mathbf{d}}_{Ox}(t)$，但 ${\mathbf{d}}_{Oy}(t+\delta t)={\mathbf{d}}_{Oy}(t)+\delta t{\mathbf{d}}_{Oz}$ 。因此，对时间导数为
$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}{\mathbf{d}}_{Ox}& =\underset{\delta t\to 0}{\lim }\frac{{\mathbf{d}}_{Ox}(t+\delta t)-{\mathbf{d}}_{Ox}(t)}{\delta t}\\ & =\underset{\delta t\to 0}{\lim }\frac{{\mathbf{d}}_{Ox}(t)-{\mathbf{d}}_{Ox}(t)}{\delta t}=0\end{array}$$

和

$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}{\mathbf{d}}_{Oy}& =\underset{\delta t\to 0}{\lim }\frac{{\mathbf{d}}_{Oy}(t+\delta t)-{\mathbf{d}}_{Oy}(t)}{\delta t}\\ & =\underset{\delta t\to 0}{\lim }\frac{{\mathbf{d}}_{Oy}(t)+\delta t{\mathbf{d}}_{Oz}-{\mathbf{d}}_{Oy}(t)}{\delta t}={\mathbf{d}}_{Oz}.\end{array}$$

这就解释了第一张表格中第一行的前两个条目。同样，图 2.5(b) 展示了一个坐标系，它的运动速度等于 ${\mathbf{d}}_x$ ，这意味着它在 $x$ 方向上以单位线速度平移。在 $\delta t$ 个时间单位内，该运动对直线 $Ox$ 没有影响，但却使 $Oy$ 在 $x$ 方向上移动了 $\delta t$ 个长度单位。因此我们可以得${\mathbf{d}}_{Ox}(t+\delta t)={\mathbf{d}}_{Ox}(t)$，可得 ${\dot{\mathbf{d}}}_{Ox}=0$, 但 ${\mathbf{d}}_{Oy}(t+\delta t)={\mathbf{d}}_{Oy}(t)+\delta t{\mathbf{d}}_z$， 可得 ${\dot{\mathbf{d}}}_{Oy}={\mathbf{d}}_z$ 。 这就解释了第一个表格中第四行的前两个条目。所有其他条目的计算方法与此类似。

使用图 2.5 中的表，我们可以推导出 Plücker 坐标系中表示运算符 $\hat{\mathbf{v}}\times$ 和 $\hat{\mathbf{v}}\times \ast$ 的 $6\times 6$ 矩阵为
$${\hat{\mathbf{v}}}_O\times =\left[\begin{array}{c}\mathbf{\omega }\\ {\mathbf{v}}_O\end{array}\right]\times =\left[\begin{array}{cc}\mathbf{\omega }\times & 0\\ {\mathbf{v}}_O\times & \mathbf{\omega }\times \end{array}\right]\text{\hspace{1em}(2.31)}$$

和

$${\hat{\mathbf{v}}}_O{\times }^{\ast }=\left[\begin{array}{c}\mathbf{\omega }\\ {\mathbf{v}}_O\end{array}\right]{\times }^{\ast }=\left[\begin{array}{cc}\mathbf{\omega }\times & {\mathbf{v}}_O\times \\ 0& \mathbf{\omega }\times \end{array}\right]=-{\left({\hat{\mathbf{v}}}_O\times \right)}^{\mathrm{T}}\text{\hspace{1em}(2.32)}$$

两个空间坐标矢量的叉积为

$$\left[\begin{array}{c}\mathbf{\omega }\\ {\mathbf{v}}_O\end{array}\right]\times \left[\begin{array}{c}\mathbf{m}\\ {\mathbf{m}}_O\end{array}\right]=\left[\begin{array}{c}\mathbf{\omega }\times \mathbf{m}\\ \mathbf{\omega }\times {\mathbf{m}}_O+{\mathbf{v}}_O\times \mathbf{m}\end{array}\right]\text{\hspace{1em}(2.33)}$$

和

$$\left[\begin{array}{c}\mathbf{\omega }\\ {\mathbf{v}}_O\end{array}\right]{\times }^{\ast }\left[\begin{array}{c}{\mathbf{f}}_O\\ \mathbf{f}\end{array}\right]=\left[\begin{array}{c}\mathbf{\omega }\times {\mathbf{f}}_O+{\mathbf{v}}_O\times \mathbf{f}\\ \mathbf{\omega }\times \mathbf{f}\end{array}\right]\text{\hspace{1em}(2.34)}$$

表 2.3 列出了这些算子的各种特性。

表 2.3: 空间叉积算子的特性

rbdl 代码

crossm 和 crossf 对应公式 2.31 - 2.34