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

kuka机器人集成商

产品:38    
联系我们
您当前的位置：首页 » » 库卡KUKA机器人：编程之欧拉角与四元数转换
产品分类
库卡KUKA机器人：编程之欧拉角与四元数转换
发布时间：2020-02-05        浏览次数：669        返回列表
 库卡KUKA机器人：编程之欧拉角与四元数转换

1. 库卡KUKA机器人在空间点位表示方法通常为X、Y、Z、A、B、C，其中A、B、C表示该点的旋转姿态(A/B/C分别对应Z/Y/X轴的旋转),也成为欧拉角。        
u=1798346908,2215464149&fm=26&gp=0
定义u=2683690912,309303897&fm=26&gp=0分别为绕Z、Y、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。


微信图片_20200131114540

2.四元数：

四元数(quaternion)可以看作中学时学的复数的扩充，它有三个虚部：Q=w+xi+yj+zk,
其中w,x,y,z又叫四元数的元素。

微信图片_20200202230330

3.库卡欧拉角转四元数：


 
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
important; overflow-wrap: break-word !important;">DEFFCT QUATERNION EulerZYXToQuaternion(ABC:IN )important; overflow-wrap: break-word !important;">DECL EulerZYX ABCimportant; overflow-wrap: break-word !important;">DECL QUATERNION Qimportant; overflow-wrap: break-word !important;">DECL REAL SINA,COSAimportant; overflow-wrap: break-word !important;">DECL REAL SINB,COSBimportant; overflow-wrap: break-word !important;">DECL REAL SINC,COSCimportant; overflow-wrap: break-word !important;">DECL INT Sumimportant; overflow-wrap: break-word !important;">IF VARSTATE("ABC")<>#INITIALIZED THENimportant; overflow-wrap: break-word !important;">   MsgQuit("Input Euler Angles not initialized!")important; overflow-wrap: break-word !important;">   LOOPimportant; overflow-wrap: break-word !important;">     HALTimportant; overflow-wrap: break-word !important;">   ENDLOOPimportant; overflow-wrap: break-word !important;">ENDIFimportant; overflow-wrap: break-word !important;">SINC=SIN(ABC.Rx*0.5)important; overflow-wrap: break-word !important;">COSC=COS(ABC.Rx*0.5)important; overflow-wrap: break-word !important;">SINB=SIN(ABC.Ry*0.5)important; overflow-wrap: break-word !important;">COSB=COS(ABC.Ry*0.5)important; overflow-wrap: break-word !important;">SINA=SIN(ABC.Rz*0.5)important; overflow-wrap: break-word !important;">COSA=COS(ABC.Rz*0.5)important; overflow-wrap: break-word !important;">Q.W=COSC*COSB*COSA+SINC*SINB*SINAimportant; overflow-wrap: break-word !important;">Q.X=SINC*COSB*COSA-COSC*SINB*SINAimportant; overflow-wrap: break-word !important;">Q.Y=COSC*SINB*COSA+SINC*COSB*SINAimportant; overflow-wrap: break-word !important;">Q.Z=COSC*COSB*SINA-SINC*SINB*COSAimportant; overflow-wrap: break-word !important;">Sum=Q.W*Q.W+Q.X*Q.X+Q.Y*Q.Y+Q.Z*Q.Zimportant; overflow-wrap: break-word !important;">IF Sum<>1 THENimportant; overflow-wrap: break-word !important;">   MsgQuit("Sum of Quaternion elements power NOT 1!")important; overflow-wrap: break-word !important;">   LOOPimportant; overflow-wrap: break-word !important;">     HALTimportant; overflow-wrap: break-word !important;">   ENDLOOPimportant; overflow-wrap: break-word !important;">ELSEimportant; overflow-wrap: break-word !important;">  RETURN (Q)important; overflow-wrap: break-word !important;">ENDIFimportant; overflow-wrap: break-word !important;">ENDFCT
4.四元数转化库卡欧拉角：

  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
important; overflow-wrap: break-word !important;">DEFFCT EulerZYX QuaternionToEulerZYX(Q:IN )important; overflow-wrap: break-word !important;">DECL EulerZYX Retimportant; overflow-wrap: break-word !important;">DECL QUATERNION Qimportant; overflow-wrap: break-word !important;">DECL INT Sumimportant; overflow-wrap: break-word !important;">DECL REAL Tempz1,Tempz2important; overflow-wrap: break-word !important;">DECL REAL Tempyimportant; overflow-wrap: break-word !important;">DECL REAL Tempx1,Tempx2important; overflow-wrap: break-word !important;">IF VARSTATE("Q")<>#INITIALIZED THENimportant; overflow-wrap: break-word !important;">  MsgQuit("Input Quaternion not initialized!")important; overflow-wrap: break-word !important;">  LOOPimportant; overflow-wrap: break-word !important;">    HALTimportant; overflow-wrap: break-word !important;">  ENDLOOPimportant; overflow-wrap: break-word !important;">ENDIFimportant; overflow-wrap: break-word !important;">Sum=Q.W*Q.W+Q.X*Q.X+Q.Y*Q.Y+Q.Z*Q.Zimportant; overflow-wrap: break-word !important;">IF Sum<>1 THENimportant; overflow-wrap: break-word !important;">   MsgQuit("Sum of Quaternion elements power NOT 1!")important; overflow-wrap: break-word !important;">   LOOPimportant; overflow-wrap: break-word !important;">     HALTimportant; overflow-wrap: break-word !important;">   ENDLOOPimportant; overflow-wrap: break-word !important;">ENDIFimportant; overflow-wrap: break-word !important;">Tempx1=2*(Q.Y*Q.Z+Q.W*Q.X)important; overflow-wrap: break-word !important;">Tempx2=Q.W*Q.W-Q.X*Q.X-Q.Y*Q.Y+Q.Z*Q.Zimportant; overflow-wrap: break-word !important;">Tempy=2*(Q.W*Q.Y-Q.X*Q.Z)important; overflow-wrap: break-word !important;">Tempz1=2*(Q.X*Q.Y+Q.W*Q.Z)important; overflow-wrap: break-word !important;">TempZ2=Q.W*Q.W+Q.X*Q.X-Q.Y*Q.Y-Q.Z*Q.Zimportant; overflow-wrap: break-word !important;">Ret.Rx=ATAN2(Tempx1,Tempx2)important; overflow-wrap: break-word !important;">Ret.Rz=ATAN2(Tempz1,tempz2)important; overflow-wrap: break-word !important;">Ret.Ry=90-ACOS(Tempy)important; overflow-wrap: break-word !important;">RETURN (RET)important; overflow-wrap: break-word !important;">ENDFCT
5.测试：

  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
  •  
important; overflow-wrap: break-word !important;">DEF FunctionTest( )important; overflow-wrap: break-word !important;">DECL QUATERNION Q1,Q2
 important; overflow-wrap: break-word !important;">DECL EulerZYX ZYX1,ZYX2important; overflow-wrap: break-word !important;">ZYX1={Rz 3,Ry 2,Rx 1}important; overflow-wrap: break-word !important;">Q2={W 0.5,X 0.5,Y 0.5,Z 0.5}important; overflow-wrap: break-word !important;">Q1=EulerZYXToQuaternion(ZYX1)important; overflow-wrap: break-word !important;">HALTimportant; overflow-wrap: break-word !important;">ZYX2=QuaternionToEulerZYX(Q2)important; overflow-wrap: break-word !important;">HALT important; overflow-wrap: break-word !important;">END                       更多信息请咨询：库卡KUKA机器人



   

分享到: 


 

联系热线：18521393525   联系人：黄敏 联系地址：上海市宝山区富联一路98弄6号

技术和报价服务：星期一至星期六8：00-22：00 kuka机器人集成商