【中英字幕】现代机器人学 | Modern Robotics_哔哩哔哩 (゜-゜)つロ 干杯~-bilibili​www.bilibili.com/video/BV1KV411Z7sC?p=17

在求解刚体的角速度时，我们使用了  来表示。现在，我们将刚体运动加上了移动，可不可以使用类似的方式得到类似的求解？

任何一个包含线速度和角速度的刚体速度都可以等效于某一螺旋轴的瞬时速度。所以旋转矩阵R对应的是转轴，T对应的为螺旋轴。

螺旋轴的一种表示形式为 { }，其中q为螺旋轴上任一点，  表示螺旋轴方位的单位向量，h是螺旋轴的节距，其值等于线速度/角速度。  表示绕轴转动的角速度。

但我们并不使用这种表达形式，而是使用一个六维向量S来表示螺旋轴。

角速度很好理解，现在来看看螺旋运动的线速度的组成。螺旋运动是一个转动与移动复合的运动：  表示坐标系原点验证螺旋轴转动的速度，  为沿着螺旋轴的转动。

我们定义运动旋量：  ，这里的 为一个六维向量。

当角速度  时，螺旋轴的角速度  ,  ;

当角速度  时（此时为平移），螺旋轴的角速度  , 

【定义】给定参考坐标系，螺旋轴  可写成

式中：  若（1）满足，则  ，式中q为轴上任一点，h为螺旋的节距（若为纯滚动，则h=0）；若（2）满足，则螺旋的节距无穷大，对应的运动旋量为纯移动，参数中只有  。

正如我们的的角速度有  两种定义，我们定义：

物体运动旋量：  , 

空间运动旋量：  ， 

这里我们需要注意的是：物体运动旋量和空间运动旋量表达的是相同的运动，只是参考系不同。物体运动旋量不受空间坐标系选择的影响，空间运动旋量也不受物体坐标系选择的影响。

当我们希望进行运动旋量之间的转换时，发现：

我们知道对于角速度有  和  两种形式。相对应的：

我们将这种左上角为3x3反对称矩阵集合（so(s)），最后一行为4个0形式的4x4实数矩阵称为se(3)。

注：

•  是表示在{b}中的角速度， 是表示在{s}中的角速度。
•  是表示在{b}中与{b}原点重合点的线速度，而 是表示在{s}中与{s}原点重合点的线速 度。

由上面我们可以推出

我们定义给定  ,其伴随矩阵为

这样我们就可以使用伴随矩阵来进行运动旋量间的转换，同样也是螺旋轴  的转换。

 。 

类似于转动指数坐标  ,我们定义  为T的六维指数坐标。  为T的矩阵对数。

下面为旋转与刚体移动的联系与对比图：

矩阵对数算法：

给定（R，p）写作  ,总是能找到  及螺旋轴  (其 中的  或  必为1)，使得  。使得向量 为T的指数坐标，  为T的矩阵对数。

（1）若  ,则  且 

(2)否则，利用R求其矩阵对数来确定  和  。然后，  通过下式计算：

式中

算法应用：

描述：在  平面刚体坐标系{b}移动至坐标系{c}。坐标系{b}和坐标系{c}相对于固定坐标系的描述为：

 和 

坐标系{b}绕着螺旋轴  以  旋转至坐标系{c}。

有， ，得 

求得T，在使用  求得[w]与旋转角度θ，在使用上面的算法求得速度v。代码如下：

T_sb=[cosd(30) -sind(30) 0 1; ...
      sind(30) cosd(30) 0 2; ...
      0 0 1 0; ...
      0 0 0 1]
T_sc=[cosd(60) -sind(60) 0 2; ...
      sind(60) cosd(60) 0 1; ...
      0 0 1 0; ...
      0 0 0 1]
T=T_sc/(T_sb)
R=T(1:3,1:3)
p=T(1:3,4)
theta=acosd(0.5*(trace(R)-1))
w=1/(2*sind(theta))*(R-R')
theta1=deg2rad(theta)
invG=1/(theta1)*eye(3,3)-0.5*w+ ...
    (1/theta1-0.5*cot(theta1/2))*w^2
v=invG*p

结果为：

故，  ，