機器人學——2.4-坐標系的旋轉和運動增量

我們已經討論了如何產生坐標系的運動，其中包含平移和旋轉兩部分。平移速度代表了坐標系原點位置的變化率，而旋轉速度則要更復雜一些。

旋轉坐標系

物體在三維空間中旋轉時有一個角速度向量 $\omega=(\omega_x, \omega_y, \omega_z)$。這個向量的方向定義了瞬時轉動軸，即在某個特定時間點坐標系旋轉所繞的軸。通常情況下，這個軸是隨時間變化而改變的。向量長度代表繞該軸的轉速，這有點類似於旋轉的角軸表示法。力學中有一個眾所周知的時變旋轉矩陣微分表達式（理論力學）：
$\dot{R}(t)=S(\omega)R(t)$式中： $R(t)$ 為空間旋轉矩陣； $S(w)$ 為角速度矩陣，其具體形式如下：
$\begin{matrix} R\text{(}t\text{)}=\left( \begin{matrix} n_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\\ \end{matrix} \right)& S\left( \omega \right) =\left( \begin{matrix} 0& -\omega _z& \omega _y\\ \omega _z& 0& -\omega _x\\ -\omega _y& \omega _x& 0\\ \end{matrix} \right)\\ \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}\approx\dfrac{R(t+\delta_t)-R(t)}{\delta_t}$即
$R(t+\delta_t)\approx\delta_t\dot{R}+R(t)$將其代入方程中，得到
$R(t+\delta_t)\approx\delta_tS(\omega)R(t)+R(t)=(\delta_tS(\omega)+I_{3\times 3})R(t)$它描述了標准正交旋轉矩陣是如何作為一個角速度的函數變化的。

增量運動

現在考慮一個坐標系經微小旋轉從 $R_0$ 變到 $R_1$。這時可以將上個方程寫為
$R_1=(\delta_tS(\omega)+I_{3\times3})R_0$將上式重新整理后，得
$\delta_tS(\omega)=R_1R_0^T-I_{3\times3}$再對其兩側均使用運算符vex，即求 $S(\omega)$的逆，得到
$\delta_\theta=\text{vex}(R_1R_0^T-I_{3\times3})$其中， $\delta_\theta = \delta_t\omega$ 是一個三維向量，單位是角度，它表示一個繞世界坐標系的 $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_\theta$：

>> 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_\theta)$它由位移增量和旋轉增量兩部分組成。 $\delta\in\mathbb{R}^6$ 的值實際上是用空間速度乘以 $\delta_t$ 得到的。如果位姿都用齊次變換矩陣的形式表示，那么位姿差異是
$\delta=\Delta(T_0, T_1)=\left( \begin{array}{c} t_1-t_0\\[1em] \text{vex}\left( R_1R_0^T-I_{3\times 3} \right)\\ \end{array} \right)$其中， $T_0=(R_0, t_0)$， $T_1=(R_1, t_1)$。在工具箱中它可以用函數tr2delta求得。
方程的逆運算為
$\xi=\Delta^{-1}(\delta)$而齊次變換表示法的逆運算是
$T=\left( \begin{matrix} S\delta _{\theta}& \delta _d\\ 0_{3\times 1}& 0\\ \end{matrix} \right) +I_{4\times4}$在工具箱里它用函數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給出的真實值，誤差是由於位移量不是無窮小這一事實所致。