CN112077853B - 主从随动系统的力反馈方法、装置以及主从随动系统 - Google Patents

Abstract

本申请涉及一种主从随动系统的力反馈方法、装置、主从随动系统和存储介质，通过获取从设备各关节对应电机的实际电机扭矩和从设备各关节对应电机与相应关节之间的传动比，根据二者确定从设备各关节的实际关节扭矩，其中，从设备各关节均采用齿轮联动耦合；确定从设备在空载状态下因自重产生的各关节的理论关节扭矩；根据从设备各关节的实际关节扭矩和理论关节扭矩，确定从设备各关节因从设备的预设末端负载引起的关节扭矩增量；根据对应于从设备各关节的关节扭矩增量，对从设备对应的主设备进行力反馈控制，解决了无法基于传感器获取机械设备的力反馈信息的问题，实现了不通过传感器去获取机械设备的力反馈信息的效果。

Description

主从随动系统的力反馈方法、装置以及主从随动系统

技术领域

本申请涉及主从随动系统技术领域，特别是涉及一种主从随动系统的力反馈方法、装置、主从随动系统以及计算机可读存储介质。

背景技术

在核工业中，许多工作要在密闭的箱室中进行。为防止核辐射泄漏，箱室内各种设备的移动、操作和检修需要通过机械设备以远程方式实现，远程遥控操作技术将大幅减轻操作人员的辐照伤害与操作疲劳。

核工业箱室中常常充满辐射，由于辐射具有穿透性和危害性，需要对暴露在辐射环境中的机械设备进行屏蔽，若完全屏蔽辐射，需将机械设备置于150mm以上的铅层，然而该环境中的机械设备无法使用电子元器件。

相关技术（专利号为CN 104908046A）提供了一种基于旋钮式力反馈手控器及机械臂遥操作控制方法，包括底板、扭矩传感器、外壳、传感器法兰、距离套、编码器、电机座板、电机、设有精粗调开关的开关旋钮，外壳和底板固定连接，形成密闭空间；距离套固定连接在电机座板和传感器法兰之间，电机座板和传感器法兰之间形成安装空间，扭矩传感器固定连接在传感器法兰的底面，编码器位于安装空间中，且编码器的壳体固定连接在电机座板的底面，电机固定连接在电机座板的顶面，且电机输出轴的一端穿过电机座板，与编码器的码盘固定连接；开关旋钮位于外壳的外侧，且开关旋钮的旋钮与电机输出轴的另一端连接。

现有的相关力控制方法均基于内置传感器获得信号，信号直接作用于内置执行电机进行直接控制，虽然精度较高，但是在核辐射环境中，通常都是些纯机械结构的设备，传感器无法在该环境中起效。

目前针对相关技术中无法基于传感器获取机械设备的力反馈信息的问题，尚未提出有效的解决方案。

发明内容

本申请实施例提供了一种主从随动系统的力反馈方法、装置、主从随动系统以及计算机可读存储介质，以至少解决相关技术中无法基于传感器获取机械设备的力反馈信息的问题。

第一方面，本申请实施例提供了一种主从随动系统的力反馈方法，所述方法包括：

获取从设备各关节对应的电机的实际电机扭矩和所述从设备各关节对应的电机与相应关节之间的传动比，其中，所述从设备各关节均采用齿轮联动耦合；

根据所述从设备各关节对应的电机的实际电机扭矩和所述传动比，确定所述从设备各关节的实际关节扭矩；

确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩；

根据所述从设备各关节的实际关节扭矩和所述理论关节扭矩，确定所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量；

根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制。

在其中一些实施例中，在确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩之前，所述方法还包括：

获取所述从设备各关节对应的电机的实际电机转角；

获取所述从设备各关节需补偿的补偿转角；

根据对应于所述从设备各关节对应的电机的实际电机转角和补偿转角，得到所述从设备各关节的关节转角。

在其中一些实施例中，获取所述从设备各关节对应的电机需补偿的补偿转角包括：

根据所述从设备中对应于上一级关节的实际电机转角和传动比，得到该上一级关节的关节转角；

根据该上一级关节的关节转角、所述从设备中各关节对应的电机与相应关节之间的传动比，确定下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的大小；

获取该上一级关节和下一级关节之间的耦合关系，其中，所述耦合关系包括传递耦合关系和从动耦合关系，具备所述传递耦合关系的两个关节之间的自驱转动方向相同，具备所述从动耦合关系的两个关节之间的自驱转动方向相反；

根据所述耦合关系确定对应于该下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的方向，其中，在所述从设备中上一级关节和下一级关节之间属于所述传递耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的转动的补偿转角的方向和上一级关节的关节转角的方向相反，在所述从设备中上一级关节和下一级关节之间属于所述从动耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的转动的补偿转角的方向和上一级关节的关节转角的方向相同。

在其中一些实施例中，确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩包括：

根据所述从设备各关节的关节转角和D-H参数，得到所述从设备各关节的变化矩阵；

获取所述从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数；

根据所述从设备各关节的变化矩阵、所述从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数，确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩。

在其中一些实施例中，根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制包括：

将所述关节扭矩增量和与所述预设末端负载对应的预设关节扭矩增量作差，得到关节扭矩增量偏差值；

判断所述关节扭矩增量偏差值是否在预设阈值范围之内；

在判断到所述关节扭矩增量偏差值在预设阈值范围之内的情况下，根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制。

在其中一些实施例中，在判断到所述关节扭矩增量偏差值不在预设阈值范围之内的情况下，所述方法还包括：

判断所述关节扭矩增量落入的预设负载扭矩区间，并将该预设负载扭矩区间对应的预设末端负载作为新的预设末端负载，再执行根据所述从设备各关节的实际关节扭矩和所述理论关节扭矩，确定所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量的步骤，直至最终计算得到的关节扭矩增量偏差值在预设范围之内。

第二方面，本申请实施例提供了一种主从随动系统的力反馈装置，所述装置包括：

获取模块，用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备各关节对应的电机与相应关节之间的传动比，其中，所述从设备各关节均采用齿轮联动耦合；

第一确定模块，用于根据所述从设备各关节对应的电机的实际电机扭矩和所述传动比，确定所述从设备各关节的实际关节扭矩；

第二确定模块，用于确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩；

第三确定模块，用于根据所述从设备各关节的实际关节扭矩和所述理论关节扭矩，确定所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量；

反馈控制模块，用于根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制。

第三方面，本申请实施例提供了一种主从随动系统，所述系统包括：主设备、从设备以及控制器，所述主设备和所述从设备分别与所述控制器连接；

所述从设备包括多个关节，所述从设备各关节均采用齿轮联动耦合，所述从设备中的每个关节均对应于一个从电机，电机用于在所述主设备的控制下驱动所述从设备中相应的关节进行转动；

所述控制器用于执行如上述第一方面所述的主从随动系统的力反馈方法。

在其中一些实施例中，所述主设备为与所述从设备同构的设备，所述主设备包括多个关节，所述主设备中的每个关节均对应于一个电机，电机用于驱动所述主设备中相应的关节转动。

第四方面，本申请实施例提供了一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现如上述第一方面所述的主从随动系统的力反馈方法。

相比于相关技术，本申请实施例提供的主从随动系统的力反馈方法、装置、主从随动系统以及计算机可读存储介质，通过获取从设备各关节对应的电机的实际电机扭矩和从设备各关节对应的电机与相应关节之间的传动比，其中，从设备各关节均采用齿轮联动耦合；根据从设备各关节对应的电机的实际电机扭矩和传动比，确定从设备各关节的实际关节扭矩；确定从设备在空载状态下因自重产生的各关节的理论关节扭矩；根据从设备各关节的实际关节扭矩和理论关节扭矩，确定从设备各关节因从设备的预设末端负载引起的关节扭矩增量；根据从设备各关节因从设备的预设末端负载引起的关节扭矩增量，对从设备对应的主设备进行力反馈控制，解决了相关技术中无法基于传感器获取机械设备的力反馈信息的问题，实现了不通过传感器也能够获取机械设备的力反馈信息的有益效果。

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

附图说明

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

图1是根据本申请实施例的主从随动系统的力反馈方法的流程图；

图2是根据本申请实施例的主从随动系统的力反馈装置的结构框图；

图3是根据本申请实施例的主从随动系统的结构示意图一；

图4是根据本申请实施例的主从随动系统的结构示意图二；

图5是根据本申请实施例的主从随动系统的结构示意图三；

图6是根据本申请实施例的主从随动系统的力反馈原理框图。

附图标记：

100、主设备；111、第一主关节；112、第二主关节；113、第三主关节；114、第四主关节；115、第五主关节；116、第六主关节；

200、从设备；211、第一从关节；212、第二从关节；213、第三从关节；214、第四从关节；215、第五从关节；216、第六从关节；221、第一从电机；222、第二从电机；223、第三从电机；224、第四从电机；225、第五从电机；226、第六从电机；227、第七从电机；228、齿轮；

300、控制器；400、通讯设备；500、屏蔽装置；600、电机箱；

21、获取模块；22、第一确定模块；23、第二确定模块；24、第三确定模块；25、反馈控制模块。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请提供的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

显而易见地，下面描述中的附图仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。此外，还可以理解的是，虽然这种开发过程中所做出的努力可能是复杂并且冗长的，然而对于与本申请公开的内容相关的本领域的普通技术人员而言，在本申请揭露的技术内容的基础上进行的一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本申请公开的内容不充分。

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

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

本实施例提供了一种主从随动系统的力反馈方法，图1是根据本申请实施例的主从随动系统的力反馈方法的流程图，

如图1所示，该流程包括如下步骤：

步骤S101，获取从设备各关节对应的电机的实际电机扭矩和从设备各关节对应的电机与相应关节之间的传动比，其中，从设备各关节均采用齿轮联动耦合。

图3是根据本申请实施例的主从随动系统的结构示意图一，如图3所示，该系统包括主设备100和从设备200，主设备100和从设备200连接，从设备200包括多个关节，从设备各关节均采用齿轮联动耦合，从设备中各关节均配置有一个电机。图4是根据本申请实施例的主从随动系统的结构示意图二，如图4所示，第一从关节211和第一从电机221连接，传动比为i1；第二从关节212和第二从电机222，传动比为i2；第三从关节213和第三从电机223连接，传动比为i3；第四从关节214和第四从电机224连接，传动比为i4；第五从关节215和第五从电机225连接，传动比为i5；第六从关节216和第六从电机226连接，传动比为i6。通过将主设备100各关节的运动姿态映射至从设备200的相应关节，使得主设备100和从设备200各关节的转动方向一一对应。

在充满放射性物质的屏蔽室中，从设备各关节可以设置在屏蔽室内，电机设置在屏蔽室外，通过读取电机输出的信号，能够获取得到电机的实际电机扭矩。从设备各关节对应的电机与相应关节之间的传动比是预先设定的。

步骤S102，根据从设备各关节对应的电机的实际电机扭矩和传动比，确定从设备各关节的实际关节扭矩。

由于从设备各关节均采用齿轮联动耦合，因此，在电机的驱动下，从设备各关节的实际关节扭矩和相应电机成比例关系，通过从设备各关节对应的电机的实际电机扭矩和传动比，可以换算得到从设备各关节的实际关节扭矩。

步骤S103，确定从设备在空载状态下因自重产生的各关节的理论关节扭矩。

从设备的空载状态是指从设备末端没有挂负载的情况，在该情况下，从设备各关节的理论关节扭矩可通过关节转角换算得到理论值。

步骤S104，根据从设备各关节的实际关节扭矩和理论关节扭矩，确定从设备各关节因从设备的预设末端负载引起的关节扭矩增量。

预设末端负载可以是从设备末端负载为空的情况，也可以是从设备末端挂有负载的情况。

从设备各关节在电机的驱动下，会产生实际关节扭矩，实际关节扭矩的一部分贡献来自于关节因自重产生的理论关节扭矩，另一部分贡献来自于关节因克服负载的重力作用力而产生的关节扭矩增量。在负载重力已知的情况下，可以根据负载重力得到关节扭矩增量，但是由于系统误差或者负载重力的变化，仅根据负载重力得到的关节扭矩增量是不准确的。在本实施例中，将从设备各关节的实际关节扭矩和理论关节扭矩进行作差，能够准确地得到从设备各关节的关节扭矩增量。

步骤S105，根据从设备各关节因从设备的预设末端负载引起的关节扭矩增量，对从设备对应的主设备进行力反馈控制。

实际应用中，在从设备各电机的实际电机扭矩不变的情况下，由于受系统误差和负载重力影响，关节扭矩增量将发生变化，因此，通过将从设备各关节的关节扭矩增量反馈至主设备，能够为主设备确定控制策略提供准确的反馈信息，从而起到有效控制从设备的作用。

在本实施例中，通过读取电机信号，能够得到实际电机扭矩，根据实际电机扭矩和相应传动比，确定从设备各关节的实际关节扭矩，根据从设备各关节的实际关节扭矩和理论关节扭矩，确定从设备各关节因从设备的末端负载引起的关节扭矩增量，根据该关节扭矩增量对主设备进行力反馈控制，以实时调节关节角度与力度反馈值。本实施例通过读取电机信号得到机械设备的力反馈信息，不需要在从设备中内置传感器，也不需要通过传感器直接测量各关节的实际关节扭矩，本实施例可应用于充满放射性的核工业环境中的纯机械结构的机械设备力学控制。

通过上述步骤S101至步骤S105，解决了相关技术中无法基于传感器获取机械设备的力反馈信息的问题，实现了不通过传感器也能够获取机械设备的力反馈信息的有益效果。

在其中一些实施例中，在确定从设备在空载状态下因自重产生的各关节的理论关节扭矩之前，方法还包括如下步骤：

获取从设备各关节对应的电机的实际电机转角；获取从设备各关节需补偿的补偿转角；根据对应于从设备各关节的实际电机转角和补偿转角，得到从设备各关节的关节转角。

关节的关节转角决定着关节的理论关节扭矩。电机通过相应传动比将转动力施加给关节，例如，当前第一从关节211的从关节转角为θ1，第一从电机221的实际从电机转角rθ1=i1×θ1，即若要使第一从关节211转动θ1，则第一从电机221需转动rθ1=i1×θ1。

由于从设备各关节均采用齿轮联动耦合，上一级关节的转动将会对下一级关节的转动产生从动作用，而下一级关节的转动不会对上一级关节的转动造成从动作用，受从动作用的关节需对应电机进行反转来补偿运动。当第一从关节211转动θ1之后，其他下级关节的电机需补偿转动in×θ1（n为2至6的自然数），当第二从关节212转动θ2之后，其他下级关节的电机需补偿转动in×θ2（n为3至6的自然数），以此类推。例如，当第一从关节211转动θ1之后，若要使第二从关节212转动θ2，则第二从电机222除了转动i2×θ2，还需转动i2×θ1，即第二从电机222的实际从电机转角rθ2=i2×θ2+i2×θ1。又例如，当第一从关节211转动θ1，且第二从关节212转动θ2之后，则第三从电机223需补偿转动i3×(θ1+θ2)，即第三从电机223的实际从电机转角rθ3=i3×θ3+i3×(θ1+θ2)。

从设备各关节的实际电机转角可以通过读取电机信号获得，从设备各关节的补偿转角可以根据各关节之间的耦合关系、相应传动比获得，根据对应于从设备各关节的实际电机转角和补偿转角，能够得到从设备各关节的关节转角。本实施例不需要在从设备中内置传感器，也不需要通过传感器直接测量各关节的关节转角，本实施例可应用于充满放射性的核工业环境中的纯机械结构的机械设备力学控制。

在一些实施例中，从设备各关节之间不仅仅存在从动耦合关系，还存在传递耦合关系，具备从动耦合关系的两个关节对应电机之间的自驱转动方向相反，具备传递耦合关系的两个关节对应电机之间的自驱转动方向相同。例如，第一从关节211和第二从关节212为直接相连的两个关节，二者属于从动耦合关系，且二者对应电机的自驱转动方向相反；第二从关节212和第三从关节213通过传动臂连接，二者属于传递耦合关系，传动臂用于传递运动，所以二者的自驱转动方向相同。

在从设备中上一级关节和下一级关节之间属于传递耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的转动的补偿转角的方向和上一级关节的关节转角的方向相反，在从设备中上一级关节和下一级关节之间属于从动耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的转动的补偿转角的方向和上一级关节的关节转角的方向相同。

在本实施例中，电机需补偿的补偿转角正负值由是否经过传动臂来判定，每经过一次传动臂，补偿转角值取一次负值。例如，第一从关节211和第二从关节212为直接相连的两个关节，二者属于从动耦合关系，当第一从关节211转动θ1之后，第二从电机222需补偿转动i2×θ1；第一从关节211和第三从关节213为间接相连的两个关节，中间经过一次传动臂，当第一从关节211转动θ1之后，第三从电机223需补偿转动-i3×θ1，以此类推。

对于从设备中的6个关节，对应关节转角分别为θ1至θ6，第一从电机221至第六从电机226的实际电机转角分别为：

rθ1=i1×θ1。

rθ2=i2×θ1+i2×θ2。

rθ3=i3×θ3+i3×θ2-i3×θ1。

rθ4=i4×θ4+i4×θ3-i4×θ2+i4×θ1。

rθ5=i5×θ5+i5×θ4-i5×θ3+i5×θ2-i5×θ1。

rθ6=i6×θ6+i6×θ5-i6×θ4+i6×θ3-i6×θ2+i6×θ1。

本实施例根据该上一级关节的关节转角、从设备中各关节对应的电机与相应关节之间的传动比，确定下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的大小；根据耦合关系确定对应于该下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的方向，能够获得各关节的关节转角。

在其中一些实施例中，确定从设备在空载状态下因自重产生的各关节的理论关节扭矩包括如下步骤：

根据从设备各关节的关节转角和D-H参数，得到从设备各关节的变化矩阵；获取从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数；根据从设备各关节的变化矩阵、从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数，确定从设备在空载状态下因自重产生的各关节的理论关节扭矩。

在本实施例中，D-H参数是一种对连杆的坐标描述，相同的参数代表相同的物理含义。

从设备各关节的变化矩阵R{n}如下所示：

其中，

为由D-H参数得到的固定值，n为1至6的自然数，分别对应于从设备的6个关节。

从设备各关节的理论关节扭矩

如下所示：

其中，

；

为n+1坐标系原点在n坐标系的位置；

为表达各关节质心在当前关节坐标系下空间位置的1×3向量；

，

为表达各关节质心在当前关节坐标系下力值的1×3向量，

为表达各关节质心在世界坐标系下力值的1×3向量；

，

，

为当前关节坐标系所受的力，n为1至6的自然数。

从设备各关节的关节扭矩增量e如下所示：

其中，

为从设备各关节的实际关节扭矩，

为从设备各关节的理论关节扭矩。

其中，

为从设备各关节对应的电机的实际电机扭矩，

为从设备各关节对应的电机与相应关节之间的传动比。

一方面，从设备各关节均采用齿轮联动耦合，多齿轮耦合将会产生多重齿隙叠加，导致各关节的关节转角的期望值与实际值偏差较大，各关节齿隙与变形将引起扭矩误差，又由于多齿轮运动传递仍存在耦合关系，导致电机转向与对应关节转向之间无确切关系，这种由系统误差引起的偏差会体现在关节扭矩增量偏差值中。

针对该问题，在其中一些实施例中，根据从设备各关节因从设备的预设末端负载引起的关节扭矩增量，对从设备对应的主设备进行力反馈控制包括如下步骤：

将关节扭矩增量和与预设末端负载对应的预设关节扭矩增量作差，得到关节扭矩增量偏差值；判断关节扭矩增量偏差值是否在预设阈值范围之内；在判断到关节扭矩增量偏差值在预设阈值范围之内的情况下，根据从设备各关节因从设备的预设末端负载引起的关节扭矩增量，对从设备对应的主设备进行力反馈控制。

另一方面，负载重力的变化或者误测，也会导致产生关节扭矩增量偏差值。

针对该问题，在其中一些实施例中，在判断到关节扭矩增量偏差值不在预设阈值范围之内的情况下，方法还包括如下步骤：

判断关节扭矩增量落入的预设负载扭矩区间，并将该预设负载扭矩区间对应的预设末端负载作为新的预设末端负载，再执行根据从设备各关节的实际关节扭矩和理论关节扭矩，确定从设备各关节因从设备的预设末端负载引起的关节扭矩增量的步骤，直至最终计算得到的关节扭矩增量偏差值在预设范围之内。

通常，因系统误差产生的关节扭矩增量偏差值小于因负载重力的变化或者误测导致的关节扭矩增量偏差值。

在上述两个实施例中，将关节扭矩增量e和预设关节扭矩增量比较，得到关节扭矩增量偏差值Δ，如果关节扭矩增量偏差值Δ较小，代表引起该关节扭矩增量偏差值Δ的原因可能为系统误差导致，则对从设备对应的主设备进行力反馈控制，以触发对从设备做运动补偿；如果关节扭矩增量偏差值Δ较大，代表引起该关节扭矩增量偏差值Δ的原因可能为负载重力的变化或者误测导致，则调整负载重量参数，并得到相应的预设关节扭矩增量，使得关节扭矩增量和调整后的预设关节扭矩增量进行比较后得到的关节扭矩增量偏差值Δ处于预设阈值范围之内。

相关技术在进行力反馈控制的过程中，没有考虑引起关节扭矩增量偏差值Δ的原因，往往忽略了负载重量变化或者误测引起的关节扭矩增量偏差值Δ，导致力反馈控制效果不好。本实施例在进行力反馈控制过程中，对关节扭矩增量偏差值Δ进行分析，确定引起关节扭矩增量偏差值Δ的原因，并给出相应的控制策略，提升了力反馈控制的效用。

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

图2是根据本申请实施例的主从随动系统的力反馈装置的结构框图，如图2所示，该装置包括：获取模块21、第一确定模块22、第二确定模块23、第三确定模块24以及反馈控制模块25。

获取模块21，用于获取从设备各关节对应的电机的实际电机扭矩和从设备各关节对应的电机与相应关节之间的传动比，其中，从设备各关节均采用齿轮联动耦合。

第一确定模块22，耦合至获取模块21，用于根据从设备各关节对应的电机的实际电机扭矩和传动比，确定从设备各关节的实际关节扭矩。

第二确定模块23，用于确定从设备在空载状态下因自重产生的各关节的理论关节扭矩。

第三确定模块24，分别耦合至第一确定模块22和第二确定模块23，用于根据从设备各关节的实际关节扭矩和理论关节扭矩，确定从设备各关节因从设备的预设末端负载引起的关节扭矩增量。

反馈控制模块25，耦合至第三确定模块24，用于根据从设备各关节因从设备的预设末端负载引起的关节扭矩增量，对从设备对应的主设备进行力反馈控制。

在一些实施例中，装置还包括：第一获取单元，用于获取从设备各关节对应的电机的实际电机转角；第二获取单元，用于获取从设备各关节对应的电机需补偿的补偿转角；得到模块，用于根据对应于从设备各关节对应的电机的实际电机转角和补偿转角，得到从设备各关节的关节转角。

在一些实施例中，第二获取单元还用于：根据从设备中对应于上一级关节的实际电机转角和传动比，得到该上一级关节的关节转角；根据该上一级关节的关节转角、从设备中各关节对应的电机与相应关节之间的传动比，确定下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的大小；获取该上一级关节和下一级关节之间的耦合关系，其中，耦合关系包括传递耦合关系和从动耦合关系，具备传递耦合关系的两个关节对应的电机之间的自驱转动方向相同，具备从动耦合关系的两个关节对应的电机之间的自驱转动方向相反；根据耦合关系确定对应于该下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的方向，其中，在从设备中上一级关节和下一级关节之间属于传递耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的转动的补偿转角的方向和上一级关节的关节转角的方向相反，在从设备中上一级关节和下一级关节之间属于从动耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的补偿转角的方向和上一级关节的关节转角的方向相同。

在一些实施例中，第二确定模块23还用于：根据从设备各关节的关节转角和D-H参数，得到从设备各关节的变化矩阵；获取从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数；根据从设备各关节的变化矩阵、从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数，确定从设备在空载状态下因自重产生的各关节的理论关节扭矩。

在一些实施例中，反馈控制模块25还用于：将关节扭矩增量和与预设末端负载对应的预设关节扭矩增量作差，得到关节扭矩增量偏差值；判断关节扭矩增量偏差值是否在预设阈值范围之内；在判断到关节扭矩增量偏差值在预设阈值范围之内的情况下，根据从设备各关节因从设备的预设末端负载引起的关节扭矩增量，对从设备对应的主设备进行力反馈控制。

在一些实施例中，反馈控制模块25还用于：判断关节扭矩增量落入的预设负载扭矩区间，并将该预设负载扭矩区间对应的预设末端负载作为新的预设末端负载，再执行根据从设备各关节的实际关节扭矩和理论关节扭矩，确定从设备各关节因从设备的预设末端负载引起的关节扭矩增量的步骤，直至最终计算得到的关节扭矩增量偏差值在预设范围之内。

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

结合上述实施例的主从随动系统的力反馈方法，本实施例提供了一种主从随动系统，图5是根据本申请实施例的主从随动系统的结构示意图三，如图5所示，该系统包括：主设备100、从设备200以及控制器300，主设备100和从设备200分别与控制器300连接。参考图4，从设备200包括多个关节，从设备200各关节均采用齿轮联动耦合，从设备200中的每个关节均对应于一个从电机，电机用于驱动从设备200中相应的关节进行转动；控制器300用于执行如上述实施例所描述的主从随动系统的力反馈方法。

在本实施例中，控制器300通过读取从设备200的电机信号，能够得到实际电机扭矩，根据实际电机扭矩和相应传动比，确定从设备200各关节的实际关节扭矩，根据从设备200各关节的实际关节扭矩和理论关节扭矩，确定从设备200各关节因从设备200的末端负载引起的关节扭矩增量，根据该关节扭矩增量对主设备100进行力反馈控制。本实施例通过读取电机信号得到机械设备的力反馈信息，不需要在从设备中内置传感器，也不需要通过传感器直接测量各关节的实际关节扭矩，本实施例可应用于充满放射性的核工业环境中的纯机械结构的机械设备力学控制。通过本实施例，解决了相关技术中无法基于传感器获取机械设备的力反馈信息的问题，实现了不通过传感器也能够获取机械设备的力反馈信息的有益效果。

参考图4，从设备200包括齿轮228和多个关节，从设备各关节均采用齿轮228联动耦合，从设备中各关节均连接有一个电机，各关节的转动方向如箭头所示。其中，第一从关节211和第一从电机221连接，传动比为i1；第二从关节212和第二从电机222，传动比为i2；第三从关节213和第三从电机223连接，传动比为i3；第四从关节214和第四从电机224连接，传动比为i4；第五从关节215和第五从电机225连接，传动比为i5；第六从关节216和第六从电机226连接，传动比为i6。

在一些实施例中，主设备100为与从设备200同构的设备，主设备100包括多个关节，主设备100中的每个关节均对应于一个电机，电机用于驱动主设备100中相应的关节转动。参考图3，主设备100包括多个关节，分别为第一主关节111，第二主关节112，第三主关节113，第四主关节114，第五主关节115，第六主关节116，主设备100中的每个关节均连接有一个电机，用于驱动主设备100中相应的关节进行转动。在控制器300的控制下，主设备100各关节的运动姿态映射至从设备200的相应关节，使得主设备100和从设备200各关节的转动方向一一对应。

参考图4，在一些实施例中，从设备200还包括第七从电机227，用于控制夹爪开合。

参考图5，在一些实施例中，系统还包括通讯设备400，通讯设备400用于连接主设备100、从设备200以及控制器300。

在一些实施例中，系统还包括屏蔽装置500，从设备各关节置于屏蔽装置500内，与从设备连接的电机、主设备100和控制器300均置于屏蔽装置500外，屏蔽装置500用于隔绝放射性物质和外界环境。

在一些实施例中，系统还包括电机箱600，电机箱600中设置有和从设备各关节连接的电机。

在一些实施例中，通讯设备400可采用通讯线缆。

在一些实施例中，控制器300可采用控制柜。

在一些优选实施例中，主从随动系统在进行主从功能应用时，从设备末端负载作用在各关节上的扭矩反馈至主设备各关节电机进行比例输出，使操作者具有临场操作力感。本实施例通过主设备的关节转动信号输入，来控制密闭屏蔽室内的从设备进行对应关节转动执行。系统执行的过程中，主设备的关节转动动力来自于关节内置的电机，关节转动信号由关节电机直接输出，从设备的关节转动动力来源于外置电机箱，由多级齿轮耦合传递进入屏蔽室后驱动关节转动。从设备的受力分布在每个关节上，实际关节扭矩值可由外置电机转矩值耦合计算后求得，耦合关系由实际齿轮布局决定。通过力学分析，去除关节扭矩值中因自重引起的扭矩值，得到因末端负载引起的扭矩值，将该扭矩值通过通讯协议传输给主设备，对应至主设备各关节对应的电机，进行扭矩叠加，实现力反馈。

参考图3至图5，从设备具有6个自由度的驱动力，均由外置电机通过齿轮组与同心轴耦合后传递至各个关节，第七从电机227用于控制夹爪开合。

本实施例的主从随动系统，进行了全齿轮传递条件下的扭矩传递解耦设计，关节扭矩与电机扭矩成比例关系，比例为传动链的传动比。基于该结构设计，可通过采集从设备电机扭矩输入值，换算得到主设备关节补偿扭矩输出值。算法逻辑为采集从设备电机的实际电机扭矩DM{n}，确定各电机与关节驱动齿轮之间的传动比为in，则从设备各关节的实际关节扭矩为GM{n}=DM{n}/in。求解从设备自重产生的理论关节扭矩LGM{n}，两者相减可得从设备负载产生的关节扭矩增量e，即为需反馈至主设备的反馈信息。

本实施例通过电机的实际电机转角与关节转角之间传动耦合关系，得到从设备各关节的关节转角。实际电机转角与关节转角之间传动耦合关系如下：

从设备的全齿轮耦合结构决定了各关节齿轮的传动耦合关系，根据设计结构，关节臂的下一级所有齿轮均受关节臂转动的影响而产生从动结果，受从动影响的齿轮需对应电机进行反转来补偿运动；传动臂内齿轮只传递运动，不需要补偿；下级的齿轮转动不会影响上一级齿轮。

例如，当第一从关节211转动θ1之后，其他下级关节的电机需补偿转动

×θ1（n为2至6的自然数），当第二从关节212转动θ2之后，其他下级关节的电机需补偿转动

×θ2（n为3至6的自然数），以此类推。例如，当第一从关节211转动θ1之后，若要使第二从关节212转动θ2，则第二从电机222除了转动i2×θ2，还需转动i2×θ1，即第二从电机222的实际从电机转角rθ2=i2×θ2+i2×θ1。又例如，当第一从关节211转动θ1，且第二从关节212转动θ2之后，则第三从电机223需补偿转动i3×(θ1+θ2)，即第三从电机223的实际从电机转角rθ3=i3×θ3+i3×(θ1+θ2)。

电机需补偿的转角的正负取向由是否经过传动臂来判定，每经过一次传动臂，补偿值取一次负值。关节臂与关节臂直接连接时，补偿值为正；关节臂与关节臂之间存在传动臂时，补偿值取反。

本实施例通过各关节的关节转角得到理论关节扭矩LGM{n}，与实际关节扭矩GM{n}进行对比分析，得到关节扭矩增量e，根据e判断各关节的运动补偿量。

在实际应用中，全齿轮传动由于因多齿轮耦合产生的多重齿隙叠加，导致关节转角的理论值与实际值偏差较大，虽然本实施例的主从随动系统实现了力传递解耦，但是其多齿轮运动传递仍存在耦合关系，导致电机转向与对应关节转向之间无确切关系。针对该问题，本实施例通过分析齿轮耦合关系、采用力反馈解耦规律，实现复杂齿轮耦合关系下的齿隙补偿。即根据关节扭矩增量e的方向与大小，来判定是否进行关节运动补偿，以及确定补偿的大小和方向。

图6是根据本申请实施例的主从随动系统的力反馈原理框图，如图6所示，各参数物理意义如下所示：

s：s指微分环节，1/s指积分环节，1/s²指两个积分环节的串联。

：主设备各关节输入转角，m为1至7的自然数。

：从设备各关节输出转角，n为1至7的自然数。

：主设备速度。

：主设备阻尼系数。

：从设备系统刚度系数。

：主设备输入力。

：从设备反馈力。

：位置前馈增益。

：力反馈增益。

：主设备质量。

：从设备质量。

、

、

：从设备的PID控制参数，PID为比例-积分-微分控制器（Proportional Integral Derivative)。

其中，该PID位置闭环控制中引入了关节扭矩增量e，本实施例将关节扭矩增量e引入从设备系统刚度系数

，从设备系统刚度系数

用于描述从设备关节转角

与从设备反馈力

之间的系统关系。

关节扭矩增量e是由实际负载（实际关节扭矩）和理论负载（理论关节扭矩）相减得到的，理论负载由输入角度求解，实际负载由电机反馈负载得到，关节扭矩增量e的存在说明在同一信号输入的条件下，实际执行的关节位置与求解的理论关节位置存在偏差，通过调整系统刚度系数

增加当前角度情况下的从设备反馈力

，将从设备反馈力

传递给主设备，改变主设备的力反馈值，此时会增加从设备的关节角度输出值；同时作为变量引入了主设备输出的力经PID控制求解之后的环节，作为判定条件来保证从设备关节角度的稳定输出。该PID控制框图为本领域常用原理，本实施例和相关技术中的PID控制框图相比，主要区别包括引入了关节扭矩增量e的判定。

综上，本申请中的主从随动系统的力反馈方法可适用于全齿轮传递手臂结构的力反馈控制，通过读取外置电机的实际电机扭矩信号，不涉及传感器信号直接读取，可应用于充满放射性的核工业环境中的纯机械结构的控制。本申请中主从随动系统的力反馈方法通过采集实际电机扭矩与获取理论关节扭矩进行分析计算，实现了信号滤波后的力反馈控制与齿隙补偿。

另外，结合上述实施例中的主从随动系统的力反馈方法，本申请实施例可提供一种计算机可读存储介质来实现。该计算机可读存储介质上存储有计算机程序指令；该计算机程序指令被处理器执行时实现上述实施例中的任意一种主从随动系统的力反馈方法。

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

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

Claims (8)

1.一种主从随动系统的力反馈方法，其特征在于，所述方法包括：

获取从设备各关节对应的电机的实际电机扭矩和所述从设备各关节对应的电机与相应关节之间的传动比，其中，所述从设备各关节均采用齿轮联动耦合；

根据所述从设备各关节对应的电机的实际电机扭矩和所述传动比，确定所述从设备各关节的实际关节扭矩；其中，所述从设备各关节的实际关节扭矩为所述从设备各关节对应的电机的实际电机扭矩和所述传动比之间的比值；

确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩；

根据所述从设备各关节的实际关节扭矩和所述理论关节扭矩，确定所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量；

根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制；

其中，根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制包括：

将所述关节扭矩增量和与所述预设末端负载对应的预设关节扭矩增量作差，得到关节扭矩增量偏差值；判断所述关节扭矩增量偏差值是否在预设阈值范围之内；在判断到所述关节扭矩增量偏差值在预设阈值范围之内的情况下，根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制，其中包括：调整所述从设备各关节的刚度系数，根据调整后的所述从设备各关节的刚度系数确定所述从设备各关节的反馈力，并将所述从设备各关节的反馈力传递给与所述从设备对应的主设备；在判断到所述关节扭矩增量偏差值不在预设阈值范围之内的情况下，判断所述关节扭矩增量落入的预设负载扭矩区间，并将该预设负载扭矩区间对应的预设末端负载作为新的预设末端负载，再执行根据所述从设备各关节的实际关节扭矩和所述理论关节扭矩，确定所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量的步骤，直至最终计算得到的关节扭矩增量偏差值在预设范围之内。

2.根据权利要求1所述的主从随动系统的力反馈方法，其特征在于，在确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩之前，所述方法还包括：

获取所述从设备各关节对应的电机的实际电机转角；

获取所述从设备各关节对应的电机需补偿的补偿转角；

根据对应于所述从设备各关节对应的电机的实际电机转角和补偿转角，得到所述从设备各关节的关节转角。

3.根据权利要求2所述的主从随动系统的力反馈方法，其特征在于，获取所述从设备各关节对应的电机需补偿的补偿转角包括：

根据所述从设备中对应于上一级关节的实际电机转角和传动比，得到该上一级关节的关节转角；

根据该上一级关节的关节转角、所述从设备中各关节对应的电机与相应关节之间的传动比，确定下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的大小；

获取该上一级关节和下一级关节之间的耦合关系，其中，所述耦合关系包括传递耦合关系和从动耦合关系，具备所述传递耦合关系的两个关节对应的电机之间的自驱转动方向相同，具备所述从动耦合关系的两个关节对应的电机之间的自驱转动方向相反；

根据所述耦合关系确定对应于该下一级关节对应的电机用于补偿该上一级关节的转动的补偿转角的方向，其中，在所述从设备中上一级关节和下一级关节之间属于所述传递耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的转动的补偿转角的方向和上一级关节的关节转角的方向相反，在所述从设备中上一级关节和下一级关节之间属于所述从动耦合关系的情况下，该下一级关节对应的电机用于补偿上一级关节的转动的补偿转角的方向和上一级关节的关节转角的方向相同。

4.根据权利要求2所述的主从随动系统的力反馈方法，其特征在于，确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩包括：

根据所述从设备各关节的关节转角和D-H参数，得到所述从设备各关节的变化矩阵；

获取所述从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数；

根据所述从设备各关节的变化矩阵、所述从设备各关节的关节坐标系参数、各关节质心在各关节坐标系中的空间位置参数和力值参数，确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩。

5.一种主从随动系统的力反馈装置，其特征在于，所述装置包括：

获取模块，用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备各关节对应的电机与相应关节之间的传动比，其中，所述从设备各关节均采用齿轮联动耦合；

第一确定模块，用于根据所述从设备各关节对应的电机的实际电机扭矩和所述传动比，确定所述从设备各关节的实际关节扭矩；

第二确定模块，用于确定所述从设备在空载状态下因自重产生的各关节的理论关节扭矩，其中，所述从设备各关节的实际关节扭矩为所述从设备各关节对应的电机的实际电机扭矩和所述传动比之间的比值；

第三确定模块，用于根据所述从设备各关节的实际关节扭矩和所述理论关节扭矩，确定所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量；

反馈控制模块，用于根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制；

其中，根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制包括：

将所述关节扭矩增量和与所述预设末端负载对应的预设关节扭矩增量作差，得到关节扭矩增量偏差值；判断所述关节扭矩增量偏差值是否在预设阈值范围之内；在判断到所述关节扭矩增量偏差值在预设阈值范围之内的情况下，根据所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制，其中包括：调整所述从设备各关节的刚度系数，根据调整后的所述从设备各关节的刚度系数确定所述从设备各关节的反馈力，并将所述从设备各关节的反馈力传递给与所述从设备对应的主设备；在判断到所述关节扭矩增量偏差值不在预设阈值范围之内的情况下，判断所述关节扭矩增量落入的预设负载扭矩区间，并将该预设负载扭矩区间对应的预设末端负载作为新的预设末端负载，再执行根据所述从设备各关节的实际关节扭矩和所述理论关节扭矩，确定所述从设备各关节因所述从设备的预设末端负载引起的关节扭矩增量的步骤，直至最终计算得到的关节扭矩增量偏差值在预设范围之内。

6.一种主从随动系统，其特征在于，所述系统包括：主设备、从设备以及控制器，所述主设备和所述从设备分别与所述控制器连接；

所述从设备包括多个关节，所述从设备各关节均采用齿轮联动耦合，所述从设备中的每个关节均对应于一个电机，电机用于驱动所述从设备中相应的关节转动；

所述控制器用于执行如权利要求1至4中任一项所述的主从随动系统的力反馈方法。

7.根据权利要求6所述的主从随动系统，其特征在于，所述主设备为与所述从设备同构的设备，所述主设备包括多个关节，所述主设备中的每个关节均对应于一个电机，电机用于驱动所述主设备中相应的关节转动。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1至4中任一项所述的主从随动系统的力反馈方法。

Images