Modern Robotics — 剛體運動

前言

在三維空間中驻啤，為了指定一個剛體的位置和姿態(tài)农猬，至少需要6個數(shù)。本章通過一個附著到物體上的參考坐標系來描述它的位置和姿態(tài)。這個坐標系相對于固定坐標系的構(gòu)型通過一個4x4的矩陣描述血公。這個矩陣是C-space的隱式表達：剛體的6維C-space空間通過對16維的矩陣施加10個約束獲得。

這樣的一個矩陣不僅可以表示坐標系的構(gòu)型戒职，還可以用來：

移動或旋轉(zhuǎn)一個向量/坐標系
將一個向量/坐標系的描述從一個坐標系轉(zhuǎn)換到另一個坐標系

通過選擇用4x4矩陣來表達構(gòu)型域蜗，以上這些操作就可以由簡單的線性代數(shù)理論完成。這也是為什么我們選擇用矩陣表達構(gòu)型的原因凛剥。

任意一個剛體的構(gòu)型都可以通過從固定參考坐標系出發(fā)侠仇，對一個恒定的速度（twist）積分指定的時間得到。這種運動類似于螺旋運動（screw motion）：圍繞一個固定的軸旋轉(zhuǎn)并沿著該軸作平移運動犁珠÷叽叮基于此，我們可以通過6個參數(shù)描述的指數(shù)坐標（exponential coordinates）來表達構(gòu)型盲憎。這6個參數(shù)可以分為：描述螺旋軸方向的參數(shù)和一個沿著該軸運動路程的標量參數(shù)嗅骄。

1.1 平面上的剛體運動

考慮圖1中的平面物體，其運動被限制在平面中饼疙。圖中參考坐標系為固定不動的坐標系 ${\{ s \}}$ 溺森， $\hat{x} _{s}$ , $\hat{y} _{s}$ 為坐標軸單位向量， $\hat{x} _窑眯$ , $\hat{y} _屏积$ 附著在這個平面物體上，隨著物體運動而運動磅甩，我們稱該坐標系為載體坐標系（body frame）炊林，用 ${\{ b \}}$ 來表示。

圖1 平面物體的構(gòu)型描述

為了描述這個平面物體的構(gòu)型卷要，只需要指定 ${\{ b \}}$ 相對于 ${\{ s \}}$ 的位置和姿態(tài)渣聚。 ${\{ b \}}$ 的原點可通過 ${\{ s \}}$ 的坐標軸表達： ${ p={p} _{x}\hat{x} _{s} +{p} _{y}\hat{y} _{s} }$
在確定參考坐標系的情況下，也可以將向量 $p$ 簡單的記為 $({p} _{x},{p} _{y})$ 僧叉。

描述 ${\{ b \}}$ 相對于 ${\{ s \}}$ 的姿態(tài)最簡單的方式就是指定一個旋轉(zhuǎn)角度 $\theta$ 奕枝。另一種方法是指定 $\hat{x} _$ , $\hat{y} _瓶堕$ 相對于 $\hat{x} _{s}$ , $\hat{y} _{s}$ 的方向： $\hat{x} _隘道=\cos\theta\ \hat{x}_{s} + \sin\theta\ \hat{y}_{s}$ $\hat{y} _=-\sin\theta\ \hat{x}_{s} + \cos\theta\ \hat{y}_{s}$
看起來上面的表達式似乎不是一個很好的用于表達姿態(tài)的方法郎笆，但是想象一下這個物體在三維空間中任意運動谭梗，一個角度 $\theta$ 將不足以描述坐標系的姿態(tài)。實際上我們需要三個角度宛蚓，但目前為止我們還不清楚如何定義這樣一組角度激捏。而將 $\{ b\}$ 的坐標軸用 $\{ s\}$ 坐標軸的系數(shù)來表達卻是很直接的一種方式。

正如點 P 可以表達為一個列向量： $p=\begin{bmatrix} {p}_{x} \\ {p}_{y} \\ \end{bmatrix} \quad$

$\hat{x} _凄吏$ , $\hat{y} _缩幸$ 也可以寫作兩個列向量壹置，組合成一個2x2 的矩陣 $P= \begin{bmatrix} \hat{x} _ & \hat{y} _表谊 \\ \end{bmatrix} \quad=\begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \\ \end{bmatrix} \quad$

矩陣 $P$ 是一個旋轉(zhuǎn)矩陣钞护，盡管 $P$ 包含4個數(shù)字，但它們受到3個約束：每一列是單位向量爆办，且兩列互相垂直难咕。剩余的一個自由度由 $\theta$ 來參數(shù)化。

綜上距辆，將 $P$ 和 $p$ 合并表達為 $(P,p)$ 余佃，可作為坐標系 $\{ b\}$ 相對于 $\{ s\}$ 的位置和姿態(tài)的一種表達。

圖2 {c}在{s}中的表達

如圖2跨算，將 $\{ c\}$ 在 $\{ s\}$ 中表達爆土，得到： $r=\begin{bmatrix} {r}_{x} \\ {r}_{y} \\ \end{bmatrix}\quad R=\begin{bmatrix} \cos\phi & -\sin\phi \\ \sin\phi & \cos\phi \\ \end{bmatrix} \quad$

同樣地，我們也能夠在 $\{ b\}$ 中表達 $\{ c\}$ ： $q=\begin{bmatrix} {q}_{x} \\ {q}_{y} \\ \end{bmatrix}\quad Q=\begin{bmatrix} \cos\psi & -\sin\psi \\ \sin\psi & \cos\psi \\ \end{bmatrix} \quad$
如果我們知道了 $(Q,q)$ （ $\{ c\}$ 相對于 $\{ b\}$ 的構(gòu)型）诸蚕， $(P,p)$ （ $\{ b\}$ 相對于 $\{ s\}$ 的構(gòu)型）步势，那么我們可以計算出 $\{ c\}$ 相對于 $\{ s\}$ 的構(gòu)型：

$R=PQ （將\ Q\ 轉(zhuǎn)換到坐標系\{ s\}\ ）$ $r=Pq+p （將\ q\ 轉(zhuǎn)換到坐標系\{ s\}，并和\ p\ 相加 )$

因此 $(P,p)$ 不僅可以作為 $\{ b\}$ 在 $\{ s\}$ 中的構(gòu)型表達背犯，還能夠?qū)⒁粋€點或坐標系從 $\{ b\}$ 坐標變換到 $\{ s\}$ 坐標坏瘩。

考慮圖3中的一個剛體，坐標系 $\{ d\}$ 和 $\{ c\}$ 固定在它上面漠魏。初始狀態(tài)時倔矾， $\{ d\}$ 和 $\{ s\}$ 重合， $\{ c\}$ 在 $\{ s\}$ 中的表達為 $(R,r)$ 柱锹。而后物體移動哪自，使得 $\{ d\}$ 與 $\{ b\}$ 重合， $\{ b\}$ 在 $\{ s\}$ 中的描述為 $(P,p)$ 禁熏。記 $\{ c'\}$ 的構(gòu)型為 $(R',r')$ 壤巷，可驗證下面的關(guān)系式成立： $R'=PR$ $r'=Pr+p$

與之前的公式很類似，不同點在于這里的 $(P,p)$ 和 $(R,r)$ 都是在 $\{ s'\}$ 下表達的匹层。因此這個表達式不是用于改變坐標隙笆，而是用作剛體位移（rigid-body displacement）锌蓄。圖3中發(fā)生的變換是：（1）根據(jù) $P$ 轉(zhuǎn)動 $\{ c\}$ . (2) 根據(jù) $p$ 移動 $\{ c\}$ 升筏。

綜上瘸爽，一個旋轉(zhuǎn)矩陣-向量對 $(P,p)$ 有3個作用：

表示一個剛體在 $\{ s\}$ 中的構(gòu)型
改變一個向量或坐標系的參考坐標系
移動一個向量或坐標系

圖3 剛體位移

注意到圖3(a)中的先旋轉(zhuǎn)和移動的剛體位移可以直接通過繞一個圖3(b)中的固定點 $s$ 旋轉(zhuǎn)角度 $\beta$ 完成剪决。這是螺旋運動（screw motion）在平面上的特例檀训。因此享言，這個位移可以由3個螺旋坐標描述： $(\beta, {s}_{x}, {s}_{y})$ 。其中 $({s}_{x}, {s}_{y})=(0,2)$ 代表點s在坐標系 $\{ s\}$ 的坐標（螺旋軸指向紙面朝外）荧琼。

另一種表示螺旋運動的方式是將位移看作是以一定的角速度和線速度運動一段距離后產(chǎn)生差牛。觀察圖3(b)命锄，繞s點以一個單位角速度 $\omega=1\ {rad}/s$ 旋轉(zhuǎn)，意味著在點坐標系 $\{ s\}$ 原點處物體上的一點線速度為 $v=({v}_{x},{v}_{y}) =(2,0)$ 偏化。我們可以將這三個數(shù)組合起來用一個向量 $S =(\omega,{v}_{x},{v}_{y})=(1,2,0)$ 來表示脐恩，這是螺旋軸的一種表達。繞該螺旋軸運動一個距離 $\theta = \pi/2$ 將產(chǎn)生最終的位移侦讨。因此可以用三個坐標 $S\theta=(\pi/2,\pi,0)$ 來表達該物體的位移驶冒，我們稱它為平面上的剛體位移指數(shù)坐標(exponential coordinates)。

角速度和線速度統(tǒng)稱為twist搭伤，將螺旋軸 $S =(\omega,{v}_{x},{v}_{y})$ 乘以某個旋轉(zhuǎn)速度 $\dot{\theta}$ 得到速度 $V=S\dot{\theta}$ 只怎。繞螺旋軸 $S$ 旋轉(zhuǎn) $\theta$ 產(chǎn)生的位移和以速度 $\theta=\dot\theta$ 繞螺旋軸 $S$ 旋轉(zhuǎn)一個單位時間得到的位移是相等的，因此 $V=S\dot{\theta}$ 也可以被看作是一組指數(shù)坐標怜俐。

1.2 旋轉(zhuǎn)和角速度

1.2.1 旋轉(zhuǎn)矩陣

旋轉(zhuǎn)矩陣 $R$ 的9個參數(shù)受到6個約束身堡，只有3個獨立參數(shù)。注意到矩陣? $R$ 的三列對應(yīng)的是載體坐標系的三個坐標軸 $\{ \hat{x} _拍鲤 , \hat{y} _贴谎 ,\hat{z} _ \}$ 擅这，因此矩陣? $R$ 需滿足以下三個條件：

單位模長條件： $\hat{x} _仲翎 , \hat{y} _溯香 ,\hat{z} _浓恶$ 都是單位向量
正交條件： ${ \hat{x} _包晰 \cdot \hat{y} _ = \hat{x} _赫模 \cdot \hat{z} _嘴瓤 = \hat{y} _廓脆 \cdot \hat{z} _磁玉=0 }$

這六個約束可以用一個關(guān)系式表達： $R^{T}\cdot R = I$

所有3x3旋轉(zhuǎn)矩陣構(gòu)成的集合稱為特殊正交群（special orthogonal group）蚊伞，用符號 $SO(3)$ 表示时迫。和平面情況一樣掠拳，一個旋轉(zhuǎn)矩陣有以下三個作用：

表示一個旋轉(zhuǎn)
改變一個向量或坐標系的參考坐標系
旋轉(zhuǎn)一個向量或坐標系

這里重點解釋一下第三個作用旋轉(zhuǎn)一個向量或坐標系。如圖4喊熟，初始狀態(tài)下芥牌，坐標系 $\{c\}$ 和 $\{s \}$ 重合壁拉，坐標軸為 $\{ \hat{x} , \hat{y} ,\hat{z} \}$ 弃理，如果我們讓 $\{c\}$ 繞著某個軸 $\hat\omega$ 旋轉(zhuǎn)角度 $\theta$ 案铺，則會得到一個新的坐標系 $\{ c^{'} \}$ 梆靖，其坐標軸為 $\{ {\hat{x}^{'} , \hat{y}^{'} ,\hat{z}^{'}} \}$ 返吻。令旋轉(zhuǎn)矩陣 $R=R_{sc^{'}}$ 表示 $\{c\}$ 相對于 $\{s \}$ 的旋轉(zhuǎn)测僵，那么就可以將 $R$ 看成一個旋轉(zhuǎn)算子捍靠，而不是一個姿態(tài)表達，我們可以寫作： $R=Rot(\hat\omega,\theta)$
這個旋轉(zhuǎn)算子將單位旋轉(zhuǎn)矩陣 $I$ 旋轉(zhuǎn)至 $R$ 磁携。

圖4 旋轉(zhuǎn)矩陣作為算子

繞坐標軸的旋轉(zhuǎn)矩陣如下： $Rot(\hat{x},\theta)= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix} \quad$ $Rot(\hat{y},\theta)= \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \quad$ $Rot(\hat{z},\theta)= \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \quad$
更一般地，對于 $\hat\omega = (\hat\omega_{1},\hat\omega_{2},\hat\omega_{3})$ 烟央， $Rot(\hat{\omega},\theta)= \begin{bmatrix} c_{\theta}+{\hat{\omega}_{1}}^{2}(1-c_{\theta}) & \hat{\omega}_{1}\hat{\omega}_{2}(1-c_{\theta})-\hat{\omega}_{3} s_{\theta} & \hat{\omega}_{1}\hat{\omega}_{3}(1-c_{\theta})+\hat{\omega}_{2} s_{\theta} \\ \hat{\omega}_{1}\hat{\omega}_{2}(1-c_{\theta})+\hat{\omega}_{3} s_{\theta} & c_{\theta}+{\hat{\omega}_{2}}^{2}(1-c_{\theta}) & \hat{\omega}_{2}\hat{\omega}_{3}(1-c_{\theta})-\hat{\omega}_{1} s_{\theta} \\ \hat{\omega}_{1}\hat{\omega}_{3}(1-c_{\theta})-\hat{\omega}_{2} s_{\theta} & \hat{\omega}_{2}\hat{\omega}_{3}(1-c_{\theta})+\hat{\omega}_{1} s_{\theta} & c_{\theta}+{\hat{\omega}_{3}}^{2}(1-c_{\theta}) \end{bmatrix} \quad$
其中粮呢， $s_{\theta}=\sin\theta$ 鬼贱， $c_{\theta}=\cos\theta$ 这难。任意的 $R\in SO(3)$ 都可以由單位矩陣繞某個 $\hat\omega$ 旋轉(zhuǎn)某個 $\theta$ 獲得姻乓。注意一個特性蹋岩， $Rot(\hat\omega,\theta) = Rot(-\hat\omega,-\theta)$ 学少。

假設(shè) $R_{sb}$ 代表坐標系 $\{s \}$ 相對于 $\{ b\}$ 的旋轉(zhuǎn)版确，現(xiàn)在我們想讓 $\{ b\}$ 繞單位向量? $\hat\omega$ 旋轉(zhuǎn)角度 $\theta$ ，該旋轉(zhuǎn)由旋轉(zhuǎn)矩陣 $R=Rot(\hat\omega,\theta)$ 表示侵歇。為了更清楚的表達該旋轉(zhuǎn)惕虑，我們還需要指明旋轉(zhuǎn)軸 $\hat\omega$ 是在 $\{s \}$ 中還是 $\{ b\}$ 中表達溃蔫。令坐標系 $\{ b^{'}\}$ 為 $\{ b\}$ 繞? $\hat\omega_{s}=\hat\omega$ 旋轉(zhuǎn)角度 $\theta$ 得到（ $\hat\omega$ 在固定坐標系中描述）伟叛， $\{ b^{''}\}$ 為 $\{ b\}$ 繞? $\hat\omega_痪伦=\hat\omega$ 旋轉(zhuǎn)角度 $\theta$ 得到（ $\hat\omega$ 在載體坐標系中描述）。這兩個新的坐標系可以通過下面的表達式計算出來： $R_{s癞蚕^{'}} =RR_{s桦山}$ $R_{s恒水^{''}} =R_{s钉凌}R$
也就是說捂人，前乘一個矩陣 $R=Rot(\hat\omega,\theta)$ 代表繞固定坐標系中的 $\hat\omega$ 旋轉(zhuǎn)滥搭，后乘 $R$ 代表繞載體坐標系中的 $\hat\omega$ 旋轉(zhuǎn)瑟匆。如果對一個向量 $v$ 旋轉(zhuǎn)，由于只涉及到 $v$ 所在的坐標系外厂，那么 $\hat\omega$ 也是在該坐標系中描述的，旋轉(zhuǎn)后的向量 $v^{'}=Rv$ 交惯。

1.2.2 角速度

如圖5席爽，假設(shè)一個旋轉(zhuǎn)的物體上固連著一個坐標系，其坐標軸單位向量為 $\{{\hat{x}},\hat{y},\hat{z} \}$ 玖像，現(xiàn)在來求取這三個單位坐標軸對時間的導(dǎo)數(shù)捐寥。注意坐標軸的長度是不變的握恳，只有方向會發(fā)生變化捺僻。研究載體坐標系在 $t$ 和 $t+\Delta{t}$ 時刻的姿態(tài)的變化匕坯，這個變化可以描述為繞過原點的某個軸 $\hat\omega$ 旋轉(zhuǎn)角度 $\theta$ 得到葛峻。注意這個 $\hat\omega$ 目前還沒有指明在哪個坐標系中描述术奖。

圖5 瞬時旋轉(zhuǎn)軸

當趨于0時，就變?yōu)榱诵D(zhuǎn)的速度榛丢，就是瞬時旋轉(zhuǎn)軸晰赞。我們可以將和結(jié)合起來表示為角速度 （angular velocity）：
參考圖5(b)掖鱼，可得到：
為了以坐標形式表達這三個關(guān)系式戏挡，我們需要選定一個參考坐標系。理論上可以選擇任意坐標系拆檬，兩個很自然的選擇是固定坐標系和載體坐標系竟贯。

首先選擇 $\{ s\}$ 為參考坐標系屑那。令旋轉(zhuǎn)矩陣 $R(t)$ 表示載體坐標系在 $t$ 時刻時相對于固定坐標系的姿態(tài)艘款， $\dot{R(t)}$ 是其時間變化率哗咆， $R(t)$ 的第一列 $r_{1}(t)$ 描述了 $\hat{x}$ 軸在固定坐標系下的坐標岳枷， $r_{2}(t)$ 和 $r_{3}(t)$ 分別為 $\hat{y}$ 和 $\hat{z}$ 軸在固定坐標系下的坐標空繁。在某個時刻 $t$ 處，令 $\omega_{s}\in{R}^3$ 為角速度 $\omega$ 在固定坐標系下的表達闷祥，則上面的三個關(guān)系式可以寫為： $\dot{r_{i}}=\omega_{s}\times{r}_{i}, \ \ \ \ \ \ i=1,2,3$
這三個關(guān)系式可寫成矩陣形式： $\dot{R}= \begin{bmatrix} \omega_{s}\times{r}_{1} & \omega_{s}\times{r}_{2} & \omega_{s}\times{r}_{3} \end{bmatrix}=\ \omega_{s}\times{R}$
為了消除右側(cè)的叉乘符號凯砍，我們引入一個新的表示符號悟衩，將 $\omega_{s}\times{R}$ 寫為 $[\omega_{s}]R$ 座泳，其中 $[\omega_{s}]$ 為 $\omega_{s}$ 的 $3\times{3}$ 反對稱（skew-symmetric）矩陣表達：

給定一個向量 $x={ \begin{bmatrix} x_{1} & x_{2} & x_{3} \end{bmatrix}}^{T} \in {R}^{3}$ ，定義矩陣 $[x]$ 為 $x$ 的 $3\times{3}$ 反對稱矩陣表達镇防，即： $[x]=-{[x]}^{T}$
所有 $3\times{3}$ 實反對稱矩陣構(gòu)成的集合叫做 $so(3)$ （李代數(shù)）来氧，它包含了當 $R=I$ 時啦扬，所有可能的 $\dot{R}$ 考传。

反對稱矩陣一個有用的特性如下：給定任意的 $\omega\in{R}^{3}$ 和 $R\in{SO(3)}$ 证鸥，有以下關(guān)系式恒成立： $R[\omega]R^{T}=[R\omega]$

有了新的符號[]枉层，旋轉(zhuǎn)矩陣的導(dǎo)數(shù)就可以寫為： $\dot{R}=[\omega_{s}]R$
兩邊同乘以 $R^{-1}$ ,可得到： $[\omega_{s}]=\dot{R}R^{-1}$

現(xiàn)在我們將 $\omega$ 表達在 $\{ b\}$ 坐標系中鸟蜡，記為 $\omega_揉忘$ 端铛。注意 $\omega_{s}$ 和 $\omega_禾蚕$ 是同一個角速度 $\omega$ 在不同坐標系下的向量表達，由下標消除法則哗总， $\omega_{s}=R_{sb}\omega_讯屈$ 耻煤，因此 $\omega_=R_{sb}^{-1} \omega_{s} = R^{-1}\omega_{s}=R^{T}\omega_{s}$
將上面的關(guān)系式表達為反對稱矩陣形式：
$[\omega_棺妓]=[R^{T}\omega_{s}]$ $\ \ \ \ \ \ \ \ =R^{T}[\omega_{s}]R$ $\ \ \ \ \ \ \ \ \ \ \ =R^{T}\dot{R}R^{T} R$ $\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ =R^{T}\dot{R}=R^{-1}\dot{R}$
綜上怜跑，有兩個關(guān)系式將 $R$ 和 $\dot{R}$ 同角速度 $\omega$ 聯(lián)系起來： $[\omega_{s}]=\dot{R}R^{-1}$ $[\omega_性芬]=R^{-1}\dot{R}$

需要注意兩點：

$[\omega_剧防]$ 并不是相對于正在運動的坐標系來描述的峭拘，而是相對于一個靜止坐標系 $\{ b\}$ 描述，這個靜止坐標系同固結(jié)在運動物體上的某個坐標系在這一瞬時重合辉饱。
$\omega_{s}$ 不依賴于載體坐標系的選擇彭沼， $\omega_姓惑$ 不依賴于固定坐標系的選擇按脚。表面上看乘寒，它們都依賴于 $R$ 和 $\dot{R}$ ，實際上 $\dot{R}R^{-1}$ 的乘積獨立于 $\{ b\}$ 烂翰， $R^{-1}\dot{R}$ 的乘積獨立于 $\{ s\}$ 甘耿。

1.2.3 旋轉(zhuǎn)的指數(shù)坐標表示法

指數(shù)坐標（Exponential Coordinates）是旋轉(zhuǎn)的一種三參數(shù)表達法佳恬。指數(shù)坐標用 $\hat\omega\theta$ 表示，其中 $\hat\omega$ 是旋轉(zhuǎn)軸單位向量垫言， $\theta$ 是旋轉(zhuǎn)角度倾剿。如果分開寫前痘，就變?yōu)榱诵D(zhuǎn)的角-軸（axis-angle）表達法。

旋轉(zhuǎn)矩陣 $R$ 對應(yīng)的指數(shù)坐標 $\hat\omega\theta$ 可作如下的解讀：

旋轉(zhuǎn)軸 $\hat\omega$ 和旋轉(zhuǎn)角度 $\theta$ 坯癣，某一個坐標系初始狀態(tài)同 $\{ s\}$ 重合示罗，繞 $\hat\omega$ 旋轉(zhuǎn) $\theta$ 角度鹉勒，得到的坐標系相對于 $\{ s\}$ 的姿態(tài)可用旋轉(zhuǎn)矩陣 $R$ 表達
在 $\{ s\}$ 中描述的一個角速度 $\hat\omega\theta$ 吵取，某一個坐標系初始狀態(tài)同 $\{ s\}$ 重合皮官，沿著 $\hat\omega\theta$ 運動單位時間（對 $\hat\omega\theta$ 在這段時間內(nèi)積分）捺氢，得到的坐標系相對于 $\{ s\}$ 的姿態(tài)可用旋轉(zhuǎn)矩陣 $R$ 表達
在 $\{ s\}$ 中描述的一個角速度 $\hat\omega$ 剪撬，某一個坐標系初始狀態(tài)同 $\{ s\}$ 重合残黑，沿著 $\hat\omega$ 運動 $\theta$ 時間（對 $\hat\omega$ 在這段時間內(nèi)積分）梨水，得到的坐標系相對于 $\{ s\}$ 的姿態(tài)可用旋轉(zhuǎn)矩陣 $R$ 表達

后兩種解讀方式暗示我們可以用線性微分方程形式來看待指數(shù)坐標。先了解一下向量形式線性微分方程的一些結(jié)論：

設(shè)線性微分方程為 ${\dot{x}}(t)=Ax(t)$ 舅世，初始條件為 $x(0)=x_{0}$ ， $A\in{R}^{n\times{n}}$ 為常矩陣缨硝， $x(t)\in{R^{n}}$ 追葡。那么該方程的解為： $x(t)=e^{At}x_{0}$
其中 $e^{At}$ 稱為矩陣指數(shù)（matrix exponential）： $e^{At}=I + tA + \frac{t^2}{2!}A^{2} + \frac{t^3}{3!}A^{3}+ \dots$

$e^{At}$ 還滿足下面的特性：

$d(e^{At})/dt=Ae^{At}=e^{At}A$
如果 $A$ 可以寫為 $A=PDP^{-1}$ 且 $P$ 可逆宜肉，則 $e^{At}=Pe^{Dt}P^{-1}$
如果 $AB=BA$ 谬返，則 $e^{A}e^{B}=e^{A+B}$ （可以展開后驗證）
$(e^{A})^{-1}=e^{-A}$ （在上面表達式中令 $B=-A$ 可驗證）

如圖6日杈，假設(shè)一個三維向量 $p(0)$ 繞 $\hat\omega$ 旋轉(zhuǎn) $\theta$ 至 $p(\theta)$ 莉擒，可以想象一下這個旋轉(zhuǎn)過程： $p(0)$ 以一個恒定速度1 $rad/s$ 從 $t=0s$ 旋轉(zhuǎn)到 $t={\theta}{s}$ 涨冀，令 $p(t)$ 表示向量頭部走過的路徑，其速度 $\dot{p}$ 為： $\dot{p}=\hat{\omega}\times{p}=[\hat{\omega}]p$
上面的方程是一個線性微分方程扁眯，其解為： $p(t)=e^{[\hat\omega]t}p(0)$
由于 $t$ 和 $\theta$ 是可交換的姻檀，上式可寫為： $p(\theta)=e^{[\hat\omega]\theta}p(0)$
從上式可看出 $e^{[\hat\omega]\theta}$ 的一個作用涝滴，它可以將 $p(0)$ 旋轉(zhuǎn)至 $p(\theta)$ 歼疮， $\theta>0$ 對應(yīng)繞 $\hat\omega$ 逆時針旋轉(zhuǎn)， $\theta<0$ 則順時針旋轉(zhuǎn)默怨。

向量p的旋轉(zhuǎn)

對 $e^{[\hat\omega]\theta}$ 按照定義展開得到： $e^{[\hat\omega]\theta}=I+[\hat\omega]\theta+[\hat\omega]^2\frac{{\theta}^2}{2!}+[\hat\omega]^3\frac{{\theta}^3}{3!}+\dots$
由 $[\hat\omega]^3=-[\hat\omega]$ 匙睹，上式可化簡為：
$e^{[\hat\omega]\theta}=I+\left(\theta-\frac{{\theta}^3}{3!}+\frac{{\theta}^5}{5!}-\dots\right)[\hat\omega]+\left(\frac{{\theta}^2}{2!}-\frac{{\theta}^4}{4!}+\frac{{\theta}^6}{6!}-\dots\right)[\hat\omega]^2$
由 $\sin\theta$ 和 $\cos\theta$ 的級數(shù)展開式可得：
$Rot(\hat\omega,\theta)=e^{[\hat\omega]\theta}=I+\sin\theta[\hat\omega]+(1-\cos\theta)[\hat\omega]^2$
上式稱為羅德里格旋轉(zhuǎn)公式(Rodrigues’ formula)痕檬。

以上，展示了如何通過矩陣指數(shù)丘跌，從 $\hat\omega$ 和 $\theta$ 創(chuàng)建旋轉(zhuǎn)矩陣闭树。如前所述荒澡， $e^{[\hat\omega]\theta}$ 的一個作用单山，它可以將一個向量 $p(0)$ 旋轉(zhuǎn)至 $p(\theta)$ ，同樣的昼接，矩陣 $R$ 由三個列向量構(gòu)成慢睡，那么旋轉(zhuǎn)矩陣 $R'=e^{[\hat\omega]\theta}R$ 就是由 $R$ 繞固定坐標系的 $\hat\omega$ 旋轉(zhuǎn)角度 $\theta$ 獲得膨疏。旋轉(zhuǎn)矩陣 $R''=Re^{[\hat\omega]\theta}$ 就是由 $R$ 繞載體坐標系的 $\hat\omega$ 旋轉(zhuǎn)角度 $\theta$ 獲得佃却。

下面我們將證明窘俺，對于任意的旋轉(zhuǎn)矩陣 $R\in{SO(3)}$ ，我們總能找到一個單位向量 $\hat\omega$ 和標量 $\theta$ 灶泵，使得 $R=e^{[\hat\omega]\theta}$ 赦邻。

1.2.4 旋轉(zhuǎn)的矩陣對數(shù)

如果 $\hat\omega\theta\in{R}^3$ 代表一個旋轉(zhuǎn)矩陣的指數(shù)坐標实檀，那么反對稱矩陣 $[\hat\omega\theta]=[\hat\omega]\theta$ 就是旋轉(zhuǎn)矩陣 $R$ 的矩陣對數(shù)（matrix logarithm）。矩陣對數(shù)是矩陣指數(shù)的逆運算恬吕。矩陣指數(shù)是將角速度的矩陣表達 $[\hat\omega]\theta\in{so(3)}$ 積分1s得到一個旋轉(zhuǎn)矩陣 $R\in{SO(3)}$ 铐料，矩陣對數(shù)是對旋轉(zhuǎn)矩陣 $R\in{SO(3)}$ 微分，得到一個恒定角速度的矩陣表達 $[\hat\omega]\theta\in{so(3)}$ 柒凉，如果對它積分1s篓跛，就會將矩陣 $I$ 旋轉(zhuǎn)至 $R$ 举塔。簡單來說： $\exp:[\hat\omega]\theta\in{so(3)} → R\in{SO(3)}$ $\log:R\in{SO(3)} → [\hat\omega]\theta\in{so(3)}$

1.3 剛體運動和速度（Twist）

這一小節(jié)討論剛體構(gòu)型（configuration）和速度，與1.2節(jié)討論的旋轉(zhuǎn)和角速度在某些方面是類似的计盒。齊次變換矩陣 $T$ 與旋轉(zhuǎn)矩陣 $R$ 類似北启，螺旋軸

最后編輯于：2019.12.09 21:02:49

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者

人面猴
序言：七十年代末咕村，一起剝皮案震驚了整個濱河市蚊俺，隨后出現(xiàn)的幾起案子，更是在濱河造成了極大的恐慌批钠，老刑警劉巖得封，帶你破解...
沈念sama閱讀 218,122評論 6贊 505
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件忙上，死亡現(xiàn)場離奇詭異，居然都是意外死亡茬斧，警方通過查閱死者的電腦和手機，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 93,070評論 3贊 395
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進店門悯恍，熙熙樓的掌柜王于貴愁眉苦臉地迎上來涮毫，“玉大人贷屎，你說我怎么就攤上這事唉侄。” “怎么了恬叹？”我有些...
開封第一講書人閱讀 164,491評論 0贊 354
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵绽昼，是天一觀的道長须蜗。經(jīng)常有香客問我，道長菱农，這世上最難降的妖魔是什么柿估？我笑而不...
開封第一講書人閱讀 58,636評論 1贊 293
?港島之戀（遺憾婚禮）
正文為了忘掉前任官份，我火速辦了婚禮，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘河咽。我一直安慰自己，他們只是感情好飒房，可當我...
茶點故事閱讀 67,676評論 6贊 392
惡毒庶女頂嫁案：這布局不是一般人想出來的
文/花漫我一把揭開白布。她就那樣靜靜地躺著护糖，像睡著了一般嫡良。火紅的嫁衣襯著肌膚如雪献酗。梳的紋絲不亂的頭發(fā)上，一...
開封第一講書人閱讀 51,541評論 1贊 305
城市分裂傳說
那天很澄，我揣著相機與錄音颜及，去河邊找鬼俏站。笑死，一個胖子當著我的面吹牛爱葵，可吹牛的內(nèi)容都是我干的反浓。我是一名探鬼主播，決...
沈念sama閱讀 40,292評論 3贊 418
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼辆雾，長吁一口氣：“原來是場噩夢啊……” “哼度迂！你這毒婦竟也來了猜揪？” 一聲冷哼從身側(cè)響起，我...
開封第一講書人閱讀 39,211評論 0贊 276
萬榮殺人案實錄
序言：老撾萬榮一對情侶失蹤腊凶，失蹤者是張志新（化名）和其女友劉穎钧萍，沒想到半個月后，有當?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體队魏，經(jīng)...
沈念sama閱讀 45,655評論 1贊 314
?護林員之死
正文獨居荒郊野嶺守林人離奇死亡胡桨，尸身上長有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點故事閱讀 37,846評論 3贊 336
?白月光啟示錄
正文我和宋清朗相戀三年蟹略，在試婚紗的時候發(fā)現(xiàn)自己被綠了。大學(xué)時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片揽浙。...
茶點故事閱讀 39,965評論 1贊 348
活死人
序言：一個原本活蹦亂跳的男人離奇死亡馅巷，死狀恐怖礼搁，靈堂內(nèi)的尸體忽然破棺而出帐萎，到底是詐尸還是另有隱情敞曹，我是刑警寧澤澳迫，帶...
沈念sama閱讀 35,684評論 5贊 347
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布剧劝，位于F島的核電站，受9級特大地震影響拢锹，放射性物質(zhì)發(fā)生泄漏卒稳。R本人自食惡果不足惜取胎，卻給世界環(huán)境...
茶點故事閱讀 41,295評論 3贊 329
男人毒藥：我在死后第九天來索命
文/蒙蒙一湃窍、第九天我趴在偏房一處隱蔽的房頂上張望匪傍。院中可真熱鬧役衡，春花似錦薪棒、人聲如沸。這莊子的主人今日做“春日...
開封第一講書人閱讀 31,894評論 0贊 22
一樁弒父案邮辽，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽贸营。三九已至，卻和暖如春揣云，著一層夾襖步出監(jiān)牢的瞬間冰啃，已是汗流浹背。一陣腳步聲響...
開封第一講書人閱讀 33,012評論 1贊 269
情欲美人皮
我被黑心中介騙來泰國打工焚刚，沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留汪榔，地道東北人肃拜。一個月前我還...
沈念sama閱讀 48,126評論 3贊 370
代替公主和親
正文我出身青樓，卻偏偏與公主長得像士聪，于是被迫代替她去往敵國和親猛蔽。傳聞我的和親對象是個殘疾皇子灵寺，可洞房花燭夜當晚...
茶點故事閱讀 44,914評論 2贊 355