CN113954078B - 力控关节控制方法、装置、机器人和可读存储介质 - Google Patents

Abstract

本申请实施例提供一种力控关节控制方法、装置、机器人和可读存储介质，该方法包括：基于获取的上一时刻的力矩测量值和力控关节的标称动力学模型，通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值；根据当前时刻的期望力矩、力矩估计值、力矩微分估计值基于预设力矩控制器，获得当前时刻的电流控制量；根据所述标称动力学模型、上一时刻的力控关节电机的输入电流和力矩估计值，获得当前时刻的扰动补偿量；根据所述电流控制量和所述扰动补偿量计算得到当前时刻电机所需的输入电流，以控制力控关节执行相应操作。该方法可以在实现高精度力矩控制的情况下，还具有较好的***鲁棒性，可应用于各种交互场景等。

Description

力控关节控制方法、装置、机器人和可读存储介质

技术领域

本申请涉及机器人技术领域，尤其涉及一种力控关节控制方法、装置、机器人和可读存储介质。

背景技术

传统的机器人为了追求高速度和高精度，控制模式以位置控制为主，这样的机器人只能在结构化的环境中沿着事先规划好的轨迹运动，以满足特定的应用需求。然而随着机器人的应用场景不断拓展，单纯的位置控制已经不能满足对机器人的应用需求，特别是在刚性环境下，由于不可避免的建模误差和不确定性会导致接触力的突变，单纯的位置控制容易引发***的不稳定行为(如振荡等)。因此，机器人需要能准确检测接触力的大小并进行控制，以具备在非结构化环境中与外界交互的能力。

虽然部分工业场景中机器人仅需感知末端执行器的受力情况，例如工业机器人打磨、装配等，然而在另外一些场景中，需要机器人的全身都能够感知外力并进行关节力矩控制，例如机器人全身碰撞检测、人机物理交互等。此外，机器人的运动学模型只能部分反映机器人的实际运动状态，为了实现高性能运动控制，往往要基于机器人的动力学模型设计控制算法，此时需以关节力矩作为***的控制输入。目前应用最广泛的力矩测量方式就是在关节处安装一个基于电阻应变片式的力矩传感器，然而这种力矩测量方式其测量值往往伴随着大量噪声，不仅降低了力控精度，还会因为信号质量较差对控制参数的调节造成不利影响，进而出现超调过大、震荡等现象。

发明内容

本申请实施例提供一种力控关节控制方法，所述力控关节处设有力矩传感器，用于测量得到对应时刻的力矩测量值；所述方法包括：

基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型，通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值；

根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器，获得当前时刻的电流控制量；

根据所述标称动力学模型、上一时刻的所述力控关节电机的输入电流和所述力矩估计值，获得当前时刻的扰动补偿量；

根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机所需的输入电流，并根据所述输入电流控制所述力控关节进行运动。

在一些实施例中，所述力控关节的标称动力学模型通过预先构建得到，包括：

基于所述力矩传感器将所述力控关节解耦为电机端与负载端，分别建立所述电机端与所述负载端的动力学模型；

根据所述电机端与所述负载端的动力学模型构建在所述负载端固定时的标称动力学模型。

在一些实施例中，所述通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值，包括：

根据上一时刻的所述力控关节的电机输入电流、力矩预测值、力矩微分预测值利用第一状态方程，得到当前时刻的力矩预测值和力矩微分预测值，其中，所述第一状态方程通过所述标称动力学模型预先构建；

根据上一时刻的力矩估计值与力矩真实值之间的误差协方差及过程噪声计算当前时刻的力矩预测值与力矩真实值之间的误差协方差，并根据所述当前时刻的力矩预测值与力矩真实值之间的误差协方差、及上一时刻的测量噪声计算当前时刻的卡尔曼滤波器的增益；

根据当前时刻的所述力矩预测值、所述力矩微分预测值、所述卡尔曼滤波器的增益和上一时刻的所述力矩测量值利用第二状态方程，计算当前时刻的力矩估计值和力矩微分估计值，其中，所述第二状态方程通过***测量模型预先构建。

在一些实施例中，所述预设力矩控制器为PD-前馈控制器，所述获得当前时刻的电流控制量，包括：

根据所述力控关节在当前时刻的期望力矩得到期望力矩微分；

根据当前时刻的所述期望力矩与所述力矩估计值之间的偏差、所述期望力矩微分与所述力矩微分估计值之间的偏差通过所述PD-前馈控制器，得到当前时刻的电流控制量。

在一些实施例中，所述获得当前时刻的扰动补偿量，包括：

根据所述标称动力学模型的逆模型和所述力矩估计值通过预设低通滤波模型，得到第一扰动补偿分量；

根据上一时刻的所述力控关节电机的输入电流通过预设低通滤波模型，得到第二扰动补偿分量；

将所述第一扰动补偿分量和所述第二扰动补偿分量相加，得到扰动补偿量。

在一些实施例中，所述力控关节的标称动力学模型的表达式如下：

其中，τ_s＝K_sΔθ；

Δθ＝θ_m-θ_l；

其中，P_n(s)表示所述标称动力学模型，i(s)表示所述力控关节电机的输入电流，τ_s(s)表示所述力控关节的输出力矩，β表示所述电机的等效力矩系数，K_s表示所述力矩传感器的刚度，J_m表示电机端的转动惯量，B_m表示电机的阻尼项，θ_m表示电机的转动角度，θ_l表示负载的转动角度，Δθ表示所述力矩传感器建模的弹簧的形变量，s表示拉普拉斯变换中的复数变量。

在一些实施例中，所述PD-前馈控制器的表达式如下：

其中，k表示第k个时刻，i_PD(k)为所述PD-前馈控制器输出的电流控制量，τ_ref(k)为所述力控关节的期望力矩，

为所述期望力矩微分，K_P和K_D分别为所述PD-前馈控制器中的比例环节系数和微分环节系数，τ_k(k)为力矩估计值，

为力矩微分估计值，β表示所述电机的等效力矩系数。

本申请的实施例提供一种力控关节控制装置，所述力控关节处设有力矩传感器，用于测量得到对应时刻的力矩测量值；包括：

滤波模块，用于基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型，通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值；

计算模块，用于根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器，获得当前时刻的电流控制量；

补偿模块，用于根据所述标称动力学模型、上一时刻的所述力矩估计值和所述力控关节电机的输入电流，获得当前时刻的扰动补偿量；

控制模块，用于根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机的输入电流，并根据所述输入电流控制所述力控关节进行运动。

本申请的实施例还提供一种机器人，所述机器人包括力控关节、处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的力控关节控制方法，以控制所述力控关节进行运动。

本申请的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上执行时，实施上述的力控关节控制方法。

本申请的实施例具有如下有益效果：

本申请实施例的力控关节控制方法通过利用卡尔曼滤波获得的力矩估计值以及力矩微分的估计值，同时通过扰动观测来实现力控关节***中扰动的观测和补偿，来计算出需要输入至关节电机的电流，从而实现对力控关节的精准力控，该方法采用卡尔曼滤波来计算，得到的结果更加平滑，而进行扰动补偿不仅提高了***对外界扰动的鲁棒性，而且提高了卡尔曼滤波中所用模型的准确度，估计精度更高等。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本申请实施例的力控关节控制方法的第一流程图；

图2示出了本申请实施例的力控关节控制方法的***架构图；

图3示出了本申请实施例的力控关节控制方法的第二流程图；

图4示出了本申请实施例的力控关节控制方法的第三流程图；

图5示出了本申请实施例的力控关节控制装置的结构示意图。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

在下文中，可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本申请的各种实施例中被清楚地限定。

实施例1

图1所示为本实施例的力控关节控制方法的第一流程图。

本实施例中，该力控关节是指机器人的关节能够直接控制输出力或力矩。其中，该力控关节处设有一个力矩传感器，用于实时测量该力控关节在对应时刻的力矩，例如，该力矩传感器可以采用基于电阻应变片式的传感器等。应当理解，上述的时刻是指机器人的指令控制周期，一个任务的执行通常包括很多个指令控制周期。

本实施例通过将该力矩传感器建模为一个具有一定刚度的理想弹簧，考虑到关节处的力矩传感器的刚度明显小于该力控关节中其他部分的刚度，因此，本实施例可以对该力控关节进行电机端与负载端的解耦，然后分别基于电机端与负载端建立各自的动力学模型，进而利用该动力学模型来构建出该力控关节的标称动力学模型以用于后续控制。其中，这里的电机端是指用于驱动关节转动的电机侧，而负载端是指连接电机的用于与外界环境进行直接接触的部位。

示范性地，在一种实施方式中，可构建出如下的电机端的动力学模型：

其中，J_m表示电机端的转动惯量，B_m表示电机的阻尼项，θ_m、

和

分别表示电机的转动角度、转动速度和转动加速度，K_s表示力矩传感器的刚度，θ_l表示负载的转动角度，τ_dm表示电机端未建模存在的不确定因素，如库伦摩擦等，τ_m表示电机的输出力矩，其与输入至电机的电流i的关系为τ_m＝βi，β为电机端的等效力矩系数。

相应地，该负载端的动力学模型满足：

其中，J_l表示负载端的转动惯量，B_l表示负载的阻尼项，

和

分别表示负载的转动角速度和转动角加速度，τ_ext表示负载受到的来自外界环境的力矩。

进而，基于上述的电机端与负载端的动力学模型，为了方便分析电机侧的力矩控制，这里将负载端视为静止状态，然后建立在负载端固定时的标称动力学模型。在一种实施方式中，利用上述两个动力学模型可构建出如下的标称动力学模型：

其中，τ_s＝K_sΔθ；

Δθ＝θ_m-θ_l；

其中，P_n(s)表示标称动力学模型，i(s)表示该力控关节电机的输入电流，τ_s(s)表示该力控关节的输出力矩，Δθ表示上述力矩传感器所建模的理想弹簧的形变量，其为电机与负载之间的转动角度的差值；s表示拉普拉斯变换中的复数变量。

于是，在构建出该力控关节的标称动力学模型后，本实施例还将结合卡尔曼滤波器和扰动观测器来共同实现力矩控制，一方面，通过卡尔曼滤波所获得的该力控关节的力矩估计值以及力矩微分的估计值，可以使滤波后的结果更加平滑，而且相比于采用低通滤波的结果滞后更小；另一方面，利用扰动观测实现力控关节***中扰动的观测和补偿，不仅提高了***对外界扰动的鲁棒性，还提高了卡尔曼滤波中所用模型的准确度，估计精度更高。

为了便于进行力矩分析，本实施例通过构建出该力矩控制***的离散状态方程，然后基于卡尔曼滤波器进行***状态的最优估计，从而得到每个时刻的力矩控制相关量的最优估计。示范性地，该离散状态方程包括第一状态方程和第二状态方程，其中，第一状态方程可用于反映上一时刻与当前时刻的***状态之间的关系；第二状态方程为观测方程，用于反映***状态的测量值与估计值之间的关系。

在一种实施方式中，可通过上述标称动力学模型来构建得到该第一状态方程，以及通过当前的***测量模型来构建得到该第二状态方程，具体地，这两个状态方程的表达式如下：

x(k)＝Ax(k-1)+Bu(k-1)+w(k-1)；

y(k)＝Hx(k)+n(k)；

其中，x(k)是k时刻的***状态，在本实施例中，如图2所示，该***状态主要指电机输出的力矩值和力矩值的微分；u(k)是k时刻***的控制量，这里主要指用于驱动电机产生上述力矩的输入电流i_PD(k)；A和B是由标称动力学模型P_n(s)转换为离散形式时获得的***参数；w(k-1)表示k-1时刻的过程噪声，这里考虑了在数据传递等过程中可能会存在的一些噪声。y(k)是k时刻力矩的测量值，可由力矩传感器测量得到，其包含有测量噪声n(k)的力矩值；H是测量***的***参数矩阵，这里可设为矩阵H＝[1 0]。

可以理解，利用力矩传感器进行力矩测量时，可能存在因外界环境、信号采样及信号转换等各种因素的影响，导致最终得到的测量值会伴随着大量的噪声，故这里将测量噪声也考虑在内，可以进一步提高力控精度等。

基于上述的***状态方程，本实施例将通过卡尔曼滤波对上述的力矩及力矩微分进行最优估计。下面将结合力控关节控制方法的具体步骤来进行说明。在控制过程中，示范性地，该力控关节控制方法包括：

步骤S110，基于获取的上一时刻的力矩测量值和该力控关节的标称动力学模型，通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值。

如图3所示，在一种实施方式中，该步骤S110包括以下子步骤：

子步骤S210，根据上一时刻的力控关节的电机输入电流、力矩预测值、力矩微分预测值利用第一状态方程，得到当前时刻的力矩预测值和力矩微分预测值。

示范性地，可基于上述的第一状态方程和上一时刻的力控关节的电机输入电流、力矩预测值等进行当前时刻的力矩预测值和力矩微分预测值，具体可通过如下公式计算得到：

其中，

和

分别表示k和k-1时刻的力矩预测值，u(k-1)表示上一时刻的***控制量，也即电机输入电流i_PD(k-1)。

子步骤S220，根据上一时刻的力矩估计值与力矩真实值之间的误差协方差及过程噪声计算当前时刻的力矩预测值与力矩真实值之间的误差协方差，并根据当前时刻的力矩预测值与力矩真实值之间的误差协方差、及上一时刻的测量噪声计算卡尔曼滤波器的增益。

根据卡尔曼滤波器原理，可通过***状态的预测值与真实值之间的误差协方差、测量***的噪声等来计算卡尔曼滤波器的增益系数。示范性地，该卡尔曼滤波器的增益的计算公式如下：

K(k)＝P^-(k)H^T(HP^-(k)H^T+R)^-1；

其中，K(k)表示k时刻的卡尔曼滤波器的增益，P^-(k)表示k时刻的***状态的预测误差的协方差矩阵，即k时刻的***状态的预测值与真实值之间的误差协方差，R表示k时刻的测量噪声n(k)的协方差。

对于上述的预测误差的协方差矩阵P^-(k)，示范性地，其计算公式如下：

P^-(k)＝AP(k-1)A^T+Q；

其中，P(k-1)为k-1时刻的***状态的估计误差的协方差矩阵，也即k-1时刻的***状态的估计值与真实值之间的误差协方差；Q表示过程噪声w(k)的协方差。本实施例中，该预测误差的协方差包括力矩预测值与力矩真实值之间的误差协方差、以及力矩微分的预测值与真实值之间的误差协方差。

子步骤S230，根据当前时刻的力矩预测值、力矩微分预测值、卡尔曼滤波器的增益和上一时刻的力矩测量值利用第二状态方程，计算当前时刻的力矩估计值和力矩微分估计值。

示范性地，基于上述的第二状态方程，可根据***状态的预测值和测量值来计算出估计值，如下：

其中，

是卡尔曼滤波输出的估计值，在本实施例中，即有

进一步地，基于k时刻的估计值进行控制后，则可得到k时刻的***状态的估计值与真实值之间的误差协方差，也即估计误差的协方差矩阵P(k)，其中，P(k)又可用于计算下一时刻的***状态的估计值，然后重复上述过程。

示范性地，该估计误差的协方差矩阵P(k)的计算公式如下：

P(k)＝(I-K(k)H)P^-(k)。

可以理解，本实施例中，该估计误差的协方差包括力矩的估计值与真实值之间、以及力矩微分的估计值与真实值之间的误差协方差。

于是，基于上述步骤可以计算出当前时刻的力矩预测值和力矩微分预测值，进而根据该力矩预测值和力矩微分预测值来计算出电机所需的电流。

步骤S120，根据该力控关节在当前时刻的期望力矩、力矩估计值、力矩微分估计值基于预设力矩控制器，获得当前时刻的电流控制量。

其中，上述的力矩控制器用于计算需要输入至电机的电流，本实施例中采用了比例-微分控制(即PD控制)进行反馈控制，同时还加入了前馈量控制，以实现更好的控制效果，此时，由PD反馈控制和前馈控制两种共同构成的控制器可称为PD-前馈控制器。应当理解的是，该力矩控制器也可以采用其他的控制，如比例-积分控制(即PI控制)、比例-积分-微分控制(即PID控制)等，以及是否加入前馈量控制，具体可根据实际需求来选取，这里不作限定，相应地，与力矩相关的控制量也可作适应性调整。

对于上述PD-前馈控制器，在一种实施方式中，其表达式如下：

其中，k表示第k个时刻，i_PD(k)为该PD-前馈控制器输出的电流控制量，τ_ref(k)为该力控关节的期望力矩，

为期望力矩的微分，K_P和K_D分别为该PD-前馈控制器中的比例环节系数和微分环节系数，τ_k(k)为力矩估计值，

为力矩微分估计值。

示范性地，以上述的PD-前馈控制器为例，如图所示，对于该当前时刻的电流控制量，可通过先根据该力控关节在当前时刻的期望力矩τ_ref得到期望力矩微分

然后，根据当前时刻的期望力矩τ_ref与力矩估计值τ_k之间的偏差、以及该期望力矩微分

与力矩微分估计值

之间的偏差代入该PD-前馈控制器中，则可计算得到当前时刻的电流控制量i_PD。

步骤S 130，根据该标称动力学模型、上一时刻的力控关节电机的输入电流和力矩估计值，获得当前时刻的扰动补偿量。

本实施例通过将利用上一时刻的***状态来对当前时刻进行扰动补偿，这里主要通过一个低通滤波器Q来实现，如图2所示。例如，在一种实施方式中，可采用如下的低通滤波器的模型：

其中，Q(s)表示低通滤波器模型，ω_q为该低通滤波器的截止频率。

示范性地，如图4所示，上述步骤S130可包括如下子步骤：

子步骤S310，根据所述标称动力学模型的逆模型和所述力矩估计值通过预设低通滤波模型，得到第一扰动补偿分量。

子步骤S320，根据上一时刻的所述力控关节电机的输入电流通过预设低通滤波模型，得到第二扰动补偿分量。

子步骤S330，将所述第一扰动补偿分量和所述第二扰动补偿分量相加，得到扰动补偿量。

本实施例中，该扰动补偿量主要包括两部分，分别是关于输入至电机的电流的扰动补偿，即上述的第一扰动补偿分量，以及标称动力学模型与力矩估计值的扰动补偿，即上述的第二扰动补偿分量。

示范性地，对于上述扰动补偿量的计算过程，若采用表达式表示，则有：

其中，

表示扰动补偿量；i(s)为总电流值，这里主要采用的是上一时刻的力控关节电机的输入电流；

为标称模型P_n(s)的逆模型，只需将上式转换为离散形式，代入已知量，即可获得当前时刻的扰动补偿值

步骤S140，根据该电流控制量和该扰动补偿量计算得到当前时刻力控关节电机的输入电流，以控制该力控关节执行相应操作。

示范性地，通过将由预设控制器输出的电流控制量i_PD(k)与上述计算得到的当前时刻的扰动补偿量

进行相减，即可得到该当前时刻的需要输入至关节电机的总电流i(k)，具体计算过程如下：

最后，将该总电流输入至该力控关节的电机以驱动电机转动，以控制该力控关节进行相应操作。进而根据当前任务需求，进入下一控制周期，重复上述步骤S110，直至任务完成。

本实施例的力控关节控制方法通过利用卡尔曼滤波获得的力矩估计值以及力矩微分的估计值，不仅使滤波后的结果更加平滑，而且相比于低通滤波的结果滞后更小；同时利用扰动观测器来实现力控关节***中扰动的观测和补偿，不仅提高了***对外界扰动的鲁棒性，而且提高了卡尔曼滤波中所用模型的准确度，估计精度更高。应当理解，本实施例中的扰动观测和卡尔曼滤波均位于力矩控制内环，可以实现与其他控制环进行解耦，使得外环的控制算法可在其基础上实现更好的控制效果。

实施例2

请参照图5，基于上述实施例1的方法，本实施例提出一种力控关节控制装置100，所述力控关节处设有力矩传感器，用于测量得到对应时刻的力矩测量值。示范性地，该力控关节控制装置100包括：

滤波模块110，用于基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型，通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值；

计算模块120，用于根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器，获得当前时刻的电流控制量；

补偿模块130，用于根据所述标称动力学模型、上一时刻的所述力矩估计值和所述力控关节电机的输入电流，获得当前时刻的扰动补偿量；

计算模块120还用于根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机的输入电流；

控制模块140，用于根据所述输入电流控制所述力控关节进行运动。

在一种实施方式中，上述的滤波模块110包括：方程建立子模块111、预测值计算子模块112、真实值计算子模块113、增益计算子模块114和估计值计算子模块115(未示出)，具体地，方程建立子模块111用于通过所述标称动力学模型构建得到第一状态方程，以及通过***测量模型构建第二状态方程。预测值计算子模块112用于根据上一时刻的所述力控关节的电机输入电流、力矩估计值、力矩微分估计值利用第一状态方程，得到当前时刻的力矩预测值和力矩微分预测值。真实值计算子模块113用于根据上一时刻的力矩测量值和测量噪声利用第二状态方程，得到上一时刻的力矩真实值。增益计算子模块114用于根据上一时刻的力矩预测值与力矩真实值之间的误差计算卡尔曼滤波增益。估计值计算子模块115用于根据当前时刻的所述力矩预测值、所述力矩微分预测值、上一时刻的所述力矩测量值和所述卡尔曼滤波增益计算当前时刻的力矩估计值和力矩微分估计值。

在一种实施方式中，补偿模块130包括分量计算子模块131和分量相加子模块132，具体地，分量计算子模块131用于根据所述标称动力学模型的逆模型和所述力矩估计值通过预设低通滤波模型，得到第一扰动补偿分量；以及，用于根据上一时刻的所述力控关节电机的输入电流通过预设低通滤波模型，得到第二扰动补偿分量。分量相加子模块132用于将所述第一扰动补偿分量和所述第二扰动补偿分量相加，得到扰动补偿量。

可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。

本申请还提供了一种机器人，示范性地，该机器人包括力控关节、处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使机器人执行上述的力控关节控制方法或者上述力控关节控制装置中的各个模块的功能，以控制该力控关节执行相应操作。

本申请还提供了一种可读存储介质，用于储存上述终端设备中使用的所述计算机程序。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

Claims (10)

1.一种力控关节控制方法，其特征在于，所述力控关节处设有力矩传感器，用于测量得到对应时刻的力矩测量值；所述方法包括：

基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型，通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值；

根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器，获得当前时刻的电流控制量；

根据所述标称动力学模型、上一时刻的所述力控关节电机的输入电流和所述力矩估计值，获得当前时刻的扰动补偿量；

根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机所需的输入电流，以控制所述力控关节执行相应操作。

2.根据权利要求1所述的力控关节控制方法，其特征在于，所述力控关节的标称动力学模型通过预先构建得到，包括：

基于所述力矩传感器将所述力控关节解耦为电机端与负载端，分别建立所述电机端与所述负载端的动力学模型；

根据所述电机端与所述负载端的动力学模型构建在所述负载端固定时的标称动力学模型。

3.根据权利要求1所述的力控关节控制方法，其特征在于，所述通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值，包括：

根据上一时刻的所述力控关节的电机输入电流、力矩预测值、力矩微分预测值利用第一状态方程，得到当前时刻的力矩预测值和力矩微分预测值，其中，所述第一状态方程通过所述标称动力学模型预先构建；

根据上一时刻的力矩估计值与力矩真实值之间的误差协方差及过程噪声计算当前时刻的力矩预测值与力矩真实值之间的误差协方差，并根据所述当前时刻的力矩预测值与力矩真实值之间的误差协方差、及上一时刻的测量噪声计算卡尔曼滤波器的增益；

根据当前时刻的所述力矩预测值、所述力矩微分预测值、所述卡尔曼滤波器的增益和上一时刻的所述力矩测量值利用第二状态方程，计算当前时刻的力矩估计值和力矩微分估计值，其中，所述第二状态方程通过***测量模型预先构建。

4.根据权利要求1所述的力控关节控制方法，其特征在于，所述预设力矩控制器为PD-前馈控制器，所述获得当前时刻的电流控制量，包括：

根据所述力控关节在当前时刻的期望力矩得到期望力矩微分；

根据当前时刻的所述期望力矩与所述力矩估计值之间的偏差、所述期望力矩微分与所述力矩微分估计值之间的偏差通过所述PD-前馈控制器，得到当前时刻的电流控制量。

5.根据权利要求1所述的力控关节控制方法，其特征在于，所述获得当前时刻的扰动补偿量，包括：

根据所述标称动力学模型的逆模型和所述力矩估计值通过预设低通滤波模型，得到第一扰动补偿分量；

根据上一时刻的所述力控关节电机的输入电流通过预设低通滤波模型，得到第二扰动补偿分量；

将所述第一扰动补偿分量和所述第二扰动补偿分量相加，得到扰动补偿量。

6.根据权利要求2所述的力控关节控制方法，其特征在于，所述力控关节的标称动力学模型的表达式如下：

其中，τ_s＝K_sΔθ；

Δθ＝θ_m-θ_l；

其中，P_n(s)表示所述标称动力学模型，i(s)表示所述力控关节电机的输入电流，τ_s(s)表示所述力控关节的输出力矩，β表示所述电机的等效力矩系数，K_s表示所述力矩传感器的刚度，J_m表示电机端的转动惯量，B_m表示电机的阻尼项，θ_m表示电机的转动角度，θ_l表示负载的转动角度，Δθ表示所述力矩传感器建模的弹簧的形变量，s表示拉普拉斯变换中的复数变量。

7.根据权利要求4所述的力控关节控制方法，其特征在于，所述PD-前馈控制器的表达式如下：

其中，k表示第k个时刻，i_PD(k)为所述PD-前馈控制器输出的电流控制量，τ_ref(k)为所述力控关节的期望力矩，

为所述期望力矩微分，K_P和K_D分别为所述PD-前馈控制器中的比例环节系数和微分环节系数，τ_k(k)为力矩估计值，

为力矩微分估计值，β表示所述电机的等效力矩系数。

8.一种力控关节控制装置，其特征在于，所述力控关节处设有力矩传感器，用于测量得到对应时刻的力矩测量值；包括：

滤波模块，用于基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型，通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值；

计算模块，用于根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器，获得当前时刻的电流控制量；

补偿模块，用于根据所述标称动力学模型、上一时刻的所述力矩估计值和所述力控关节电机的输入电流，获得当前时刻的扰动补偿量；

控制模块，用于根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机的输入电流，并根据所述输入电流控制所述力控关节进行运动。

9.一种机器人，其特征在于，所述机器人包括力控关节、处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-7中任一项所述的力控关节控制方法，以控制所述力控关节进行运动。

10.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上执行时，实施根据权利要求1-7中任一项所述的力控关节控制方法。

Images