CN108406770A - 串联机器人的关节旋转角度确定方法及装置 - Google Patents

Abstract

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

Description

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

技术领域

本公开涉及机器人技术领域，尤其涉及一种6自由度串联机器人的关节旋转角度确定方法及装置。

背景技术

机器人运动学分为正向和逆向运动学。通过给定各关节角度来计算机器人末端执行器的位置和方向，称为正向运动学。与此相反，逆向运动学是通过给定的位姿求出各关节对应角度，通常具有多个解。由于机器人逆运动学求解相对较难，并且关系到机器人的运动规划和实时性能，因此，逆运动学的研究更加重要。目前，常用的机器人运动学建模方法主要有D-H参数法和旋量法。尽管在机器人运动学建模中，D-H参数法广泛使用，但是当采用该方法对相邻关节平行或几乎平行的机器人进行运动学标定时，该方法参数会产生奇异性问题。由于运动学标定要求参数连续性，所以D-H模型不适合机器人运动学标定，因而机器人的执行误差得不到很好地校正。针对该问题，许多学者尝试去改进或提出其它运动学模型，然而大多数模型不能同时满足连续性、最小性和完整性的要求。

为了能够更好地对机器人进行运动学标定，基于旋量理论的指数积(POE)公式开始被很多学者广泛使用。与D-H方法相比，指数积(POE)方法具有许多优势。该方法只需建立惯性和工具两个坐标系，并且其运动学参数变化光滑，克服了运动学标定中存在的奇异性问题。除此之外，该方法对机器人的旋转和平移关节的处理是统一的。

发明内容

本公开一方面提供了一种6自由度后三关节相交于一点的串联机器人的关节旋转角度确定方法及装置。

第一方面，本公开实施例提供了一种6自由度串联机器人的关节旋转角度确定方法，所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；包括：

建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；

以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；

确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；

基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；

根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。

可选地，确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿，包括：

根据第二空间坐标系三个坐标轴分别相对于第一空间坐标系的三个坐标轴的方向余弦、第二空间坐标系和第一坐标系两原点位置矢量确定第二空间坐标系相对于第一空间坐标系下，所述6自由度串联机器人的初始位姿。

可选地，所选取的第一点、第二点、第三点、第一轴线交点在初始位形下的坐标分别如下：

其中，r₁、r₂、r₃分别为第一点、第二点和第三点在初始位形下的坐标；r₄、r₅和r₆分别为第一轴线交点的坐标；

第一至第六关节的轴线方向单位矢量如下：

其中，ω₁,ω₂,ω₃,ω₄,ω₅,ω₆分别为第一至第六关节的轴线方向单位矢量；

第一至第六关节的单位运动旋量如下：

其中，ξ₁,ξ₂,ξ₃,ξ₄,ξ₅和ξ₆分别为第一至第六关节的单位运动旋量。

可选地，基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标、所述初始位姿，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹，包括：

采用几何方法建立第一轴线交点从初始位形到当前位形下在第一至第三关节上的螺旋运动模型；所述螺旋运动模型包括第一轴线交点绕第三关节的轴线形成的第三轨迹圆，再绕第二关节的轴线形成的第二轨迹圆，以及绕第一关节的轴线形成的第一轨迹圆；

基于所述螺旋运动模型确定第一轨迹圆至第三轨迹圆之间的交点。

可选地，根据所述螺旋运动轨迹确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度，包括：

基于所述第一轨迹圆至第三轨迹圆之间的交点，以及所述第一轴线交点分别在初始位形和当前位形下的第一坐标和第二坐标之间的指数积关系，确定所述6自由度串联机器人的第一至第三关节从初始位形到当前位形的旋转角度；所述指数积关系是基于预先建立的所述6自由度串联机器人的指数积正向运动学模型确定的；

基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度。

可选地，所述第一轨迹圆、第二轨迹圆和第三轨迹圆的圆心分别位于第一至第三关节的轴线上，且所述第二轨迹圆和第三轨迹圆位于同一平面，而第一轨迹圆所在平面与第二轨迹圆和第三轨迹圆所在平面垂直。

可选地，第一轨迹圆和第二轨迹圆相交于第一交点，第二轨迹圆和第三轨迹圆相交于第二交点，所述第一交点的坐标为：

其中，p₂为第一交点，p_x,p_y,p_z分别为所述第一轴线交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；

第二交点的坐标如下表示：

y₅＝y₂±m₁,

其中，p₃为第二交点，y₂、z₂分别为第二轨迹圆圆心的y、z轴坐标，y₂、z₃分别为第三轨迹圆圆心的y、z轴坐标；b、c分别为第一轴线交点的第一坐标的y、z轴坐标。

可选地，所述指数积正向运动学模型如下表示：

其中，g_st(θ)为所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿，g_st(0)为所述初始位姿；θ₁，θ₂，θ₃，θ₄，θ₅，θ₆分别为所述6自由度串联机器人的第一至第六关节在当前位形下相对于初始位形的旋转角度；分别为第一关节至第六关节的单位运动旋量；

第一至第三关节从初始位形到当前位形的旋转角度如下表示：

θ₂＝atan2(m₂,m₃)

θ₃＝atan2(m₄,m₅)

m₄＝(z₃-c)(y₅-y₂)+(b–y₂)(z₅-z₃)

m₅＝(b–y₂)(y₅–y₂)+(c-z₃)(z₅-z₃)

其中，θ₁、θ₂、θ₃分别为第一至第三关节从初始位形到当前位形的旋转角度；p_x,p_y,p_z分别为所述第一轴线交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；y₂、z₂分别为第二轨迹圆圆心的y、z轴坐标，y₂、z₃分别为第三轨迹圆圆心的y、z轴坐标；b、c分别为第一轴线交点在初始位形下的y、z轴坐标；y₅、z₅分别为第二轨迹圆和第三轨迹圆的第二交点的y、z轴坐标。

可选地，基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度，包括：

在所述指数积正向运动学模型的方程两边依次同乘以g_st ^-1(0)可得：

用和分别同乘方程(a)两边，可得：

其中，g_st(θ)为所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿，g_st(0)为所述初始位姿；

θ₁，θ₂，θ₃，θ₄，θ₅，θ₆分别为所述6自由度串联机器人的第一至第六关节在当前位形下相对于初始位形的旋转角度；分别为第一关节至第六关节的单位运动旋量；

在第六关节的轴线上选择一点p₆，且该点p₆不在第四关节和第五关节的轴线上；

基于位置不变原则，在上述方程(b)两边同乘p₆，则得到：

其中，b、c分别为第一轴线交点在初始位形下的y、z轴坐标；

求解方程(d)，得到第四至第五关节在当前位形下相对于初始位形的旋转角度：

θ₄＝atan2(m₆,m₇)

其中，m₆＝-t₁/cosθ₅，m₇＝(c-t₃)/cosθ₅(cosθ₅≠0)；

同理，用分别同乘方程(b)两边，可得：

选择不在ξ₆轴线上的一点p₇；

在方程(e)两边同乘点p₇，可得：

求解方程(f)，可得第六关节在当前位形下相对于初始位形的旋转角度：

θ₆＝atan2(-t₄,t₅-b)。

第二方面，本公开实施例提供了一种6自由度串联机器人的关节旋转角度确定装置，所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；包括：

第一建立模块，被配置为建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；

第二建立模块，被配置为以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；

第一确定模块，被配置为确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；

第二确定模块，被配置为基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；

第三确定模块，被配置为根据所述螺旋运动轨迹、所述初始位姿以及当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。

本公开实施例通过在机器人第一关节固定的位置处建立固定不动的第一空间坐标系，并在后三关节相交于一点处建立第二空间坐标系，并采用指数积(product ofexponentials，POE)方法建立运动学模型，并采用几何、代数方法和Paden-Kahan子问题进行逆运动学求解，改进了已有技术中需要在每个关节都建立坐标系求解关节旋转角度的方法。本公开实施例中几何方法的运用是为了获得前三关节运动轨迹的交点，然后将求解转化成已知的Paden-Kahan子问题，使几何意义更加清晰、直观，并且大大简化了计算。采用代数消元和MATLAB结合的方法去求解第四、五关节，减少了人工计算，有效避免了由复杂计算产生的错误。算法可求得一组封闭的逆运动学的解，实验得出的逆解最大位姿误差为10^-12数量级，证明了本公开实施例提出的算法具有很高的精度。

附图说明

图1是根据本公开示例性实施例的6自由度串联机器人的关节旋转角度确定方法流程图；

图2示出了一种第一空间坐标系和第二空间坐标系的建立示例。

图3和图4分别是根据本公开示例性实施例中点r₄基于螺旋运动原理在前三关节中的螺旋运动对应的两种情形示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

机器人采用串联式开链结构，即机器人各连杆由旋转关节或移动关节串联连接。连杆的一端装在固定的支座上(底座)，另一端处于自由状态，可安装各种工具以实现机器人作业。关节的作用是使相互联接的两个连杆产生相对运动。关节的传动采用模块化结构，由锥齿轮、同步齿型带和谐波减速器等多种传动结构配合实现。机器人各关节采用伺服电机和步进电机混合驱动，并通过Windows环境下的软件编程和运动控制卡实现对机器人的控制，使机器人能够在工作空间内任意位置精确定位。

一般来说，刚体运动可由特殊欧式群SE(3)中的4×4齐次变换矩阵来表示。该矩阵由旋转和平移两部分组成，其中旋转部分为3×3矩阵，表示刚体的姿态；平移部分为3×1矩阵，表示刚体的位置矢量。根据Chasles定理(参见MURRAY R M,SASTRY S S,LI Z.AMathematical Introduction to Robotic Manipulation[M].CRC Press,Inc.,1994)，刚体绕着某一定轴的旋转和沿着该轴的平移可以完成其从某一位置到另一位置的运动，该运动称为螺旋运动，并且其无穷小量属于李代数元素，即运动旋量。运动旋量的两种表示形式如下：

式中，为4×4矩阵，ξ为6维矢量，ω和υ分别是旋转角速度和平移线速度，为ω的3×3的斜对称矩阵。

用g_st(0)表示刚体相对于第一空间坐标系的初始位姿，当刚体转动或平移θ后，刚体的最终位姿为

对于只有旋转关节的机器人，当给定ξ和实数θ，则的指数形式可表示如下：

其中，I是单位矩阵；

根据指数积公式，n自由度串联机器人的末端执行器位姿可表示为

式中，g_st(0)和g_st(θ)分别为末端执行器初始位姿和最终位姿。

本公开实施例提出了一种6自由度串联机器人的关节旋转角度确定方法。所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；如图1所示，该方法包括如下步骤：

在步骤S101中，建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；

在步骤S102中，以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；

在步骤S103中，确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；

在步骤S104中，基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；

在步骤S105中，根据所述螺旋运动轨迹、所述初始位姿以及当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。

在下文中将对步骤S101、S102、S103、S104和S105分别做进一步的描述。

步骤S101和步骤S102：

在本公开实施例中，图2示出了6自由度后三关节相交于一点的串联机器人在初始位形下的结构参数和旋量坐标系。由图2可知，机器人的6个关节都是旋转关节此外，机器人的后三关节即第四、五和六关节的关节轴线相交于第一轴线交点上，满足Piper准则具有封闭形式的解。

S和T分别为第一空间坐标系和第二空间坐标系，第一空间坐标系S的原点o位于第一关节1的轴线，且第一空间坐标系S为惯性坐标系，固定不动；通常情况下，第一关节1固定在机器人的基座上，可选地，第一空间坐标系S可以建立在基座上，或者建立在固定第一关节1的部件上。

图2示出了一种第一空间坐标系和第二空间坐标系的建立示例，其中第一空间坐标系S的原点位于第一关节1轴线与第一关节末端平面的交点。

图2中第一空间坐标系S的xy平面位于第一关节1的末端平面，当然可以理解的是，在其他实施例中第一空间坐标系S的原点o在第一关节1的轴线上即可，而x、y、z轴的方向可根据实际情况选择；第二空间坐标系T的原点与后三关节即第四、五和六关节的第一轴线交点重合，第一轴线交点在初始位形下用r₄表示。

图2中第二空间坐标系T的x、y、z轴分别与后三关节的轴线重合，且与第一空间坐标系S的x、y、z轴方向一致。

6自由度后三关节相交于一点的串联机器人在初始位形下，各个关节的旋转角度为0。

可选地，所述第一空间坐标系的z轴与第一关节的轴线重合；所述第二空间坐标系的z轴与第六关节的轴线重合。

步骤S103：

步骤S103进一步包括：根据第二空间坐标系三个坐标轴分别相对于第一空间坐标系的三个坐标轴的方向余弦、第二空间坐标系和第一坐标系两原点位置矢量确定第二空间坐标系相对于第一空间坐标系下，所述6自由度串联机器人的初始位姿。

在一实施例中，步骤S103可通过如下步骤实现：

设第二空间坐标系的x坐标轴相对第一空间坐标系的x，y，z坐标轴的夹角分别为θ₁₁，θ₂₁，θ₃₁，第二空间坐标系的y坐标轴相对第一空间坐标系的x，y，z坐标轴的夹角分别为θ₁₂，θ₂₂，θ₃₂，第二空间坐标系的z坐标轴分别相对第一空间坐标系的x，y，z坐标轴的夹角分别为θ₁₃，θ₂₃，θ₃₃，且第二空间坐标系原点相对于第一空间坐标系的坐标为(c₁，c₂，c₃)^T，则6自由度串联机器人的初始位姿可如下表示：

在一实施例中，可以以图2示出的结构参数和旋转变量为例说明初始位姿的解算过程。

如图2所示，选取初始位形下第一关节轴线上任意一点作为第一点r₁，选取初始位形下第二关节轴线上任意一点作为第二点r₂；选取初始位形下第三关节轴线上任意一点作为第三点r₃。可见第二空间坐标系的x轴分别相对第一空间坐标系的x，y，z轴的夹角为0°，90°，90°，即θ₁₁＝0°，θ₂₁＝90°，θ₃₁＝90°，则cosθ₁₁＝1，cosθ₂₁＝0，cosθ₃₁＝0；第二空间坐标系的y轴分别相对第一空间坐标系的x，y，z轴的夹角为90°，0°，90°，即θ₁₂＝90°，θ₂₂＝0°，θ₃₂＝90°，则cosθ₁₂＝0，cosθ₂₂＝1，cosθ₃₂＝0；第二空间坐标系的z轴分别相对第一空间坐标系的x，y，z轴的夹角为90°，90°，0°，即θ₁₃＝90°，θ₂₃＝90°，θ₃₃＝0°，则cosθ₁₃＝0，cosθ₂₃＝0，cosθ₃₃＝1。

ω₁,ω₂,ω₃,ω₄,ω₅,ω₆分别为第一至第六关节的轴线方向单位矢量，ξ₁,ξ₂,ξ₃,ξ₄,ξ₅和ξ₆分别为第一至第六关节的单位运动旋量。r₁，r₂和r₃分别取前三关节即第一至第三关节的关节轴线上的一点，r₄，r₅和r₆分别取后三关节即第四至第六关节的关节轴线的交点，初始位形下，如下表示：

根据第一至第六关节的轴线方向单位矢量和第一至第六关节轴线上的上述点r₁、r₂、r₃、r₄、r₅和r₆，以及运动旋量公式可计算得到各关节单位运动旋量如下：

其中，b,c分别为第四至第六关节的轴线交点在第一空间坐标系下的r₄、r₅和r₆的y、z坐标；x₂,y₂,z₂为第二关节2轴线上选取的点r₂在第一空间坐标系下的x、y、z坐标；z₃为第三关节3轴线上选取的点r₃的z坐标。

如图2所示，在初始位形下，第二空间坐标系的原点相对第一空间坐标系的x坐标为0，即c₁＝0，且y、z轴坐标分别为b，c即c₂＝b，c₃＝c。

综上，从图2示出6自由度串联机器人，可得第二空间坐标系相对于第一空间坐标系机器人初始位姿为：

步骤S104

步骤S104进一步包括：

在步骤S1041中，基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标、所述初始位姿，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹，包括：

在步骤S1042中，采用几何方法建立第一轴线交点从初始位形到当前位形下在第一至第三关节上的螺旋运动模型；所述螺旋运动模型包括第一轴线交点绕第三关节的轴线形成的第三轨迹圆，再绕第二关节的轴线形成的第二轨迹圆，以及绕第一关节的轴线形成的第一轨迹圆；

在步骤S1043中，基于所述螺旋运动模型确定第一轨迹圆至第三轨迹圆之间的交点。

基于指数积(POE)原理，可以建立6自由度串联机器人的正向运动学模型如下：

其中，g_st(0)表示6自由度串联机器人相对于第一空间坐标系S的初始位姿，g_st(θ)为6自由度串联机器人从初始位姿开始旋转θ之后得到的当前位姿，即最终位姿；θ₁，θ₂，θ₃，θ₄，θ₅，θ₆分别为待求解的6自由度串联机器人的第一至第六关节在当前位姿下相对于初始位姿的旋转角度。

在方程(9)两边同右乘g_st ^-1(0)可得

根据位置不变原则，用后三关节轴线交点r₄右乘方程(10)，并化简方程可以得到：

p₁为6自由度串联机器人在当前位姿下后三关节轴线的交点的齐次坐标；p_x,p_y,p_z分别为点p₁在第一空间坐标系下的x、y、z轴坐标。

方程(11)表示第一轴线交点r₄在前三关节中的螺旋运动，一般来说，刚体运动可由特殊欧式群SE(3)中的4×4齐次变换矩阵来表示。该矩阵由旋转和平移两部分组成，其中旋转部分为3×3矩阵，表示刚体的姿态；平移部分为3×1矩阵，表示刚体的位置矢量。根据Chasles定理(参见MURRAY R M,SASTRY S S,LI Z.A Mathematical Introduction toRobotic Manipulation[M].CRC Press,Inc.,1994)，刚体绕着某一定轴的旋转和沿着该轴的平移可以完成其从某一位置到另一位置的运动，该运动称为螺旋运动。

因此，基于螺旋运动原理，点r₄在前三关节中的螺旋运动对应有两种情形，分别如图3和图4所示。

在图3和图4中，轨迹圆o3、o2、o1分别为初始位形下后三关节的交点r₄依次绕第三关节3轴线、第二关节轴线2和第一关节轴线1到达当前位姿下的p₁后形成的三个轨迹圆；轨迹圆o2和轨迹圆o3位于同一平面，轨迹圆o1所在平面与轨迹圆o2和o3所在平面垂直；o₁,o₂和o₃点分别为轨迹圆o1、o2和o3的圆心，分别位于第一关节1轴线、第二关节轴线2和第三关节轴线3上。图3和图4为交点r₄向两个相反方向旋转时的两种情形；p₂₁和p₂₂分别为图3和图4中轨迹圆o1和轨迹圆o2的两个不同交点。由机器人几何结构可知，轨迹圆o1与轨迹圆o2不能同时相交于两点。p₃₁，p₃₂，p₃₃和p₃₄分别为图3和图4中轨迹圆o2和轨迹圆o3的交点。图3和图4中的x轴、y轴和z轴分别为对应于图2中第一空间坐标系S的三个坐标轴。需要说明的是，为了便于理解，在图3和图4中将o₁点与第一空间坐标系S的原点o重合。实际上，轨迹圆o1的圆心o₁不一定在第一空间坐标系原点o上，其可以是第一关节1轴线上的某一点，但它不会影响点的坐标的求解。

正如图3和图4所示，在初始位形下，首先，后三关节轴线之间的交点r₄通过绕第三关节3的轴线ξ₃旋转角度θ₃运动到p₃₁点(或p₃₂,p₃₃,p₃₄)，然后，该点通过绕第二关节2的轴线ξ₂旋转角度θ₂运动到p₂₁点(或p₂₂)。最终，该点通过绕第一关节1的轴线ξ₁旋转角度θ₁运动到当前位姿下的p₁点。

当前位形和初始位形下后三关节轴线的交点r₄和p₁是已知的，为了能够简单、直观地求出前三关节角度变量，则可以首先计算出各个轨迹圆的交点，即p₂₁,p₂₂,p₃₁,p₃₂,p₃₃和p₃₄。

各个轨迹圆的交点计算过程如下：

假设轨迹圆o1是在平面α内，轨迹圆o3在平面β内，由于关节2和关节3的轴线是平行的，因此轨迹圆o2也在平面β内。由图3和图4可知，ξ₁轴和第一空间坐标系S的z轴是重合的。因o₁和p₁两点都在平面α内，且第一空间坐标系S的z轴垂直于平面α,所以o₁和p₁的z坐标相等，都为p_z。又因为o₁点为第一空间坐标系S的z轴上的一点，所以o₁点的x和y坐标都为0。由此可得，o₁点的坐标为(0,0,p_z)。

同理，因第一空间坐标系S的z轴垂直于平面α，且p₁,p₂₁和p₂₂三点都在平面α内，所以p₁,p₂₁和p₂₂点的z坐标相同，都为p_z。此外，由于第一空间坐标系的y轴与平面α位于同一平面，因此p₂₁和p₂₂两点都在第一空间坐标系S的y轴上或两点的连线平行于y轴且都在平面β内，则该两点的x坐标相同，都为0，且p₁,p₂₁和p₂₂三点都在轨迹圆o1上，则p₂₁和p₂₂两点的y坐标分别为||o₁-p₁||和-||o₁-p₁||。综上，可以很容易得到p₂₁和p₂₂两点坐标，其值为

由图3和4知，ξ₂和ξ₃轴线平行于第一空间坐标系S的x轴，且都垂直于平面β。此外，r₄,p₃₁,p₃₂,p₃₃,p₃₄,o₂和o₃点都在β内，则结合图2的几何关系可知，这些点的x坐标是相同的，都为0。根据机器人的特殊结构和所建第一空间坐标系之间的关系，可以很容易确定o₂和o₃两点的坐标：

为了描述方便，用p₂表示轨迹圆o1与轨迹圆o2的交点p₂₁和p₂₂，用p₃表示轨迹圆o2和轨迹圆o3的交点p₃₁,p₃₂,p₃₃和p₃₄。根据图3和图4特殊几何关系建立方程如下：

由方程(14)可得p₃点坐标为

式中，y₅＝y₂±m₁,

通过求解可知，p₃点坐标有4个解，因此，可以得到对应p₃₁,p₃₂,p₃₃和p₃₄四点坐标。

步骤S105

步骤S105进一步包括：

在步骤S1051中，基于所述第一轨迹圆至第三轨迹圆之间的交点，以及所述第一轴线交点分别在初始位形和当前位形下的第一坐标和第二坐标之间的指数积关系，确定所述6自由度串联机器人的第一至第三关节从初始位形到当前位形的旋转角度；所述指数积关系是基于预先建立的所述6自由度串联机器人的指数积正向运动学模型确定的；

在步骤S1052中，基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度。

根据图3、图4以及方程(11)可知，前三关节螺旋运动表示如下：

方程(16)由三个子问题构成，根据Paden-Kahan子问题1，可以求得前三关节角度变量值为：

θ₂＝atan2(m₂,m₃) (18)

θ₃＝atan2(m₄,m₅) (19)

式中，

m₄＝(z₃-c)(y₅–y₂)+(b–y₂)(z₅-z₃),m₅＝(b–y₂)(y₅–y₂)+(c-z₃)(z₅-z₃)

从图3和图4中，可以清晰、直观地看出前三关节角度变量的解共有4种组合，如下所示：

(θ₁₁,θ₂₁,θ₃₁)；(θ₁₁,θ₂₂,θ₃₂)；(θ₁₂,θ₂₃,θ₃₃)；(θ₁₂,θ₂₄,θ₃₄)。

步骤S1052进一步包括：

在步骤S10521中，在所述指数积正向运动学模型的方程(9)两边依次同乘以g_st ^-1(0)可得：

在步骤S10522中，用和分别左乘方程(10)，得到

在步骤S10523中，选择一点p₆，使得其在第六关节6的轴线ξ₆上但不在第四关节和第五关节的轴线ξ₄和ξ₅上，该点初始位形下的坐标为

p₆＝(0,b,0,1)^T (21)

在步骤S10524中，基于位置不变原则，用点p₆右乘方程(20)，可以得：

应用方程(4)和(5)，并借助MATLAB工具化简方程(22)，可得

在步骤S10525中，采用消元法求解方程(23)，得到

式中，m₆＝-t₁/cosθ₅,m₇＝(c-t₃)/cosθ₅(cosθ₅≠0).

在步骤S10526中，同理，用分别左乘方程(20)，则

在步骤S10527中，选择不在ξ₆轴线上的一点p₇，其坐标为

p₇＝(0,b+1,0,1)^T (26)

在步骤S10528中，用p₇点右乘方程(25)，则

在步骤S10529中，根据Paden-Kahan子问题1可以得到方程(27)的解为

θ₆＝atan2(-t₄,t₅-b) (28)

可见，本公开实施例提出的6自由度串联机器人在当前位姿下的关节角度已计算得到。本公开通过几何方法的巧妙运用，简化了逆解运算，加深了对多解组合的理解，有助于选择合适的逆解。

本公开还提出了一种6自由度串联机器人的关节旋转角度确定装置，包括：

第一建立模块，被配置为建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；

第二建立模块，被配置为以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；

第一确定模块，被配置为确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；

第二确定模块，被配置为基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；

第三确定模块，被配置为根据所述螺旋运动轨迹、所述初始位姿以及当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。

该装置与上述6自由度串联机器人的关节旋转角度确定方法对应一致，具体细节可参见对上述方法的描述，在此不再赘述。

下面通过一具体实验验证本公开实施例提出的上述方法。

“钱江一号”工业机器人满足本公开实施例所提出的机器人的结构。为了验证本公开实施例提出的方法的正确性，将该机器人作为一个实例进行逆运动学求解，计算得到机器人在当前位姿下的关节角度。算法验证过程如下：

(1)在机器人工作空间内，当前位姿下任意给出6个关节角度。

θ₁＝100，θ₂＝50，θ₃＝-60，θ₄＝180，θ₅＝30，θ₆＝-110 (29)

(2)应用正向运动学方程求解机器人当前位姿。

(3)利用本公开实施例提出的方法和上述位姿，计算各关节对应角度。通过计算，共得到了8组逆运动学的解，如表1所示。从表1可以看出，第1组解与给定的6个角度完全相同。(4)计算求出的8组逆解所对应的位姿。(5)计算(2)中给定位姿与(4)中逆解对应位姿差矩阵的2范数。(6)比较差矩阵范数并找出最大范数及其对应的位姿误差。通过计算，第7组逆解所对应位姿差矩阵的2范数norm₇为最大，其值为

norm₇＝6.864709873407309×10^-13 (31)

该范数对应的最大位姿误差Δg_max为

式中，g_st为(2)中给定角度所对应的位姿，g_st7为第7组逆解所对应的位姿。

实验结果表明，最大位姿误差极其地小，为10^-12数量级，从而验证了论文提出逆解算法的正确性。与文献^[28]相比，其最大位姿误差为10^-2数量级，因此，本文提出的算法在精度方面具有很大优势。

表1逆运动学的8组解

综上所述，本公开实施例提出了一种基于几何、代数和Paden-Kahan子问题方法的6自由度串联机器人的逆运动学求解方法。几何方法的运用将机器人前三关节螺旋运动的所有情形全部表示出来，然后根据机器人特殊结构与坐标系之间的关系，快速地求出了各关节运动轨迹圆的交点，并将逆运动学的求解转化为熟悉的Paden-Kahan子问题，简单直观，便于理解，且具有明确的几何意义。采用代数消元与MATLAB相结合的方法求解第4,5关节变量，减少了人工计算和由复杂计算产生的错误。算法可以求得逆运动学的封闭解。实验中求得的逆解位姿误差非常小，验证了本文提出算法的正确性和高精度性。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种6自由度串联机器人的关节旋转角度确定方法，所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；其特征在于，包括：

建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；

以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；

确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；

基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；

根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。

2.根据权利要求1所述的方法，其特征在于，确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿，包括：

根据第二空间坐标系三个坐标轴分别相对于第一空间坐标系的三个坐标轴的方向余弦、第二空间坐标系和第一坐标系两原点位置矢量确定第二空间坐标系相对于第一空间坐标系下，所述6自由度串联机器人的初始位姿。

3.根据权利要求2所述的方法，其特征在于，所选取的第一点、第二点、第三点、第一轴线交点在初始位形下的坐标分别如下：

其中，r₁、r₂、r₃分别为第一点、第二点和第三点在初始位形下的坐标；r₄、r₅和r₆分别为第一轴线交点的坐标；

第一至第六关节的轴线方向单位矢量如下：

其中，ω₁,ω₂,ω₃,ω₄,ω₅,ω₆分别为第一至第六关节的轴线方向单位矢量；

第一至第六关节的单位运动旋量如下：

其中，ξ₁,ξ₂,ξ₃,ξ₄,ξ₅和ξ₆分别为第一至第六关节的单位运动旋量。

4.根据权利要求1所述的方法，其特征在于，基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标、所述初始位姿，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹，包括：

采用几何方法建立第一轴线交点从初始位形到当前位形下在第一至第三关节上的螺旋运动模型；所述螺旋运动模型包括第一轴线交点绕第三关节的轴线形成的第三轨迹圆，再绕第二关节的轴线形成的第二轨迹圆，以及绕第一关节的轴线形成的第一轨迹圆；

基于所述螺旋运动模型确定第一轨迹圆至第三轨迹圆之间的交点。

5.根据权利要求1所述的方法，其特征在于，根据所述螺旋运动轨迹确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度，包括：

基于所述第一轨迹圆至第三轨迹圆之间的交点，以及所述第一轴线交点分别在初始位形和当前位形下的第一坐标和第二坐标之间的指数积关系，确定所述6自由度串联机器人的第一至第三关节从初始位形到当前位形的旋转角度；所述指数积关系是基于预先建立的所述6自由度串联机器人的指数积正向运动学模型确定的；

基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度。

6.根据权利要求4所述的方法，其特征在于，所述第一轨迹圆、第二轨迹圆和第三轨迹圆的圆心分别位于第一至第三关节的轴线上，且所述第二轨迹圆和第三轨迹圆位于同一平面，而第一轨迹圆所在平面与第二轨迹圆和第三轨迹圆所在平面垂直。

7.根据权利要求6所述的方法，其特征在于，第一轨迹圆和第二轨迹圆相交于第一交点，第二轨迹圆和第三轨迹圆相交于第二交点，所述第一交点的坐标为：

其中，p₂为第一交点，p_x,p_y,p_z分别为所述第一轴线交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；

第二交点的坐标如下表示：

其中，p₃为第二交点，y₂、z₂分别为第二轨迹圆圆心的y、z轴坐标，y₂、z₃分别为第三轨迹圆圆心的y、z轴坐标；b、c分别为第一轴线交点的第一坐标的y、z轴坐标。

8.根据权利要求5所述的方法，其特征在于，所述指数积正向运动学模型如下表示：

其中，g_st(θ)为所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿，g_st(0)为所述初始位姿；θ₁，θ₂，θ₃，θ₄，θ₅，θ₆分别为所述6自由度串联机器人的第一至第六关节在当前位形下相对于初始位形的旋转角度；分别为第一关节至第六关节的单位运动旋量；

第一至第三关节从初始位形到当前位形的旋转角度如下表示：

θ₂＝atan2(m₂,m₃)

θ₃＝atan2(m₄,m₅)

m₄＝(z₃-c)(y₅-y₂)+(b–y₂)(z₅-z₃)

m₅＝(b–y₂)(y₅–y₂)+(c-z₃)(z₅-z₃)

其中，θ₁、θ₂、θ₃分别为第一至第三关节从初始位形到当前位形的旋转角度；p_x,p_y,p_z分别为所述第一轴线交点在当前位形下在第一空间坐标系下的x、y、z轴坐标；y₂、z₂分别为第二轨迹圆圆心的y、z轴坐标，y₂、z₃分别为第三轨迹圆圆心的y、z轴坐标；b、c分别为第一轴线交点在初始位形下的y、z轴坐标；y₅、z₅分别为第二轨迹圆和第三轨迹圆的第二交点的y、z轴坐标。

9.根据权利要求5所述的方法，其特征在于，基于所述第一至第三关节从初始位形到当前位形的旋转角度、所述初始位姿、所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿以及所述指数积正向运动学模型确定第四至第六关节的旋转角度，包括：

在所述指数积正向运动学模型的方程两边依次同乘以g_st ^-1(0)可得：

用和分别同乘方程(a)两边，可得：

其中，g_st(θ)为所述6自由度串联机器人在当前位形下第二空间坐标系相对于第一空间坐标系的当前位姿，g_st(0)为所述初始位姿；

θ₁，θ₂，θ₃，θ₄，θ₅，θ₆分别为所述6自由度串联机器人的第一至第六关节在当前位形下相对于初始位形的旋转角度；分别为第一关节至第六关节的单位运动旋量；

在第六关节的轴线上选择一点p₆，且该点p₆不在第四关节和第五关节的轴线上；

基于位置不变原则，在上述方程(b)两边同乘p₆，则得到：

其中，b、c分别为第一轴线交点在初始位形下的y、z轴坐标；

求解方程(d)，得到第四至第五关节在当前位形下相对于初始位形的旋转角度：

θ₄＝atan2(m₆,m₇)

其中，m₆＝-t₁/cosθ₅，m₇＝(c-t₃)/cosθ₅(cosθ₅≠0)；

同理，用分别同乘方程(b)两边，可得：

选择不在ξ₆轴线上的一点p₇；

在方程(e)两边同乘点p₇，可得：

求解方程(f)，可得第六关节在当前位形下相对于初始位形的旋转角度：

θ₆＝atan2(-t₄,t₅-b)。

10.一种6自由度串联机器人的关节旋转角度确定装置，所述6自由度串联机器人包括依次连接的第一至第六关节，第四至第六关节的轴线相交于第一轴线交点；其特征在于，包括：

第一建立模块，被配置为建立第一空间坐标系，所述第一空间坐标系的原点位于第一关节的轴线，且固定不动；

第二建立模块，被配置为以所述第一轴线交点为原点建立第二空间坐标系；在所述6自由度串联机器人的初始位形下，所述6自由度串联机器人的第一至第六关节的旋转角度为0；

第一确定模块，被配置为确定所述6自由度串联机器人在初始位形下，第二空间坐标系相对于第一空间坐标系的初始位姿；

第二确定模块，被配置为基于所述第一轴线交点在初始位形和当前位形下相对于第一空间坐标系的第一坐标和第二坐标，确定所述6自由度串联机器人从初始位形到当前位形，所述第一轴线交点在所述6自由度串联机器人的前三关节上的螺旋运动轨迹；

第三确定模块，被配置为根据所述螺旋运动轨迹、所述初始位姿和当前位姿确定所述6自由度串联机器人从初始位形到当前位形，所述第一至第六关节的旋转角度。