运动学逆解方法、机器人以及计算机可读储存介质

摘要

本申请公开了一种运动学逆解方法、机器人以及计算机可读储存介质。该机器人至少包括固定机架、第一轴、第二轴、第三轴、第四轴、第五轴、第六轴，第一轴至第六轴依次连接，第一轴固定在固定机架上；该方法包括：获取第二轴的直线轴角度初始值；根据第二轴的直线轴角度初始值计算得到第三轴的轴角度初始值；通过第三轴的轴角度初始值，计算得到当前第六轴末端执行器的法兰中心点在基坐标系下的y坐标；移动末端执行器，得到末端执行器的法兰中心点在基坐标系下的移动距离；根据移动距离以及y坐标计算得到第三轴轴角度；基于第三轴轴角度，计算得到第二轴的直线轴角度。通过上述方式，能够满足机器人运动学需要的准确性和实时性，提高计算效率。

说明书

技术领域

本申请涉及机器人的运动学技术领域，特别是涉及一种运动学逆解方法、机器人以及计算机可读储存介质。

背景技术

机器人的使用在现代非常广泛，机器人运动学包括正向运动学和逆向运动学，正向运动学即给定机器人各关节变量，计算机器人末端执行器的位置姿态；逆向运动学即已知机器人末端执行器的位置姿态，计算机器人对应位置的全部关节变量。

一般正向运动学的解是唯一和容易获得的，而逆向运动学往往有多个解而且分析更为复杂。机器人逆运动分析是运动规划和控制中的重要问题，但由于机器人逆运动问题的复杂性和多样性，机器人逆运动解计算精度和计算实时性较差，导致计算效率低下。

发明内容

本申请提供一种机器人的运动学逆解方法、机器人以及计算机可读储存介质，以提高机器人运动学的逆解过程中计算精度和计算实时性以及计算效率。

为解决上述技术问题，本申请采用的一个技术方案是：提供一种机器人的运动学逆解方法，该机器人至少包括固定机架、第一轴、第二轴、第三轴、第四轴、第五轴、第六轴，第一轴至第六轴依次连接，第一轴固定在固定机架上；且逆解方法包括：获取所述第二轴的直线轴角度初始值；根据所述直线轴角度初始值计算得到所述第三轴的轴角度初始值；通过所述第三轴的轴角度初始值，计算得到当前第六轴末端执行器的法兰中心点在基坐标系下的y坐标；给定所述第六轴末端执行器的法兰中心点在所述基坐标系下的移动距离；根据所述移动距离以及所述y坐标计算得到所述第三轴轴角度；基于所述第三轴轴角度，计算得到所述第二轴的直线轴角度。

为解决上述技术问题，本申请采用的又一个技术方案是：提供一种机器人，该机器人至少包括固定机架、第一轴、第二轴、第三轴、第四轴、第五轴、第六轴，所述第一轴至所述第六轴依次连接，所述第一轴安装在所述固定机架上，所述第一轴为旋转轴，所述第二轴为直线轴，以及处理器和存储器，存储器中存储有计算机程序，处理器用于执行计算机程序以实现如上述的逆解方法。

为解决上述技术问题，本申请采用的又一个技术方案是：提供一种计算机可读存储介质，该计算机可读存储介质存储有计算机程序，计算机程序能够被处理器执行以实现如上述的逆解方法。

本申请的有益效果是：区别于现有技术，本申请实施例通过第二轴的直线轴角度初始值计算得到第三轴的轴角度初始值以及当前第六轴末端执行器的法兰中心点在基坐标系下的y坐标；给定第六轴末端执行器的法兰中心点在基坐标系下的移动距离；根据移动距离以及y坐标计算得到第三轴轴角度；基于第三轴轴角度，计算得到第二轴的直线轴角度，也就是说从机器人第六轴末端执行器移动后的法兰中心点位置和y轴初始值计算出机器人的第二轴需要被推动的直线轴角度，建立逆解方法，能够满足机器人运动学需要的准确性和实时性，提高计算效率。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请机器人的结构示意图；

图2是本申请图1中双摇杆结构的局部细节结构示意图；

图3是本申请图1中机器人的简化结构示意图；

图4是本申请机器人逆解方法第一实施例的流程示意框图；

图5是确定迭代步长的一具体实施方式的流程示意图；

图6是迭代后的第三轴轴角度是否满足预设阈值的一具体实施方式的流程示意图；

图7是本申请另一机器人控制模块实施例的电路示意框图；

图8是本申请计算机可读存储介质实施例的电路示意框图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，均属于本申请保护的范围。

本申请中的术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

通常来说，六轴工业机器人一般有6个自由度，常见的六轴工业机器人包含旋转(S轴)，下臂(L轴)、上臂(U轴)、手腕旋转(R轴)、手腕摆动(B轴)和手腕回转(T轴)。6个关节合成实现末端的6自由度动作。

本申请首先提供一种机器人，请参阅图1和图2，图1是本申请机器人的结构示意图，图2是本申请图1中双摇杆结构的局部细节结构示意图。

该机器人10包括基座11、固定机架13、第一轴12、第二轴14、第三轴15、第四轴16、第五轴17、第六轴18，第一轴12至第六轴18依次连接，第一轴12安装在固定机架13上；其中，第一轴12以及第二轴14均为主动轴，并且第一轴12是旋转轴，第二轴14是移动轴。第三轴15、第四轴16、第五轴17以及第六轴18均为从动轴，以上为不影响该机器人10的整体自由度，仅为方便进行运动学分析而定义。

请参阅图3，图3是本申请图1中机器人的简化结构示意图，图1中的基座11等效于图3中的阴影部分。第一轴12是靠近基座11一端的第一个旋转轴；可以旋转产生角度θ

其中，该机器人10包括四边形结构BB’DD’、四边形结构FF’HH’、四边形结构FF’HH’以及双摇杆结构DEFG；第三轴15与第四轴16之间的四边形结构BB’DD’为第一平行四连杆、第五轴17与第六轴18之间的四边形结构FF’HH’为第二平行四连杆；第四轴16与第五轴17之间通过双摇杆结构DEFG进行连接。

双摇杆机构DEFG就是两连架杆均是摇杆的铰链四杆机构，由机架、摇杆、连杆、摇杆组成。其中，双摇杆结构DEFG的FD是固定轴，DE为主动轴，FG为输出轴，能等距离传输，杆OC随B’推动杆B’D’移动，从而可以推动杆DE，进而推动杆FG，使得法兰中心H工具点做曲线运动，法兰中心H工具点夹长管进行左右推动，比如用于码管，可应用于深井领域，本申请以排管机器人为例，以做详细说明。

“工具中心点”的英文名称为“Tool Central Point”，简称“TCP”，机器人10初始状态的TCP是工具坐标系(Tool Coordinate System)的原点H，简称“TCS”，当用手动或者编程的方式让机器人10的第六轴18去接近坐标系的某一点时，其实就是让TCP去靠近该点，通常工具坐标系也称法兰坐标系，为了能与后文对应文中所有TCP和工具坐标系，这里的工具坐标系统一使用法兰中心(或基坐标系原点)和法兰坐标系。因此机器人10工装的运动，也是TCP的运动。也即机器人10的实际工作点相对于机器人10的末端的位置会发生变化。

因此，通过平行四连杆的设置方式，可以使得平行四连杆对角与z轴的夹角绝对值相等，也即第三轴15与z轴的角度大小的绝对值等于第四轴16与z轴的角度大小的绝对值；第五轴17与z轴的角度大小的绝对值等于第六轴18与z轴的角度大小的绝对值。

另外通过双摇杆结构DEFG的连接，可以将推动的第四轴16的直线距离等距离传送到第五轴17，从而带动第六轴18移动。

由图1以及图2可知，排管机器人包含两个主动轴，一个是旋转轴，一个是移动轴。根据排管机器人的工作需求，这两个轴的运动是相互独立的，因此可将旋转轴作为机器人外部轴进行控制，外部轴不包含在运动学逆解中，第二轴14为直线轴。移动轴作为机器人轴进行控制，因此排管机器人的运动学逆解问题可以看作：

输入：每个插补周期法兰中心H点在基坐标系上的y坐标；

输出：第二轴14在基坐标系上的直线距离d。

为了得到每个插补周期法兰中心H点在基坐标系上的y坐标，因此需要获取插补周期第六轴法兰中心H点移动前在基坐标系上的y轴初始值以及第六轴18法兰中心H点移动后的移动距离。

通过获取第六轴18法兰中心H点移动后的移动距离，结合移动前第六轴18法兰中心H点在基坐标系上的y轴初始值进行计算，即可以得到移动后第六轴法兰中心H点在基坐标系上的y坐标。

由图1以及图2可知，由于双摇杆机构的存在，适合选用数值法进行求解。数值法，也称为数值分析。迭代法和直接法不同，用迭代法求解问题时，其步骤没有固定的次数，而且只能求得问题的近似解，所找到的一系列近似解会收敛到问题的精确解。

结合图3，请参阅图4，图4是本申请排管机器人逆解方法第一实施例的流程示意框图。本实施例提供的方法具体包括以下步骤：

S41：获取第二轴14的直线轴角度初始值；

要想获取第二轴14的直线轴角度初始值，首先，可以机器人10的固定机架13与第二轴14的交点为原点O建立基坐标系，基坐标系包括x轴、y轴以及z轴，其中x轴、y轴以及z轴之间相互垂直设置；并且为更好地确定第六轴18法兰中心H点在基坐标系上的移动距离提供了原始基准点。

因为本实施例中的在末端执行器法兰中心点H的移动在水平位置上进行水平移动时，OB固定机架13未移动，为了以示区别和计算的方便，本实施例中采用以机器人10的固定机架13的一端为原点O建立基坐标系，基坐标系包括x轴、y轴以及z轴。

具体地，以OB固定机架13的O点端作为基坐标系的原点O，以OB固定机架13的OB作为基坐标系的y轴坐标系，并设置与y轴垂直的x轴，其中，杆B’D’、杆BD、杆H’F’以及杆HF与x轴正方向一侧，因为该机器人10在z轴方向上并未有较大的移动，因此在计算的过程中，可以将该机器人10在z轴方向上的运动忽略不计。其中，第二轴14是杆OC上的直线轴，包括直线段OA以及AC。

因此，当确定了基坐标系以及其他相关参数，则可以方便地获取第二轴14的直线轴角度初始值，一般来说，机器人10的初始位置是固定的，通过预设的方式事先存放于机器人10中的控制软件中，在使用时，打开控制软件可以直接调用，也即可以通过调用的方式获取第二轴14的长度初始值。当然，本领域相关技术人员也可以通过测量进行实时获取，此处不做限定。

S42：根据第二轴14的直线轴角度初始值计算得到第三轴15的轴角度初始值；

因此，通过获取第二轴14的直线轴角度初始值、第二轴14的原始长度值以及第三轴15的延长距离，并输入第一计算公式进行计算，得到第三轴15的轴角度初始值，第一计算公式为：

其中，θ

S43：通过第三轴15的轴角度初始值，计算得到当前第六轴18末端执行器的法兰中心点在基坐标系下的y坐标；

获取第一平行四连杆的长度初始值、第二平行四连杆的长度初始值、第六轴的轴周角度初始值、双摇杆机构的固定杆长度值相对于x轴的投影以及相对于y轴的投影、第二轴相对于x轴的投影以及相对于y轴的投影，并输入第二计算公式进行计算，得到当前第六轴末端执行器的法兰中心点在基坐标系下的坐标(x

其中，θ

提取当前第六轴末端执行器的法兰中心点在基坐标系下的坐标(x

S44：获取末端执行器的法兰中心点在基坐标系下的移动距离；

可以移动末端执行器，相当于给定了第六轴末端执行器的法兰中心点在基坐标系下的移动距离，从而可以得到末端执行器的法兰中心点在基坐标系下的移动距离，其中，该移动距离可以是预先给定的距离。另一方面，该移动距离也可以是预设距离，根据程序求解需要设置于处理器或存储器中。

S45：根据移动距离以及y坐标计算得到第三轴15轴角度；

得到移动距离的过程中，可以确定迭代总次数N以及迭代步长，例如，可以设置迭代总次数N小于100次，当然迭代总次数N也可以设置为其他数字例如200、500等；

其中，请参阅图5，图5是确定迭代步长的一具体实施方式的流程示意图，具体包括以下步骤：

S51：获取第三轴15与第三轴的零位之间的夹角；

基于对y轴初始值以及移动距离进行计算，可以得到第二轴14的直线轴角度，因为是通过机器人10的法兰中心H点采用正解模型进行计算处理，因此，多次迭代求解非常有必要的。

机器人10的法兰中心H点进行水平方向上移动时，会带动第三轴15绕着z轴方向进行转动，根据第三轴15绕着z轴转动方向的不同，并且通过与获取第三轴15的轴角度初始值

S52：判断第三轴15与第三轴的零位之间的夹角是否小于0；

在θ

若第三轴15与第三轴的零位之间的夹角小于0，表示与第六周18法兰中心H点在z轴的方向是一致的，此时，可以适当增加迭代步长k的值的大小，根据实验经验，则进入步骤S53：表示θ

若第三轴15与第三轴的零位之间的夹角大于或等于0，则进入步骤S54表示θ

因此，通过根据第三轴15与z轴之间角度是否小于0来判断选取迭代步长k的取值，可以有效地提高逆解方法的计算精度和计算效率。通过仿真，也进一步地验证了这个结果，假设给定逆解第二轴14的直线距离d的误差阈值为1e-10mm，第二轴14的实际位置为46.22mm。

通过不断迭代的过程，每一次都计算出比前一次更贴近真实值的第三轴15的角度θ

接下来，还需获取对应迭代次数为i时第三轴15轴角度的y轴斜率以及对应迭代次数为i时第三轴15的轴角度的y坐标，其中，i为区间[1，N]以内的整数；

然后，将迭代次数、迭代次数为i时第三轴15轴角度、迭代步长、移动距离、迭代次数为i时第三轴15轴角度的y坐标以及迭代次数为i时第三轴15轴角度的y轴斜率，输入第三计算公式进行计算，得到迭代后的第三轴15轴角度，第三计算公式为：

其中，i表示迭代次数，θ

另外，迭代次数为i时第三轴15轴角度的y轴斜率，具体包括：

获取迭代次数为i时第一平行四连杆的长度值、迭代次数为i时第三轴15的轴角度、迭代次数为i时第二平行四连杆的长度值以及迭代次数为i时第六轴的轴角度，并输入第四计算公式进行计算，得到迭代次数为i时第三轴15轴角度的y轴斜率，第四计算公式为：

其中，

其中，l

当迭代计算得到的第三轴15轴角度满足预设误差阈值时，则停止迭代过程，并以最后一次迭代后计算得到的第三轴15轴的轴角度为需要的第三轴15轴角度的计算结果，请参阅图6，图6是迭代后的第三轴15轴角度是否满足预设阈值的一具体实施方式的流程示意图，具体包括以下步骤：

S61：判断迭代计算得到的第三轴15轴角度是否满足预设误差阈值；

若是满足预设误差阈值，则步入步骤S62，表示可以停止迭代过程并确定第三轴15轴角度θ

若不满足预设误差阈值，则步入步骤S63，表示还需要增加迭代次数并继续迭代过程，以得到满足预设误差阈值的第三轴15轴角度θ

S46：基于第三轴15轴角度，计算得到第二轴的直线轴角度。

获取第三轴15轴角度初始值、迭代后需要的第三轴15轴角度、输入第五计算公式进行计算，得到第二轴的直线轴角度，在三角形OB’C中，应用根据余弦定理，有第五计算公式为：

l

其中，l

l

通过直接进行计算，迭代次数超过100次；而通过第三轴15与z轴的角度对迭代进行选取配置，进行计算迭代次数为20次；相比之下，基于第三轴15与z轴的角度对迭代进行选取配置具备明显的优势。

因此，通过将y轴初始值y

其中，在三角形OB’C中，满足以下公式：

由此可得：

其式(7)中，l

其式(8)以及式(9)中，θ

机构BB’DD’为第一平行四连杆，由其特性可得：

θ

其中，θ

机构DEFG为一双摇杆机构，在三角形DEF和三角形GFD中，由余弦定理和正弦定理可得：

其式(11)、式(12)以及式(13)中，l

又因为

其式(14)以及式(15)中，

由此可以得到θ

机构FF’HH’为第二平行四连杆机构，因此有：

θ

其式(16)中，θ

进一步地，请参见图7，图7是本申请实施例另一种机器人的结构示意图。本申请实施例提供一种机器人7，该机器人7至少包括：固定机架、第一轴、第二轴、第三轴、第四轴、第五轴、第六轴、第一平行四连杆、第二平行四连杆、双摇杆机构以及处理器71和存储器72。

其中，第一轴至第六轴依次连接，第一轴安装在固定机架上，第一轴为旋转轴，第二轴为直线轴。

其中，第三轴以及第四轴设置于第一平行四连杆上，以及第五轴以及第六轴设置于第二平行四连杆上，其中，在第一平行四连杆以及第二平行四连杆之间设置双摇杆机构。

处理器71、存储器72以及存储在存储器72中并在处理器71上运行的计算机程序721，处理器71用于执行计算机程序721以实现本申请实施例第一方面提供的方法的步骤，在此不再赘述。

参阅图8，图8是本申请计算机可读存储介质实施例的电路示意框图。如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在计算机可读存储介质8中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个可读存储介质中，包括若干指令(程序数据81)用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的可读存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种介质以及具有上述可读存储介质的电脑、手机、笔记本电脑、平板电脑、相机等电子设备。

关于具有存储功能的装置中的程序数据的执行过程的阐述可以参照上述本申请机器人的逆解方法实施例中阐述，在此不再赘述。

以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。