CN115847371A - 一种主从手机器人的控制方法及装置 - Google Patents

Abstract

本申请实施例提供一种主从手机器人的控制方法及装置。所述方法包括：获取从手电机在当前周期的映射位置和初始运动参数后，对从手电机在当前周期的运动轨迹进行规划并获取最大规划距离，最终将映射运动距离和最大规划距离中能够优先达到的值确定为从手电机在当前周期需要运动的实际距离。如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，可以将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。

Description

一种主从手机器人的控制方法及装置

技术领域

本申请涉及机器人技术领域，特别涉及一种主从手机器人的控制方法及装置。

背景技术

人手在进行微小尺度操作时往往会存在一定的局限性，例如会发生手部震颤等细微手部动作。为了克服人手在微小尺度操作的局限性，能够代替人手部分工作的主从手机器人应运而生。

主从手机器人主要包括主手和与主手对应的从手，其中，主手完全由操作者进行控制，并如实反映操作者人手的运动参数，从手通常为机械臂，机械臂上设置有至少一个关节，每个关节上设置有电机。在实际控制中，从手的各个电机的运动参数是根据主手(通常是指主手上的设定点)的三维坐标映射得到的，主手的运动通过主从手机器人的机械结构映射传递给从手，最终使得从手的目标设定点(比如末端点)跟随主手进行对应的运动。

采用上述控制方式，从手各个电机的运动参数是直接根据主手的运动映射得到的，然而，由于人手的加速度通常比电机快很多，部分情况下产生的加速度，比如：在向第一方向快速移动时突然反向加速移动，在映射到从手电机后可能会超出从手电机的极限转动加速度，进而导致电机报错，造成从手无法准确跟随主手进行运动，或者从手的电机寿命受影响的问题。

发明内容

现有的控制方法可能出现映射后的从手电机转动加速度超出极限加速度，进而导致电机报错，造成从手无法准确跟随主手进行运动，或者从手的电机寿命受影响的技术问题。为了解决这一问题，本申请实施例提供一种主从手机器人的控制方法及装置，具体地，本申请公开了以下技术方案：

第一方面，本申请实施例提供一种主从手机器人的控制方法，主从手机器人的从手上设置有至少一个从手电机，方法用于对从手电机的运动进行控制；方法包括：

获取从手电机在当前周期的映射位置；映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的，主手的三维坐标是按照预设采集周期采集得到的，当前周期为任一采集周期；

获取从手电机在当前周期的初始运动参数；初始运动参数包括初始速度V_s和初始加速度A_s；

根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划；

获取从手电机在当前周期内的最大规划距离P_cm；

从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离P_cm中，能够优先达到的值，确定为目标距离；映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值，实际位置用于表示实际旋转脉冲数，前一周期为当前周期的前一个采集周期；

将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。

结合第一方面，在第一方面的一种可实现方式中，还包括：

根据从手电机在当前周期的实际位置，确定从手在当前周期的实际三维坐标；

检测实际三维坐标是否超过从手的极限坐标；

如果实际三维坐标未超过从手的极限坐标，则控制从手电机运动到当前周期的实际位置。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果实际三维坐标超过从手的极限坐标，则根据从手的极限坐标，获取从手电机在当前周期的修正位置；

控制从手电机运动到当前周期的修正位置。

结合第一方面，在第一方面的一种可实现方式中，在控制从手电机运动之前，还包括：

如果主从手机器人的各个从手在当前周期的多轴运动不同步，则根据各个从手在当前周期的目标三维坐标，以及主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置；

根据各个从手上的从手电机的目标位置，获取主手的目标三维坐标；

如果主手的目标三维坐标超出主手的极限坐标，则将主手在当前周期的三维坐标修正为主手的极限坐标；

根据主手的极限坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出从手电机在当前周期的实际位置。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果主手的目标三维坐标未超出主手的极限坐标，则将主手在当前周期的三维坐标修正为主手的目标三维坐标；

根据主手的目标三维坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出从手电机在当前周期的实际位置。

结合第一方面，在第一方面的一种可实现方式中，获取从手电机在当前周期的初始运动参数，包括：

分别获取从手电机在当前周期以前的三个采集周期的实际位置；

根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度V_s和初始加速度A_s。

结合第一方面，在第一方面的一种可实现方式中，根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度V_s和初始加速度A_s，包括：

根据第一实际位置与第二实际位置的差值，以及采集周期的时长，确定从手电机在前一周期内的第一平均速度；第一实际位置为从手电机在前一周期的实际位置，第二实际位置为从手电机在前一周期的前一个采集周期的实际位置；

根据第二实际位置与第三实际位置的差值，以及采集周期的时长，确定从手电机在前一周期的前一个采集周期内的第二平均速度；第三实际位置为从手电机在当前周期以前的第三个采集周期的实际位置；

根据第二平均速度、第一平均速度，以及采集周期的时长，确定平均加速度；

将第一平均速度确定为初始速度V_s，将平均加速度确定为初始加速度A_s。

结合第一方面，在第一方面的一种可实现方式中，在根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划之前，方法还包括：

根据从手电机在当前周期的映射位置，以及从手电机在前一周期的实际位置，确定从手电机在当前周期内的第一运动方向；

根据从手电机在前一周期的实际位置，以及从手电机在前一周期的前一个采集周期的实际位置，确定从手电机在前一周期内的第二运动方向；

检测第一运动方向与第二运动方向是否相同。

结合第一方面，在第一方面的一种可实现方式中，在第一运动方向与第二运动方向相同的情况下，根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划，包括：

检测初始速度V_s是否等于从手电机的极限速度V_m；

如果初始速度V_s等于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划按照极限速度V_m匀速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果初始速度V_s小于极限速度V_m，则检测初始加速度A_s是否等于极限加速度A_m；

如果初始加速度A_s等于极限加速度A_m，则确定从初始速度V_s，按照极限加速度A_m匀加速到极限速度V_m所需的第一时长；

如果第一时长小于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照极限加速度A_m匀加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果第一时长大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划按照极限加速度A_m，从初始速度V_s匀加速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果初始加速度A_s小于极限加速度A_m，则确定从初始加速度A_s，按照第一加加速度，加速到极限加速度A_m所需的第二时长；

如果第二时长小于当前周期的时长，则获取第二时长结束后的中间速度；

如果中间速度小于极限速度V_m，则确定从中间速度，按照极限加速度A_m匀加速到极限速度V_m所需的第三时长；

如果第二时长和第三时长的和小于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度加速至极限加速度A_m，再按照极限加速度A_m匀加速至极限速度V_m，最后在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果第二时长和第三时长的和大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划从初始加速度A_s先加速至极限加速度A_m，再在当前周期的剩余时间内，按照极限加速度A_m匀加速至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果中间速度大于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度从初始速度V_s匀加加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果第二时长大于当前周期的时长，则确定从初始速度V_s，按照第一加加速度，匀加加速到当前周期结束后的末速度；

如果末速度小于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划从初始速度V_s，按照第一加加速度，匀加加速到当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果末速度大于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度，从初始速度V_s匀加加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，获取从手电机在当前周期内的最大规划距离P_cm，包括：

将从手电机在当前周期内的运动总距离，确定为从手电机在当前周期内的最大规划距离P_cm。

结合第一方面，在第一方面的一种可实现方式中，在第一运动方向与第二运动方向相反的情况下，根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划，包括：

根据初始运动参数，以及第二加加速度，检测从手电机在当前周期内，是否能够从初始速度V_s减速为零；

如果从手电机在当前周期内能够从初始速度V_s减速为零，则获取从手电机从初始速度V_s减速为零时的中间位置和中间加速度；

根据从手电机在中间位置时的运动参数，以及当前周期的剩余时长，对从手电机在当前周期的剩余时长的运动轨迹进行规划；其中，从手电机在中间位置时的初始速度为零、初始加速度为中间加速度。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果从手电机在当前周期内不能从初始速度V_s减速为零，则检测从手电机在当前周期内是否能够达到减速段最大加速度；

如果从手电机在当前周期内能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划先按照第二加加速度减速到减速段最大加速度，再在当前周期的剩余时间内，按照减速段最大加速度，匀减速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，还包括：

如果从手电机在当前周期内不能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划按照第二加加速度，减速运动至当前周期结束。

结合第一方面，在第一方面的一种可实现方式中，获取从手电机在当前周期内的最大规划距离P_cm，包括：

获取从手电机在当前周期结束时的末位置；

将末位置与从手电机在前一周期的实际位置的差值，确定为从手电机在当前周期内的最大规划距离P_cm。

第二方面，本申请实施例提供一种主从手机器人的控制装置，主从手机器人的从手上设置有至少一个从手电机，装置用于对从手电机的运动进行控制；装置包括：

映射位置获取模块，用于获取从手电机在当前周期的映射位置；映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的，主手的三维坐标是按照预设采集周期采集得到的，当前周期为任一采集周期；

初始运动参数获取模块，用于获取从手电机在当前周期的初始运动参数；初始运动参数包括初始速度V_s和初始加速度A_s；

运动规划模块，用于根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划；

最大规划距离获取模块，用于获取从手电机在当前周期内的最大规划距离P_cm；

目标距离确定模块，用于从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离P_cm中，能够优先达到的值，确定为目标距离；映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值，实际位置用于表示实际旋转脉冲数，前一周期为当前周期的前一个采集周期；

实际位置确定模块，用于将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。

本申请实施例提供一种主从手机器人的控制方法及装置。该方法中，在主手和从手之间虚拟增加一个虚拟轴，利用虚拟轴获取从手电机在当前周期的映射位置和初始运动参数后，对从手电机在当前周期的运动轨迹进行规划并获取最大规划距离，最终将映射运动距离和最大规划距离中能够优先达到的值确定为从手电机在当前周期需要运动的实际距离。如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，可以将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。

附图说明

图1为本申请实施例提供的一种主从手机器人的控制方法的工作流程示意图；

图2为本申请实施例提供的主从手机器人的控制方法的步骤S103的一种实现方式的流程示意图；

图3为本申请实施例提供的主从手机器人的控制方法的步骤S103的另一种实现方式的流程示意图；

图4为本申请实施例提供的主从手机器人的控制方法与传统主从手机器人的控制方法的从手电机运动对比示意图；

图5为本申请实施例提供的一种主从手机器人的控制装置的结构示意图。

具体实施方式

为使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请实施方式作进一步地详细描述。

为了解决传统控制方法可能出现映射后的从手电机转动加速度超出极限加速度，进而导致电机报错，造成从手无法准确跟随主手进行运动，或者从手的电机寿命受影响的技术问题，本申请通过以下实施例公开了一种主从手机器人的控制方法。本申请实施例提供的控制方法，应用于主从手机器人。主从手机器人包括主手和至少一个从手，每个从手与主手均为一一对应，各个从手之间同步运动。每个从手上均设置有至少一个从手电机。本申请实施例提供的控制方法具体用于对任一从手电机的运动进行控制。为了更清楚地说明本申请实施例，后续未做特殊说明的情况下，以主手仅对应有一个从手，并且该从手上仅设置有一个从手电机为例，进行具体说明。

在对本申请实施例进行介绍之前，首先对本申请实施例提及的部分概念进行说明。

本申请实施例中提及的主手，可以表示主手上的某一设定点，设定点可以位于掌心位置，或者手指末梢位置，具体可根据经验和实际情况确定，本申请实施例对此不做具体限定。主手的三维坐标，表示设定点的空间三维坐标。本申请实施例中提及的从手，可以表示从手上的目标设定点，比如：从手上的末端点，具体可根据经验和实际情况确定，本申请实施例对此不做具体限定。从手的三维坐标，表示目标设定点的空间三维坐标，与主手的三维坐标处于同一空间坐标系。从手电机的位置表示的是从手电机的旋转脉冲数，从手电机的运动指的是转动，从手电机的速度指的是转动速度，从手电机的加速度指的是转动加速度，从手电机的运动方向包括正转和反转。

下面结合附图对本申请实施例提供的一种主从手机器人的控制方法进行介绍。

参见图1所示的工作流程示意图，本申请实施例提供的一种主从手机器人的控制方法具体包括以下步骤：

步骤S101，获取从手电机在当前周期的映射位置。

其中，映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的。主手的三维坐标是按照预设采集周期采集得到的。当前周期为任一采集周期。具体地，采集周期可以根据需要和实际情况进行预设，本申请实施例对此不作具体限定。主手的三维坐标是指在每个采集周期的结束时刻主手位置的空间坐标。从手电机在当前周期的映射位置，是指从手电机在当前周期结束时刻的映射位置。

步骤S102，获取从手电机在当前周期的初始运动参数。

其中，初始运动参数包括初始速度V_s和初始加速度A_s。

在步骤S102的一种实现方式中，可以首先分别获取从手电机在当前周期以前的三个采集周期的实际位置。接下来，根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度V_s和初始加速度A_s。其中，从手电机在前一周期的第一实际位置，是指从手电机在前一周期结束时刻的实际位置。从手电机在前一周期的前一个采集周期的第二实际位置，是指从手电机在前一周期的前一个采集周期的结束时刻的实际位置。从手电机在前一周期的前一个采集周期的前一个采集周期的第三实际位置，也就是从手电机在当前周期以前的第三个采集周期的第三实际位置，是指从手电机在当前周期以前的第三个采集周期的结束时刻的实际位置。

在确定初始速度V_s和初始加速度A_s的一种实现方式中，可以通过以下步骤确定初始速度V_s和初始加速度A_s：

步骤一，根据第一实际位置与第二实际位置的差值，以及采集周期的时长T，确定从手电机在前一周期内的第一平均速度。

具体地，第一平均速度＝(第一实际位置-第二实际位置)/T。

步骤二，根据第二实际位置与第三实际位置的差值，以及采集周期的时长T，确定从手电机在前一周期的前一个采集周期内的第二平均速度。

具体地，第二平均速度＝(第二实际位置-第三实际位置)/T。

步骤三，根据第二平均速度、第一平均速度，以及采集周期的时长T，确定平均加速度。

具体地，平均加速度＝(第一平均速度-第二平均速度)/T。

步骤四，将第一平均速度确定为初始速度V_s，将平均加速度确定为初始加速度A_s。

在确定初始速度V_s和初始加速度A_s的其他实现方式中，可以根据实际应用场景，设定其他方式来确定速度和加速度，比如：可以将第一平均速度和第二平均速度的平均值，确定为初始速度V_s。或者，设置初始速度V_s＝第一平均速度+平均加速度×当前周期的时长/2。或者，也可以通过加加速度来得到初始速度V_s和初始加速度A_s。这样，可以根据实际情况匹配更多的应用场景，从而更具有灵活性。此外，如果初始运动参数已知的情况下，也可以无需根据当前周期的前三个周期的数据进行初始运动参数计算。这样，可以减少三个周期的数据延迟，在任意时刻(比如：要求主手运动过某个位置，或运动具体时间后再进行主从手联动的需求情况下等)应用本申请实施例提供的方法，适应性更广。

另外，在执行步骤S102之后，执行步骤S103之前，本申请实施例提供的控制方法还可以包括：首先，根据从手电机在当前周期的映射位置P₄，以及从手电机在前一周期的实际位置P₃，确定从手电机在当前周期内的第一运动方向。示例性地，从手电机在当前周期的映射位置P₄为10000个脉冲，在前一周期的实际位置P₃为5000个脉冲，则确定第一运动方向为正转。从手电机在当前周期的映射位置P₄为5000个脉冲，在前一周期的实际位置P₃为10000个脉冲，则确定第一运动方向为反转。然后，根据从手电机在前一周期的实际位置P₃，以及从手电机在前一周期的前一个采集周期的实际位置P₁，确定从手电机在前一周期内的第二运动方向。最后，检测第一运动方向与第二运动方向是否相同。其中，实际位置用于表示实际旋转脉冲数。前一周期为当前周期的前一个采集周期。

步骤S103，根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划。

图2为本申请实施例提供的主从手机器人的控制方法的步骤S103的一种实现方式的流程示意图。在第一运动方向与第二运动方向相同的情况下，如图2所示，步骤S103可以通过以下实现方式基于S形速度曲线进行规划：

步骤S201，检测初始速度V_s是否等于从手电机的极限速度V_m。如果初始速度V_s等于极限速度V_m，则执行步骤S202。如果初始速度V_s小于极限速度V_m，则执行步骤S203。

需要说明的是，如果出现初始速度V_s大于极限速度V_m的情况，则从手电机就会报错，因此通常不会出现初始速度V_s大于极限速度V_m的情况，对于初始速度V_s大于极限速度V_m的情况本申请实施例中不进行单独说明。

步骤S202，对于从手电机在当前周期的运动轨迹，规划按照极限速度V_m匀速运动至当前周期结束。

步骤S203，检测初始加速度A_s是否等于极限加速度A_m。如果初始加速度A_s等于极限加速度A_m，则执行步骤S204。如果初始加速度A_s小于极限加速度A_m，则执行步骤S208。

需要说明的是，如果出现初始加速度A_s大于极限加速度A_m的情况，则从手电机就会报错，因此通常不会出现初始加速度A_s大于极限加速度A_m的情况，对于初始加速度A_s大于极限加速度A_m的情况本申请实施例中不进行单独说明。

步骤S204，确定从初始速度V_s，按照极限加速度A_m匀加速到极限速度V_m所需的第一时长。

具体地，第一时长T₁可以通过以下公式确定：

T₁＝(V_m-V_s)/A_m 公式(1)

步骤S205，检测第一时长是否小于当前周期的时长。如果第一时长小于当前周期的时长，则执行步骤S206。如果第一时长大于当前周期的时长，则执行步骤S207。

另外，如果第一时长等于当前周期的时长，则规划按照极限加速度A_m匀加速至极限速度V_m。对于第一时长等于当前周期的时长的情况，不再单独说明。

步骤S206，对于从手电机在当前周期的运动轨迹，规划先按照极限加速度A_m匀加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

此时，从手电机可以在当前周期内达到极限速度V_m，因此规划先匀加速运动，再继续匀速运动。

步骤S207，对于从手电机在当前周期的运动轨迹，规划按照极限加速度A_m，从初始速度V_s匀加速运动至当前周期结束。

此时，从手电机在当前周期内无法达到极限速度V_m，因此规划按照最大能力匀加速整个周期。

步骤S208，确定从初始加速度A_s，按照第一加加速度，加速到极限加速度A_m所需的第二时长。

具体地，第二时长T_a可以通过以下公式确定：

T_a＝(A_m-A_s)/J₁ 公式(2)

其中，J₁为第一加加速度，表示匀加加速段预设加加速度，J₁的数值为正值。

步骤S209，检测第二时长是否小于或等于当前周期的时长。如果第二时长小于或等于当前周期的时长，则执行步骤S210。如果第二时长大于当前周期的时长，则执行步骤S217。

步骤S210，获取第二时长结束后的中间速度。

具体地，第二时长结束后的中间速度V可以通过以下公式确定：

其中，T_a为第二时长，J₁为第一加加速度，V_s为初始速度。

步骤S211，检测中间速度是否小于极限速度V_m。如果中间速度小于极限速度V_m，则执行步骤S212。如果中间速度大于极限速度V_m，则执行步骤S216。

另外，如果中间速度等于极限速度V_m，则规划按照第一加加速度从初始速度V_s匀加加速至极限速度V_m。此时，从手电机既达到最大速度，也达到最大加速度。

步骤S212，确定从中间速度，按照极限加速度A_m匀加速到极限速度V_m所需的第三时长。

具体地，第三时长T₃可以通过以下公式确定：

T₃＝(V_m-V)/A_m 公式(4)

其中，V为中间速度。

步骤S213，检测第二时长和第三时长的和是否小于当前周期的时长。如果小于，则执行步骤214。如果大于，则执行步骤215。

另外，如果第二时长和第三时长的和等于当前周期的时长，则先按照第一加加速度加速至极限加速度A_m，再按照极限加速度A_m匀加速至极限速度V_m。

步骤S214，对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度加速至极限加速度A_m，再按照极限加速度A_m匀加速至极限速度V_m，最后在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

此时，从手电机在当前周期内，既可以达到极限速度V_m，也可以达到极限加速度A_m。

步骤S215，对于从手电机在当前周期的运动轨迹，规划从初始加速度A_s先加速至极限加速度A_m，再在当前周期的剩余时间内，按照极限加速度A_m匀加速至当前周期结束。

此时，从手电机在当前周期内，可以达到极限加速度A_m，但无法达到极限速度V_m。

步骤S216，对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度从初始速度V_s匀加加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

其中，从初始速度V_s匀加加速至极限速度V_m所需的时长T_v可以通过以下公式确定：

其中，J₁为第一加加速度。

此时，从手电机在当前周期内，无法达到极限加速度A_m，但可以达到极限速度V_m。

步骤S217，确定从初始速度V_s，按照第一加加速度，匀加加速到当前周期结束后的末速度。

具体地，末速度V_e可以通过以下公式确定：

V_e＝V_s+1/2×J₁×T² 公式(6)

其中，J₁为第一加加速度，T为当前周期的时长。

步骤S218，检测末速度是否小于或等于极限速度V_m。如果末速度小于或等于极限速度V_m，则执行步骤S219。如果末速度大于极限速度V_m，则执行步骤S216。

步骤S219，对于从手电机在当前周期的运动轨迹，规划从初始速度V_s，按照第一加加速度，匀加加速到当期周期结束。

此时，如果末速度小于极限速度V_m，从手电机在当前周期内，既无法达到极限速度V_m，也无法达到极限加速度A_m。

如此，通过上述基于S形速度曲线的轨迹规划方式，可以使得从手电机的速度较为平缓，且不报错，速度和加速度均可以满足系统电机的参数要求，而且运动轨迹可以大致追随主手的运动轨迹。

图3为本申请实施例提供的主从手机器人的控制方法的步骤S103的另一种实现方式的流程示意图。在第一运动方向与第二运动方向相反的情况下，如图3所示，步骤S103可以通过以下实现方式实现：

步骤S301，根据初始运动参数，以及第二加加速度，检测从手电机在当前周期内，是否能够从初始速度V_s减速为零。如果能够从初始速度V_s减速为零，则执行步骤S302。如果不能从初始速度V_s减速为零，则执行步骤S304。

其中，第二加加速度，表示匀加减速段预设加加速度，且数值为负值。

步骤S302，获取从手电机从初始速度V_s减速为零时的中间位置和中间加速度。

具体地，根据从手电机的初始运动参数，以及到达中间位置时的速度，再结合第二加加速度，运动学分析后可以求解出中间位置和中间加速度。

步骤S303，根据从手电机在中间位置时的运动参数，以及当前周期的剩余时长，对从手电机在当前周期的剩余时长的运动轨迹进行规划。其中，从手电机在中间位置时的初始速度为零、初始加速度为中间加速度。

具体地，运动轨迹规划方式可以参照前述步骤S201至步骤S219，此处不再单独赘述。

步骤S304，检测从手电机在当前周期内是否能够达到减速段最大加速度。如果能，则执行步骤S305，否则，执行步骤S306。

具体地，可以首先确定从手电机从初始运动参数按照第二加加速度减速到减速段最大加速度时所需的时长，如果该时长大于当前周期的时长，则确定从手电机在当前周期内无法达到减速段最大加速度，否则，确定从手电机在当前周期内能够达到减速段最大加速度。

步骤S305，对于从手电机在当前周期的运动轨迹，规划先按照第二加加速度减速到减速段最大加速度，再在当前周期的剩余时间内，按照减速段最大加速度，匀减速运动至当前周期结束。

具体地，从初始加速度A_s按照第二加加速度减速到减速段最大加速度，所需的第四时长T₄可以通过以下公式确定：

T₄＝(A_smd-A_s)/J₂ 公式(7)

其中，J₂为第二加加速度，表示匀加减速段预设加加速度，且数值为负值。A_smd为减速段最大加速度。

第四时长T₄结束后的中间速度V₂可以通过以下公式确定：

其中，T₄为第四时长，J₂为第二加加速度，V_s为初始速度。

步骤S306，对于从手电机在当前周期的运动轨迹，规划按照第二加加速度，减速运动至当前周期结束。

如此，通过上述基于S形速度曲线的轨迹规划方式，可以使得从手电机的速度较为平缓，且不报错，速度和加速度均可以满足系统电机的参数要求，而且运动轨迹可以大致追随主手的运动轨迹。

此外，还可以通过其他方式进行轨迹规划，比如：基于T形速度曲线进行规划。如此，轨迹规划可以更灵活，从而可以具有更广泛的应用范围。

需要说明的是，本申请实施例中，在当前周期为初始采集周期时，将从手电机在初始采集周期的初始速度和初始加速度均设置为零，无需进行运动规划。如果当前周期为初始采集周期之后的采集周期，则可以执行步骤S103来进行运动轨迹规划。其中，如果当前周期为初始采集周期之后的第一个周期，则无需进行运动方向检测，直接默认为运动方向与前一周期的方向相同，并且初始速度和初始加速度均为零。如果当前周期为初始采集周期之后的第一个周期以后的周期，则需要首先进行运动方向检测。

步骤S104，获取从手电机在当前周期内的最大规划距离P_cm。

具体地，步骤S104与前述各种规划方式相对应。

在一种实现方式中，在第一运动方向与第二运动方向相同的情况下，按照上述步骤S201至步骤S219，对从手电机在当前周期的运动轨迹进行规划以后，可以将从手电机在当前周期内的运动总距离，确定为从手电机在当前周期内的最大规划距离P_cm。

下面对最大规划距离P_cm的计算进行具体介绍。

在执行步骤S202以后，最大规划距离P_cm可以通过以下公式进行确定：

P_cm＝V_m×T 公式(9)

其中，V_m为极限速度，T为当前周期的时长。

此外，在执行步骤S206以后，最大规划距离P_cm可以通过以下公式进行确定：

其中，V_s为初始速度，T₁为第一时长，A_m为极限加速度，V_m为极限速度，T为当前周期的时长。

此外，在执行步骤S207以后，最大规划距离P_cm可以通过以下公式进行确定：

P_cm＝V_s×T+1/2×A_m×T² 公式(11)

其中，V_s为初始速度，A_m为极限加速度，T为当前周期的时长。

此外，在执行步骤S214以后，最大规划距离P_cm可以通过以下公式进行确定：

其中，J₁为第一加加速度，T_a为第二时长，T₃为第三时长，T为当前周期的时长，V为第二时长结束后的中间速度，V_s为初始速度，V_m为极限速度。

此外，在执行步骤S215以后，最大规划距离P_cm可以通过以下公式进行确定：

其中，J₁为第一加加速度，T_a为第二时长，T为当前周期的时长，V为第二时长结束后的中间速度，V_s为初始速度，A_m为极限加速度。

此外，在执行步骤S216以后，最大规划距离P_cm可以通过以下公式进行确定：

其中，J₁为第一加加速度，V_s为初始速度，V_m为极限速度，T为当前周期的时长，T_v为从初始速度V_s匀加加速至极限速度V_m所需的时长，具体可通过公式(5)确定。

此外，在执行步骤S219以后，最大规划距离P_cm可以通过以下公式进行确定：

P_cm＝1/6×J₁×T³+V_s×T 公式(15)

其中，J₁为第一加加速度，V_s为初始速度，T为当前周期的时长。

在另一种实现方式中，在第一运动方向与第二运动方向相反的情况下，按照上述步骤S301至步骤S306，对从手电机在当前周期的运动轨迹进行规划以后，可以首先获取从手电机在当前周期结束时的末位置，然后将末位置与从手电机在前一周期的实际位置的差值，确定为从手电机在当前周期内的最大规划距离P_cm。具体地，末位置等于从手电机的初始位置(即在前一周期的实际位置)加上当前周期的运动总距离。需要说明的是，由于涉及反向运动，因此运动距离是具有方向性的矢量。

下面对末位置的计算进行具体介绍。

在执行步骤S303之后，从手电机是首先维持与初始速度相同的方向进行减速运动，直至速度减为零，然后反向运动，直至当前周期结束。末位置是从手电机的初始位置加上减速段的运动总距离后，再减去反向运动的运动总距离所得到的结果。对于反向运动的运动轨迹规划，可参照前述步骤S201至步骤S219规划，对于反向运动的运动总距离，可以参照步骤S104的一种实现方式中所对应的最大规划距离计算方式进行计算，此处不再赘述。需要说明的是，反向运动的时长为当前周期的剩余时长，且反向运动的初始速度为零。

在执行步骤S305和步骤S306之后，从手电机是维持与初始速度相同的方向进行减速运动，直至当前周期结束。此时，末位置是从手电机的初始位置加上最大规划距离P_cm所得到的结果。

具体地，在执行步骤S305之后，最大规划距离P_cm可以通过以下公式进行确定：

其中，J₂为第二加加速度，V_s为初始速度，T为当前周期的时长，T₄为第四时长，V₂为第四时长T₄结束后的中间速度，A_smd为减速段最大加速度。

在执行步骤S306之后，最大规划距离P_cm可以通过以下公式进行确定：

P_cm＝1/6×J₂×T³+V_s×T 公式(17)

其中，J₂为第一加加速度，V_s为初始速度，T为当前周期的时长。

步骤S105，从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离P_cm中，能够优先达到的值，确定为目标距离。

其中，映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值。实际位置用于表示实际旋转脉冲数。前一周期为当前周期的前一个采集周期。

示例性地，从手电机在前一周期的实际位置为5000个脉冲，从手电机在当前周期的映射位置为2000个脉冲，映射运动距离为-3000个脉冲。根据从手电机的初始运动参数进行运动轨迹规划后，从手电机首先沿与初始速度相同的方向继续减速运动，直至速度减为零，此时中间位置为6500个脉冲，然后反向运动至当前周期结束，最后末位置为3000个脉冲，最大规划距离P_cm为-2000个脉冲。此时，由于从手电机需要反向运动，因此必要要把速度减为零后才能反向运动，从而从5000个脉冲反向运动后，能够优先达到的值为-2000个脉冲，因此，确定目标距离为-2000个脉冲。又示例性地，从手电机在前一周期的实际位置为5000个脉冲，从手电机在当前周期的映射位置为2000个脉冲，映射运动距离为-3000个脉冲。从手电机规划的运动轨迹是沿与初始速度相同的方向一直减速运动到当前周期结束，此时速度仍未减为零，且末位置为8000个脉冲，最大规划距离P_cm为3000个脉冲。根据电机的运动可知，能够优先达到的值为8000个脉冲，因此，确定目标距离为3000个脉冲。需要说明的是，本申请实施例中，映射运动距离和最大规划距离P_cm的符号均用于表示方向。

步骤S106，将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。

如此，本申请实施例提供的控制方法，相当于在主手和从手之间增加了一个虚拟轴，通过虚拟轴对数据进行处理后，将映射运动距离和最大规划距离中能够优先达到的值发送给从手电机，如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。

在执行步骤S106之后，本申请实施例提供的控制方法还可以包括：

步骤S107，根据从手电机在当前周期的实际位置，确定从手在当前周期的实际三维坐标。

步骤S108，检测实际三维坐标是否超过从手的极限坐标。如果超过，则执行步骤S109。如果未超过，则执行步骤S110。

步骤S109，根据从手的极限坐标，获取从手电机在当前周期的修正位置。

在检测实际三维坐标是否超过从手的极限坐标之后，无论是否对从手电机在的当前周期的实际位置进行修正，均继续执行如下步骤：

步骤S110，检测主从手机器人的各个从手在当前周期的多轴运动是否同步。如果不同步，则执行步骤S111。如果同步，则执行步骤S116。

步骤S111，根据各个从手在当前周期的目标三维坐标，以及主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置。

步骤S112，根据各个从手上的从手电机的目标位置，获取主手的目标三维坐标。

步骤S113，检测主手的目标三维坐标是否超出主手的极限坐标。如果主手的目标三维坐标超出主手的极限坐标，则执行步骤S114。如果主手的目标三维坐标未超出主手的极限坐标，则执行步骤S115。

步骤S114，将主手在当前周期的三维坐标修正为主手的极限坐标。然后返回步骤S101重新获取各个从手电机在当前周期的映射位置。

步骤S115，将主手在当前周期的三维坐标修正为主手的目标三维坐标。然后返回步骤S101重新获取各个从手电机在当前周期的映射位置。

步骤S116，检测从手电机在当前周期的实际位置是否经过修正。如果未经过修正，则执行步骤S117。如果经过修正，则执行步骤S118。

步骤S117，控制从手电机运动到当前周期的实际位置。

步骤S118，控制从手电机运动到当前周期的修正位置。

如此，通过上述方式，在实际应用中如发现影响从手多轴协同，可通过机器人运动正逆解来得到主手的运动限制参数，基于主手的运动限制参数再重新进行本申请实施例提供的虚拟轴算法处理，最终映射到从手各关节，可以有效避免特殊情况下从手联动出现的多轴协同问题。

图4为本申请实施例提供的主从手机器人的控制方法与传统主从手机器人的控制方法的从手电机运动对比示意图。如图4所示，各个曲线的横轴都为采样周期点。其中，在运动距离变化曲线中，曲线A1为采用传统主从手机器人的控制方法(即从主手直接映射到从手电机)所得到的从手电机的运动距离变化曲线；曲线B1为采用本申请实施例提供的主从手机器人的控制方法所得到的从手电机的运动距离变化曲线。曲线A1是直接根据主手的运动映射得到的运动轨迹，但由于电机能力不足，从手电机无法实现该段轨迹。经本申请实施例中的虚拟轴处理方法进行处理后，曲线B1中从手电机大致追随主手的运动轨迹，在斜率(即运动速度)上，相比曲线A1的原始数据有所降低，以适应从手电机的实际运动能力，避免报错。在运动速度变化曲线中，曲线A2为采用传统主从手机器人的控制方法所得到的从手电机的运动速度变化曲线；曲线B2为采用本申请实施例提供的主从手机器人的控制方法所得到的从手电机的运动速度变化曲线。曲线A2中速度是离散的，且突变明显，速度经常会很大，而曲线B2中的速度则较为平缓，在到达从手电机极限值时，不再上升，保持匀速运动。在加速度变化曲线中，曲线A3为采用传统主从手机器人的控制方法所得到的从手电机的加速度变化曲线；曲线B3为采用本申请实施例提供的主从手机器人的控制方法所得到的从手电机的加速度变化曲线。曲线A3中加速度非常大，从手电机通常无法达到。而曲线B3中加速度变化很小，仅在变向时有一些突变，相比曲线A3中的数据，电机将运行更平稳，且不报错。

下述为本申请的装置实施例，可以用于执行本申请的方法实施例。对于本申请装置实施例中未披露的细节，请参照本申请的方法实施例。

作为对上述各实施例的实现，本申请实施例提供一种主从手机器人的控制装置。其中，主从手机器人的从手上设置有至少一个从手电机，该控制装置用于对从手电机的运动进行控制。参见图5所示的结构示意图，本申请实施例提供的主从手机器人的控制装置包括：映射位置获取模块501、初始运动参数获取模块502、运动规划模块503、最大规划距离获取模块504、目标距离确定模块505和实际位置确定模块506。此外，所述装置还可以包括从手位置检测模块和从手运动控制模块等更多或更少的单元和模块，本实施例对该装置的结构不做限制。

映射位置获取模块501，用于获取从手电机在当前周期的映射位置；映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的，主手的三维坐标是按照预设采集周期采集得到的，当前周期为任一采集周期；

初始运动参数获取模块502，用于获取从手电机在当前周期的初始运动参数；初始运动参数包括初始速度V_s和初始加速度A_s；

运动规划模块503，用于根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划；

最大规划距离获取模块504，用于获取从手电机在当前周期内的最大规划距离P_cm；

目标距离确定模块505，用于从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离P_cm中，能够优先达到的值，确定为目标距离；映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值，实际位置用于表示实际旋转脉冲数，前一周期为当前周期的前一个采集周期；

实际位置确定模块506，用于将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。

在一种可实现方式中，该装置还包括：

从手位置检测模块，用于根据从手电机在当前周期的实际位置，确定从手在当前周期的实际三维坐标；以及，检测实际三维坐标是否超过从手的极限坐标；

从手运动控制模块，用于如果实际三维坐标未超过从手的极限坐标，则控制从手电机运动到当前周期的实际位置。

在一种可实现方式中，从手运动控制模块还用于：如果实际三维坐标超过从手的极限坐标，则根据从手的极限坐标，获取从手电机在当前周期的修正位置；以及，控制从手电机运动到当前周期的修正位置。

在一种可实现方式中，从手运动控制模块在控制从手电机运动之前，还用于：

如果主从手机器人的各个从手在当前周期的多轴运动不同步，则根据各个从手在当前周期的目标三维坐标，以及主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置；根据各个从手上的从手电机的目标位置，获取主手的目标三维坐标；如果主手的目标三维坐标超出主手的极限坐标，则将主手在当前周期的三维坐标修正为主手的极限坐标；根据主手的极限坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出从手电机在当前周期的实际位置。

在一种可实现方式中，从手运动控制模块还用于：

如果主手的目标三维坐标未超出主手的极限坐标，则将主手在当前周期的三维坐标修正为主手的目标三维坐标，并发送回映射位置获取模块501。

在一种可实现方式中，初始运动参数获取模块502具体用于：

分别获取从手电机在当前周期以前的三个采集周期的实际位置；根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度V_s和初始加速度A_s。

在一种可实现方式中，初始运动参数获取模块502具体用于：

根据第一实际位置与第二实际位置的差值，以及采集周期的时长，确定从手电机在前一周期内的第一平均速度；第一实际位置为从手电机在前一周期的实际位置，第二实际位置为从手电机在前一周期的前一个采集周期的实际位置；根据第二实际位置与第三实际位置的差值，以及采集周期的时长，确定从手电机在前一周期的前一个采集周期内的第二平均速度；第三实际位置为从手电机在当前周期以前的第三个采集周期的实际位置；根据第二平均速度、第一平均速度，以及采集周期的时长，确定平均加速度；将第一平均速度确定为初始速度V_s，将平均加速度确定为初始加速度A_s。

在一种可实现方式中，该装置在运动规划模块503之前，还包括：

方向检测模块，用于根据从手电机在当前周期的映射位置，以及从手电机在前一周期的实际位置，确定从手电机在当前周期内的第一运动方向；根据从手电机在前一周期的实际位置，以及从手电机在前一周期的前一个采集周期的实际位置，确定从手电机在前一周期内的第二运动方向；检测第一运动方向与第二运动方向是否相同。

在一种可实现方式中，在第一运动方向与第二运动方向相同的情况下，运动规划模块503具体用于：

检测初始速度V_s是否等于从手电机的极限速度V_m；如果初始速度V_s等于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划按照极限速度V_m匀速运动至当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果初始速度V_s小于极限速度V_m，则检测初始加速度A_s是否等于极限加速度A_m；如果初始加速度A_s等于极限加速度A_m，则确定从初始速度V_s，按照极限加速度A_m匀加速到极限速度V_m所需的第一时长；如果第一时长小于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照极限加速度A_m匀加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果第一时长大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划按照极限加速度A_m，从初始速度V_s匀加速运动至当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果初始加速度A_s小于极限加速度A_m，则确定从初始加速度A_s，按照第一加加速度，加速到极限加速度A_m所需的第二时长；如果第二时长小于当前周期的时长，则获取第二时长结束后的中间速度；如果中间速度小于极限速度V_m，则确定从中间速度，按照极限加速度A_m匀加速到极限速度V_m所需的第三时长；如果第二时长和第三时长的和小于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度加速至极限加速度A_m，再按照极限加速度A_m匀加速至极限速度V_m，最后在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果第二时长和第三时长的和大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划从初始加速度A_s先加速至极限加速度A_m，再在当前周期的剩余时间内，按照极限加速度A_m匀加速至当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果中间速度大于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度从初始速度V_s匀加加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果第二时长大于当前周期的时长，则确定从初始速度V_s，按照第一加加速度，匀加加速到当前周期结束后的末速度；如果末速度小于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划从初始速度V_s，按照第一加加速度，匀加加速到当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果末速度大于极限速度V_m，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度，从初始速度V_s匀加加速至极限速度V_m，再在当前周期的剩余时间内，按照极限速度V_m匀速运动至当前周期结束。

在一种可实现方式中，最大规划距离获取模块504具体用于：

将从手电机在当前周期内的运动总距离，确定为从手电机在当前周期内的最大规划距离P_cm。

在一种可实现方式中，在第一运动方向与第二运动方向相反的情况下，运动规划模块503具体用于：

根据初始运动参数，以及第二加加速度，检测从手电机在当前周期内，是否能够从初始速度V_s减速为零；如果从手电机在当前周期内能够从初始速度V_s减速为零，则获取从手电机从初始速度V_s减速为零时的中间位置和中间加速度；根据从手电机在中间位置时的运动参数，以及当前周期的剩余时长，对从手电机在当前周期的剩余时长的运动轨迹进行规划；其中，从手电机在中间位置时的初始速度为零、初始加速度为中间加速度。

在一种可实现方式中，运动规划模块503具体还用于：

如果从手电机在当前周期内不能从初始速度V_s减速为零，则检测从手电机在当前周期内是否能够达到减速段最大加速度；如果从手电机在当前周期内能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划先按照第二加加速度减速到减速段最大加速度，再在当前周期的剩余时间内，按照减速段最大加速度，匀减速运动至当前周期结束。

在一种可实现方式中，运动规划模块503具体还用于：

如果从手电机在当前周期内不能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划按照第二加加速度，减速运动至当前周期结束。

在一种可实现方式中，最大规划距离获取模块504具体用于：

获取从手电机在当前周期结束时的末位置；将末位置与从手电机在前一周期的实际位置的差值，确定为从手电机在当前周期内的最大规划距离P_cm。

如此，本申请实施例提供的控制装置，相当于在主手和从手之间增加了一个虚拟轴，通过虚拟轴对数据进行处理后，将映射运动距离和最大规划距离中能够优先达到的值发送给从手电机，如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。

本说明书的各个实施例之间相同相似的部分互相参见即可，每个实施例重点介绍的都是与其他实施例不同之处。尤其，对于装置和系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例部分的说明即可。

本领域的技术人员可以清楚地了解到本发明实施例中的技术可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明实施例中的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。

以上所述的本申请实施方式并不构成对本申请保护范围的限定。

Claims (22)

1.一种主从手机器人的控制方法，其特征在于，所述主从手机器人的从手上设置有至少一个从手电机，所述方法用于对所述从手电机的运动进行控制；所述方法包括：

获取所述从手电机在当前周期的映射位置；所述映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及所述主从手机器人的机械结构直接映射得到的，所述主手的三维坐标是按照预设采集周期采集得到的，所述当前周期为任一采集周期；

获取所述从手电机在所述当前周期的初始运动参数；所述初始运动参数包括初始速度V_s和初始加速度A_s；

根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划；

获取所述从手电机在所述当前周期内的最大规划距离P_cm；

从所述从手电机在前一周期的实际位置开始，根据所述初始运动参数，将映射运动距离和所述最大规划距离P_cm中，能够优先达到的值，确定为目标距离；所述映射运动距离为所述从手电机在所述当前周期的映射位置与所述从手电机在前一周期的实际位置的差值，所述实际位置用于表示实际旋转脉冲数，所述前一周期为所述当前周期的前一个采集周期；

将所述从手电机在前一周期的实际位置与所述目标距离的和，确定为所述从手电机在所述当前周期的实际位置。

2.根据权利要求1所述的方法，其特征在于，还包括：

根据所述从手电机在所述当前周期的实际位置，确定所述从手在所述当前周期的实际三维坐标；

检测所述实际三维坐标是否超过所述从手的极限坐标；

如果所述实际三维坐标未超过所述从手的极限坐标，则控制所述从手电机运动到所述当前周期的实际位置。

3.根据权利要求2所述的方法，其特征在于，还包括：

如果所述实际三维坐标超过所述从手的极限坐标，则根据所述从手的极限坐标，获取所述从手电机在所述当前周期的修正位置；

控制所述从手电机运动到所述当前周期的修正位置。

4.根据权利要求2或3所述的方法，其特征在于，在控制所述从手电机运动之前，还包括：

如果所述主从手机器人的各个从手在所述当前周期的多轴运动不同步，则根据各个从手在所述当前周期的目标三维坐标，以及所述主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置；

根据各个从手上的从手电机的目标位置，获取所述主手的目标三维坐标；

如果所述主手的目标三维坐标超出所述主手的极限坐标，则将所述主手在所述当前周期的三维坐标修正为所述主手的极限坐标；

根据所述主手的极限坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出所述从手电机在所述当前周期的实际位置。

5.根据权利要求4所述的方法，其特征在于，还包括：

如果所述主手的目标三维坐标未超出所述主手的极限坐标，则将所述主手在所述当前周期的三维坐标修正为所述主手的目标三维坐标；

根据所述主手的目标三维坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出所述从手电机在所述当前周期的实际位置。

6.根据权利要求1所述的方法，其特征在于，获取所述从手电机在所述当前周期的初始运动参数，包括：

分别获取所述从手电机在所述当前周期以前的三个采集周期的实际位置；

根据所述从手电机在所述当前周期以前的三个采集周期的实际位置，确定所述初始速度V_s和所述初始加速度A_s。

7.根据权利要求6所述的方法，其特征在于，所述根据所述从手电机在所述当前周期以前的三个采集周期的实际位置，确定所述初始速度V_s和所述初始加速度A_s，包括：

根据第一实际位置与第二实际位置的差值，以及所述采集周期的时长，确定所述从手电机在所述前一周期内的第一平均速度；所述第一实际位置为所述从手电机在所述前一周期的实际位置，所述第二实际位置为所述从手电机在所述前一周期的前一个采集周期的实际位置；

根据所述第二实际位置与第三实际位置的差值，以及所述采集周期的时长，确定所述从手电机在所述前一周期的前一个采集周期内的第二平均速度；所述第三实际位置为所述从手电机在所述当前周期以前的第三个采集周期的实际位置；

根据所述第二平均速度、所述第一平均速度，以及所述采集周期的时长，确定平均加速度；

将所述第一平均速度确定为所述初始速度V_s，将所述平均加速度确定为所述初始加速度A_s。

8.根据权利要求1所述的方法，其特征在于，在根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划之前，所述方法还包括：

根据所述从手电机在所述当前周期的映射位置，以及所述从手电机在所述前一周期的实际位置，确定所述从手电机在所述当前周期内的第一运动方向；

根据所述从手电机在所述前一周期的实际位置，以及所述从手电机在所述前一周期的前一个采集周期的实际位置，确定所述从手电机在所述前一周期内的第二运动方向；

检测所述第一运动方向与所述第二运动方向是否相同。

9.根据权利要求8所述的方法，其特征在于，在所述第一运动方向与所述第二运动方向相同的情况下，所述根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划，包括：

检测所述初始速度V_s是否等于所述从手电机的极限速度V_m；

如果所述初始速度V_s等于所述极限速度V_m，则对于所述从手电机在所述当前周期的运动轨迹，规划按照所述极限速度V_m匀速运动至所述当前周期结束。

10.根据权利要求9所述的方法，其特征在于，还包括：

如果所述初始速度V_s小于所述极限速度V_m，则检测所述初始加速度A_s是否等于所述极限加速度A_m；

如果所述初始加速度A_s等于所述极限加速度A_m，则确定从所述初始速度V_s，按照所述极限加速度A_m匀加速到所述极限速度V_m所需的第一时长；

如果所述第一时长小于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述极限加速度A_m匀加速至所述极限速度V_m，再在所述当前周期的剩余时间内，按照所述极限速度V_m匀速运动至所述当前周期结束。

11.根据权利要求10所述的方法，其特征在于，还包括：

如果所述第一时长大于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划按照所述极限加速度A_m，从所述初始速度V_s匀加速运动至所述当前周期结束。

12.根据权利要求10所述的方法，其特征在于，还包括：

如果所述初始加速度A_s小于所述极限加速度A_m，则确定从所述初始加速度A_s，按照第一加加速度，加速到所述极限加速度A_m所需的第二时长；

如果所述第二时长小于所述当前周期的时长，则获取所述第二时长结束后的中间速度；

如果所述中间速度小于所述极限速度V_m，则确定从所述中间速度，按照所述极限加速度A_m匀加速到所述极限速度V_m所需的第三时长；

如果所述第二时长和所述第三时长的和小于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第一加加速度加速至所述极限加速度A_m，再按照所述极限加速度A_m匀加速至所述极限速度V_m，最后在所述当前周期的剩余时间内，按照所述极限速度V_m匀速运动至所述当前周期结束。

13.根据权利要求12所述的方法，其特征在于，还包括：

如果所述第二时长和所述第三时长的和大于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划从所述初始加速度A_s先加速至所述极限加速度A_m，再在所述当前周期的剩余时间内，按照所述极限加速度A_m匀加速至所述当前周期结束。

14.根据权利要求12所述的方法，其特征在于，还包括：

如果所述中间速度大于所述极限速度V_m，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第一加加速度从所述初始速度V_s匀加加速至所述极限速度V_m，再在所述当前周期的剩余时间内，按照所述极限速度V_m匀速运动至所述当前周期结束。

15.根据权利要求12所述的方法，其特征在于，还包括：

如果所述第二时长大于所述当前周期的时长，则确定从所述初始速度V_s，按照所述第一加加速度，匀加加速到所述当前周期结束后的末速度；

如果所述末速度小于所述极限速度V_m，则对于所述从手电机在所述当前周期的运动轨迹，规划从所述初始速度V_s，按照所述第一加加速度，匀加加速到所述当前周期结束。

16.根据权利要求15所述的方法，其特征在于，还包括：

如果所述末速度大于所述极限速度V_m，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第一加加速度，从所述初始速度V_s匀加加速至所述极限速度V_m，再在所述当前周期的剩余时间内，按照所述极限速度V_m匀速运动至所述当前周期结束。

17.根据权利要求9-16任一项所述的方法，其特征在于，所述获取所述从手电机在所述当前周期内的最大规划距离P_cm，包括：

将所述从手电机在所述当前周期内的运动总距离，确定为所述从手电机在所述当前周期内的最大规划距离P_cm。

18.根据权利要求8所述的方法，其特征在于，在所述第一运动方向与所述第二运动方向相反的情况下，所述根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划，包括：

根据所述初始运动参数，以及第二加加速度，检测所述从手电机在所述当前周期内，是否能够从所述初始速度V_s减速为零；

如果所述从手电机在所述当前周期内能够从所述初始速度V_s减速为零，则获取所述从手电机从所述初始速度V_s减速为零时的中间位置和中间加速度；

根据所述从手电机在所述中间位置时的运动参数，以及所述当前周期的剩余时长，对所述从手电机在所述当前周期的剩余时长的运动轨迹进行规划；其中，所述从手电机在所述中间位置时的初始速度为零、初始加速度为所述中间加速度。

19.根据权利要求18所述的方法，其特征在于，还包括：

如果所述从手电机在所述当前周期内不能从所述初始速度V_s减速为零，则检测所述从手电机在所述当前周期内是否能够达到减速段最大加速度；

如果所述从手电机在所述当前周期内能够达到减速段最大加速度，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第二加加速度减速到所述减速段最大加速度，再在所述当前周期的剩余时间内，按照所述减速段最大加速度，匀减速运动至所述当前周期结束。

20.根据权利要求19所述的方法，其特征在于，还包括：

如果所述从手电机在所述当前周期内不能够达到减速段最大加速度，则对于所述从手电机在所述当前周期的运动轨迹，规划按照所述第二加加速度，减速运动至所述当前周期结束。

21.根据权利要求18-20任一项所述的方法，其特征在于，所述获取所述从手电机在所述当前周期内的最大规划距离P_cm，包括：

获取所述从手电机在所述当前周期结束时的末位置；

将所述末位置与所述从手电机在前一周期的实际位置的差值，确定为所述从手电机在所述当前周期内的最大规划距离P_cm。

22.一种主从手机器人的控制装置，其特征在于，所述主从手机器人的从手上设置有至少一个从手电机，所述装置用于对所述从手电机的运动进行控制；所述装置包括：

映射位置获取模块，用于获取所述从手电机在当前周期的映射位置；所述映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及所述主从手机器人的机械结构直接映射得到的，所述主手的三维坐标是按照预设采集周期采集得到的，所述当前周期为任一采集周期；

初始运动参数获取模块，用于获取所述从手电机在所述当前周期的初始运动参数；所述初始运动参数包括初始速度V_s和初始加速度A_s；

运动规划模块，用于根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划；

最大规划距离获取模块，用于获取所述从手电机在所述当前周期内的最大规划距离P_cm；

目标距离确定模块，用于从所述从手电机在前一周期的实际位置开始，根据所述初始运动参数，将映射运动距离和所述最大规划距离P_cm中，能够优先达到的值，确定为目标距离；所述映射运动距离为所述从手电机在所述当前周期的映射位置与所述从手电机在前一周期的实际位置的差值，所述实际位置用于表示实际旋转脉冲数，所述前一周期为所述当前周期的前一个采集周期；

实际位置确定模块，用于将所述从手电机在前一周期的实际位置与所述目标距离的和，确定为所述从手电机在所述当前周期的实际位置。

Images