史陶比爾機器人案例

機器人項目，需要機器人拿着擦膠的工具去固定針頭擦膠！

擦膠工具是兩條海綿夾着針頭，海綿旋轉，擦完下次通過機器人偏移換個位置擦膠。

海綿夾針頭，擦膠會導緻擦膠針頭兩邊擦得幹淨，中間部分不幹淨，于是客戶提出擦膠讓機器人抓手上的海綿繞着針頭旋轉一下。

看着很合理的要求，實現起來挺麻煩的。

機器人噴塗

難點如下：

如果隻在海綿的固定位置擦膠，建一個TCP在這個位置，機器人繞着TCP旋轉就行，難點在于不是在海綿固定的點擦膠，擦完一次需要偏移個位置再擦膠，這樣就會導緻下一次擦膠時候機器人需要再新建一個新的TCP，擦膠海綿很長，需要很多TCP，而且每次擦膠都要精确到達TCP位置。程序的靈活性差，碰撞風險大。

所以我想了個實現動态TCP的程序。

思路如下：

1、正常示教擦膠的第一個點位，下次擦膠的點位通過設定偏移值計算得到，偏移長度随意設定。

2、寫一個自動計算TCP的算法，比如我知道第一個擦膠點位，根據點位的坐标齊次變換算出一個TCP，TCP和這個擦膠點位重合。

3、偏移過去的點位也是根據偏移的點位坐标換算出新的TCP，和當前點位重合。這樣每個點位自動生成一個TCP，實現動态的TCP功能。

詳解程序框架：

機器人點位、工具、坐标系的數據結構通常都是下面例子這樣：

例如，工具坐标系TCP[1]表示為{x,y,z,rx,ry,rz}，用x/y/z的坐标值，和rx/ry/rz的旋轉值，定為一個點。

想要進行坐标系轉換，就得讓這些數據參與其次變換運算，首先就要轉換成矩陣的形式。然後矩陣和矩陣相乘，得到個變換後的坐标系。

齊次變換公式

齊次變化會用到下面的算法：

1、機器人位置變量轉換成Matrik

寫一個函數塊，輸入接口輸入P1[x,y,z,rx,ry,rz]這個點位；

定義六個内部變量x,y,z,rx,ry,rz；

定義一個輸出齊次變換矩陣 Matrix1[4,4];

定義矩陣内需要變量的名稱：nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;

(不知道齊次變換矩陣公式的可以關注，看我以前的文章或視頻)

程序如下：

//通過接口輸入給内部變量賦值

x=P1.x

y= P1.y

z= P1.z

rx= P1.rx

ry= P1.ry

rz= P1.rz

//給矩陣内元素賦值

nx=cos(ry)*cos(rz)

ny=sin(rx)*sin(ry)*cos(rz) cos(rx)*sin(rz)

nz=-cos(rx)*sin(ry)*cos(rz) sin(rx)*sin(rz)

ox=-cos(ry)*sin(rz)

oy=-sin(rx)*sin(ry)*sin(rz) cos(rx)*cos(rz)

oz=cos(rx)*sin(ry)*sin(rz) sin(rx)*cos(rz)

ax=sin(ry)

ay=-sin(rx)*cos(ry)

az=cos(rx)*cos(ry)

px=x

py=y

pz=z

//矩陣内元素寫入矩陣

Matrix1[0,0]=nx

Matrix1[0,1]=ny

Matrix1[0,2]=nz

Matrix1[0,3]=0

Matrix1[1,0]=ox

Matrix1[1,1]=oy

Matrix1[1,2]=oz

Matrix1[1,3]=0

Matrix1[2,0]=ax

Matrix1[2,1]=ay

Matrix1[2,2]=az

Matrix1[2,3]=0

Matrix1[3,0]=px

Matrix1[3,1]=py

Matrix1[3,2]=pz

Matrix1[3,3]=1

2、根據需求先算出變換矩陣，其實就是示教的一個原始TCP坐标，也是用上面的程序把這個坐标轉換成齊次矩陣形式輸出Matrix2[4,4]。

3、兩矩陣相乘，Matrix1* Matrix2，寫個矩陣乘法函數，輸出矩陣Matrix3[4,4]

程序如下：

定義a和b開頭加數字的變量都為矩陣内的數據；

//初始化變量

a11=Matrix1[0,0]

a21=Matrix1[0,1]

a31=Matrix1[0,2]

a41=Matrix1[0,3]

a12=Matrix1[1,0]

a22=Matrix1[1,1]

a32=Matrix1[1,2]

a42=Matrix1[1,3]

a13=Matrix1[2,0]

a23=Matrix1[2,1]

a33=Matrix1[2,2]

a43=Matrix1[2,3]

a14=Matrix1[3,0]

a24=Matrix1[3,1]

a34=Matrix1[3,2]

a44=Matrix1[3,3]

b11=Matrix2[0,0]

b21=Matrix2[0,1]

b31=Matrix2[0,2]

b41=Matrix2[0,3]

b12=Matrix2[1,0]

b22=Matrix2[1,1]

b32=Matrix2[1,2]

b42=Matrix2[1,3]

b13=Matrix2[2,0]

b23=Matrix2[2,1]

b33=Matrix2[2,2]

b43=Matrix2[2,3]

b14=Matrix2[3,0]

b24=Matrix2[3,1]

b34=Matrix2[3,2]

b44=Matrix2[3,3]

//矩陣相乘

Matrix3[0,0]=a11*b11 a12*b21 a13*b31 a14*b41

Matrix3[0,1]=a21*b11 a22*b21 a23*b31 a24*b41

Matrix3[0,2]=a31*b11 a32*b21 a33*b31 a34*b41

Matrix3[0,3]=a41*b11 a42*b21 a43*b31 a44*b41

Matrix3[1,0]=a11*b12 a12*b22 a13*b32 a14*b42

Matrix3[1,1]=a21*b12 a22*b22 a23*b32 a24*b42

Matrix3[1,2]=a31*b12 a32*b22 a33*b32 a34*b42

Matrix3[1,3]=a41*b12 a42*b22 a43*b32 a44*b42

Matrix3[2,0]=a11*b13 a12*b23 a13*b33 a14*b43

Matrix3[2,1]=a21*b13 a22*b23 a23*b33 a24*b43

Matrix3[2,2]=a31*b13 a32*b23 a33*b33 a34*b43

Matrix3[2,3]=a41*b13 a42*b23 a43*b33 a44*b43

Matrix3[3,0]=a11*b14 a12*b24 a13*b34 a14*b44

Matrix3[3,1]=a21*b14 a22*b24 a23*b34 a24*b44

Matrix3[3,2]=a31*b14 a32*b24 a33*b34 a34*b44

Matrix3[3,3]=a41*b14 a42*b24 a43*b34 a44*b44

4、最後把Matrix3再轉換成機器人坐标形式即可；

程序如下：

//Matrik矩陣再轉換成機器人坐标形式， 輸出P2坐标。

nx=Matrix3[0,0]

ny=Matrix3[0,1]

nz=Matrix3[0,2]

ox=Matrix3[1,0]

oy=Matrix3[1,1]

oz=Matrix3[1,2]

ax=Matrix3[2,0]

ay=Matrix3[2,1]

az=Matrix3[2,2]

px=Matrix3[3,0]

py=Matrix3[3,1]

pz=Matrix3[3,2]

//求P2的x、y、z

P2.x=px

P2.y=py

P2.z=pz

//求P2的Rx、Ry、Rz需要通過齊次變換裡的公式反向計算，因為有可能不止一個值，所以需要加判斷條件。

call Atan2(ax,sqrt(power(nx,2) power(ox,2)),ry1)

if ry1==90

rx1=0

call

Atan2(ny,-nz,rz1)

elseIf ry1==-90

rx1=0

call

Atan2(ny,nz,rz1)

else

call

Atan2(-ay/cos(ry1),az/cos(ry1),rx1)

call

Atan2(-ox/cos(ry1),nx/cos(ry1),rz1)

endIf

call Atan2(ax,sqrt(power(nx,2) power(ox,2)),ry2)

if ry2==90

rx2=0

call

Atan2(ny,-nz,rz2)

elseIf ry2==-90

rx2=0

call

Atan2(ny,nz,rz2)

else

call

Atan2(-ay/cos(ry2),az/cos(ry2),rx2)

call

Atan2(-ox/cos(ry2),nx/cos(ry2),rz2)

endIf

if abs(rx1) abs(ry1) abs(rz1)<=abs(rx2) abs(ry2) abs(rz2)

x_trTrsf.rx=rx1

x_trTrsf.ry=ry1

x_trTrsf.rz=rz1

else

x_trTrsf.rx=rx2

x_trTrsf.ry=ry2

x_trTrsf.rz=rz2

endIf

5、到此為止，整個換算流程就是這樣，真正應用需要根據實際情況設定點位的坐标系，坐标系需要一些賦值轉換，在此我就提供個思路，大家自己嘗試吧，或者有啥問題的，可以問我，或私信我。

,

更多精彩资讯请关注tft每日頭條，我们将持续为您更新最新资讯!

查看全部