坐標(biāo)變換(5)—用旋轉(zhuǎn)軸和旋轉(zhuǎn)角表示旋轉(zhuǎn)

任何旋轉(zhuǎn)合砂，都可以用一個旋轉(zhuǎn)軸\hat \omega和一個旋轉(zhuǎn)角\theta來描述目胡。

1. 坐標(biāo)系的線速度和角速度

如上圖锯七，在旋轉(zhuǎn)的剛體上，附加一個body frameb誉己，記為\hat{x},\hat{y},\hat{z}眉尸。對于三個軸而言，繞著\hat \omega旋轉(zhuǎn)的軌跡為圓巨双。當(dāng)然效五，上述坐標(biāo)軸\hat{x},\hat{y},\hat{z}和\hat \omega是在fixed frameS坐標(biāo)系下的，下面將\hat \omega記為\hat \omega_s炉峰，

繞著軸\hat \omega的角速度為畏妖，
w_s=\hat{w}\dot{\theta} \tag{1}
運動的線速度記為\dot{\hat{x}}，三個軸的線速度則為疼阔，
\begin{aligned} \dot{\hat{x}}&=w_s \times \hat{x} \\ \dot{\hat{y}}&=w_s \times \hat{y} \\ \dot{\hat{z}}&=w_s \times \hat{z} \end{aligned} \tag{2}
將三個軸的線速度統(tǒng)一寫為戒劫，
\dot{R}= \begin{bmatrix} w_s \times \hat{x} & w_s \times \hat{y} & w_s \times \hat{z} \end{bmatrix}=w_s \times R \tag{3}
為了簡化公式(3)中的叉乘，特引入了[]符號婆廊，將w \times R可以記為矩陣的乘法[w]R迅细，其中[w]的定義如下：
對于\mathbb{R}^3中的向量x=\begin{bmatrix}x_1 & x_2 &x_3\end{bmatrix},定義[x]為一個反對稱矩陣，
[x]=\left[\begin{array}{ccc} 0 & -x_{3} & x_{2} \\ x_{3} & 0 & -x_{1} \\ -x_{2} & x_{1} & 0 \end{array}\right]\tag{4}
[x]=-[x]^T \tag{5}
上述所有3 \times 3的反對稱矩陣統(tǒng)稱為so(3)淘邻，小的茵典。前面說過，旋轉(zhuǎn)矩陣屬于SO(3)宾舅，大的统阿。下面有一個兩者結(jié)合起來有趣的性質(zhì)，假定r_i^T為R的第i行筹我，即r_i是R^T的第i列扶平，則
\begin{aligned} R[w]R^T &= R\begin{bmatrix} w_s\times r_1 & w_s\times r_2 &w_s\times r_3 \end{bmatrix}\\ &= \begin{bmatrix} r_1^{T}(w_s\times r_1) & r_1^T(w_s\times r_2) & r_1^T (w_s\times r_3) \\ r_2^{T}(w_s\times r_1) & r_2^T(w_s\times r_2) & r_2^T (w_s\times r_3) \\ r_3^{T}(w_s\times r_1) & r_3^T(w_s\times r_2) & r_3^T (w_s\times r_3) \end{bmatrix} \\ &= \begin{bmatrix} 0 & -r_3^Tw_s & r_2^Tw_s\\ r_3^Tw_s & 0 &-r_1^Tw_s\\ -r_2^Tw_s&r_1^Tw_s&0 \end{bmatrix}\\ &=[Rw_s] \end{aligned} \tag{6}

對于(6)中矩陣中的r_1^{T}(w_s\times r_2)，是三個向量r_1,w_s,r_2的混合積蔬蕊，也就是三個向量組成的六面體的體積结澄，而我們知道矩陣的行列式的值的物理意義就是體積。根據(jù)下面的混合積的圖岸夯，很容易得到矩陣中對應(yīng)元素的反對稱的關(guān)系麻献。

下面我們將三個軸的線速度表示為[w]的寫法，
\dot{R}=[w_s]R \tag{7}

[w_s]=\dot{R}R^{-1} \tag{8}
前面我們提到的所有的向量和R都是在fixed frameS下描述的猜扮，下面我們將w_s在body frameb下進(jìn)行描述勉吻，易得，
w_s=R_{sb}w_b\tag{9}
則旋轉(zhuǎn)軸在body frameb下破镰，
w_b=R_{sb}^{-1}w_s=R^{-1}w_s=R^{T}w_s \tag{10}
因此可以得到餐曼，
\begin{aligned} [w_b]&=[R^{T}w_s] \\ &= R^T[w_s]R \\ &= R^T(\dot{R}R^{T})R \\ &=R^T\dot{R} \\ &=R^{-1}\dot{R} \end{aligned}\tag{11}

需要注意的是w_b是在body frameb下的描述，所以它描述的角速度不是一個旋轉(zhuǎn)的坐標(biāo)系的角速度(例如b相對于S旋轉(zhuǎn))鲜漩，而是在某一瞬時源譬，w_b相對于body frameb的旋轉(zhuǎn)。

2. 微分方程的解

給定下面一個簡單的線性微分方程孕似，其中x(t) \in \mathbb{R}踩娘，a \in \mathbb{R}，初始狀態(tài)x(t) =x(0)喉祭，
\dot x(t)=ax(t) \tag{12}
易得上述的解為养渴，
x(t)=e^{at}x(0) \tag{13}
對e^{at}在0附近進(jìn)行泰勒展開，可得泛烙，
e^{a t}=1+a t+\frac{(a t)^{2}}{2 !}+\frac{(a t)^{3}}{3 !}+\cdots \tag{14}
同理理卑，當(dāng)a為矩陣A時，x(t)為列向量蔽氨，
\dot x(t)=Ax(t) \tag{15}
可得解為,
x(t)=e^{At}x(0) \tag{16}
其中藐唠，
e^{A t}=1+A t+\frac{(A t)^{2}}{2 !}+\frac{(A t)^{3}}{3 !}+\cdots \tag{17}

3. 指數(shù)形式的旋轉(zhuǎn)

任何旋轉(zhuǎn)，都可以用一個旋轉(zhuǎn)軸\hat \omega和一個旋轉(zhuǎn)角\theta來描述鹉究。其中\hat \omega \in \mathbb{R^3},\Vert{\hat \omega}\Vert=1宇立，\theta \in \mathbb{R^3}。
下面我們來分析如何利用一根旋轉(zhuǎn)軸和旋轉(zhuǎn)角來描述旋轉(zhuǎn)自赔，

假設(shè)向量p(t)從p(0)繞著\hat{\omega}以恒定的角速度1rad/s旋轉(zhuǎn)了\theta秒妈嘹，最終到p(\theta)，定義p(t)間斷的線速度為绍妨，
\dot{p}= \hat{\omega} \times p \tag{18}
由前面的分析润脸，引入[\hat{\omega}]，則
\dot{p}= [\hat{\omega}] p \tag{19}
該微分方程如前面介紹為他去，
p(t)=e^{[\hat{\omega}] t} p(0) \tag{20}

則津函，
p(\theta)=e^{[\hat{\omega}] \theta} p(0) \tag{21}

容易得到[\hat{\omega}]兩個計算性質(zhì)，如下孤页，
[\hat{\omega}][\hat{\omega}]=\hat{\omega}\hat{\omega}^{T}-I \tag{22}

[\hat{\omega}][\hat{\omega}][\hat{\omega}]= [\hat{\omega}]^{3}=-[\hat{\omega}] \tag{23}
所以公式21可以化簡為尔苦，
\begin{aligned} e^{[\hat{\omega}] \theta} &=I+[\hat{\omega}] \theta+[\hat{\omega}]^{2} \frac{\theta^{2}}{2 !}+[\hat{\omega}]^{3} \frac{\theta^{3}}{3 !}+\cdots \\ &=I+\left(\theta-\frac{\theta^{3}}{3 !}+\frac{\theta^{5}}{5 !}-\cdots\right)[\hat{\omega}]+\left(\frac{\theta^{2}}{2 !}-\frac{\theta^{4}}{4 !}+\frac{\theta^{6}}{6 !}-\cdots\right)[\hat{\omega}]^{2} \\ &= I+sin(\theta)[\hat{\omega}]+(1-cos(\theta))[\hat{\omega}]^{2} \end{aligned} \tag{24}
上式就是著名的羅德里格斯公式，即指數(shù)形式的旋轉(zhuǎn)行施，
\operatorname{Rot}(\hat{\omega}, \theta)=e^{[\hat{\omega}] \theta}=I+\sin \theta[\hat{\omega}]+(1-\cos \theta)[\hat{\omega}]^{2} \in S O(3) \tag{25}
經(jīng)過指數(shù)映射允坚，將s o(3)和旋轉(zhuǎn)的角度\theta通過指數(shù)映射為S O(3)，即三維的旋轉(zhuǎn)矩陣蛾号。

在前面文章中介紹過稠项，旋轉(zhuǎn)矩陣左乘和右乘的區(qū)別，這里也是類似的鲜结，假設(shè)body frameb在fixed frameS中的描述為R_{sb}展运，則\operatorname{Rot}(\hat{\omega}, \theta)R_{sb}活逆，左乘，表示將R_{sb}順著S中的\hat{\omega}旋轉(zhuǎn)\theta拗胜。而R_{sb}\operatorname{Rot}(\hat{\omega}, \theta)蔗候，右乘，表示將R_{sb}順著b中的\hat{\omega}旋轉(zhuǎn)\theta埂软。

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

上面描述的是從\hat{\omega}\theta到旋轉(zhuǎn)矩陣R的過程锈遥，下面介紹從旋轉(zhuǎn)矩陣R求\hat{\omega}\theta的過程，也就是求得旋轉(zhuǎn)向量和具體的旋轉(zhuǎn)角度勘畔，求R矩陣的“對數(shù)”所灸。可以將兩個對應(yīng)的過程描述成下面的形式炫七，

\begin{array}{cll} \exp : & [\hat{\omega}] \theta \in s o(3) & \rightarrow & R \in S O(3) \\ \log : & R \in S O(3) & \rightarrow & [\hat{\omega}] \theta \in \operatorname{so}(3) \end{array}

下面將公式(25)展開爬立，如下，
\left[\begin{array}{ccc} c_{\theta}+\hat{\omega}_{1}^{2}\left(1-c_{\theta}\right) & \hat{\omega}_{1} \hat{\omega}_{2}\left(1-c_{\theta}\right)-\hat{\omega}_{3} \mathrm{s}_{\theta} & \hat{\omega}_{1} \hat{\omega}_{3}\left(1-c_{\theta}\right)+\hat{\omega}_{2} \mathrm{s}_{\theta} \\ \hat{\omega}_{1} \hat{\omega}_{2}\left(1-\mathrm{c}_{\theta}\right)+\hat{\omega}_{3} \mathrm{S}_{\theta} & c_{\theta}+\hat{\omega}_{2}^{2}\left(1-c_{\theta}\right) & \hat{\omega}_{2} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)-\hat{\omega}_{1} \mathrm{s}_{\theta} \\ \hat{\omega}_{1} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)-\hat{\omega}_{2} \mathrm{S}_{\theta} & \hat{\omega}_{2} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)+\hat{\omega}_{1} \mathrm{s}_{\theta} & \mathrm{c}_{\theta}+\hat{\omega}_{3}^{2}\left(1-\mathrm{c}_{\theta}\right) \end{array}\right] \tag{26}
其中万哪，\hat{\omega} = \begin{bmatrix} \hat{w_1} \\ \hat{w_2} \\ \hat{w_3} \end{bmatrix}懦尝，\mathrm{c}_{\theta}=cos(\theta)，\mathrm{s}_{\theta}=sin(\theta)壤圃。
記旋轉(zhuǎn)矩陣R為r_{ij}陵霉，則可以得到，
\begin{aligned} r_{32}-r_{23}&=2 \hat{\omega}_{1}sin(\theta) \\ r_{13}-r_{31}&=2 \hat{\omega}_{2}sin(\theta) \\ r_{21}-r_{12}&=2 \hat{\omega}_{3}sin(\theta) \end{aligned} \tag{27}
上式在sin(\theta)\ne 0的情況下伍绳，可以得到踊挠，
\begin{aligned} \hat{\omega}_{1}=\frac{1}{2sin(\theta)}(r_{32}-r_{23}) \\ \hat{\omega}_{2}=\frac{1}{2sin(\theta)}(r_{13}-r_{31}) \\ \hat{\omega}_{3}=\frac{1}{2sin(\theta)}(r_{21}-r_{12}) \end{aligned} \tag{28}
上式也可以寫成，
[\hat{\omega}]=\left[\begin{array}{ccc} 0 & -\hat{\omega}_{3} & \hat{\omega}_{2} \\ \hat{\omega}_{3} & 0 & -\hat{\omega}_{1} \\ -\hat{\omega}_{2} & \hat{\omega}_{1} & 0 \end{array}\right]=\frac{1}{2 \sin \theta}\left(R-R^{\mathrm{T}}\right) \tag{29}

此外冲杀，由式(26)可以得到另外一個計算\theta的公式效床，
\operatorname{tr} R=r_{11}+r_{22}+r_{33}=1+2 \cos \theta \tag{30}
至此，sin(\theta)\ne 0的情況下权谁，利用旋轉(zhuǎn)矩陣R剩檀，我們計算出了\hat{\omega}和\theta。接下來討論sin(\theta) = 0的情況：

  1. 當(dāng)\theta=k\pi旺芽，且k是偶數(shù)的情況下沪猴，此時相當(dāng)于沒有旋轉(zhuǎn)，回到了原位置采章，R=I运嗜；
  2. 當(dāng)\theta=k\pi，且k是奇數(shù)的情況下悯舟，此時有担租，
    R=e^{[\hat{\omega}] \pi}=I+2[\hat{\omega}]^{2} \tag{31}
    因為式(31)三個矩陣都是對角矩陣，所以可以得到下面的結(jié)果(利用R對角元素)
    \hat{\omega}_{i}=\pm \sqrt{\frac{r_{i i}+1}{2}}, \quad i=1,2,3 \tag{32}
    利用R非對角元素抵怎，可得奋救，
    \begin{aligned} 2 \hat{\omega}_{1} \hat{\omega}_{2} &=r_{12} \\ 2 \hat{\omega}_{2} \hat{\omega}_{3} &=r_{23} \\ 2 \hat{\omega}_{1} \hat{\omega}_{3} &=r_{13} \end{aligned} \tag{33}
    利用式(32)和式(33)我們就能計算出\hat{\omega}岭参，同時此時旋轉(zhuǎn)的角為\theta=\pm \pi, \pm 3 \pi, \ldots。
    從上面的計算過程很容易看出來尝艘，旋轉(zhuǎn)角度是以2\pi為周期演侯，其實也是符合物理意義的，旋轉(zhuǎn)\pi和旋轉(zhuǎn)3\pi的效果是一樣的利耍，因此我們可以將旋轉(zhuǎn)的角度限定在[-\pi,\pi]。此時計算的\hat{\omega}\theta的長度是\le\pi的盔粹。因此我們可以把S O (3)想象為一個半徑為\pi的實心球隘梨，如下圖所示，

當(dāng)給定球中的一點r\in\mathbb{R}^3舷嗡，我們可以將\hat{\omega}=\frac{r}{\Vert r\Vert}作為單位長度的旋轉(zhuǎn)軸轴猎，\Vert r\Vert作為\theta。和r相對應(yīng)的旋轉(zhuǎn)矩陣R可以被看作是繞著\hat{\omega}旋轉(zhuǎn)了\theta角进萄。對于R\in SO(3)捻脖，同時trR \ne -1，此時在實心球中總能找到一個唯一的r中鼠，使得e^{[r]}=R可婶。當(dāng)trR = -1時，此時\Vert r\Vert=\pi援雇，在實心球的表面有一對正好相反的一對點矛渴，兩者的效果是一樣的，r和-r都對應(yīng)了同一個R惫搏。

最后編輯于 ：
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
  • 人面猴
    序言：七十年代末具温，一起剝皮案震驚了整個濱河市，隨后出現(xiàn)的幾起案子筐赔，更是在濱河造成了極大的恐慌铣猩，老刑警劉巖，帶你破解...
    沈念sama閱讀 217,509評論 6贊 504
  • 死咒
    序言：濱河連續(xù)發(fā)生了三起死亡事件茴丰，死亡現(xiàn)場離奇詭異达皿，居然都是意外死亡，警方通過查閱死者的電腦和手機贿肩，發(fā)現(xiàn)死者居然都...
    沈念sama閱讀 92,806評論 3贊 394
  • 救了他兩次的神仙讓他今天三更去死
    文/潘曉璐 我一進(jìn)店門鳞绕，熙熙樓的掌柜王于貴愁眉苦臉地迎上來，“玉大人尸曼，你說我怎么就攤上這事们何。” “怎么了控轿？”我有些...
    開封第一講書人閱讀 163,875評論 0贊 354
  • 道士緝兇錄：失蹤的賣姜人
    文/不壞的土叔 我叫張陵冤竹，是天一觀的道長拂封。 經(jīng)常有香客問我，道長鹦蠕，這世上最難降的妖魔是什么冒签？ 我笑而不...
    開封第一講書人閱讀 58,441評論 1贊 293
  • ?港島之戀（遺憾婚禮）
    正文 為了忘掉前任，我火速辦了婚禮钟病，結(jié)果婚禮上萧恕，老公的妹妹穿的比我還像新娘。我一直安慰自己肠阱，他們只是感情好票唆，可當(dāng)我...
    茶點故事閱讀 67,488評論 6贊 392
  • 惡毒庶女頂嫁案：這布局不是一般人想出來的
    文/花漫 我一把揭開白布。 她就那樣靜靜地躺著屹徘，像睡著了一般走趋。 火紅的嫁衣襯著肌膚如雪。 梳的紋絲不亂的頭發(fā)上噪伊，一...
    開封第一講書人閱讀 51,365評論 1贊 302
  • 城市分裂傳說
    那天簿煌，我揣著相機與錄音，去河邊找鬼鉴吹。 笑死姨伟，一個胖子當(dāng)著我的面吹牛，可吹牛的內(nèi)容都是我干的豆励。 我是一名探鬼主播授滓，決...
    沈念sama閱讀 40,190評論 3贊 418
  • 雙鴛鴦連環(huán)套：你想象不到人心有多黑
    文/蒼蘭香墨 我猛地睜開眼，長吁一口氣：“原來是場噩夢啊……” “哼肆糕！你這毒婦竟也來了般堆？” 一聲冷哼從身側(cè)響起，我...
    開封第一講書人閱讀 39,062評論 0贊 276
  • 萬榮殺人案實錄
    序言：老撾萬榮一對情侶失蹤诚啃，失蹤者是張志新（化名）和其女友劉穎淮摔，沒想到半個月后，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體始赎，經(jīng)...
    沈念sama閱讀 45,500評論 1贊 314
  • ?護林員之死
    正文 獨居荒郊野嶺守林人離奇死亡和橙，尸身上長有42處帶血的膿包…… 初始之章·張勛 以下內(nèi)容為張勛視角 年9月15日...
    茶點故事閱讀 37,706評論 3贊 335
  • ?白月光啟示錄
    正文 我和宋清朗相戀三年，在試婚紗的時候發(fā)現(xiàn)自己被綠了造垛。 大學(xué)時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片魔招。...
    茶點故事閱讀 39,834評論 1贊 347
  • 活死人
    序言：一個原本活蹦亂跳的男人離奇死亡，死狀恐怖五辽，靈堂內(nèi)的尸體忽然破棺而出办斑，到底是詐尸還是另有隱情，我是刑警寧澤，帶...
    沈念sama閱讀 35,559評論 5贊 345
  • ?日本核電站爆炸內(nèi)幕
    正文 年R本政府宣布乡翅，位于F島的核電站鳞疲，受9級特大地震影響，放射性物質(zhì)發(fā)生泄漏蠕蚜。R本人自食惡果不足惜尚洽，卻給世界環(huán)境...
    茶點故事閱讀 41,167評論 3贊 328
  • 男人毒藥：我在死后第九天來索命
    文/蒙蒙 一、第九天 我趴在偏房一處隱蔽的房頂上張望靶累。 院中可真熱鬧腺毫，春花似錦、人聲如沸挣柬。這莊子的主人今日做“春日...
    開封第一講書人閱讀 31,779評論 0贊 22
  • 一樁弒父案，背后竟有這般陰謀
    文/蒼蘭香墨 我抬頭看了看天上的太陽凛忿。三九已至澈灼，卻和暖如春竞川，著一層夾襖步出監(jiān)牢的瞬間店溢，已是汗流浹背。 一陣腳步聲響...
    開封第一講書人閱讀 32,912評論 1贊 269
  • 情欲美人皮
    我被黑心中介騙來泰國打工委乌， 沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留床牧，地道東北人。 一個月前我還...
    沈念sama閱讀 47,958評論 2贊 370
  • 代替公主和親
    正文 我出身青樓遭贸，卻偏偏與公主長得像戈咳，于是被迫代替她去往敵國和親。 傳聞我的和親對象是個殘疾皇子壕吹，可洞房花燭夜當(dāng)晚...
    茶點故事閱讀 44,779評論 2贊 354

推薦閱讀更多精彩內(nèi)容