五轴机器人的近似位姿计算方法、装置和存储介质

技术领域

本申请实施例涉及机械控制领域，具体涉及五轴机器人的近似位姿计算方法、装置和存储介质。

背景技术

五轴机器人是一种工业上广泛应用的机器人，具备五个自由度。通用工业机器人通常六轴构型的，机械臂末端在运动范围内能够到达任意一个笛卡尔空间内的位姿。而五轴机器人在结构上少了一个自由度后，所有机械臂都处在同一空间平面中，并不能控制机械臂末端达到机械臂长度内的所有位姿。也就是说，

如果目标位姿的机械臂末端需要到达的空间位置是一个不可达的空间位置，五轴机器人就需要在移动过程中不断进行迭代计算，直至无法对五轴机器人的当前位姿进行改动时，就确认已经到达了近似位姿的空间位置。这种算法需要的计算次数较多，过多占用了五轴机器人控制装置的处理能力。

发明内容

针对上述问题，本申请实施例提供了五轴机器人的近似位姿计算方法、装置和存储介质，用于控制五轴机器人。

本申请第一方面提供了一种五轴机器人的近似位姿调整方法，包括：

获取五轴机器人的基点坐标；

获取目标位姿，所述目标位姿包括目标TCP点的坐标与目标Z轴方向；

根据所述目标TCP点与所述五轴机器人的基点坐标确定近似平面；

将所述目标位姿投影至所述近似平面，得到近似位姿；

按照所述近似位姿调整所述五轴机器人。

可选地，所述根据所述目标位姿的TCP点与所述五轴机器人的基点坐标确定近似平面，包括：

构建第一向量，所述第一向量由所述基点坐标指向所述目标TCP点；

构建第二向量，所述第二向量垂直于水平面，且经过所述五轴机器人的基点坐标；

将所述第一向量和所述第二向量叉乘，确认近似平面的法向量。

可选地，所述将所述目标位姿投影至所述近似平面，得到近似位姿，包括：

根据所述目标Z轴方向构建第三向量，所述第三向量与所述末端机械臂的Z轴方向相同，且所述第三向量经过所述目标TCP点；

将所述第三向量投影至所述近似平面，得到第四向量；

确认所述第四向量所在直线为所述近似位姿下的末端机械臂的Z轴方向；

根据所述末端机械臂的Z轴方向和所述目标TCP点的坐标，计算近似位姿。

可选地，所述按照所述近似位姿调整所述五轴机器人，包括：

获取所述五轴机器人的DH参数模型；

将所述末端机械臂的Z轴方向和所述目标TCP点的坐标代入所述DH参数模型求解，得到所述五轴机器人各机械臂的旋转角度，

根据所述五轴机器人各机械臂的旋转角度调整所述五轴机器人。

本申请第二方面提供一种五轴机器人的近似位姿调整装置，包括：

第一获取单元，用于获取五轴机器人的基点坐标；

第二获取单元，用于获取目标位姿，所述目标位姿包括目标TCP点的坐标与目标Z轴方向；

确定单元，用于根据所述目标TCP点与所述五轴机器人的基点坐标确定近似平面；

投影单元，用于将所述目标位姿投影至所述近似平面，得到近似位姿；

调整单元，用于按照所述近似位姿调整所述五轴机器人。

可选地，所述确定单元具体包括：

第一构建模块，用于构建第一向量，所述第一向量由所述基点坐标指向所述目标TCP点；

第二构建模块，用于构建第二向量，所述第二向量垂直于水平面，且经过所述五轴机器人的基点坐标；

确认模块，用于将所述第一向量和所述第二向量叉乘，确认近似平面的法向量。

可选地，所述投影单元具体包括：

第三构建模块，用于根据所述目标Z轴方向构建第三向量，所述第三向量与所述末端机械臂的Z轴方向相同，且所述第三向量经过所述目标TCP点；

投影模块，用于将所述第三向量投影至所述近似平面，得到第四向量；

确认模块，用于确认所述第四向量所在直线为所述近似位姿下的末端机械臂的Z轴方向；

第一计算模块，用于根据所述目标Z轴方向和所述目标TCP点的坐标，计算近似位姿。

可选地，所述调整单元具体包括：

获取模块，用于获取所述五轴机器人的DH参数模型；

第二计算模块，用于将所述末端机械臂的Z轴方向和所述目标TCP点的坐标代入所述DH参数模型求解，得到所述五轴机器人各机械臂的旋转角度，

调整模块，用于根据所述五轴机器人各机械臂的旋转角度调整所述五轴机器人。

本申请第三方面提供一种五轴机器人的近似位姿计算装置，包括处理器和存储器，所述处理器执行存储器中存储的计算机程序时，实现第一方面中任意一项所述方法的步骤。

本申请第四方面提供，一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现第一方面中任意一项所述方法的步骤。

从以上技术方案可以看出，本申请实施例具有以下优点：本申请的五轴机器人的近似位姿计算方法和五轴机器人控制装置在面对一个不可达的目标位位姿时，先确定一个近似平面来使得机械臂的TCP点达到目标位姿的TCP点，而后通过向近似平面投影的方式，计算出机械臂尽可能接近目标位姿的空间位置，相比传统的数值迭代法，对五轴机器人控制装置的处理能力的要求更低。

附图说明

图1是本申请适用的五轴机器人的一个结构示意图；

图2是本申请五轴机器人的近似位姿计算方法的一个实施例示意图；

图3是本申请五轴机器人的近似位姿计算方法的一个实施例示意图；

图4是本申请五轴机器人的近似位姿计算方法的一个实施例示意图；

图5是本申请五轴机器人的近似位姿计算方法的一个实施例示意图；

图6是本申请五轴机器人的近似位姿计算方法的一个实施例示意图；

图7是本申请五轴机器人的近似位姿计算方法的一个实施例示意图；

图8是本申请五轴机器人的近似位姿计算装置的一个实施例示意图；

图9是本申请五轴机器人的近似位姿计算装置的一个实施例示意图。

具体实施方式

本申请实施例提供了五轴机器人的近似位姿计算方法、装置和存储设备，用于在面对不可达位姿时，控制五轴机器人调整至最接近的近似位姿。

图1出示了本申请适用的一种五轴机器人构型，本机械臂的运动描述不包括图中外轴(夹爪)部分，外轴被认为不属于机械臂本体的一部分，外轴沿本申请中目标位姿的Z轴方向，安装在目标TCP点上。对这种构型，可以看出仅能通过一轴，也就是直接与底部基座连接的关节轴活动，实现空间内绕与大地垂直的竖直轴转动的旋转运动；除第一轴以外，第二、三、四轴的旋转轴都互相平行；当1轴固定时，整个机构所在的平面也随之固定，其他第二到第五轴无论如何旋转也不能够使末端的TCP点离开整个机构所在的平面；这也就是前面所说，这种五自由度构型所缺失的那个旋转自由度。对这样的构型来说，只要在机械臂的臂长可达范围内，所有点的空间位置坐标都是可达的，因为不可达的缺失自由度属于姿态的部分；换句话说，对一个不可达的目标位姿来说，末端机械臂可以准确到达位置坐标(x，y，z)，而姿态只能在机构所在平面内尽量向其靠近。针对这个特点，本发明提出一种十分方便的近似位姿获取法，即将目标位姿向机构所在的机构平面进行投影。可以理解的是，图1仅仅是本申请使用的五轴机器人的一种构型。

图2展示了本申请五轴机器人的近似位姿计算方法的一个实施例，该实施例包括：

201、获取五轴机器人的基点坐标；

本申请适用于固定的五轴机器人，该五轴机器人固定在水平面的基座上。由于缺少一个自由度，该机器人的各个机构会处于同一平面中，称为机构平面。该基座可以进行旋转，一变换机构平面的空间位置，将该基座旋转轴与水平面的交点定义为五轴机器人的基点坐标，通常为方便计算，会定义为(0,0,0)。

202、获取目标位姿；

目标位姿包括目标TCP点的坐标与目标Z轴方向，控制器要求机械臂末端抵达工具中心点(Tool Center Point,TCP)，且机械臂末端指向的方向与Z轴方向相同。但是由于五轴机器人的机械臂仅有5个自由度，因此五轴机器人并不能达到长度范围内的任意位姿。本申请针对的就是目标位姿是不可到达位姿的情况下，计算一个近似位姿，控制五轴机器人到达该近似位姿，从而尽可能地接近目标位姿。

203、根据所述目标TCP点与所述五轴机器人的基点坐标确定近似平面；

五轴机器人的机械臂在运动中会处在同一平面上，称为机构平面。由于近似位姿的TCP点与目标位姿的TCP点相同，因此目标TCP点就处在近似位姿的机构平面上。本申请中所说的末端机械臂，是指在TCP点上安装的工具，该工具指向的方向称为末端机械臂的Z轴方向。机构平面垂直于水平面，在水平面上寻找一个垂直于水平面、且经过基点坐标与目标TCP点的机构平面，该机构平面就是本申请的近似平面，当五轴机器人处于近似平面时，五轴机器人的所有机械臂都处在近似平面上。

204、将所述目标位姿投影至所述近似平面，得到近似位姿；

如图3、图4所示，将目标位姿下末端机械臂的Z轴和投影到近似平面上，得到的投影记为Z’轴。该Z’轴就是近似位姿下五轴机器人末端机械臂的所指向的方向。然后根据Z’轴确认近似位姿下末端机械臂的X’轴与Y’轴的朝向，就获得了末端机械臂在近似位姿下的朝向与TCP点的坐标。

205、按照所述近似位姿调整所述五轴机器人。

计算五轴机器人各机械臂需要运动的角度，然后根据计算结果调整五轴机器人的位姿至近似位姿，当五轴机器人完全按照计算结果调整到近似位姿时，五轴机器人的末端TCP点会与目标TCP点重合，五轴机器人的各机械臂都处在该近似平面上。

在一个详细实施例中，还提供了确定近似平面的具体步骤。请参阅图5，步骤203可以具体包括：

2031、构建第一向量，所述第一向量由所述基点坐标指向所述目标TCP点；

根据基点坐标和目标TCP点的坐标，构建第一向量。需要说明的是，第一向量的选取仅对方向有要求，而对长度没有要求，因此第一向量的长度可以调整。

2032、构建第二向量，所述第二向量垂直于水平面，且经过所述五轴机器人的基点坐标；

从基座位置的基点坐标触发，垂直于水平面向上作一个第二向量。如果基点坐标为(0,0,0)，那么第二向量的可以选取为(0，0,1)。与第一向量类似，第二向量对于长度没有要求。

2033、将第一向量和第二向量叉乘，确认近似平面的法向量。

将第一向量与第二向量进行叉乘，可以得到近似平面(最接近目标位姿的机构平面)的法向量。该法向量和近似平面上的任意一个向量可以方便地表示该近似平面在空间中的位置。特别地，由于该五轴机器人的基座是固定的，机构平面只能围绕以基座中心为旋转轴旋转，因此知晓近似平面的法向量和基点坐标，就能确定机构平面的旋转角度。

在本申请的另一实施例中，还展示了目标位姿投影的一种具体方法。具体请参阅图6，在图2或图6所示实施例的基础上，步骤204具体包括：

2041、根据所述目标Z轴方向构建第三向量；

构建一个与末端机械臂Z轴方向相同，且经过所述目标TCP点的第三向量。。从目标TCP点出发，沿目标位姿提供的末端机械臂Z轴方向，构建一个任意长度的第三向量。

2042、将所述第三向量投影至所述近似平面，得到第四向量；

在步骤203中，已经确认了近似平面的具体位置。将第三向量投影到近似平面上，将投影得到的向量记为第四向量。

2043、确认所述第四向量所在直线为所述近似位姿下的末端机械臂的Z轴方向；

第四向量由第三向量在近似平面上投影得到，且第三向量又与目标位姿下末端机械臂Z轴重合，因此第四向量相当于目标位姿下末端机械臂Z轴的投影。第四向量即表示了近似位姿下末端机械臂Z轴的朝向。

2044、根据所述目标Z轴方向和所述目标TCP点的坐标，计算近似位姿。

已知近似位姿下末端机械臂的Z轴方向和末端机械臂上的TCP点后，逆推其他各机械臂的旋转角度，可以计算出近似位姿下其他机械臂的旋转角度，以便控制机械臂按照计算的结果进行动作调整。

需要说明的是，本实施例仅仅提供了一种较优的投影计算过程，本领域技术人员可以酌情使用其他的计算方式来获得目标位姿在近似平面上的投影，具体此处不作限定。

基于图2、图5或6所示的实施例，在一个更详细的实施例中，步骤205还包括计算五轴机器人其他机械臂三轴坐标的过程，具体请参阅图7，步骤205可以包括：

2051、获取所述五轴机器人的DH参数模型；

DH参数模型就是一个用四个参数表达两对关节连杆之间位置角度关系的机械臂数学模型和坐标系确定系统，它只需要用四个参数即可表达关节之间原本是六自由度的坐标变换。获取该五轴机器人对应的DH参数模型，其中标注了五轴机器人每个机械臂的长度和活动角度。

2052、将所述目标Z轴方向和所述目标TCP点的坐标代入所述DH参数模型求解，得到所述五轴机器人各机械臂的旋转角度。

本方案已经在步骤204中获得了末端机械臂的Z轴方向和TCP点坐标，Y轴方向可以默认与近似平面垂直，那么Z轴和Y轴叉乘即可得到X轴方向。将末端机械臂的四个参数输入DH模型进行逆解，即可得到其他四个机械臂的三轴坐标，确定所有机械臂的旋转角度。可以理解的是，在一些情况下，末端机械臂的X轴或Y轴是已知的，仍然可以使用本实施例的方法，求解末端机械臂的三轴坐标。

2053、根据所述五轴机器人各机械臂的旋转角度调整所述五轴机器人。

根据步骤2052中得到的五个机械臂的三轴坐标，对五轴机器人的各机械臂的旋转角度进行变动，最后使得五轴机器人在到达接近于目标位姿的近似位姿。

可以理解的是，上述根据DH模型计算五轴机器人机械臂三轴坐标，调整各机械臂旋转角度的方法仅仅是一种较优的实施方式，本领域技术人员可以通过其他方式求解五轴机器人各个机械臂的旋转角度，具体此处不作限定。

可以看到，本申请实施例的五轴机器人的近似位姿计算方法不包括迭代计算的步骤，控制器可以使用常规的向量计算，确定一个接近于目标位姿的近似位姿，使得五轴机器人面对不可达位姿时能够快速做出相应。

为实现上述方法，本申请实施例还提供了一种五轴机器人的近似位置计算装置，具体请参阅图8，该装置包括：

第一获取单元801，用于获取五轴机器人的基点坐标；

第二获取单元802，用于获取目标位姿，所述目标位姿包括目标TCP点的坐标与目标Z轴方向；

确定单元803，用于根据所述目标TCP点与所述五轴机器人的基点坐标确定近似平面；

投影单元804，用于将所述目标位姿投影至所述近似平面，得到近似位姿；

调整单元805，用于按照所述近似位姿调整所述五轴机器人。

可选地，所述确定单元803具体包括：

第一构建模块，用于构建第一向量，所述第一向量由所述基点坐标指向所述目标TCP点；

第二构建模块，用于构建第二向量，所述第二向量垂直于水平面，且经过所述五轴机器人的基点坐标；

确认模块，用于将所述第一向量和所述第二向量叉乘，确认近似平面的法向量。

可选地，所述投影单元804具体包括：

第三构建模块，用于根据所述目标Z轴方向构建第三向量，所述第三向量与所述末端机械臂的Z轴方向相同，且所述第三向量经过所述目标TCP点；

投影模块，用于将所述第三向量投影至所述近似平面，得到第四向量；

确认模块，用于确认所述第四向量所在直线为所述近似位姿下的末端机械臂的Z轴方向；

第一计算模块，用于根据所述目标Z轴方向和所述目标TCP点的坐标，计算近似位姿。

可选地，所述调整单元805具体包括：

获取模块，用于获取所述五轴机器人的DH参数模型；

第二计算模块，用于将所述末端机械臂的Z轴方向和所述目标TCP点的坐标代入所述DH参数模型求解，得到所述五轴机器人各机械臂的旋转角度，

调整模块，用于根据所述五轴机器人各机械臂的旋转角度调整所述五轴机器人。

本发明还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时，可以实现如下步骤：

获取五轴机器人的基点坐标；

获取目标位姿，所述目标位姿包括目标TCP点的坐标与目标Z轴方向；

根据所述目标TCP点与所述五轴机器人的基点坐标确定近似平面；

将所述目标位姿投影至所述近似平面，得到近似位姿；

按照所述近似位姿调整所述五轴机器人。

作为一种可能的实施方式，处理器还可以用于实现如下步骤：

构建第一向量，所述第一向量由所述基点坐标指向所述目标TCP点；

构建第二向量，所述第二向量垂直于水平面，且经过所述五轴机器人的基点坐标；

将所述第一向量和所述第二向量叉乘，确认近似平面的法向量。

作为一种可能的实施方式，处理器还可以用于实现如下步骤：

根据所述目标Z轴方向构建第三向量，所述第三向量与所述末端机械臂的Z轴方向相同，且所述第三向量经过所述目标TCP点；

将所述第三向量投影至所述近似平面，得到第四向量；

确认所述第四向量所在直线为所述近似位姿下的末端机械臂的Z轴方向；

根据所述末端机械臂的Z轴方向和所述目标TCP点的坐标，计算近似位姿。

作为一种可能的实施方式，处理器还可以用于实现如下步骤：

获取所述五轴机器人的DH参数模型；

将所述末端机械臂的Z轴方向和所述目标TCP点的坐标代入所述DH参数模型求解，得到所述五轴机器人各机械臂的旋转角度，

根据所述五轴机器人各机械臂的旋转角度调整所述五轴机器人。

图9展示了五轴机器人的近似位姿计算装置的另一结构示意图。五轴机器人的近似位姿计算装置900可以包括一个或一个以上中央处理器(central processing units，CPU)901和存储器905，该存储器905中存储有一个或一个以上的应用程序或数据。其中，存储器905可以是易失性存储或持久存储。存储在存储器905的程序可以包括一个或一个以上模块，每个模块可以包括对五轴机器人的近似位姿计算装置900中的一系列指令操作。更进一步地，中央处理器901可以设置为与存储器905通信，在五轴机器人的近似位姿计算装置900上执行存储器905中的一系列指令操作。五轴机器人的近似位姿计算装置900还可以包括一个或一个以上电源902，一个或一个以上有线或无线网络接口903，一个或一个以上输入输出接口904，和/或，一个或一个以上操作系统，例如Windows ServerTM，Mac OS XTM，UnixTM,LinuxTM，FreeBSDTM等。该中央处理器901可以执行前述任一实施例的对应操作，具体此处不再赘述。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，read-onlymemory)、随机存取存储器(RAM，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。