三維空間的剛體運(yùn)動(dòng)

旋轉(zhuǎn)矩陣

點(diǎn),向量,坐標(biāo)系

剛體不光有位置,還有自身的姿態(tài).位置是指剛體在空間中的哪個(gè)地方,姿態(tài)是指剛體的朝向.

歐式變換

相機(jī)運(yùn)動(dòng)是一個(gè)剛體運(yùn)動(dòng),它保證了同一個(gè)向量在各坐標(biāo)系下的長(zhǎng)度和夾角都不會(huì)發(fā)生變化,這種變換稱(chēng)為歐式變換.
假設(shè)有一組正交基為 $(e_1,e_2,e_3)$ 下某個(gè)向量的值為 $[a_1,a_2,a_3]^T$ ,
那么在 $(e_1',e_2',e_3')$ 下,這個(gè)向量的坐標(biāo)為 $[a_1',a_2',a_3']^T$
有
$a=[e_1,e_2,e_3] \left [ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right ] =[e_1',e_2',e_3'] \left [ \begin{matrix} a_1' \\ a_2' \\ a_3' \end{matrix} \right ]$
于是有
$\left [ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right ]= \left [ \begin{matrix} e_1^Te_1'&e_1^Te_2'&e_1^Te_3' \\ e_2^Te_1'&e_2^Te_2'&e_2^Te_3' \\ e_3^Te_1'&e_3^Te_2'&e_3^Te_3' \end{matrix} \right ] \left [ \begin{matrix} a_1' \\ a_2' \\ a_3' \end{matrix} \right ]\triangleq Ra'$

$\triangleq$ 意思是定義為

在上式中,我們把兩組基之間的內(nèi)積定義為矩陣 $R$ ,這個(gè)矩陣刻畫(huà)了旋轉(zhuǎn)前后同一個(gè)向量的坐標(biāo)變化關(guān)系,只要旋轉(zhuǎn)時(shí)一樣的,這個(gè)矩陣就是一樣的,因此我們把 $R$ 稱(chēng)為旋轉(zhuǎn)矩陣
另外,旋轉(zhuǎn)矩陣是一個(gè)行列式為1的正交矩陣,反之亦然.
旋轉(zhuǎn)矩陣的集合叫做特殊正交群群（ Special Orthogonal Group）定義如下:
$SO(n)=\{R\in\mathbb{R}^{n\times n}|RR^T=I,det(R)=1\}$

上式中, $SO(n)$ 是特殊正交群的意思,n為向量的維度, $I$ 是單位矩陣.
綜上,通過(guò)旋轉(zhuǎn)矩陣,我們可以直接談?wù)搩蓚€(gè)坐標(biāo)系之間的旋轉(zhuǎn)變換,而不用再?gòu)幕_(kāi)始談起.也就是說(shuō)旋轉(zhuǎn)矩陣可以描述相機(jī)的旋轉(zhuǎn).
且由于旋轉(zhuǎn)矩陣為正交矩陣教馆，它的逆和轉(zhuǎn)置相同,故有
$a' = R^{-1}a=R^{T}a$
所以 $R^T$ 是 $R$ 的逆運(yùn)算.

綜上,向量 $a$ ,經(jīng)過(guò)一次旋轉(zhuǎn)(用 $R$ 描述)和一次平移t之后,得到的 $a'$
$a'=Ra+t$
其中t為平移向量.

變換矩陣與齊次坐標(biāo)

假設(shè)進(jìn)行兩次變換: $R_1,t_1$ 和 $R_2,t_2$ ,滿(mǎn)足:
$b = R_1a+t_1$
$c = R_1b+t_1$
即向量a變成c為:
$c=R_2(R_1a+t_1)+t_2$
這樣的形式在多次變換后會(huì)過(guò)于復(fù)雜,引入齊次坐標(biāo)和變換矩陣重寫(xiě)式:
$\left [ \begin{matrix} a'\\ 1 \end{matrix} \right ] = \left [ \begin{matrix} R&t\\ 0^T&1 \end{matrix} \right ] \left [ \begin{matrix} a\\ 1 \end{matrix} \right ] \triangleq T \left [ \begin{matrix} a\\ 1 \end{matrix} \right ]$
可以看到,我們?cè)谌S向量 $a$ 末尾加上了 $1$ ,使其變成了四維向量,稱(chēng)為齊次坐標(biāo) $^*$ .

$T$ 稱(chēng)為變換矩陣:左上角為旋轉(zhuǎn)矩陣 $R$ ,右上角為平移 $t$

將 $a$ 的齊次坐標(biāo)用 $\tilde a$

有:
$\tilde b = T_1\tilde a$

$\tilde c=T_2\tilde b$
于是有
$\tilde c = T_1T_2\tilde a$

另外,在不致歧義的情況下,我們一般不區(qū)分齊次坐標(biāo)和非齊次坐標(biāo)的寫(xiě)法 $^{**}$ ,也就是說(shuō),上式可以寫(xiě)成
$c = T_1T_2a$

對(duì)于變換T,具有特殊結(jié)構(gòu):左上角為旋轉(zhuǎn)矩陣 $R$ ,右上角為平移向量 $t$ ,左下角為0,右下角為1,這種矩陣又稱(chēng)為特殊歐式群(Special Euclidean Grop):
$SE(3)= \left \{ T = \left [ \begin{matrix} R&t\\ 0^T& 1 \end{matrix} \right ] \in \mathbb{R}^{4\times 4}|R\in SO(3),t\in \mathbb R \right \}$

T的逆為:
$T^{-1}= \left [ \begin{matrix} R^T&-R^Tt\\ 0^T&1 \end{matrix} \right ]$

*: 齊次坐標(biāo),通過(guò)添加最后一維,我們用四個(gè)實(shí)數(shù)描述了一個(gè)三維向量,這顯然多了一個(gè)自由度,齊次坐標(biāo)中,某個(gè)點(diǎn)乘以一個(gè)常數(shù)k,得到的向量與原來(lái)相同,也就說(shuō) $[1,1,1,1]^T=[2,2,2,2]^T$ ,所以,但最后一項(xiàng)不為1時(shí),我們可以將所有數(shù)除以最后一項(xiàng),以獲得標(biāo)準(zhǔn)坐標(biāo).
**:當(dāng)我們使用 $Ta$ 時(shí),默認(rèn)是齊次坐標(biāo);當(dāng)使用 $Ra$ 時(shí),默認(rèn)是非齊次坐標(biāo).

旋轉(zhuǎn)向量和歐拉角

剛體旋轉(zhuǎn)自由度:一一個(gè)剛體在空間任意運(yùn)動(dòng)時(shí)概作，可分解為質(zhì)心O的平動(dòng)和繞通過(guò)質(zhì)心軸的轉(zhuǎn)動(dòng)

平動(dòng)自由度:確定到質(zhì)心的位置,需要三個(gè)坐標(biāo)(x,y,z)

轉(zhuǎn)動(dòng)自由度: $(\alpha,\beta,\gamma)$ \三個(gè)正交的坐標(biāo)軸上的旋轉(zhuǎn)

旋轉(zhuǎn)矩陣有9個(gè)量(3x3矩陣),卻只能表達(dá)物體的旋轉(zhuǎn)(3個(gè)自由度),也就是說(shuō):SO(3)的旋轉(zhuǎn)矩陣有9個(gè)量,但每次旋轉(zhuǎn)只有3個(gè)自由度;
同理,SE(3)有16個(gè)變量,可以表達(dá)6個(gè)自由度(旋轉(zhuǎn),平移).
綜上,我們想知道有沒(méi)有更緊湊的表達(dá)方式.

旋轉(zhuǎn)向量

任意一個(gè)旋轉(zhuǎn),都可以用一個(gè)旋轉(zhuǎn)軸和一個(gè)旋轉(zhuǎn)角來(lái)刻畫(huà).于是,我們可以使用一個(gè)向量,其方向和旋轉(zhuǎn)軸一致,而長(zhǎng)度等于旋轉(zhuǎn)角.這種向量稱(chēng)為旋轉(zhuǎn)向量.(旋轉(zhuǎn)軸過(guò)質(zhì)心)
對(duì)于旋轉(zhuǎn)矩陣,這種表示法只需要一個(gè)三維向量即可描述旋轉(zhuǎn).
同樣的,對(duì)于變換矩陣,我們是有一個(gè)旋轉(zhuǎn)向量和一個(gè)平移向量即可表示一個(gè)變換,此時(shí)正好為6維.

所以,旋轉(zhuǎn)向量可以如下表示:
$\theta n$
其中,n為單位旋轉(zhuǎn)軸, $\theta$ 為旋轉(zhuǎn)角度.

旋轉(zhuǎn)向量-->旋轉(zhuǎn)矩陣

旋轉(zhuǎn)向量 $\theta n$ 和旋轉(zhuǎn)矩陣 $R$ 的轉(zhuǎn)換叫做羅德里格斯公式:
$R = \cos\theta I +(1-\cos\theta)nn^T+\sin\theta n^\wedge$
其中 $I$ 是單位矩陣, $\wedge$ 是向量到反對(duì)稱(chēng)的轉(zhuǎn)換符. $\wedge$ 的定義如下:
$a\times b = \left [ \begin{matrix} i&j&k\\ a_1&a_2&a_3\\ b_1&b_2&b_3 \end{matrix} \right ]= \left [ \begin{matrix} a_2b_3-a_3b_2\\ a_3b_1-a_1b_3\\ a_1b_2-a_2b_1 \end{matrix} \right ]= \left [ \begin{matrix} 0&-a_3&a_2\\ a_3&0&-a_1\\ -a_2&a_1&0 \end{matrix} \right ]b \triangleq a^{\wedge}b$

旋轉(zhuǎn)矩陣-->旋轉(zhuǎn)向量
$\theta = \arccos(\frac{tr(R)-1}{2})$
$tr(R)$ 是矩陣 $R$ 的跡.

關(guān)于旋轉(zhuǎn)軸,由于旋轉(zhuǎn)軸向量 $n$ 在經(jīng)過(guò)旋轉(zhuǎn) $R$ 之后,不發(fā)生改變,于是有:
$Rn=n$
根據(jù)線(xiàn)性代數(shù)的知識(shí),我們可以得出轉(zhuǎn)軸 $n$ 是矩陣 $R$ 特征值為1的特征向量.

歐拉角

無(wú)論是旋轉(zhuǎn)矩陣,還是旋轉(zhuǎn)向量都能描述旋轉(zhuǎn).但是,對(duì)人類(lèi)卻十分不直觀.

歐拉角提供一種非常直觀的方式來(lái)描述旋轉(zhuǎn)--使用3個(gè)分離的轉(zhuǎn)角,把一個(gè)旋轉(zhuǎn)分解成3次繞不同軸的旋轉(zhuǎn).
航空中的偏航角,就是ZYX旋轉(zhuǎn)

繞物體Z軸旋轉(zhuǎn),偏航角,yaw
繞旋轉(zhuǎn)之后的Y軸旋轉(zhuǎn),俯仰角,pitch
繞旋轉(zhuǎn)之后的X軸旋轉(zhuǎn),滾動(dòng)角,roll

此時(shí),可以用 $[r,p,y]^T$ 這樣一個(gè)三維向量來(lái)描述任意旋轉(zhuǎn).
不同的歐拉叫是按照旋轉(zhuǎn)軸的順序來(lái)稱(chēng)呼的.

歐拉角具有奇異性問(wèn)題,也就是萬(wàn)向鎖問(wèn)題:在俯仰角pitch為 $\pm 90$ 時(shí),第一次旋轉(zhuǎn)yaw與第三次roll將使用同一個(gè)軸,使得系統(tǒng)丟失一個(gè)自由度.
所以,歐拉角不適于插值和迭代,往往只用于人機(jī)交互.

四元數(shù)

旋轉(zhuǎn)矩陣使用9個(gè)量描述3個(gè)自由度旋轉(zhuǎn),具有冗余性;歐拉角和旋轉(zhuǎn)向量是緊湊的,但是具有奇異性;

事實(shí)上,我們找不到不帶奇異性的三維向量描述方式.

在表達(dá)三維空間的旋轉(zhuǎn)的時(shí)候,有一種類(lèi)似于復(fù)數(shù)的代數(shù):四元數(shù).四元數(shù)是一種擴(kuò)展復(fù)數(shù),它即是緊湊的,又是沒(méi)有奇異性的.雖然,四元數(shù)不夠直觀,計(jì)算也較為復(fù)雜.

一個(gè)四元數(shù)q擁有一個(gè)實(shí)部和三個(gè)虛部,下面式子把實(shí)部寫(xiě)在前面(有的寫(xiě)在后面):
$q = q_0+q_1i+q_2j+q_3k$
$i,j,k$ 為四元數(shù)的三個(gè)虛部,滿(mǎn)足:
$\left \{ \begin{aligned} i^2 = j^2=k^2=-1 \\ij=k,ji=-k \\jk=i,kj=-i\\ ki=j,ik=-j \end{aligned} \right .$

由于它的這種特殊表現(xiàn)形式,有時(shí)人們也用一個(gè)標(biāo)量和一個(gè)向量來(lái)表示四元數(shù).
$q = [s,v]$
其中
$s=q_0\in \mathbb{R},v=[q_1,q_2,q_3]^T\in \mathbb{R}^3$
這里s稱(chēng)為四元數(shù)的實(shí)部,v稱(chēng)為虛部.
如果一個(gè)四元數(shù)的虛部為0,那么為實(shí)四元數(shù),反之為虛四元數(shù).
假設(shè)某個(gè)旋轉(zhuǎn)是繞單位向量 $n=[n_x,n_y,n_z]^T$ 進(jìn)行了角度為 $\theta$ 的旋轉(zhuǎn),那么該旋轉(zhuǎn)的四元數(shù)表示為:
$q = [\cos\frac{\theta}{2},n_x\sin\frac{\theta}{2},n_y\sin\frac{\theta}{2},n_z\sin\frac{\theta}{2}]^T$

反之,從單位四元數(shù)中可以計(jì)算出對(duì)應(yīng)的旋轉(zhuǎn)軸和夾角:
$\left \{ \begin{aligned} \theta = 2\arccos{q_0}\\ [n_x,n_y,n_z]^T = \frac{[q_1,q_2,q_3]^T}{\sin\frac{\theta}{2}} \end{aligned} \right .$
上式給我們一種"轉(zhuǎn)了一半"的感覺(jué),事實(shí)上,任意旋轉(zhuǎn)都可以由兩個(gè)互為相反數(shù)的四元數(shù)表示,同理,取 $\theta$ 為0,得到一個(gè)沒(méi)有任何旋轉(zhuǎn)的四元數(shù):
$q_0=[\pm 1,0,0,0]^T$

最后編輯于：2019.04.29 04:55:40

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

人面猴
序言：七十年代末咧织，一起剝皮案震驚了整個(gè)濱河市聋庵，隨后出現(xiàn)的幾起案子昂儒，更是在濱河造成了極大的恐慌，老刑警劉巖孽锥，帶你破解...
沈念sama閱讀 207,113評(píng)論 6贊 481
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件概行，死亡現(xiàn)場(chǎng)離奇詭異，居然都是意外死亡儒鹿，警方通過(guò)查閱死者的電腦和手機(jī)化撕，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 88,644評(píng)論 2贊 381
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門(mén)，熙熙樓的掌柜王于貴愁眉苦臉地迎上來(lái)约炎，“玉大人植阴，你說(shuō)我怎么就攤上這事≌录兀” “怎么了墙贱？”我有些...
開(kāi)封第一講書(shū)人閱讀 153,340評(píng)論 0贊 344
道士緝兇錄：失蹤的賣(mài)姜人
文/不壞的土叔我叫張陵，是天一觀的道長(zhǎng)贱傀。經(jīng)常有香客問(wèn)我惨撇，道長(zhǎng)，這世上最難降的妖魔是什么府寒？我笑而不...
開(kāi)封第一講書(shū)人閱讀 55,449評(píng)論 1贊 279
?港島之戀（遺憾婚禮）
正文為了忘掉前任魁衙，我火速辦了婚禮，結(jié)果婚禮上株搔，老公的妹妹穿的比我還像新娘剖淀。我一直安慰自己，他們只是感情好纤房，可當(dāng)我...
茶點(diǎn)故事閱讀 64,445評(píng)論 5贊 374
惡毒庶女頂嫁案：這布局不是一般人想出來(lái)的
文/花漫我一把揭開(kāi)白布纵隔。她就那樣靜靜地躺著，像睡著了一般。火紅的嫁衣襯著肌膚如雪捌刮。梳的紋絲不亂的頭發(fā)上碰煌，一...
開(kāi)封第一講書(shū)人閱讀 49,166評(píng)論 1贊 284
城市分裂傳說(shuō)
那天，我揣著相機(jī)與錄音绅作，去河邊找鬼芦圾。笑死，一個(gè)胖子當(dāng)著我的面吹牛俄认，可吹牛的內(nèi)容都是我干的个少。我是一名探鬼主播，決...
沈念sama閱讀 38,442評(píng)論 3贊 401
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開(kāi)眼眯杏，長(zhǎng)吁一口氣：“原來(lái)是場(chǎng)噩夢(mèng)啊……” “哼夜焦！你這毒婦竟也來(lái)了？” 一聲冷哼從身側(cè)響起役拴，我...
開(kāi)封第一講書(shū)人閱讀 37,105評(píng)論 0贊 261
萬(wàn)榮殺人案實(shí)錄
序言：老撾萬(wàn)榮一對(duì)情侶失蹤糊探，失蹤者是張志新（化名）和其女友劉穎，沒(méi)想到半個(gè)月后河闰，有當(dāng)?shù)厝嗽跇?shù)林里發(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 43,601評(píng)論 1贊 300
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡褥紫，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 36,066評(píng)論 2贊 325
?白月光啟示錄
正文我和宋清朗相戀三年姜性，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片髓考。...
茶點(diǎn)故事閱讀 38,161評(píng)論 1贊 334
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡部念，死狀恐怖，靈堂內(nèi)的尸體忽然破棺而出氨菇，到底是詐尸還是另有隱情儡炼，我是刑警寧澤，帶...
沈念sama閱讀 33,792評(píng)論 4贊 323
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布查蓉，位于F島的核電站乌询，受9級(jí)特大地震影響，放射性物質(zhì)發(fā)生泄漏豌研。R本人自食惡果不足惜妹田，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 39,351評(píng)論 3贊 307
男人毒藥：我在死后第九天來(lái)索命
文/蒙蒙一、第九天我趴在偏房一處隱蔽的房頂上張望鹃共。院中可真熱鬧鬼佣，春花似錦、人聲如沸霜浴。這莊子的主人今日做“春日...
開(kāi)封第一講書(shū)人閱讀 30,352評(píng)論 0贊 19
一樁弒父案，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽(yáng)。三九已至晌纫，卻和暖如春税迷，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背缸匪。一陣腳步聲響...
開(kāi)封第一講書(shū)人閱讀 31,584評(píng)論 1贊 261
情欲美人皮
我被黑心中介騙來(lái)泰國(guó)打工翁狐，沒(méi)想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留，地道東北人凌蔬。一個(gè)月前我還...
沈念sama閱讀 45,618評(píng)論 2贊 355
代替公主和親
正文我出身青樓露懒，卻偏偏與公主長(zhǎng)得像，于是被迫代替她去往敵國(guó)和親砂心。傳聞我的和親對(duì)象是個(gè)殘疾皇子懈词，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 42,916評(píng)論 2贊 344