1 前言
有时，已知刚体上一些速度信息，想确定刚体当前的状态或者想求刚体的角速度、或者任一点速度。比如，已知任意非共线三点位置与速度，求刚体角速度；已知任意非共线三点的速度方向共线，这时推断刚体运行状态等等。这时可以通过刚体动力学知识来求解。
2 理论
假设刚体上不共线三点的位置与速度分别为\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}_3\)以及\(\mathbf{v}_1、\mathbf{v}_2、\mathbf{v}_3\), 刚体的角速度为\(\omega\), 则根据刚体上速度与角速度关系可以得到
(1)
$$
\begin{cases}
\mathbf{v}_2 = \omega \times (\mathbf{p}_2 – \mathbf{p}_1) + \mathbf{v}_1
\\
\mathbf{v}_3 = \omega \times (\mathbf{p}_3 – \mathbf{p}_1) + \mathbf{v}_1
\end{cases}
$$
联合(1)中的两式有
(2)
$$
\begin{aligned}
(\mathbf{v}_2 – \mathbf{v}_1)\times (\mathbf{v}_3 – \mathbf{v}_1) &=(\omega \times (\mathbf{p}_2 – \mathbf{p}_1))\times (\omega \times (\mathbf{p}_3 – \mathbf{p}_1))
\\
&=( (\mathbf{p}_3 – \mathbf{p}_1))\times (\omega \times (\mathbf{p}_2 – \mathbf{p}_1))\times \omega
\\
&=((\omega \times (\mathbf{p}_2 – \mathbf{p}_1))\cdot (\mathbf{p}_3 – \mathbf{p}_1))\omega
\end{aligned}
$$
在上面的算式里，注意\(((\omega \times (\mathbf{p}_2 – \mathbf{p}_1))\cdot (\mathbf{p}_3 – \mathbf{p}_1))\)为一个数值，由于等式两边均是向量，因此这里必然存在一个缩放系数\(s\)，使得下式成立
(3)
$$
\omega = s(\mathbf{v}_2 – \mathbf{v}_1)\times (\mathbf{v}_3 – \mathbf{v}_1)
$$
将(3)代入(1)里的第1个等式有
(4)
$$
\begin{aligned}
\mathbf{v}_2 &=\omega \times (\mathbf{p}_2 – \mathbf{p}_1) + \mathbf{v}_1
\\
&=s(\mathbf{v}_2 – \mathbf{v}_1)\times (\mathbf{v}_3 – \mathbf{v}_1) \times (\mathbf{p}_2 – \mathbf{p}_1) + \mathbf{v}_1
\\
&\Downarrow
\\
s &=\dfrac{||\mathbf{v}_2-\mathbf{v}_1||^2}{(\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1)\times (\mathbf{p}_2-\mathbf{p}_1)\cdot (\mathbf{v}_2-\mathbf{v}_1)}
\end{aligned}
$$
将(4)代入(3)得到角速度
(5)
$$
\omega = \dfrac{||\mathbf{v}_2-\mathbf{v}_1||^2(\mathbf{v}_2 – \mathbf{v}_1)\times (\mathbf{v}_3 – \mathbf{v}_1)}{(\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1)\times (\mathbf{p}_2-\mathbf{p}_1)\cdot (\mathbf{v}_2-\mathbf{v}_1)}
$$
另外，通过公式(1)第1式可以发现速度与位置满足如下关系
(6)
$$
(\mathbf{v}_2 – \mathbf{v}_1)\cdot (\mathbf{p}_2 – \mathbf{p}_1)=0
$$
也就是说，刚体上任意两点的速度与位置必然满足(6)式，因此在构造数据验证刚体速度与位置关系时，不能随意构造，构造的数据必须满足(6)式，在一些工程应用场景里，这个式子也可以作为速度的修正。
这里将(6)代入(5)有
(7)
$$
\begin{aligned}
\omega &=\dfrac{||\mathbf{v}_2-\mathbf{v}_1||^2(\mathbf{v}_2 – \mathbf{v}_1)\times (\mathbf{v}_3 – \mathbf{v}_1)}{(\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1)\times (\mathbf{p}_2-\mathbf{p}_1)\cdot (\mathbf{v}_2-\mathbf{v}_1)}
\\
&=\dfrac{||\mathbf{v}_2-\mathbf{v}_1||^2(\mathbf{v}_2 – \mathbf{v}_1)\times (\mathbf{v}_3 – \mathbf{v}_1)}{(\mathbf{v}_2-\mathbf{v}_1)\times ((\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1))\cdot (\mathbf{p}_2-\mathbf{p}_1)}
\\
&=\dfrac{||\mathbf{v}_2-\mathbf{v}_1||^2(\mathbf{v}_2 – \mathbf{v}_1)\times (\mathbf{v}_3 – \mathbf{v}_1)}{((\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{v}_3-\mathbf{v}_1))((\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_2-\mathbf{p}_1))-((\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{v}_2-\mathbf{v}_1))((\mathbf{v}_3-\mathbf{v}_1)\cdot (\mathbf{p}_2-\mathbf{p}_1))}
\\
&=\dfrac{(\mathbf{v}_3-\mathbf{v}_1)\times (\mathbf{v}_2-\mathbf{v}_1)}{(\mathbf{v}_3-\mathbf{v}_1)\cdot (\mathbf{p}_2-\mathbf{p}_1)}
\end{aligned}
$$
同理，可以得到\(\omega\)另一种表达式
(8)
$$
\omega = \dfrac{(\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1)}{(\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)}
$$
很明显，(8)成立的条件是\((\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)\ne 0\)。这时需要确定\((\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)=0\)时\(\omega\)的取值。
要使得\((\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)=0\), 这里存在如下三种情况
(9)
$$
\begin{cases}
①\mathbf{p}_3-\mathbf{p}_1=\mathbf{0}
\\
②\mathbf{v}_2-\mathbf{v}_1=\mathbf{0}
\\
③\mathbf{p}_3-\mathbf{p}_1\ne\mathbf{0},\mathbf{v}_2-\mathbf{v}_1\ne\mathbf{0},\mathbf{p}_3-\mathbf{p}_1 \perp \mathbf{v}_2-\mathbf{v}_1
\end{cases}
$$
对于第①种情况，因为三点不共线，因此这种情况不成立。
对于第②种情况，如果要成立，则对于刚体上任意一点\(\mathbf{p}\)以及其速度\(\mathbf{v}\)有如下等式成立
(10)
$$
\mathbf{v}-\mathbf{v}_1=\omega \times (\mathbf{p}-\mathbf{p}_1)=\omega \times (\mathbf{p}-\mathbf{p}_2)
$$
对于公式(10)分两种情况：第一种情况，\(\omega = \mathbf{0}\), 这时\(\mathbf{v}=\mathbf{v}_1\)，即刚体上任意一点速度一致；第二种情况，\(\omega \ne \mathbf{0}\)，这时由公式(10)可知\(\mathbf{v}-\mathbf{v}_1\)垂直于\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}\)组成的平面，由于\(\omega\)也与\(\mathbf{v} – \mathbf{v}_1\)垂直，因此\(\omega\)必然在\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}\)组成的平面上，由于\(\mathbf{p}\)的任意性，\(\omega\)不可能同时多个平面上，因此这种情况不存在。
最终，②的条件成立时，\(\omega=\mathbf{0}\)，这也说明刚体上任意不同位置两点的速度一样，则刚体这时仅做平移运动，刚体上任意一点的速度大小和方向一致。
对于第③种情况，如果要成立，结合公式(6)可以发现\(\mathbf{v}_2-\mathbf{v}_1\)垂直于\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}_3\)组成的平面, 这里对于(1)的第2式做如下变换
(11)
$$
\begin{aligned}
(\mathbf{v}_3-\mathbf{v}_1)\cdot (\mathbf{p}_2-\mathbf{p}_1)&=\omega \times (\mathbf{p}_3 – \mathbf{p}_1) \cdot (\mathbf{p}_2-\mathbf{p}_1)
\\
&=(\mathbf{p}_2-\mathbf{p}_1) \times \omega \cdot (\mathbf{p}_3-\mathbf{p}_1)
\\
&=-(\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)
\\
&=0
\end{aligned}
$$
通过(11)可以发现\(\mathbf{v}_3-\mathbf{v}_1\)与\(\mathbf{p}_2-\mathbf{p}_1\)垂直，而通过(6)的性质可以发现\(\mathbf{v}_3-\mathbf{v}_1\)也与\(\mathbf{p}_3-\mathbf{p}_1\)垂直，因此\(\mathbf{v}_3-\mathbf{v}_1\)也垂直于\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}_3\)组成的平面，则可以得到\(\mathbf{v}_3-\mathbf{v}_1\)与\(\mathbf{v}_2-\mathbf{v}_1\)线性相关，这里不妨设置\(\mathbf{v}_3-\mathbf{v}_1=\alpha(\mathbf{v}_2-\mathbf{v}_1)\), 结合公式(1)有
(12)
$$
\begin{aligned}
\alpha ||\mathbf{v}_2-\mathbf{v}_1||^2&=(\mathbf{v}_3-\mathbf{v}_1)\cdot (\mathbf{v}_2-\mathbf{v}_1)
\\
&=(\omega \times (\mathbf{p}_3-\mathbf{p}_1))\cdot (\omega \times (\mathbf{p}_2-\mathbf{p}_1))
\\
&=(\omega \times (\mathbf{p}_3-\mathbf{p}_1) \times \omega)\cdot (\mathbf{p}_2-\mathbf{p}_1)
\\
&=0
\end{aligned} \Rightarrow \alpha = 0
$$
通过(12)可以得到\(\mathbf{v}_3=\mathbf{v}_1\)，这与③的条件矛盾，因此第③种情况不存在。
综合上面的情况，角速度可按如下方式求解
(13)
$$
\omega=\begin{cases}
\mathbf{0} &,\mathbf{v}_1=\mathbf{v}_2 \text{ or } \mathbf{v}_2=\mathbf{v}_3 \text{ or } \mathbf{v}_3=\mathbf{v}_1
\\
\dfrac{(\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1)}{(\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)} &,\mathbf{v}_1\ne\mathbf{v}_2 \text{ or } \mathbf{v}_2\ne\mathbf{v}_3 \text{ or } \mathbf{v}_3\ne\mathbf{v}_1
\end{cases}
$$
得到了角速度，对于刚体上任意一点\(\mathbf{p}\),其速度\(\mathbf{v}\)可按如下公式求得
(14)
$$
\mathbf{v}=\omega\times(\mathbf{p}-\mathbf{p}_1)+\mathbf{v}_1
$$
3 进阶
3.1 三个点速度线性相关
对于刚体上不共线的三点\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}_3\)，速度分别为\(\mathbf{v}_1、\mathbf{v}_2、\mathbf{v}_3\)，这些速度均不为\(\mathbf{0}\), 是否存在这么一种场景，它们的速度不一样，但是它们两两之间的速度线性相关？
假设存在上面这种场景，则存在非0系数\(\alpha、\beta\)使得下式成立
(15)
$$
\begin{cases}
\mathbf{v}_2=\alpha \mathbf{v}_1
\\
\mathbf{v}_3=\beta \mathbf{v}_1
\end{cases}
$$
将(15)代入(8)可以得到
(16)
$$
\begin{aligned}
\omega &=\dfrac{(\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1)}{(\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)}
\\
&=\dfrac{(\alpha – 1)(\beta-1)\mathbf{v}_1\times \mathbf{v}_1}{(\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)}
\\
&=\mathbf{0}
\end{aligned}
$$
从(16)式可以发现，刚体这时没有旋转，即刚体各个点速度应该一致，这个和假设矛盾。
因此，可以得到结论：刚体上任意不共线的三点，如果它们之间的速度线性相关，则刚体这时仅做平移运动，刚体上任意一点的速度大小和方向一致。
3.2 两个点速度方向线性相关
对于刚体上不共线的三点\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}_3\)，速度分别为\(\mathbf{v}_1、\mathbf{v}_2、\mathbf{v}_3\)，这些速度均不为\(\mathbf{0}\), 是否存在这么一种场景，仅两个点的速度线性相关，第三个点的速度不与另外两个点的速度线性相关？
假设存在上面这种场景，不失一般性，设仅\(\mathbf{v}_1、\mathbf{v}_2\)线性相关，则存在非0系数\(\alpha\)使得下式成立
(17)
$$
\mathbf{v}_2=\alpha \mathbf{v}_1
$$
将(17)代入(8)有
(18)
$$
\omega = \dfrac{\mathbf{v}_1\times \mathbf{v}_3}{\mathbf{v}_1\cdot (\mathbf{p}_3-\mathbf{p}_1)}
$$
对于(18)这里分两种情况讨论。
第一种情况:\(\mathbf{v}_1\cdot (\mathbf{p}_3-\mathbf{p}_1)=0\)，由于\(\mathbf{v}_1、\mathbf{v}_2\)线性相关，结合公式(6)可以发现\(\mathbf{v}_1、\mathbf{v}_2\)垂直于\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}_3\)组成的平面，如果 \(\omega = \mathbf{0}\), 这和第3个点速度线性无关的假设相悖，因此这时只能 \(\omega \ne \mathbf{0}\), 而在此时，通过公式(1)的第1式可以发现 \(\mathbf{v}_1\)与 \(\omega\)垂直，因此\(\omega\)在\(\mathbf{p}_1、\mathbf{p}_2、\mathbf{p}_3\)组成的平面上，而这时(1)的第2式里的 \(\omega \times (\mathbf{p}_3 – \mathbf{p}_1)\)将得到一个和 \(\mathbf{v}_1\)线性相关的向量，因此这时的\(\mathbf{v}_3\)将与\(\mathbf{v}_1\)线性相关，这与假设相悖，因此第一种情况不成立。
第二种情况:\(\mathbf{v}_1\cdot (\mathbf{p}_3-\mathbf{p}_1)\ne0\)，这时结合(6)(17)可以得到
(19)
$$
\begin{aligned}
0&=(\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_2-\mathbf{p}_1)
\\
&=(\alpha-1)\mathbf{v}_1\cdot (\mathbf{p}_2-\mathbf{p}_1)
\\
&\Downarrow
\\
0&=\mathbf{v}_1\cdot (\mathbf{p}_2-\mathbf{p}_1)
\end{aligned}
$$
从(19)式可以看出，当\(\mathbf{v}_1\)的方向不与\(\mathbf{p}_2、\mathbf{p}_1\)连线方向垂直时，结论就会与假设冲突，因此这里仅讨论速度方向与点位置连线垂直的情况，进一步这里根据速度关系有
(20)
$$
\begin{aligned}
\mathbf{v}_2 &=\omega (\mathbf{p}_2-\mathbf{p}_1)+\mathbf{v}_1
\\
&=\dfrac{\mathbf{v}_1\times \mathbf{v}_3}{\mathbf{v}_1\cdot (\mathbf{p}_3-\mathbf{p}_1)}\times (\mathbf{p}_2-\mathbf{p}_1)+\mathbf{v}_1
\\
&=(1-\dfrac{\mathbf{v}_3\cdot (\mathbf{p}_2-\mathbf{p}_1)}{\mathbf{v}_1\cdot (\mathbf{p}_3-\mathbf{p}_1)})\mathbf{v}_1
\\
&=\alpha \mathbf{v}_1
\end{aligned}
\Rightarrow \alpha =1-\dfrac{\mathbf{v}_3\cdot (\mathbf{p}_2-\mathbf{p}_1)}{\mathbf{v}_1\cdot (\mathbf{p}_3-\mathbf{p}_1)}
$$
因此，这里有如下结论：
①刚体上，有两点的速度线性相关，且速度方向不与两点位置连线垂直，则刚体这时仅做平移运动，刚体上任意一点的速度大小和方向一致。
②刚体上，有两点的速度线性相关，其它任意非共线点的速度均不与此两点的速度线性相关，则这两点的速度方向一定与两点位置的连线垂直，且其速度大小满足(17)(20)式。
从上面的结论，可以解释我们平时开车时，如果车非直线行驶，我们转动方向盘，两个前轮转过的角度不一样(轮速方向不一样)。很明显，在车辆坐标系下，如果前轮胎不与车侧面平行，则这时轮速方向不与前左右车轮轴心连线垂直，根据上面②的结论，可以反推出前左右车轮轮速方向不一致。这里我们使用刚体动力学理论解释了这个现象。
3.3 阿克曼几何车模型
常见的四轮汽车阿克曼转向模型中，车两前轮可以单独转向，车两后轮只能沿着车身方向转动，在不考虑打滑，且所有车轮轴中心在一个平面上等理想情况下，能否通过两后轮轮速求出当前车的角速度呢？
这里以车两后轮轴中心为原点，车身右侧为x正轴，车身前进方向为y正轴，向上为z正轴建立车辆局部坐标系。且已知左右轮轴中心距离为\(l\),前后轮轴中心距离为\(h\),左后轮轮速为\(v_1\),右后轮轮速为\(v_2\)。
这里记左后轮中心位置为\(\mathbf{p}_1=[-\dfrac{l}{2},0,0]\),右后轮中心位置为\(\mathbf{p}_2=[\dfrac{l}{2},0,0]\),左前轮中心位置为\(\mathbf{p}_3=[-\dfrac{l}{2},h,0]\),左后轮轮速为\(\mathbf{v}_1=[0,v_1,0]\),右后轮轮速为\(\mathbf{v}_2=[0,v_2,0]\)。
这里设左前轮轮速为 \(\mathbf{v}_3=[v_x,v_y,0]\), 车辆角速度为\(\omega\)。
结合公式(13)(14)可以得到
(21)
$$
\begin{cases}
\omega &= \dfrac{(\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1)}{(\mathbf{v}_2-\mathbf{v}_1)\cdot (\mathbf{p}_3-\mathbf{p}_1)}
\\
&=\dfrac{1}{h}\begin{bmatrix}0\\1\\0\end{bmatrix}^\wedge(\mathbf{v}_3-\mathbf{v}_1)
\\
&=-\dfrac{1}{h}\begin{bmatrix}0\\0\\v_x\end{bmatrix}
\\
\\
\mathbf{v}_2-\mathbf{v}_1&=\omega\times(\mathbf{p}_2-\mathbf{p}_1)
\\
&=(\mathbf{p}_1-\mathbf{p}_2)^\wedge \omega
\\
&=-\dfrac{l}{h}\begin{bmatrix}1\\0\\0\end{bmatrix}^\wedge\begin{bmatrix}0\\1\\0\end{bmatrix}^\wedge(\mathbf{v}_3-\mathbf{v}_1)
\\
&=-\dfrac{l}{h}\begin{bmatrix}0&0&0\\1&0&0\\0&0&0\end{bmatrix}\begin{bmatrix}v_x\\v_y-v_1\\0\end{bmatrix}
\\
&=\begin{bmatrix}0\\v_2-v_1\\0\end{bmatrix}
\\
&\Downarrow
\\
v_x&=-\dfrac{h}{l}(v_2-v_1)
\\
\omega&=\begin{bmatrix}0&0&\dfrac{v_2-v_1}{l}\end{bmatrix}^T
\end{cases}
$$ 注意，可以通过同样的方式导出右前轮在x方向的分量依然为\(-\dfrac{h}{l}(v_2-v_1)\),即两个前轮轮速在x方向的分量一直一致。
最终，从(21)的结果可知，在阿克曼转向模型下，已知汽车两个后轮的轮速，则可以计算车的角速度，进而可以计算出车身任意一点的速度。
4 附加
在原坐标系下刚体的角速度为\(\omega\)，将原坐标系下的变量先旋转\(R\)再偏移\(\mathbf{t}\)到新坐标系下，这时刚体角速度变成了\(\omega'\)，求这时新坐标系下角速度的表示。这里根据(8)可以得到
(22)$$
\begin{aligned}
\omega'&=\dfrac{(R(\mathbf{v}_2-\mathbf{v}_1))\times (R(\mathbf{v}_3-\mathbf{v}_1))}{(R(\mathbf{v}_2-\mathbf{v}_1))\cdot(R(\mathbf{p}_3-\mathbf{p}_1))}
\\
&=\dfrac{R((\mathbf{v}_2-\mathbf{v}_1)\times (\mathbf{v}_3-\mathbf{v}_1))}{(\mathbf{v}_2-\mathbf{v}_1)\cdot(\mathbf{p}_3-\mathbf{p}_1)}
\\
&=R\omega
\end{aligned}
$$