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

若基矢量相互正交，即它們在原點o處兩兩相交成直角，則它們構成直角坐標系或笛卡兒坐標系。斜角坐標系若按右手法則繞oz軸轉900可以使ox軸轉向oy軸，則稱為右手坐標系；按左手法則形成的坐標系稱左手坐標系。

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

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

既表示了剛體F在{A}系中的方位，也描述了{B}系在{A}系中的姿態。其中：xByBzBxA

yA

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

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

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

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

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

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

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

如果與坐標軸重合，則可得到繞x,y和z軸旋轉的基