关于机器人运动学与动力学建模的几点领悟

关于机器人运动学与动力学建模的几点迷思

疑问1：运动学应该从速度开始推还是从位置开始推？

为什么会产生这样的疑问？我们可以从下面这个问题开始！

简单说明这幅图的含义：
我们定义了两个坐标系，惯性系{s}和移动系{b}。s和b分别是两个坐标系的原点，P是由s点指向b点的向量。

现在我们要来考察，两个坐标系原点间的相对速度了！！！

表达形式1：直接写速度的关系式
$$V_b=R_s^b\cdot V_s$$
其中，$V_s$代表了{b}系相对{s}系的速度在{s}系下的表示形式，$V_b$则代表了{b}系相对{s}系的速度在{b}系下的表示形式，而$R_s^b$就是从{s}系转换到{b}系的旋转矩阵。

表达形式2：由位置的关系式推导速度的关系式
位置的关系：
$$P_b=R_s^b\cdot P_s$$
其中，$P_b$是向量P在{b}系下的表达形式，而$P_s$是向量P在{s}系下的表达形式。
对等式左右两侧求导，得到速度的关系：
$$\begin{array}{rl}\dot{P_b}& =\dot{R_s^b}\cdot P_s+R_s^b\cdot \dot{P_s}\\ & ={\omega }_b\times R_s^b\cdot P_s+R_s^b\cdot \dot{P_s}\\ & ={\omega }_b\times P_b+R_s^b\cdot \dot{P_s}\end{array}$$
其中，${\omega }_b$代表{b}系相对{s}系的角速度在{b}系下的表示。

那么现在问题来了，这两种速度关系，哪一种是对的呢？？？

结论是，这两个关系式都是对的，如何理解呢？

首先，我们得澄清，$\dot{P_b}$和$V_b$不是同一个速度，而$\dot{P_s}$和$V_s$是同一个速度。
$\dot{P_b}$这个速度，可以认为是有一个人站在{b}系的原点处，去观察{s}系相对于自身的速度，这是这个人认为自身是不动的。（这个容易想象，因为$P_b$是向量P在{b}系下的表达）
$V_b$这个速度，是{b}系相对{s}系的速度在{b}系下的表示形式，可以认为是一个人站在{s}系的原点去观察{b}系相对于自身的速度，然后将这个速度经由旋转矩阵，在{b}系下表达这个速度。

因此，$\dot{P_b}$是从{b}观测{s}，$V_b$是从{s}观测{b}，这两个速度是不同的。如何理解呢？

我们可以简化上图中的问题，现在让我们假设{b}系相对于{s}系不再有平移速度，而只有旋转速度。我们来考察从{b}观测{s}的速度，以及从{s}观测{b}的速度有什么不同！！！

从{b}观测{s}的速度 ： 因为{b}系相对于{s}系还在旋转，作为{b}系上的观测者，我们会认为自身是不动的，因此我们看{s}系原点的速度一定是$\omega \times P$。这个和地球自转的例子一摸一样（不考虑公转的因素）。地球相对于太阳一直在自转，我们浑然不知，还以为是太阳在围绕地球运动。

从{s}观测{b}的速度 ： 因为{b}系相对于{s}系只有旋转，没有平移了，我们观察{b}系原点的时候，速度肯定为0。

由此可见，这两个速度是完全不同的。

为什么会产生这种分别？主要是因为两个坐标系间产生了相对旋转。

而以同样一套分析的思想，我们可以知道$\dot{P_s}$和$V_s$是同一个速度。
$\dot{P_s}$这个速s度，可以认为是有一个人站在{s}系的原点处，去观察{b}系相对于自身的速度。
$V_s$这个速度，可以认为是一个人站在{s}系的原点去观察{b}系相对于自身的速度，然后将这个速度经由旋转矩阵，在{s}系下表达这个速度。
既然都是从{s}系去观测{b}系，因此速度肯定就是一样的。

理解了上面的论述以后，我们就明白了涉及到旋转的问题里面有两种速度，一是从{b}观测{s}的速度，一是从{s}观测{b}的速度 。换句话说，一是从非惯性系下观测惯性系的速度，一是从惯性系下观测非惯性系的速度。

现在我们来看表达形式1
$$V_b=R_s^b\cdot V_s$$
$V_s$表示的就是从{s}观测{b}的速度在{s}系下的表达，$V_b$表示的就是从{s}观测{b}的速度在{b}系下的表达。这个公式代表的是同一个速度，在不同坐标系下的表示的变换关系。

再看表达形式2
$$\dot{P_b}={\omega }_b\times P_b+R_s^b\cdot \dot{P_s}$$
$\dot{P_b}$表示的是从{b}观测{s}的速度，而$\dot{P_s}$表示的则是从{s}观测{b}的速度。因此，这个公式表达的是两种速度之间的一个转换关系。

下面这是更关键的一个问题
我们在机器人的运动学推导过程中，往往都是直接用表达形式一，直接写出速度关系式，而不是从位置关系式往下推速度关系式。但是，当我们推加速度关系式的时候，却都是从速度关系式往下推的，而不直接写出来加速度的关系式。这到底是为什么呢？

因为我们关心的是机器人在惯性系下的速度和加速度，也就是说我们作为外部观测者，观测机器人的速度和加速度。我们并不需要去考虑机器人观测惯性系（世界系）的速度。从这个意义上说，机器人上安装的加速度计和陀螺仪都是测量的机器人相对于惯性系的加速度和角速度，都是我们关心的量。而机器人上安装的视觉传感器，他如果能直接测量视野中的目标相对于本体的速度，那么这个速度就需要经过一个转换，才能被使用。

总而言之，我们关心的是机器人在惯性系下的速度和加速度，因此我们用表达形式一就可以了。表达形式二是用来考察惯性系和非惯性系下观测到的相对速度的变换关系的。

而对于速度推导到加速度时，为什么不能直接写出加速度的关系式呢？
首先说明，速度推导加速度的形式如下：
$$\dot{V_b}={\omega }_b\times R_s^b\cdot V_s+R_s^b\cdot \dot{V_s}$$
这里的$\dot{V_b}$是 {b}系相对于{s}系的速度在{b}系下的导数 。（这个加速度没有太强的物理含义）
这里的$\dot{V_s}$是{b}系相对于{s}系的速度在{s}系下的导数。（也就是机器人在惯性系下的加速度）
$R_s^b\dot{V_s}$是可以由加速度计直接测量的

其次，如果直接写出加速度的关系式那么就是：
$$A_b=R_s^b\cdot A_s$$
其中，$A_s$是$\dot{V_s}$，也就是{b}系相对于{s}系的速度在{s}系下的导数，可以由加速度计直接测量。（也就是机器人在惯性系下的加速度）
$A_b$是 {b}系相对于{s}系的速度在{s}系下的导数在{b}系下的表示，这个加速度不等于$\dot{V_b}$。

我们可以直接写出加速度的关系式，但是如果这样做，对于分析运动学没有任何帮助。

因为我们想要知道的量是$V_b$，我们需要知道机器人相对于惯性系的速度在机器人自身坐标系下的表示。比如说我有一个轮式机器人，我想知道我相对于地面的速度在我自己坐标系下的表示，这样我就可以建立轮子转速和相对地面的速度之间的关系了。

而我们直接写加速度的关系式，我们求出了$A_b$，但是$A_b̸=\dot{V_b}$，我们无法对其积分，得到$V_b$。

因此我们需要建立$V_b$和$A_s$（或$\dot{V_s}$）的联系，所以我们要从速度推导到加速度。

小结1：

在这个疑问中，我主要纠结的点在于，为什么我们写移动机器人运动学部分的时候公式往往式下面这样的：
$$\begin{array}{c}{V}_s=R_b^s\cdot V_b\\ \dot{V_s}=\omega \times V_s+R_b^s\dot{V_b}\end{array}$$
我感到很困惑，为什么不从位置公式的地方开始推呢？你怎么就直接写出速度的公式来了？你既然可以直接写速度的公式，为什么不可以直接写加速度的公式？

经过这一番思考，我明白了。

首先，你得知道你考察机器人运动学的时候，你关心的是什么？

我们关心只的有三个：

• 机器人相对惯性系的速度在惯性系下的表示，就是$V_s$。
• 机器人相对惯性系的速度在机器人系下的表示，就是$V_b$。
• 机器人相对惯性系的加速度，就是$\dot{V_s}$。

这是因为，我们需要知道机器人相对于惯性系的运动$V_s$和$\dot{V_s}$，另外，我们也需要知道$V_b$，这样对于我们控制机器人才有帮助。

说到这里，我们也就明白了运动学也可以写成这种形式：
$$\begin{array}{c}{V}_b=R_s^b\cdot V_s\\ V_s=\int \dot{V_s}dt\end{array}$$

这种写法也是对的，和上面的那个公式结果应该是相等的。

疑问2：是否该对旋转矩阵求导？

在运动学推导过程中，有些资料里是对旋转矩阵求导的，有些是不对旋转矩阵求导的，有时候就很懵逼，什么时候该求，什么时候不该求呢？

我总结了一下，其实是取决于你看待旋转矩阵的方式。

方式一： 旋转矩阵就是一个算子，它是一个常量，可用来转换坐标系。
方式二： 旋转矩阵就是一个时间的函数，它也是等式中的一个变量。

接下来，我们就详细解释这两种方式的区别。

我们假设有两个坐标系，{s}系和{b}系。同时有一个向量$p$，他在{s}系下表示为$p_s$，在{b}系下表示为$p_b$，$R_b^s$表示从{b}到{s}的旋转矩阵。

由此，我们有：
$$p_s=R_b^s\cdot p_b$$

我们可以把$p_s$和$p_b$，写为如下形式：
$$p_s=[x_s,y_s,z_s{]}^T$$

$$p_b=[x_b,y_b,z_b{]}^T$$

另外，我们可以把$\overrightarrow{p_s}$和$\overrightarrow{p_b}$写成如下形式：

$$\overrightarrow{p_s}=x_s\cdot \overrightarrow{i_s}+y_s\cdot \overrightarrow{j_s}+x_s\cdot \overrightarrow{k_s}$$

$$\overrightarrow{p_b}=x_b\cdot \overrightarrow{i_b}+y_b\cdot \overrightarrow{j_b}+x_b\cdot \overrightarrow{k_b}$$

当我们采用方式一来看待旋转矩阵时：
旋转矩阵就是一个算子，类似于加减乘除，类似于一种操作。
所以，我们可以理解为旋转矩阵就是对向量的一种操作，即：
$$p_s=R_b^s\cdot p_b\rightleftharpoons \overrightarrow{p_s}=\Theta (\overrightarrow{p_b})$$
$\Theta (\cdot )$代表一种对向量的操作，那么我们对等式求导的手，就不用考虑对旋转矩阵求导了。

如何对这个等式求导呢？
因为这两个向量表达在不同的坐标系下，我们在求导的时候，必须统一到一个坐标系下求导。也就是说，要么左右两边都在{s}系下求导，要么都在{b}系下求导，这里我们以在{s}系下求导为例，说明如何进行求导：
$$\begin{array}{rl}\frac{d^s{p}_s}{dt}& =\frac{d^s}{dt}\left(x_s\cdot \overrightarrow{i_s}+y_s\cdot \overrightarrow{j_s}+z_s\cdot \overrightarrow{k_s}\right)\\ & =\frac{d^s{x}_s}{dt}\cdot \overrightarrow{i_s}+x_s\cdot \frac{d^s\overrightarrow{i_s}}{dt}+\frac{d^s{y}_s}{dt}\cdot \overrightarrow{j_s}+y_s\cdot \frac{d^s\overrightarrow{j_s}}{dt}+\frac{d^s{z}_s}{dt}\cdot \overrightarrow{k_s}+z_s\cdot \frac{d^s\overrightarrow{k_s}}{dt}\\ & ={\dot{x}}_s\cdot \overrightarrow{i_s}+{\dot{y}}_s\cdot \overrightarrow{j_s}+{\dot{z}}_s\cdot \overrightarrow{k_s}\\ & ={\dot{p}}_s\end{array}$$

$$\begin{array}{rl}\frac{d^s{p}_b}{dt}& =\frac{d^s}{dt}\left(x_b\cdot \overrightarrow{i_b}+y_s\cdot \overrightarrow{j_b}+z_b\cdot \overrightarrow{k_b}\right)\\ & =\frac{d^s{x}_b}{dt}\cdot \overrightarrow{i_b}+x_b\cdot \frac{d^s\overrightarrow{i_b}}{dt}+\frac{d^s{y}_b}{dt}\cdot \overrightarrow{j_b}+y_b\cdot \frac{d^s\overrightarrow{j_b}}{dt}+\frac{d^s{z}_b}{dt}\cdot \overrightarrow{k_b}+z_b\cdot \frac{d^s\overrightarrow{k_b}}{dt}\\ & ={\dot{x}}_b\cdot \overrightarrow{i_b}+{\dot{y}}_b\cdot \overrightarrow{j_b}+{\dot{z}}_b\cdot \overrightarrow{k_b}+x_b\cdot \omega \times \overrightarrow{i_b}++y_b\cdot \omega \times \overrightarrow{j_b}+z_b\cdot \omega \times \overrightarrow{k_b}\\ & ={\dot{p}}_b+\omega \times p_b\end{array}$$

所以，
$$\dot{\overrightarrow{p_s}}=\Theta (\dot{\overrightarrow{p_b})}=\Theta ({\dot{p}}_b+{\omega }_b\times p_b)$$
即：
$$\dot{p_s}=R_b^s\cdot ({\dot{p}}_b+\omega \times p_b)=R_b^s\cdot {\dot{p}}_b+R_b^s\cdot ({\omega }_b\times p_b)$$

当我们采用方式二来看待旋转矩阵时：
旋转矩阵是个时间的函数，$p_s$和$p_b$也都是时间的函数，那么就有：
$$p_s=R_b^s\cdot p_b\rightleftharpoons p_s(t)=R_b^s(t)\cdot p_b(t)$$
所以，对等号两边求导：
$$\begin{array}{rl}\dot{p_s}(t)& =\dot{R_b^s}(t)\cdot p_b(t)+R_b^s(t)\cdot \dot{p_b}(t)\\ & ={\omega }_s\times R_b^s(t)\cdot p_b(t)+R_b^s(t)\cdot \dot{p_b}(t)\end{array}$$
即：
$$\dot{p_s}={\omega }_s\times R_b^s\cdot p_b+R_b^s\cdot \dot{p_b}=R_b^s\cdot {\dot{p}}_b+R_b^s\cdot ({\omega }_b\times p_b)$$
这里蕴含了一个公式$R\cdot (a\times b)=R\cdot a\times R\cdot b$

算到这里，我们就明白了，两种方式的结论是一摸一样的，是不是很神奇。

小结2：

通过这部分的阐述，我们知道了：
当某些资料上对旋转矩阵求导时，他是把旋转矩阵看成了一个与时间相关的函数；
当某些资料上不对旋转矩阵求导时，他把旋转矩阵看成了一个算子，或者说是一种操作。

所以下次，当我们遇到类似的问题的时候必须注意啦，不能即对旋转矩阵求导了，然后又对向量展开求导，这样就相当于，你即把旋转矩阵当函数，又把他当一种操作，那旋转矩阵好累呀！内心OS：老子不想干了！

领悟3：牛顿定律只在惯性系下成立！

这个不能说是疑问，这个是最近对其产生深刻认识的一件事情。
动能对速度求导是动量
动能对角速度求导是角动量
这些都只能发生在惯性系内
所以，动能必须是相对于惯性系的动能
而速度也是相对于惯性系的速度
动量也是相对于惯性系的动量
这一点千万要注意