一种机器人控制方法与装置

摘要

本发明涉及机器人控制技术领域，公开了一种机器人控制方法与装置，该机器人控制方法包括：设置追踪周期；在追踪周期内采集力传感器信息，根据采集的力传感器信息获取机器人多轴机构各轴关节的追踪位置，其中，追踪位置为多轴机构各轴关节在下一追踪周期的位置；记录追踪周期与追踪位置，在追踪周期内对追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至追踪位置。本发明实施方式还提供了一种机器人控制装置。使得控制机器人的过程精度更高，灵敏度更好。

权利要求

1.一种机器人控制方法，其特征在于，包括：

设置追踪周期；

在所述追踪周期内采集力传感器信息，根据采集的所述力传感器信息获取机器人多轴机构各轴关节的追踪位置，其中，所述追踪位置为所述多轴机构各轴关节在下一追踪周期的位置；

记录所述追踪周期与所述追踪位置，在所述追踪周期内对所述追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至所述追踪位置；

所述在所述追踪周期内采集力传感器信息，根据采集的所述力传感器信息获取机器人多轴机构各轴关节的追踪位置，包括：在所述追踪周期内采集力传感器信息，根据采集的所述力传感器信息获取多轴机构的法兰工装末端速度；根据所述多轴机构的法兰工装末端速度，获取多轴机构各轴关节的追踪速度；根据所述多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的追踪位置；

所述根据所述多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的追踪位置包括：在所述追踪周期内，获取多轴机构各轴关节的速度；根据所述多轴机构各轴关节的速度以及所述多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的加速度；根据所述多轴机构各轴关节的速度、所述多轴机构各轴关节的加速度以及所述追踪周期，获取多轴机构各轴关节的追踪位置；

所述获取多轴机构各轴关节的加速度后，还包括：

如果在所述多轴机构各轴关节的加速度中，存在超出预设的限制关节加速度的加速度，则对加速度超出限制关节加速度的轴关节，进行加速度修正，将限制关节加速度作为修正后的加速度，并且其余各轴关节的加速度按相同比例降低。

2.根据权利要求1所述的机器人控制方法，其特征在于，所述在所述追踪周期内采集力传感器信息，根据采集的所述力传感器信息获取多轴机构的法兰工装末端速度，包括：在所述追踪周期内，采集6维力传感器信息，其中，所述6维力传感器信息包括X、Y、Z方向的平移力和A、B、C方向的扭矩；

根据采集的所述6维力传感器信息、预设的平移力比例关系与扭矩比例关系，获取多轴机构的法兰工装末端速度；所述多轴机构的法兰工装末端速度的表达式为：

6v＝[kpx，kpy，kpz，kra，krb，krc]

其中，x、y、z分别为多轴机构的法兰工装末端在X、Y、Z方向的平移力，a、b、c分别为多轴机构的法兰工装末端在A、B、C方向的扭矩，kp与kr分别为预设的平移力比例关系与扭矩比例关系。

3.根据权利要求2所述的机器人控制方法，其特征在于，所述根据所述多轴机构的法兰工装末端速度，获取多轴机构各轴关节的追踪速度，具体包括：在所述追踪周期内，获取多轴机构各轴关节的位置；

根据所述多轴机构各轴关节的位置，获取雅可比矩阵；

根据所述多轴机构的法兰工装末端速度与所述雅可比矩阵，获取多轴机构各轴关节的追踪速度；所述多轴机构各轴关节的追踪速度的公式为：其中，6v是所述多轴机构的法兰工装末端速度，6J(Θ)是所述雅可比矩阵， 是所述多轴机构各轴关节的追踪速度。

4.根据权利要求3所述的机器人控制方法，其特征在于，所述获取多轴机构各轴关节的追踪速度后，还包括：如果在所述多轴机构各轴关节的追踪速度中，存在超出预设的限制关节速度的追踪速度，则对追踪速度超出限制关节速度的轴关节，进行速度修正，将限制关节速度作为修正后的追踪速度，并且其余各轴关节的追踪速度按相同比例降低。

5.一种机器人控制装置，其特征在于，包括：

设置模块，用于设置追踪周期；

获取模块，用于在所述追踪周期内采集力传感器信息，根据采集的所述力传感器信息获取机器人多轴机构各轴关节的追踪位置，其中，所述追踪位置为所述多轴机构各轴关节在下一追踪周期的位置；

位置下发模块，用于记录所述追踪周期与所述追踪位置，在所述追踪周期内对所述追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至所述追踪位置；

法兰工装末端速度获取子模块，用于在所述追踪周期内采集力传感器信息，根据采集的所述力传感器信息获取多轴机构的法兰工装末端速度；

追踪速度获取子模块，用于根据所述多轴机构的法兰工装末端速度，获取多轴机构各轴关节的追踪速度；

追踪位置获取子模块，用于根据所述多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的追踪位置；

所述追踪位置获取子模块，具体用于在所述追踪周期内，获取多轴机构各轴关节的速度；根据所述多轴机构各轴关节的速度以及所述多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的加速度；根据所述多轴机构各轴关节的速度、所述多轴机构各轴关节的加速度以及所述追踪周期，获取多轴机构各轴关节的追踪位置；

还包括：

追踪速度修正子模块，用于当所述多轴机构各轴关节的追踪速度中，存在超出预设的限制关节速度的追踪速度时，对追踪速度超出限制关节速度的轴关节，进行速度修正，将限制关节速度作为修正后的追踪速度，并且其余各轴关节的追踪速度按相同比例降低。

说明书

一种机器人控制方法与装置

技术领域

[0001] 本发明涉及机器人控制技术领域，特别涉及一种机器人控制方法与装置。

背景技术

[0002] 现有技术中，在末端带有力传感器的机器人拖动示教方法中，通常有以下三种处理方式，第一种方法是通过获取传感器力矩，获取机器人的法兰工装末端速度，并将获取的力矩与法兰工装末端速度作为一个PID(比例、积分、微分)控制环处理，进而得到机器人各轴关节的下发位置，实现拖动示教的功能。第二种方法是通过动力学方程直接由获取的传感器力矩换算出机器人各关节轴的下发位置。第三种方法是由获取的传感器力矩直接规划机器人法兰工装末端的轨迹，并逆解得到机器人各轴关节的轨迹。

[0003] 在实现本发明的过程中，发明人发现现有技术中至少存在以下问题：第一种方法由于不需要进一步规划机器人各轴关节的速度，因而，在拖动机器人时，无法有效的规避奇异位置。第二种方法直接由获取的传感器力矩换算出机器人各关节轴的下发位置，因而，机器人拖动示教的精度比较差。第三种方法由于不通过获取的机器人各轴关节的速度实现轨迹规划，因而，机器人拖动示教的精度比较差，且在拖动示教的过程中难以规避奇异位置。

发明内容

[0004] 本发明实施方式的目的在于提供一种机器人控制方法与装置，使得控制机器人的过程精度更高，灵敏度更好。

[0005] 为解决上述技术问题，本发明的实施方式提供了一种机器人控制方法，包括：设置追踪周期；在追踪周期内采集力传感器信息，根据采集的力传感器信息获取机器人多轴机构各轴关节的追踪位置，其中，追踪位置为多轴机构各轴关节在下一追踪周期的位置；记录追踪周期与追踪位置，在追踪周期内对追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至追踪位置。

[0006] 本发明的实施方式还提供了一种机器人控制装置，包括：设置模块，用于设置追踪周期；获取模块，用于在追踪周期内采集力传感器信息，根据采集的力传感器信息获取机器人多轴机构各轴关节的追踪位置，其中，追踪位置为多轴机构各轴关节在下一追踪周期的位置。位置下发模块，用于记录追踪周期与追踪位置，在追踪周期内对追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至追踪位置。

[0007] 本发明实施方式相对于现有技术而言，通过在追踪周期内采集力传感信息，并由此规划机器人多轴机构各轴关节的追踪位置，这样，有利于在控制机器人的过程中提高各关节轴运动的精度，以及提高灵敏度。通过记录及下发各位置下发周期的下发位置至伺服控制系统，控制多轴机构各轴关节移动至各下发位置，这样，有利于在拖动机器人移动一次后，实现示教机器人的功能。

[0008] 另外，在追踪周期内采集力传感器信息，根据采集的力传感器信息获取机器人多轴机构各轴关节的追踪位置，包括：在追踪周期内采集力传感器信息，根据采集的力传感器信息获取多轴机构的法兰工装末端速度；根据多轴机构的法兰工装末端速度，获取多轴机构各轴关节的追踪速度；根据多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的追踪位置。本发明实施方式中，根据采集的力传感器信息，可以获取多轴机构各轴关节的追踪速度，这样，有利用在获取机器人多轴机构各轴关节的追踪位置后，在将机器人拖动至追踪位置的过程中，提高多轴机构各轴关节移动的精度以及灵敏度。

[0009] 另外，根据多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的追踪位置包括：在追踪周期内，获取多轴机构各轴关节的速度；根据多轴机构各轴关节的速度以及多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的加速度；根据多轴机构各轴关节的速度、多轴机构各轴关节的加速度以及追踪周期，获取多轴机构各轴关节的追踪位置。本发明实施方式中，根据获取的多轴机构各轴关节的速度、加速度以及追踪周期，可以获取多轴机构各轴关节的追踪位置，这样，使得获取的追踪位置更准确。

[0010] 另外，获取多轴机构各轴关节的追踪速度后，还包括：如果在多轴机构各轴关节的追踪速度中，存在超出预设的限制关节速度的追踪速度，则对追踪速度超出限制关节速度的轴关节，进行速度修正，将限制关节速度作为修正后的追踪速度，并且其余各轴关节的追踪速度按相同比例降低。本发明实施方式中，对于超出预设的限制关节速度的追踪速度，可以进行速度修正，并将限制关节速度作为修正后的追踪速度，同时其余各轴关节的追踪速度可以按相同比例降低，这样，有利于在拖动多轴机构各轴关节移动的过程中，有效地规避奇异位置。

附图说明

[0011] 图1是根据本发明第一实施方式的机器人控制方法的流程图；

[0012] 图2是根据本发明第二实施方式的机器人控制方法的流程图；

[0013] 图3是根据本发明第三实施方式的机器人控制装置的结构图。

具体实施方式

[0014] 为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。然而，本领域的普通技术人员可以理解，在本发明各实施方式中，为了使读者更好地理解本申请而提出了许多技术细节。但是，即使没有这些技术细节和基于以下各实施方式的种种变化和修改，也可以实现本申请所要求保护的技术方案。

[0015] 本发明的第一实施方式涉及一种机器人控制方法，如图1所示，包括：

[0016] 步骤101：设置追踪周期。具体地说，终端可以预先设置追踪周期，其中，追踪周期为获取的力以及力矩值的更新周期。

[0017] 更具体地说，力传感器可以安装在机器人多轴机构的法兰工装末端上。终端可以预先设置追踪周期，并在追踪周期内多次采集力传感器信息，同时，对采集的力传感器信息进行滤波处理，从而，得到需要更新的力以及力矩值。例如，追踪周期为0.01秒时，终端可以在0.01秒内多次采集力传感器信息，得到多个力以及力矩值，并对采集到的力传感器信息进行滤波处理。终端可以获取得到的多个力以及力矩值的平均值，并将上述平均值作为追踪周期内获取的力以及力矩值。这样，终端可以每0.01秒更新一次获取的力以及力矩值。

[0018] 步骤102：在追踪周期内采集力传感器信息，根据采集的力传感器信息获取机器人多轴机构各轴关节的追踪位置。其中，追踪位置为多轴机构各轴关节在下一追踪周期的位置。

[0019] 具体地说，终端通过对在追踪周期内采集的力传感器信息进行滤波处理，获取到需要更新的力以及力矩值后，可以在追踪周期内更新上述力以及力矩值，并且，可以根据更新后的力以及力矩值获取多轴机构的法兰工装末端的速度，进而，根据上述获取的法兰工装末端速度可以获取机器人多轴机构各轴关节的追踪位置。

[0020] 在步骤103：记录追踪周期与追踪位置，在追踪周期内对追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至追踪位置。具体地说，在获取多轴机构各轴关节的追踪位置后，终端可以记录上述追踪位置以及追踪周期，并且可以根据上述追踪位置以及预设的位置下发周期，在追踪周期内对多轴机构各轴关节进行轨迹规划，得到追踪周期内各位置下发周期的下发位置，同时，可以将各下发位置下发至伺服控制系统，控制多轴机构各轴关节在各位置下发周期内移动至各位置下发周期的下发位置，并最终在追踪周期内移动至追踪位置。同时，终端还可以记录各位置下发周期的下发位置，这样，机器人多轴机构各轴关节可以复现上述规划轨迹。

[0021] 需要说明的是，预设的位置下发周期可以小于或等于追踪周期。例如，追踪周期为0.01秒时，终端可以将位置下发周期设置为0.002秒。根据获取的多轴机构各轴关节的追踪位置，终端可以在0.01秒内对多轴机构各轴关节进行轨迹规划，得到每0.002秒内多轴机构各轴关节需移动至的下发位置，同时，可以将各下发位置下发至伺服控制系统，控制多轴机构在各0.002秒内移动至相应的下发位置，并最终在0.01秒内移动至追踪位置。

[0022] 本发明实施方式相对于现有技术而言，通过在追踪周期内采集力传感信息，并由此获取机器人多轴机构各轴关节的追踪位置，同时，根据获取的追踪位置在追踪周期内对多轴机构各轴关节进行轨迹规划，得到多轴机构各轴关节的各下发位置，这样，有利于在控制机器人的过程中提高各关节轴运动的精度，以及提高灵敏度。通过记录及下发各位置下发周期的下发位置至伺服控制系统，控制多轴机构各轴关节移动至各下发位置，这样，有利于在拖动机器人移动一次后，实现示教机器人的功能。

[0023] 本发明的第二实施方式涉及一种机器人控制方法。第二实施方式在第一实施方式的基础上作了进一步的改进，主要改进之处在于：本发明第二实施方式中，提出了一种更为具体的根据采集的力传感器信息获取机器人多轴机构各轴关节的追踪位置的方法。如图2所示，包括：

[0024] 步骤201：设置追踪周期。具体地说，终端可以预先设置追踪周期，其中，追踪周期为获取的力以及力矩值的更新周期。

[0025] 更具体地说，力传感器可以安装在机器人多轴机构的法兰工装末端上。终端可以预先设置追踪周期，并在追踪周期内采集力传感器信息，同时，对采集的力传感器信息进行滤波处理，从而，得到需要更新的力以及力矩值。

[0026] 步骤202：在追踪周期内采集力传感器信息，根据采集的力传感器信息获取多轴机构的法兰工装末端速度。具体地说，多轴机构的法兰工装末端上安装的力传感器可以为6维力传感器，终端通过对在追踪周期内采集的6维力传感器信息进行滤波处理，可以获取到需要更新的力以及力矩值，其中，上述需要更新的力以及力矩值为6维向量，可以包括X、Y、Z方向的平移力和A、B、C方向的扭矩。进一步，终端可以根据上述平移力、扭矩、预设的平移力比例关系以及扭矩比例关系，获取多轴机构的法兰工装末端速度，其中，上述多轴机构的法兰工装末端速度的表达式为：

[0027] 6v＝[kpx，kpy，kpz，kra，krb，krc]

[0028] 其中，x、y、z分别为多轴机构的法兰工装末端在X、Y、Z方向的平移力，a、b、c分别为多轴机构的法兰工装末端在A、B、C方向的扭矩，kp与kr分别为预设的平移力比例关系与扭矩比例关系。

[0029] 需要说明的是，本步骤中力传感器相对于多轴机构的法兰工装末端速度也可以是非正比关系，比如，多轴机构的法兰工装末端速度可以为力传感器所获力矩的分段函数。这样，在力传感器所获力矩较小时，可以获取更小的法兰工装末端速度，有利于提高精度。

[0030] 此外，本步骤中终端还可以根据采集的力传感器信息获取多轴机构的法兰工装末端加速度，并可以通过上述获取的加速度得到多轴机构的法兰工装末端速度。具体地说，终端通过对在追踪周期内采集的6维力传感器信息进行滤波处理，可以获取到需要更新的力以及力矩值，并且由上述力以及力矩值可以得到多轴机构的法兰工装末端加速度，进一步，可以根据上述加速度获取多轴机构的法兰工装末端速度。

[0031] 步骤203：在追踪周期内，获取多轴机构各轴关节的位置。具体地说，终端可以在追踪周期内，通过伺服控制系统获取多轴机构各轴关节当前的位置。

[0032] 步骤204：根据多轴机构各轴关节的位置，获取雅克比矩阵。具体地说，根据获取的多轴机构各轴关节当前的位置，可以获取雅可比矩阵，其中，多轴机构各轴关节的位置坐标可以表示为Θ，雅克比矩阵为与Θ相关的函数，可以表示为6J(Θ)。

[0033] 步骤205：根据多轴机构的法兰工装末端速度与雅克比矩阵，获取多轴机构各轴关节的追踪速度。具体地说，根据多轴机构的法兰工装末端速度6v与雅可比矩阵6J(Θ)，并依据多轴机构各轴关节的追踪速度的公式

[0034]

[0035] 通过求逆解，可以获取多轴机构各轴关节的追踪速度。其中，6v是多轴机构的法兰工装末端速度，6J(Θ)是雅可比矩阵， 是多轴机构各轴关节的追踪速度。

[0036] 步骤206：在追踪周期内，获取多轴机构各轴关节的速度。具体地说，终端可以在追踪周期内，通过伺服控制系统获取多轴机构各轴关节当前的速度。

[0037] 步骤207：根据多轴机构各轴关节的速度以及多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的加速度。具体地说，终端可以根据获取的多轴机构各关节轴当前的速度以及追踪速度，分别获取多轴机构各轴关节当前的角速度以及追踪角速度，进一步，终端可以根据多轴机构各轴关节当前的角速度、追踪角速度、追踪周期以及公式[0038]

[0039] 获取多轴机构各轴关节的角加速度。其中， 以及 分别为多轴机构各轴关节当前的角速度以及追踪角速度， 为多轴机构各轴关节的角加速度，T为追踪周期。

[0040] 步骤208：根据多轴机构各轴关节的速度、多轴机构各轴关节的加速度以及追踪速度，获取多轴机构各轴关节的追踪位置。具体地说，根据在追踪周期内获取的多轴机构各轴关节当前的速度、多轴机构各轴关节的加速度以及追踪速度，并依据相关的运动学公式，终端可以获取多轴机构各轴关节的追踪位置。

[0041] 步骤209：记录追踪周期与追踪位置，在追踪周期内对追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至追踪位置。具体地说，在获取多轴机构各关节轴的追踪位置后，终端可以记录上述追踪位置以及追踪周期，并且可以根据上述追踪位置以及预设的位置下发周期，在追踪周期内对多轴机构各轴关节进行轨迹规划，得到追踪周期内各位置下发周期的下发位置，同时，可以将各下发位置下发至伺服控制系统，控制多轴机构各轴关节在各位置下发周期内移动至各位置下发周期的下发位置，并最终在追踪周期内移动至追踪位置。同时，终端还可以记录各位置下发周期的下发位置，这样，机器人多轴机构各轴关节可以复现上述规划轨迹。其中，预设的位置下发周期可以小于或等于追踪周期。

[0042] 需要说明的是，在步骤205获取多轴机构各轴关节的追踪速度后，如果多轴机构各轴关节的追踪速度中，存在超出预设的限制关节速度的追踪速度，则对追踪速度超出限制关节速度的轴关节，进行速度修正，将限制关节速度作为修正后的追踪速度，并且其余各轴关节的追踪速度按相同比例降低。

[0043] 此外，在步骤207获取多轴机构各轴关节的加速度后，如果在多轴机构各轴关节的加速度中，存在超出预设的限制关节加速度的加速度，则对加速度超出限制关节加速度的轴关节，进行加速度修正，将限制关节加速度作为修正后的加速度，并且其余各轴关节的加速度按相同比例降低。

[0044] 本发明实施方式中，根据在追踪周期内采集的力传感器信息，可以获取多轴机构各轴关节的追踪速度与各轴关节的加速度，并由此可以获取到多轴机构各轴关节的追踪位置。这样，可以使得获取的多轴机构各轴关节的追踪位置更准确，且根据上述追踪位置在追踪周期内对多轴机构各轴关节进行轨迹规划，得到追踪周期内各位置下发周期的下发位置，使得拖动机器人多轴机构各轴关节移动的过程中，精度与灵敏度都有所提高。同时在获取的多轴机构各轴关节的追踪速度与各轴关节的加速度后，对超出限制关节速度与限制关节加速度的轴关节，进行修正，有利于在拖动机器人多轴机构各轴关节移动的过程中，有效的规避奇异位置。

[0045] 上面各种方法的步骤划分，只是为了描述清楚，实现时可以合并为一个步骤或者对某些步骤进行拆分，分解为多个步骤，只要包含相同的逻辑关系，都在本专利的保护范围内；对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计，但不改变其算法和流程的核心设计都在该专利的保护范围内。

[0046] 本发明第三实施方式涉及一种机器人控制装置，如图3所示，机器人控制装置300包括设置模块301、获取模块302、位置下发模块303以及追踪速度修正子模块304。

[0047] 设置模块301用于设置追踪周期。具体地说，设置模块301可以预先设置追踪周期，其中，追踪周期为获取的力以及力矩值的更新周期。

[0048] 获取模块302用于在追踪周期内采集力传感器信息，根据采集的力传感器信息获取机器人多轴机构各轴关节的追踪位置，其中，追踪位置为多轴机构各轴关节在下一追踪周期的位置。具体地说，多轴机构的法兰工装末端上可以安装6维力传感器，获取模块302可以在追踪周期内多次采集6维力传感器信息，同时，对采集的力传感器信息进行滤波处理，从而，得到需要更新的力以及力矩值。获取模块302在追踪周期内更新上述力以及力矩值后，可以根据更新后的力以及力矩值获取多轴机构的法兰工装末端的速度，进而，根据上述获取的法兰工装末端速度可以获取机器人多轴机构各轴关节的追踪位置。

[0049] 位置下发模块303用于记录追踪周期与追踪位置，在追踪周期内对追踪位置进行轨迹规划并下发至伺服控制系统，控制多轴机构各轴关节移动至追踪位置。具体地说，在获取模块302获取多轴机构各关节轴的追踪位置后，位置下发模块303可以记录上述追踪位置以及追踪周期，并且可以根据上述追踪位置以及预设的位置下发周期，在追踪周期内对多轴机构各轴关节进行轨迹规划，得到追踪周期内各位置下发周期的下发位置，同时，可以将各下发位置下发至伺服控制系统，控制多轴机构各轴关节在各位置下发周期内移动至各位置下发周期的下发位置，并最终在追踪周期内移动至追踪位置。同时，位置下发模块303还可以记录各位置下发周期的下发位置，这样，机器人多轴机构各轴关节可以复现上述规划轨迹。

[0050] 追踪速度修正子模块304用于当多轴机构各轴关节的追踪速度中，存在超出预设的限制关节速度的追踪速度时，对追踪速度超出限制关节速度的轴关节，进行速度修正，将限制关节速度作为修正后的追踪速度，并且其余各轴关节的追踪速度按相同比例降低。

[0051] 获取模块302还可以包括法兰工装末端速度获取子模块3021、追踪速度获取子模块3022以及追踪位置获取子模块3023。

[0052] 法兰工装末端速度获取子模块3021用于在追踪周期内采集力传感器信息，根据采集的力传感器信息获取多轴机构的法兰工装末端速度。具体地说，多轴机构的法兰工装末端上可以安装6维力传感器，法兰工装末端速度获取子模块3021通过对在追踪周期内采集的6维力传感器信息进行滤波处理，可以获取到需要更新的力以及力矩值，并且，可以根据上述力以及力矩值获取多轴机构的法兰工装末端速度。

[0053] 追踪速度获取子模块3022用于根据多轴机构的法兰工装末端速度，获取多轴机构各轴关节的追踪速度。具体地说，追踪速度获取子模块3022可以在追踪周期内，通过伺服控制系统获取多轴机构各轴关节当前的位置以及与上述位置相关的雅可比矩阵。进而，可以根据多轴机构的法兰工装末端速度与雅克比矩阵，获取多轴机构各轴关节的追踪速度。

[0054] 追踪位置获取子模块3023用于根据多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的追踪位置。具体地说，追踪速度获取子模块3022还可以在追踪周期内，通过伺服控制系统获取多轴机构各轴关节当前的速度。追踪位置获取子模块3023可以根据多轴机构各轴关节当前的速度以及多轴机构各轴关节的追踪速度，获取多轴机构各轴关节的加速度。进而，可以根据多轴机构各轴关节当前的速度、多轴机构各轴关节的加速度以及追踪速度，获取多轴机构各轴关节的追踪位置。

[0055] 本发明实施方式中，根据在追踪周期内采集的力传感器信息，可以获取多轴机构各轴关节的追踪速度与各轴关节的加速度，并由此可以获取到多轴机构各轴关节的追踪位置。这样，可以使得获取的多轴机构各轴关节的追踪位置更准确，且根据上述追踪位置在追踪周期内对多轴机构各轴关节进行轨迹规划，得到追踪周期内各位置下发周期的下发位置，使得拖动机器人多轴机构各轴关节移动的过程中，精度与灵敏度都有所提高。同时在获取的多轴机构各轴关节的追踪速度与各轴关节的加速度后，对超出限制关节速度与限制关节加速度的轴关节，进行修正，有利于在拖动机器人多轴机构各轴关节移动的过程中，有效的规避奇异位置。

[0056] 不难发现，本实施方式为与第二实施方式相对应的装置实施例，本实施方式可与第二实施方式互相配合实施。第二实施方式中提到的相关技术细节在本实施方式中依然有效，为了减少重复，这里不再赘述。相应地，本实施方式中提到的相关技术细节也可应用在第二实施方式中。

[0057] 值得一提的是，本实施方式中所涉及到的各模块均为逻辑模块，在实际应用中，一个逻辑单元可以是一个物理单元，也可以是一个物理单元的一部分，还可以以多个物理单元的组合实现。此外，为了突出本发明的创新部分，本实施方式中并没有将与解决本发明所提出的技术问题关系不太密切的单元引入，但这并不表明本实施方式中不存在其它的单元。

[0058] 本领域技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得一个设备(可以是单片机，芯片等)或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

[0059] 本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。