1、第2章機器人位置運動學2.1引言本章將研究機器人正逆運動學。當已知所有的關節變量時，可用正運動學來確定機器人末端手的位姿。如果要使機器人末端手放在特定的點上并且具有特定的姿態，可用逆運動學來計算出每一關節變量的值。首先利用矩陣建立物體、位置、姿態以及運動的表示方法，然后研究直角坐標型、圓柱坐標型以及球坐標型等不同構型機器人的正逆運動學，最后利用Denavit-Hartenberg（D-H）表示法來推導機器人所有可能構型的正逆運動學方程。實際上，機器手型的機器人沒有末端執行器，多數情況下，機器人上附有一個抓持器。根據實際應用，用戶可為機器人附加不同的末端執行器。顯然，末端執行器的大小和長度決定了

2、機器人的末端位置，即如果末端執行器的長短不同，那么機器人的末端位置也不同。在這一章中，假設機器人的末端是一個平板面，如有必要可在其上附加末端執行器，以后便稱該平板面為機器人的“手”或“端面”如有必要，還可以將末端執行器的長度加到機器人的末端來確定末端執行器的位姿。2.2機器人機構機器手型的機器人具有多個自由度（DOF），并有三維開環鏈式機構。在具有單自由度的系統中，當變量設定為特定值時，機器人機構就完全確定了，所有其他變量也就隨之而定。如圖2.1所示的四桿機構，當曲柄轉角設定為120時，則連桿與搖桿的角度也就確定了。然而在一個多自由度機構中，必須獨立設定所有的輸入變量才能知道其余的參數。機器人

3、就是這樣的多自由度機構，必須知道每一關節變量才能知道機器人的手處在什么位置。圖2.1具有單自由度閉環的四桿機構如果機器人要在空間運動，那么機器人就需要具有三維的結構。雖然也可能有二維多自由度的機器人，但它們并不常見。機器人是開環機構，它與閉環機構不同（例如四桿機構），即使設定所有的關節變量，也不能確保機器人的手準確地處于給定的位置。這是因為如果關節或連桿有絲毫的偏差，該關節之后的所有關節的位置都會改變且沒有反饋。例如，在圖2.2所示的四桿機構中，如果連桿AB偏移，它將影響OB桿。而在開環系統中（例如機器人），由于沒有反饋，之后的所有2構件都會發生偏移。于是，在開環系統中，必須不斷測量所有關節和

4、連桿的參數，或者監控系統的末端，以便知道機器的運動位置。通過比較如下的兩個連桿機構的向量方程，可以表示出這種差別，該向量方程表示了不同連桿之間的關系。OA+AB=OO+OB（2.1）1122OA+AB+BC=OC（2.2）11可見，如果連桿AB偏移，連桿OB也會相應地移動，式(2.1)的兩邊隨連桿的變化而2改變。而另一方面，如果機器人的連桿AB偏移，所有的后續連桿也會移動，除非Of有其他方法測量，否則這種變化是未知的。為了彌補開環機器人的這一缺陷，機器人手的位置可由類似攝像機的裝置來進行不斷測量，于是機器人需借助外部手段(比如輔助手臂或激光束)來構成閉環系統。或者按照常規做法，也可通過增加機器

5、人連桿和關節強度來減少偏移，采用這種方法將導致機器人重量重、體積大、動作慢，而且它的額定負載與實際負載相比非常小。圖2.2(a)閉環機構；(b)開環機構2.3機器人運動學的矩陣表示矩陣可用來表示點、向量、坐標系、平移、旋轉以及變換，還可以表示坐標系中的物體和其他運動元件。231空間點的表示空間點P(如圖2.3所示)可以用它的相對于參考坐標系的三個坐標來表示：P=ai+bj+ck(2.3)xyz其中，a,b,c是參考坐標系中表示該點的坐標。顯然，也可以用其他坐標來表示空間點的xyz位置。2.3.2空間向量的表示向量可以由三個起始和終止的坐標來表示。如果一個向量起始于點A,終止于點B,那么它可以表

6、示為PAB=(B-A)i+(B-A)j+(B-A)k。特殊情況下，如果一個向量起始于ABxxyyzz原點(如圖2.4所示)，則有：(2.4)其中a,b,c是該向量在參考坐標系中的三個分量。實際上，前一節的點P就是用連接到該點yz的向量來表示的，具體地說，也就是用該向量的三個坐標來表示。圖2.4空間向量的表示向量的三個分量也可以寫成矩陣的形式，如式（2.5）所示。在本書中將用這種形式來表示運動分量：a(2.5)xbycz這種表示法也可以稍做變化：加入一個比例因子w,如果x,y,z各除以w,則得到bycz于是，這時向量可以寫為：(2.6)x7y，其中a=,b=,等等xwyw變量w可以為任意數，而且

7、隨著它的變化，向量的大小也會發生變化，這與在計算機圖形學中縮放一張圖片十分類似。隨著w值的改變，向量的大小也相應地變化。如果w大于1，向量的所有分量都變大；如果w小于1,向量的所有分量都變小。這種方法也用于計算機圖形學中改變圖形與畫片的大小。如果w是1，各分量的大小保持不變。但是，如果w=0，a,b,c則為無窮大。在這種情xyz況下，x，y和z（以及a,b,c）表示一個長度為無窮大的向量，它的方向即為該向量所表示xyz的方向。這就意味著方向向量可以由比例因子w=0的向量來表示，這里向量的長度并不重要,而其方向由該向量的三個分量來表示。例2.1有一個向量P=3i+5j+2k，按如下要求將其表示成

8、矩陣形式：（1）比例因子為2（2）將它表示為方向的單位向量解：該向量可以表示為比例因子為2的矩陣形式，當比例因子為0時，則可以表示為方向向量，結果如下：103-5P=20然而，為了將方向向量變為單位向量，須將該向量歸一化使之長度等于1。這樣，向量的每一個分量都要除以三個分量平方和的開方：也;丐丐=616,其中p=忌=487,Py=6，等等和0.487Punit0.8110.32402.3.3坐標系在固定參考坐標系原點的表示一個中心位于參考坐標系原點的坐標系由三個向量表示，通常著三個向量相互垂直，稱為單位向量n,o,a，分別表示法線（normal）、指向（orientation）和接近（appr

9、oach）向量（如圖2.5所示）。正如2.3.3節所述，每一個單位向量都由它們所在參考坐標系著的三個分量表示。這樣，坐標系F可以由三個向量以矩陣的形式表示為：noaxxnoayynoazzxF=(2.7)yz圖2.5坐標系在參考坐標系原點的表示2.3.4坐標系在固定參考坐標系中的表示(2.8)如果一個坐標系不再固定參考坐標系的原點（實際上也可包括在原點的情況），那么該坐標系的原點相對于參考坐標系的位置也必須表示出來。為此，在該坐標系原點與參考坐標系原點之間做一個向量來表示該坐標系的位置（如圖2.6所示）。這個向量由相對于參考坐標系的三個向量來表示。這樣，這個坐標系就可以由三個表示方向的單位向量

10、以及第四個位置向量來表示。noaPxxxxnoapyyyynoaPzzzz0001F=如式（2.8）所示，前三個向量是w=o的方向向量，表示該坐標系的三個單位向量n,o,a的方向，而第四個w=1的向量表示該坐標系原點相對于參考坐標系的位置。與單位向量不同，向量P的長度十分重要，因而使用比例因子為1。坐標系也可以由一個沒有比例因子的3x4矩陣表示，但不常用。例2.2如圖2.7所示的F坐標系位于參考坐標系中3,5,7的位置，它的n軸與x軸平行，o軸相對于y軸的角度為45,a軸相對于z軸的角度為45。該坐標系可以表示為：TOC o 1-5 h z10000.707-0.707F=00.7070.70

11、73571圖2.7坐標系在空間的表示舉例0002.3.5剛體的表示一個物體在空間的表示可以這樣實現：通過在它上面固連一個坐標系，再將該固連的坐標系在空間表示出來。由于這個坐標系一直固連在該物體上，所以該物體相對于坐標系的位姿是已知的。因此，只要這個坐標系可以在空間表示出來，那么這個物體相對于固定坐標系的位姿也就已知了（如圖2.8所示）。如前所述，空間坐標系可以用矩陣表示，其中坐標原點以及相對于參考坐標系的表示該坐標系姿態的三個向量也可以由該矩陣表示出來。于是有：(2.9)object如第1章所述，空間中的一個點只有三個自由度，它只能沿三條參考坐標軸移動。但在空間的一個鋼體有六個自由度，也就是說

12、，它不僅可以沿著X,Y,Z三軸移動，而且還可繞這三個軸轉動。因此，要全面地定義空間以物體，需要用6條獨立的信息來描述物體原點在參考坐標系中相對于三個參考坐標軸的位置，以及物體關于這三個坐標軸的姿態。而式（2.9）給出了12條信息，其中9條為姿態信息，三條為位置信息（排除矩陣中最后一行的比例因子,因為它們沒有附加信息）。顯然，在該表達式中必定存在一定的約束條件將上述信息數限制為6。因此，需要用6個約束方程將12條信息減少到6條信息。這些約束條件來自于目前尚未利用的已知的坐標系特性，即：三個向量n,o,a相互垂直每個單位向量的長度必須為1圖2.8空間物體的表示我們可以將其轉換為以下六個約束方程：（

13、1）n-o=0（2）n-a=0（3）a-o=0（4）|n|=1（向量的長度必須為1）（2.10）（5）|o|=1（6）a=1因此，只有前述方程成立時，坐標系的值才能用矩陣表示。否則，坐標系將不正確。式（2.10）中前三個方程可以換用如下的三個向量的叉積來代替：nxo=a（2.11）例2.3對于下列坐標系，求解所缺元素的值，并用矩陣來表示這個坐標系。TOC o 1-5 h z_0?5_ HYPERLINK l bookmark64 0.707?3F=?02 HYPERLINK l bookmark50 _0001_解：顯然，表示坐標系原點位置的值5,3,2對約束方程無影響。注意在三個方向向量中只

14、有三個值是給定的，但這也已足夠了。根據式（2.10），得：no+no+no=0或n（0）+0.707（o）+n（o）=0 xxyyzzxyzzna+na+na=0或xxyyzzao+ao+ao=0或xxyyzzn2+n2+n2=1或xyzo2+o2+o2=1或xyza2+a2+a2=1或xyz將這些方程化簡得：n(a)+0.707(a)+n(0)=0 xxyza(0)+a(o)+0(o)=0 xyyzn2+0.7072+n2=1xz02+o2+o2=1yza2+a2+02=1xy0.707o+no=0yzzna+0.707a=0 xxyao=0yyn2+n2=0.5xzo2+o2=1yza2+

15、a2=1xy解這六個方程得：n=0.707,n=0,o=0,o=1,a=+0.707和a=-0.707。應注意，n和axzyzxyxx必須同號。非唯一解的原因是由于給出的參數可能得到兩組在相反方向上相互垂直的向-0.7070.707000.707-0.7075_3F=0102_0001-0.7070-0.7075_F=0.7070-0.70730102或_0001量。最終得到的矩陣如下：由此可見，兩個矩陣都滿足約束方程的要求。但應注意三個方向向量所表述的值不是任意的，而是受這些約束方程的約束，因此不可任意給矩陣賦值。同樣，可通過n與o叉乘并令其等于a，即nxo=a來求解，表示如下:ijknnn

16、=ai+aj+akxyzxyzoooxyz或表示為：i(no-no)-j(no-no)+k(no-no)=ai+aj+akyzzyxzzxxyyxxyz將值代入方程得：i(0.707o-no)-j(no)+k(no)=ai+aj+0kzzyxzxyxy同時解下面這三個方程：0.707ono二azzyxno=axzyno=0 xy以此來代替三個點乘方程。聯立三個單位向量長度的約束方程，便得到六個方程，求解這六個方程，可得到相同的未知參數的解。2.4齊次變換矩陣由于各種原因，變換矩陣應寫成方型形式，3x3或4x4均可。首先，如后面看到的，計算方形矩陣的逆要比計算長方形矩陣的逆容易得多。其次，為使兩

17、矩陣相乘，它們的維數必須匹配，即第一矩陣的列數必須與第二矩陣的行數相同。如果兩矩陣是方陣則無上述要求。由于要以不同順序將許多矩陣乘在一起得到機器人運動方程，因此，應采用方陣進行計算。為保證所表示的矩陣為方陣，如果在同一矩陣中既表示姿態又表示位置，那么可在矩陣中加入比例因子使之成為4x4矩陣。如果只表示姿態，則可去掉比例因子得到3x3矩陣，或加入第四列全為0的位置數據以保持矩陣為方陣。這種形式的矩陣稱為齊次矩陣，它們寫為：nynz0oyoz0ayaz0pypz12.12)2.5變換的表示變換定義為空間的一個運動。當空間的一個坐標系（一個向量、一個物體或一個運動坐標系）相對于固定參考坐標系運動時，

18、這一運動可以用類似于表示坐標系的方式來表示。這是因為變換本身就是坐標系狀態的變化（表示坐標系位姿的變化），因此變換可以用坐標系來表示。變換可為如下幾種形式中的一種：純平移繞一個軸的純旋轉平移與旋轉的結合為了解它們的表示方法，我們將逐一進行探討。2.5.1純平移變換的表示如果一坐標系（它也可能表示一個物體）在空間以不變的姿態運動，那么該坐標就是純dxdydz1平移。在這種情況下，它的方向單位向量保持同一方向不變。所有的改變只是坐標系原點相對于參考坐標系的變化，如圖2.9所示。相對于固定參考坐標系的新的坐標系的位置可以用原來坐標系的原點位置向量加上表示位移的向量求得。若用矩陣形式，新坐標系的表示可

19、以通過坐標系左乘變換矩陣得到。由于在純平移中方向向量不改變，變換矩陣T可以簡單地表示為：1002.13)010T=001000圖2.9空間純平移變換的表示100d-noaPnoaP+dxxxxxxxxxx010dnoapnoap+dF=yxyyyyyyyyynew001dnoaPnoaP+dzzzzzzzzzz000100010001其中d,d,d是純平移向量d相對于參考坐標系x,y,z軸的三個分量。可以看到，矩陣的前三xyz列表示沒有旋轉運動（等同于單位陣），而最后一列表示平移運動。新的坐標系位置為：(2.14)(2.15)這個方程也可用符號寫為:F=Trans(d,d,d)xFnewxyz

20、old首先，如前面所看到的，新坐標系的位置可通過在坐標系矩陣前面左乘變換矩陣得到，后面將看到，無論以何種形式，這種方法對于所有的變換都成立。其次可以注意到，方向向量經過純平移后保持不變。但是，新的坐標系的位置是d和P向量相加的結果。最后應該注意到,齊次變換矩陣與矩陣乘法的關系使得到的新矩陣的維數和變換前相同。例2.4坐標系F沿參考坐標系的x軸移動9個單位，沿z軸移動5個單位。求新的坐標系位置。0.5270.3690.5740.8190.6280.43953F0.76600.64380001解：由式（2.14）或式（2.15）得:7newTrans(d,d,xyd)xFzold=Trans(9,

21、0,5)xF=old1009-0.5270.5270.62850100 x0.3690.8190.439300150.76600.6438000100010.5270.5740.62814-0.3690.8190.43930.76600.643130001F2.5.2繞軸純旋轉變換的表示為簡化繞軸旋轉的推導，首先假設該坐標系位于參考坐標系的原點并且與之平行，之后將結果推廣到其他的旋轉以及旋轉的組合。假設坐標系（n,o,a）位于參考坐標系（x,y,z）的原點，坐標系（n,o,a）繞參考坐標系的x軸旋轉一個角度0，再假設旋轉坐標系（n,o,a）上有一點P相對于參考坐標系的坐標為P,P,P，相對于運

22、動坐標系的坐標為P,P,P。當坐標系繞x軸旋轉時，坐標系上的點P也xyznoa隨坐標系一起旋轉。在旋轉之前，P點在兩個坐標系中的坐標是相同的（這時兩個坐標系位置相同，并且相互平行）。旋轉后，該點坐標P,P,P在旋轉坐標系（x,y,z）中保持不變，但noa在參考坐標系中P,P,P卻改變了（如圖2.10所示）。現在要求找到運動坐標系旋轉后P相對xyz于固定參考坐標系的新坐標。讓我們從x軸來觀察在二維平面上的同一點的坐標，圖2.10顯示了點P在坐標系旋轉前后的坐標。點P相對于參考坐標系的坐標是P,P,P，而相對于旋轉坐標系（點P所固連的坐xyz標系）的坐標仍為P,P,P。noa而P和P卻改變了，可以

23、證明:yz(2.16)由圖2.11可以看出，P不隨坐標系統x軸的轉動而改變,xP=PTOC o 1-5 h zxnP=l-1=Pcos0-Psin0y120aP=1+1=Psin0+Pcos0z340a寫成矩陣形式為：(2.17)圖2.10在坐標系旋轉前后的點的坐標圖2.11相對于參考坐標系的點的坐標和從x軸上觀察旋轉坐標系可見，為了得到在參考坐標系中的坐標，旋轉坐標系中的點P（或向量P）的坐標必須左乘旋轉矩陣。這個旋轉矩陣只適用于繞參考坐標系的x軸做純旋轉變換的情況，它可表示為：P二Rot（x,9）xP（2.18）xyznoa注意在式（2.17）中，旋轉矩陣的第一列表示相對于x軸的位置，其值

24、為1，0，0,它表示沿x軸的坐標沒有改變。為簡化書寫，習慣用符號C9表示cos9以及用S9表示sin9。因此，旋轉矩陣也可寫為：100Rot(x,9)=0C9-S9(2.19)0S9C9可用同樣的方法來分析坐標系繞參考坐標系y軸和z軸旋轉的情況，可以證明其結果為:100-100-Rot(x,9)=0C9-S9和Rot(x,9)=0C9-S9(2.20)0S9C90S9C9式（2.18）也可寫為習慣的形式，以便于理解不同坐標系間的關系，為此，可將該變換表示為uTr（讀作坐標系R相對于坐標系U（Universe）的變換），將Pno表示為RP（P相對于坐標系R）,將Pyz表示為uP（P相對于坐標系U

25、）,式（2.18）可簡化為：uP=uTxrP（2.21）R由上式可見，去掉R便得到了P相對于坐標系U的坐標。例2.5旋轉坐標系中有一點P（234片，此坐標系繞參考坐標系x軸旋轉90度。求旋轉后該點相對于參考坐標系的坐標，并且用圖解法檢驗結果。解：由于點P固連在旋轉坐標系中，因此點P相對于旋轉坐標系的坐標在旋轉前后保持不變。該點相對于參考坐標系的坐標為：_P_xP=yPz如圖2.12所示，根據前面的變換,100_P100_2-2_0cos0-sin0nP=00-13=-40sin0cos0oPa01043得到旋轉后P點相對于參考坐標系的坐標為2,-4,3.圖2.12相對于參考坐標系的坐標系旋轉2

26、.5.3復合變換的表示復合變換是由固定參考坐標系或當前運動坐標系的一系列沿軸平移和繞軸旋轉變換所組成的。任何變換都可以分解為按一定順序的一組平移和旋轉變換。例如，為了完成所要求的變換，可以先繞x軸旋轉，再沿x,y,z軸平移，最后繞y軸旋轉。在后面將會看到，這個變換順序很重要，如果顛倒兩個依次變換的順序，結果將會完全不同為了探討如何處理復合變換，假定坐標系(n,0,a)相對于參考坐標系(x,y,z)依次進行了下面三個變換：繞x軸旋轉a度；接著平移11/2/3(分別相對于x,y,z軸);(3)最后繞y軸旋轉0度。比如點P固定在旋轉坐標系，開始時旋轉坐標系的原點與參考坐標系的原點重合。隨著noa坐標

27、系(n,o,a)相對于參考坐標系旋轉或者平移時，坐標系中的P點相對于參考坐標系也跟著改變。如前面所看到的，第一次變換后，P點相對于參考坐標系的坐標可用下列方程進行計算。(2.22)P=Rot(x,a)xP1,xyznoa其中，P是第一次變換后該點相對于參考坐標系的坐標。第二次變換后，該點相對于xyz參考坐標系的坐標是：P=Trans(11,12,/3)xP=Trans(11,12,/3)xRot(x,a)xPxyz1,xyznoa同樣，第三次變換后，該點相對于參考坐標系的坐標為：Pxyznoa=P=Rot(y,0)xP=Rot(y,0)xTrans(11,12,13)xRot(x,a)xPxy

28、z2,xyz可見，每次變換后該點相對于參考坐標系的坐標都是通過用每個變換矩陣左乘該點的坐標得到的。當然，矩陣的順序不能改變。同時還應注意，對于相對于參考坐標系的每次變換,矩陣都是左乘的。因此，矩陣書寫的順序和進行變換的順序正好相反。例2.6固連在坐標系（n,o,a）上的點p（732）T經歷如下變換，求出變換后該點相對于參考坐標系的坐標。繞z軸旋轉90度；接著繞y軸旋轉90度；（3）接著再平移4,-3,7。解：表示該變換的矩陣方程為：P=Trans（4,-3,7）Rot（y,90）P=xyznoa1004-0010-0-10076010-3X0100X1000X340017-1000001021

29、000010001000111從圖2.13可以看到，（n,o,a）坐標系首先繞z軸旋轉90度，接著繞y軸旋轉，最后相對于參考坐標系的x,y,z軸平移。坐標系中的p點相對于n,o,a軸的位置如圖所示，最后該點在x,y,z軸上的坐標分別為4+2=6，-3+7=4，7+3=10。請確認也能從圖中理解上述結果。圖2.13三次順序變換的結果例2.7根據上例，假定（n,o,a）坐標系上的點P（732）t經歷相同變換，但變換按如下不同順序進行，求出變換后該點相對于參考坐標系的坐標。（1）繞z軸旋轉90度；接著平移4,-3,7；接著再繞y軸旋轉90度。解：表示該變換的矩陣方程為：P=Rot(y,90)Tran

30、s(4,-3,7)Rot(z,90)P=_0010_1004-0-100790100010-3100034XXX=-1000001700102-100010001000111xyznoa不難發現，盡管所有的變換與例2.6完全相同，但由于變換的順序變了，該點最終坐標與前例完全不同。用圖2.14可以清楚地說明這點，這時可以看出，盡管第一次變換后坐標系的變化與前例完全相同，但第二次變換后結果就完全不同，這是由于相對于參考坐標系軸的平移使得旋轉坐標系C,o,a)向外移動了。經第三次變換，該坐標系將繞參考坐標系y軸旋轉,因此向下旋轉了，坐標系上點P的位置也顯示在圖中。請證明該點相對于參考坐標系的坐標為7

31、+2=9,-3+7=4和-4+3=-1,它與解析的結果相同。第段變撫后圖2.14變換的順序改變將改變最終結果2.5.4相對于旋轉坐標系的變換到目前為止，本書所討論的所有變換都是相對于固定參考坐標系。也就是說，所有平移、旋轉和距離(除了相對于運動坐標系的點的位置)都是相對參考坐標系軸來測量的。然而事實上，也有可能做相對于運動坐標系或當前坐標系的軸的變換。例如，可以相對于運動坐標系(也就是當前坐標系)的n軸而不是參考坐標系的x軸旋轉90度。為計算當前坐標系中的點的坐標相對于參考坐標系的變化，這時需要右乘變換矩陣而不是左乘。由于運動坐標系中的點或物體的位置總是相對于運動坐標系測量的，所以總是右乘描述

32、該點或物體的位置矩陣例2.8假設與例2.7中相同的點現在進行相同的變換，但所有變換都是相對于當前的運動坐標系，具體變換出如下。求出變換完成后該點相對于參考坐標系的坐標。(1)繞a軸旋轉90度;(2)然后沿n,o,a軸平移4,-3,7；(3)接著繞0軸旋轉90度。解./Vrr*在本例中，因為所作變換是相對于當前坐標系的，因此右乘每個變換矩陣，可得表示該坐標的方程為：P=Rot(a,90)Trans(4,-3,7)Rot(o,90)P=xyznoa0-100-1004-0010701000X010-3X0100X3600100017-10002000010001000111如所期望的，結果與其他各

33、例完全不同，不僅因為所作變換是相對于當前坐標系的，而且也因為矩陣順序的改變。下面的圖展示了這一結果，應注意它是怎樣相對于當前坐標來完成這個變換的。同時應注意，在當前坐標系中p點的坐標7,3,2是變換后得到相對于參考坐標系的坐標0,6，0的。y圖2.15相對于當前坐標系的變換例2.9坐標系B繞x軸旋轉90，然后沿當前坐標系a軸做了3英寸的平移，然后在繞z軸旋轉90，最后沿當前坐標系o軸做5英寸的平移。(a)寫出描述該運動的方程。(b)求坐標系中的點p(1,5,4)相對于參考坐標系的最終位置。解：在本例中，相對于參考坐標系以及當前坐標系的運動是交替進行的。相應地左乘或右乘每個運動矩陣，得到：uT=

34、Rot(z,90)Rot(x,90)Trans(0,0,3)Trans(0,5,0)B帶入具體的矩陣并將它們相乘，得到：uP=uTxbP=B0-100_1000_1000_1000_100000-100100010500100100001300100001000100010001TOC o 1-5 h z0017 HYPERLINK l bookmark42 _1001 HYPERLINK l bookmark86 -01010 HYPERLINK l bookmark84 _00012.6變換矩陣的逆正如前面所提到的，在機器人分通過析中有很多地方要用到矩陣的逆，在下面的例子中可以看到一種涉及

35、變換矩陣的情況。在圖2.16中，假設機器人要在零件p上鉆孔而須向零件p處移動。機器人基座相對于參考坐標系u的位置用坐標系R來描述，機器人手用坐標系H來描述，末端執行器(即用來鉆孔的鉆頭的末端)用坐標系E來描述，零件的位置用坐標系P來描述。鉆孔的點的位置于參考坐標系U可以通過兩個獨立的路徑發生聯系：一個是通過該零件的路徑，另一個是通過機器人的路徑。因此，可以寫出下面的方程：(2.24)_uTrTht_uTpTERHEPE這就是說，該零件中點E的位置可以通過從U變換到P,并從P變換到E來完成，或者從U變換到R,從R變換到H,再從H變換到E。事實上，由于在任何情況下機器人的基座位置在安裝時就是已知的

36、，因此變換uT(坐標系RR相對于坐標系U的變換)時已知的。比如，一個機器人安裝在一個工作臺上，由于它被緊固在工作臺上，所以它的基座的位置時已知的。即使機器人時可移的或放在傳送帶上，因為控制器始終掌控著機器人基座的運動，因此它在任意時刻的位置也是已知的。由于用于末端執行器的任何器械都是已知的，而且其尺寸和結構也是已知的，所以HT(機器人末端執行器相E對于機器人手的變換)也是已知的。此外，UT(零件相對于全局坐標系的變換)也是已知P的，還必須要知道將在其上面鉆孔的零件的位置，該位置可以通過將該零件放在鉆模上，讓后用照相機，視覺系統，傳送帶，傳感器或其他類似儀器來確定。最后需要知道零件上鉆孔的位置，

37、所以PT也是已知的。此時，唯一未知的變換就是RT(機器人手相對于機器人基座EH的變換)。因此，必須找出機器人的關節變量(機器人旋轉關節的角度以及滑動關節的連桿長度)，以便將末端執行器定位在要鉆孔的位置上。可見，必須要計算出這個變換，它指出機器人需要完成的工作。后面將用所求出的變換來求解機器人關節的角度和連桿的長度。不能像在代數方程中那樣來計算這個矩陣，即不能簡單的用方程的右邊除以方程的左邊，而應該用合適的矩陣的逆并通過左乘或右乘來將它們從左邊去調。因此有：2.25)(uT)-i(uTrThT)(hT)-i二(uT)-i(uTpT)(hT)-1RRHEERPEE由于(uT)-i(uT)二1和(h

38、T)(hT)-i二1，式(2.25)的左邊可簡化為rT，于是得：2.26)RREEHrT=uT-1uTpTh1-1HRPEE該方程的正確性可以通過認為ETh與(HTe)-1相同來加以檢驗。因此，該方程可重寫為：rT=uT-1uTpThT-1=rTuTpTeT=rT(2.27)HRPEEuPEHH顯然為了對機器人運動學進行分析，需要能夠計算變換矩陣的逆。我們來看看關于x軸的簡單旋轉矩陣的求逆計算情況。關于x軸的旋轉矩陣是：100Rot(x，6)=0C6-S6(2.28)0S6C6必須采用一下的步驟來計算矩陣的逆：計算矩陣的行列式；將矩陣轉置；將轉置矩陣的每個元素用它的子行列式(伴隨矩陣)代替用轉

39、換后的矩陣除以行列式。將上面的步驟用到該旋轉，得到：A二1(C26+S26)+0二1100Rot(x，6)T=0C6S60-S6C6現在計算每一個子行列式(伴隨矩陣)。例如，元素2,2的子行列式是C6-0二C6，元素1，1的子行列式C20+S26=1。可以注意到，這里的每一個元素的子行列式與其本身相同,因此有：Rot(x,0)t=Rot(x,0)tminor由于原旋轉矩陣的行列式為1,因此用Rot(x,0)t矩陣除以行列式仍得出相同的結果。minor因此,關于x軸的旋轉矩陣的逆的行列式與它的轉置矩陣相同,即：Rot(x,0)-i=Rot(x,0)t(2.29)當然，如果采用附錄A中提到的第二種

40、方法也能得到同樣的結果。具有這種特征的矩陣稱為酉矩陣,也就是說所有的旋轉矩陣都是酉矩陣。因此,計算旋轉矩陣的逆就是將該矩陣轉置。可以證明，關于y軸和z軸的旋轉矩陣同樣也是酉矩陣。應注意，只有旋轉矩陣才是酉矩陣。如果一個矩陣不是一個簡單的旋轉矩陣，那么它也許就不是酉矩陣。以上結論只對簡單的不表示位置的3X3旋轉矩陣成立。對一個齊次的4X4變換矩陣而言，它的求逆可以將矩陣分為兩部分。矩陣的旋轉部分仍是酉矩陣，只需簡單的轉置；矩陣的位置部分是向量p分別與n,o,a向量點積的負值，其結果為：noapxxxxnoapF二yyyy（2.30）noapzzzz0001nnn-P-nxyzooo-P-oF=x

41、yzaaa-P-axyz0001如上所示，矩陣的旋轉部分是簡單的轉置，位置的部分由點乘的負值代替，而最后一行(比例因子)則不受影響。這樣做對于計算變換矩陣的逆是很有幫助的，而直接計算4X4矩陣的逆是一個很冗長的過程。例2.10計算表示Rot(x,40。)-1的矩陣解：繞x軸旋轉40的矩陣為:100000.766-0.6430Rot（x,40。）=00.6430.76600001其矩陣的逆是：100000.7660.6430Rot（x,40。）-i=0-0.6430.7660_0001需注意的是，由于矩陣的位置向量為0,它與n,o,a向量的點積也為零。例2.11計算如下變換矩陣的逆。0.500.

42、86630.866052010500010.50.8660001T1=0.8660.50_0000.50.86600010.8660.50000可以證明TT-1是單位陣。解：根據先前的計算，變換矩陣的逆是：一(3x0.5+2x0.866+5x0)一(3x0+2x0+5x1)(3x0.866+2x0.5+5x0)13.23_51.5981例2.12一個具有六個自由度的機器人的第五個連桿上裝有照相機，照相機觀察物體并測定它相對于照相機坐標系的位置，然后根據以下數據來確定末端執行器要到達物體所必須完成的運動。-00130100_010010005T=5T=cam1005H00140001000100

43、12100010020100camT=HT=obj0104E001300010001解：JVrr參照式（2.24），可以寫出一個與它類似的方程，它將不同的變換和坐標系聯系在一起。RTx5TxhTxeT=RTx5TxcamT5HEobj5camobj由于方程兩邊都有RT，所以可以將它消去。除了ET之外所有其他矩陣都是已知的，所5obj以：ET=HT1x5T1x5TxcamT=ETxHTx5TxcamTobjEHcamobjH5camobj1000-1100_0100-1000hT-1=5T-1=E001-3H001-400010001將矩陣及矩陣的逆代入前面的方程，得：100000100-eT=

44、ob100-20101eT=obj00-1-40001100_00-13_0012_0000-100100201-4-10050104001000100012.7機器人的正逆運動學假設有一個構型已知的機器人即它的所有連桿長度和關節角度都是已知的，那么計算機器人手的位姿就稱為正運動學分析。換言之，如果已知所有機器人關節變量，用正運動學方程就能計算任一瞬間機器人的位姿。然而，如果想要將機器人的手放在一個期望的位姿，就必須知道機器人的每一個連桿的長度和關節的角度，才能將手定位在所期望的位姿，這就叫做逆運動學分析，也就是說，這里不是把已知的機器人變量代入正向運動學方程中，而是

45、要設法找到這些方程的逆，從而求得所需的關節變量，使機器人放置在期望的位姿。事實上逆運動學方程更為重要，機器人的控制器將用這些方程來計算關節值，并以此來運行機器人到達期望的位姿。下面首先推導機器人的正運動學方程，然后利用這些方程來計算逆運動學方程。對正運動學，必須推導出一組與機器人特定構型（將構件組合在一起構成機器人的方法）有關的方程，以使得將有關的關節和連桿變量代入這些方程就能計算出機器人的位姿，然后可用這些方程推出逆運動學方程。根據第一章中的相關內容，要確定一個剛體在空間的位姿，須在物體上固連一個坐標系，然后描述該坐標系的原點位置和它三個軸的姿態，總共需要六個自由度或六條信息來完整地定義該物

46、體的位姿。同理，如果要確定或找到機器人手在空間的位姿，也必須在機器人手上固連一個坐標系并確定機器人手坐標系的位姿，這正是機器人正運動學方程所要完成的任務。換言之，根據機器人連桿和關節的構型配置，可用一組特定的方程來建立機器人手的坐標系和參考坐標系之間的聯系。圖2.17所示為機器人手的坐標系、參考坐標系以及它們的相對位姿，兩個坐標系之間的關系與機器人的構型有關。當然，機器人可能有許多不同的構型，后面將會看到將如何根據機器人的構型來推導出與這兩個坐標系相關的方程。為使過程簡化，可分別分析位置和姿態問題，首先推導出位置方程，然后再推導出姿態方程，再將兩者結合在一起而形成一組完整的方程。最后，將看到關

47、于Denavit-Hartenberg表示法的應用，該方法可用于對任何機器人構型建模。圖2.17機器人的手坐標系相對于參考坐標系2.7.1位置的正逆運動學方程對于機器人的定位，可以通過相對于任何慣用坐標系的運動來實現。比如，基于直角坐標系對空間的一個點定位，這意味著有三個關于x,y,z軸的線性運動，此外，如果用球坐標來實現，就意味著需要有一個線性運動和兩個旋轉運動。常見的情況有：（a）笛卡爾（臺架，直角）坐標（b）圓柱坐標（c）球坐標（d）鏈式（擬人或全旋轉）坐標2.7.11笛卡爾（臺架，直角）坐標這種情況下有三個沿x,y,z軸的線性運動，這一類型的機器人所有的驅動機構都是線性的（比如液壓活塞

48、或線性動力絲杠），這時機器人手的定位是通過三個線性關節分別沿三個軸的運動來完成的（如圖2.18所示）。臺架式機器人基本上就是一個直角坐標機器人，只不過是個臺架式直角坐標機器人。當然，如果沒有旋轉運動，表示向P點運動的變換矩陣是一種簡單的平移變換矩陣，下面將可以看到這一點。注意這里只涉及坐標系原點的定位，而不涉及姿態。在直角坐標系中,表示機器人手位置的正運動學變換矩陣為：Pcart100010001000PxPyPz12.31)其中RT是參考坐標系與手坐標系原點P的變換矩陣，而T表示直角坐標變換矩陣。對Pcart于逆運動學的求解，只需簡單地設定期望的位置等于P.例2.13要求笛卡爾坐標機器人手坐

49、標系原點定位在點P=347JT，計算所需要的笛卡兒坐標運動。解：設定正運動學方程用式(2.31)中的rT矩陣表示，根據期望的位置可得知如下結果:PP00000PxPyPz11003_010400170001或Px=3,P=4,P=7yz2.7.1.2圓柱坐標圓柱型坐標系統包括兩個線性平移運動和一個旋轉運動。其順序為：先沿x軸移動r，再繞z軸旋轉a角，最后沿z軸移動1，如圖2.19所示。這三個變換建立了手坐標系與參考坐標系之間的聯系。由于這些變換都是相對于全局參考坐標系的坐標軸的，因此由這三個變換所產生的總變換可以通過依次左乘每一個矩陣而求得：2.32)rT=T(r,a,l)=Trans(0,0

50、,l)Rot(z,a)Trans(r,0,0)Pcyl10000100P=001l0001CaSa00-100rSaCa00010000100010000100012.33)PcylCaSa0Sa0rCaCa0rSa01l001經過一系列變換后，前三列表示了坐標系的姿態，然而我們只對坐標系的原點位置即最后一列感興趣。顯然，在圓柱型坐標運動中，由于繞z軸旋轉了a角，運動坐標系的姿態也將改變，這一改變將在后面討論。實際上，可以通過繞n,o,a坐標系中的a軸旋轉-a角度使坐標系回轉到和初始參考坐標系平行的狀態，它等效于圓柱坐標矩陣右乘旋轉矩陣(a,a)，其結果是，該坐標系的位置仍在同一地方，但其姿態

51、再次平行于參考坐標系，如下所示：CaTxRot(z,-a)=cylSa0一SaCa00rCaC(-a)-S(-a)rSalS(-a)x0C(-a)0100rCa010rSa01L0001由此可見，運動坐標系的原點位置沒有改變，但它轉回到了與參考坐標系平行的狀態。需注意的是，最后的旋轉是繞本地坐標系的a軸的，其目的是為了不引起坐標系位置的任何改變,而只改變姿態。例2.14假設要將圓柱坐標機器人手坐標系的原點放在b47JT，計算該機器人的關節變量。解：根據式（2.33）的T，矩陣，將手坐標系原點的位置分量設置為期望值，可以得到：cyll=7rCa=3rSa=44于是有：tana=和么=53.1。3

52、將a代入其中任何一個方程，可得r=5，最終結果是：r=5,a=53.1,l=7應注意：必須確保在機器人運動學中計算的角度位于正確的象限。在這個例子中，請注意rCa和rSa都是正的，并且r也是正的，這樣角度a便在第一象限，且為53.1。2.7.1.3球坐標如圖2.20所示，球坐標系統由一個線性運動和兩個旋轉運動組成，運動順序為：先沿z軸平移r,再繞y軸旋轉0并繞z軸旋轉丫。這三個變換建立了手坐標系與參考坐標系之間的聯系。由于這些變換都是相對于全局參考坐標系的坐標軸的，因此有這三個變換所產生的總變換可以通過一次左乘每一個矩陣而求得：rT=TC,0,丫）=RotQ,Y）Rot（y,0）Trans（0

53、,0,丫）（2.34）PsphCy-Sy00-Cp0Sp01000_SyCy00010001000010-Sp0Cp0001r000100010001rTprTcpsy-SYcyspcSpSyrSpCrSpSy-SpCpCp0sphP0001圖2.20球坐標(2.35)前三列表是了經過一系列變換后的坐標系的姿態，而最后一列則表示了坐標系原點的位置。以后還要進一步討論該矩陣的姿態部分。這里也可回轉最后一個坐標系，使它與參考坐標系平行。這一問題將作為練習留給讀者,要求找出正確的運動順序來獲得正確的答案。球坐標的逆運動學方程比簡單的直角坐標和圓柱坐標更復雜，因為兩個角度p和y是耦合的。讓我們通過下例

54、來說明如何求解球坐標的逆運動學方程。例2.15假設要將球坐標機器人手坐標系原點放在b,4,7丄，計算機器人的關節變量。解:根據式（2.35）的T矩陣，將手坐標系原點的位置分量設置為期望值，可以得到:sphrSpCy=3rSpSy=4rCp=7由第三個方程，我們得出cp是正數，但沒有關于Sp是正或負的信息。將前兩個方程彼此相除，因為不知道Sp的實際符號是什么，因此可能會有兩個解。下面的方法給出了兩個可能的解，后面還必須對這最后的結果進行檢驗以確保它們是正確的。Y=53.1。或233.1。S丫=0.8或-0.8C丫=0.6或-0.63ySP=50.6或-5yCP=7P=35.5。或-35.5。Y=

55、8.6可以對這兩組解進行檢驗并正是這兩組解都能滿足所有的位置方程。如果沿給定的三維坐標軸旋轉這些角度，物理上的確能到達同一點。然而必須注意，其中只有一組解能滿足姿態方程。換句話說，前兩種解將產生同樣的位置，但處于不同的姿態。由于目前并不關心手坐標系在這點的姿態，因此兩個位置解都是正確的。實際上，由于不能對三自由度的機器人指定姿態，所以無法確定兩個解中哪一個和特定的姿態有關。2.7.1.4鏈式坐標如圖2.21所示，鏈式坐標由三個旋轉組成。后面在討論Denavit-Hartenberg表示法時，將推導鏈式坐標的矩陣表示法。圖2.21鏈式坐標2.7.2姿態的正逆運動學方程假設固連在機器人手上的運動坐

56、標系已經運動到期望的位置上，但它仍然平行于參考坐標系，或者假設其姿態并不是所期望的，下一步是要在不改變位置的情況下，適當地旋轉坐標系而使其達到所期望的姿態。合適的旋轉順序取決于機器人手腕的設計以及關節裝配在一起的方式。考慮以下三中常見的構型配置：滾動角、俯仰角、偏航(RPY)角歐拉角鏈式關節2.7.2.1滾動角、俯仰角和偏航角這是分別繞當前a,o,n軸的三個旋轉順序，能夠把機器人的手調整到所期望的姿態。此時,假定當前的坐標系平行于參考坐標系，于是機器人手的姿態在RPY(滾動角、俯仰角、偏航角）運動前與參考坐標系相同。如果當前坐標系不平行于參考坐標系，那么機器人手最終的姿態將會是先前的姿態與RP

57、Y右乘的結果。因為不希望運動坐標系原點的位置有任何改變（它已被放在一個期望的位置上，所以只需要旋轉她到所期望的姿態），所以RPY的旋轉運動都是相對于當前的運動軸的。否則。如前面所看到的，運動坐標系的位置將會改變。于是，右乘所有由RPY和其他旋轉所產生的與參考圖2.22，可看到RPY旋轉包括以下幾種：繞a軸（運動坐標系的z軸）旋轉叫滾動；a繞o軸（運動坐標系的y軸）旋轉叫俯仰；o繞n軸（運動坐標系的x軸）旋轉叫偏航。n表示RPY姿態變化的矩陣為：RPY（,）=Rot（a,）Rot（o,）Rot（n,）=aonaonCCaoSCao-So0CSS-SCaonanSSS+CCaonanCSon0 H

58、YPERLINK l bookmark227 CSC+SS0aonan HYPERLINK l bookmark229 SSC-CS0aonan HYPERLINK l bookmark231 CC0on HYPERLINK l bookmark28 01(2.36)這個矩陣表示了僅由RPY引起的姿態變化。該坐標系相對于參考坐標系的位置和最終姿態是表示位置變化和RPY的兩個矩陣的乘積。例如，假設一個機器人是根據球坐標和RPY來設計的，那么這個機器人就可以表示為：rT=T（r,卩,y）xRPyG,）Hsphaon關于RPY的你運動學方程的解比球坐標更復雜，因為這里有三個耦合角，所以需要所有三個角

59、各自的正弦和余弦值的信息才能解出這個角。為解出這三個角的正弦值和余弦值，必須將這些角解耦。因此，用Rot（a,0）的逆左乘方程兩邊，得：aRot（a,0LRPyG,0,0）=Rot（o,0）Rot（n,0）aaonon假設用rpy得到的最后所期望的姿態是用（n,o,a）矩陣來表示的，則有:noa0 xxxnoa0yyynoa0zzz0001Rot(a,0)-1a=Rot(o,0)Rot(n,0)on2.37)2.38)CSSSC0oonon0C-S0-SnCSnCC0oonon00012.39)進行矩陣相乘后得：nC+nSoC+oSaC+aS0 xayaxayaxayanC-nSoC-oSaC

60、-aS0yaxayaxayaxanoa0zzz0001在式（2.38）中的n,o,a分量表示了最終的期望值，它們通常是給定或已知的，而RPY角的值是未知的變量。讓式（2.39）左右兩邊對應的元素相等，將產生如下結果（關于ATAN2函數的解釋請參考附錄A）：根據2，1元素得：根據3，2.40)2.41)2.42)nC0-nS0=0t0=ATAN2(n,n)0=ATAN2n，-n)yaxaayxayx1元素和1，1元素得：S0=-n(C0=nC0+nS0t0=ATAN2-n,hC0+nS0oxayaozxaya根據2，2元素和2，3元素得：C0=oC0-oS0nyaxaS0=-aC0+aS0t0=