机器人开发大赛编程题目（自己写机器人动态TCP的算法案例）

史陶比尔机器人案例

机器人项目，需要机器人拿着擦胶的工具去固定针头擦胶！

擦胶工具是两条海绵夹着针头，海绵旋转，擦完下次通过机器人偏移换个位置擦胶。

海绵夹针头，擦胶会导致擦胶针头两边擦得干净，中间部分不干净，于是客户提出擦胶让机器人抓手上的海绵绕着针头旋转一下。

看着很合理的要求，实现起来挺麻烦的。

机器人喷涂

难点如下：

如果只在海绵的固定位置擦胶，建一个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、到此为止，整个换算流程就是这样，真正应用需要根据实际情况设定点位的坐标系，坐标系需要一些赋值转换，在此我就提供个思路，大家自己尝试吧，或者有啥问题的，可以问我，或私信我。

,