机器人学——2.4-坐标系的旋转和运动增量

我们已经讨论了如何产生坐标系的运动，其中包含平移和旋转两部分。平移速度代表了坐标系原点位置的变化率，而旋转速度则要更复杂一些。

旋转坐标系

物体在三维空间中旋转时有一个角速度向量$omega=(omega_x, omega_y, omega_z)$。这个向量的方向定义了瞬时转动轴，即在某个特定时间点坐标系旋转所绕的轴。通常情况下，这个轴是随时间变化而改变的。向量长度代表绕该轴的转速，这有点类似于旋转的角轴表示法。力学中有一个众所周知的时变旋转矩阵微分表达式（理论力学）：
$dot{R}(t)=S(omega)R(t)$式中：$R(t)$ 为空间旋转矩阵；$S(w)$ 为角速度矩阵，其具体形式如下：
$egin{matrix} R ext{(}t ext{)}=left( egin{matrix} n_x& o_x& a_x\ n_y& o_y& a_y\ n_z& o_z& a_z\ end{matrix} ight)& Sleft( omega ight) =left( egin{matrix} 0& -omega _z& omega _y\ omega _z& 0& -omega _x\ -omega _y& omega _x& 0\ end{matrix} ight)\ end{matrix}$利用机器人工具箱，可以得到

>> S = skew([1 2 3])
S =
     0    -3     2
     3     0    -1
    -2     1     0

工具箱函数vex具有逆解的功能：将斜对称矩阵转换成一个向量：

>> vex(S)
ans =
     1
     2
     3

那么方程中的 $dot R$ 是什么意思呢？我们用微分近似法可得
$dot{R}approxdfrac{R(t+delta_t)-R(t)}{delta_t}$即
$R(t+delta_t)approxdelta_tdot{R}+R(t)$将其代入方程中，得到
$R(t+delta_t)approxdelta_tS(omega)R(t)+R(t)=(delta_tS(omega)+I_{3 imes 3})R(t)$它描述了标准正交旋转矩阵是如何作为一个角速度的函数变化的。

增量运动

现在考虑一个坐标系经微小旋转从 $R_0$ 变到 $R_1$。这时可以将上个方程写为
$R_1=(delta_tS(omega)+I_{3 imes3})R_0$将上式重新整理后，得
$delta_tS(omega)=R_1R_0^T-I_{3 imes3}$再对其两侧均使用运算符vex，即求 $S(omega)$的逆，得到
$delta_ heta= ext{vex}(R_1R_0^T-I_{3 imes3})$其中，$delta_ heta = delta_tomega$ 是一个三维向量，单位是角度，它表示一个绕世界坐标系的 $x$、$y$ 和 $z$ 三轴的无穷小转动。

我们之前强调过旋转变换的不可交换性，但对于无穷小角度变化的乘法来说却是可交换的。可以用具体数字的例子说明如下：

>> Rdelta = rotx(0.001) * roty(0.002) * rotz(0.003)
Rdelta =
    1.0000   -0.0030    0.0020
    0.0030    1.0000   -0.0010
   -0.0020    0.0010    1.0000

以上结果是与下面的计算相同的（取4位有效数字）：

>> Rdelta = roty(0.002) * rotx(0.001) *rotz(0.003)
Rdelta =
    1.0000   -0.0030    0.0020
    0.0030    1.0000   -0.0010
   -0.0020    0.0010    1.0000

使用前面推导的方程，可以重新获得上面矩阵对应的微小旋转角 $delta_ heta$：

>> vex(Rdelta - eye(3, 3))
ans =
    0.0010
    0.0020
    0.0030

现在给出两个差异极小的位姿 $xi_0$ 和 $xi_1$，可以用一个六维向量来表示它们间的差异：
$delta=Delta(xi_0, xi_1)=(delta_d, delta_ heta)$它由位移增量和旋转增量两部分组成。$deltainmathbb{R}^6$ 的值实际上是用空间速度乘以 $delta_t$ 得到的。如果位姿都用齐次变换矩阵的形式表示，那么位姿差异是
$delta=Delta(T_0, T_1)=left( egin{array}{c} t_1-t_0\[1em] ext{vex}left( R_1R_0^T-I_{3 imes 3} ight)\ end{array} ight)$其中，$T_0=(R_0, t_0)$，$T_1=(R_1, t_1)$。在工具箱中它可以用函数tr2delta求得。
方程的逆运算为
$xi=Delta^{-1}(delta)$而齐次变换表示法的逆运算是
$T=left( egin{matrix} Sdelta _{ heta}& delta _d\ 0_{3 imes 1}& 0\ end{matrix} ight) +I_{4 imes4}$在工具箱里它用函数delta2tr求解。

>> T0 = transl(1,2,3)*trotx(1)*troty(1)*trotz(1);
>> T1 = T0*transl(0.01,0.02,0.03)*trotx(0.001)*troty(0.002)*trotz(0.003)
T1 =
    0.2889   -0.4547    0.8425    1.0191
    0.8372   -0.3069   -0.4527    1.9887
    0.4644    0.8361    0.2920    3.0301
         0         0         0    1.0000

函数 $Delta(cdot)$ 的计算是由工具箱函数tr2delta完成的：

>> d = tr2delta(T0, T1);
>> d'
ans =
    0.0191   -0.0113    0.0301    0.0019   -0.0011    0.0030

该位姿变化（位移）由相对于世界坐标系的平移增量和旋转增量组成。给定这个位移量以及初始位姿，可求得最终位姿为

>> delta2tr(d) * T0
ans =
    0.2889   -0.4547    0.8425    1.0096
    0.8372   -0.3069   -0.4527    1.9859
    0.4644    0.8361    0.2920    3.0351
         0         0         0    1.0000

它非常接近于上面T1给出的真实值，误差是由于位移量不是无穷小这一事实所致。