第三章 位姿描述和齊次變換

第三章位姿描述和齊次變換第一頁，共四十六頁，編輯于2023年，星期四3.列矩陣4.矩陣相等：兩同型矩陣（行數(shù)和列數(shù)都相等）對(duì)應(yīng)元素相等。2.行矩陣第二頁，共四十六頁，編輯于2023年，星期四（2）矩陣與數(shù)相乘：該數(shù)與矩陣各元素相乘。5.單位矩陣：主對(duì)角線元素為1，其它所有的元素都為0的方陣。6.矩陣的運(yùn)算（1）矩陣的加法：兩同型矩陣的對(duì)應(yīng)元素相加。第三頁，共四十六頁，編輯于2023年，星期四（3）矩陣與矩陣相乘：（4）矩陣的轉(zhuǎn)置：把矩陣的行換成同序數(shù)的列，記為第四頁，共四十六頁，編輯于2023年，星期四7.矩陣的逆（逆矩陣）8.分塊矩陣：分塊后的矩陣與普通矩陣的運(yùn)算相同。第五頁，共四十六頁，編輯于2023年，星期四9.正交矩陣：如果，則A為正交矩陣。它滿足：如果是正交矩陣，則行列式和矩陣的區(qū)別：矩陣是按一定方式排成的數(shù)表；行列式是一個(gè)數(shù)。第六頁，共四十六頁，編輯于2023年，星期四（b）左手坐標(biāo)系（a）右手坐標(biāo)系二、直角坐標(biāo)系

若基矢量相互正交，即它們?cè)谠c(diǎn)o處兩兩相交成直角，則它們構(gòu)成直角坐標(biāo)系或笛卡兒坐標(biāo)系。斜角坐標(biāo)系若按右手法則繞oz軸轉(zhuǎn)900可以使ox軸轉(zhuǎn)向oy軸，則稱為右手坐標(biāo)系；按左手法則形成的坐標(biāo)系稱左手坐標(biāo)系。

本課程使用右手坐標(biāo)系。第七頁，共四十六頁，編輯于2023年，星期四其中θ是a和b兩矢量間的夾角，如圖所示。三、矢量的點(diǎn)積（內(nèi)乘積或標(biāo)量積）換句話說：一個(gè)矢量在另一個(gè)矢量上的投影等于該矢量與另一矢量方向上單位矢量的點(diǎn)積。再令a=j（j為a方向上的單位矢量），則即兩矢量方向上單位矢量的點(diǎn)乘等于兩矢量夾角的余弦。標(biāo)量積令b=i（i為b方向上的單位矢量），則第八頁，共四十六頁，編輯于2023年，星期四四、矢量的叉積（矢量積或叉乘積）其中矢量c的模為:其中θ是a和b間小于等于1800的夾角，若將a按右手法則繞c轉(zhuǎn)θ角至b，右手拇指指向?yàn)閏的正方向（如上圖所示），c與a、b兩者垂直。則叉乘積若a和b用分量的形式表示為:第九頁，共四十六頁，編輯于2023年，星期四a和b的點(diǎn)乘為：將點(diǎn)乘和叉乘應(yīng)用于右手笛卡爾坐標(biāo)系的單位矢量i，j，k，有：第十頁，共四十六頁，編輯于2023年，星期四3.2位姿描述與坐標(biāo)變換3.2.1

剛體位置姿態(tài)（位姿）描述第十一頁，共四十六頁，編輯于2023年，星期四a)位置的描述采用直角坐標(biāo)描述點(diǎn)的位置，因此，剛體F的位置描述，即OB點(diǎn)在{A}中描述可用一個(gè)3×1的列矢量（位置矢量）表示，即其中Px、Py和Pz是點(diǎn)OB在{A}系中的三個(gè)坐標(biāo)分量。第十二頁，共四十六頁，編輯于2023年，星期四b)姿態(tài)（方位）的描述采用旋轉(zhuǎn)矩陣來表示剛體姿態(tài)（方位），即由{B}系的三個(gè)單位主矢量相對(duì)于坐標(biāo)系{A}的方向余弦組成：

既表示了剛體F在{A}系中的方位，也描述了{(lán)B}系在{A}系中的姿態(tài)。其中：xByBzBxA

yA

zA第十三頁，共四十六頁，編輯于2023年，星期四3.2.2坐標(biāo)變換如圖所示，坐標(biāo)系{B}與{A}方向相同，但原點(diǎn)不重合。坐標(biāo)平移

一、坐標(biāo)平移此式稱為平移方程。其中是B系中的原點(diǎn)在A系中的表示。第十四頁，共四十六頁，編輯于2023年，星期四二、坐標(biāo)旋轉(zhuǎn)坐標(biāo)旋轉(zhuǎn)如圖所示，{B}與{A}有共同的坐標(biāo)原點(diǎn)，但方位不同。令和分別是{A}和{B}中的單位主矢量，點(diǎn)P在兩坐標(biāo)系中各坐標(biāo)軸上的坐標(biāo)分量分別為：和第十五頁，共四十六頁，編輯于2023年，星期四利用點(diǎn)乘的性質(zhì)和上式共同求解得將代入上面三式中并寫成矩陣形式得所以有第十六頁，共四十六頁，編輯于2023年，星期四上式簡(jiǎn)寫為：

此式稱為坐標(biāo)旋轉(zhuǎn)方程。其中旋轉(zhuǎn)矩陣表示了坐標(biāo)系{B}相對(duì)于{A}的方位，正好與剛體姿態(tài)的描述相同。同理也可得和都是正交矩陣，因此滿足由與互逆，可得第十七頁，共四十六頁，編輯于2023年，星期四旋轉(zhuǎn)矩陣的幾何意義：旋轉(zhuǎn)矩陣在幾何上表示了發(fā)生相互旋轉(zhuǎn)的兩坐標(biāo)系各主軸之間的相互方位關(guān)系。若把寫成行向量的形式，則其中每一個(gè)元素都是一個(gè)列向量。容易得出滿足六個(gè)約束條件（稱正交條件）：第十八頁，共四十六頁，編輯于2023年，星期四因此寫出三個(gè)基本的旋轉(zhuǎn)矩陣，即分別繞x、y和z軸轉(zhuǎn)θ角的旋轉(zhuǎn)矩陣：x’y’z’xyzx’y’z’xyzx’y’z’xyz第十九頁，共四十六頁，編輯于2023年，星期四例3.1若從基坐標(biāo)系({B})到手爪坐標(biāo)系({E})的旋轉(zhuǎn)變換矩陣為。（1）畫出兩坐標(biāo)系的相互方位關(guān)系（不考慮{E}的原點(diǎn)位置）；（2）如果給出OE({E}系的原點(diǎn)）在{B}中的位置矢量為（1，2，2），畫出兩坐標(biāo)系的相對(duì)位姿關(guān)系；（3）求a，b，c的值。解：xEyEzExByBzB(1)(2)(3)a=0，b=1，c=0第二十頁，共四十六頁，編輯于2023年，星期四三、一般變換一般的情況：坐標(biāo)系{B}的原點(diǎn)既不與{A}重合，方位也不相同。{C}系與｛Ｂ｝系原點(diǎn)重合，但方位不同，所以得{C}系與｛A｝系原點(diǎn)不重合，但方位相同，所以得進(jìn)而有和第二十一頁，共四十六頁，編輯于2023年，星期四例3.2已知坐標(biāo)系{B}初始位姿與{A}重合，首先{B}相對(duì){A}的zA軸轉(zhuǎn)30°，再沿{A}的xA軸移動(dòng)10個(gè)單位，并沿{A}的yA軸移動(dòng)5個(gè)單位。求位置矢量和旋轉(zhuǎn)矩陣。若,求。解：第二十二頁，共四十六頁，編輯于2023年，星期四所以有：最后得：第二十三頁，共四十六頁，編輯于2023年，星期四3.3齊次坐標(biāo)與齊次變換復(fù)合變換式可以表示成等價(jià)的齊次變換式。簡(jiǎn)寫成綜合地表示了平移和旋轉(zhuǎn)變換。第二十四頁，共四十六頁，編輯于2023年，星期四3.3.1齊次坐標(biāo)一般來說，以N+1維矢量表達(dá)N維位置矢量的方法稱為齊次坐標(biāo)表示法。在三維直角坐標(biāo)系中，一個(gè)點(diǎn)可以表示為，它的齊次坐標(biāo)就是，即滿足Px=ωPx/ω，Py=ωPy/ω，Pz=ωPz/ω（ω是非零整數(shù)）?？梢钥闯觯谌S直角坐標(biāo)系中，由于ω取值的不同，一個(gè)點(diǎn)的齊次坐標(biāo)的表達(dá)不唯一。第二十五頁，共四十六頁，編輯于2023年，星期四齊次坐標(biāo)不僅可以規(guī)定點(diǎn)的位置（ω為非零整數(shù)），還可以用來規(guī)定矢量的方向（第四個(gè)元素為零時(shí)）。列向量(）表示空間的無窮遠(yuǎn)點(diǎn)，a，b和c稱為它的方向數(shù)。分別代表了ox，oy和oz軸的無窮遠(yuǎn)點(diǎn)，用它們分別表示這三個(gè)坐標(biāo)軸的方向。另外，代表坐標(biāo)原點(diǎn)，沒有意義。注意：位置矢量究竟是3×1的直角坐標(biāo)還是4×1的齊次坐標(biāo)，應(yīng)根據(jù)上下文而定。第二十六頁，共四十六頁，編輯于2023年，星期四在機(jī)器人研究中，齊次變換矩陣T為：3.3.2齊次變換齊次變換矩陣是4×4的矩陣，它的完整形式可以看成是由四個(gè)子矩陣組成：第二十七頁，共四十六頁，編輯于2023年，星期四

純旋轉(zhuǎn)的齊次變換矩陣中P3×1為零矩陣，即，因此寫出繞x，y和z軸旋轉(zhuǎn)θ角的基本齊次變換矩陣為：

純平移的齊次變換矩陣中R3×3=I3×3（單位陣），因此可以寫出沿x，y和z軸移動(dòng)Px，Py和Pz單位的基本平移變換陣：第二十八頁，共四十六頁，編輯于2023年，星期四從而定義復(fù)合變換。給定坐標(biāo)系{A}，{B}和{C}，已知{B}相對(duì){A}的描述為，{C}相對(duì){B}的描述為，則有同理得出：即一個(gè)坐標(biāo)系變換至另一坐標(biāo)系的齊次變換矩陣等于依次經(jīng)歷中間坐標(biāo)系各齊次變換矩陣的連乘積。第二十九頁，共四十六頁，編輯于2023年，星期四例3.4已知，畫出{A}和{B}的相互位姿關(guān)系圖。結(jié)論：齊次變換不僅可以表示同一點(diǎn)相對(duì)不同坐標(biāo)系{B}和{A}中的變換，也可用來描述坐標(biāo)系{B}相對(duì)于另一坐標(biāo)系{A}的位姿，同時(shí)還可用來作為點(diǎn)的運(yùn)動(dòng)算子。例：書上P20例2.4。第三十頁，共四十六頁，編輯于2023年，星期四3.4齊次變換的性質(zhì)1、繞固定坐標(biāo)系依次進(jìn)行的坐標(biāo)系轉(zhuǎn)換，各齊次變換矩陣按“從右向左”依次相乘原則進(jìn)行運(yùn)算（右乘）。一.變換過程的相對(duì)性=

RPY角RPY第三十一頁，共四十六頁，編輯于2023年，星期四RPY角反解：第三十二頁，共四十六頁，編輯于2023年，星期四2、繞動(dòng)坐標(biāo)系依次進(jìn)行的齊次變換，按“從左向右”的原則依次相乘(左乘）。=z-y-x歐拉角：第三十三頁，共四十六頁，編輯于2023年，星期四相對(duì)于固定坐標(biāo)系運(yùn)動(dòng)相對(duì)于活動(dòng)坐標(biāo)系運(yùn)動(dòng)齊次變換的相對(duì)性第三十四頁，共四十六頁，編輯于2023年，星期四齊次坐標(biāo)變換過程是可逆的.若有，則逆變換。二.變換過程的可逆性所以有對(duì)應(yīng)元素相等得第三十五頁，共四十六頁，編輯于2023年，星期四所以得第三十六頁，共四十六頁，編輯于2023年，星期四三.變換過程的封閉性因此有由上面兩式得變換方程：畫出空間尺寸鏈圖為:第三十七頁，共四十六頁，編輯于2023年，星期四例3.5如圖所示，從{0}系到{3}系依次經(jīng)過{1}系和{2}系的變換,①用兩種方法求和，第一種根據(jù)齊次變換矩陣的幾何意義求解，另一種采用坐標(biāo)系依次變換的方法；②求（用兩種方法）；③畫出{0}到{3}的空間尺寸鏈圖。第三十八頁，共四十六頁，編輯于2023年，星期四空間尺寸鏈圖：第三十九頁，共四十六頁，編輯于2023年，星期四3.5旋轉(zhuǎn)變換通式一.旋轉(zhuǎn)變換通式

如果不是，要采用其對(duì)應(yīng)的單位方向矢量令是過{A}系原點(diǎn)的單位矢量，求繞K旋轉(zhuǎn)θ角到{B}系的旋轉(zhuǎn)矩陣R(K,θ)，即。第四十頁，共四十六頁，編輯于2023年，星期四因此將上式展開得尺寸鏈圖第四十一頁，共四十六頁，編輯于2023年，星期四把上式右端相乘，并利用旋轉(zhuǎn)矩陣的正交性質(zhì)進(jìn)行化簡(jiǎn)整理后得其中，sθ=sinθ;cθ=cosθ;Versθ=(1-cosθ)。

如果與坐標(biāo)軸重合，則可得到繞x,y和z軸旋轉(zhuǎn)的基