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

第三章位姿描述和齊次變換第1頁(yè)，共46頁(yè)，2023年，2月20日，星期三3.列矩陣4.矩陣相等：兩同型矩陣（行數(shù)和列數(shù)都相等）對(duì)應(yīng)元素相等。2.行矩陣第2頁(yè)，共46頁(yè)，2023年，2月20日，星期三（2）矩陣與數(shù)相乘：該數(shù)與矩陣各元素相乘。5.單位矩陣：主對(duì)角線元素為1，其它所有的元素都為0的方陣。6.矩陣的運(yùn)算（1）矩陣的加法：兩同型矩陣的對(duì)應(yīng)元素相加。第3頁(yè)，共46頁(yè)，2023年，2月20日，星期三（3）矩陣與矩陣相乘：（4）矩陣的轉(zhuǎn)置：把矩陣的行換成同序數(shù)的列，記為第4頁(yè)，共46頁(yè)，2023年，2月20日，星期三7.矩陣的逆（逆矩陣）8.分塊矩陣：分塊后的矩陣與普通矩陣的運(yùn)算相同。第5頁(yè)，共46頁(yè)，2023年，2月20日，星期三9.正交矩陣：如果，則A為正交矩陣。它滿足：如果是正交矩陣，則行列式和矩陣的區(qū)別：矩陣是按一定方式排成的數(shù)表；行列式是一個(gè)數(shù)。第6頁(yè)，共46頁(yè)，2023年，2月20日，星期三（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)系。第7頁(yè)，共46頁(yè)，2023年，2月20日，星期三其中θ是a和b兩矢量間的夾角，如圖所示。三、矢量的點(diǎn)積（內(nèi)乘積或標(biāo)量積）換句話說(shuō)：一個(gè)矢量在另一個(gè)矢量上的投影等于該矢量與另一矢量方向上單位矢量的點(diǎn)積。再令a=j（j為a方向上的單位矢量），則即兩矢量方向上單位矢量的點(diǎn)乘等于兩矢量夾角的余弦。標(biāo)量積令b=i（i為b方向上的單位矢量），則第8頁(yè)，共46頁(yè)，2023年，2月20日，星期三四、矢量的叉積（矢量積或叉乘積）其中矢量c的模為:其中θ是a和b間小于等于1800的夾角，若將a按右手法則繞c轉(zhuǎn)θ角至b，右手拇指指向?yàn)閏的正方向（如上圖所示），c與a、b兩者垂直。則叉乘積若a和b用分量的形式表示為:第9頁(yè)，共46頁(yè)，2023年，2月20日，星期三a和b的點(diǎn)乘為：將點(diǎn)乘和叉乘應(yīng)用于右手笛卡爾坐標(biāo)系的單位矢量i，j，k，有：第10頁(yè)，共46頁(yè)，2023年，2月20日，星期三3.2位姿描述與坐標(biāo)變換3.2.1

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

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

yA

zA第13頁(yè)，共46頁(yè)，2023年，2月20日，星期三3.2.2坐標(biāo)變換如圖所示，坐標(biāo)系{B}與{A}方向相同，但原點(diǎn)不重合。坐標(biāo)平移

一、坐標(biāo)平移此式稱為平移方程。其中是B系中的原點(diǎn)在A系中的表示。第14頁(yè)，共46頁(yè)，2023年，2月20日，星期三二、坐標(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)分量分別為：和第15頁(yè)，共46頁(yè)，2023年，2月20日，星期三利用點(diǎn)乘的性質(zhì)和上式共同求解得將代入上面三式中并寫成矩陣形式得所以有第16頁(yè)，共46頁(yè)，2023年，2月20日，星期三上式簡(jiǎn)寫為：

此式稱為坐標(biāo)旋轉(zhuǎn)方程。其中旋轉(zhuǎn)矩陣表示了坐標(biāo)系{B}相對(duì)于{A}的方位，正好與剛體姿態(tài)的描述相同。同理也可得和都是正交矩陣，因此滿足由與互逆，可得第17頁(yè)，共46頁(yè)，2023年，2月20日，星期三旋轉(zhuǎn)矩陣的幾何意義：旋轉(zhuǎn)矩陣在幾何上表示了發(fā)生相互旋轉(zhuǎn)的兩坐標(biāo)系各主軸之間的相互方位關(guān)系。若把寫成行向量的形式，則其中每一個(gè)元素都是一個(gè)列向量。容易得出滿足六個(gè)約束條件（稱正交條件）：第18頁(yè)，共46頁(yè)，2023年，2月20日，星期三因此寫出三個(gè)基本的旋轉(zhuǎn)矩陣，即分別繞x、y和z軸轉(zhuǎn)θ角的旋轉(zhuǎn)矩陣：x’y’z’xyzx’y’z’xyzx’y’z’xyz第19頁(yè)，共46頁(yè)，2023年，2月20日，星期三例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第20頁(yè)，共46頁(yè)，2023年，2月20日，星期三三、一般變換一般的情況：坐標(biāo)系{B}的原點(diǎn)既不與{A}重合，方位也不相同。{C}系與｛Ｂ｝系原點(diǎn)重合，但方位不同，所以得{C}系與｛A｝系原點(diǎn)不重合，但方位相同，所以得進(jìn)而有和第21頁(yè)，共46頁(yè)，2023年，2月20日，星期三例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)矩陣。若,求。解：第22頁(yè)，共46頁(yè)，2023年，2月20日，星期三所以有：最后得：第23頁(yè)，共46頁(yè)，2023年，2月20日，星期三3.3齊次坐標(biāo)與齊次變換復(fù)合變換式可以表示成等價(jià)的齊次變換式。簡(jiǎn)寫成綜合地表示了平移和旋轉(zhuǎn)變換。第24頁(yè)，共46頁(yè)，2023年，2月20日，星期三3.3.1齊次坐標(biāo)一般來(lái)說(shuō)，以N+1維矢量表達(dá)N維位置矢量的方法稱為齊次坐標(biāo)表示法。在三維直角坐標(biāo)系中，一個(gè)點(diǎn)可以表示為，它的齊次坐標(biāo)就是，即滿足Px=ωPx/ω，Py=ωPy/ω，Pz=ωPz/ω（ω是非零整數(shù)）?？梢钥闯?，在三維直角坐標(biāo)系中，由于ω取值的不同，一個(gè)點(diǎn)的齊次坐標(biāo)的表達(dá)不唯一。第25頁(yè)，共46頁(yè)，2023年，2月20日，星期三齊次坐標(biāo)不僅可以規(guī)定點(diǎn)的位置（ω為非零整數(shù)），還可以用來(lái)規(guī)定矢量的方向（第四個(gè)元素為零時(shí)）。列向量(）表示空間的無(wú)窮遠(yuǎn)點(diǎn)，a，b和c稱為它的方向數(shù)。分別代表了ox，oy和oz軸的無(wú)窮遠(yuǎn)點(diǎn)，用它們分別表示這三個(gè)坐標(biāo)軸的方向。另外，代表坐標(biāo)原點(diǎn)，沒(méi)有意義。注意：位置矢量究竟是3×1的直角坐標(biāo)還是4×1的齊次坐標(biāo)，應(yīng)根據(jù)上下文而定。第26頁(yè)，共46頁(yè)，2023年，2月20日，星期三在機(jī)器人研究中，齊次變換矩陣T為：3.3.2齊次變換齊次變換矩陣是4×4的矩陣，它的完整形式可以看成是由四個(gè)子矩陣組成：第27頁(yè)，共46頁(yè)，2023年，2月20日，星期三

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

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

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

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

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