《机器人与数字人：基于MATLAB的建模与控制》——3.2节线速度和角速度

3.2线速度和角速度
为了求得给定的3×1位置向量p∈瘙綆3所表示平移运动的瞬时速度，即通常的刚体运动的线速度，可以简单地对位置向量p按时间求导，即v=。但是在求导之前，需要先完成一些准备工作，必须先把位置向量p投影到一个固定的基础坐标系上，而不能是移动的坐标系上。也就是说，如果pi当前投影到一个非固定坐标系i上，那么必须找到坐标系i相对于基础坐标系的方向矩阵Rib，在计算线速度vb=b之前，需要先求pb=Ribpi。原因很明显，vb=b=ibpi+Ribi≠Ribi，除非Rib是常数矩阵，然而计算很繁琐而且这表明坐标系i是不动的。
相比之下，求解一个坐标系i相对于基础坐标系的角速度ω的过程，比线速度求解过程要复杂得多。因为并没有一个有效的3×1向量来唯一表达坐标系的旋转运动。只有数学中的SO(3)群可以唯一地、稳妥地表示坐标系的方向和旋转运动。一般来说，一个3×1的角速度ω并不完全是某个3×1向量的时间导数。换言之，不存在角位置向量ρ∈瘙綆3，使得ω=，除非旋转运动是绕着一个固定轴进行的，如二维自旋。当使用外微积分时，第一类微分σ=ωdt 是不恰当的，也通常是不闭合的。
为了更好地理解和洞察三维旋转运动和方向的本质，需要寻找角速度ω的传统定义和旋转矩阵R∈SO(3)的时间导数之间的关系。
基于旋转矩阵R∈SO(3)的正交特性，可以得到，RRT = I。两边分别对时间求导，可得出：

RT+RT＝O
式中，O是一个3×3的零矩阵。通过这个结果，立即可知RRT是一个斜对称矩阵。另一方面，回顾第2章中介绍并讨论的k过程的概念，对于一个固定的单位轴k，角速度的传统定义应该解释成
ω=·k（38）
或者其相应的斜对称矩阵
S(ω)=ω×=Ω=K
换言之，角速度通常定义为一个坐标系绕着某个固定轴的旋转速率在其坐标系上的投影。
现在，设K轴在一个确定的旋转周期中是固定不动的，运用式（28），并将式（28）对时间t求导，有
＝cosK+sinK2(39)

用RT右乘，其中RT可以在式（210）中求出，同时注意第2章中所有的单位斜对称矩阵K所拥有的特性，最后可以得到

RT＝（cosK+sinK2）［I-sinK+(1-cos)K2］=K=S(ω)=Ω
(310)

这个简洁的公式，即Ω=RT，直接得到一个关于的坐标系的方向变化速率与传统坐标系旋转角速度ω之间的关系。
为了更好地理解由式（310）得到的式子Ω=RT的几何意义，用矩阵R10表示给定坐标系1相对于基础坐标系0的方向。希望坐标系1通过改变方向得到坐标系2，坐标系2相对于基础坐标系的方向用矩阵R20表示。所以，两个方向的“差”可以表示为R21=R01R20=(R10)TR20，在k过程中，将式（29）和式（211）应用在R21上，能够求出做这样方向变化的和k。直观看来，这种方向的变化既可以用矩阵R21关于时间的变化率来表示，也可以用式（38）中定义的传统角速度ω=k来表示。
现在，式（310）给出了一种重要的、紧密的关系式。因为单位向量k是关于坐标系1的，所以Ω=ω×也应该投影到坐标系1上，而不是基础坐标系上。注意，当利用k过程从矩阵R21中确定了和k以后，单位向量k就要作为固定的轴保持不动，直到R21变化完成。如果方向再次变化，如从坐标系2到坐标系3的变换R32，那么要再次计算出新的和 k。一般来说，新的k不必和第一次产生的相同。这意味着式（38）定义的传统角速度中的单位向量k是暂时固定的，但并不是一直不变的。

举一个简单例子，假设一个坐标系绕本身的z轴以ρ rad/s的角速度旋转。根据式（22），这个旋转运动的矩阵可以写成：

R=cosρt-sinρt0

sinρtcosρt0

001

其时间导数为
＝-ρsinρt-ρcosρt0

ρcosρt-ρsinρt0

000
那么，很容易证明乘积
RT=Ω=0-ρ0

ρ00

000

是一个斜对称矩阵，其相应向量刚好是这个特定坐标系绕z轴旋转的三维角速度ω=(00ρ)T，这个向量只有z分量是非零的。
又如，计算欧拉角的导数，并证明从式（310）导出的简洁的等式Ω=RT的实用性。设一个RollPitchYaw欧拉角形成的旋转矩阵为R。根据式（37）有

R＝R(z,)R(y,θ)R(x,ψ)

对时间求导数可得：
=(z,)R(y,θ)R(x,ψ)+R(z,)(y,θ)R(x,ψ)+R(z,)R(y,θ)(x,ψ)
相应的，用斜对称矩阵形式表示的角速度变成了
Ω=RT=(z,)R(z,)T+R(z,)(y,θ)R(y,θ)TR(z,)T

+R(z,)R(y,θ)(x,ψ)R(x,ψ)TR(y,θ)TR(z,)T(311)
等式右边每一项都包含绕各自轴旋转的RT因子，这些因子位于每项的中间，而且加了下划线。首先简化一下等式：
（z,）R(z,)T=0-10

100
000,(y,θ)R(y,θ)T=001
000
-100
60

以及

(x,ψ)R(x,ψ)T=000
00-1
010

然后，将上面的因子代入式（311），结合一些乘法运算，得到的第一项与上面关于的第一项是一样的，而关于的第二项为

c-s0
sc0
001001
000
-100cs0
-sc0
001=00c
00s
-c-s0

关于的第三项为

ccθ-scsθ

scθcssθ

-sθ0θ

000
00-1
010

ccθscθ-sθ

-sc0

csθssθcθ

=
0sθscθ

-sθ0-ccθ

-scθccθ0

将它们加在一起，并把斜对称矩阵和的形式转换成向量形式，可以得到：

w=-s+ccθ

c+scθ

-sθ＝0-sccθ

0cscθ

10-sθ

(312)

这个结果表明了RollPitchYaw欧拉角的时间导数和角速度ω之间的关系。因为式（312）右边的一列()T时间导数的3×3系数矩阵不再是单位矩阵或常数矩阵，所以第一类微分σ=ωdt几乎是不成立的。换句话说，欧拉角的角速度ω不可能是RollPitchYaw欧拉角的时间导数。
按照相同的过程，可以求出zyz欧拉角的导数和角速度ω之间的关系。这项工作留作本章结尾部分的练习题。
在将来的计算过程中，通常有这样一个问题：如果一个斜对称矩阵S(a)=a×变化了，其相应的向量a是否会被投影到一个新的坐标系上？

类似的，在式（310）中，怎样将关于坐标系1的角速度ω1对应的斜对称矩阵Ω1转换成关于基础坐标系的Ω0？假设重新投影用旋转矩阵R来表示，希望知道S(Ra)的表达式是什么。事实上，可以使用S(Ra)R和RS(a)的元素间相乘的展开来证明，即

S(Ra)=RS(a)RT（313）
以上分析揭示了一个事实：一个向量重新投影以后，它的新的斜对称矩阵和原始的矩阵通过一个正交变换相关联。显然，式（313）的两边都是斜对称矩阵。所以根据式（313），因为ω0=R10ω1，所以
Ω0=S(ω0)=R10Ω1(R10)T=R10Ω1R01
很明显，因为Ra×Rb=S(Ra)Rb=RS(a)RTRb=RS(a)b，式（313）也就意味着
R(a×b)=Ra×Rb（314）
对于任意两个向量a,b∈瘙綆3，会通过一个旋转矩阵R∈SO(3)重新投影。
进一步观察看出，根据式（38）的定义，在旋转轴固定的前提下，角速度ω可能正好是某个向量ρ∈瘙綆3的时间导数。这主要是因为：如果ρ=k，那么
=k+=ω+

上式意味着，如果=0，则ω=。但是，在k过程中，k轴对于一个给定方向变化R的旋转过程，仅仅是暂时固定的，并不是永久固定的。事实上，在下一章将看到，对于角速度ω，即使投影到一个固定的基础坐标系上，第一类微分ωdt=σ也并不是闭合的，即在许多一般的实例中，它的第二类微分dσ≠0。
此外，如果使用第2章中引用的对偶数值计算，在求解角速度ω时，从数值上计算所需要的更容易，而且不需要任何符号操作。假设一个符号旋转矩阵是机器人的两个关节转角θ1和θ2的函数，即：

R=c1c2s1c1s2

s1c2-c1s1s2

s20-c2

式中，si=sinθi且ci=cosθi，i=1, 2。假设指定一个时间点，θ1=π/6且θ2=π/3，单位为弧度，同时，1=04，2=-06，单位为弧度/秒。用θi+i替代给定矩阵R中的每一个θi，
然后把对偶旋转矩阵R∧=R+拆分出实数部分和对偶部分，利用MATLAB直接就能得到数值上的R和，并进一步得到Ω=RT。
所以，角速度的解ω=(-030000519604000)T。