CN109129469B - 机械臂运动学逆解方法、装置及机械臂 - Google Patents

Abstract

本发明公开了一种机械臂运动学逆解方法、装置及机械臂，其中，该方法包括：确定机械臂的几何参数，其中，机械臂包括多个旋转关节，旋转关节分为第一类旋转关节和第二类旋转关节；根据机械臂的几何参数，采用几何代数法确定第一类旋转关节对应的第一类关节角；根据第一类关节角，采用欧拉角旋转矩阵变换法确定第二类旋转关节对应的第二类关节角，其中第一类关节角和第二类关节角用于控制机械臂的运转。本发明解决了现有技术中求解过程复杂的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

Description

机械臂运动学逆解方法、装置及机械臂

技术领域

本发明涉及机器人技术领域，具体而言，涉及一种机械臂运动学逆解方法、装置及机械臂。

背景技术

在已知工具坐标系相对于基座坐标系的期望位置和姿态,计算出满足期望要求的关节角的过程，运动学称此过程为运动学逆解。由于工业机器人属于6R型，在运动学求逆问题中会出现多重解和解的存在性，当末端执行器遇到奇异点时则会出现解的不存在性。目前工业机器人在逆运动学求解过程中依赖于D-H参数法，三轴相交的PIEPER法和代数几何分析法实现。诸如此类的方法存在以下不足：D-H参数法导致机器人建模单一且有局限性，代数几何法求解过程复杂难懂，计算公式繁杂且求解速度相对较慢。

针对相关技术中求解过程复杂的问题，目前尚未提出有效地解决方案。

发明内容

本发明提供了一种机械臂运动学逆解方法、装置及机械臂，以至少解决现有技术中求解过程复杂的问题。

为解决上述技术问题，根据本发明实施例的一个方面，提供了一种机械臂运动学逆解方法，包括：确定机械臂的几何参数，其中，机械臂包括多个旋转关节，旋转关节分为第一类旋转关节和第二类旋转关节；根据机械臂的几何参数，采用几何代数法确定第一类旋转关节对应的第一类关节角；根据第一类关节角，采用欧拉角旋转矩阵变换法确定第二类旋转关节对应的第二类关节角，其中第一类关节角和第二类关节角用于控制机械臂的运转。

进一步地，机械臂包括六个旋转关节，第一类旋转关节包括第一至第三旋转关节，第一类关节角包括第一至第三关节角，第二类旋转关节包括第四至第六旋转关节，第二类关节角包括第四至第六关节角。

进一步地，机械臂还包括五个连杆结构和一个末端执行器；确定机械臂的几何参数，包括：以第一个连杆结构为基准建立基准坐标系，其中，基准坐标系包括X轴、Y轴和Z轴，基准坐标系的原点位于第一个连杆结构的底部中央；确定参考结构在基准坐标系的坐标位置和欧拉姿态角，其中参考结构为末端执行器，或第四旋转关节，或第三旋转关节。

进一步地，根据机械臂的几何参数，采用几何代数法确定第一类旋转关节对应的第一类关节角，包括：根据参考结构的坐标位置和欧拉姿态角确定第五个旋转关节的坐标位置；根据第五个旋转关节的坐标位置确定第一关节角；根据第五个旋转关节的坐标位置，采用几何代数法确定第二关节角和第三关节角。

进一步地，根据参考结构的坐标位置和欧拉姿态角确定第五个旋转关节的坐标位置，包括：将参考结构的欧拉姿态角(A,B,C)转换成X-Y-Z欧拉角旋转矩阵R；将参考结构的坐标系平移至第五个旋转关节的坐标系，确定平移算子P₅ ^a；根据X-Y-Z欧拉角旋转矩阵R、参考结构的坐标位置Pa(X，Y，Z)和平移算子P₅ ^a，确定第五个旋转关节的坐标位置P5(P₅.X，P₅.Y，P₅.Z)。

进一步地，X-Y-Z欧拉角旋转矩阵

第五个旋转关节的坐标位置

P₅·X＝X-5.X*r12；P₅.Y＝Y-5.X*r22；P₅.Z＝Z-5.X*r32

其中，在参考结构为末端执行器时a＝6，在参考结构为第四旋转关节时a＝4，在参考结构为第三旋转关节时a＝3。

进一步地，根据第五个旋转关节的坐标位置确定第一关节角，包括：根据第五个旋转关节的X轴坐标位置P₅.X和Y轴坐标位置P₅.Y确定第一关节角Q1；其中，Q1＝arctan2(P₅.Y，P₅.X)。

进一步地，根据第五个旋转关节的坐标位置，采用几何代数法确定第二和第三关节角，包括：以第二旋转关节为基准建立第二旋转关节坐标系，其中，第二旋转关节坐标系包括X轴、Y轴和Z轴，第二旋转关节坐标系的原点位于第二旋转关节的底部中央；判断第二连杆结构和第三连杆结构在第二旋转关节坐标系中的组合形态；根据组合形态确定不同形态对应的第二关节角和第三关节角。

进一步地，组合形态包括：凸型结构和凹型结构；根据组合形态确定不同形态对应的第二关节角和第三关节角，包括：确定第二旋转关节、第三旋转关节与第五旋转关节组成的三角形中各个边长；根据边长确定三角形中的夹角以及边长与第二旋转关节坐标系的夹角；根据三角形中的夹角和边长与第二旋转关节坐标系的夹角，确定凸型结构中的第二关节角和第三关节角，以及凹型结构中的第二关节角和第三关节角。

进一步地，确定第二旋转关节、第三旋转关节与第五旋转关节组成的三角形中各个边长，包括：确定第二旋转关节与第三旋转关节之间的边AB、第二旋转关节与第五旋转关节之间的边AD、第三旋转关节与第五旋转关节之间的边BD；其中，

AB＝P2.Z

其中，AH＝OG-P2.X HD＝P₅.Z-P2.Z

进一步地，根据边长确定三角形中的夹角以及边长与第二旋转关节坐标系的夹角，包括：根据AB、AD、BD确定AB与AD的夹角β、AB与BD的夹角γ、AD与第二旋转关节坐标系X轴的夹角ψ、BD与第二旋转关节坐标系Z轴的夹角θ；其中，

cosγ＝(BD²+AB²-AD²)/(2*BD*AB)

cosγ＝(BD²+AB²-AD²)/(2*BD*AB)

θ＝arctan2(ED，EB)。

进一步地，根据三角形中的夹角和边长与第二旋转关节坐标系的夹角，确定凸型结构中的第二关节角和第三关节角，以及凹型结构中的第二关节角和第三关节角，包括：在凸型结构中，第二关节角

和第三关节角

在凹型结构中，第二关节角

和第三关节角

进一步地，根据第一至第三关节角，采用欧拉角旋转矩阵变换法确定第四至第六个旋转关节对应的第四至第六关节角，包括：根据第一至第三关节角确定腕部旋转矩阵Rw1；根据第四至第六关节角确定X-Y-X欧拉角旋转矩阵Rw2；根据Rw1和Rw2求解第四关节角Q4、第五关节角Q5和第六关节角Q6。

进一步地，腕部旋转矩阵Rw1＝Ra^T*R，其中，

Qz＝Q1，Qy＝Q2+Q3，其中，Q1为第一关节角、Q2为第二关节角，Q3为第三关节角；X-Y-X欧拉角旋转矩阵

令Rw1＝Rw2,Rw1和Rw2矩阵内的元素一一对应，Q5得到两个解：

Q4得到一个解：

Q4.1＝arctan2(rw21，rw31)

Q6得到一个解：

Q6.1＝arctan2(rw12，rw13)。

进一步地，根据Rw1和Rw2确定第四至第六关节角，包括：分别计算Q5.1、Q5.2与第五旋转关节上一次旋转时的关节角的差值；对比差值，确定差值小的第五关节角作为最优解。

进一步地，根据Rw1和Rw2确定第四至第六关节角，还包括：在第五关节角为奇异点时，将第四旋转关节上一次旋转时的关节角作为第四关节角Q4；第六关节角Q6＝arctan2(r23，r33)-Q4。

根据本发明实施例的另一方面，提供了一种机械臂运动学逆解装置，包括：检测器，用于确定机械臂的几何参数，其中，机械臂包括多个旋转关节，旋转关节分为第一类旋转关节和第二类旋转关节；第一处理器，用于根据机械臂的几何参数，采用几何代数法确定第一类旋转关节对应的第一类关节角；第二处理器，用于根据第一类关节角，采用欧拉角旋转矩阵变换法确定第二类旋转关节对应的第二类关节角，其中第一类关节角和第二类关节角用于控制机械臂的运转。

根据本发明实施例的又一方面，提供了一种机械臂，包括如上述的机械臂运动学逆解装置。

根据本发明实施例的又一方面，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现如上述实施例的方法。

根据本发明实施例的又一方面，提供了一种包含计算机可执行指令的存储介质，计算机可执行指令在由计算机处理器执行时用于执行如上述实施例的方法。

在本发明中，将机器人机械臂分为前端三臂和后端三臂两部分，分别利用几何代数法和腕部欧拉角旋转变换求解各个关节角，不依赖于传统的D-H参数建模，有效地解决了现有技术中求解过程复杂难懂，计算量大，求解效率慢的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

附图说明

图1是根据本发明实施例1的机械臂运动学逆解方法的一种可选的流程图；

图2是根据本发明实施例1的机械臂的结构示意图；

图3是根据本发明实施例1的凸型结构机械臂的几何结构图；

图4是根据本发明实施例1的凹型结构机械臂的几何结构图；

图5是根据本发明实施例1的机械臂运动学逆解方法的另一种可选的流程图；以及

图6是根据本发明实施例2的机械臂运动学逆解装置的一种可选的结构框图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本发明相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本发明的一些方面相一致的装置和方法的例子。

实施例1

在本发明优选的实施例1中提供了一种机械臂运动学逆解方法，该方法可以直接应用至各种工业机器人上，也可以应用至具有工业机器人部分功能的其他装置上，具体实现时，可以通过在工业机器人或其他装置安装软件、APP、或者写入工业机器人或其他装置控制器程序的方式来实现。具体来说，图1示出该方法的一种可选的流程图，如图1所示，该方法包括如下步骤S102-S106：

S102：确定机械臂的几何参数，其中，机械臂包括多个旋转关节，旋转关节分为第一类旋转关节和第二类旋转关节；

S104：根据机械臂的几何参数，采用几何代数法确定第一类旋转关节对应的第一类关节角；

S106：根据第一类关节角，采用欧拉角旋转矩阵变换法确定第二类旋转关节对应的第二类关节角，其中第一类关节角和第二类关节角用于控制机械臂的运转。

在上述实施方式中，将机器人机械臂分为前端三臂和后端三臂两部分，分别利用几何代数法和腕部欧拉角旋转变换求解各个关节角，不依赖于传统的D-H参数建模，有效地解决了现有技术中求解过程复杂难懂，计算量大，求解效率慢的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

在本发明一个优选的实施方式中，如图2所示，机械臂包括六个旋转关节，五个连杆结构和一个末端执行器，第一类旋转关节包括第一至第三旋转关节，第一类关节角包括第一至第三关节角，第二类旋转关节包括第四至第六旋转关节，第二类关节角包括第四至第六关节角。以第一个连杆结构为基准建立基准坐标系，其中，基准坐标系包括X轴、Y轴和Z轴，基准坐标系的原点位于第一个连杆结构的底部中央。进一步地，机械臂还包括五个连杆结构和一个末端执行器；确定机械臂的几何参数，包括：以第一个连杆结构为基准建立基准坐标系，其中，基准坐标系包括X轴、Y轴和Z轴，基准坐标系的原点位于第一个连杆结构的底部中央；确定参考结构在基准坐标系的坐标位置和欧拉姿态角，其中参考结构为末端执行器，或第四旋转关节，或第三旋转关节。在上述实施方式中，末端执行器替可以换为第四关节角，或，第三关节角，在末端执行器变化后，上述凸型结构和凹型结构的三角形随之变化，在变化后的三角形中确定相应的三角形夹角及与坐标轴的夹角即可。这种方式增加了计算各关节角的灵活性，不依赖于末端执行器的位置也可以计算出各个关节角。

优选地，根据机械臂的几何参数，采用几何代数法确定第一类旋转关节对应的第一类关节角，包括：根据参考结构的坐标位置和欧拉姿态角确定第五个旋转关节的坐标位置；根据第五个旋转关节的坐标位置确定第一关节角；根据第五个旋转关节的坐标位置，采用几何代数法确定第二关节角和第三关节角。

在上述实施方式中，根据参考结构的坐标位置和欧拉姿态角确定第五个旋转关节的坐标位置，包括：将参考结构的欧拉姿态角(A,B,C)转换成X-Y-Z欧拉角旋转矩阵R；将参考结构的坐标系平移至第五个旋转关节的坐标系，确定平移算子P₅ ^a；根据X-Y-Z欧拉角旋转矩阵R、参考结构的坐标位置Pa(X，Y，Z)和平移算子P₅ ^a，确定第五个旋转关节的坐标位置P5(P₅.X，P₅.Y，P₅.Z)。

进一步地，X-Y-Z欧拉角旋转矩阵

第五个旋转关节的坐标位置

P₅.X＝X-5.X*r12；P₅.Y＝Y-5.X*r22；P₅.Z＝Z-5.X*r32

5.X为坐标系从末端执行器移动到第五个旋转关节时的坐标移动距离。

其中，在参考结构为末端执行器时a＝6，在参考结构为第四旋转关节时a＝4，在参考结构为第三旋转关节时a＝3。

进一步地，根据第五个旋转关节的坐标位置确定第一关节角，包括：根据第五个旋转关节的X轴坐标位置P₅.X和Y轴坐标位置P₅.Y确定第一关节角Q1；其中，Q1＝arctan2(P₅.Y，P₅.X)。

在本发明另一个优选的实施方式中，根据第五个旋转关节的坐标位置，采用几何代数法确定第二和第三关节角，包括：以第二旋转关节为基准建立第二旋转关节坐标系，其中，第二旋转关节坐标系包括X轴、Y轴和Z轴，第二旋转关节坐标系的原点位于第二旋转关节的底部中央；判断第二连杆结构和第三连杆结构在第二旋转关节坐标系中的组合形态；根据组合形态确定不同形态对应的第二关节角和第三关节角。

在上述实施方式中，组合形态包括：凸型结构和凹型结构，如图3和图4所示，图3为凸型结构，图4为凹型结构；根据组合形态确定不同形态对应的第二关节角和第三关节角，包括：确定第二旋转关节、第三旋转关节与第五旋转关节组成的三角形中各个边长；根据边长确定三角形中的夹角以及边长与第二旋转关节坐标系的夹角；根据三角形中的夹角和边长与第二旋转关节坐标系的夹角，确定凸型结构中的第二关节角和第三关节角，以及凹型结构中的第二关节角和第三关节角。

其中，确定第二旋转关节、第三旋转关节与第五旋转关节组成的三角形中各个边长，包括：确定第二旋转关节与第三旋转关节之间的边AB、第二旋转关节与第五旋转关节之间的边AD、第三旋转关节与第五旋转关节之间的边BD；其中，

AB=P2.Z

其中，AH＝OG-P2.X HD＝P₅.Z-P2.Z

上述公式中，P2.Z、P3.X、P3.Z、4.X均为坐标系移动距离，.X为X轴的移动距离，.Z为Z轴的移动距离。

优选地，根据边长确定三角形中的夹角以及边长与第二旋转关节坐标系的夹角，包括：根据AB、AD、BD确定AB与AD的夹角β、AB与BD的夹角γ、AD与第二旋转关节坐标系X轴的夹角ψ、BD与第二旋转关节坐标系Z轴的夹角θ；其中，

cosγ＝(BD²+AB²-AD²)/(2*BD*AB)

cosγ＝(BD²+AB²-AD²)/(2*BD*AB)

θ＝arctan2(ED，EB)。

可选地，根据三角形中的夹角和边长与第二旋转关节坐标系的夹角，确定凸型结构中的第二关节角和第三关节角，以及凹型结构中的第二关节角和第三关节角，包括：在凸型结构中，第二关节角

和第三关节角

在凹型结构中，第二关节角

和第三关节角

在本发明又一个优选的实施方式中，根据第一至第三关节角，采用欧拉角旋转矩阵变换法确定第四至第六个旋转关节对应的第四至第六关节角，包括：根据第一至第三关节角确定腕部旋转矩阵Rw1；根据第四至第六关节角确定X-Y-X欧拉角旋转矩阵Rw2；根据Rw1和Rw2求解第四关节角Q4、第五关节角Q5和第六关节角Q6。

其中，腕部旋转矩阵Rw1＝Ra^T*R，

Qz＝Q1，Qy＝Q2+Q3，其中，Q1为第一关节角、Q2为第二关节角，Q3为第三关节角；X-Y-X欧拉角旋转矩阵

令Rw1＝Rw2,Rw1和Rw2矩阵内的元素一一对应，Q5得到两个解：

Q4得到一个解：

Q4.1＝arctan2(rw21，rw31)

Q6得到一个解：

Q6.1＝arctan2(rw12，rw13)。

在上述实施方式中，根据Rw1和Rw2确定第四至第六关节角，包括：分别计算Q5.1、Q5.2与第五旋转关节上一次旋转时的关节角的差值；对比差值，确定差值小的第五关节角作为最优解。优选地，根据Rw1和Rw2确定第四至第六关节角，还包括：在第五关节角为奇异点时，将第四旋转关节上一次旋转时的关节角作为第四关节角Q4；第六关节角Q6＝arctan2(r23，r33)-Q4。

在本发明优选的实施例1中还提供了一种机械臂运动学逆解方法，具体来说，图5示出该方法的一种可选的流程图，如图5所示，该方法包括如下步骤：

开始；

S1：确定整个工业机器人各连杆结构的几何参数，末端执行器的位置和姿态(X,Y,Z,A,B,C)；

S2：通过末端姿态欧拉角A,B,C变换成一个3x3的旋转矩阵R；

S3：通过平移算子和旋转算子从末端转换到关节5的位置坐标，然后可以确定了关节1的关节角，然后通过坐标变换把坐标系转换到关节2，通过几何代数法把关节2和关节3的关节角求出；

S4：通过前面求出的关节1、2、3的关节角然后通过旋转矩阵变换成关节4、5、6的腕部旋转矩阵，然后通过欧拉变换的旋转矩阵里面的参数定义分别求出关节4,5,6的关节角；

S5：由于关节角4,5,6会出现二个解，通过与上一个关节角的比较选出最优解；

结束。

在上述实施方式中，将机器人机械臂分为前端三臂和后端三臂两部分，分别利用几何代数法和腕部欧拉角旋转变换求解各个关节角，不依赖于传统的D-H参数建模，有效地解决了现有技术中求解过程复杂难懂，计算量大，求解效率慢的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

实施例2

基于上述实施例1中提供的机械臂运动学逆解方法，在本发明优选的实施例2中还提供了一种机械臂运动学逆解装置，具体地，图6示出该装置的一种可选的结构框图，如图6所示，该装置包括：

检测器602，用于确定机械臂的几何参数，其中，机械臂包括多个旋转关节，旋转关节分为第一类旋转关节和第二类旋转关节；

第一处理器604，与检测器602连接，用于根据机械臂的几何参数，采用几何代数法确定第一类旋转关节对应的第一类关节角；

第二处理器606，与第一处理器604连接，用于根据第一类关节角，采用欧拉角旋转矩阵变换法确定第二类旋转关节对应的第二类关节角，其中第一类关节角和第二类关节角用于控制机械臂的运转。

在上述实施方式中，将机器人机械臂分为前端三臂和后端三臂两部分，分别利用几何代数法和腕部欧拉角旋转变换求解各个关节角，不依赖于传统的D-H参数建模，有效地解决了现有技术中求解过程复杂难懂，计算量大，求解效率慢的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

关于上述实施例中的装置，其中各个单元、模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

实施例3

基于上述实施例2中提供的机械臂运动学逆解装置，在本发明优选的实施例3中还提供了一种机械臂，包括如上述的机械臂运动学逆解装置。

在上述实施方式中，将机器人机械臂分为前端三臂和后端三臂两部分，分别利用几何代数法和腕部欧拉角旋转变换求解各个关节角，不依赖于传统的D-H参数建模，有效地解决了现有技术中求解过程复杂难懂，计算量大，求解效率慢的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

实施例4

基于上述实施例1中提供的机械臂运动学逆解方法，在本发明优选的实施例4中还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现如上述实施例1中的机械臂运动学逆解方法。

在上述实施方式中，将机器人机械臂分为前端三臂和后端三臂两部分，分别利用几何代数法和腕部欧拉角旋转变换求解各个关节角，不依赖于传统的D-H参数建模，有效地解决了现有技术中求解过程复杂难懂，计算量大，求解效率慢的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

实施例5

基于上述实施例1中提供的机械臂运动学逆解方法，在本发明优选的实施例5中还提供了一种包含计算机可执行指令的存储介质，计算机可执行指令在由计算机处理器执行时用于执行如上述实施例1中的机械臂运动学逆解方法。

在上述实施方式中，将机器人机械臂分为前端三臂和后端三臂两部分，分别利用几何代数法和腕部欧拉角旋转变换求解各个关节角，不依赖于传统的D-H参数建模，有效地解决了现有技术中求解过程复杂难懂，计算量大，求解效率慢的问题，实现求解过程简单易懂，代码量精简，计算速率快且精度高。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本发明的其它实施方案。本申请旨在涵盖本发明的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本发明未发明的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本发明的真正范围和精神由下面的权利要求指出。

应当理解的是，本发明并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本发明的范围仅由所附的权利要求来限制。

Claims (18)

1.一种机械臂运动学逆解方法，其特征在于，包括：

确定机械臂的几何参数，其中，所述机械臂包括多个旋转关节，所述旋转关节分为第一类旋转关节和第二类旋转关节；

根据所述机械臂的几何参数，采用几何代数法确定所述第一类旋转关节对应的第一类关节角；

根据所述第一类关节角，采用欧拉角旋转矩阵变换法确定所述第二类旋转关节对应的第二类关节角，其中所述第一类关节角和所述第二类关节角用于控制所述机械臂的运转；

根据所述第一类关节角，采用欧拉角旋转矩阵变换法确定所述第二类旋转关节对应的第二类关节角，包括：

根据所述第一类关节角确定腕部旋转矩阵Rw1；

根据所述第二类关节角确定X-Y-X欧拉角旋转矩阵Rw2；

根据所述Rw1和所述Rw2求解所述第二类关节角；

其中，所述机械臂包括六个旋转关节，所述第一类旋转关节包括第一至第三旋转关节，所述第一类关节角包括第一至第三关节角，所述第二类旋转关节包括第四至第六旋转关节，所述第二类关节角包括第四至第六关节角；

所述腕部包括所述第四至第六旋转关节，所述腕部旋转矩阵Rw1为所述第四至第六旋转关节的旋转矩阵。

2.根据权利要求1所述的方法，其特征在于，所述机械臂还包括五个连杆结构和一个末端执行器；所述确定机械臂的几何参数，包括：

以第一个连杆结构为基准建立基准坐标系，其中，所述基准坐标系包括X轴、Y轴和Z轴，所述基准坐标系的原点位于所述第一个连杆结构的底部中央；

确定参考结构在所述基准坐标系的坐标位置和欧拉姿态角，其中所述参考结构为所述末端执行器，或所述第四旋转关节，或所述第三旋转关节。

3.根据权利要求2所述的方法，其特征在于，所述根据所述机械臂的几何参数，采用几何代数法确定所述第一类旋转关节对应的第一类关节角，包括：

根据所述参考结构的坐标位置和欧拉姿态角确定第五个旋转关节的坐标位置；

根据所述第五个旋转关节的坐标位置确定所述第一关节角；

根据所述第五个旋转关节的坐标位置，采用几何代数法确定所述第二关节角和第三关节角。

4.根据权利要求3所述的方法，其特征在于，所述根据所述参考结构的坐标位置和欧拉姿态角确定第五个旋转关节的坐标位置，包括：

将所述参考结构的欧拉姿态角(A，B，C)转换成X-Y-Z欧拉角旋转矩阵R；

将所述参考结构的坐标系平移至第五个旋转关节的坐标系，确定平移算子P₅ ^a；

根据所述X-Y-Z欧拉角旋转矩阵R、所述参考结构的坐标位置Pa(X，Y，Z)和所述平移算子P5^a，确定所述第五个旋转关节的坐标位置P5(P₅.X，P₅.Y，P₅.Z)。

5.根据权利要求4所述的方法，其特征在于，

所述X-Y-Z欧拉角旋转矩阵

所述第五个旋转关节的坐标位置

P₅.X＝X-5.X*r12；P₅.Y＝Y-5.X*r22；P₅.Z＝Z-5.X*r32

其中，在所述参考结构为所述末端执行器时a＝6，在所述参考结构为所述第四旋转关节时a＝4，在所述参考结构为所述第三旋转关节时a＝3。

6.根据权利要求4所述的方法，其特征在于，所述根据所述第五个旋转关节的坐标位置确定所述第一关节角，包括：

根据所述第五个旋转关节的X轴坐标位置P₅.X和Y轴坐标位置P₅.Y确定所述第一关节角Q1；其中，

Q1＝arctan2(P₅.Y，P₅.X)。

7.根据权利要求4所述的方法，其特征在于，所述根据所述第五个旋转关节的坐标位置，采用几何代数法确定所述第二和第三关节角，包括：

以第二旋转关节为基准建立第二旋转关节坐标系，其中，所述第二旋转关节坐标系包括X轴、Y轴和Z轴，所述第二旋转关节坐标系的原点位于所述第二旋转关节的底部中央；

判断所述第二连杆结构和所述第三连杆结构在所述第二旋转关节坐标系中的组合形态；其中，所述第二连杆结构和所述第三连杆结构通过所述第二旋转关节连接；

根据所述组合形态确定不同形态对应的所述第二关节角和第三关节角。

8.根据权利要求7所述的方法，其特征在于，所述组合形态包括：凸型结构和凹型结构；所述根据所述组合形态确定不同形态对应的所述第二关节角和第三关节角，包括：

确定所述第二旋转关节、所述第三旋转关节与所述第五旋转关节组成的三角形中各个边长；

根据所述边长确定所述三角形中的夹角以及所述边长与所述第二旋转关节坐标系的夹角；

根据所述三角形中的夹角和所述边长与第二旋转关节坐标系的夹角，确定所述凸型结构中的所述第二关节角和第三关节角，以及所述凹型结构中的所述第二关节角和第三关节角。

9.根据权利要求8所述的方法，其特征在于，所述确定所述第二旋转关节、所述第三旋转关节与所述第五旋转关节组成的三角形中各个边长，包括：

确定所述第二旋转关节与所述第三旋转关节之间的边AB、所述第二旋转关节与所述第五旋转关节之间的边AD、所述第三旋转关节与所述第五旋转关节之间的边BD；其中，

AB＝P2.Z；

其中，AH＝OG-P2.X HD＝P₅.Z-P2.ZP2.X代表所述第二个旋转关节的X轴坐标位置，P2.Z代表所述第二个旋转关节的Z轴坐标位置，P3.X代表所述第三个旋转关节的X轴坐标位置，P3.Z代表所述第三个旋转关节的Z轴坐标位置，P4.X代表所述第四个旋转关节的X轴坐标位置。

10.根据权利要求9所述的方法，其特征在于，所述根据所述边长确定所述三角形中的夹角以及所述边长与所述第二旋转关节坐标系的夹角，包括：

根据AB、AD、BD确定AB与AD的夹角β、AB与BD的夹角γ、AD与所述第二旋转关节坐标系X轴的夹角Ψ、BD与所述第二旋转关节坐标系Z轴的夹角θ；其中，

cosγ＝(BD²+AB²-AD²)/(2*BD*AB)；

cosγ＝(BD²+AB²-AD²)/(2*BD*AB)；

θ＝arctan2(ED，EB)；其中，HD为所述第五旋转关节在所述第二旋转关节坐标系中Z轴的坐标，AH为所述第五旋转关节在所述第二旋转关节坐标系中X轴的坐标，ED为所述第四个连杆结构的拐角至所述第五旋转关节在所述第二旋转关节坐标系中X轴的距离，EB为所述第四个连杆结构的拐角至所述第三旋转关节在所述第二旋转关节坐标系中Z轴的距离。

11.根据权利要求10所述的方法，其特征在于，所述根据所述三角形中的夹角和所述边长与第二旋转关节坐标系的夹角，确定所述凸型结构中的所述第二关节角和第三关节角，以及所述凹型结构中的所述第二关节角和第三关节角，包括：

在所述凸型结构中，所述第二关节角

和第三关节角Q3＝180-γ-θ；

在所述凹型结构中，所述第二关节角

和第三关节角Q3＝γ+θ-180。

12.根据权利要求1所述的方法，其特征在于，所述腕部旋转矩阵Rw1＝Ra^T*R，其中，Ra代表第一类关节角旋转的旋转矩阵，Ra^T代表Ra的转置矩阵，Rt代表的是整体旋转关节的旋转矩阵；

Qz＝Q1，Qy＝Q2+Q3，其中，Q1为所述第一关节角、Q2为所述第二关节角，Q3为所述第三关节角，Rz代表坐标绕z轴旋转的旋转矩阵，Ry代表坐标绕y轴旋转的旋转矩阵；

所述X-Y-X欧拉角旋转矩阵

其中，Q4为所述第四关节角、Q5为所述第五关节角，Q6为所述第三关节角，令Rw1＝Rw2，所述Rw1和所述Rw2矩阵内的元素一一对应，Q5得到两个解：

Q4得到一个解：

Q4.1＝arctan2(rw21，rw31)

Q6得到一个解：

Q6.1＝arctan2(rw12，rw13)。

13.根据权利要求12所述的方法，其特征在于，所述根据所述Rw1和所述Rw2确定所述第四至第六关节角，包括：

分别计算所述Q5.1、所述Q5.2与所述第五旋转关节上一次旋转时的关节角的差值；

对比所述差值，确定所述差值小的第五关节角作为最优解。

14.根据权利要求13所述的方法，其特征在于，所述根据所述Rw1和所述Rw2确定所述第四至第六关节角，还包括：

在所述第五关节角为奇异点时，将所述第四旋转关节上一次旋转时的关节角作为所述第四关节角Q4；

所述第六关节角Q6＝arctan2(r23，r33)-Q4；其中，r23代表旋转矩阵Rt中第二行第三列的元素，r33代表旋转矩阵Rt中第三行第三列的元素。

15.一种机械臂运动学逆解装置，其特征在于，包括：

检测器，用于确定机械臂的几何参数，其中，所述机械臂包括多个旋转关节，所述旋转关节分为第一类旋转关节和第二类旋转关节；

第一处理器，用于根据所述机械臂的几何参数，采用几何代数法确定所述第一类旋转关节对应的第一类关节角；

第二处理器，用于根据所述第一类关节角，采用欧拉角旋转矩阵变换法确定所述第二类旋转关节对应的第二类关节角，其中所述第一类关节角和所述第二类关节角用于控制所述机械臂的运转；

其中，第二处理器具体用于根据所述第一类关节角确定腕部旋转矩阵Rw1；

根据所述第二类关节角确定X-Y-X欧拉角旋转矩阵Rw2；

根据所述Rw1和所述Rw2求解所述第二类关节角；

其中，所述机械臂包括六个旋转关节，所述第一类旋转关节包括第一至第三旋转关节，所述第一类关节角包括第一至第三关节角，所述第二类旋转关节包括第四至第六旋转关节，所述第二类关节角包括第四至第六关节角；

所述腕部包括所述第四至第六旋转关节，所述腕部旋转矩阵Rw1为所述第四至第六旋转关节的旋转矩阵。

16.一种机械臂，其特征在于，包括如权利要求15所述的机械臂运动学逆解装置。

17.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至14中任一项所述的机械臂运动学逆解方法。

18.一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行如权利要求1至14中任一项所述的机械臂运动学逆解方法。

Images