位姿描述和齊次變換第一頁，共六十九頁，2022年，8月28日

1.方向角與方向余弦=AOB(0)為向量，的夾角，記作=方向角的余弦稱為其方向余弦.

方向余弦2.向量在軸u上的投影等于向量的模乘以軸與向量的夾角的余弦：第二頁，共六十九頁，2022年，8月28日向量補充已知：a=(a1,a2,a3),b=(b1,b2,b3)第三頁，共六十九頁，2022年，8月28日空間任意兩直線的公法線長度公式給定一直線過p點，具有方向矢量m，另一直線過點q，具有方向矢量n，則：第四頁，共六十九頁，2022年，8月28日位置描述(position)---點在坐標(biāo)系的位置一旦建立了一個坐標(biāo)系，我們就能夠用某個3×1位置矢量來確定該空間內(nèi)任一點的位置。對于直角坐標(biāo)系{A}，空間任一點p的位置可用3×1的列矢量AP表示。其中，px、py、pz入是點p在坐標(biāo)系{A}中的三個坐標(biāo)分量。Ap的上標(biāo)A代表參考坐標(biāo)系{A}。我們稱Ap為位置矢量，見圖2．1。

第五頁，共六十九頁，2022年，8月28日方位描述(orientation)物體的方位可由某個固接于此物體的坐標(biāo)系描述為了規(guī)定空間某剛體B的方位，設(shè)置一直角坐標(biāo)系{B}與此剛體固接。用坐標(biāo)系{B}的三個單位主矢量xB、yB、zB相對于參考坐標(biāo)系{A}方向余弦組成的3×3矩陣來表示剛體B相對于坐標(biāo)系{A}的方位。稱為姿態(tài)矩陣/旋轉(zhuǎn)矩陣。式中，上標(biāo)A代表參考坐標(biāo)系{A}，下標(biāo)B代表被描述的坐標(biāo)系{B}。共有9個元素，但只有3個是獨立的。由于的三個列矢量AxB、AyB、和AzB都是單位矢量，且雙雙相互垂直，因而它的9個元素滿足6個約束條件(正交條件)。

第六頁，共六十九頁，2022年，8月28日位姿描述要完全描述剛體B在空間的位姿(位置和姿態(tài))，通常將物體B與某一坐標(biāo)系{B}相固接。{B}的坐標(biāo)原點一般選在物體B的特征點上，如質(zhì)心等。相對參考系{A}，坐標(biāo)系{B}的原點位置和坐標(biāo)軸的方位，分別由位置矢量B和旋轉(zhuǎn)矩陣描述。這樣，剛體B的位姿可由坐標(biāo)系{B}來描述，即有

(2.9)Y(orientation)

x(normal)

z(approach)

第七頁，共六十九頁，2022年，8月28日手抓坐標(biāo)系Y(orientation)

x(normal)

z(approach)

第八頁，共六十九頁，2022年，8月28日平移坐標(biāo)變換

(2.10)前面討論的是在一個坐標(biāo)系中位姿的描述，在大量的機器人問題中，涉及到用不同的坐標(biāo)系來描述同一個剛體的位置及姿態(tài)問題，這就涉及到從一個坐標(biāo)系的描述到另一個坐標(biāo)系的描述之間的變換關(guān)系，這種變換關(guān)系包括：平移變換和旋轉(zhuǎn)變換

第九頁，共六十九頁，2022年，8月28日

旋轉(zhuǎn)矩陣

設(shè)固定參考坐標(biāo)系直角坐標(biāo)為ΣOxyz，動坐標(biāo)系為ΣO′uvw，研究旋轉(zhuǎn)變換情況。①

初始位置時，動靜坐標(biāo)系重合，O、O′重合，如圖。各軸對應(yīng)重合，設(shè)P點是動坐標(biāo)系ΣO′uvw中的一點，且固定不變。則P點在ΣO′uvw中可表示為：

、、為坐標(biāo)系ΣO′uvw的單位矢量，則P點在Σoxyz中可表示為：第十頁，共六十九頁，2022年，8月28日當(dāng)動坐標(biāo)系ΣO′uvw繞O點回轉(zhuǎn)時，求P點在固定坐標(biāo)系Σoxyz中的位置已知：P點在ΣO′uvw中是不變的仍然成立，由于ΣO′uvw回轉(zhuǎn)，則：用矩陣表示為:（2-7）第十一頁，共六十九頁，2022年，8月28日反過來：第十二頁，共六十九頁，2022年，8月28日旋轉(zhuǎn)矩陣的幾何意義第十三頁，共六十九頁，2022年，8月28日三個基本旋轉(zhuǎn)矩陣即動坐標(biāo)系求的旋轉(zhuǎn)矩陣，也就是求出坐標(biāo)系中各軸單位矢量在固定坐標(biāo)系中各軸的投影分量，很容易得到在重合時，有：第十四頁，共六十九頁，2022年，8月28日由圖2-5可知，在y軸上的投影為，在z軸上的投影為,在y軸上的投影為，在z軸上的投影為，所以有：方向余弦陣第十五頁，共六十九頁，2022年，8月28日同理：三個基本旋轉(zhuǎn)矩陣:

第十六頁，共六十九頁，2022年，8月28日繞坐標(biāo)軸轉(zhuǎn)動的旋轉(zhuǎn)矩陣式中，s表示sin，c表示cos。以后將一律采用此約定。

第十七頁，共六十九頁，2022年，8月28日旋轉(zhuǎn)矩陣---舉例[例1]已知轉(zhuǎn)動坐標(biāo)系OUVW中的兩點aUVW＝(4，3，2)T和bUVW＝(6，2，4)T，若OUVW系統(tǒng)繞OZ軸轉(zhuǎn)動了60。，試求參考坐標(biāo)系中的相應(yīng)點axyz和bxyz。[解]第十八頁，共六十九頁，2022年，8月28日旋轉(zhuǎn)矩陣---舉例[例2]已知參考坐標(biāo)系OXYZ中的兩點aXYZ＝(4，3，2)T和bXYZ＝(6，2，4)T，若OUVW系統(tǒng)繞OZ軸轉(zhuǎn)動了60。，試求轉(zhuǎn)動坐標(biāo)系中的相應(yīng)點aUVW和bUVW。[解]第十九頁，共六十九頁，2022年，8月28日

合成旋轉(zhuǎn)矩陣:例1：在動坐標(biāo)中有一固定點，相對固定參考坐標(biāo)系做如下運動：①R（x,90°）；②R(z,90°)；③R(y,90°)。求點在固定參考坐標(biāo)系下的位置。解1：用畫圖的簡單方法第二十頁，共六十九頁，2022年，8月28日解2：用分步計算的方法①R（x,90°）②R（z,90°）③R（y,90°）（2-14）（2-15）（2-16）第二十一頁，共六十九頁，2022年，8月28日

上述計算方法非常繁瑣，可以通過一系列計算得到上述結(jié)果。將式（2-14）（2-15）（2-16）聯(lián)寫為如下形式：R3x3為二者之間的關(guān)系矩陣，我們令：定義1：當(dāng)動坐標(biāo)系繞固定坐標(biāo)系各坐標(biāo)軸順序有限次轉(zhuǎn)動時，其合成旋轉(zhuǎn)矩陣為各基本旋轉(zhuǎn)矩陣依旋轉(zhuǎn)順序左乘。注意：旋轉(zhuǎn)矩陣間不可以交換第二十二頁，共六十九頁，2022年，8月28日旋轉(zhuǎn)次序?qū)ψ儞Q結(jié)果的影響第二十三頁，共六十九頁，2022年，8月28日合成旋轉(zhuǎn)矩陣為了表示繞OXYZ坐標(biāo)系各軸的一連串有限轉(zhuǎn)動，可把基本旋轉(zhuǎn)矩陣連乘起來。由于矩陣乘法不可交換，故完成轉(zhuǎn)動的次序是重要的。例如，先繞OX軸轉(zhuǎn)α角，然后繞OZ袖轉(zhuǎn)θ角，再繞OY轉(zhuǎn)ψ角；表示這種轉(zhuǎn)動的旋轉(zhuǎn)矩陣為

如果轉(zhuǎn)動的次序變化為，先繞OY轉(zhuǎn)ψ角繞OX軸轉(zhuǎn)α角，然后繞OZ袖轉(zhuǎn)θ角，再繞OX軸轉(zhuǎn)α角；表示這種轉(zhuǎn)動的旋轉(zhuǎn)矩陣為

第二十四頁，共六十九頁，2022年，8月28日除繞OXYZ參考系的坐標(biāo)軸轉(zhuǎn)動外，OUVW坐標(biāo)系也可以繞它自己的坐標(biāo)軸轉(zhuǎn)動。這時，合成旋轉(zhuǎn)矩陣可按下述簡單規(guī)則求得：1.兩坐標(biāo)系最初重合，因此旋轉(zhuǎn)矩陣是一個3×3單位矩陣I3。2．如果OUVW坐標(biāo)系繞OXYZ坐標(biāo)系的一坐標(biāo)軸轉(zhuǎn)動，則可對上述旋轉(zhuǎn)矩陣左乘相應(yīng)的基本旋轉(zhuǎn)矩陣。3．如果OUVW坐標(biāo)系繞自己的一坐標(biāo)鈾轉(zhuǎn)動，則可對上述旋轉(zhuǎn)矩陣右乘相應(yīng)的基本旋轉(zhuǎn)矩陣合成旋轉(zhuǎn)矩陣規(guī)則先繞OY軸轉(zhuǎn)ψ角，然后繞OW袖轉(zhuǎn)θ角，再繞OU轉(zhuǎn)α角；表示這種轉(zhuǎn)動的旋轉(zhuǎn)矩陣為第二十五頁，共六十九頁，2022年，8月28日位姿坐標(biāo)變換/一般變換(2.13)第二十六頁，共六十九頁，2022年，8月28日位姿坐標(biāo)變換---示例例2．1已知坐標(biāo)系{B}的初始位姿與{A}重合，首先{B}相對于坐標(biāo)系{A}的zA軸轉(zhuǎn)30°，再沿{A}的xA軸移動12單位，并沿{A}的yA軸移動6單位。求位置矢量APB0和旋轉(zhuǎn)矩陣。假設(shè)點p在坐標(biāo)系{B}的描述為BP＝[5．9，0]T，求它在坐標(biāo)系{A}中的描述AP。

第二十七頁，共六十九頁，2022年，8月28日齊次坐標(biāo)

一般來說，n維空間的齊次坐標(biāo)表示是一個（n+1）維空間實體。有一個特定的投影附加于n維空間，也可以把它看作一個附加于每個矢量的特定坐標(biāo)—比例系數(shù)。

式中i,j,k為x,y,z軸上的單位矢量，a=,b=,c=，w為比例系數(shù)

顯然，齊次坐標(biāo)表達(dá)并不是唯一的，隨w值的不同而不同。在計算機圖學(xué)中，w作為通用比例因子，它可取任意正值，但在機器人的運動分析中，總是取w=1。列矩陣第二十八頁，共六十九頁，2022年，8月28日[例]：可以表示為：

V=[3451]T

或V=[68102]T

或V=[-12-16-20-4]T

第二十九頁，共六十九頁，2022年，8月28日

齊次坐標(biāo)與三維直角坐標(biāo)的區(qū)別V點在ΣOXYZ坐標(biāo)系中表示是唯一的（x、y、z）而在齊次坐標(biāo)中表示可以是多值的。不同的表示方法代表的V點在空間位置上不變。第三十頁，共六十九頁，2022年，8月28日

幾個特定意義的齊次坐標(biāo)：[0,0,0,n]T—坐標(biāo)原點矢量的齊次坐標(biāo)，n為任意非零比例系數(shù)[1000]T—指向無窮遠(yuǎn)處的OX軸[0100]T—指向無窮遠(yuǎn)處的OY軸[0010]T—指向無窮遠(yuǎn)處的OZ軸

這樣，利用齊次坐標(biāo)不僅可以規(guī)定點的位置，還可以用來規(guī)定矢量的方向。第四個元素非零時，代表點的位置；第四個元素為零時，代表方向。第三十一頁，共六十九頁，2022年，8月28日平面的齊次坐標(biāo)平面齊次坐標(biāo)由行矩陣P=[abcd]來表示當(dāng)點v=[xyzw]T處于平面P內(nèi)時，矩陣乘積PV=O，或記為

如果定義一個常數(shù)m=，則有：=可以把矢量解釋為某個平面的外法線，此平面沿著法線方向與坐標(biāo)原點的距離為。第三十二頁，共六十九頁，2022年，8月28日因此一個平行于x、y軸，且在z軸上的坐標(biāo)為單位距離的平面P可以表示為：或

有：PV=例如：點V=[102011]T必定處于此平面內(nèi)，而點V=[0021]T處于P平面的上方點V=[0001]T處于P平面下方。因為：

與點矢相仿，平面也沒有意義第三十三頁，共六十九頁，2022年，8月28日齊次變換

其中，4×1的列矢量表示三維空間的點，稱為點的齊次坐標(biāo)，

齊次變換矩陣是4×4的方陣,綜合地表示了平移變換和旋轉(zhuǎn)變換。

第三十四頁，共六十九頁，2022年，8月28日T反映了∑O′在∑O中的位置和姿態(tài)，即表示了該坐標(biāo)系原點和各坐標(biāo)軸單位矢量在固定坐標(biāo)系中的位置和姿態(tài)。該矩陣可以由4個子矩陣組成，寫成如下形式：為姿態(tài)矩陣，表示動坐標(biāo)系∑O′在固定參考坐標(biāo)系∑O中的姿態(tài)，即表示∑O′各坐標(biāo)軸單位矢量在∑O各軸上的投影為位置矢量矩陣，代表動坐標(biāo)系∑O′坐標(biāo)原點在固定參考坐標(biāo)系∑O中的位置為透視變換矩陣，在視覺中進行圖像計算，一般置為0為比例系數(shù)第三十五頁，共六十九頁，2022年，8月28日平移齊次坐標(biāo)變換

空間某點由矢量ai+bj+ck描述。其中，i,j,k為軸x,y,z上的單位矢量。此點可用平移齊次變換表示為

例2．3作為例子，讓我們考慮矢量2i+3j+2k被矢量4i-3j+7k平移變換得到的新的點矢量原坐標(biāo)系中的表示平移后形成的新坐標(biāo)系新坐標(biāo)系中的表示第三十六頁，共六十九頁，2022年，8月28日相對變換

舉例說明：例1：動坐標(biāo)系∑0′起始位置與固定參考坐標(biāo)系∑0重合,動坐標(biāo)系∑0′做如下運動：①R(Z,90o)②R（y,90o）③Trans(4，-3,7)，求合成矩陣解1：用畫圖的方法：第三十七頁，共六十九頁，2022年，8月28日解2：用計算的方法

以上均以固定坐標(biāo)系多軸為變換基準(zhǔn)，因此矩陣左乘。如果我們做如下變換，也可以得到相同的結(jié)果：例2：①先平移Trans(4,-3,7)；②繞當(dāng)前軸轉(zhuǎn)動90o；③繞當(dāng)前軸轉(zhuǎn)動90o；求合成旋轉(zhuǎn)矩陣。（2-20）第三十八頁，共六十九頁，2022年，8月28日解1：用畫圖的方法解2：用計算的方法（2-21）第三十九頁，共六十九頁，2022年，8月28日式（2-20）和式（2-21）無論在形式上，還是在結(jié)果上都是一致的。因此我們有如下的結(jié)論：動坐標(biāo)系在固定坐標(biāo)系中的齊次變換有2種情況：定義1：如果所有的變換都是相對于固定坐標(biāo)系中各坐標(biāo)軸旋轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。定義2：如果動坐標(biāo)系相對于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為相對變換。結(jié)果均為為動坐標(biāo)系在固定坐標(biāo)中的位姿（位置+姿態(tài)）。相對于固定坐標(biāo)系，

也就是說，動坐標(biāo)系繞自身坐標(biāo)軸做齊次變換，要達(dá)到繞固定坐標(biāo)系相等的結(jié)果，就應(yīng)該用相反的順序。

第四十頁，共六十九頁，2022年，8月28日旋轉(zhuǎn)齊次坐標(biāo)變換

第四十一頁，共六十九頁，2022年，8月28日合成齊次變換除繞OXYZ參考系的坐標(biāo)軸轉(zhuǎn)動外，OUVW坐標(biāo)系也可以繞它自己的坐標(biāo)軸轉(zhuǎn)動。這時，合成旋轉(zhuǎn)矩陣可按下述簡單規(guī)則求得：1.兩坐標(biāo)系最初重合，因此旋轉(zhuǎn)矩陣是一個4×4單位矩陣I4。2．如果OUVW坐標(biāo)系繞（或沿）OXYZ坐標(biāo)系的一坐標(biāo)軸轉(zhuǎn)動（或平移），則左乘相應(yīng)的齊次變換矩陣--絕對變換。3．如果OUVW坐標(biāo)系繞（或沿）自己的一坐標(biāo)鈾轉(zhuǎn)動（或平移），則可右乘相應(yīng)的齊次變換矩陣—相對變換第四十二頁，共六十九頁，2022年，8月28日齊次變換矩陣T

的意義：機器人用到相對變換的時候比較多例如機械手抓一個杯子，如右圖所示，手爪需要轉(zhuǎn)動一個角度才抓的牢，相對于固定坐標(biāo)系表達(dá)太麻煩，可以直接根據(jù)手爪的坐標(biāo)系表示但也要知道在∑O中的位姿，就用右乘的概念。

oH第四十三頁，共六十九頁，2022年，8月28日齊次變換矩陣的幾何意義第四十四頁，共六十九頁，2022年，8月28日齊次變換矩陣的逆陣第四十五頁，共六十九頁，2022年，8月28日齊次變換矩陣的逆陣第四十六頁，共六十九頁，2022年，8月28日齊次變換矩陣舉例例：動坐標(biāo)系繞參考坐標(biāo)系的z軸旋轉(zhuǎn)30度，并分別沿x,y的正向平移3個和4個單位，求齊次變換矩陣及其逆陣。第四十七頁，共六十九頁，2022年，8月28日通用齊次變換

動坐標(biāo)系繞過P=[PX,PY,PZ]點而分量為kx,ky,kz的任意單位矢量k轉(zhuǎn)動ψ角時的變換矩陣

研究這種轉(zhuǎn)動的好處是，對于某種角運動，可以用動坐標(biāo)系繞某軸k的一次運動代替繞參考坐標(biāo)系（固定坐標(biāo)系）或(和)動坐標(biāo)系坐標(biāo)系的坐標(biāo)軸的數(shù)次運動動。

第四十八頁，共六十九頁，2022年，8月28日通用齊次變換第四十九頁，共六十九頁，2022年，8月28日通用齊次變換第五十頁，共六十九頁，2022年，8月28日通用齊次變換---例題第五十一頁，共六十九頁，2022年，8月28日等效轉(zhuǎn)角和轉(zhuǎn)軸R=Rot(f，θ)=

把上式兩邊的對角線項分別相加，并化簡得

所以把式(2．47)中的非對角線項成對相減可得第五十二頁，共六十九頁，2022年，8月28日等效轉(zhuǎn)角和轉(zhuǎn)軸對上式各行平方后相加得

所以把旋轉(zhuǎn)規(guī)定為繞矢量f的正向旋轉(zhuǎn)，使得o＜θ＜180°。這時，式(2．50)中的符號取正號，轉(zhuǎn)角θ被惟一地確定為

而矢量f的各分量可由式(2．49)求得第五十三頁，共六十九頁，2022年，8月28日等效轉(zhuǎn)角和轉(zhuǎn)軸---例題第五十四頁，共六十九頁，2022年，8月28日變換方程初步

基坐標(biāo)系目標(biāo)系工具系工作站系第五十五頁，共六十九頁，2022年，8月28日例題：試求立方體中心在機座坐標(biāo)系∑0中的位置該手爪從上方把物體抓起，同時手爪的開合方向與物體的Y軸同向，那么，求手爪相對于∑0的姿態(tài)是什么？

在機器人工作臺上加裝一電視攝像機，攝像機可見到固聯(lián)著6DOF關(guān)節(jié)機器人的機座坐標(biāo)系原點，它也可以見到被操作物體（立方體）的中心，如果在物體中心建一局部坐標(biāo)系，則攝像機所見到的這個物體可由齊次變換矩陣T1來表示，如果攝像機所見到的機座坐標(biāo)系為矩陣T2表示。xyz第五十六頁，共六十九頁，2022年，8月28日解1：因此物體位于機座坐標(biāo)系的（11，10，1）T處，它的X，Y，Z軸分別與機座坐標(biāo)系的-Y，X，Z軸平行。第五十七頁，共六十九頁，2022年，8月28日解2：第五十八頁，共六十九頁，2022年，8月28日第五十九頁，共六十九頁，2022年，8月28日繞固定軸x-y-z旋轉(zhuǎn)

第六十頁，共六十九頁，2022年，8月28日z-y-x歐拉角

第六十一頁，共六十九頁，2022年，8月28日Z-Y-Z歐拉角

第六十二頁，共六十九頁，2022年，8月28日3種最常見的歐拉角類型步1步2步3類型1繞OZ軸轉(zhuǎn)φ角繞當(dāng)前OU'

軸轉(zhuǎn)θ角繞當(dāng)前OW″軸轉(zhuǎn)ψ角類型2繞OZ軸轉(zhuǎn)φ角繞當(dāng)前OV'軸轉(zhuǎn)θ角繞當(dāng)前OW″軸轉(zhuǎn)ψ角類型3繞OX軸轉(zhuǎn)φ角繞OY軸轉(zhuǎn)θ角繞OZ軸轉(zhuǎn)ψ角φφφu′v′w′①x(u)y(v)z(w)oθu"