CN112091981A - 主从运动映射方法和系统 - Google Patents

Abstract

本申请涉及一种主从运动映射方法和系统，其中，该主从运动映射方法包括：通过主系统获取从系统的各关节的目标转角，继而获取从系统的各关节对应的电机与相应关节之间的传动比，根据传动比和目标转角确定从系统的各关节对应的电机的自驱转角和补偿转角，根据补偿转角以及自驱转角，确定从系统的各关节对应的电机的实际转角，根据实际转角控制从系统的各关节的转动。通过本申请，解决了相关技术中只能通过人工操作对电机内置的从系统进行运动控制，无法实现自动化的问题，提高了在无法使用电机内置的环境中从系统自动化控制的程度。

Description

主从运动映射方法和系统

技术领域

本申请涉及工业控制技术领域，特别是涉及主从运动映射方法和系统。

背景技术

在核工业中，许多任务需要在密闭的屏蔽室中进行，为防止核辐射泄漏，屏蔽室内各种设备的移动、操作和检维修均需要通过主从系统以远程方式实现，以减轻操作人员的操作疲劳，并降低辐照伤害。通常情况下，屏蔽室内设备较多且每个设备的工作空间小，屏蔽室内的核辐射和酸雾对远程遥控操作的设备也提出了较高的要求。

在相关技术中，对工业机器人的从系统进行控制的方法是先求解关节转动转角，然后依据关节转动转角直接映射得到关节处电机转动的转角，最后通过电机转动控制从系统进行操作。普通的控制从系统的方法，能较好地适用于电机内置于关节的从系统，但由于屏蔽室中充满辐射和酸雾，无法使用电机，故无法使用传统工业机器人的从系统，所以核工业中的从系统主要为纯机械联动的主从系统。然而，所有电机集成外置，主从系统的结构布局完全一致，需要通过人工操作实现主从控制，无法实现自动化运动控制且无法进行轨迹规划。公开号CN108673509的专利公开了一种六自由度手腕偏置型串联机械臂的运动控制方法，描述了一种基于关节偏置结构的机械手运动控制方法，不适用于关节电机外置结构，公开号CN110091329A公开了一种双机械臂的协同运动控制方法，描述了一种通过逆运动学活动电机转角驱动主从机械臂关节直接转到的运动控制方法，也不适用于电机关节外置结构。

目前针对相关技术中应用工业机器人，无法实现屏蔽室内从系统自动化运动控制的问题，尚未提出有效的解决方案。

发明内容

本申请实施例提供了一种主从运动映射方法和系统，以至少解决相关技术中应用工业机器人，无法实现屏蔽室内从系统自动化运动控制的问题。

第一方面，本申请实施例提供了一种主从运动映射方法，包括：

通过主系统获取从系统的各关节的目标转角，所述从系统的各关节均采用齿轮联动耦合；

获取所述从系统的各关节对应的电机与相应关节之间的传动比，根据所述传动比和所述目标转角确定所述从系统的各关节对应的电机的自驱转角；

根据所述从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因所述各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值；以及根据所述从系统的各关节与各关节的下一级关节的传动耦合关系，确定所述补偿转角的补偿方向；

根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统的各关节对应的电机的实际转角，根据所述实际转角控制所述从系统的各关节的转动。

在其中一些实施例中，根据所述从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因所述各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值包括：

根据所述从系统中关节的连接顺序确定关节序列，获取所述关节序列中的目标关节，所述关节序列中位于所述目标关节之前的所有关节均为在前关节；

根据每个所述在前关节的目标角度和与所述目标关节对应的电机的传动比获取一个补偿值，根据所有在前关节的补偿值得到所述目标关节的补偿转角的数值。

在其中一些实施例中，根据所述从系统的各关节与各关节的下一级关节的传动耦合关系，确定所述补偿转角的补偿方向包括：

当引起所述在前关节转动的关节的转动方向与所述在前关节同向时，所述在前关节的补偿值具有正方向；

当引起所述在前关节转动的关节的转动方向与所述在前关节反向时，所述在前关节的补偿值具有负方向。

在其中一些实施例中，所述主系统包括同构设备，通过主系统获取从系统的各关节的目标转角包括：

转动所述同构设备中的各个关节，获取所述同构设备中各关节的目标转角，根据所述同构设备中各关节的目标转角获取所述从系统各关节的目标转角。

在其中一些实施例中，所述主系统包括异构设备，通过主系统获取从系统的各关节的目标转角还包括：

获取所述异构设备末端的空间位置和姿态，通过逆解算法得到所述从系统中各关节的目标转角。

在其中一些实施例中，根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统的各关节对应的电机的实际转角，根据所述实际转角控制所述从系统的各关节的转动包括：

根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统各关节对应的电机的期望转角；

根据所述电机的期望转角和所述电机的当前转角的差值，获取所述电机的实际转角；

根据所述实际转角，通过S型曲线控制所述电机进行旋转；

通过所述电机进行旋转带动与所述电机对应的关节进行转动。

在其中一些实施例中，通过S型曲线控制所述电机进行旋转包括：

在所述电机接收到的输入信号为连续信号的情况下，采用所述S型曲线的加速至匀速段控制所述电机进行旋转。

在其中一些实施例中，通过S型曲线控制所述电机进行旋转还包括：

在所述电机接收到的输入信号中断的情况下，采用所述S型曲线的匀速至减速段控制所述电机进行旋转。

第二方面，本申请实施例提供了一种主从运动映射系统，包括主系统、从系统和控制器，所述主系统、从系统和控制器之间通过信号通讯线缆连接：

所述控制器通过所述主系统获取所述从系统的各关节的目标转角，所述从系统的各关节均采用齿轮联动耦合；

所述控制器获取所述从系统的各关节对应的电机与相应关节之间的传动比，根据所述传动比和所述目标转角确定所述从系统的各关节对应的电机的自驱转角；

所述控制器根据所述从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因所述各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值；以及根据所述从系统的各关节与各关节的下一级关节的传动耦合关系，确定所述补偿转角的补偿方向；

所述控制器根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统的各关节对应的电机的实际转角，根据所述实际转角控制所述从系统的各关节的转动。

在其中一些实施例中，所述主系统包括同构设备、异构设备和手持设备，其中，所述手持设备用于键入所述从系统中的关节转角。

相比于相关技术，本申请实施例提供的主从运动映射方法，通过主系统获取从系统的各关节的目标转角，继而获取从系统的各关节对应的电机与相应关节之间的传动比，根据传动比和目标转角确定从系统的各关节对应的电机的自驱转角，根据从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定各关节对应的电机的补偿转角的数值，以及根据从系统的各关节与各关节的下一级关节的传动耦合关系，确定补偿转角的补偿方向，根据补偿转角的数值、补偿方向以及自驱转角，确定从系统的各关节对应的电机的实际转角，根据实际转角控制从系统的各关节的转动，解决了相关技术中应用工业机器人，无法实现屏蔽室内从系统自动化运动控制的问题，提高了对工业机器人进行操作的过程中主从系统的自动化程度。

本申请的一个或多个实施例的细节在以下附图和描述中提出，以使本申请的其他特征、目的和优点更加简明易懂。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是根据本申请实施例的主从运动映射系统的示意图；

图2是根据本申请实施例的从系统的机械手的示意图；

图3是根据本申请实施例的从系统的机械手的齿轮结构示意图；

图4是根据本申请实施例的主从运动映射方法的流程图；

图5是根据本申请实施例的从系统关节转动的控制方法的流程图；

图6是根据本申请实施例的主从运动映射方法的终端的硬件结构框图；

图7是根据本申请实施例的主从运动映射系统的结构框图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请提供的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。此外，还可以理解的是，虽然这种开发过程中所作出的努力可能是复杂并且冗长的，然而对于与本申请公开的内容相关的本领域的普通技术人员而言，在本申请揭露的技术内容的基础上进行的一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本申请公开的内容不充分。

在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是，本申请所描述的实施例在不冲突的情况下，可以与其它实施例相结合。

除非另作定义，本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制，可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含；例如包含了一系列步骤或模块（单元）的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或单元，或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电气的连接，不管是直接的还是间接的。本申请所涉及的“多个”是指大于或者等于两个。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象，不代表针对对象的特定排序。

本申请提供的主从运动映射方法，可以应用于如图1所示的主从运动映射系统中，图1是根据本申请实施例的主从运动映射系统的示意图，如图1所示。主从运动映射系统应用于放射环境下，包括主系统、从系统和控制柜，其中，主系统包括同构主手、异构主手和手持盒，均在屏蔽室外，从系统的机械手置于屏蔽室内，从系统机械手对应的各个电机集成于外置电机箱中，控制柜分别与同构主手、异构主手和手持盒以及从系统的外置电机箱连接，主系统、从系统和控制柜通过信号通讯线缆进行数据传输与处理。具体地，主系统中的每个关节里，均存在电机，从系统中的机械手包括7个关节，依次以数字11至17表示，关节之间为关节臂，关节臂包括旋转关节臂和传动关节臂，第二关节12与第三关节13之间、第四关节14与第五关节15之间是传动关节臂，其余关节之间均为旋转关节臂，图中箭头表示不同关节的旋转方向。

图2是根据本申请实施例的从系统的机械手的示意图，如图2所示，包括电机21、减速器22、机械臂23和执行器24，其中，机械臂23设置于屏蔽室内，电机21和减速器22设置于屏蔽室外，减速器22和机械臂23之间通过墙体隔离辐射。机械臂23包括多个关节臂和关节，除与减速器22直接相连的关节臂以外，其他关节臂两端均设有齿轮组件。与减速器22直接相连的为一臂231，此后依次连接二臂232、三臂233、四臂234、五臂235、六臂236和七臂237，七臂237之后通过齿轮连接执行器24，执行器24可以选用直边夹爪、圆边夹爪、异形夹爪等不同型式的抓手，其中一臂231、二臂232、四臂234、六臂236和七臂237是旋转关节臂，三臂233和五臂235是传动关节臂。

图3是根据本申请实施例的从系统的机械手的齿轮结构示意图，如图3所示，第一电机31至第七电机37集成于外置电机箱内，用于驱动不同的关节进行旋转。旋转关节臂两端齿轮的个数相差1，传动关节臂两端齿轮的个数相同，具体地，一臂231一端设有齿轮组件，其余关节臂的两端均设有齿轮组件。

本实施例提供了一种主从运动映射方法，图4是根据本申请实施例的主从运动映射方法的流程图，如图4所示，该方法包括如下步骤：

步骤S410，通过主系统获取从系统的各关节的目标转角。

本实施例中的主系统和从系统均可以为具有多个关节的机械手，其中，从系统的各关节均采用齿轮联动耦合，关节内的齿轮做从动运动或传递运动。从系统中关节转角θ_n表示第n个关节转动的目标转角，本实施例中的目标转角为关节相对于标准线的角度。

步骤S420，获取从系统的各关节对应的电机与相应关节之间的传动比，根据传动比和目标转角确定从系统的各关节对应的电机的自驱转角。

在得到从系统中各个关节的目标转角之后，由于从系统中的关节不能自主转动，需要将目标转角转化为与关节对应的电机的转角，然后由电机驱动从系统中的关节转动，从而完成从系统机械手的运动。

在计算电机的转角时，首先要计算电机的自驱转角，自驱转角是电机驱动对应关节直接转动的角度，可以通过如下公式1计算得到自驱转角：

r1θ_n=i_n×θ_n 公式1

在公式1中，r1θ_n表示第n个关节对应的电机的自驱转角，i_n表示电机转角与对应驱动的第n个关节的目标转角的传动比，θ_n表示第n个关节转动的目标转角。在机械手结构确定的情况下，电机转角与对应关节的目标转角的传动比就确定。

步骤S430，根据从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值，以及根据从系统的各关节与各关节的下一级关节的传动耦合关系，确定补偿转角的补偿方向。

根据本实施例中机械手的全齿轮耦合结构可以确定各关节齿轮的传动耦合关系，由于传动耦合关系的存在，关节臂的下一级所有齿轮均受该关节臂转动的影响而产生从动结果，受从动影响的齿轮需要对与之对应的电机进行反转来实现补偿，从而消除从动结果的影响。进一步地，传动关节臂内部的齿轮只传递运动，不需要补偿，且下一级的齿轮转动不会影响上一级齿轮。

具体地，在第一关节21的目标转角为θ₁，以及共有7个关节的条件下，其他各关节或结构对应的电机需要的补偿转角由如下公式2确定：

r2θ_1n =i_n×θ₁，n=2、3、4、5、6、7 公式2

在公式2中，r2θ_1n表示第一关节11对其他关节的补偿转角的数值。

同理，在第二关节12的目标转角为θ₂，以及共有7个关节的条件下，其他各关节或结构对应的电机需要的补偿转角由如下公式3确定：

r2θ_2n =i_n×θ₂，n=3、4、5、6、7 公式3

在公式3中，r2θ_2n表示第二关节12对其他关节的补偿转角的数值。

公式1和公式2给出了补偿转角的数值计算方式，进一步地，由于传动耦合关系，传动关节臂会导致下一级关节中的齿轮方向发生变化，为了保证各个关节中齿轮的转动方向一致，还需要根据传动耦合关系对补偿转角的方向进行修正。

步骤S440，根据补偿转角的数值、补偿方向以及自驱转角，确定从系统的各关节对应的电机的实际转角，根据实际转角控制从系统的各关节的转动。

在计算得到每个电机的自驱转角和补偿转角之后，可以进一步根据电机的当前角度确定电机需要实际转动的角度，本实施例中的实际转角为电机需要转至的角度与当前转角的差值，其中，电机的当前转角可以直接读取，电机需要转至的角度为补偿转角与自驱转角之和。

电机根据该实际转角进行运动，从而控制各个关节的转动。本实施例中，电机通过动力贯穿结构将动力传递给机械手，并通过齿轮组件与多级同心轴进行动力传递，间接驱动对应关节自由度动作。

通过上述步骤S410至步骤S440，本实施例基于从系统中各个电机与关节之间转动的传动比，以及各个关节之间的传动耦合关系，可以得到驱动各个关节转动时电机的实际转动角度，同时对从系统中的各个关节进行驱动，适用于核工业机械手中所有电机集成外置的情况，且能够实现多齿轮耦合的关节的准确控制，解决了相关技术中应用工业机器人，无法实现屏蔽室内从系统自动化运动控制的问题，提高了电机集成外置时各个关节的操作效率，同时提高了从系统的自动化程度。

在其中一些实施例中，确定补偿转角的数值包括：根据从系统中关节的连接顺序确定关节序列，优选地，可以按照各个关节与电机之间的连接关系，确定关节序列，例如，关节序列可以确定为：第一关节11，第二关节12，第三关节13，第四关节14，第五关节15，第六关节16，第七关节17，在关节序列中确定一个目标关节，则目标关节之前的所有关节均为在前关节，例如，在选定第五关节15为目标关节的情况下，第一关节11，第二关节12，第三关节13和第四关节14均为在前关节，由于传动耦合关系的存在，关节臂的下一级所有齿轮均受该关节臂转动的影响而产生从动结果，因此需要根据每个在前关节的目标角度和与目标关节对应的电机的传动比获取一个补偿值，根据所有在前关节的补偿值得到目标关节的补偿转角的数值，例如，在选定第五关节15为目标关节的情况下，第五关节15的补偿转角的数值需要根据与第一关节11对应的补偿值i₅×θ₁，与第二关节12对应的补偿值i₅×θ₂，与第三关节13对应的补偿值i₅×θ₃，与第四关节14对应的补偿值i₅×θ₄共同得到。本实施例中考虑到所有在前关节对目标关节的转动影响，在此基础上确定目标关节补偿转角的数值，可以提高对机械手的控制精度。

在其中一些实施例中，由于本实施例中从系统的关节之间均采用齿轮联动耦合，因此传动关节臂会导致下一级关节中的齿轮方向发生变化，为了保证各个关节中齿轮的转动方向一致，需要确定补偿转角的补偿方向，具体为，当引起在前关节转动的关节的转动方向与在前关节同向时，在前关节的补偿值具有正方向，取正值；当引起在前关节转动的关节的转动方向与在前关节反向时，在前关节的补偿值具有负方向，取负值。具体地，旋转关节臂与旋转关节臂直接连接时，补偿值为正，如第一关节11的目标转角为θ₁时，第二电机32需转动i₂×θ₁+i₂×θ₂才能实现第二关节12的目标转角θ₂；旋转关节臂与旋转关节臂之间存在传动关节臂时，补偿值取反，例如，第二关节12与第三关节13之间为传动关节臂，第一关节11、第二关节12的目标转角分别为θ₁、θ₂，所以第三电机33需转动i₃×θ₁+i₃×θ₂-i₃×θ₃才能实现第三关节13的目标转角θ₃。本实施例中各个电机补偿转角的方向由是否经过传动关节臂来判定，每经过一次传动关节臂，补偿值取一次负值，例如，第四关节14与第五关节15之间为第二个传动关节臂，与第五关节15对应的补偿转角仍为正值。进一步地，电机自驱转角的正负采用同样的方法进行判定，每经过一次传动关节臂，自驱转角取一次负值。

在其中一些实施例中，主系统包括同构设备，此时获取从系统各关节的目标转角包括：直接转动同构设备中的各个关节，获取同构设备中各关节的目标转角，根据同构设备中各关节的目标转角获取从系统各关节需要转至的目标转角。其中，同构主手为与从系统的机械手运动形式完全相同的机械手，二者自由度构型相同，同构主手的动力源内置于关节处，直接驱动对应关节的自由度动作。采用同构主手作为信号输入时，操作效果直观，适合大范围运动控制，此时操作信号属于关节转角信号，且操作信号可直接传送给从系统的机械手。

具体地，同构主手的关节转角信号通过控制柜内的控制器以一定频率进行采集，关节转角信号转换为电机转动的转角信号后发送给控制柜内的驱动器，驱动对应的从系统的电机进行转动，通常情况下以20ms每次的频率进行同构主手的信号采集。

在其中一些实施例中，主系统包括异构设备，此时获取从系统的各关节的目标转角包括：获取异构设备末端的空间位置和姿态，通过逆解算法得到从系统中各关节的目标转角。其中，异构主手为并联机械手结构，选择异构主手时可得到异构主手末端的空间位置和姿态，根据末端的空间位置和姿态通过逆解算法求解得到机械手各关节需转动的关节转角。具体地，逆解算法求解为通过对机械手末端的空间位置姿态与关节转角之间的三角函数关系进行矩阵与反三角函数运算，经过多解择优后求解。采用异构主手作为信号输入时，操作更为轻巧，可使用单手操作，适合小范围运动控制，操作信号属于空间位置信号，需通过逆解算法求解得到关节转角信号之后再发送给从系统的机械手。

逆解算法通过控制柜内的控制器进行求解，在计算得到各关节对应的电机的实际转角之后，控制器将实际转角进行差值细分，然后叠加在电机当前角度上，以一定频率发送给控制柜内的驱动器，驱动从系统中的电机进行转动，其中，细分指将求解值进行一定比例分割，差值细分后的发送频率可选为30ms每次。

在其中一些实施例中，图5是根据本申请实施例的从系统关节转动的控制方法的流程图，如图5所示，该方法包括如下步骤：

步骤S510，根据补偿转角的数值、补偿方向以及自驱转角，确定从系统各关节对应的电机的期望转角。

本实施例中，期望转角为各电机转动时，相对于标准线的角度，其中，期望转角为补偿转角与自驱转角之和。第n个关节对应的电机的期望转角可以由如下公式4确定：

Rθ_n=i_n×θ₁+……+i_n×θ_n-1+i_n×θ_n 公式4

在公式4中，Rθ_n为第n个关节对应的电机的期望转角，其中，i_n×θ_n为自驱转角，i_n×θ₁+……+i_n×θ_n-1为来自多个在前关节的补偿转角。

具体地，在从系统机械手结构共7个自由度的情况下，由于第二关节12与第三关节13之间存在只做传递运动的传动关节臂，第三关节13对应的补偿转角取负值，各电机的期望转角如公式5至公式11所示：

Rθ₁=i₁×θ₁ 公式5

Rθ₂=i₂×θ₁+i₂×θ₂ 公式6

Rθ₃=i₃×θ₁+i₃×θ₂-i₃×θ₃ 公式7

Rθ₄=i₄×θ₁+i₄×θ₂-i₄×θ₃+i₄×θ₄ 公式8

Rθ₅=i₅×θ₁+i₅×θ₂-i₅×θ₃+i₅×θ₄+i₅×θ₅ 公式9

Rθ₆=i₆×θ₁+i₆×θ₂-i₆×θ₃+i₆×θ₄+i₆×θ₅+i₆×θ₆ 公式10

Rθ₇=i₇×θ₁+i₇×θ₂-i₇×θ₃+i₇×θ₄+i₇×θ₅+i₇×θ₆+i₇×θ₇ 公式11

步骤S520，根据电机的期望转角和电机的当前转角的差值，获取电机的实际转角。

本实施例中的实际转角为电机需要转动的角度，根据期望转角与当前转角的差值得到，电机的当前转角可以直接读取。

步骤S530，根据实际转角，通过S型曲线控制电机进行旋转。

S型曲线由于其加速度和速度曲线具有连续性，因此能够保证电机在运动过程中速度和加速度没有突变，减小信号输入突变对电机的冲击，提高电机运动的平稳性。

步骤S540，通过电机进行旋转带动与电机对应的关节进行转动。

在得到各关节对应的电机的期望转角Rθ_n的信号之后，可以通过控制柜里的驱动器驱动各关节对应的电机转动至Rθ_n，最终实现从系统机械手各关节角度转动θ_n。

通过上述步骤S510至步骤S540，本实施例基于电机的期望转角和当前转角，计算电机需要转动的实际转角，进而通过S型曲线控制从系统中的各个关节进行平稳转动。

在其中一些实施例中，通过S型曲线控制电机进行旋转包括具体为，在电机接收到的输入信号为连续信号的情况下，采用S型曲线的加速至匀速段控制电机进行旋转；在电机接收到的输入信号中断了的情况下，采用S型曲线的匀速至减速段控制电机进行旋转。本实施例中，通过S型曲线来计算电机的速度曲线，通过速度曲线来控制电机旋转，当输入信号处于持续输入的状态下，采用S型曲线的加速至匀速段控制电机旋转，保证输入信号的连续执行，进而控制电机稳定运行；当输入信号中断时，采用S型曲线的匀速至减速段控制电机旋转，保证电机实现无冲击停止。

下面通过优选实施例对本申请实施例进行描述和说明，对核工业从系统的机械手进行控制，具体包括以下步骤：

步骤S1，机械手的各个关节处于原始位置时，如果选择同构主手作为信号输入，则旋转同构主手，例如，转动同构主手第一关节、第二关节与第三关节各90°，得到从系统各个对应关节的目标转角为θ₁=90°，θ₂=90°，θ₃=90°，θ₄至θ₇均为0°；如果选择异构主手作为信号输入，那么需要将空间位置姿态信号通过逆解算法求解，得到从系统各关节的目标转角。

步骤S2，计算从系统第n个关节对应的电机的自驱转角，例如，第一关节11对应的电机的自驱转角r1θ₁为i₁×90°；第二关节12对应的电机的自驱转角r1θ₂为i₂×90°；第三关节13对应的电机的自驱转角r1θ₃为i₃×90°。

步骤S3，计算第n个关节引起的其他电机的补偿转角，例如，第一关节11的目标转角θ₁为90°，则第一电机11对其他各关节对应的电机的补偿转角为i_n×90°，其中，n=2~7；第二关节12的目标转角θ₂为90°，则第二电机12对其他各关节对应的电机的补偿转角为i_n×90°，其中，n=3~7；第三关节13的目标转角θ₃为90°，则第三电机13对其他各关节对应的电机的补偿转角为i_n×90°，其中，n=4~7，以此类推。

步骤S4，计算第n个关节对应电机的期望转角Rθ_n，期望转角Rθ_n为电机的自驱转角与补偿转角之和，由于θ₁=90°，θ₂=90°，θ₃=90°，θ₄至θ₇均为0°，根据公式5至公式11可得如下公式12至公式18：

Rθ₁=i₁×90° 公式12

Rθ₂=i₂×90°+i₂×90° 公式13

Rθ₃=i₃×90°+i₃×90°-i₃×90° 公式14

Rθ₄=i₄×90°+i₄×90°-i₄×90° 公式15

Rθ₅=i₅×90°+i₅×90°-i₅×90° 公式16

Rθ₆=i₆×90°+i₆×90°-i₆×90° 公式17

Rθ₇=i₇×90°+i₇×90°-i₇×90° 公式18

在公式12至公式18中，由于第二关节12与第三关节13之间存在只做传递运动的传动关节臂，第三关节13对应的补偿转角取负值。

步骤S5，根据期望转角Rθ_n的输出信号，驱动各关节对应的电机n转动到Rθ_n，实现从系统各关节转动转角至θ₁=90°，θ₂=90°，θ₃=90°，θ₄至θ₆为0°。

需要说明的是，本发明的控制方法适用于任意自由度、任意关节布局方式与任意齿间传动比的全齿轮顺序逻辑传动的机械手。

需要进一步说明的是，在上述流程中或者附图的流程图中示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

本申请提供的方法实施例可以在终端、计算机或者类似的运算装置中执行。以运行在终端上为例，图6为本申请实施例的主从运动映射方法的终端的硬件结构框图。如图6所示，终端60可以包括一个或多个（图6中仅示出一个）处理器602（处理器602可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置）和用于存储数据的存储器604，可选地，上述终端还可以包括用于通信功能的传输设备606以及输入输出设备608。本领域普通技术人员可以理解，图6所示的结构仅为示意，其并不对上述终端的结构造成限定。例如，终端60还可包括比图6中所示更多或者更少的组件，或者具有与图6所示不同的配置。

存储器604可用于存储控制程序，例如，应用软件的软件程序以及模块，如本申请实施例中的主从运动映射方法对应的控制程序，处理器602通过运行存储在存储器604内的控制程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器604可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器604可进一步包括相对于处理器602远程设置的存储器，这些远程存储器可以通过网络连接至终端60。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输设备606用于经由一个网络接收或者发送数据。上述的网络具体实例可包括终端60的通信供应商提供的无线网络。在一个实例中，传输设备606包括一个网络适配器（Network Interface Controller，简称为NIC），其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输设备606可以为射频（Radio Frequency，简称为RF）模块，其用于通过无线方式与互联网进行通讯。

本实施例还提供了一种主从运动映射系统，该系统用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”、“单元”、“子单元”等可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图7是根据本申请实施例的主从运动映射系统的结构框图，如图7所示，该系统包括主系统71、从系统72和控制器73，主系统71、从系统72和控制器73之间通过信号通讯线缆连接：

控制器73通过主系统71获取从系统72的各关节的目标转角，从系统72的各关节均采用齿轮联动耦合；控制器73获取从系统72的各关节对应的电机与相应关节之间的传动比，根据传动比和目标转角确定从系统72的各关节对应的电机的自驱转角；控制器73根据从系统72的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值；以及根据从系统72的各关节与各关节的下一级关节的传动耦合关系，确定补偿转角的补偿方向；控制器73根据补偿转角的数值、补偿方向以及自驱转角，确定从系统72的各关节对应的电机的实际转角，根据实际转角控制从系统72的各关节的转动。

本实施例基于从系统72中各个电机与关节之间转动的传动比，以及各个关节之间的传动耦合关系，可以得到驱动各个关节转动时电机的实际转动角度，同时对从系统72中的各个关节进行驱动，适用于核工业机械手中所有电机集成外置的情况，且能够实现多齿轮耦合的关节的准确控制，解决了相关技术中应用工业机器人，无法实现屏蔽室内从系统自动化运动控制的问题，提高了电机集成外置时各个关节的操作效率，同时提高了从系统的自动化程度。

进一步地，本实施例中，控制器73集成于控制柜中，主系统71包括同构主手、异构主手和手持盒。在操作本系统时，可以选择主系统71中的同构主手、异构主手或者手持盒。操作该主从运动映射系统时，首先需打开控制柜上的主电源，通过手持盒选择信号输入源，可选的信号输入源包括同构主手、异构主手与通过手持盒键入从系统中各个关节的转角，本实施例中机械手在同一时间能接受的信号源只有一种。

采用同构主手作为信号输入时，操作效果直观，适合大范围运动控制；采用异构主手作为信号输入时，操作更为轻巧，可使用单手操作，适合小范围运动控制；采用移动式的手持盒时，可在手持盒的操作界面上选择关节转角信号或空间位置信号作为信号输入，采用空间位置信号作为信号输入时，可控制从系统机械手末端进行直线、圆弧等预设轨迹运动。

手持盒中的空间位置信号将通过控制柜内的控制器进行逆解算法求解，先将空间位置信号转换为关节转角信号，再由控制器求解出对应从系统各个关节对应的电机转动的期望转角，然后计算各关节对应的电机的期望转角与其当前转角的差值，得到实际转角，由控制器进行差值细分后，叠加在电机当前角度上，以一定频率，例如30ms每次，发送至控制柜内的驱动器，驱动从系统机械手对应的电机进行转动，其中，细分是指将求解值进行一定比例分割。

需要说明的是，上述各个模块可以是功能模块也可以是程序模块，既可以通过软件来实现，也可以通过硬件来实现。对于通过硬件来实现的模块而言，上述各个模块可以位于同一处理器中；或者上述各个模块还可以按照任意组合的形式分别位于不同的处理器中。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种主从运动映射方法，其特征在于，包括：

通过主系统获取从系统的各关节的目标转角，所述从系统的各关节均采用齿轮联动耦合；

获取所述从系统的各关节对应的电机与相应关节之间的传动比，根据所述传动比和所述目标转角确定所述从系统的各关节对应的电机的自驱转角；

根据所述从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因所述各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值；以及根据所述从系统的各关节与各关节的下一级关节的传动耦合关系，确定所述补偿转角的补偿方向；

根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统的各关节对应的电机的实际转角，根据所述实际转角控制所述从系统的各关节的转动。

2.根据权利要求1所述的主从运动映射方法，其特征在于，根据所述从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因所述各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值包括：

根据所述从系统中关节的连接顺序确定关节序列，获取所述关节序列中的目标关节，所述关节序列中位于所述目标关节之前的所有关节均为在前关节；

根据每个所述在前关节的目标角度和与所述目标关节对应的电机的传动比获取一个补偿值，根据所有在前关节的补偿值得到所述目标关节的补偿转角的数值。

3.根据权利要求2所述的主从运动映射方法，其特征在于，根据所述从系统的各关节与各关节的下一级关节的传动耦合关系，确定所述补偿转角的补偿方向包括：

当引起所述在前关节转动的关节的转动方向与所述在前关节同向时，所述在前关节的补偿值具有正方向；

当引起所述在前关节转动的关节的转动方向与所述在前关节反向时，所述在前关节的补偿值具有负方向。

4.根据权利要求1所述的主从运动映射方法，其特征在于，所述主系统包括同构设备，通过主系统获取从系统的各关节的目标转角包括：

转动所述同构设备中的各个关节，获取所述同构设备中各关节的目标转角，根据所述同构设备中各关节的目标转角获取所述从系统各关节的目标转角。

5.根据权利要求1所述的主从运动映射方法，其特征在于，所述主系统包括异构设备，通过主系统获取从系统的各关节的目标转角还包括：

获取所述异构设备末端的空间位置和姿态，通过逆解算法得到所述从系统中各关节的目标转角。

6.根据权利要求1所述的主从运动映射方法，其特征在于，根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统的各关节对应的电机的实际转角，根据所述实际转角控制所述从系统的各关节的转动包括：

根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统各关节对应的电机的期望转角；

根据所述电机的期望转角和所述电机的当前转角的差值，获取所述电机的实际转角；

根据所述实际转角，通过S型曲线控制所述电机进行旋转；

通过所述电机进行旋转带动与所述电机对应的关节进行转动。

7.根据权利要求6所述的主从运动映射方法，其特征在于，通过S型曲线控制所述电机进行旋转包括：

在所述电机接收到的输入信号为连续信号的情况下，采用所述S型曲线的加速至匀速段控制所述电机进行旋转。

8.根据权利要求6所述的主从运动映射方法，其特征在于，通过S型曲线控制所述电机进行旋转还包括：

在所述电机接收到的输入信号中断的情况下，采用所述S型曲线的匀速至减速段控制所述电机进行旋转。

9.一种主从运动映射系统，其特征在于，包括主系统、从系统和控制器，所述主系统、从系统和控制器之间通过信号通讯线缆连接：

所述控制器通过所述主系统获取所述从系统的各关节的目标转角，所述从系统的各关节均采用齿轮联动耦合；

所述控制器获取所述从系统的各关节对应的电机与相应关节之间的传动比，根据所述传动比和所述目标转角确定所述从系统的各关节对应的电机的自驱转角；

所述控制器根据所述从系统的各关节的目标转角和各关节对应的电机与相应关节之间的传动比，确定用于消除因所述各关节的转动对各关节的下一级关节的从动结果的各关节对应的电机的补偿转角的数值；以及根据所述从系统的各关节与各关节的下一级关节的传动耦合关系，确定所述补偿转角的补偿方向；

所述控制器根据所述补偿转角的数值、补偿方向以及所述自驱转角，确定所述从系统的各关节对应的电机的实际转角，根据所述实际转角控制所述从系统的各关节的转动。

10.根据权利要求9所述的主从运动映射系统，其特征在于，所述主系统包括同构设备、异构设备和手持设备，其中，所述手持设备用于键入所述从系统中的关节转角。

Images