串联机器人的关节旋转角度确定方法及装置制造方法及图纸

本发明专利技术公开了一种6自由度串联机器人的关节旋转角度确定方法及装置。所述方法包括：建立第一空间坐标系，第一空间坐标系的原点位于第一关节的轴线，且固定不动；以第一轴线交点为原点建立第二空间坐标系；确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；基于第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；根据所述螺旋运动轨迹、所述初始位姿以及当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。

Method and device for determining joint rotation angle of tandem robot

The invention discloses a method and a device for determining the joint rotation angle of a 6 degree of freedom serial robot. The method includes: establishing the first space coordinate system, the origin of the first space coordinate system is located on the axis of the first joint, and is fixed; establishing the second space coordinate system with the intersection point of the first axis as the origin; determining the 6-DOF serial robot in the initial configuration, the second space coordinate system relative to the first space coordinate system. Initial posture; based on the first and second coordinates of the intersection point of the first axis relative to the first spatial coordinate system in the initial and current configuration, the spiral motion of the 6-DOF serial robot on the first three joints of the 6-DOF serial robot is determined from the initial configuration to the current configuration, and the intersection point of the first axis on the first three joints of the 6-DOF serial robot. According to the spiral trajectory, the initial position and the current position, the rotation angles of the first to sixth joints of the 6-DOF serial robot from the initial position to the current position are determined.

【技术实现步骤摘要】
串联机器人的关节旋转角度确定方法及装置
本公开涉及机器人
，尤其涉及一种6自由度串联机器人的关节旋转角度确定方法及装置。
技术介绍
机器人运动学分为正向和逆向运动学。通过给定各关节角度来计算机器人末端执行器的位置和方向，称为正向运动学。与此相反，逆向运动学是通过给定的位姿求出各关节对应角度，通常具有多个解。由于机器人逆运动学求解相对较难，并且关系到机器人的运动规划和实时性能，因此，逆运动学的研究更加重要。目前，常用的机器人运动学建模方法主要有D-H参数法和旋量法。尽管在机器人运动学建模中，D-H参数法广泛使用，但是当采用该方法对相邻关节平行或几乎平行的机器人进行运动学标定时，该方法参数会产生奇异性问题。由于运动学标定要求参数连续性，所以D-H模型不适合机器人运动学标定，因而机器人的执行误差得不到很好地校正。针对该问题，许多学者尝试去改进或提出其它运动学模型，然而大多数模型不能同时满足连续性、最小性和完整性的要求。为了能够更好地对机器人进行运动学标定，基于旋量理论的指数积(POE)公式开始被很多学者广泛使用。与D-H方法相比，指数积(POE)方法具有许多优势。该方法只需建立惯性和工具两个坐标系，并且其运动学参数变化光滑，克服了运动学标定中存在的奇异性问题。除此之外，该方法对机器人的旋转和平移关节的处理是统一的。
技术实现思路
本公开一方面提供了一种6自由度后三关节相交于一点的串联机器人的关节旋转角度确定方法及装置。第一方面，本公开实施例提供了一种6自由度串联机器人的关节旋转角度确定方法，所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；包括：建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。可选地，确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿，包括：根据第二空间坐标系三个坐标轴分别相对于第一空间坐标系的三个坐标轴的方向余弦、第二空间坐标系和第一坐标系两原点位置矢量确定第二空间坐标系相对于第一空间坐标系下，所述6自由度串联机器人的初始位姿。可选地，所选取的第一点、第二点、第三点、第一轴线交点在初始位形下的坐标分别如下：其中，r1、r2、r3分别为第一点、第二点和第三点在初始位形下的坐标；r4、r5和r6分别为第一轴线交点的坐标；第一至第六关节的轴线方向单位矢量如下：其中，ω1,ω2,ω3,ω4,ω5,ω6分别为第一至第六关节的轴线方向单位矢量；第一至第六关节的单位运动旋量如下：其中，ξ1,ξ2,ξ3,ξ4,ξ5和ξ6分别为第一至第六关节的单位运动旋量。可选地，基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标、所述初始位姿，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹，包括：采用几何方法建立第一轴线交点从初始位形到当前位形下在第一至第三关节上的螺旋运动模型；所述螺旋运动模型包括第一轴线交点绕第三关节的轴线形成的第三轨迹圆，再绕第二关节的轴线形成的第二轨迹圆，以及绕第一关节的轴线形成的第一轨迹圆；基于所述螺旋运动模型确定第一轨迹圆至第三轨迹圆之间的交点。可选地，根据所述螺旋运动轨迹确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度，包括：基于所述第一轨迹圆至第三轨迹圆之间的交点，以及所述第一轴线交点分别在初始位形和当前位形下的第一坐标和第二坐标之间的指数积关系，确定所述6自由度串联机器人的第一至第三关节从初始位形到当前位形的旋转角度；所述指数积关系是基于预先建立的所述6自由度串联机器人的指数积正向运动学模型确定的；基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度。可选地，所述第一轨迹圆、第二轨迹圆和第三轨迹圆的圆心分别位于第一至第三关节的轴线上，且所述第二轨迹圆和第三轨迹圆位于同一平面，而第一轨迹圆所在平面与第二轨迹圆和第三轨迹圆所在平面垂直。可选地，第一轨迹圆和第二轨迹圆相交于第一交点，第二轨迹圆和第三轨迹圆相交于第二交点，所述第一交点的坐标为：其中，p2为第一交点，px,py,pz分别为所述第一轴线交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；第二交点的坐标如下表示：y5＝y2±m1,其中，p3为第二交点，y2、z2分别为第二轨迹圆圆心的y、z轴坐标，y2、z3分别为第三轨迹圆圆心的y、z轴坐标；b、c分别为第一轴线交点的第一坐标的y、z轴坐标。可选地，所述指数积正向运动学模型如下表示：其中，gst(θ)为所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿，gst(0)为所述初始位姿；θ1，θ2，θ3，θ4，θ5，θ6分别为所述6自由度串联机器人的第一至第六关节在当前位形下相对于初始位形的旋转角度；分别为第一关节至第六关节的单位运动旋量；第一至第三关节从初始位形到当前位形的旋转角度如下表示：θ2＝atan2(m2,m3)θ3＝atan2(m4,m5)m4＝(z3-c)(y5-y2)+(b–y2)(z5-z3)m5＝(b–y2)(y5–y2)+(c-z3)(z5-z3)其中，θ1、θ2、θ3分别为第一至第三关节从初始位形到当前位形的旋转角度；px,py,pz分别为所述第一轴线交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；y2、z2分别为第二轨迹圆圆心的y、z轴坐标，y2、z3分别为第三轨迹圆圆心的y、z轴坐标；b、c分别为第一轴线交点在初始位形下的y、z轴坐标；y5、z5分别为第二轨迹圆和第三轨迹圆的第二交点的y、z轴坐标。可选地，基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度，包括：在所述指数积正向运动学模型的方程两边依次同乘以gst-1(0)可得：用和分别同乘方程(a)两边，可得：其中，gst(θ)为所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿，gst(0)为所述初始位姿；θ1，θ2，θ3，θ4，θ5，θ6分别为所述6自由度串联机器人的第一至第六关节在当前位形下相对于初始位形的旋转角度；分别为第一关节至第六关节的单位运动旋量；在第六关节的轴本文档来自技高网...

【技术保护点】
1.一种6自由度串联机器人的关节旋转角度确定方法，所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；其特征在于，包括：建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。

【技术特征摘要】
1.一种6自由度串联机器人的关节旋转角度确定方法，所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；其特征在于，包括：建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。2.根据权利要求1所述的方法，其特征在于，确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿，包括：根据第二空间坐标系三个坐标轴分别相对于第一空间坐标系的三个坐标轴的方向余弦、第二空间坐标系和第一坐标系两原点位置矢量确定第二空间坐标系相对于第一空间坐标系下，所述6自由度串联机器人的初始位姿。3.根据权利要求2所述的方法，其特征在于，所选取的第一点、第二点、第三点、第一轴线交点在初始位形下的坐标分别如下：其中，r1、r2、r3分别为第一点、第二点和第三点在初始位形下的坐标；r4、r5和r6分别为第一轴线交点的坐标；第一至第六关节的轴线方向单位矢量如下：其中，ω1,ω2,ω3,ω4,ω5,ω6分别为第一至第六关节的轴线方向单位矢量；第一至第六关节的单位运动旋量如下：其中，ξ1,ξ2,ξ3,ξ4,ξ5和ξ6分别为第一至第六关节的单位运动旋量。4.根据权利要求1所述的方法，其特征在于，基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标、所述初始位姿，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹，包括：采用几何方法建立第一轴线交点从初始位形到当前位形下在第一至第三关节上的螺旋运动模型；所述螺旋运动模型包括第一轴线交点绕第三关节的轴线形成的第三轨迹圆，再绕第二关节的轴线形成的第二轨迹圆，以及绕第一关节的轴线形成的第一轨迹圆；基于所述螺旋运动模型确定第一轨迹圆至第三轨迹圆之间的交点。5.根据权利要求1所述的方法，其特征在于，根据所述螺旋运动轨迹确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度，包括：基于所述第一轨迹圆至第三轨迹圆之间的交点，以及所述第一轴线交点分别在初始位形和当前位形下的第一坐标和第二坐标之间的指数积关系，确定所述6自由度串联机器人的第一至第三关节从初始位形到当前位形的旋转角度；所述指数积关系是基于预先建立的所述6自由度串联机器人的指数积正向运动学模型确定的；基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度。6.根据权利要求4所述的方法，其特征在于，所述第一轨迹圆、第二轨迹圆和第三轨迹圆的圆心分别位于第一至第三关节的轴线上，且所述第二轨迹圆和第三轨迹圆位于同一平面，而第一轨迹圆所在平面与第二轨迹圆和第三轨迹圆所在平面垂直。7.根据权利要求6所述的方法，其特征在于，第一轨迹圆和第二轨迹圆相交于第一交点，第二轨迹圆和第三轨迹圆相交于第二交点，所述第一交点的坐标为：其中，p2为第一交点，px,py,pz分别为所述第一轴线交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；第二交点的坐标如下表示：其中...

【专利技术属性】
技术研发人员：施智平，赵荣波，关永，张倩颖，王国辉，邵振洲，王瑞，李晓娟，
申请(专利权)人：首都师范大学，
类型：发明
国别省市：北京,11