cohen

我们现实中大部分的机器人都可以视作由刚体组合而成，所以我们先讨论刚体的速度。

物理量与数字之间的联系与区别#

在物理的抽象世界中，我们认为真实存在着两种东西，分别是点和向量。

点就是物理世界中物理量的抽象表达，不依赖于任何坐标系而存在。

刚体的位置就可以抽象为一个点，无论是否建立坐标系以及建立什么样的坐标系，机器人的位置都不会随之改变。只要刚体本身是静止的，它在物理世界中的位置 (点)，就是固定的。

向量同样是物理世界中物理量的抽象表达，不同于点的是，它有大小和方向。

刚体的速度、加速度就是一个向量。同样的，无论坐标系怎么改变，速度和加速度代表的向量是不会改变的。

这里有一点需要注意的是，我在说的坐标系和高中物理我们学习过的参考系不是同一件事情。
![[机器人研究中的坐标系与参考系之间的区别与联系 #移动机器人的示例]]

当我们使用物理定律，对物理量进行表达和计算时，并且也不关心物理量具体的大小，只关心物理量之间的关系时，我们不需要坐标系，也不需要数字。

质点 $b$ 在空间运动，我们可以用一个矢量描述它的速度 $v_b$ ，同时我们可以使用速度的合成与分解这一物理定律，任意选取两个矢量，使之满足 $v_{b_1} + v_{b_2} = v_b$ 。
我们可以说质点 $b$ 具有一个速度 $v_b$， 它以 $|v_b|$ 大小的速度沿着 $v_b$ 方向进行运动。
我们也可以说质点 $b$ 具有两个速度 $v_{b_1}$ , $v_{b_2}$ 。

在上述这个例子中，我们没有使用到坐标系和数字，我们使用了一个物理定律用符号表达了三个矢量之间的关系，同时也解释了它们的物理意义。

针对点和向量，我们总是在应用的时候需要关心点到底在哪里，向量的大小是多少，向量方向是什么。

在实际应用过程中，我们当然希望知道机器人的位置在哪里，机器人的速度是多少，它朝着什么方向运动等等。

这个时候我们就需要定义坐标系，由坐标系而来，我们就可以用数字而不只是符号去表示物理量。

刚体的速度#

首先，我们要明确一点，到底什么是刚体的速度？

在学习这门课之前，我对刚体速度的直观理解就是刚体质心的速度以及刚体围绕质心所在轴旋转的角速度，它们整合起来就是刚体的速度。

这显然是不够准确的，所以在这里，我们需要先定义刚体的速度。

首先，我们知道刚体是由无穷多个点组合而成。同时，我们知道刚体中的这些点之间的速度与它在刚体中的位置和其他的参数有关系（比如刚体绕轴旋转的角速度）。

所以，我们可以用隐函数的形式表示刚体中任意一个点 $p_i$ 的速度是 $v_{p_i}=f(p_i, \text{parameters})$ 。

基于这个形式，我们定义刚体的速度就是这里的 $\text{parameters}$ 。

换而言之，只要我们知道了刚体的速度和一个点在刚体上的坐标，我们可以计算这个点的速度。

我们在实际中，一般都不关心刚体的速度。我们关心机械臂的末端速度、小车几何中心的速度、四足机器人身体上中心点的速度等等。
但是如果我们一开始就计算出刚体的速度，那么我们可以非常方便地计算出所有我们想要的点的速度。
反之，一开始就只算一个点，后续还想算其他点的话，就会变得麻烦且混乱。

当然，到这儿为止还太过于抽象了。接下来，我们一步一步的扩展和解释，直到 spatial velocity。

如图 1 所示刚体正在绕着一个固定轴旋转，我们选择刚体中在旋转轴上的一个点 $p_0$ 作为参考点。根据高中物理中角速度与线速度的关系，我们可以知道对于刚体中任意一个点 $p_i$ , 有 $v_{p_i}=\omega \times \overrightarrow{p_0p_i}$。

然后，为了让我们的讨论更具有普适性，向前迈进一步。假设刚体的旋转轴不是固定，而是运动的。

$p_0$ 仍然是刚体中在旋转轴上的一个点，我们选择 $p_0$ 作为描述质点速度的参考点，对于刚体中任意一个点 $p_i$ , 有 $v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}$。

再次提醒，我们目前的描述都是矢量之间的关系，所以不涉及任何坐标系，同时也意味着在任何一个坐标系中这些关系都是满足的。

从物理意义上，我们来看看 $v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}$ 是如何得来的。

首先，本质上来说一个点的速度就是这个点的位置的变化，也就是它的导数，即$v_{p_i}=\dot{p_i}$ 。同样的，$v_{p_0}=\dot{p_0}$ 。

接下来，为方便理解。我们先设坐标系原点 $o$ , 根据矢量合成的原则（对，你没有想错，这是物理规律！不是数学规则），必然有 $\overrightarrow{op_i}=\overrightarrow{op_0}+\overrightarrow{p_0p_i}$ 。

同时， 由于刚体在运动，点 $p_i \text{, } p_0$ 是随时间变化的。并且在任意 $t$ 时刻均有 $\overrightarrow{op_i}(t)=\overrightarrow{op_0}(t)+\overrightarrow{p_0p_i}(t)$ 。

后续我们的符号表示中 $p_i$ 与 $\overrightarrow{op_i}$ 是同一个意思，使用 $p_i$主要是为了强调它与坐标系无关，它本身以及它与其他矢量、点的关系都是无关乎坐标系的。

因此，我们有 $p_i(t)=p_0(t)+\overrightarrow{p_0p_i}(t)$ ，等号左右两边同时求导 $\dot{p_i}=\dot{p_0}+(\overrightarrow{p_0p_i})'$ 。最终，可以得到 $v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}$

$(\overrightarrow{p_0p_i})'$ 中的 $\overrightarrow{p_0p_i}$ 长度始终没有改变（因为这两个点是刚体中的，刚体不会发生形变），只有方向改变了。
矢量 $\overrightarrow{p_0p_i}$ 的导数为$\omega \times \overrightarrow{p_0p_i}$ 。
从物理意义上去理解$\omega \times \overrightarrow{p_0p_i}$ 是 $p_i$ 这个点由角速度 $\omega$ 所带来的线速度。
{ps: 通过绘图加上使用导数基本的定义公式很容易理解这一点}。

我们可以发现， $v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}$ 与 $v_{p_i}= \omega \times \overrightarrow{p_0p_i}$ 区别就是 $v_{p_0}=0$ 与否。

所以无论刚体在空间中是不是在做纯滚动，刚体中任意一点的速度与参考点 $p_0$ 的速度之间的关系都满足 $v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}$

结合 $v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}$ 与隐函数表达式 $v_{p_i}=f(p_i, \text{parameters})$ ，我们根据刚体速度的定义，可以知道，此时刚体的速度是 $(v_{p_0}\text{, }\omega)$。

回顾前文：
我们可以用隐函数的形式表示刚体中任意一个点 $p_i$ 的速度是 $v_{p_i}=f(p_i, \text{parameters})$ 。基于这个形式，我们定义刚体的速度就是其中的 $\text{parameters}$ 。

如图 2，让我们继续往前走第二步，我们选择一个刚体上任意一点 $q$ 作为表述速度的参考点。

这一步的意义是：让我们在刚体速度的表达式中参考点的选择范围更广一些，证明我们的参考点并不一定要选在旋转轴上。

将参考点选在旋转轴上自然很方便，因为可以不用考虑参考点会旋转的问题。

但是在实际工程中，我们很多时候都没办法直接获取旋转轴的速度。
我们只能把测量速度的传感器放在机器上合适的位置，没办法总是安装在旋转轴上。

CallBack：
只要我们知道了刚体的速度和一个点在刚体上的坐标，我们可以计算这个点的速度。
让我们能更广泛的选择参考点，就是为了在众多不完美的现实情况中，也能很好的计算刚体的速度。

则有 $v_{p_i}=v_{q} + \omega \times \overrightarrow{qp_i}$
我们使用物理定律继续来推导一下，这个式子是如何得来的。

对于点 $q$ ，有 $v_{q}=v_{p_0} + \omega \times \overrightarrow{p_0q} \quad\text{(3)}$
对于点 $p_i$，有 $v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i} \quad\text{(4)}$

公式 $(4) - (3)$ 可得

同上，根据刚体速度的定义，此时刚体的速度是 $(v_{q}\text{, }\omega)$。

CallBack:
我们可以用隐函数的形式表示刚体中任意一个点 $p_i$ 的速度是 $v_{p_i}=f(p_i, \text{parameters})$ 。基于这个形式，我们定义 ** 刚体的速度就是其中的 $\text{parameters}$ 。

好了，进行到这里，我们已经完成所有的基础部分。

接下来需要往前迈出重要的第三步，如图 3，在刚体之外，随机选定一个 body-fixed 的点 $q'$ 。

这一步的意义是：让我们在刚体速度的表达式中参考点的选择范围再广一些，证明我们的参考点并不一定要选在刚体上，只要满足 body-fixed 就行。

在上述的第二步中，我们选的是刚体上任意的一点，现在我们选的是刚体之外但是 body-fixed 的任意一点。这二者意义完全一样，其中的关键就是 body-fixed。

这里可以用物理中” 场 “的概念来帮助理解 body-fixed。
比如磁铁的磁场，如果我们在一个磁铁的磁场中选定一个点 A, 那么虽然这个点不在磁铁上，但它依然会随着磁铁运动而运动，也就是所谓的 body-fixed。

所以，基于第二步的结果我们还是能得到 $v_{p_i}=v_{q'} + \omega \times \overrightarrow{q'p_i} \quad\text{(5)}$
再重复一次，这时我们图中刚体的速度就是 $(v_{q'}, \omega)$

好了，让我们走向最终的第四步：让我们在刚体速度的表达式中参考点的选择范围达到极限，证明我们的参考点并不一定要满足 body-fixed。可以是任意的点，无论它动或者不动、是否 body-fixed 等等。

我们选择一个坐标系 $\{A\}$ ，我们可以说每时每刻都有一个 body-fixed 的点与坐标系 $\{A\}$ 的原点 $O_A$ 重合。

我们规定：数学符号 $v_{O_A}(t)$ 代表 t 时刻与点 $O_A$ 重合的 body-fixed 的点 $q'$ 的速度。

这句话很重要，符号$v_{O_A}(t)$ 代表 t 时刻与点 $O_A$ 重合的 body-fixed 的点 $q'$ 的速度。

而不是点 $O_A$ 的速度，点 $O_A$ 的速度我不关心，无论它是多少都无所谓。

所以，根据公式 (5) 我们将其中的 $v_{q'}$ 替换为 $v_{O_A}$ $v_{p_i}=v_{O_A} + \omega \times \overrightarrow{O_Ap_i} \quad\text{(6)}$
又因为点 $O_A$ 是原点，所以如果我在坐标系 $\{A\}$ 下表述这个式子就可以写作 $^{A}v_{p_i}=^{A}v_{O_A} + ^{A}\omega \times ^{A}\overrightarrow{O_Ap_i} \quad\text{(6)}$
$^{A}v_{p_i}=^{A}v_{O_A} + ^{A}\omega \times ^{A}p_i \quad\text{(7)}$

最后一次，根据刚体速度的定义，我们可以知道这个时候刚体的速度是 $(v_{O_A}, \omega)$。

你应该感觉到了，这个 $(v_{O_A}, \omega)$ 很不符合直觉，它肯定不是刚体质心在 XYZ 方向的速度，和绕 XYZ 的角速度，组合而成。它感觉 “很不像” 刚体的速度......
直觉的牺牲是有价值的，有了这个东西。我们后面算多刚体结构的机器人正逆运动学时会非常方便。

在教科书中给 $(v_{O_A}, \omega)$ 的命名就是 spatial velocity 或者 旋量 (twist)。

看到这儿，肯定还是有点迷糊的，我们再多举两个例子。

令 $\omega = 50 \> rad/s$，$|\overrightarrow{O_BO_A}| = 0.04$ . 其中坐标系 $\{A\}$ 是固定不动的世界坐标系，坐标系 $\{B\}$ 是会跟着圆锥一起旋转的。

注意 $\omega = 50 rad/s$ 意思就是圆锥相对着世界坐标系的角速度。

我们先来看在坐标系 $\{A\}$ 的表述下，圆锥的旋量是：$^{A}\nu=\left( \begin{array} {ccc} ^{A}\omega\\ ^{A}v_{O_A} \end{array} \right)$
其中很自然的，$^{A}\omega = \left( \begin{array} {ccc} 0\\ 0 \\ 50 \end{array} \right)$ 。再看 $^{A}v_{O_A}$ 是多少？

CallBack：
符号$v_{O_A}(t)$ 代表 t 时刻与点 $O_A$ 重合的 body-fixed 的点 $q'$ 的速度

所以，我们就直接找出这个 body-fixed 的点 $q'$ 即可。

而 body-fixed 的点和刚体上的点，意义完全一样，它们都随着刚体运动而运动。

那么，刚体上的点 $q'$ 的速度怎么求？ 直接套用公式 (5) $v_{q'}=v_{r} + \omega \times \overrightarrow{rq'}$
其中 $v_r$ 就是参考点速度， $r$ 是参考点。

注意速度 $v$ 和旋量 $\nu$ 是不一样的哈。

我们就可以选一个好用的参考点 $O_B$ 来方便的求 $v_{q'}$，因为我们知道 $v_{O_B}$ 为零。

圆锥只是在旋转，没有平移，所以 $v_{O_B}$ 为零。 ps：这个 $v_{O_B}$ 就是指刚体上 body-fixed 的点 $O_B$ 的速度。
因为我们现在是在 $\{A\}$ 下表述旋量，所以只有 $v_{O_A}$ 是特殊。
再次 CallBack：
符号$v_{O_A}(t)$ 代表 t 时刻与点 $O_A$ 重合的 body-fixed 的点 $q'$ 的速度，不是点 $O_A$ 的速度。

于是，我们可以得到 $^{A}v_{q'}=^{A}v_{O_B} + ^{A}\omega \times \overrightarrow{^{A}O_Bq'}$
不要忘了！ 我们这里的 $^{A} v_{q'}$ 其实就是我们规定的 $^{A}v_{O_A}$ 。

至此，我们得到了旋量表示的刚体圆锥的速度，在坐标系 $\{A\}$ 下的表示为$^{A}\nu=\left( \begin{array} {ccc} 0 \\ 0 \\ 50 \\ 2 \\ 0 \\ 0 \end{array} \right)$
接下来，再来计算一下刚体圆锥的速度，在坐标系 $\{B\}$ 下的表示是多少？

CallBack：
注意，这里我们描述的圆锥的速度始终是相对于世界坐标系的。

我们只是在换不同的坐标系对这个已经存在的物理量进行数学表述。

$^{B}\nu=\left( \begin{array} {ccc} ^{B}\omega\\ ^{B}v_{O_A} \end{array} \right)$
首先是 $^{B}\omega$ ，从图中可以看到 $^{A}\omega$ 是绕 $\widehat {z}$ 旋转的，而 $^{B}\omega$ 是绕 $\widehat {x}$ 轴旋转的。所以，$^{B}\omega = \left( \begin{array} {ccc} 50\\ 0 \\ 0 \end{array} \right)$.

从图中我们可以看出，在圆锥旋转的整个过程里，所有与这个圆锥 body-fixed 的点始终没有变过，一直是圆锥的顶点，而且它既没有旋转又没有平移。所以，$^{B}v_{O_A} = \left( \begin{array} {ccc} 0\\ 0 \\ 0 \end{array} \right)$.
综上所述 $^{B}\nu=\left( \begin{array} {ccc} 50\\ 0 \\0 \\0\\0\\0 \end{array} \right)$
到这里，我们可以引入一个重要的知识点：坐标变换。

既然，我们只是在不同的坐标系表述同一个物理矢量。那么这些不同坐标系的表示，它们之间应该如何转换。这有利于我把我们关心的物理量，转换了便于计算或者写代码的坐标系中。

[[Todo：旋量的坐标变换]]

再来一个移动机器人的例子。

图中描述的是一个移动机器人绕着旋转中心 $r$ 点旋转， $r$ 点在坐标系 $\{s\}$ 中的坐标 $^{s}r = (2,-1,0)$，在坐标系 $\{b\}$ 中的坐标是 $^{b}r=(2,-1.4,0)$ 。旋转的角速度是 $\omega = 2 \> rad/s$ 。

图中共有两个坐标系，世界坐标系 $\{s\}$ 与车身坐标系 $\{b\}$ . 都采用右手原则，其中 $\{s\}$ 的 $z$ 轴是指向纸面外，坐标系 $\{b\}$ 的 $z$ 轴与之相反。

类似圆锥的例子，我们先看坐标系 $\{s\}$ 中的旋量 $^{s}\nu=\left( \begin{array} {ccc} ^{s}\omega\\ ^{s}v \end{array} \right)$

这里的符号 $^{s}v$ 与 $^{s}v_{O_s}$ 同义，后续如果不是明确需要都用 $^{s}v$ 表示 $^{s}v_{O_s}$。

其中，$^{s}\omega = \left( \begin{array} {ccc} 0\\ 0 \\ 2 \end{array} \right)$,

再算一次，坐标系 $\{b\}$ 中的旋量，答案是（过程尝试手算一下以作练习） $^{b}\nu=\left( \begin{array} {ccc} 0\\ 0\\-2\\2.8\\4\\0 \end{array} \right)$

参考资料：
南科大张巍老师的高等机器人控制课程课程官方页面（作业 + PPT）B 站课程视频地址
Modern Robotics: Mechanics, Planning, and Control," Kevin M. Lynch and Frank C. Park, Cambridge University Press, 2017 书籍官方网站（示例程序源码 + PDF 下载）