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

本公开实施例公开了一种相邻三关节相互平行的6自由度串联机器人的关节旋转角度确定方法，包括：建立第一空间坐标系；以第五关节和第六关节轴线的第二交点为原点建立第二空间坐标系；在6自由度串联机器人的初始位形下，6自由度串联机器人的第一至第六关节的旋转角度为0；确定6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；基于第二交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定第二交点在6自由度串联机器人的前四关节上的螺旋运动轨迹；根据螺旋运动轨迹、初始位姿和当前位姿确定6自由度串联机器人从初始位形移动到当前位形后，第一至第六关节的旋转角度。

Method and device for determining joint rotation angle of tandem robot

The disclosed embodiment discloses a method for determining the rotation angle of a 6-DOF serial robot with three adjacent joints parallel to each other, including: establishing a first spatial coordinate system; establishing a second spatial coordinate system with the second intersection point of the fifth joint and the sixth joint axis as the origin; and establishing a second spatial coordinate system under the initial configuration of a 6-DOF serial robot. The rotation angle of the first to sixth joints of the 6-DOF serial robot is 0; the initial position and attitude of the 6-DOF serial robot in the initial configuration are determined in the second space coordinate system relative to the first space coordinate system; and the first coordinate and the first coordinate relative to the first space coordinate system based on the second intersection point in the initial configuration and the current configuration. Two coordinates are used to determine the spiral trajectory of the second intersection point on the first four joints of the 6-DOF serial robot, and the rotation angles of the first to the sixth joints of the 6-DOF serial robot are determined according to the spiral trajectory, the initial posture and the current posture.

【技术实现步骤摘要】
串联机器人的关节旋转角度确定方法及装置
本公开涉及机器人识别
，具体涉及一种相邻三关节相互平行的6自由度串联机器人的关节旋转角度确定方法及装置。
技术介绍
随着传感器、人工智能、物联网以及大数据技术的发展，各种高级的机器人被用于工业生产，或者代替人类做一些重复性高或维修的工作。灵活性、可操作性是度量机器人能力的重要标准，多关节、高自由度使机器人的功能更强大，可以完成更多的任务。传统Denavit-Hatenberg(D-H)参数法是一种相对成熟的运动学建模方法，然而该方法具有明显缺陷，其参数不连续，导致奇异性问题。因此，该模型不适合机器人运动学标定。机器人运动学包含正向和逆向运动学两种。根据已知关节变量求得机器人末端执行器位姿的过程被视为正向运动学，而逆向运动学则是根据所要到达的目标位置来确定机器人所有关节的运动变量。由于工业机器人的广泛使用，6自由度机器人的逆运动学得到很多学者的关注。目前，主要采用Denavit-Hatenberg(D-H)参数法和旋量法建立机器人运动学模型。尽管D-H参数法比较成熟，但其存在一些不足。主要不足为在运动学标定中，当机器人有着相邻平行或几乎平行的关节时，该模型的参数不是连续的，导致模型奇异性。因此，D-H模型的实际参数很难辨识，并且该模型不适合机器人运动学标定。为了解决奇异性问题，提出了许多其它运动学模型，如S模型，CPC模型，零参考位置模型等。机器人运动学标定涉及建模、测量、辨识和补偿4个步骤，其目的是为了辨识机器人实际运动学参数从而提高机器人的执行精度。一个完美的运动学模型需同时满足完整性、连续性和非冗余性，然而以上列举的模型不满足该特性。幸运的是，指数积(productofexponentials，POE)方法作为一种新的运动学建模方法，避免了由运动学参数带来的奇异性问题。重要的是，基于指数积模型同时满足完整性、连续性和非冗余特性，被视为一种理想的运动学模型。因此，该模型非常适合机器人运动学标定，从而可有效解决机器人执行误差问题。目前，许多学者主要关注后三关节相交于一点的机器人的逆运动学，然而对三个相邻平行关节的机器人却关注较少。基于指数积的逆运动学通常根据已知的Paden-Kahan子问题进行求解，然而，这些子问题通常适用于低自由度器人。
技术实现思路
本公开实施例提供一种相邻三关节相互平行的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分别为第一至第六关节的单位运动旋量；a2和a3分别为第二关节2和第三关节3的连杆长度；d4为第四关节与第五关节之间的连杆偏置，d5为第五关节与第六关节之间的连杆偏置。可选地，基于所述第二交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第二交点在所述6自由度串联机器人的前四关节上的螺旋运动轨迹，包括：采用几何方法建立第二交点从初始位形到当前位形下在第一至第四关节上的螺旋运动模型；所述螺旋运动模型包括第二交点绕第四关节的轴线形成的第四轨迹圆，再绕第三关节的轴线形成的第三轨迹圆，再绕第二关节的轴线形成的第二轨迹圆，以及绕第一关节的轴线形成的第一轨迹圆；基于所述螺旋运动模型确定第一轨迹圆与第二轨迹圆之间的交点。可选地，根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形移动到当前位形后，所述第一至第六关节的旋转角度，包括：基于所述第一轨迹圆与第二轨迹圆之间的交点，以及所述第二交点分别在初始位形和当前位形下的第一坐标和第二坐标之间的指数积关系，确定所述6自由度串联机器人的第一关节从初始位形到当前位形的旋转角度；所述指数积关系是基于预先建立的所述6自由度串联机器人的指数积正向运动学模型确定的；基于所述第一关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第二至第六关节的旋转角度。可选地，所述第一轨迹圆、第二轨迹圆、第三轨迹圆和第四轨迹圆的圆心分别位于第一至第四关节的轴线上，且所述第二轨迹圆、第三轨迹圆和第四轨迹圆位于同一平面，而第一轨迹圆所在平面与第二轨迹圆、第三轨迹圆、第四轨迹圆所在平面垂直。可选地，第一轨迹圆和第二轨迹圆之间交点的坐标为：其中，p2为第一轨迹圆和第二轨迹圆之间交点，px,py,pz分别为所述第二交点在当前位形下在第一空间坐标系下的x、y、z轴坐标，d4为第四关节与第五关节之间的连杆偏置。可选地，所述指数积正向运动学模型如下表示：其中，gst(θ)为所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿，gst(0)为所述初始位姿；θ1，θ2，θ3，θ4，θ5，θ6分别为所述6自由度串联机器人的第一至第六关节在当前位形下相对于初始位形的旋转角度；分别为第一关节至第六关节的单位运动旋量；第一关节从初始位形到当前位形的旋转角度如下表示：其中，θ1分别为第一关节从初始位形到当前位形的旋转角度；px,py,pz分别为所述第二交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；d4为第四关节与第五关节之间的连杆偏置。可选地，基于所述第一关节从初始位形到当前位形的旋转角度、所述初始位姿、所述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分别为第一至第六关节的单位运动旋量；a2和a3分别为第二关节2和第三关节3的连杆长度；d4为第四关节与第五关节之间的连杆偏置，d5为第五关节与第六关节之间的连杆偏置。4.根据权利要求1所述的方法，其特征在于，基于所述第二交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第二交点在所述6自由度串联机器人的前四关节上的螺旋运动轨迹，包括：采用几何方法建立第二交点从初始位形到当前位形下在第一至第四关节上的螺旋运动模型；所述螺旋运动模型包括第二交点绕第四关节的轴线形成的第四轨迹圆，再绕第三关节的轴线形成的第三轨迹圆，再绕第二关节的轴线形成的第二轨迹圆，以及绕第一关节的轴线形成的第一轨迹圆；基于所述螺旋运动模型确定第一轨迹圆与第二轨迹圆之间的交点。5.根据权利要求4所述的方法，其特征在于，根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形移动到当前位形后，所述第一至第六关节的旋转角度，包括：基于所述第一轨迹圆与第二轨迹圆之间的交点，以及所述第二交点分别在初始位形和当前位形下的第一坐标和第二坐标之间的指数积关系，确定所述6自由度串联机器人的第一关节从初始位形到当前位形的旋转角度；所述指数积关系是基于预先建立的所述6自由度串联机器人的指数积正向运动学模型确定的；基于所述第一关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第二至第六关节的旋转角度。6.根据权利要求4所述的方法，其特征在于，所述第一轨迹圆、第二轨迹圆、第三轨迹圆和第四轨迹圆的圆心分别位于第一至第四关节的轴线上，且所述第二轨迹圆、第三轨迹圆和第四轨迹圆位于同一平面，而第一轨迹圆所在平面与第二轨迹圆、第三轨迹圆、第四轨迹圆所在平面垂直。7.根据权利要求6所述的方法，其特征在于，第一轨迹圆和第二轨迹圆之间交点的坐标为：其中，p2为第一轨迹圆...

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