機器人運動學

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

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

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

4、不斷測量所有關節(jié)和連桿的參數(shù)，或者監(jiān)控系統(tǒng)的末端，以便知道機器的運動位置。通過比較如下的兩個連桿機構(gòu)的向量方程，可以表示出這種差別，該向量方程表示了不同連桿之間的關系。（2.1）（2.2）可見，如果連桿AB偏移，連桿也會相應地移動，式（2.1）的兩邊隨連桿的變化而改變。而另一方面，如果機器人的連桿AB偏移，所有的后續(xù)連桿也會移動，除非有其他方法測量，否則這種變化是未知的。為了彌補開環(huán)機器人的這一缺陷，機器人手的位置可由類似攝像機的裝置來進行不斷測量，于是機器人需借助外部手段（比如輔助手臂或激光束）來構(gòu)成閉環(huán)系統(tǒng)?；蛘甙凑粘Ｒ?guī)做法，也可通過增加機器人連桿和關節(jié)強度來減少偏移，采用這種方法將導致機

5、器人重量重、體積大、動作慢，而且它的額定負載與實際負載相比非常小。圖2.2 (a)閉環(huán)機構(gòu)；(b)開環(huán)機構(gòu)2.3 機器人運動學的矩陣表示 矩陣可用來表示點、向量、坐標系、平移、旋轉(zhuǎn)以及變換，還可以表示坐標系中的物體和其他運動元件。2.3.1 空間點的表示空間點P（如圖2.3所示）可以用它的相對于參考坐標系的三個坐標來表示： (2.3)其中， 是參考坐標系中表示該點的坐標。顯然，也可以用其他坐標來表示空間點的位置。圖2.3 空間點的表示2.3.2 空間向量的表示 向量可以由三個起始和終止的坐標來表示。如果一個向量起始于點A，終止于點B，那么它可以表示為。特殊情況下，如果一個向量起始于原點（如圖2

6、.4所示），則有：（2.4）其中是該向量在參考坐標系中的三個分量。實際上，前一節(jié)的點P就是用連接到該點的向量來表示的，具體地說，也就是用該向量的三個坐標來表示。圖2.4 空間向量的表示向量的三個分量也可以寫成矩陣的形式，如式（2.5）所示。在本書中將用這種形式來表示運動分量：（2.5）這種表示法也可以稍做變化：加入一個比例因子w,如果x, y, z各除以w,則得到。于是，這時向量可以寫為：，其中等等 （2.6）變量w可以為任意數(shù)，而且隨著它的變化，向量的大小也會發(fā)生變化，這與在計算機圖形學中縮放一張圖片十分類似。隨著w值的改變，向量的大小也相應地變化。如果w大于1，向量的所有分量都變大；如果w

7、小于1，向量的所有分量都變小。這種方法也用于計算機圖形學中改變圖形與畫片的大小。 如果w是1，各分量的大小保持不變。但是，如果w=0，則為無窮大。在這種情況下，x，y和z（以及）表示一個長度為無窮大的向量，它的方向即為該向量所表示的方向。這就意味著方向向量可以由比例因子w=0的向量來表示，這里向量的長度并不重要，而其方向由該向量的三個分量來表示。例2.1 有一個向量P=3i+5j+2k,按如下要求將其表示成矩陣形式：（1）比例因子為2（2）將它表示為方向的單位向量解：該向量可以表示為比例因子為2的矩陣形式，當比例因子為0時，則可以表示為方向向量，結(jié)果如下： 和 然而，為了將方向向量變?yōu)閱挝幌蛄?/p>

8、，須將該向量歸一化使之長度等于1。這樣，向量的每一個分量都要除以三個分量平方和的開方：，等等和2.3.3 坐標系在固定參考坐標系原點的表示 一個中心位于參考坐標系原點的坐標系由三個向量表示，通常著三個向量相互垂直，稱為單位向量，分別表示法線（normal）、指向（orientation）和接近（approach）向量（如圖2.5所示）。正如2.3.3節(jié)所述，每一個單位向量都由它們所在參考坐標系著的三個分量表示。這樣，坐標系F可以由三個向量以矩陣的形式表示為：（2.7）圖2.5 坐標系在參考坐標系原點的表示2.3.4 坐標系在固定參考坐標系中的表示如果一個坐標系不再固定參考坐標系的原點（實際上也

9、可包括在原點的情況），那么該坐標系的原點相對于參考坐標系的位置也必須表示出來。為此，在該坐標系原點與參考坐標系原點之間做一個向量來表示該坐標系的位置（如圖2.6所示）。這個向量由相對于參考坐標系的三個向量來表示。這樣，這個坐標系就可以由三個表示方向的單位向量以及第四個位置向量來表示。（2.8）圖2.6 一個坐標系在另一個坐標系中的表示如式（2.8）所示，前三個向量是w=0的方向向量，表示該坐標系的三個單位向量的方向，而第四個w=1的向量表示該坐標系原點相對于參考坐標系的位置。與單位向量不同，向量P的長度十分重要，因而使用比例因子為1。坐標系也可以由一個沒有比例因子的34矩陣表示，但不常用。例2

10、.2 如圖2.7所示的F坐標系位于參考坐標系中3，5，7的位置，它的n軸與x軸平行，o軸相對于y軸的角度為45°，a軸相對于z軸的角度為45°。該坐標系可以表示為：圖2.7 坐標系在空間的表示舉例2.3.5 剛體的表示一個物體在空間的表示可以這樣實現(xiàn)：通過在它上面固連一個坐標系，再將該固連的坐標系在空間表示出來。由于這個坐標系一直固連在該物體上，所以該物體相對于坐標系的位姿是已知的。因此，只要這個坐標系可以在空間表示出來，那么這個物體相對于固定坐標系的位姿也就已知了（如圖2.8所示）。如前所述，空間坐標系可以用矩陣表示，其中坐標原點以及相對于參考坐標系的表示該坐標系姿態(tài)的三

11、個向量也可以由該矩陣表示出來。于是有： （2.9）如第1章所述，空間中的一個點只有三個自由度，它只能沿三條參考坐標軸移動。但在空間的一個鋼體有六個自由度，也就是說，它不僅可以沿著X,Y,Z三軸移動，而且還可繞這三個軸轉(zhuǎn)動。因此，要全面地定義空間以物體，需要用6條獨立的信息來描述物體原點在參考坐標系中相對于三個參考坐標軸的位置，以及物體關于這三個坐標軸的姿態(tài)。而式（2.9）給出了12條信息，其中9條為姿態(tài)信息，三條為位置信息（排除矩陣中最后一行的比例因子，因為它們沒有附加信息）。顯然，在該表達式中必定存在一定的約束條件將上述信息數(shù)限制為6。因此，需要用6個約束方程將12條信息減少到6條信息。這些

12、約束條件來自于目前尚未利用的已知的坐標系特性，即：三個向量相互垂直每個單位向量的長度必須為1圖2.8 空間物體的表示我們可以將其轉(zhuǎn)換為以下六個約束方程：（1）（2）（3）（4）（向量的長度必須為1） （2.10）（5）（6）因此，只有前述方程成立時，坐標系的值才能用矩陣表示。否則，坐標系將不正確。式（2.10）中前三個方程可以換用如下的三個向量的叉積來代替： （2.11）例2.3對于下列坐標系，求解所缺元素的值，并用矩陣來表示這個坐標系。解：顯然，表示坐標系原點位置的值5，3，2對約束方程無影響。注意在三個方向向量中只有三個值是給定的，但這也已足夠了。根據(jù)式（2.10），得： 或 或 或 或

13、或 或 將這些方程化簡得：解這六個方程得：。應注意，必須同號。非唯一解的原因是由于給出的參數(shù)可能得到兩組在相反方向上相互垂直的向量。最終得到的矩陣如下： 或 由此可見，兩個矩陣都滿足約束方程的要求。但應注意三個方向向量所表述的值不是任意的，而是受這些約束方程的約束，因此不可任意給矩陣賦值。同樣，可通過叉乘并令其等于，即來求解，表示如下：或表示為： 將值代入方程得： 同時解下面這三個方程：以此來代替三個點乘方程。聯(lián)立三個單位向量長度的約束方程，便得到六個方程，求解這六個方程，可得到相同的未知參數(shù)的解。2.4 齊次變換矩陣由于各種原因，變換矩陣應寫成方型形式，33或44均可。首先，如后面看到的，計

14、算方形矩陣的逆要比計算長方形矩陣的逆容易得多。其次，為使兩矩陣相乘，它們的維數(shù)必須匹配，即第一矩陣的列數(shù)必須與第二矩陣的行數(shù)相同。如果兩矩陣是方陣則無上述要求。由于要以不同順序?qū)⒃S多矩陣乘在一起得到機器人運動方程，因此，應采用方陣進行計算。為保證所表示的矩陣為方陣，如果在同一矩陣中既表示姿態(tài)又表示位置，那么可在矩陣中加入比例因子使之成為44矩陣。如果只表示姿態(tài)，則可去掉比例因子得到33矩陣，或加入第四列全為0的位置數(shù)據(jù)以保持矩陣為方陣。這種形式的矩陣稱為齊次矩陣，它們寫為： （2.12） 2.5 變換的表示變換定義為空間的一個運動。當空間的一個坐標系（一個向量、一個物體或一個運動坐標系）相對于

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

16、標系的表示可以通過坐標系左乘變換矩陣得到。由于在純平移中方向向量不改變，變換矩陣T可以簡單地表示為： （2.13） 圖2.9 空間純平移變換的表示其中是純平移向量相對于參考坐標系軸的三個分量?？梢钥吹?，矩陣的前三列表示沒有旋轉(zhuǎn)運動（等同于單位陣），而最后一列表示平移運動。新的坐標系位置為： （2.14） 這個方程也可用符號寫為： （2.15） 首先，如前面所看到的，新坐標系的位置可通過在坐標系矩陣前面左乘變換矩陣得到，后面將看到，無論以何種形式，這種方法對于所有的變換都成立。其次可以注意到，方向向量經(jīng)過純平移后保持不變。但是，新的坐標系的位置是和向量相加的結(jié)果。最后應該注意到，齊次變換矩陣與矩

17、陣乘法的關系使得到的新矩陣的維數(shù)和變換前相同。例2.4 坐標系F沿參考坐標系的x軸移動9個單位，沿z軸移動5個單位。求新的坐標系位置。解：由式（2.14）或式（2.15）得：= 和2.5.2 繞軸純旋轉(zhuǎn)變換的表示為簡化繞軸旋轉(zhuǎn)的推導，首先假設該坐標系位于參考坐標系的原點并且與之平行，之后將結(jié)果推廣到其他的旋轉(zhuǎn)以及旋轉(zhuǎn)的組合。假設坐標系（）位于參考坐標系（）的原點，坐標系（）繞參考坐標系的x軸旋轉(zhuǎn)一個角度，再假設旋轉(zhuǎn)坐標系（）上有一點P相對于參考坐標系的坐標為，相對于運動坐標系的坐標為。當坐標系繞x軸旋轉(zhuǎn)時，坐標系上的點P也隨坐標系一起旋轉(zhuǎn)。在旋轉(zhuǎn)之前，P點在兩個坐標系中的坐標是相同的（這時兩個

18、坐標系位置相同，并且相互平行）。旋轉(zhuǎn)后，該點坐標在旋轉(zhuǎn)坐標系（x ,y ,z）中保持不變，但在參考坐標系中卻改變了（如圖2.10所示）?，F(xiàn)在要求找到運動坐標系旋轉(zhuǎn)后P相對于固定參考坐標系的新坐標。 讓我們從x軸來觀察在二維平面上的同一點的坐標，圖2.10顯示了點P在坐標系旋轉(zhuǎn)前后的坐標。點P相對于參考坐標系的坐標是，而相對于旋轉(zhuǎn)坐標系（點P所固連的坐標系）的坐標仍為。由圖2.11可以看出，不隨坐標系統(tǒng)x軸的轉(zhuǎn)動而改變，而和卻改變了，可以證明： （2.16）寫成矩陣形式為：（2.17） 圖2.10 在坐標系旋轉(zhuǎn)前后的點的坐標圖2.11 相對于參考坐標系的點的坐標和從x軸上觀察旋轉(zhuǎn)坐標系可見，為了

19、得到在參考坐標系中的坐標，旋轉(zhuǎn)坐標系中的點P（或向量P）的坐標必須左乘旋轉(zhuǎn)矩陣。這個旋轉(zhuǎn)矩陣只適用于繞參考坐標系的x軸做純旋轉(zhuǎn)變換的情況，它可表示為： （2.18） 注意在式（2.17）中，旋轉(zhuǎn)矩陣的第一列表示相對于x軸的位置，其值為1，0，0，它表示沿x軸的坐標沒有改變。為簡化書寫，習慣用符號表示以及用表示。因此，旋轉(zhuǎn)矩陣也可寫為： （2.19） 可用同樣的方法來分析坐標系繞參考坐標系y軸和z軸旋轉(zhuǎn)的情況，可以證明其結(jié)果為： 和 （2.20）式（2.18）也可寫為習慣的形式，以便于理解不同坐標系間的關系，為此，可將該變換表示為 (讀作坐標系R相對于坐標系U（Universe）的變換)，將表示

20、為（P相對于坐標系R），將表示為（P相對于坐標系U），式（2.18）可簡化為： （2.21） 由上式可見，去掉R便得到了P相對于坐標系U的坐標。例2.5旋轉(zhuǎn)坐標系中有一點P，此坐標系繞參考坐標系x軸旋轉(zhuǎn)90度。求旋轉(zhuǎn)后該點相對于參考坐標系的坐標，并且用圖解法檢驗結(jié)果。解：由于點P固連在旋轉(zhuǎn)坐標系中，因此點P相對于旋轉(zhuǎn)坐標系的坐標在旋轉(zhuǎn)前后保持不變。該點相對于參考坐標系的坐標為：如圖2.12所示，根據(jù)前面的變換，得到旋轉(zhuǎn)后P點相對于參考坐標系的坐標為2，-4，3.圖2.12 相對于參考坐標系的坐標系旋轉(zhuǎn)2.5.3 復合變換的表示復合變換是由固定參考坐標系或當前運動坐標系的一系列沿軸平移和繞軸旋轉(zhuǎn)

21、變換所組成的。任何變換都可以分解為按一定順序的一組平移和旋轉(zhuǎn)變換。例如，為了完成所要求的變換，可以先繞x軸旋轉(zhuǎn)，再沿軸平移，最后繞y軸旋轉(zhuǎn)。在后面將會看到，這個變換順序很重要，如果顛倒兩個依次變換的順序，結(jié)果將會完全不同。 為了探討如何處理復合變換，假定坐標系（）相對于參考坐標系（）依次進行了下面三個變換：（1）繞x軸旋轉(zhuǎn)度；（2）接著平移（分別相對于軸）；（3）最后繞y軸旋轉(zhuǎn)度。比如點固定在旋轉(zhuǎn)坐標系，開始時旋轉(zhuǎn)坐標系的原點與參考坐標系的原點重合。隨著坐標系（）相對于參考坐標系旋轉(zhuǎn)或者平移時，坐標系中的P點相對于參考坐標系也跟著改變。如前面所看到的，第一次變換后，P點相對于參考坐標系的坐標可

22、用下列方程進行計算。 （2.22） 其中，是第一次變換后該點相對于參考坐標系的坐標。第二次變換后，該點相對于參考坐標系的坐標是： 同樣，第三次變換后，該點相對于參考坐標系的坐標為： 可見，每次變換后該點相對于參考坐標系的坐標都是通過用每個變換矩陣左乘該點的坐標得到的。當然，矩陣的順序不能改變。同時還應注意，對于相對于參考坐標系的每次變換，矩陣都是左乘的。因此，矩陣書寫的順序和進行變換的順序正好相反。例2.6 固連在坐標系（）上的點P經(jīng)歷如下變換，求出變換后該點相對于參考坐標系的坐標。(1)繞z軸旋轉(zhuǎn)90度；(2)接著繞y軸旋轉(zhuǎn)90度；(3)接著再平移4，-3，7。解：表示該變換的矩陣方程為：從

23、圖2.13可以看到，（）坐標系首先繞z軸旋轉(zhuǎn)90度，接著繞y軸旋轉(zhuǎn)，最后相對于參考坐標系的軸平移。坐標系中的P點相對于軸的位置如圖所示，最后該點在軸上的坐標分別為4+2=6，-3+7=4，7+3=10。請確認也能從圖中理解上述結(jié)果。圖2.13 三次順序變換的結(jié)果例2.7根據(jù)上例，假定()坐標系上的點經(jīng)歷相同變換，但變換按如下不同順序進行，求出變換后該點相對于參考坐標系的坐標。（1）繞z軸旋轉(zhuǎn)90度；（2）接著平移4，-3，7；（3）接著再繞y軸旋轉(zhuǎn)90度。解：表示該變換的矩陣方程為：不難發(fā)現(xiàn)，盡管所有的變換與例2.6完全相同，但由于變換的順序變了，該點最終坐標與前例完全不同。用圖2.14可以清

24、楚地說明這點，這時可以看出，盡管第一次變換后坐標系的變化與前例完全相同，但第二次變換后結(jié)果就完全不同，這是由于相對于參考坐標系軸的平移使得旋轉(zhuǎn)坐標系向外移動了。經(jīng)第三次變換，該坐標系將繞參考坐標系y軸旋轉(zhuǎn)，因此向下旋轉(zhuǎn)了，坐標系上點P的位置也顯示在圖中。請證明該點相對于參考坐標系的坐標為7+2=9，-3+7=4和-4+3=-1，它與解析的結(jié)果相同。圖2.14 變換的順序改變將改變最終結(jié)果2.5.4 相對于旋轉(zhuǎn)坐標系的變換到目前為止，本書所討論的所有變換都是相對于固定參考坐標系。也就是說，所有平移、旋轉(zhuǎn)和距離（除了相對于運動坐標系的點的位置）都是相對參考坐標系軸來測量的。然而事實上，也有可能做相

25、對于運動坐標系或當前坐標系的軸的變換。例如，可以相對于運動坐標系（也就是當前坐標系）的n軸而不是參考坐標系的x軸旋轉(zhuǎn)90度。為計算當前坐標系中的點的坐標相對于參考坐標系的變化，這時需要右乘變換矩陣而不是左乘。由于運動坐標系中的點或物體的位置總是相對于運動坐標系測量的，所以總是右乘描述該點或物體的位置矩陣。例2.8 假設與例2.7中相同的點現(xiàn)在進行相同的變換，但所有變換都是相對于當前的運動坐標系，具體變換出如下。求出變換完成后該點相對于參考坐標系的坐標。（1）繞軸旋轉(zhuǎn)90度；（2）然后沿軸平移4，-3，7；（3）接著繞軸旋轉(zhuǎn)90度。解：在本例中，因為所作變換是相對于當前坐標系的，因此右乘每個變換

26、矩陣，可得表示該坐標的方程為：如所期望的，結(jié)果與其他各例完全不同，不僅因為所作變換是相對于當前坐標系的，而且也因為矩陣順序的改變。下面的圖展示了這一結(jié)果，應注意它是怎樣相對于當前坐標來完成這個變換的。同時應注意，在當前坐標系中p點的坐標7，3，2是變換后得到相對于參考坐標系的坐標0，6，0的。圖2.15 相對于當前坐標系的變換例2.9 坐標系B繞x軸旋轉(zhuǎn)90°，然后沿當前坐標系a軸做了3英寸的平移，然后在繞z軸旋轉(zhuǎn)90，最后沿當前坐標系o軸做5英寸的平移。(a)寫出描述該運動的方程。(b)求坐標系中的點p（1，5，4）相對于參考坐標系的最終位置。解：在本例中，相對于參考坐標系以及當前

27、坐標系的運動是交替進行的。(a)相應地左乘或右乘每個運動矩陣，得到：=Rot（z,90)Rot(x,90)Trans(0,0,3)Trans(0,5,0)(b)帶入具體的矩陣并將它們相乘，得到：2.6 變換矩陣的逆 正如前面所提到的，在機器人分通過析中有很多地方要用到矩陣的逆，在下面的例子中可以看到一種涉及變換矩陣的情況。在圖2.16中，假設機器人要在零件p上鉆孔而須向零件p處移動。機器人基座相對于參考坐標系u的位置用坐標系R來描述，機器人手用坐標系H來描述，末端執(zhí)行器（即用來鉆孔的鉆頭的末端）用坐標系E來描述，零件的位置用坐標系P來描述。鉆孔的點的位置于參考坐標系U可以通過兩個獨立的路徑發(fā)生

28、聯(lián)系：一個是通過該零件的路徑，另一個是通過機器人的路徑。因此，可以寫出下面的方程： （2.24） 這就是說，該零件中點E的位置可以通過從U變換到P，并從P變換到E來完成，或者從U變換到R，從R變換到H，再從H變換到E。圖2.16 全局坐標系、機器人坐標系、手坐標系、零件坐標系及末端執(zhí)行器坐標系事實上，由于在任何情況下機器人的基座位置在安裝時就是已知的，因此變換(坐標系R相對于坐標系U的變換)時已知的。比如，一個機器人安裝在一個工作臺上，由于它被緊固在工作臺上，所以它的基座的位置時已知的。即使機器人時可移的或放在傳送帶上，因為控制器始終掌控著機器人基座的運動，因此它在任意時刻的位置也是已知的。由

29、于用于末端執(zhí)行器的任何器械都是已知的，而且其尺寸和結(jié)構(gòu)也是已知的，所以（機器人末端執(zhí)行器相對于機器人手的變換）也是已知的。此外，（零件相對于全局坐標系的變換）也是已知的，還必須要知道將在其上面鉆孔的零件的位置，該位置可以通過將該零件放在鉆模上，讓后用照相機，視覺系統(tǒng)，傳送帶，傳感器或其他類似儀器來確定。最后需要知道零件上鉆孔的位置，所以也是已知的。此時，唯一未知的變換就是（機器人手相對于機器人基座的變換）。因此，必須找出機器人的關節(jié)變量（機器人旋轉(zhuǎn)關節(jié)的角度以及滑動關節(jié)的連桿長度），以便將末端執(zhí)行器定位在要鉆孔的位置上。可見，必須要計算出這個變換，它指出機器人需要完成的工作。后面將用所求出的變

30、換來求解機器人關節(jié)的角度和連桿的長度。不能像在代數(shù)方程中那樣來計算這個矩陣，即不能簡單的用方程的右邊除以方程的左邊，而應該用合適的矩陣的逆并通過左乘或右乘來將它們從左邊去調(diào)。因此有： （2.25）由于和，式（2.25）的左邊可簡化為,于是得： （2.26）該方程的正確性可以通過認為與相同來加以檢驗。因此，該方程可重寫為： （2.27）顯然為了對機器人運動學進行分析，需要能夠計算變換矩陣的逆。我們來看看關于x軸的簡單旋轉(zhuǎn)矩陣的求逆計算情況。關于x軸的旋轉(zhuǎn)矩陣是： （2.28）必須采用一下的步驟來計算矩陣的逆：計算矩陣的行列式；將矩陣轉(zhuǎn)置；將轉(zhuǎn)置矩陣的每個元素用它的子行列式（伴隨矩陣）代替；用轉(zhuǎn)換

31、后的矩陣除以行列式。將上面的步驟用到該旋轉(zhuǎn)，得到：現(xiàn)在計算每一個子行列式（伴隨矩陣）。例如，元素2，2的子行列式是，元素1，1的子行列式?？梢宰⒁獾?，這里的每一個元素的子行列式與其本身相同，因此有：由于原旋轉(zhuǎn)矩陣的行列式為1，因此用矩陣除以行列式仍得出相同的結(jié)果。因此，關于x軸的旋轉(zhuǎn)矩陣的逆的行列式與它的轉(zhuǎn)置矩陣相同，即： （2.29）當然，如果采用附錄A中提到的第二種方法也能得到同樣的結(jié)果。具有這種特征的矩陣稱為酉矩陣，也就是說所有的旋轉(zhuǎn)矩陣都是酉矩陣。因此，計算旋轉(zhuǎn)矩陣的逆就是將該矩陣轉(zhuǎn)置。可以證明，關于y軸和z軸的旋轉(zhuǎn)矩陣同樣也是酉矩陣。應注意，只有旋轉(zhuǎn)矩陣才是酉矩陣。如果一個矩陣不是一

32、個簡單的旋轉(zhuǎn)矩陣，那么它也許就不是酉矩陣。 以上結(jié)論只對簡單的不表示位置的3×3旋轉(zhuǎn)矩陣成立。對一個齊次的4×4變換矩陣而言，它的求逆可以將矩陣分為兩部分。矩陣的旋轉(zhuǎn)部分仍是酉矩陣，只需簡單的轉(zhuǎn)置；矩陣的位置部分是向量分別與向量點積的負值，其結(jié)果為： （2.30）如上所示，矩陣的旋轉(zhuǎn)部分是簡單的轉(zhuǎn)置，位置的部分由點乘的負值代替，而最后一行（比例因子）則不受影響。這樣做對于計算變換矩陣的逆是很有幫助的，而直接計算4×4矩陣的逆是一個很冗長的過程。例2.10計算表示的矩陣解：繞x軸旋轉(zhuǎn)40°的矩陣為：其矩陣的逆是： 需注意的是，由于矩陣的位置向量為0，它與向

33、量的點積也為零。例2.11 計算如下變換矩陣的逆。解：根據(jù)先前的計算，變換矩陣的逆是：可以證明是單位陣。例2.12 一個具有六個自由度的機器人的第五個連桿上裝有照相機，照相機觀察物體并測定它相對于照相機坐標系的位置，然后根據(jù)以下數(shù)據(jù)來確定末端執(zhí)行器要到達物體所必須完成的運動。解：參照式（2.24），可以寫出一個與它類似的方程，它將不同的變換和坐標系聯(lián)系在一起。由于方程兩邊都有，所以可以將它消去。除了之外所有其他矩陣都是已知的，所以：將矩陣及矩陣的逆代入前面的方程，得：2.7 機器人的正逆運動學假設有一個構(gòu)型已知的機器人，即它的所有連桿長度和關節(jié)角度都是已知的，那么計算機器人手的位姿就稱為正運動

34、學分析。換言之，如果已知所有機器人關節(jié)變量，用正運動學方程就能計算任一瞬間機器人的位姿。然而，如果想要將機器人的手放在一個期望的位姿，就必須知道機器人的每一個連桿的長度和關節(jié)的角度，才能將手定位在所期望的位姿，這就叫做逆運動學分析，也就是說，這里不是把已知的機器人變量代入正向運動學方程中，而是要設法找到這些方程的逆，從而求得所需的關節(jié)變量，使機器人放置在期望的位姿。事實上，逆運動學方程更為重要，機器人的控制器將用這些方程來計算關節(jié)值，并以此來運行機器人到達期望的位姿。下面首先推導機器人的正運動學方程，然后利用這些方程來計算逆運動學方程。對正運動學，必須推導出一組與機器人特定構(gòu)型（將構(gòu)件組合在一

35、起構(gòu)成機器人的方法）有關的方程，以使得將有關的關節(jié)和連桿變量代入這些方程就能計算出機器人的位姿，然后可用這些方程推出逆運動學方程。根據(jù)第一章中的相關內(nèi)容，要確定一個剛體在空間的位姿，須在物體上固連一個坐標系，然后描述該坐標系的原點位置和它三個軸的姿態(tài)，總共需要六個自由度或六條信息來完整地定義該物體的位姿。同理，如果要確定或找到機器人手在空間的位姿，也必須在機器人手上固連一個坐標系并確定機器人手坐標系的位姿，這正是機器人正運動學方程所要完成的任務。換言之，根據(jù)機器人連桿和關節(jié)的構(gòu)型配置，可用一組特定的方程來建立機器人手的坐標系和參考坐標系之間的聯(lián)系。圖2.17所示為機器人手的坐標系、參考坐標系以

36、及它們的相對位姿，兩個坐標系之間的關系與機器人的構(gòu)型有關。當然，機器人可能有許多不同的構(gòu)型，后面將會看到將如何根據(jù)機器人的構(gòu)型來推導出與這兩個坐標系相關的方程。為使過程簡化，可分別分析位置和姿態(tài)問題，首先推導出位置方程，然后再推導出姿態(tài)方程，再將兩者結(jié)合在一起而形成一組完整的方程。最后，將看到關于Denavit-Hartenberg表示法的應用，該方法可用于對任何機器人構(gòu)型建模。圖2.17 機器人的手坐標系相對于參考坐標系2.7.1 位置的正逆運動學方程對于機器人的定位，可以通過相對于任何慣用坐標系的運動來實現(xiàn)。比如，基于直角坐標系對空間的一個點定位，這意味著有三個關于軸的線性運動，此外，如果

37、用球坐標來實現(xiàn)，就意味著需要有一個線性運動和兩個旋轉(zhuǎn)運動。常見的情況有：(a)笛卡爾（臺架，直角）坐標(b)圓柱坐標(c)球坐標(d)鏈式(擬人或全旋轉(zhuǎn))坐標2.7.1.1 笛卡爾(臺架，直角)坐標這種情況下有三個沿軸的線性運動，這一類型的機器人所有的驅(qū)動機構(gòu)都是線性的（比如液壓活塞或線性動力絲杠），這時機器人手的定位是通過三個線性關節(jié)分別沿三個軸的運動來完成的（如圖2.18所示）。臺架式機器人基本上就是一個直角坐標機器人，只不過是將機器人固連在一個朝下的直角架上。IBM7565機器人就是一個臺架式直角坐標機器人。圖2.18 直角坐標當然，如果沒有旋轉(zhuǎn)運動，表示向P點運動的變換矩陣是一種簡單的

38、平移變換矩陣，下面將可以看到這一點。注意這里只涉及坐標系原點的定位，而不涉及姿態(tài)。在直角坐標系中，表示機器人手位置的正運動學變換矩陣為： （2.31）其中是參考坐標系與手坐標系原點P的變換矩陣，而表示直角坐標變換矩陣。對于逆運動學的求解，只需簡單地設定期望的位置等于P.例2.13要求笛卡爾坐標機器人手坐標系原點定位在點P=，計算所需要的笛卡兒坐標運動。解：設定正運動學方程用式（2.31）中的矩陣表示，根據(jù)期望的位置可得知如下結(jié)果：2.7.1.2 圓柱坐標圓柱型坐標系統(tǒng)包括兩個線性平移運動和一個旋轉(zhuǎn)運動。其順序為：先沿軸移動,再繞軸旋轉(zhuǎn)角，最后沿軸移動，如圖2.19所示。這三個變換建立了手坐標系

39、與參考坐標系之間的聯(lián)系。由于這些變換都是相對于全局參考坐標系的坐標軸的，因此由這三個變換所產(chǎn)生的總變換可以通過依次左乘每一個矩陣而求得： （2.32） （2.33）經(jīng)過一系列變換后，前三列表示了坐標系的姿態(tài)，然而我們只對坐標系的原點位置即最后一列感興趣。顯然，在圓柱型坐標運動中，由于繞z軸旋轉(zhuǎn)了角，運動坐標系的姿態(tài)也將改變，這一改變將在后面討論。實際上，可以通過繞坐標系中的軸旋轉(zhuǎn)角度使坐標系回轉(zhuǎn)到和初始參考坐標系平行的狀態(tài)，它等效于圓柱坐標矩陣右乘旋轉(zhuǎn)矩陣，其結(jié)果是，該坐標系的位置仍在同一地方，但其姿態(tài)再次平行于參考坐標系，如下所示：圖2.19 圓柱坐標由此可見，運動坐標系的原點位置沒有改變，

40、但它轉(zhuǎn)回到了與參考坐標系平行的狀態(tài)。需注意的是，最后的旋轉(zhuǎn)是繞本地坐標系的軸的，其目的是為了不引起坐標系位置的任何改變，而只改變姿態(tài)。例2.14 假設要將圓柱坐標機器人手坐標系的原點放在,計算該機器人的關節(jié)變量。解：根據(jù)式（2.33）的矩陣，將手坐標系原點的位置分量設置為期望值，可以得到：于是有： 將代入其中任何一個方程，可得，最終結(jié)果是：應注意：必須確保在機器人運動學中計算的角度位于正確的象限。在這個例子中，請注意和都是正的，并且也是正的，這樣角度便在第一象限，且為53.1°。2.7.1.3 球坐標如圖2.20所示，球坐標系統(tǒng)由一個線性運動和兩個旋轉(zhuǎn)運動組成，運動順序為：先沿z軸平

41、移r,再繞y軸旋轉(zhuǎn)并繞z軸旋轉(zhuǎn)。這三個變換建立了手坐標系與參考坐標系之間的聯(lián)系。由于這些變換都是相對于全局參考坐標系的坐標軸的，因此有這三個變換所產(chǎn)生的總變換可以通過一次左乘每一個矩陣而求得： (2.34) (2.35)圖2.20球坐標前三列表是了經(jīng)過一系列變換后的坐標系的姿態(tài)，而最后一列則表示了坐標系原點的位置。以后還要進一步討論該矩陣的姿態(tài)部分。這里也可回轉(zhuǎn)最后一個坐標系，使它與參考坐標系平行。這一問題將作為練習留給讀者，要求找出正確的運動順序來獲得正確的答案。球坐標的逆運動學方程比簡單的直角坐標和圓柱坐標更復雜，因為兩個角度和是耦合的。讓我們通過下例來說明如何求解球坐標的逆運動學方程。例

42、2.15 假設要將球坐標機器人手坐標系原點放在，計算機器人的關節(jié)變量。解：根據(jù)式（2.35）的矩陣，將手坐標系原點的位置分量設置為期望值，可以得到：=3=4=7由第三個方程，我們得出是正數(shù)，但沒有關于是正或負的信息。將前兩個方程彼此相除，因為不知道的實際符號是什么，因此可能會有兩個解。下面的方法給出了兩個可能的解，后面還必須對這最后的結(jié)果進行檢驗以確保它們是正確的。tan=或 233.1S=0.8 或 -0.8C=0.6 或 -0.6S=5 或 -5C=7 =35.5或 -35.5=8.6可以對這兩組解進行檢驗并正是這兩組解都能滿足所有的位置方程。如果沿給定的三維坐標軸旋轉(zhuǎn)這些角度，物理上的確

43、能到達同一點。然而必須注意，其中只有一組解能滿足姿態(tài)方程。換句話說，前兩種解將產(chǎn)生同樣的位置，但處于不同的姿態(tài)。由于目前并不關心手坐標系在這點的姿態(tài)，因此兩個位置解都是正確的。實際上，由于不能對三自由度的機器人指定姿態(tài)，所以無法確定兩個解中哪一個和特定的姿態(tài)有關。鏈式坐標如圖2.21所示，鏈式坐標由三個旋轉(zhuǎn)組成。后面在討論Denavit-Hartenberg表示法時，將推導鏈式坐標的矩陣表示法。圖2.21鏈式坐標2.7.2 姿態(tài)的正逆運動學方程假設固連在機器人手上的運動坐標系已經(jīng)運動到期望的位置上，但它仍然平行于參考坐標系，或者假設其姿態(tài)并不是所期望的，下一步是要在不改變位置的情況下，適當?shù)匦?/p>

44、轉(zhuǎn)坐標系而使其達到所期望的姿態(tài)。合適的旋轉(zhuǎn)順序取決于機器人手腕的設計以及關節(jié)裝配在一起的方式?？紤]以下三中常見的構(gòu)型配置：（a） 滾動角、俯仰角、偏航（RPY）角（b） 歐拉角（c） 鏈式關節(jié)2.7.2.1 滾動角、俯仰角和偏航角這是分別繞當前軸的三個旋轉(zhuǎn)順序，能夠把機器人的手調(diào)整到所期望的姿態(tài)。此時，假定當前的坐標系平行于參考坐標系，于是機器人手的姿態(tài)在RPY（滾動角、俯仰角、偏航角）運動前與參考坐標系相同。如果當前坐標系不平行于參考坐標系，那么機器人手最終的姿態(tài)將會是先前的姿態(tài)與RPY右乘的結(jié)果。因為不希望運動坐標系原點的位置有任何改變（它已被放在一個期望的位置上，所以只需要旋轉(zhuǎn)她到所期望

45、的姿態(tài)），所以RPY的旋轉(zhuǎn)運動都是相對于當前的運動軸的。否則。如前面所看到的，運動坐標系的位置將會改變。于是，右乘所有由RPY和其他旋轉(zhuǎn)所產(chǎn)生的與姿態(tài)改變有關的矩陣。圖2.22繞當前坐標軸的RPY旋轉(zhuǎn)參考圖2.22，可看到RPY旋轉(zhuǎn)包括以下幾種：l 繞軸（運動坐標系的z軸）旋轉(zhuǎn)叫滾動；l 繞軸（運動坐標系的y軸）旋轉(zhuǎn)叫俯仰；l 繞軸（運動坐標系的x軸）旋轉(zhuǎn)叫偏航。表示RPY姿態(tài)變化的矩陣為：（2.36）這個矩陣表示了僅由RPY引起的姿態(tài)變化。該坐標系相對于參考坐標系的位置和最終姿態(tài)是表示位置變化和RPY的兩個矩陣的乘積。例如，假設一個機器人是根據(jù)球坐標和RPY來設計的，那么這個機器人就可以表示

46、為：關于RPY的你運動學方程的解比球坐標更復雜，因為這里有三個耦合角，所以需要所有三個角各自的正弦和余弦值的信息才能解出這個角。為解出這三個角的正弦值和余弦值，必須將這些角解耦。因此，用的逆左乘方程兩邊，得：（2.37）假設用RPY得到的最后所期望的姿態(tài)是用矩陣來表示的，則有：（2.38）進行矩陣相乘后得：（2.39）在式（2.38）中的分量表示了最終的期望值，它們通常是給定或已知的，而RPY角的值是未知的變量。讓式（2.39）左右兩邊對應的元素相等，將產(chǎn)生如下結(jié)果（關于ATAN2函數(shù)的解釋請參考附錄A）：根據(jù)2，1元素得：（2.40）根據(jù)3，1元素和1，1元素得：（2.41）根據(jù)2，2元素和

47、2，3元素得：（2.42）例2.16下面給出了一個笛卡爾坐標-RPY型機器人手所期望的最終位姿，求滾動角，俯仰角，偏航角和位移。解：根據(jù)上述方程，得到兩組解：或235或128或195例2.17與例2.16中的位姿一樣，如果機器人是圓柱坐標-RPY型，求所有關節(jié)變量。解：在這種情況下，可用：這個方程右邊有4個角，它們是耦合的，因此必須像前面那樣將它們解耦。但是，因為對于圓柱坐標系z軸旋轉(zhuǎn)角并不影響a軸，所以它仍平行于z軸。其結(jié)果是，對于RPY繞a軸旋轉(zhuǎn)的角可簡單地加到上。這意味著，求出的實際上是（參見圖2.23）。根據(jù)給定的位置數(shù)據(jù)，可求得例2.16的解。參考式（2.33），得到：同例2.16一

48、樣當然，可以用類似的解法求出第二組解。圖2.23例2.17的圓柱和RPY坐標2.7.2.2 歐拉角除了最后的旋轉(zhuǎn)使然當前的a軸外，歐拉角的其他方面均與RPY相似（參見圖2.24）。我們?nèi)孕枰顾行D(zhuǎn)都是繞當前的軸轉(zhuǎn)動以防止機器人的位置有任何改變。表示歐拉角的轉(zhuǎn)動如下：l 繞軸（運動坐標系的z軸）旋轉(zhuǎn)度；l 接著繞軸（運動坐標系的y軸）旋轉(zhuǎn)度；l 最后再繞軸（運動坐標系的x軸）旋轉(zhuǎn)度。表示歐拉角姿態(tài)變化的矩陣是：（2.43）圖2.24繞當前坐標軸的歐拉旋轉(zhuǎn)再次強調(diào)，該矩陣只是表示了由歐拉角所引起的姿態(tài)變化。相對于參考坐標系，這個坐標系的最終位姿是表示位置變化的矩陣和表示歐拉角的矩陣的乘積。歐拉

49、角的逆運動學求解與RPY非常相似?？梢允箽W拉方程的兩邊左乘來消去其中一邊的。讓兩邊的對應元素相等，就可得到以下方程（假設由歐拉角得到的最終所期望的姿態(tài)是由矩陣表示）：（2.44）或（2.45）記住，式（2.44）中的表示了最終的期望值，它們通常是給定或已知的。歐拉角的值是未知變量。讓式（2.45）左右兩邊對應的元素相等，可得到如下結(jié)果：根據(jù)2，3元素，可得：或（2.46）由于求得了值，因此式（2.45）左邊所有的元素都是已知的。根據(jù)2，1元素和2，2元素得：（2.47）最后根據(jù)1，3元素和3，3元素，得：（2.48）例2.18 給定一個直角坐標系歐拉角型機器人手的最終期望狀態(tài)，求相應的歐拉角。

50、解：根據(jù)前面的方程，得到：或?qū)⒑偷恼抑岛陀嘞抑祽糜谄渌糠?，可得：或?.7.2.3鏈式關節(jié)鏈式關節(jié)由3個旋轉(zhuǎn)組成，而不是上面剛提出來的旋轉(zhuǎn)模型，這些旋轉(zhuǎn)取決于關節(jié)的設計。就像在（d）中所做的那樣，我們將在討論D-H表示法時來推導表示鏈式關節(jié)的矩陣。2.7.3 位姿的正逆運動學方程表示機器人最終位姿的矩陣是前面方程的組合，該矩陣取決于所用的坐標。假設機器人的運動是由直角坐標和RPY的組合關節(jié)組成，那么該坐標系相對于參考坐標系的最終位姿是表示直角坐標位置變化的矩陣和RPY矩陣的乘積。它可表示為：（2.49）如果機器人是采用球坐標定位、歐拉角定姿的方式所設計的，那么將得到下列方程。其中位置由球

51、坐標決定，而最終姿態(tài)既受球坐標角度的影響也受歐拉角的影響。（2.50）由于有多種不同的組合，所以這種情況下的正逆運動學解不在這里討論。對于復雜的設計，推薦用D_H表示法來求解，并將在下面對此進行討論。2.8 機器人正運動學方程的D-H表示法在1955年，Denavit和Hartenberg在“ASME Journal of Applied Mechanics”發(fā)表了一篇論文，后來利用那個這篇論文來對機器人進行表示和建模，并導出了它們的運動方程，這已成為表示機器人和對機器人運動進行建模的標準方法，所以必須學習這部分內(nèi)容。Denavit-Hartenberg(D_H)模型表示了對機器人連桿和關節(jié)進

52、行建模的一種非常簡單的方法，可用于任何機器人構(gòu)型，而不管機器人的結(jié)構(gòu)順序和復雜程度如何。它也可用于表示已經(jīng)討論過的在任何坐標中的變換，例如直角坐標、圓柱坐標、球坐標、歐拉角坐標及RPY坐標等。另外，它也可以用于表示全旋轉(zhuǎn)的鏈式機器人、SCARA機器人或任何可能的關節(jié)和連桿組合。盡管采用前面的方法對機器人直接建模會更快、更直接，但D-H表示法有其附加的好處，使用它已經(jīng)開發(fā)了許多技術(shù)，例如，雅克比矩陣的計算和力分析等。假設機器人由一系列關節(jié)和連桿組成。這些關節(jié)可能是滑動（線性）的或旋轉(zhuǎn)（轉(zhuǎn)動）的，它們可以按任意的順序放置并處于任意的平面。連桿也可以是任意的長度（包括零），它可能被彎曲或扭曲，也可能

53、位于任意平面上。所以任何一組關節(jié)和連桿都可以構(gòu)成一個我們想要建模和表示的機器人。為此，需要給每個關節(jié)指定一個參考坐標系，然后，確定從一個關節(jié)到下一個關節(jié)（一個坐標系到下一個坐標系）來進行變換的步驟。如果將從基座到第一個關節(jié)，再從第一個關節(jié)到第二個關節(jié)直至到最后一個關節(jié)的所有變換結(jié)合起來，就得到了機器人的總變換矩陣。在下一節(jié)，將根據(jù)D-H表示法確定一個一般步驟來為每個關節(jié)指定參考坐標系，然后確定如何實現(xiàn)任意兩個相鄰坐標系之間的變換，最后寫出機器人的總變換矩陣。圖2.25通用關節(jié)連桿組合的D-H表示假設一個機器人由任意多的連桿和關節(jié)以任意形式構(gòu)成。圖2.25表示了三個順序的關節(jié)和兩個連桿。雖然這些

54、關節(jié)和連桿并不一定與任何實際機器人的關節(jié)或連桿相似，但是他們非常常見，且能很容易地表示實際機器人的任何關節(jié)。這些關節(jié)可能是旋轉(zhuǎn)的、滑動的、或兩者都有。盡管在實際情況下，機器人的關節(jié)通常只有一個自由度，但圖2.25中的關節(jié)可以表示一個或兩個自由度。圖2.25（a）表示了三個關節(jié)，每個關節(jié)都是可以轉(zhuǎn)動或平移的。第一個關節(jié)指定為關節(jié)n，第二個關節(jié)為關節(jié)n+1，第三個關節(jié)為關節(jié)n+2。在這些關節(jié)的前后可能還有其他關節(jié)。連桿也是如此表示，連桿n位于關節(jié)n與n+1之間，連桿n+1位于關節(jié)n+1與n+2之間。為了用D-H表示法對機器人建模，所要做的第一件事是為每個關節(jié)指定一個本地的參考坐標系。因此，對于每個關節(jié)，都必須指定一個z軸和x軸，通常并