五轴装卸机器人的逆解方法、装置及可读介质

1.本发明涉及自动化装卸设备逆解方法领域，具体涉及一种五轴装卸机器人的逆解方法、装置及可读介质。


背景技术：

2.货物在装车前通常是打包在纸箱或木箱里，长方体的箱包可以整齐得摆放在货车车厢内，更节省空间，提高运输效率。
3.随着近几年制造业、食品业、物流业、电商业的蓬勃发展，“体积小、重量轻”的货物箱包越来越多的出现在了装卸车环节中，叉车一类的传统搬运工具在面对小型化、轻量化的货物箱包装卸任务时，效率很低，极不实用。人工搬运劳动强度太大，且效率同样很低，往往还会出现招不到搬运工人的尴尬局面。现有物流环节中存在“体积小、重量轻、数量大”的箱包难以自动化装卸的问题，并且装卸效率低，人工成本高，因此，设计专用的自动化装卸设备，成为了解决该问题的有效途径。


技术实现要素：

4.针对上述提到的技术问题。本技术的实施例的目的在于提出了一种五轴装卸机器人的逆解方法、装置及可读介质，来解决以上背景技术部分提到的技术问题。
5.第一方面，本发明提供了一种五轴装卸机器人的逆解方法，包括以下步骤：
6.s1，基于五轴装卸机器人的机械臂三维模型建立其连杆坐标系，并建立各坐标系之间的转换矩阵，其中，五轴装卸机器人包括第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂、横向旋转臂、第一支撑臂和第二支撑臂；
7.s2，在机器人基坐标系{ob}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{oj5d}下设定第二控制点和第二控制方向，根据转换矩阵建立第二控制点和第二控制方向与第一控制点和第一控制方向之间转换的运动学关系；
8.s3，根据等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θn为第n个旋转轴jn的旋转角，n＝1、2、3、4、5；
9.s4，根据θ2和θ3计算出第一支撑臂的臂长d1和第二支撑臂的臂长d2。
10.作为优选，五轴装卸机器人还包括可移动底座、旋转平台，旋转平台所绕的旋转轴为j1，第一纵向旋转臂与旋转平台连接，两者间的旋转轴为j2；第二纵向旋转臂与第一纵向旋转臂连接，两者间的旋转轴为j3；第三纵向旋转臂与第二纵向旋转臂连接，两者间的旋转轴为j4；横向旋转臂与第三纵向旋转臂连接，两者间的旋转轴为j5；第一支撑臂的一侧安装在第一纵向旋转臂上，另一侧安装在旋转平台上，s1为第一支撑臂在第一纵向旋转臂上的安装尺寸，s2为第一支撑臂在旋转平台上的安装尺寸；第二支撑臂的一侧安装在第二纵向旋转臂上，另一侧安装在旋转平台上，s3为第二支撑臂在第二纵向旋转臂上的安装尺寸，s4为第二支撑臂在旋转平台上的安装尺寸。
11.作为优选，连杆坐标系包括机器人基坐标系{ob}、第一关节轴静坐标系{oj1s}、第
一关节轴动坐标系{oj1d}、第二关节轴静坐标系{oj2s}、第二关节轴动坐标系{oj2d}、第三关节轴静坐标系{oj3s}、第三关节轴动坐标系{oj3d}、第四关节轴静坐标系{oj4s}、第四关节轴动坐标系{oj4d}、第五关节轴静坐标系{oj5s}、第五关节轴动坐标系{oj5d}；
12.依次建立以上相邻两个坐标系之间的转换矩阵
[0013][0014][0015][0016][0017][0018][0019][0020]
[0021][0022][0023][0024]
其中，ln为第n个关节臂的臂长。
[0025]
作为优选，步骤s2具体包括：
[0026]
在机器人基坐标系{ob}下设定第一控制点bp＝[x y z 1]
t
和第一控制方向bv＝[i j k 0]
t
，在第五关节轴动坐标系{oj5d}下设定第二控制点
j5d
p＝[0 l
5 0 1]
t
和第二控制方向
j5d
v＝[0 1 0 0]
t
；
[0027]
将第五关节轴动坐标系{oj5d}下的第二控制点和第二控制方向转化到机器人基坐标系{ob}下，建立等式关系：
[0028][0029]
作为优选，步骤s3包括：
[0030]
在机器人基坐标系{ob}下计算第五关节轴动坐标系{oj5d}的原点坐标bp
oj5d
＝[x
oj5d y
oj5d z
oj5d 1]
t
，计算公式为：bp
oj5d
＝bp-l5*norm(bv)，其中norm(bv)为向量bv的单位化向量，展开后得到如下结果：
[0031][0032]
θ1由x
oj5d
与y
oj5d
决定，计算公式如下：
[0033]
θ1＝-arctan2(x
oj5d
,y
oj5d
)；
[0034]
第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂共同构建成为纵向旋转面，在机器人基坐标系{ob}下计算纵向旋转面的单位法向量
bvver-lon-plane
：
[0035][0036]
θ5为第一控制方向bv＝[i j k 0]
t
与纵向旋转面之间的夹角，因此θ5的计算公式如下：
[0037][0038]
同时求出第一控制方向bv＝[i j k 0]
t
在纵向旋转面内的投影向量
bvpro-in-lon-plane
，公式如下：
[0039]
bvpro-in-lon-plane
＝norm(bv)-(norm(bv)
·
bvver-lon-plane
)*
bvver-lon-plane
；
[0040]
在机器人基坐标系{ob}下计算第四关节轴动坐标系{oj4d}的原点坐标bp
oj4d
＝[x
oj4d y
oj4d z
oj4d 1]
t
，公式如下：
[0041]bp
oj4d
＝bp
oj5d-l4*norm(
bvpro-in-lon-plane
)；
[0042]
在机器人基坐标系{ob}下计算第三纵向旋转臂在纵向旋转面内与旋转平台的夹角bc
j4
，公式如下：
[0043]bc
j4
＝arccos(norm(
bvpro-in-lon-plane
)
·
[0 0 1 0]
t
)；
[0044]
在第二关节轴静坐标系{oj2s}内，求解θ2、θ3、θ4，公式如下：
[0045][0046][0047]
作为优选，步骤s4具体采用下式将θ2、θ3转换成d1、d2：
[0048][0049]
作为优选，还包括：采用伺服液压缸间接控制θ2、θ3，并将θ1、d1、d2、θ4、θ5发送给各
自对应关节上的驱动器和电机，实现横向旋转臂在笛卡尔空间中五自由度的运动。
[0050]
第二方面，本发明提供了一种五轴装卸机器人的逆解装置，包括：
[0051]
坐标建立模块，被配置为基于五轴装卸机器人的机械臂三维模型建立其连杆坐标系，并建立各坐标系之间的转换矩阵，其中，五轴装卸机器人包括第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂、横向旋转臂、第一支撑臂和第二支撑臂；
[0052]
运动学关系构建模块，被配置为在机器人基坐标系{ob}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{oj5d}下设定第二控制点和第二控制方向，根据转换矩阵建立第二控制点和第二控制方向与第一控制点和第一控制方向之间转换的运动学关系；
[0053]
第一求解模块，被配置为根据等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θn为第n个旋转轴jn的旋转角，n＝1、2、3、4、5；
[0054]
第二求解模块，被配置为根据θ2和θ3计算出第一支撑臂的臂长d1和第二支撑臂的臂长d2。
[0055]
第三方面，本发明提供了一种电子设备，包括一个或多个处理器；存储装置，用于存储一个或多个程序，当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现如第一方面中任一实现方式描述的方法。
[0056]
第四方面，本发明提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如第一方面中任一实现方式描述的方法。
[0057]
相比于现有技术，本发明具有以下有益效果：
[0058]
(1)本发明通过对五轴装卸机器人进行运动学建模推导运动学关系，并利用几何法求出逆解，实现了五轴装卸机器人末端连杆的五自由度运动控制。
[0059]
(2)本发明解决了物流环节中“体积小、重量轻、数量大”的箱包难以自动化装卸的问题，提高了装卸效率并降低了人工成本。
附图说明
[0060]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0061]
图1是本技术的一个实施例可以应用于其中的示例性装置架构图；
[0062]
图2为本技术的实施例的五轴装卸机器人的逆解方法的流程示意图；
[0063]
图3为本技术的实施例的五轴装卸机器人的逆解方法的五轴装卸机器人的正视图；
[0064]
图4为本技术的实施例的五轴装卸机器人的逆解方法的五轴装卸机器人的俯视图；
[0065]
图5为本技术的实施例的五轴装卸机器人的逆解方法的建立运动学关系涉及的坐标系示意图；
[0066]
图6为本技术的实施例的五轴装卸机器人的逆解方法的求解d1、d2时的几何关系示意图；
[0067]
图7为本技术的实施例的五轴装卸机器人的逆解装置的示意图；
[0068]
图8是适于用来实现本技术实施例的电子设备的计算机装置的结构示意图。
具体实施方式
[0069]
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0070]
图1示出了可以应用本技术实施例的五轴装卸机器人的逆解方法或五轴装卸机器人的逆解装置的示例性装置架构100。
[0071]
如图1所示，装置架构100可以包括终端设备101、102、103，网络104和服务器105。网络104用以在终端设备101、102、103和服务器105之间提供通信链路的介质。网络104可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
[0072]
用户可以使用终端设备101、102、103通过网络104与服务器105交互，以接收或发送消息等。终端设备101、102、103上可以安装有各种应用，例如数据处理类应用、文件处理类应用等。
[0073]
终端设备101、102、103可以是硬件，也可以是软件。当终端设备101、102、103为硬件时，可以是各种电子设备，包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。当终端设备101、102、103为软件时，可以安装在上述所列举的电子设备中。其可以实现成多个软件或软件模块(例如用来提供分布式服务的软件或软件模块)，也可以实现成单个软件或软件模块。在此不做具体限定。
[0074]
服务器105可以是提供各种服务的服务器，例如对终端设备101、102、103上传的文件或数据进行处理的后台数据处理服务器。后台数据处理服务器可以对获取的文件或数据进行处理，生成处理结果。
[0075]
需要说明的是，本技术实施例所提供的五轴装卸机器人的逆解方法可以由服务器105执行，也可以由终端设备101、102、103执行，相应地，五轴装卸机器人的逆解装置可以设置于服务器105中，也可以设置于终端设备101、102、103中。
[0076]
应该理解，图1中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。在所处理的数据不需要从远程获取的情况下，上述装置架构可以不包括网络，而只需服务器或终端设备。
[0077]
图2示出了本技术的实施例提供的一种五轴装卸机器人的逆解方法，包括以下步骤：
[0078]
s1，基于五轴装卸机器人的机械臂三维模型建立其连杆坐标系，并建立各坐标系之间的转换矩阵，其中，五轴装卸机器人包括第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂、横向旋转臂、第一支撑臂和第二支撑臂。
[0079]
具体的，参考图3-6，该五轴装卸机器人包括一个可移动底座1，一个旋转平台2，三个纵向旋转臂，一个横向旋转臂6，两个支撑臂，即五轴装卸机器人包括第一纵向旋转臂3、第二纵向旋转臂4、第三纵向旋转臂5、横向旋转臂6、第一支撑臂7和第二支撑臂8。该五轴装卸机器人用于搬运货堆9，可移动底座1可以实现装卸机器人的整体空间运动，当机器人的目标位置超出了可达范围时，才会进行整体运动。旋转平台2所绕的旋转轴为j1，其他设备
都安装在旋转平台2上。第一纵向旋转臂3与旋转平台2连接，两者间的旋转轴为j2；第二纵向旋转臂4与第一纵向旋转臂3连接，两者间的旋转轴为j3；第三纵向旋转臂5与第二纵向旋转臂4连接，两者间的旋转轴为j4；横向旋转臂6与第三纵向旋转臂5连接，两者间的旋转轴为j5；第一支撑臂7的一侧安装在第一纵向旋转臂3上，另一侧安装在旋转平台2上，s1为第一支撑臂7在第一纵向旋转臂3上的安装尺寸，s2为第一支撑臂7在旋转平台2上的安装尺寸；第二支撑臂8的一侧安装在第二纵向旋转臂4上，另一侧安装在旋转平台2上，s3为第二支撑臂8在第二纵向旋转臂4上的安装尺寸，s4为第二支撑臂8在旋转平台2上的安装尺寸。l1为第一个关节臂的臂长，即旋转轴j2到旋转轴j1的长度；l2为第二个关节臂的臂长，即旋转轴j3到旋转轴j2的长度，也就是第一纵向旋转臂3的长度；l3为第三个关节臂的臂长，即旋转轴j4到旋转轴j3的长度，也就是第二纵向旋转臂4的长度；l4为第四个关节臂的臂长，即旋转轴j5到旋转轴j4的长度，也就是第三纵向旋转臂5的长度；l5为第五个关节臂的臂长，即为横向旋转臂6的长度。
[0080]
在具体的实施例中，参考图5，步骤s1具体包括：连杆坐标系包括机器人基坐标系{ob}、第一关节轴静坐标系{oj1s}、第一关节轴动坐标系{oj1d}、第二关节轴静坐标系{oj2s}、第二关节轴动坐标系{oj2d}、第三关节轴静坐标系{oj3s}、第三关节轴动坐标系{oj3d}、第四关节轴静坐标系{oj4s}、第四关节轴动坐标系{oj4d}、第五关节轴静坐标系{oj5s}、第五关节轴动坐标系{oj5d}；
[0081]
依次建立以上相邻两个坐标系之间的转换矩阵依次建立以上相邻两个坐标系之间的转换矩阵
[0082][0083][0084][0085]
[0086][0087][0088][0089][0090][0091][0092]
其中，ln为第n个关节臂的臂长。
[0093]
上述转换矩阵中涉及的l1、l2、l3、l4均为机械结构长度尺寸或安装距离尺寸，参考图3，都是已知量。
[0094]
s2，在机器人基坐标系{ob}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{oj5d}下设定第二控制点和第二控制方向，根据转换矩阵建立第二控制点和第二控制方向与第一控制点和第一控制方向之间转换的运动学关系。
[0095]
在具体的实施例中，步骤s2具体包括：
[0096]
建立运动学关系，在机器人基坐标系{ob}下设定第一控制点bp＝[x y z 1]
t
和第一控制方向bv＝[i j k 0]
t
，在第五关节轴动坐标系{oj5d}下设定第二控制点
j5d
p＝[0 l
5 0 1]
t
和第二控制方向
[0097]
将第五关节轴动坐标系{oj5d}下的第二控制点和第二控制方向转化到机器人基
坐标系{ob}下，建立等式关系：
[0098][0099]
具体的，上式中未知数为五个旋转角θ1、θ2、θ3、θ4、θ5。机器人基坐标系{ob}下设定的第一控制点bp＝[x y z 1]
t
和第一控制方向bv＝[i j k 0]
t
是已知量，在本技术涉及的场景中，可由测距设备或视觉设备测量获得原始数据，再转换成加工代码(g代码)输入控制系统。
[0100]
s3，根据等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θn为第n个旋转轴jn的旋转角，n＝1、2、3、4、5。
[0101]
在具体的实施例中，步骤s3包括：
[0102]
在机器人基坐标系{ob}下计算第五关节轴动坐标系{oj5d}的原点坐标bp
oj5d
＝[x
oj5d y
oj5d z
oj5d 1]
t
，计算公式为：bp
oj5d
＝bp-l5*norm(bv)，其中norm(bv)为向量bv的单位化向量，展开后得到如下结果：
[0103][0104]
在图4中，θ1由x
oj5d
与y
oj5d
决定，计算公式如下：
[0105]
θ1＝-arctan 2(x
oj5d
,y
oj5d
)；
[0106]
第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂共同构建成为纵向旋转面，bp
oj5d
已求出，进而可在机器人基坐标系{ob}下计算纵向旋转面的单位法向量
bvver-lon-plane
：
[0107][0108]
θ5本质上是第一控制方向bv＝[i j k 0]
t
与纵向旋转面之间的夹角，纵向旋转面的法向量
bvver-lon-plane
已知，因此θ5的计算公式如下：
[0109][0110]
同时求出第一控制方向bv＝[i j k 0]
t
在纵向旋转面内的投影向量
bvpro-in-lon-plane
，公式如下：
[0111]
bvpro-in-lon-plane
＝norm(bv)-(norm(bv)
·
bvver-lon-plane
)*
bvver-lon-plane
；
[0112]
在机器人基坐标系{ob}下计算第四关节轴动坐标系{oj4d}的原点坐标bp
oj4d
＝[x
oj4d y
oj4d z
oj4d 1]
t
，公式如下：
[0113]bp
oj4d
＝bp
oj5d-l4*norm(
bvpro-in-lon-plane
)；
[0114]
在机器人基坐标系{ob}下计算第三纵向旋转臂在纵向旋转面内与旋转平台的夹角bc
j4
，公式如下：
[0115]bc
j4
＝arccos(norm(
bvpro-in-lon-plane
)
·
[0 0 1 0]
t
)；
[0116]
在第二关节轴静坐标系{oj2s}内，求解θ2、θ3、θ4，公式如下：
[0117][0118]
s4，根据θ2和θ3计算出第一支撑臂的臂长d1和第二支撑臂的臂长d2。
[0119]
在具体的实施例中，参考图6，为计算过程中的辅助角，设第二支撑臂与旋转平台的交点为a2，在三角形a2j2j3中，j2j3与j3a2的夹角即为设第二纵向旋转臂与第二支撑臂的交点为a1，在三角形a1j3a2中，j3a2与j3a1的夹角即为因此步骤s4具体采用下式将θ2、θ3转换成d1、d2：
[0120][0121]
具体的，利用三角函数关系可将θ2、θ3转换成d1、d2，因为θ1、θ4、θ5三个旋转角可由安装在关节上的电机旋转实现角度调整，而θ2、θ3所处的位置等效转动惯量较大，若直接用电机在关节上进行驱动，稳定性难以保证，所以采用伺服液压缸间接控制θ2、θ3。
[0122]
在具体的实施例中，还包括：采用伺服液压缸间接控制θ2、θ3，并将θ1、d1、d2、θ4、θ5发送给各自对应关节上的驱动器和电机，实现横向旋转臂在笛卡尔空间中五自由度的运动。
[0123]
进一步参考图7，作为对上述各图所示方法的实现，本技术提供了一种五轴装卸机器人的逆解装置的一个实施例，该装置实施例与图2所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。
[0124]
本技术实施例提供了一种五轴装卸机器人的逆解装置，包括：
[0125]
坐标建立模块1，被配置为基于五轴装卸机器人的机械臂三维模型建立其连杆坐
标系，并建立各坐标系之间的转换矩阵，其中，五轴装卸机器人包括第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂、横向旋转臂、第一支撑臂和第二支撑臂；
[0126]
运动学关系构建模块2，被配置为在机器人基坐标系{ob}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{oj5d}下设定第二控制点和第二控制方向，根据转换矩阵建立第二控制点和第二控制方向与第一控制点和第一控制方向之间转换的运动学关系；
[0127]
第一求解模块3，被配置为根据等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θn为第n个旋转轴jn的旋转角，n＝1、2、3、4、5；
[0128]
第二求解模块4，被配置为根据θ2和θ3计算出第一支撑臂的臂长d1和第二支撑臂的臂长d2。
[0129]
下面参考图8，其示出了适于用来实现本技术实施例的电子设备(例如图1所示的服务器或终端设备)的计算机装置800的结构示意图。图8示出的电子设备仅仅是一个示例，不应对本技术实施例的功能和使用范围带来任何限制。
[0130]
如图8所示，计算机装置800包括中央处理单元(cpu)801和图形处理器(gpu)802，其可以根据存储在只读存储器(rom)803中的程序或者从存储部分809加载到随机访问存储器(ram)804中的程序而执行各种适当的动作和处理。在ram 804中，还存储有装置800操作所需的各种程序和数据。cpu 801、gpu802、rom 803以及ram 804通过总线805彼此相连。输入/输出(i/o)接口806也连接至总线805。
[0131]
以下部件连接至i/o接口806：包括键盘、鼠标等的输入部分807；包括诸如、液晶显示器(lcd)等以及扬声器等的输出部分808；包括硬盘等的存储部分809；以及包括诸如lan卡、调制解调器等的网络接口卡的通信部分810。通信部分810经由诸如因特网的网络执行通信处理。驱动器811也可以根据需要连接至i/o接口806。可拆卸介质812，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器811上，以便于从其上读出的计算机程序根据需要被安装入存储部分809。
[0132]
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分810从网络上被下载和安装，和/或从可拆卸介质812被安装。在该计算机程序被中央处理单元(cpu)801和图形处理器(gpu)802执行时，执行本技术的方法中限定的上述功能。
[0133]
需要说明的是，本技术所述的计算机可读介质可以是计算机可读信号介质或者计算机可读介质或者是上述两者的任意组合。计算机可读介质例如可以是——但不限于——电、磁、光、电磁、红外线或半导体的装置、装置或器件，或者任意以上的组合。计算机可读介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd-rom)、光存储器件、磁存储器件或者上述的任意合适的组合。在本技术中，计算机可读介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行装置、装置或者器件使用或者与其结合使用。而在本技术中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程
序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行装置、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、rf等等，或者上述的任意合适的组合。
[0134]
可以以一种或多种程序设计语言或其组合来编写用于执行本技术的操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如java、smalltalk、c++，还包括常规的过程式程序设计语言—诸如“c”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(lan)或广域网(wan)—连接到用户计算机，或者，也可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
[0135]
附图中的流程图和框图，图示了按照本技术各种实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，该模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的装置来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0136]
描述于本技术实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中。
[0137]
作为另一方面，本技术还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：基于五轴装卸机器人的机械臂三维模型建立其连杆坐标系，并建立各坐标系之间的转换矩阵，其中，五轴装卸机器人包括第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂、横向旋转臂、第一支撑臂和第二支撑臂；在机器人基坐标系{ob}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{oj5d}下设定第二控制点和第二控制方向，根据转换矩阵建立第二控制点和第二控制方向与第一控制点和第一控制方向之间转换的运动学关系；根据等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θn为第n个旋转轴jn的旋转角，n＝1、2、3、4、5；根据θ2和θ3计算出第一支撑臂的臂长d1和第二支撑臂的臂长d2。
[0138]
以上描述仅为本技术的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本技术中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本技术中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

技术特征：
1.一种五轴装卸机器人的逆解方法，其特征在于，包括以下步骤：s1，基于五轴装卸机器人的机械臂三维模型建立其连杆坐标系，并建立各坐标系之间的转换矩阵，其中，五轴装卸机器人包括第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂、横向旋转臂、第一支撑臂和第二支撑臂；s2，在机器人基坐标系{ob}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{oj5d}下设定第二控制点和第二控制方向，根据所述转换矩阵建立所述第二控制点和第二控制方向与所述第一控制点和第一控制方向之间转换的运动学关系；s3，根据所述等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θ
n
为第n个旋转轴j
n
的旋转角，n＝1、2、3、4、5；s4，根据θ2和θ3计算出第一支撑臂的臂长d1和第二支撑臂的臂长d2。2.根据权利要求1所述的五轴装卸机器人的逆解方法，其特征在于，所述五轴装卸机器人还包括可移动底座、旋转平台，所述旋转平台所绕的旋转轴为j1，所述第一纵向旋转臂与所述旋转平台连接，两者间的旋转轴为j2；所述第二纵向旋转臂与第一纵向旋转臂连接，两者间的旋转轴为j3；所述第三纵向旋转臂与第二纵向旋转臂连接，两者间的旋转轴为j4；所述横向旋转臂与第三纵向旋转臂连接，两者间的旋转轴为j5；所述第一支撑臂的一侧安装在第一纵向旋转臂上，另一侧安装在旋转平台上，s1为第一支撑臂在第一纵向旋转臂上的安装尺寸，s2为第一支撑臂在旋转平台上的安装尺寸；所述第二支撑臂的一侧安装在第二纵向旋转臂上，另一侧安装在旋转平台上，s3为第二支撑臂在第二纵向旋转臂上的安装尺寸，s4为第二支撑臂在旋转平台上的安装尺寸。3.根据权利要求2所述的五轴装卸机器人的逆解方法，其特征在于，所述步骤s1具体包括：所述连杆坐标系包括机器人基坐标系{ob}、第一关节轴静坐标系{oj1s}、第一关节轴动坐标系{oj1d}、第二关节轴静坐标系{oj2s}、第二关节轴动坐标系{oj2d}、第三关节轴静坐标系{oj3s}、第三关节轴动坐标系{oj3d}、第四关节轴静坐标系{oj4s}、第四关节轴动坐标系{oj4d}、第五关节轴静坐标系{oj5s}、第五关节轴动坐标系{oj5d}；依次建立以上相邻两个坐标系之间的转换矩阵依次建立以上相邻两个坐标系之间的转换矩阵依次建立以上相邻两个坐标系之间的转换矩阵依次建立以上相邻两个坐标系之间的转换矩阵
其中，l
n
为第n个关节臂的臂长。4.根据权利要求3所述的五轴装卸机器人的逆解方法，其特征在于，所述步骤s2具体包括：
在机器人基坐标系{ob}下设定第一控制点
b
p＝[x y z 1]
t
和第一控制方向
b
v＝[i j k 0]
t
，在第五关节轴动坐标系{oj5d}下设定第二控制点
j5d
p＝[0 l
5 0 1]
t
和第二控制方向
j5d
v＝[0 1 0 0]
t
；将第五关节轴动坐标系{oj5d}下的第二控制点和第二控制方向转化到机器人基坐标系{ob}下，建立所述等式关系：5.根据权利要求4所述的五轴装卸机器人的逆解方法，其特征在于，所述步骤s3包括：在机器人基坐标系{ob}下计算第五关节轴动坐标系{oj5d}的原点坐标
b
p
oj5d
＝[x
oj5d y
oj5d z
oj5d 1]
t
，计算公式为：
b
p
oj5d
＝
b
p-l5*norm(
b
v)，其中norm(
b
v)为向量
b
v的单位化向量，展开后得到如下结果：θ1由x
oj5d
与y
oj5d
决定，计算公式如下：θ1＝-arctan2(x
oj5d
,y
oj5d
)；第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂共同构建成为纵向旋转面，在机器人基坐标系{ob}下计算纵向旋转面的单位法向量
bvver-lon-plane
：θ5为第一控制方向
b
v＝[i j k 0]
t
与所述纵向旋转面之间的夹角，因此θ5的计算公式如下：同时求出第一控制方向
b
v＝[i j k 0]
t
在纵向旋转面内的投影向量
bvpro-in-lon-plane
，公式如下：
bvpro-in-lon-plane
＝norm(
b
v)-(norm(
b
v)
·
bvver-lon-plane
)*
bvver-lon-plane
；在机器人基坐标系{ob}下计算第四关节轴动坐标系{oj4d}的原点坐标
b
p
oj4d
＝[x
oj4d y
oj4d z
oj4d 1]
t
，公式如下：
b
p
oj4d
＝
b
p
oj5d-l4*norm(
bvpro-in-lon-plane
)；在机器人基坐标系{ob}下计算第三纵向旋转臂在纵向旋转面内与旋转平台的夹角
b
c
j4
，公式如下：
b
c
j4
＝arccos(norm(
bvpro-in-lon-plane
)
·
[0 0 1 0]
t
)；在第二关节轴静坐标系{oj2s}内，求解θ2、θ3、θ4，公式如下：6.根据权利要求5所述的五轴装卸机器人的逆解方法，其特征在于，所述步骤s4具体采用下式将θ2、θ3转换成d1、d2：7.根据权利要求1所述的五轴装卸机器人的逆解方法，其特征在于，还包括：采用伺服液压缸间接控制θ2、θ3，并将θ1、d1、d2、θ4、θ5发送给各自对应关节上的驱动器和电机，实现横向旋转臂在笛卡尔空间中五自由度的运动。8.一种五轴装卸机器人的逆解装置，其特征在于，包括：坐标建立模块，被配置为基于五轴装卸机器人的机械臂三维模型建立其连杆坐标系，并建立各坐标系之间的转换矩阵，其中，五轴装卸机器人包括第一纵向旋转臂、第二纵向旋转臂、第三纵向旋转臂、横向旋转臂、第一支撑臂和第二支撑臂；运动学关系构建模块，被配置为在机器人基坐标系{ob}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{oj5d}下设定第二控制点和第二控制方向，根据所述转换矩阵建立所述第二控制点和第二控制方向与所述第一控制点和第一控制方向之间转换的运动学关系；第一求解模块，被配置为根据所述等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θ
n
为第n个旋转轴j
n
的旋转角，n＝1、2、3、4、5；第二求解模块，被配置为根据θ2和θ3计算出第一支撑臂的臂长d1和第二支撑臂的臂长d2。9.一种电子设备，包括：一个或多个处理器；
存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-7中任一所述的方法。10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一所述的方法。

技术总结
本发明公开了一种五轴装卸机器人的逆解方法、装置及可读介质，涉及物流自动化装备领域，通过基于五轴装卸机器人的机械臂三维模型建立其连杆坐标系，并建立各坐标系之间的转换矩阵；在机器人基坐标系{OB}下设定第一控制点和第一控制方向，在第五关节轴动坐标系{OJ5d}下设定第二控制点和第二控制方向，根据转换矩阵建立第二控制点和第二控制方向与第一控制点和第一控制方向之间转换的运动学关系；根据等式关系采用几何法求解出θ1、θ2、θ3、θ4、θ5，其中，θ


技术研发人员：魏鹏 王平江 吴奕恒 顾陆楠 孙杨 樊采薇
受保护的技术使用者：泉州华中科技大学智能制造研究院
技术研发日：2023.03.16
技术公布日：2023/7/22