您好！欢迎光临工博士商城

ABB 机器人专营店

产品:40    
联系我们
联系方式
  • 联系人：庾文科 
  • 电话：18616561800
  • 邮件：ywk@gongboshi.com
  • 手机：18616561800
新闻分类
  • 暂无分类
站内搜索
 
首页 > 新闻中心 > 详解ABB机器人计算坐标系原点姿态
新闻中心
详解ABB机器人计算坐标系原点姿态
发布时间：2020-07-01        浏览次数：861        返回列表
 

1. ABB机器人可以通过如上三点，确立坐标系原点，其中x1到x2构成x方向，y1点到x1x2连线的垂线为y方向，z方向满足右手法则，即z方向为x方向叉乘y方向得到。


2. 之前已经介绍过如何通过x1x2和y1三点坐标，计算原点o的坐标。

3. 本文介绍如何通过x1x2和y1三点，计算坐标系原点o的方向。结果输出为欧拉角。也可使用ABB机器人函数OrientZYX将欧拉角转化为四元数q1-q4

4.计算思路为：

1）获取向量x1x2并单位化

2）获取向量x1y1并单位化

3）单位向量向量oz等于单位向量x1x2叉乘单位向量x1y1

4）单位向量向量oy1等于单位向量向量oz叉乘单位向量向量oy1

5）得到单位向量ox1，单位向量ox1, 单位向量oy1, 单位向量oz

6）将旋转矩阵[ox1,oy1,oz]T转化为欧拉角

7）利用OrientZYX函数将欧拉角转化为四元数

4. 创建计算程序cal_frame_orient，输入参数为px1，px2，和py三个点，输出为ABC（即欧拉角ABC，对应ABB机器人的rz，ry，rz）

代码如下：


 

  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
proc cal_frame_orient(pos px1,pos px2,pos py,inout num a,inout num b,inout num c)        !A:Rot_z() , B:Rot_y() ,c: Rot_x()        VAR pos vpx;        VAR pos vpxy;        VAR pos vpy;        VAR pos vpz;        vpx:=px2-px1;        !获取向量x1x2         vpx:=vec_nor(vpx);!单位化向量vpx        vpxy:=py-px1;!获取向量x1y1        vpxy:=vec_nor(vpxy);！单位化向量vpx        vpz:=vpx*vpxy;!向量vpx叉乘向量vpxy        vpy:=vpz*vpx;！向量vpz叉乘向量vpx                !将单位向量存入数组nrT             nrT{1,1}:=vpx.x;        nrT{1,2}:=vpy.x;        nrT{1,3}:=vpz.x;        nrT{2,1}:=vpx.y;        nrT{2,2}:=vpy.y;        nrT{2,3}:=vpz.y;        nrT{3,1}:=vpx.z;        nrT{3,2}:=vpy.z;        nrT{3,3}:=vpz.z;
        MatrixToRpy nrt,a,b,c;        !调用旋转矩阵转欧拉角函数ENDproc
PROC MatrixToRpy(num nrT{*,*},INOUT num nrA,INOUT num nrB,INOUT num nrC)        !Transform  Trafo-Matrix to RPY-Angle A, B, C        !T = Rot_z(A) * Rot_y(B) * Rot_x(C)        VAR num nrSinA;        VAR num nrCosA;        VAR num nrSinB;        VAR num nrAbsCosB;        VAR num nrSinC;        VAR num nrCosC;        nrA:=ATan2(nrT{2,1},nrT{1,1});        nrSinA:=Sin(nrA);        nrCosA:=Cos(nrA);        nrSinB:=-nrT{3,1};        nrAbsCosB:=nrCosA*nrT{1,1}+nrSinA*nrT{2,1};        !Value: -90 <:= B <:= +90 !!        nrB:=ATan2(nrSinB,nrAbsCosB);        nrSinC:=nrSinA*nrT{1,3}-nrCosA*nrT{2,3};        nrCosC:=-nrSinA*nrT{1,2}+nrCosA*nrT{2,2};        nrC:=ATan2(nrSinC,nrCosC);    ENDPROC
FUNC pos vec_nor(pos pos1)        VAR pos pos2;        pos2.x:=pos1.x/VectMagn(pos1);        pos2.y:=pos1.y/VectMagn(pos1);        pos2.z:=pos1.z/VectMagn(pos1);        RETURN pos2;    ENDFUNC




 

联系热线：18616561800 联系人：庾文科 联系地址：上海市宝山区富联一路98弄6号

技术和报价服务：星期一至星期六8：00-22：00 ABB 机器人专营店

返回
顶部