淺析工業(yè)機(jī)器人的坐標(biāo)轉(zhuǎn)換矩陣算法

殷 卓

（北京光電技術(shù)研究所,北京， 10010）

淺析工業(yè)機(jī)器人的坐標(biāo)轉(zhuǎn)換矩陣算法

殷 卓

（北京光電技術(shù)研究所,北京， 10010）

本文主要闡述筆者在使用史陶比爾六軸工業(yè)機(jī)器人進(jìn)行激光加工的過程中總結(jié)出的一種計算方法，該方法基于線性代數(shù)理論對機(jī)器人的關(guān)節(jié)坐標(biāo)和笛卡爾坐標(biāo)進(jìn)行相互轉(zhuǎn)換。這種方法易于理解，既不要求機(jī)器人編程者掌握高深的數(shù)學(xué)知識，也不要求編程者購買價格昂貴的機(jī)器人模擬軟件，可以實(shí)現(xiàn)六軸工業(yè)機(jī)器人位置的笛卡爾坐標(biāo)與關(guān)節(jié)坐標(biāo)的相互轉(zhuǎn)換求解。通過實(shí)例驗(yàn)證，用此方法計算出的機(jī)器人關(guān)節(jié)坐標(biāo)值，和史陶比爾機(jī)器人得出的關(guān)節(jié)坐標(biāo)值相差在千分之一以內(nèi)。

工業(yè)機(jī)器人，坐標(biāo)轉(zhuǎn)換

0 概述

工業(yè)機(jī)器人是面向工業(yè)領(lǐng)域的多關(guān)節(jié)機(jī)械手。關(guān)節(jié)坐標(biāo)系和世界坐標(biāo)系是工業(yè)機(jī)器人的兩個基本坐標(biāo)系，兩者之間相互轉(zhuǎn)換的算法是機(jī)器人算法的基礎(chǔ)。各機(jī)器人廠家有各自的算法，但不會向使用者詳細(xì)介紹，所以機(jī)器人使用者往往不易更加深入了解機(jī)器人的內(nèi)部算法，這在某種程度上對機(jī)器人的編程使用造成了一定的難度。本文淺顯地論述了一種矩陣算法，可以加深機(jī)器人使用者對機(jī)器人工作原理的理解，對其在進(jìn)行機(jī)器人編程、選型購買、方案設(shè)計等工作時起到一定的幫助作用。此方法獲得的結(jié)果與實(shí)際機(jī)器人自身的運(yùn)行結(jié)果高度吻合，驗(yàn)證了此方法的正確性。

1 機(jī)器人坐標(biāo)系

1.1 關(guān)節(jié)坐標(biāo)系

圖1 關(guān)節(jié)坐標(biāo)系

關(guān)節(jié)坐標(biāo)是工業(yè)機(jī)器人最基本的坐標(biāo)系。如圖1所示的工業(yè)機(jī)器人具有六個自由度，j1,j2,j3,j4,j5,j6這六個自由度給定六個關(guān)節(jié)的角度值（單位為°,旋轉(zhuǎn)方向如圖1中箭頭方向所示）。習(xí)慣上將這六個自由度叫作機(jī)器人的六個軸。

1.2 世界坐標(biāo)系

世界坐標(biāo)系如圖2所示,是一種笛卡爾坐標(biāo)系，它類似于一般數(shù)控機(jī)床的機(jī)床坐標(biāo)系。它包括原點(diǎn)和符合右手定則的XYZ三個坐標(biāo)軸。其中，原點(diǎn)是機(jī)器人底座的中心點(diǎn)，X軸正向指向機(jī)器人的正前方，Z軸指向機(jī)器人的正上方，Y軸由右手定則規(guī)定，即指向如圖2所示的右方。世界坐標(biāo)系同樣具有六個自由度。這六個自由度包括三個位置x,y,z和三個角rx,ry,rz。在機(jī)器人尚未夾持工具的情況下，x,y,z代表第六軸法蘭盤圓心相對于原點(diǎn)的位置偏移量。rx,ry,rz代表第六軸法蘭盤的軸線角度，由初始姿態(tài)即豎直向上繞z軸旋轉(zhuǎn)rz度，再繞y軸旋轉(zhuǎn)ry度，再繞x軸旋轉(zhuǎn)rx度得到。

圖2 世界坐標(biāo)系

1.3 工具坐標(biāo)系

圖3 機(jī)器人第六軸法蘭盤

工具坐標(biāo)系類似于數(shù)控機(jī)床的刀補(bǔ)。首先，在機(jī)器人尚未夾持工具（如焊槍、油漆噴頭等）的時候，機(jī)器人默認(rèn)第六軸法蘭盤的中心位置（如圖3所示）為工具坐標(biāo)原點(diǎn)，相應(yīng)的X軸、Y軸在法蘭盤平面內(nèi)，Z軸由右手定則決定，即為第六軸的軸線。如果機(jī)器人手臂末端需要安裝工具的時候，使用者不再關(guān)心機(jī)器人第六軸法蘭盤的位置和角度，而更關(guān)心焊槍、油漆噴頭等工具最終的位置和角度，這時，就需要對工具坐標(biāo)系進(jìn)行賦值。工具坐標(biāo)系記錄執(zhí)行工具的尺寸信息，其中包含toolx,tooly,toolz三個位置偏移信息和toolrx,toolry,toolrz三個角度旋轉(zhuǎn)信息。這即說明，最終工具的工作點(diǎn)相對于圖3所示的原點(diǎn)的位置偏移量為toolx,tooly,toolz，工具的工作軸線是繞相對于圖3坐標(biāo)系中的z軸旋轉(zhuǎn)rz度，再繞y軸旋轉(zhuǎn)ry度，再繞x軸旋轉(zhuǎn)rx度得到。例如，本項(xiàng)目使用激光頭作為工具，如圖4所示，激光頭的幾何尺寸如圖5所示。激光頭的幾何尺寸經(jīng)過計算，激光頭對應(yīng)的工具坐標(biāo)即為：toolx=-15.097;tooly=0;toolz=307.405;toolrx=0;toolry=35; toolrz=0。（實(shí)際中，機(jī)器人廠家可以通過五點(diǎn)示教的方法對工具坐標(biāo)進(jìn)行賦值，在此不再贅述）。

圖4 機(jī)器人與工具（激光頭）

圖5 激光頭的關(guān)鍵幾何尺寸

2 線性代數(shù)理論基礎(chǔ)

圖6 坐標(biāo)系旋轉(zhuǎn)

3 已知關(guān)節(jié)坐標(biāo)求解世界坐標(biāo)

3.1 機(jī)器人尺寸和關(guān)節(jié)坐標(biāo)與直角坐標(biāo)

不帶工具坐標(biāo)系的機(jī)器人初始狀態(tài)，即各關(guān)節(jié)j1,j2,j3,j4,j5,j6（單位為度）為0度位置，如7所示(圖中長度數(shù)據(jù)單位為mm)。

圖7 機(jī)器人關(guān)鍵尺寸和初始姿態(tài)

當(dāng)關(guān)節(jié)坐標(biāo)j1,j2,j3,j4,j5,j6不為零時，機(jī)器人的工作狀態(tài)如圖8所示。

圖8 機(jī)器人的一般工作姿態(tài)

3.2 不帶工具坐標(biāo)系的角度公式

對于圖7所示的初始位置,機(jī)器人的第六軸豎直向上，指向Z軸正方向，用向量表示此方向?yàn)椤６鴮τ谀硞€工作位置（圖8），是由第一二三四五六各軸旋轉(zhuǎn)角度為j1，j2，j3，j4，j5，j6得到的。

六個軸的旋轉(zhuǎn)分六步。第六軸先旋轉(zhuǎn)j6角度，相當(dāng)于繞Z軸逆時針旋轉(zhuǎn)j6度后，第六軸Z軸指向變?yōu)?，同理，第五軸旋轉(zhuǎn)j5角度后，第六軸Z軸指向相當(dāng)于向量再繞X軸順時針再旋轉(zhuǎn)j5角度，用向量表示

第四軸的旋轉(zhuǎn)j4角度相當(dāng)于上述向量再繞Z軸逆時針旋轉(zhuǎn)j4角度，第三軸的旋轉(zhuǎn)相當(dāng)于再繞X軸逆時針旋轉(zhuǎn)j3角度，第二軸的旋轉(zhuǎn)相當(dāng)于再繞X軸逆時針旋轉(zhuǎn)j2角度，第一軸的旋轉(zhuǎn)相當(dāng)于繞Z軸逆時針旋轉(zhuǎn)j1角度，最終向量表示為

為了方便起見，不妨采用以下簡化記法:

[j2矩陣]！= [j1矩陣]× [j2矩陣]

[j3矩陣]！= [j1矩陣]× [j2矩陣] × [j3矩陣]

[j4矩陣]！= [j1矩陣]× [j2矩陣] × [j3矩陣]× [j4矩陣]

[j5矩陣]！= [j1矩陣]× [j2矩陣] × [j3矩陣]× [j4矩陣]× [j5矩陣]

[j6矩陣]！= [j1矩陣]× [j2矩陣] × [j3矩陣]× [j4矩陣]× [j5矩陣] × [j6矩陣]

在7不帶工具坐標(biāo)的情況下，對于關(guān)節(jié)坐標(biāo)為（j1,j2,j3,j4,j5,j6）的位置時，第六軸Z軸指向?yàn)?/p>

[j6矩陣]！×。

而在世界坐標(biāo)系中，角度rx、ry、rz代表第六軸法蘭盤軸線先后繞Z軸、Y軸和X軸旋轉(zhuǎn)的角度。記：

[r矩陣]=[rx矩陣] ×[ry矩陣]×[rz矩陣]。

即，對于世界坐標(biāo)（x,y,z,rx,ry,rz）,是指第六軸軸線指向變?yōu)椋?/p>

因此，已知關(guān)節(jié)坐標(biāo)（j1,j2,j3,j4,j5,j6）求世界坐標(biāo)系中的角度問題，即求rx、ry、rz的公式為：

例如，當(dāng)關(guān)節(jié)坐標(biāo)為（10，20，30，40，50，60）時，求其世界坐標(biāo)角度值rx、ry、rz。

此方程展開后含有rx, ry, rz三個未知數(shù)及三個方程，因而可求。

3.3 不帶工具坐標(biāo)系的位置公式

機(jī)器人由六個關(guān)節(jié)組成，末端手臂的位置向量是這六個關(guān)節(jié)位置向量的矢量和。對于項(xiàng)目中的史陶比爾機(jī)器人，各關(guān)節(jié)的尺寸已知，如圖7所示，不妨記x1=150,z2=825,z3=132,z4=625-132=493,z5=110。

第一關(guān)節(jié):在j1角度為0的情況下，位置向量是在X軸正向偏移x1距離，而j1角度不為零時，位置向量則為[j1矩陣]×。

第四關(guān)節(jié): j1，j2，j3，j4不為零時，位置向量為

第五關(guān)節(jié): j1，j2，j3，j4，j5不為零時，位置向量為

第六關(guān)節(jié):對于最終位置沒有影響，位置向量為零。

綜上所述，如果設(shè)世界坐標(biāo)的位置為x、y、z,則已知關(guān)節(jié)坐標(biāo)求解世界坐標(biāo)（不帶工具坐標(biāo)系情況下）的位置公式為：

等式含三個方程，右側(cè)皆已知，等式左側(cè)含三個未知量x、y、z，因而可求。

因此，對于無工具坐標(biāo)的情況，根據(jù)公式（1）和公式（2）,如果已知關(guān)節(jié)坐標(biāo)（j1,j2,j3,j4,j5,j6）,則世界坐標(biāo)（x,y,z,rx,ry,rz）可求。

3.4 帶工具坐標(biāo)系的角度公式

在實(shí)際工作中，機(jī)器人往往需要在第六軸上加裝工具，即需要考慮工具坐標(biāo)。本項(xiàng)目工具以激光頭為例，工具坐標(biāo)為:

toolx=-15.097，tooly=0，toolz=307.405，

toolrx=0，toolry=35，toolrz=0。

記:

[tool矩陣]= [toolrx矩陣] ×[toolry矩陣]× [toolrz矩陣]

則已知關(guān)節(jié)坐標(biāo)求解世界坐標(biāo)（帶工具坐標(biāo)系情況下）的角度公式為:

其中含有三個方程，三個未知數(shù)，[tool矩陣]，[J6矩陣]!已知。方程可解。

3.5 帶工具坐標(biāo)系的位置公式

對于位置問題，可以認(rèn)為前五個關(guān)節(jié)的向量相對于不帶工具時沒有變化，而由于工具坐標(biāo)系位置向量的引入，使得第六關(guān)節(jié)的向量從0變?yōu)閇j6矩陣]！×，最終已知關(guān)節(jié)坐標(biāo)求解世界坐標(biāo)（帶工具坐標(biāo)系情況下）的位置公式為:

（其中等式右側(cè)均已知，x,y,z可求）

因此，對于帶工具坐標(biāo)的情況，根據(jù)公式(3)和公式(4),如果已知關(guān)節(jié)坐標(biāo)(j1,j2,j3,j4,j5,j6),工具坐標(biāo)（toolx,tooly,t oolz,toolrx,toolry,toolrz）則世界坐標(biāo)（x,y,z,rx,ry,rz）可求。

4 已知世界坐標(biāo)求解關(guān)節(jié)坐標(biāo)

已知關(guān)節(jié)坐標(biāo)，求解世界坐標(biāo)是比較直觀和簡單的，而且實(shí)際意義不大，因?yàn)閷?shí)際過程中往往是已知世界坐標(biāo)而反求機(jī)器人的關(guān)節(jié)角度，所以下面論述已知世界坐標(biāo)反求關(guān)節(jié)坐標(biāo)的過程。

4.1 論題

舉例來說，隨意指定一世界坐標(biāo)，如某一點(diǎn)A的世界坐標(biāo)如下：x=1044.02，y=-89.64，z=984.62，rx=16.82，ry=77.02，rz=-16.73。工具坐標(biāo)系已知如下：toolx=-15.097，tooly=0，toolz=307.405，toolrx=0，toolry=35，toolrz=0。機(jī)器人的必要尺寸已知：x1=150，z2=825，z3=132，z4=493，z5=110。求解為到達(dá)這一世界坐標(biāo)點(diǎn)所需的各關(guān)節(jié)角度，即（j1,j2,j3,j4,j5,j6）的值。

4.2 求解方法

利用上述帶工具坐標(biāo)的兩組方程，最終角度方程公式（3）和最終位置方程公式（4）求得。

4.3 求解過程

根據(jù)已知數(shù)據(jù)，計算出如下矩陣：

所以[j6矩陣]！與[j5矩陣]！的第三列是相等的，故第五關(guān)節(jié)向量中可以用 [j6矩陣]！代替[j5矩陣]！（這是很關(guān)鍵的一步簡化，正是由于這步替代才使得方程最終可解）于是最終位置方程變成:

此含有三個方程，但是j1、j2、j3、j4共4個未知數(shù)，本不可解，但同樣因?yàn)閇j4矩陣]！與[j3矩陣]！第三列相同，所以j4這個未知數(shù)被消掉，上述方程變?yōu)楹?個未知數(shù)j1,j2,j3和三個方程，即:

150×cos(j1)+825×cos(j1)×sin(j2)+625×cos(j1)×cos(j2)× sin(j3)+625×cos(j1)×sin(j2)×cos(j3)+ 270.5762=1044.02 150×sin(j1)+825×sin(j1)×sin(j2)+625×sin(j1)×cos(j2)× sin(j3)+625×sin(j1)×sin(j2)×cos(j3)- 20.2935=-89.64 825×cos(j2)-625×sin(j2)×sin(j3)+625×cos(j2)× cos(j3)+317.5398=984.62

解此方程可以用通用的數(shù)學(xué)計算軟件，如MATLAB編寫程序求解，編寫下列語句:

[j1,j2,j3]=solve('150×cos(j1)+825×cos(j1)×sin(j2)+625×cos(j1) ×cos(j2)×sin(j3)+625×cos(j1)×sin(j2)×cos(j3)+270.5762 =1044.02',

'150×sin(j1)+825×sin(j1)×sin(j2)+625×sin(j1)×cos(j2)× sin(j3)+625×sin(j1)×sin(j2)×cos(j3)-20.2935=-89.64', '825×cos(j2)-625×sin(j2)×sin(j3)+625×cos(j2)×cos(j3)+ 317.5398=984.62','j1','j2','j3')

MATLAB軟件計算出j1，j2，j3的解，并轉(zhuǎn)換成角度值得:

可見，每個角度的解并不唯一，各有四組解，但根據(jù)機(jī)器人使用常識可知，一般情況下有:

-90＜j1＜90,0＜j2＜90,0＜j3＜90故只有第二組解符合條件:

j1=-5.12340,j2=1.51080,j3=103.09769

再代入最終角度計算公式，可求j4,j5。同樣，利用MATLAB軟件編寫下列語句:

[j4,j5]=solve('(-0.2512×cos(j4)+ 0.0893×sin(j4))×sin(j5)+ 0.9638×cos(j5)=0.6748','(0.0225×cos(j4)+ 0.9960×sin(j4))×sin(j5)- 0.0864×cos(j5)=-0.0501','j4','j5')

MATLAB軟件計算出j4，j5的解，并轉(zhuǎn)換成角度值得:

j4=179.32785,j5=62.03025

同理繼續(xù)可求J6：

[j6]=solve('0.6766×cos(j6)+ 0.0114×sin(j6)=-0.6766','j6')

得: j6=-178.28299

故最終解為:

j1= -5.1234，j2=1.5108,j3=103.0976,j4=179.3278,j5=62.0302, j6=-178.2829。

4.4 驗(yàn)證過程

實(shí)際操作史陶比爾機(jī)器人，其帶動激光頭按照世界坐標(biāo)移動到點(diǎn)（x=1044.02;y=-89.64;z=984.62;rx=16.82;ry=77 .02;rz=-16.73），如圖9所示。之后，機(jī)器人的控制器切換到關(guān)節(jié)坐標(biāo)模式，讀取當(dāng)前的關(guān)節(jié)坐標(biāo)，如圖10所示，(J1=-5.1239,J2=1.5119,J3=103.097,J4=179.328,J5=62.0318 ,J6=-178.30)。

表1：機(jī)器人實(shí)際行走結(jié)果與本文算法結(jié)果的對比

表1為機(jī)器人實(shí)際行走結(jié)果與本文算法結(jié)果的對比，可以看出：機(jī)器人實(shí)際行走得到的結(jié)果與應(yīng)用本文中的算法計算結(jié)果基本一致，誤差低于千分之一。

圖9 機(jī)器人實(shí)際世界坐標(biāo)

圖10 機(jī)器人實(shí)際關(guān)節(jié)坐標(biāo)

結(jié) 論

本文介紹的基于線性代數(shù)矩陣計算理論的求解機(jī)器人關(guān)節(jié)坐標(biāo)和世界坐標(biāo)的轉(zhuǎn)換問題的方法，計算出的結(jié)果與機(jī)器人實(shí)際運(yùn)行產(chǎn)生的結(jié)果一致。通過本算法，可以使機(jī)器人使用者更加深入地領(lǐng)會機(jī)器人的算法實(shí)質(zhì)，不僅做到知其然，而且做到知其所以然。對于機(jī)器人選型，模擬仿真等工作也可以起到一定的幫助作用。