CN112060098A - 主从随动力反馈控制方法、装置、计算机设备和存储介质 - Google Patents

Abstract

本申请涉及一种主从随动力反馈控制方法、装置、计算机设备和存储介质，方法包括：获取从设备各关节对应的电机的实际电机扭矩和从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵；根据从设备各关节对应的电机的实际电机扭矩和转换矩阵，确定从设备各关节的实际关节扭矩；确定从设备在空载状态下因自重产生的各关节的初始理论关节扭矩；根据从设备各关节的实际关节扭矩和初始理论关节扭矩，确定从设备各关节因从设备的末端负载引起的关节扭矩增量；根据从设备各关节因从设备的末端负载引起的关节扭矩增量，对从设备对应的主设备进行力反馈控制。本申请实现了核工业环境中的主设备对随动从设备的力反馈控制。

Description

主从随动力反馈控制方法、装置、计算机设备和存储介质

技术领域

本申请涉及工控技术领域，特别是涉及一种主从随动力反馈控制方法、装置、计算机设备和存储介质。

背景技术

在核工业中，许多工作要在密闭的箱室中进行。为防止核辐射泄漏，箱室内各种设备的移动、操作和检验维修需要以远程方式实现，远程遥控操作技术大幅减轻操作人员的辐照伤害与操作疲劳。然而，核工业箱室中常常充满辐射，由于辐射的穿透性与危害性，需150mm以上的铅层才能完全屏蔽，因此使用电子元器件的设备无法在该环境中使用，如具有电机内置结构的传统设备、通过内部机械力矩传感器获取关节力矩与末端碰撞力的设备。

现有的相关力控制方法多采用控制系统根据各个参数采用D-H参数法和雅克比矩阵法对电随动主从手的位置、速度以及力进行映射与转换，实现电随动从手对电随动机械主手的精确力反馈。这类方法不适用于在核辐射环境中，特别是多级齿轮传递结构下进行力反馈控制。

发明内容

本申请实施例提供了一种主从随动力反馈控制方法、装置、计算机设备和存储介质，以至少解决相关技术中力反馈控制方法在核辐射环境中不适用的问题。

第一方面，本申请实施例提供了一种主从随动力反馈控制方法，包括：

获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，其中，所述从设备采用齿轮差动耦合的方式控制各关节的转动；

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

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

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

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

在其中一些实施例中，获取所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，包括：

根据所述从设备各关节的差动耦合结构确定各关节的传动关系；

基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，并根据所述自驱转角aθ_n和所述补偿转角bθ_n计算对应电机的实际转角Rθ_n；

根据各关节电机的实际转角Rθ_n和关节转角θ_n，计算得到转换矩阵。

在其中一些实施例中，当关节对应的电机齿轮水平联动时，基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，包括：

当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角为aθ_n=i_n×θ_n+i_n×θ_n-1，其他各关节对应的电机的补偿值为i_n×θ_n-2，补偿转角为bθ_n=i_n×θ_n-2+……+i_n×θ₁，n-2≥0，i_n为电机n与上级齿轮的传动比；

当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角为aθ_n= i_n×θ_n+1－i_n×θ_n，其他各关节对应的电机的补偿值为i_n×θ_n-1，补偿转角为bθ_n=i_n×θ_n-1+……+i_n×θ₁，n-1≥0。

在其中一些实施例中，当关节对应的电机齿轮垂直联动时，基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，包括：

当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角为aθ_n=(i_n×θ_n+i_n×θ_n-1)×K，其他各关节对应的电机的补偿值为i_n×θ_n-2×K，补偿转角为bθ_n=(i_n×θ_n-2+……+i_n×θ₁)×K，n-2≥0，i_n为电机n与上级齿轮的传动比，K为竖直齿轮与水平齿轮的传动比；

当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角为aθ_n=(i_n×θ_n+1－i_n×θ_n)×K，其他各关节对应的电机的补偿值为i_n×θ_n-1×K，补偿转角为bθ_n=(i_n×θ_n-1+……+i_n×θ₁)×K，n-1≥0。

在其中一些实施例中，当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时，当前关节对应的电机的补偿转角bθ_n取正；

当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时，当前关节对应的电机的补偿转角bθ_n取负。

在其中一些实施例中，根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩包括：

将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积，得到对应关节的实际关节扭矩。

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

获取从设备各关节之间的位置变化矩阵；

基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵，计算得到从设备各关节的初始理论关节扭矩。

在其中一些实施例中，根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩，确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量，包括：

将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值，确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。

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

将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较，得到关节扭矩增量偏差；

当所述关节扭矩增量偏差大于第一预设阈值时，根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值，并根据所述实际负载值调整所述预设关节扭矩增量；

当所述关节扭矩增量偏差小于第二预设阈值时，基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。

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

转换矩阵获取单元，用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，其中，所述从设备采用齿轮差动耦合的方式控制各关节的转动；

实际关节扭矩确定单元，用于根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩；

初始理论关节扭矩确定单元，用于确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩；

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

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

第三方面，本申请实施例提供了一种计算机设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述第一方面所述的主从随动力反馈控制方法。

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

相比于相关技术，本申请实施例提供的主从随动力反馈控制方法，通过所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩进行分析计算，确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量，并根据所述关节扭矩增量对所述从设备对应的主设备进行力反馈控制，不涉及传感器信号的直接读取，实现了在充满放射性的核工业环境中的对具有齿轮差动耦合纯机械结构的随动从设备的力反馈控制。

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

附图说明

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

图1是本申请实施例提供的主从随动力反馈控制方法的应用场景示意图；

图2是本申请实施例中从设备的传动结构原理图；

图3是本申请其中一个实施例中主从随动力反馈控制方法的流程示意图；

图4是本申请其中一个实施例中主从随动力反馈控制原理框图；

图5是本申请其中一个实施例中主从随动力反馈控制方法的流程框图；

图6是本申请其中一个实施例中主从随动力反馈控制装置的结构框图；

图7是本申请其中一个实施例中计算机设备的结构示意图。

具体实施方式

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

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

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

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

本实施例提供的主从随动力反馈控制方法可应用于核工业中，图1是本实施例提供的主从随动力反馈控制方法的应用场景示意图。如图1所示，主从随动结构包括主设备10和从设备11，所述从设备11设置于屏蔽室12内，所述从设备11各关节对应的电机设置于外置电机箱13内，所述外置电机箱13设置于所述屏蔽室12外面。所述外置电机箱13通过信号通讯线缆14与所述主设备10以及控制柜15连接。其中，所述主设备和所述从设备有相同和相似结构，通过对主设备关节转动信号输入，来控制屏蔽室12内的从设备进行对应关节的转动执行，并将从设备负载作用于从设备的各关节扭矩通过信号通讯线缆14反馈至主设备。

示例性的，图2是本申请实施例从设备的传动结构原理图，如图2所示，从设备包括七个关节，七个关节分别为第一关节111、第二关节112、第三关节113、第四关节114、第五关节115、第六关节116和第七关节117，它们的运动方向如图1-图2中1#、2#、3#、4#、5#、6#和7#箭头所示，每个关节由相对应的电机进行驱动，七个关节对应的七个电机置于外置电机箱13内，分别为第一电机1c、第二电机2c、第三电机3c、第四电机4c、第五电机5c、第六电机6c和第七电机7c。其中，所述第一关节至所述第六关节6个自由度的驱动力，均由外置电机箱13内对应的电机通过多级齿轮差动耦合的方式传递至各个关节，第七电机控制夹爪开合。

本实施例提供了一种主从随动力反馈控制方法。图3是根据本申请实施例的主从随动力反馈控制方法的流程图，如图3所示，该流程包括如下步骤：

步骤S201，获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，其中，所述从设备采用齿轮差动耦合的方式控制各关节的转动。

在本实施例中，作业过程中，主设备的关节转动动力来自于关节内置电机，关节转动信号直接由关节内置电机直接输出至所述从设备。所述从设备的关节由外置电机箱内对应的电机通过多级齿轮差动耦合传递至所述屏蔽室内驱动关节转动。所述从设备的受力分布在每个关节上，各关节的实际关节扭矩由对应的实际电机扭矩经转换矩阵进行耦合计算后获得。其中，所述转换矩阵由实际的齿轮布局确定，可以通过齿轮差动耦合下，电机与对应关节的传动关系计算确定。

步骤S202，根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩。

在本实施例中基于所述转换矩阵，可以得到从设备实际电机扭矩与实际关节扭矩之间的转换算法。通过采集所述从设备各关节对应关节的实际电机扭矩，与所述转换矩阵进行关系换算，可以实现在齿轮差动耦合下的力矩传递求解。

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

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

在本实施例中，从设备齿轮差动耦合产生多重齿隙叠加导致各关节的实际关节扭矩和理论关节扭矩产生偏差。通过获取从设备在空载状态下因自重产生的各关节的初始理论关节扭矩，并与所述实际关节扭矩进行比对，可以得到复杂齿轮耦合关系下由从设备的末端负载引起的关节扭矩增量。其中，将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值，确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。

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

在本实施例中，将从设备各关节的实际关节扭矩，除去空载状态下因自重产生的各关节的初始理论关节扭矩得到末端负载引起的关节扭矩增量后，可以将该值通过信号通讯线缆传输至所述从设备对应的主设备，并基于同构控制原理，对应所述主设备的各个关节扭矩进行力矩叠加，使得操作人员感测到主设备各个关节扭矩，得到临场操作手感。

综上，本申请实施例提供的主从随动力反馈控制方法，通过所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩进行分析计算，确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量，并根据所述关节扭矩增量对所述从设备对应的主设备进行力反馈控制，不涉及传感器信号的直接读取，实现了在充满放射性的核工业环境中的对具有齿轮差动耦合纯机械结构的随动从设备的力反馈控制。

下面通过优选实施例对本申请实施例进行描述和说明。

在上述实施例的基础上，在一个优选实施例中，获取所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，包括：

步骤S2011，根据所述从设备各关节的差动耦合结构确定各关节的传动关系；

步骤S2012，基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，并根据所述自驱转角aθ_n和所述补偿转角bθ_n计算对应电机的实际转角Rθ_n；

在本实施例中，首先根据从设备各关节的差动耦合结构确定各关节的传动关系。其中，竖直齿轮与水平齿轮之间的传动比为K，电机n与上级齿轮的传动比为i_n，则

当第n个关节轴由上级齿轮直接驱动时，自驱转角aθ_n=i_n×θ_n，补偿转角bθ_n=i_n×θ_n-1+……+i_n×θ₁；

当关节对应的电机齿轮水平联动时，基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，包括：当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角aθ_n=i_n×θ_n+i_n×θ_n-1，其他各关节对应的电机的补偿值为i_n×θ_n-2，补偿转角bθ_n=i_n×θ_n-2+……+i_n×θ₁，n-2≥0；当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角aθ_n= i_n×θ_n+1-i_n×θ_n，其他各关节对应的电机的补偿值为i_n×θ_n-1，补偿转角bθ_n=i_n×θ_n-1+……+i_n×θ₁，n-1≥0。其中，当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时，当前关节对应的电机的补偿转角bθ_n取正；当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时，当前关节对应的电机的补偿转角bθ_n取负。

如图2所示，在一种具体的实施方式中，以从设备七个关节，说明从设备各关节对应电机的实际转角Rθ_n的计算过程：

计算第一关节111对应的第一电机1c的实际转动角度Rθ₁：

当如图2所示的第一关节111转动关节转角θ₁时，上级齿轮组所对应的第七电机1c的自驱转角aθ₁=i₁×θ₁，补偿转角bθ_n=i_n×θ_n-1+……+i_n×θ₁，n-1≥1，由于第一关节111对应的齿轮为最上层齿轮，n-1=0，不满足补偿转角公式，故第一关节111转动关节转角θ₁时对应的第一电机1c只需要旋转自驱转角aθ₁=i₁×θ₁，即第一电机1c的实际转角Rθ₁=i₁×θ₁，以实现第一关节111转动θ₁。

计算第二关节112对应第二电机2c的实际转动角度Rθ₂：

当如图2所示的第二关节112转动关节转角θ₂时，第二上级齿轮由上级齿轮直接驱动，第二上级齿轮所对应的第二电机2c的自驱转角aθ₂=i₂×θ₂，补偿转角bθ_n=i_n×θ_n-1+……+i_n×θ₁，n-1≥0。当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时，当前关节对应的电机的补偿转角bθ_n取正；当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时，当前关节对应的电机的补偿转角bθ_n取负，第二电机2c的补偿转角bθ₂=i₂×θ₁，即第二电机2c的实际转角Rθ₂=i₂×θ₂+i₂×θ₁，以实现第二关节112转动θ₂。

计算第三关节113对应第三电机3c的实际转动角度Rθ₃：

当如图2所示的第三关节113转动关节转角θ₃时，第三上级齿轮所对应的第三电机3c的自驱转角aθ₃=i₃×θ₄－i₃×θ₃，补偿转角bθ_n=i_n×θ_n-1+……+i_n×θ₁，n-1≥0。当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时，当前关节对应的电机的补偿转角bθ_n取正；当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时，当前关节对应的电机的补偿转角bθ_n取负，第三电机3c的补偿转角bθ₃=－i₃×θ₂+i₃×θ₁，即第三电机3c的实际转角Rθ₃=i₃×θ₄－i₃×θ₃－i₃×θ₂+i₃×θ₁，以实现第三关节113转动关节转角θ₃。

计算第四关节114对应第四电机4c的实际转动角度Rθ₄：

当如图2所示的第四关节114转动关节转角θ₄时，第四上级齿轮所对应的第四电机4c的自驱转角aθ₄=i₄×θ₄+i₄×θ₃，补偿转角bθ_n=i_n×θ_n-2+……+i_n×θ₁，n-2≥0。当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时，当前关节对应的电机的补偿转角bθ_n取正；当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时，当前关节对应的电机的补偿转角bθ_n取负，第四电机4c的补偿转角bθ₄=i₄×θ₂+i₄×θ₁，即第四电机4c的实际转角Rθ₄=i₄×θ₄+i₄×θ₃+i₄×θ₂+i₄×θ₁，以实现第四关节114转动关节转角θ₄。

计算第五关节115对应第五电机5c的实际转动角度Rθ₅，因均为上级齿轮异向差动摆动结构，第五电机5c的实际转动角度Rθ₅的求解方法与第三电机3c的实际转角Rθ₃相同。

计算第六关节116对应第六电机6c的实际转动角度Rθ₆，同理，第六电机6c的实际转动角度Rθ₆的求解方法与第四电机4c的实际转角Rθ₄相同。

计算第七关节117对应第七电机7c的实际转动角度Rθ₇：

当如图2所示的第七关节117转动关节转角θ₇时，第七上级齿轮所对应的第七电机7c的转角受到第六关节~第一关节的影响，因此，

Rθ₇=i₇×θ₁+i₇×θ₂－i₇×θ₃－i₇×θ₄+i₇×θ₅+i₇×θ₆－i₇×θ₇。

综上，电机n的实际转动角度Rθ_n为：

Rθ₇=i₇×θ₁+i₇×θ₂－i₇×θ₃－i₇×θ₄+i₇×θ₅+i₇×θ₆－i₇×θ₇；

Rθ₆=i₆×θ₆－i₆×θ₅－i₆×θ₄－i₆×θ₃－i₆×θ₂－i₆×θ₁；

Rθ₅=i₅×θ₆－i₅×θ₅－i₅×θ₄+i₅×θ₃+i₅×θ₂－i₅×θ₁；

Rθ₄=i₄×θ₄+i₄×θ₃+i₄×θ₂+i₄×θ₁；

Rθ₃=i₃×θ₄－i₃×θ₃－i₃×θ₂+i₃×θ₁；

Rθ₂=i₂×θ₂+i₂×θ₁；

Rθ₁=i₁×θ₁。

在另一种具体的实施方式中，当关节对应的电机齿轮垂直联动时，竖直齿轮与水平齿轮之间的传动比为K，则基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，包括：当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角aθ_n=(i_n×θ_n+i_n×θ_n-1)×K，其他各关节对应的电机的补偿值为i_n×θ_n-2×K，补偿转角bθ_n=(i_n×θ_n-2+……+i_n×θ₁)×K，n-2≥0，K为竖直齿轮与水平齿轮的传动比；当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角aθ_n=(i_n×θ_n+1－i_n×θ_n)×K，其他各关节对应的电机的补偿值为i_n×θ_n-1×K，补偿转角bθ_n=(i_n×θ_n-1+……+i_n×θ₁)×K，n-1≥0。可以理解，当关节对应的电机齿轮水平联动时，从设备各关节对应电机的实际转角Rθ_n的计算方法与电机齿轮水平联动时相同，本申请在此不做赘述。

需要说明的是，在本实施例中，从设备的关节和对应的电机可根据实际作业要求设置更多或更少的数量，所述实际转角Rθ_n的计算式可根据各关节之间的传动耦合关系进行适应性变形，本申请不作具体限定。

步骤S2013，根据各关节电机的实际转角Rθ_n和关节转角θ_n，计算得到转换矩阵。

在本实施例中，当计算得到从设备各电机的实际转角Rθ_n后，联立对应关节的关节转角θ_n，可以得到两者之间的转换矩阵，即转换矩阵A。具体可通过下式计算得到：

Rθ{7，6，5，4，3，2，1}=A*θ{7，6，5，4，3，2，1}，求得A：

其中，Rθ{7，6，5，4，3，2，1}表示Rθ_n引入矩阵对应位置后的列向量；θ{7，6，5，4，3，2，1}表示θ_n引入θ{n，…2，1}矩阵对应位置后的列向量。

在上述实施例的基础上，在一个优选实施例中，根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩，包括：将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积，得到对应关节的实际关节扭矩。具体的，可通过下式计算得到对应关节的实际关节扭矩：

GM{7，6，5，4，3，2，1}=DM{7，6，5，4，3，2，1}*A，即GM{n}=DM{n}*A；

其中，GM{7，6，5，4，3，2，1}与DM{7，6，5，4，3，2，1}分别表示行向量。

在上述实施例的基础上，在一个优选实施例中，步骤S203，包括：

首先获取从设备各关节之间的位置变化矩阵，具体的，可通过下式计算得到各关节之间的位置变化矩阵R{n}：

其中，

可通过上述各关节对应的关节转角

得到的关节转角序列，

为由D-H参数表得到的固定值。

然后基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵，计算得到从设备各关节的初始理论关节扭矩LGM{n}。具体的：

其中，LGM{7}=0，f{7}=G{7}；

R{n}表示

与

计算得到各关节之间的位置变化矩阵；

P{n，n+1}表示n+1坐标系原点在n坐标系的位置，坐标系可根据描述从设备各关节运动关系的D-H参数表建立；

r{n}表示从设备质心在当前关节n坐标系下空间位置的1×3向量；

G{n}表示从设备质心在当前关节n坐标系下所受力值的1×3向量；

G{n}= inv ( ∏ (R{1}*R{n} ))*m{n}，m{n}(i=1~6)为根据从设备结构得到的1×3向量，m{7}为根据负载预设的多组1×3向量；

f{n}表示当前关节的坐标系所受力，即在1~n坐标系下的受力集合。

在上述实施例的基础上，在一个优选实施例中，步骤S205，包括：

步骤S2051，将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较，得到关节扭矩增量偏差。

在本实施例中，所述关节扭矩增量由从设备的预设负载引起的实际的关节扭矩增量值，所述预设关节扭矩增量是基于对应的预设关节转角输入值和预设理论负载值计算得到的理论的关节扭矩增量值，可根据预设条件计算得到。因此，将所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较得到的关节扭矩增量偏差，即由从设备末端预设负载引起的扭矩偏差。

步骤S2052，当所述关节扭矩增量偏差大于第一预设阈值时，根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值，并根据所述实际负载值调整所述预设关节扭矩增量；当所述关节扭矩增量偏差小于第二预设阈值时，基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。

在本实施例中，当获取到关节扭矩增量偏差后，需要判定所述关节扭矩增量偏差是否在预设的合理范围内。其中，所述第一预设阈值可以是根据预设负载计算得到的理论的关节扭矩增量偏差上限，所述第二预设阈值可以是根据预设负载计算得到的理论的关节扭矩增量偏差下限。

当所述关节扭矩增量偏差大于第一预设阈值时，表明所述关节扭矩增量超出了预设合理范围，此时从设备对应的负载改变或触碰到了障碍物，预设关节扭矩增量不合理，需要根据实际负载值调节所述预设关节扭矩增量，使得理论的关节扭矩增量向实际的关节扭矩增量趋近，减小力反馈误差。当所述关节扭矩增量偏差小于第二预设阈值时，表明所述关节扭矩增量在预设合理范围内且存在系统误差，需要所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。可选地，可以根据所述关节扭矩增量偏差获取反馈力和关节转角的补偿值发送至所述主设备进行反馈控制，使主设备根据所述反馈力和关节转角的补偿值对从设备进行运动补偿。可选地，所述补偿值可以是基于关节扭矩增量偏差得到的调试值或经验值，本申请不做具体限定。

如图4所示为本申请其中一个实施例提供的主从随动力反馈控制原理框图，如图4所示，主从随动力反馈控制原理框图包括主控模块31、通讯模块32和随动控制模块33，随动控制模块33包括力反馈控制模块34，主控模块是一个开环的力控制器，将从设备的反馈力作为输入，将自身的关节转角作为输出，从设备采用了PID位置闭环控制，将主设备的关节转角作为输入，将自身的反馈力作为输出。同时，在该PID位置闭环控制中引入了关节扭矩增量偏差e，将关节扭矩增量偏差e引入从设备系统刚度系数K_e。其中，所述从设备系统刚度系数K_e用于描述从设备实际关节转角θ_n与反馈力F_e之间的系统关系。通过调整系统刚度系数K_e改变从设备反馈力F_e，将从设备反馈力F_e传递给主设备，改变主设备的力反馈值，此时会改变从设备的关节角度输出值；同时引入了主设备输出的力经PID控制求解之后的环节，作为判定条件来保证从设备关节角度的稳定输出。该控制原理为本领域常用原理，本实施例和相关技术中的PID控制框图相比，主要区别包括引入了关节扭矩增量偏差e的判定。

图中标号含义如下：

θ_m、θ_s：电随动机械主手的关节转角输入与电随动机械从手的关节转角输出；

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

v_m：电随动机械主手速度；

C_d：电随动机械主手阻尼系数；

K_e：电随动机械从手系统刚度系数；

F_h、F_e：电随动机械主手输入力与电随动机械从手反馈力；

K_p-p、K_f-f：位置前馈增益和力反馈增益；

M_m、M_s：电随动机械主手与电随动机械从手质量；

K_p、K_i、K_d：电随动机械从手的PID控制参数。

如图5所示，在上述实施例的基础上，在其中一个实施例中，主从随动力反馈控制方法包括以下步骤：

输入外部电随动机械主手的关节转动信号关节转角θ_n，然后根据同构控制原理，得到从设备的对应关节转角θ_n，然后根据各关节电机的实际转角Rθ_n和关节转角θ_n，计算得到转换矩阵。然后根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩。根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩，确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。当所述关节扭矩增量偏差大于第一预设阈值时，根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值，并根据所述实际负载值调整所述预设关节扭矩增量；当所述关节扭矩增量偏差小于第二预设阈值时，根据所述关节扭矩增量偏差获取反馈力和关节转角的补偿值发送至所述主设备进行反馈控制。

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

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

图6是根据本申请实施例的主从随动力反馈控制装置的结构框图，如图6所示，该装置包括：转换矩阵获取单元301、实际关节扭矩确定单元302、初始理论关节扭矩确定单元303、关节扭矩增量确定单元304和力反馈控制单元305。

转换矩阵获取单元301，用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，其中，所述从设备采用齿轮差动耦合的方式控制各关节的转动；

实际关节扭矩确定单元302，用于根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩；

初始理论关节扭矩确定单元303，用于确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩；

关节扭矩增量确定单元304，用于根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩，确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量；

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

在其中一个实施例中，转换矩阵获取单元301，包括：传动关系获取模块、转角获取模块和转换矩阵计算模块。

传动关系获取模块，用于根据所述从设备各关节的差动耦合结构确定各关节的传动关系；

转角获取模块，用于基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，并根据所述自驱转角aθ_n和所述补偿转角bθ_n计算对应电机的实际转角Rθ_n；

转换矩阵计算模块，用于根据各关节电机的实际转角Rθ_n和关节转角θ_n，计算得到转换矩阵。

在其中一个实施例中，当关节对应的电机齿轮水平联动时，转角获取模块包括：第一计算模块和第二计算模块。

第一计算模块，用于当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角aθ_n=i_n×θ_n+i_n×θ_n-1，其他各关节对应的电机补偿值为i_n×θ_n-2，补偿转角bθ_n=i_n×θ_n-2+……+i_n×θ₁，n-2≥0，i_n为电机n与上级齿轮的传动比；

第二计算模块，用于当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角aθ_n= i_n×θ_n+1-i_n×θ_n，其他各关节对应的电机补偿值为i_n×θ_n-1，补偿转角bθ_n=i_n×θ_n-1+……+i_n×θ₁，n-1≥0。

在其中一个实施例中，当关节对应的电机齿轮垂直联动时，转角获取模块还包括：第三计算模块和第四计算模块。

第三计算模块，用于当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角aθ_n=(i_n×θ_n+i_n×θ_n-1)×K，其他各关节对应的电机的补偿值为i_n×θ_n-2×K，补偿转角bθ_n=(i_n×θ_n-2+……+i_n×θ₁)×K，n-2≥0，i_n为电机n与上级齿轮的传动比，K为竖直齿轮与水平齿轮的传动比；

第四计算模块，用于当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角aθ_n=(i_n×θ_n+1-i_n×θ_n)×K，其他各关节对应的电机的补偿值为i_n×θ_n-1×K，补偿转角bθ_n=(i_n×θ_n-1+……+i_n×θ₁)×K，n-1≥0。

在其中一个实施例中，当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时，当前关节对应的电机的补偿转角bθ_n取正；

当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时，当前关节对应的电机的补偿转角bθ_n取负。

在其中一个实施例中，实际关节扭矩确定单元302，具体用于：

将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积，得到对应关节的实际关节扭矩。

在其中一个实施例中，初始理论关节扭矩确定单元303，包括：位置变化矩阵获取模块和初始理论关节扭矩计算模块。

位置变化矩阵获取模块，用于获取从设备各关节之间的位置变化矩阵；

初始理论关节扭矩计算模块，用于基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵，计算得到从设备各关节的初始理论关节扭矩。

在其中一个实施例中，关节扭矩增量确定单元304具体用于：

将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值，确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。

在其中一个实施例中，力反馈控制单元305，包括：关节扭矩增量偏差获取模块、第一判断模块和第二判断模块。

关节扭矩增量偏差获取模块，用于将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较，得到关节扭矩增量偏差；

第一判断模块，用于当所述关节扭矩增量偏差大于第一预设阈值时，根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值，并根据所述实际负载值调整所述预设关节扭矩增量；

第二判断模块，用于当所述关节扭矩增量偏差小于第二预设阈值时，基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。

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

另外，结合图3描述的本申请实施例主从随动力反馈控制方法可以由计算机设备来实现。图7为根据本申请实施例的计算机设备的硬件结构示意图。

计算机设备可以包括处理器71以及存储有计算机程序指令的存储器72。

具体地，上述处理器71可以包括中央处理器（CPU），或者特定集成电路（Application Specific Integrated Circuit，简称为ASIC），或者可以被配置成实施本申请实施例的一个或多个集成电路。

其中，存储器72可以包括用于数据或指令的大容量存储器。举例来说而非限制，存储器72可包括硬盘驱动器（Hard Disk Drive，简称为HDD）、软盘驱动器、固态驱动器（SolidState Drive，简称为SSD）、闪存、光盘、磁光盘、磁带或通用串行总线（Universal SerialBus，简称为USB）驱动器或者两个或更多个以上这些的组合。在合适的情况下，存储器72可包括可移除或不可移除（或固定）的介质。在合适的情况下，存储器72可在数据处理装置的内部或外部。在特定实施例中，存储器72是非易失性（Non-Volatile）存储器。在特定实施例中，存储器72包括只读存储器（Read-Only Memory，简称为ROM）和随机存取存储器（RandomAccess Memory，简称为RAM）。在合适的情况下，该ROM可以是掩模编程的ROM、可编程ROM（Programmable Read-Only Memory，简称为PROM）、可擦除PROM（Erasable ProgrammableRead-Only Memory，简称为EPROM）、电可擦除PROM（Electrically Erasable ProgrammableRead-Only Memory，简称为EEPROM）、电可改写ROM（Electrically Alterable Read-OnlyMemory，简称为EAROM）或闪存（FLASH）或者两个或更多个以上这些的组合。在合适的情况下，该RAM可以是静态随机存取存储器（Static Random-Access Memory，简称为SRAM）或动态随机存取存储器（Dynamic Random Access Memory，简称为DRAM），其中，DRAM可以是快速页模式动态随机存取存储器（Fast Page Mode Dynamic Random Access Memory，简称为FPMDRAM）、扩展数据输出动态随机存取存储器（Extended Date Out Dynamic RandomAccess Memory，简称为EDODRAM）、同步动态随机存取内存（Synchronous Dynamic Random-Access Memory，简称SDRAM）等。

存储器72可以用来存储或者缓存需要处理和/或通信使用的各种数据文件，以及处理器71所执行的可能的计算机程序指令。

处理器71通过读取并执行存储器72中存储的计算机程序指令，以实现上述实施例中的任意一种主从随动力反馈控制方法。

在其中一些实施例中，计算机设备还可包括通信接口73和总线70。其中，如图7所示，处理器71、存储器72、通信接口73通过总线70连接并完成相互间的通信。

通信接口73用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。通信接口73还可以实现与其他部件例如：外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。

总线70包括硬件、软件或两者，将计算机设备的部件彼此耦接在一起。总线70包括但不限于以下至少之一：数据总线（Data Bus）、地址总线（Address Bus）、控制总线（Control Bus）、扩展总线（Expansion Bus）、局部总线（Local Bus）。举例来说而非限制，总线70可包括图形加速接口（Accelerated Graphics Port，简称为AGP）或其他图形总线、增强工业标准架构（Extended Industry Standard Architecture，简称为EISA）总线、前端总线（Front Side Bus，简称为FSB）、超传输（Hyper Transport，简称为HT）互连、工业标准架构（Industry Standard Architecture，简称为ISA）总线、无线带宽（InfiniBand）互连、低引脚数（Low Pin Count，简称为LPC）总线、存储器总线、微信道架构（Micro ChannelArchitecture，简称为MCA）总线、外围组件互连（Peripheral Component Interconnect，简称为PCI）总线、PCI-Express（PCI-X）总线、串行高级技术附件（Serial AdvancedTechnology Attachment，简称为SATA）总线、视频电子标准协会局部（Video ElectronicsStandards Association Local Bus，简称为VLB）总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线70可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线，但本申请考虑任何合适的总线或互连。

该计算机设备可以基于获取到的程序指令，执行本申请实施例中的主从随动力反馈控制方法，从而实现结合图3描述的主从随动力反馈控制方法。

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

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

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

Claims (12)

1.一种主从随动力反馈控制方法，其特征在于，包括：

获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，其中，所述从设备采用齿轮差动耦合的方式控制各关节的转动；

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

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

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

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

2.根据权利要求1所述的主从随动力反馈控制方法，其特征在于，获取所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，包括：

根据所述从设备各关节的差动耦合结构确定各关节的传动关系；

基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，并根据所述自驱转角aθ_n和所述补偿转角bθ_n计算对应电机的实际转角Rθ_n；

根据各关节电机的实际转角Rθ_n和关节转角θ_n，计算得到转换矩阵。

3.根据权利要求2所述的主从随动力反馈控制方法，其特征在于，当关节对应的电机齿轮水平联动时，基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，包括：

当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角aθ_n=i_n×θ_n+i_n×θ_n-1，其他各关节对应的电机的补偿值为i_n×θ_n-2，补偿转角为bθ_n=i_n×θ_n-2+……+i_n×θ₁，n-2≥0，i_n为电机n与上级齿轮的传动比；

当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角 aθ_n=i_n×θ_n+1－i_n×θ_n，其他各关节对应的电机的补偿值为i_n×θ_n-1，补偿转角为bθ_n=i_n×θ_n-1+……+i_n×θ₁，n-1≥0。

4.根据权利要求2所述的主从随动力反馈控制方法，其特征在于，当关节对应的电机齿轮垂直联动时，基于所述传动关系，确定各关节对应的电机的自驱转角aθ_n和补偿转角bθ_n，包括：

当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时，自驱转角aθ_n=(i_n×θ_n+i_n×θ_n-1)×K，其他各关节对应的电机的补偿值为i_n×θ_n-2×K，补偿转角bθ_n=(i_n×θ_n-2+……+i_n×θ₁)×K，n-2≥0，i_n为电机n与上级齿轮的传动比，K为竖直齿轮与水平齿轮的传动比；

当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时，自驱转角aθ_n=(i_n×θ_n+1－i_n×θ_n)×K，其他各关节对应的电机的补偿值为i_n×θ_n-1×K，补偿转角bθ_n=(i_n×θ_n-1+……+i_n×θ₁)×K，n-1≥0。

5.根据权利要求2所述的主从随动力反馈控制方法，其特征在于，

当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时，当前关节对应的电机的补偿转角bθ_n取正；

当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时，当前关节对应的电机的补偿转角bθ_n取负。

6.根据权利要求1所述的主从随动力反馈控制方法，其特征在于，根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩包括：

将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积，得到对应关节的实际关节扭矩。

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

获取从设备各关节之间的位置变化矩阵；

基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵，计算得到从设备各关节的初始理论关节扭矩。

8.根据权利要求1所述的主从随动力反馈控制方法，其特征在于，根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩，确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量，包括：

将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值，确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。

9.根据权利要求2所述的主从随动力反馈控制方法，其特征在于，根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量，对所述从设备对应的主设备进行力反馈控制，包括：

将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较，得到关节扭矩增量偏差；

当所述关节扭矩增量偏差大于第一预设阈值时，根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值，并根据所述实际负载值调整所述预设关节扭矩增量；

当所述关节扭矩增量偏差小于第二预设阈值时，基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。

10.一种主从随动力反馈控制装置，其特征在于，包括：

转换矩阵获取单元，用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵，其中，所述从设备采用齿轮差动耦合的方式控制各关节的转动；

实际关节扭矩确定单元，用于根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵，确定所述从设备各关节的实际关节扭矩；

初始理论关节扭矩确定单元，用于确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩；

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

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

11.一种计算机设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至9中任一项所述的主从随动力反馈控制方法。

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

Images