.,1,東北大學人工智能與機器人研究所2016.9,第三章機器人坐標系統,.,2,機器人是個復雜的運動系統，它的每一個動作都是各個元部件共同作用的結果。,.,3,3.1位置與姿態3.2正交坐標系3.3運動坐標表示3.4齊次坐標變換3.5機器人坐標系統,為了系統地、精確地描述各個元部件的作用以及它們之間的關系，需要引入一套機器人坐標系統。,.,4,要全面地確定一個物體在三維空間中的狀態需要有三個位置自由度和三個姿態自由度。前者用來確定物體在空間中的具體方位，后者則是確定物體的指向。我們將物體的六個自由度的狀態稱為物體的位姿。,如果H為手坐標系，用以描述手的姿態，那再加上手的位置就構成了手的位姿。,3.1位置與姿態,一般姿態的描述可以用橫滾（Roll）、俯仰（Pitch）和側擺（Yaw）三軸的轉角來實現。,.,5,飛機飛行姿態變化,.,6,3.2正交坐標系,3.2.1正交坐標系及矢量的基礎知識,右圖是所謂的正交坐標系B(x,y,z)，用來表示機器人的基坐標，其中,分別是三個坐標軸的單位向量。B系中有另外一個坐標系H（xH，yH，zH），用來表示手坐標,其中,分別是H系三個坐標軸的單位向量。,z,y,x,B,H,H,z,H,x,H,y,a,n,o,i,j,k,P,端點P相對于機器人手坐標系H,及基座坐標系B的定位,.,7,3.2.1.1正交坐標系的性質,單位矢量,在基坐標系中可表示為:,根據矢量點積和叉積的性質，對于相互正交的單位矢量，有,對于單位矢量,也有同樣的性質。,.,8,其中是a和b兩矢量間的夾角，如圖3-2所示。,矢量的點積（內乘積或標量積）,換句話說：一個矢量在另一個矢量上的投影等于該矢量與另一矢量方向上單位矢量的點積。,再令a=j（j為a方向上的單位矢量），則,即兩矢量方向上單位矢量的點乘等于兩矢量夾角的余弦。,圖3-2標量積,令b=i（i為b方向上的單位矢量），則,.,9,矢量的叉積（矢量積或叉乘積）,其中矢量c的模為:,其中是a和b間小于等于1800的夾角，若將a按右手法則繞c轉角至b，右手拇指指向為c的正方向（如圖3-3），c與a、b兩者垂直。,則,圖3-3叉乘積,.,10,a和b的點乘為：,將點乘和叉乘應用于右手笛卡爾坐標系的單位矢量i，j，k，有：,2008-7,.,11,令矩陣R稱為正交坐標變換矩陣。,當用列向量表示單位矢量時，有,于是，變換矩陣R可以表示為：,當用矩陣表示兩個矢量的點乘時，有,.,12,3.2.1.2正交坐標變換矩陣R的性質,顯然,于是可得,1,-,=,R,R,T,.,13,3.2.1.3正交坐標變換矩陣的幾何意義,上式可寫成,其中,考慮到,上式表明正交坐標變換矩陣R實現了由手坐標系H到基坐標系B的正交坐標變換，它可以將一組3個相互正交的單位矢量變換為另一組3個相互正交的單位矢量，每一組單位矢量均代表了一個正交坐標系。這也說明了將矩陣R稱為正交坐標變換矩陣的原因。在機器人學中經常要用到這種正交坐標變換。,.,14,3.2.2位置的描述,一旦建立起一個坐標系，我們就可以用3維的位置矢量來確定該空間內任一點的位置。其中，x、y、z是p點在笛卡爾坐標系的三個坐標軸上坐標分量。用這種方法可以很容易地表示出手坐標（原點）在基坐標系中的空間位置。,3.2.3姿態的描述,物體的姿態可由某個固接在物體上的坐標系來描述。設在空間中除了有參考坐標系B外，還有物體質心上的一個笛卡爾正交坐標系H，且H系與此物體的空間位置關系是固定不變的，那么就可以以H系三個坐標軸的單位矢量相對于B系的方向來表示H系和B系的姿態。,.,15,.,16,假設為H坐標系中某軸的單位向量，即它在B坐標系的方向可以以與B系三軸夾角的余弦值為分量加以表達，見下圖.,故有,j,l,g,x,y,z,k,B,l,l,a,l,b,i,矢量的方向矢徑表示,由：,且：,.,17,因此正交坐標變換矩陣R為一方向余弦矩陣，也被稱為旋轉矩陣（具體含義將在后面小節中闡述）。,根據前面的推導可得：,當：,.,18,b)姿態（方位）的描述,采用旋轉矩陣來表示剛體姿態（方位），即由B系的三個單位主矢量相對于坐標系A的方向余弦組成：,既表示了剛體F在A系中的方位，也描述了B系在A系中的姿態。,其中：,xByBzB,xAyAzA,.,19,3.3運動坐標表示,3.3.1平動的坐標表示,設手坐標系H與基坐標系B具有相同的姿態，但H系坐標原點與B系的原點不重合。用矢量來描述H系相對于B系的位置（如右圖所示），稱為H系相對于B系的平移矢量。如果點p在H系中的位置為，那么它相對于B系的位置矢量可由矢量相加得出，即,稱其為坐標平移方程。,.,20,下面以繞z軸轉動角為例來研究繞坐標軸轉動某個角度的表示法。設H系從與B系相重合的位置繞B系的z軸轉動角，H系與B系的關系如右圖所示。,3.3.2轉動的坐標表示,(1)繞坐標軸轉動某個角度的表示法,.,21,若將H系的3個單位矢量表示在B系中，則有：,實現兩個坐標系之間轉動關系的矩陣，又叫轉動矩陣R，可表示為：,，,，,.,22,同理，可以得出當繞X軸旋轉時：,當繞Y軸旋轉時：,上面的分析說明了R矩陣可以用來表示繞坐標軸的轉動，這表征了R矩陣的另一種幾何意義。,.,23,因此寫出三個基本的旋轉矩陣，即分別繞x、y和z軸轉角的旋轉矩陣：,xyz,xyz,xyz,xyz,xyz,xyz,.,24,設B系與H系的z軸相重合，B系繞z軸轉動角就得H系，如下圖所示。,(2)兩個坐標系的投影之間的關系,P,.,25,已知矢徑在H系三軸投影分別為u,v,w。則由上圖可知,由上式可見，R矩陣可以將矢徑在手坐標系上的投影變換到該矢徑在基坐標系上的投影，這表征了R矩陣的又一種幾何意義。,于是有,（）,.,26,例3.1若從基坐標系(B)到手爪坐標系(E)的旋轉變換矩陣為。（1）畫出兩坐標系的相互方位關系（不考慮E的原點位置）；（2）如果給出OE(E系的原點）在B中的位置矢量為（1，2，2），畫出兩坐標系的相對位姿關系。,解：,xEyEzE,xByBzB,(1),(2),.,27,27,(3)具有轉動關系的兩個矢量的投影之間的關系,設矢量在坐標系Bxy的投影為u，v，w；將矢量繞z軸轉動角，得到矢量，設矢量在同一坐標系的投影為x,y,z，如下圖所示。,y,.,28,y,如果注意到在x,y軸的投影相當于在軸的投影，再對比6頁和9頁的兩個圖所示的相同幾何關系，便可得到與式（）相同結果，只是此時的u，v，w與x，y，z同前面討論的情況的幾何含義不同。這時矩陣R用來表示具有轉動關系的兩個矢量在同一坐標系中的投影之間的關系，這表征了R矩陣的最后一種幾何意義。,.,29,至此，歸納了R矩陣的四種幾何意義：1、實現了由手坐標系H到基坐標系B的正交坐標變換。2、用來表示繞坐標軸的轉動。3、將矢徑在手坐標系上的投影變換到該矢徑在基坐標系上的投影。4、表示具有轉動關系的兩個矢量在同一坐標系中的投影之間的關系。這對于認識R矩陣的本質，研究機器人的坐標系統很有幫助。,.,30,3.3.3復合運動的坐標表示,基坐標系B和手坐標系H的原點不重合，而且兩坐標系的姿態也不相同的情況。,.,31,對于任意一點P在B和H系中的描述有以下的關系,其中，,是p點相對于B系的位置矢量。,至此，我們由淺入深地介紹了物體的基本宏觀運動在坐標系中的表示方法，這是我們學習機器人復雜運動的最基本的數學工具。在后續章節中會頻繁地用到。,再由式(rp)，可得復合變換,可把上式看成坐標旋轉和坐標平移的復合變換。實際上，規定一個過渡坐標系C，使C的坐標原點與H系重合，而C的姿態和B系保持一致。根據式（）可得由H系到過渡坐標系C的坐標變換為,其中，,是點P在C中的位置矢量。,(rp),.,32,例3.2已知坐標系B初始位姿與A重合，首先B相對A的zA軸轉30，再沿A的xA軸移動10個單位，并沿A的yA軸移動5個單位。求位置矢量和旋轉矩陣。若,求。,解：,.,33,所以有：,最后得：,.,34,3.4齊次坐標變換,3.4.1齊次坐標的定義和性質,3.4.1.1齊次坐標的概念,用四個數所組成的列向量來表示三維空間中的一點，這兩個坐標向量之間的關系是:，則稱為三維空間點的齊次坐標。通常情況下取w=1,則的齊次坐標表示為。,一般說來，以（N+1)維矢量來表示N維位置矢量，稱為齊次坐標表示法。,.,35,3.4.1.2齊次坐標的性質,（1）齊次坐標的不唯一性所謂不唯一性是指某點的齊次坐標有無窮多點，不是單值確定的。例如是某點的齊次坐標，則也是該點的齊次坐標。,（2）齊次坐標的原點和坐標軸根據齊次坐標的定義，齊次坐標表示坐標原點，而，分別表示OX軸、OY軸和OZ軸的無窮遠點，即表示直角坐標的OX軸、OY軸和OZ軸。,.,36,則有,其中，,（）,.,37,3.4.2齊次變換和齊次矩陣,在引入齊次坐標之后，現在我們來看如何用齊次坐標來表示上一節中所講的內容。在上一節的最后我們曾用笛卡爾坐標系統表示出了物體的復合運動，最后得出了的結論，它表示了由到的變換。現在我們利用齊次坐標來表示出上式：,.,38,A矩陣稱為齊次矩陣（Homogeneousmatrix）,在機器人學中是個重要的術語，它將轉動和移動組合在一個44矩陣中。其中為33的轉動矩陣，為13的零陣，為表示移動的31的列陣。接下來我們將利用齊次矩陣來表示物體的運動。,.,39,3.4.2.1利用齊次矩陣表示平移變換,設向量，要和向量相加得V，即（）,欲求一變換矩陣H，使得U經過H變換之后變成向量V，即（）考慮到式（）和式（）等效，根據式（）可知,平移變換就是用于兩個向量的相加。,.,40,此變換矩陣有一性質就是它的每一個元素乘上一個非零的元素后不會改變這個變換。,由此可知得,.,41,3.4.2.2利用齊次矩陣表示旋轉變換,根據直角坐標和齊次坐標的關系，易得繞X，Y，Z軸旋轉一個角度的相應旋轉變換是,.,42,純旋轉的齊次變換矩陣中P31為零矩陣，即，因此寫出繞x，y和z軸旋轉角的基本齊次變換矩陣為：,純平移的齊次變換矩陣中R33=I33（單位陣），因此可以寫出沿x，y和z軸移動Px，Py和Pz單位的基本平移變換陣：,.,43,例如，已知一個向量U繞Z軸旋轉90變成V，則用旋轉矩陣表示為,如，一個向量U先后繞X、Y軸分別旋轉90、60得到V，用旋轉矩陣表示為,.,44,3.4.2.3利用齊次矩陣表示旋轉加平移變換,把上述兩種變換結合起來用齊次矩陣表示，這時的齊次變換矩陣就是,.,45,可見，在齊次變換矩陣中旋轉矩陣和表示平移的列陣確實是分離的。,注意，一般情況下,.,46,3.4.2.4利用齊次矩陣表示手的轉動和移動,手的轉動可以表示為繞X軸的側擺，繞Y軸的俯仰和繞Z軸橫滾，依次構成的復合轉動，采用簡化符號，則有,.,47,上式表示了手的轉動運動。如果手除了轉動運動以外還可做移動運動，只需將上式中齊次矩陣的第4列用表示移動的矩陣塊來代替，便可得到包括3個轉動和3個平動的6自由度運動的齊次矩陣。,.,48,3.4.3齊次變換的性質,3.4.3.1變換過程的相對性相對變換,前面所介紹的所有旋轉和平移變換都是相對于參考坐標系B系而言的。例如上述的變換過程是：手坐標系H首先繞著基坐標系B旋轉，然后平移。這種變換的順序是從右向左進行的。這樣的過程也可以以相反的順序進行，即從左向右進行。此時可以理解為首先手坐標系H在基坐標系B中平移然后繞當前的手坐標系H的軸旋轉。,.,49,一般的變換過程可以分兩種情況：,(1)如果我們用一個描述平移和（或）旋轉的變換C，左乘一個坐標系的變換T，那么產生的平移和（或）旋轉就是相對于靜止坐標系進行的。,(2)如果我們用一個描述平移和（或）旋轉的變換C，右乘一個坐標系的變換T，那么產生的平移和（或）旋轉就是相對于運動坐標系進行的。,.,50,相對于固定坐標系運動,相對于活動坐標系運動,.,51,3.4.3.2變換過程的可逆性逆變換,在機器人學中很多時候要用到齊次變換矩陣的逆陣，下面我們將推導齊次變換矩陣的逆陣求法。,由此可見,將上兩式表示成矩陣的形式，即,.,52,3.4.3.3變換過程的封閉性-變換方程的建立,在解機器人運動學和動力學方程時，要經常解變換方程。在這些變換方程里，一個坐標點往往要用兩種或多種方式來描述。,（1）機器人變換Z：參考坐標系U基坐標系B變換A：基坐標系B手坐標系H變換E：手坐標系H加工工具T（2）變位機變換P：參考坐標系U變位機V變換Q：變位機V被加工件W,.,53,這種聯系亦可由一有向變換圖表示，見右圖。,如果我們希望解上述方程，求出變換A，就必須對方程左乘，然后右乘，得到實際上，可以從封閉的向變換圖的任一變換開始列變換方程。從某一變換弧開始，順箭頭方向為正方向，逆箭頭方向為逆變換，一直連續列寫到相鄰于該變換弧為止（但不再包括該起點變換），如果包括該起點變換，則得到一個單位變換。,.,54,3.1.2.5旋轉變換通式,一.旋轉變換通式,令是過A系原點的單位矢量，求繞K旋轉角到B系的旋轉矩陣R(K,)，即。,.,55,因此,將上式展開得,圖3-11尺寸鏈圖,.,56,把上式右端相乘，并利用旋轉矩陣的正交性質,進行化簡整理后得,其中，s=sin;c=cos;Vers=(1-cos)。,如果與坐標軸重合，則可得到繞x,y和z軸旋轉的基本旋轉矩陣。,.,57,二.等效轉軸與等效轉角,對于給定的旋轉矩陣R,令R=R(K,)，得,任何一組經過有限次基本旋轉變換后的復合旋轉總可以等效成繞某一過原點的軸線轉角的單一旋轉。,.,58,將方程兩邊的主對角線元素分別相加，得,于是可得：,再把方程兩邊的非對角元素成對相減得：,將上式兩邊平方后再相加得：,.,59,于是：,兩點注意:多值性：K和的值不唯一。實際上，對于任意一組K和，都對應另一組-K和-，(K,)和(k,+n360)對應的轉動效果相同，的取值也有多種，一般取在0到180之間。,例：求復合變換的等效轉軸k和轉角。,病態情況：當轉角很小時，轉軸難確定；當接近0或