一种机器人及其力跟踪误差的控制方法及系统与流程

本发明涉及机器人领域，特别是涉及一种机器人及其力跟踪误差的控制方法及系统。

背景技术：

旋提手法作为辅助、替代药物治疗颈椎病的有效方式被广泛应中医临床治疗。但是该类手法受到操作不规范，临床经验匮乏，医师手法水平参差不齐的影响，容易对患者产生组织拉伤等二次伤害，因此，设计出专用于旋提手法培训的机器人，对旋提手法的每个阶段进行评估。对机器人往往通过阻抗控制方式进行控制，但是阻抗控制是通过调整参考位置间接地实现力控制的，阻抗控制的控制精度依赖于操作者对环境知识的精确了解，在实际应用中，由于各种因素的影响，由于环境刚度及环境位置未知都会导致机器人存在很大的阻抗自适应力的力跟踪误差，从而导致机器人针对旋提手法培训时模拟颈椎转动的拟合度低。

技术实现要素：

本发明的目的是提供一种机器人及其力跟踪误差的控制方法及系统，以解决由于环境刚度及环境位置导致力跟踪误差大，机器人的拟合度低的问题。

为实现上述目的，本发明提供了如下方案：

一种机器人，包括：电机、力矩传感器、中央处理器，电机驱动器；

所述中央处理器与所述电机驱动器电连接，所述电机驱动器与所述电机电连接；

所述中央处理器向所述电机驱动器发送一个转动指令，所述电机驱动器按照所述转动指令驱动所述电机，使得所述机器人的颈部转动；所述电机与所述力矩传感器同轴相连；所述力矩传感器用于接收所述机器人所受的外力并传送到所述中央处理器；

所述中央处理器与所述力矩传感器电连接；所述中央处理器用于对所述力矩传感器接收到的所述机器人所受的外力进行处理，并根据处理结果对电机的转动进行控制。

一种机器人的力跟踪误差的控制方法，包括：

设定期望跟踪力；

获取实际接触力；所述实际接触力为处于关节处的力矩传感器接收的关节所受的外力；

根据所述期望跟踪力和所述实际接触力确定所述力跟踪误差；

对所述力跟踪误差进行处理，得到中间量；所述中间量包括第一中间量和第二中间量；

确定所述电机期望转动角度；

在力跟踪过程中，根据所述中间量、所述力跟踪误差以及所述机器人的特性确定电机转动轨迹修正值；所述机器人的特性包括惯量、阻尼系数以及刚度；其中，所述刚度是根据所述中间量计算得到的；

根据所述电机期望转动角度和所述电机转动轨迹修正值，确定实际输入转动角度；

将所述实际输入转动角度输入所述机器人。

可选的，所述对所述力跟踪误差进行处理，得到中间量，具体包括：

根据公式对所述力跟踪误差进行处理，得到第一中间量；

对所述第一中间量进行比例-积分-微分运算，得到第二中间量；所述第二中间量为

其中，所述ef为第一中间量，所述为所述第一中间量的一阶导数，所述kp，ki，kd分别为所述第二中间量的比例，积分和微分增益。

可选的，所述确定所述电机期望转动角度，具体包括：

设定电机期望转动轨迹的初始值和更新率；

根据所述初始值、更新率以及所述积分值确定所述电机期望转动角度。

可选的，所述在力跟踪过程中，根据所述中间量、所述力跟踪误差以及所述机器人的特性确定电机转动轨迹修正值，具体包括：

根据公式确定电机转动轨迹修正值；其中，m为所述机器人的惯量，b为所述机器人的阻尼系数，k(t)为所述机器人的刚度xdc为所述电机转动轨迹修正值，为所述电机转动轨迹修正值的一阶导数，为所述电机转动轨迹修正值的二阶导数，ef为所述力跟踪误差；所述刚度可根据公式来计算，其中，kp，ki，kd分别为所述第二中间量的比例，积分和微分增益，k0为机器人刚度的初始值，为第一中间量的一阶导数。

一种机器人的力跟踪误差的控制系统，包括：

跟踪力设定模块，用于设定期望跟踪力；

接触力获取模块，用于获取实际接触力；所述实际接触力为处于关节处的力矩传感器接收的关节所受的外力；

力跟踪误差确定模块，用于根据所述期望跟踪力和所述实际接触力确定所述力跟踪误差；

处理模块，用于对所述力跟踪误差进行处理，得到中间量；所述中间量包括第一中间量和第二中间量；

期望转动角度获取模块，用于确定所述电机期望转动角度；

转动轨迹修正值模块，用于在力跟踪过程中，根据所述中间量、所述力跟踪误差以及所述机器人的特性确定电机转动轨迹修正值；所述机器人的特性包括惯量、阻尼系数以及刚度；其中，所述刚度是根据所述中间量计算得到的；

输入转动角度确定模块，用于根据所述电机期望转动角度和所述电机转动轨迹修正值，确定实际输入转动角度；

输入模块，将所述实际输入转动角度输入所述机器人。

可选的，所述处理模块，具体包括：

第一中间量计算单元，用于根据公式对所述力跟踪误差进行处理，得到第一中间量；

第二中间量计算单元，用于对所述第一中间量进行比例-积分-微分运算，得到第二中间量；所述第二中间量为

其中，所述ef为第一中间量，所述为所述第一中间量的一阶导数，所述kp，ki，kd分别为所述第二中间量的比例，积分和微分增益。

可选的，所述期望转动角度获取模块，具体包括：

初始值设定子单元，用于设定电机期望转动轨迹的初始值和更新率；

期望转动角度获取子单元，用于根据所述初始值、更新率以及所述积分值确定所述电机期望转动角度。

可选的，所述转动轨迹修正值模块，具体包括：

转动轨迹修正值子单元，用于根据公式确定电机转动轨迹修正值；其中，m为所述机器人的惯量，b为所述机器人的阻尼系数，k(t)为所述机器人的刚度xdc为所述电机转动轨迹修正值，为所述电机转动轨迹修正值的一阶导数，为所述电机转动轨迹修正值的二阶导数，ef为所述力跟踪误差；所述刚度可根据公式来计算，其中，kp，ki，kd分别为所述第二中间量的比例，积分和微分增益，k0为机器人刚度的初始值，为第一中间量的一阶导数。

根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明通过力矩传感器接收机器人所受外力(即接触力)，并设定期望跟踪力，根据接触力以及期望跟踪力计算力跟踪误差，通过力跟踪误差确定电机转动轨迹修正值，实时调节电机转动轨迹修正值直至力跟踪误差为0，在此控制过程中，本发明仅仅涉及到了机器人本身的特性，并不需要考虑环境刚度及环境位置，因此，避免了由于环境刚度及环境位置导致力跟踪误差大的问题，消除力跟踪误差，提高机器人用在旋提手法培训时颈椎转动的拟合度，以模拟旋提手法下人体颈椎生物力学特性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的机器人的力跟踪误差的控制方法流程图；

图2为本发明实施例的机器人的自适应阻抗控制结构图；

图3为本发明实施例的机器人的力跟踪误差的控制系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种机器人及其力跟踪误差的控制方法及系统，能够解决由于环境刚度及环境位置导致力跟踪误差大的问题。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

一种机器人，包括：电机、力矩传感器、中央处理器，电机驱动器；

所述中央处理器与所述电机驱动器电连接，所述电机驱动器与所述电机电连接；

所述中央处理器向所述电机驱动器发送一个转动指令，所述电机驱动器按照所述转动指令驱动所述电机，使得所述机器人的颈部转动；所述电机与所述力矩传感器同轴相连；所述力矩传感器用于接收所述机器人所受的外力并传送到所述中央处理器；

所述中央处理器与所述电机电连接；

所述中央处理器与所述力矩传感器电连接；所述中央处理器用于对所述力矩传感器接收到的所述机器人所受的外力进行处理，并根据处理结果对电机的转动进行控制。

图1为本发明实施例的机器人的力跟踪误差的控制方法流程图，如图1所示，一种机器人的力跟踪误差的控制方法，包括：

步骤101：设定期望跟踪力；

步骤102：获取实际接触力；所述实际接触力为处于关节处的力矩传感器接收的关节所受的外力；

步骤103：根据所述期望跟踪力和所述实际接触力确定所述力跟踪误差；用公式ef＝fref-f计算所述力跟踪误差，其中，所述fref为所述期望跟踪力，所述f为所述实际接触力；

步骤104：对所述力跟踪误差进行处理，得到中间量；所述中间量包括第一中间量和第二中间量；根据公式对所述力跟踪误差进行处理，得到第一中间量ef；对所述第一中间量进行比例-积分-微分运算，得到第二中间量；所述第二中间量为

其中，所述ef为第一中间量，所述为所述第一中间量的一阶导数，所述kp，ki，kd分别为所述第二中间量的比例，积分和微分增益。

步骤105：确定所述电机期望转动角度；设定电机期望转动轨迹的初始值和更新率；根据所述初始值、所述更新率以及所述积分值确定所述电机期望转动角度；

步骤106：在力跟踪过程中，根据所述中间量、所述力跟踪误差以及所述机器人的特性确定电机转动轨迹修正值；所述机器人的特性包括惯量、阻尼系数以及刚度；其中，所述刚度是根据所述中间量计算得到的；根据公式确定电机转动轨迹修正值；其中，m为所述机器人的惯量，b为所述机器人的阻尼系数，k(t)为所述机器人的刚度xdc为所述电机转动轨迹修正值，为所述电机转动轨迹修正值的一阶导数，为所述电机转动轨迹修正值的二阶导数，ef为所述力跟踪误差；所述刚度可根据公式来计算，其中，kp，ki，kd分别为比例，积分和微分增益，k0为机器人刚度的初始值，为第一中间量的一阶导数；

步骤107：根据所述电机期望转动角度和所述电机转动轨迹修正值，确定实际输入转动角度；其中，所述电机期望转动角度根据公式计算；所述xd为所述电机期望转动角度，所述η为误差积分的正增益，为期望轨迹的初始值；用xc表示实际输入转动角度，根据公式xc＝xd-xdc计算得到实际输入转动角度；

步骤108：将所述实际输入转动角度输入所述机器人，即将实际输入转动角度输入到电机位置环中，实现自适应阻抗控制，图2为本发明实施例的机器人的自适应阻抗控制结构图，如图2所示，通过对机器人的电机转动轨迹修正值、机器人的刚度等参数的调节得到实际输入电机转动角度并输入到图2的位置环内，实现实际输入电机转动角度与机器人实际转动角度在预设的误差范围内。

采用本发明实施例中机器人的力跟踪误差的控制方法，根据电机期望转动角度和电机转动轨迹修正值，使得电机转动受到力跟踪误差的影响，调整电机转动，最终实现力跟踪误差为0，使得机器人能够更精确的模拟出人在接受旋提手法治疗时的颈椎转动过程，并且不受环境刚度及环境位置影响。

对本发明的控制方法中不受环境刚度及环境位置影响并且力跟踪误差为0的稳定性证明：稳定性证明：

由于并且初始参考位置为常数，那么xdc的一阶和二阶导数为：

将上述方程带入化简得到

假设接触力用f＝ke(x-xe)来表示，其中x和xe分别表示末端执行器输出的实际位置和未知环境的位置，ke表示未知环境的刚度。考虑到位置跟踪误差的存在，位置控制的输入xc和输出x存在xc＝x+δx的关系；其中，δx表示为实际跟踪误差。因此，力跟踪误差可以表示为：

ef＝fref-f＝fref-ke(x-xe)，则得到末端实际位置为：

不失一般性，环境位置和环境刚度为未知且环境位置xe是可变的，所以且由上述论述中所提到的xc与x的关系，则可以得到如下式子：

和

将上述方程带入到

中可以得到

假设位置跟踪误差的一阶和二阶值相对环境位置来说非常小，重新定义新的变量新的变量能处理变化的环境位置和位置跟踪误差。将带入上式中可得：

对上式进行拉普拉斯变换，其中，令l{fref}＝fref(s)，由于ef是ef的微分，经过拉氏变换后的方程变为：

化简得到力跟踪误差的表达式为：

其中，

由此可知，当目标跟踪力为阶跃力或者斜坡力时，力跟踪的稳态误差为：

令a0＝m，a1＝b+mηke，a2＝bηke+kdke-ke+k0，a3＝k0keη+kpke和a4＝keki，为了方便计算，令a0＝m＝1由劳斯判据可以得到如下式子：

根据劳斯判据，a1＞0,a4＞0,且由于比例积分微分增益的值都为正，且目标惯量、阻尼以及刚度的值为正，而且轨迹修正增益η也为正，所以a1＞0且a4＞0，只需保证和即可保证系统稳定性。

由此可知，只要本发明满足上述限定条件，本发明实施例中的机器人通过本发明实施例中力跟踪误差控制方法以使得力跟踪误差为0，从而提高机器人的拟合度。

图3为本发明实施例的机器人的力跟踪误差的控制系统结构图，如图3所示，一种机器人的力跟踪误差的控制系统，包括：

跟踪力设定模块301，用于设定期望跟踪力；

接触力获取模块302，用于获取实际接触力；所述实际接触力为处于关节处的力矩传感器接收的关节所受的外力；

力跟踪误差确定模块303，用于根据所述期望跟踪力和所述实际接触力确定所述力跟踪误差；

处理模块304，用于对所述力跟踪误差进行处理，得到中间量；所述中间量包括第一中间量和第二中间量；

期望转动角度获取模块305，用于确定所述电机期望转动角度；

转动轨迹修正值模块306，用于在力跟踪过程中，根据所述中间量、所述力跟踪误差以及所述机器人的特性确定电机转动轨迹修正值；所述机器人的特性包括惯量、阻尼系数以及刚度；其中，所述刚度是根据所述中间量计算得到的；

输入转动角度确定模块307，用于根据所述电机期望转动角度和所述电机转动轨迹修正值，确定实际输入转动角度；

输入模块308，用于将所述实际输入转动角度输入所述机器人。

在实际应用中，所述处理模块，具体包括：第一中间量计算单元，用于根据公式对所述力跟踪误差进行处理，得到第一中间量；第二中间量计算单元，用于对所述第一中间量进行比例-积分-微分运算，得到第二中间量；所述第二中间量为

在实际应用中，所述期望转动角度获取模块，具体包括：

初始值设定子单元，用于设定电机期望转动轨迹的初始值和更新率；

期望转动角度获取子单元，用于根据所述初始值、更新率以及所述积分值确定所述电机期望转动角度。

在实际应用中，所述转动轨迹修正值模块，具体包括：

转动轨迹修正值子单元，用于根据公式确定电机转动轨迹修正值；其中，m为所述机器人的惯量，b为所述机器人的阻尼系数，k(t)为所述机器人的刚度xdc为所述电机转动轨迹修正值，为所述电机转动轨迹修正值的一阶导数，为所述电机转动轨迹修正值的二阶导数，ef为所述力跟踪误差。

采用本发明的控制系统，利用新的控制策略可以对斜坡变力和恒力进行快速无差跟踪，能够应用到旋提培训机器人模拟头部生物力学特性，提高模拟准确度；能够直接忽略未知环境刚度、环境位置的不确定性，可以实现斜坡或者恒力的无差跟踪，对于环境刚度和位置的变化具有较强的鲁棒性。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。