CN113954078B - 力控关节控制方法、装置、机器人和可读存储介质 - Google Patents
力控关节控制方法、装置、机器人和可读存储介质 Download PDFInfo
- Publication number
- CN113954078B CN113954078B CN202111360702.7A CN202111360702A CN113954078B CN 113954078 B CN113954078 B CN 113954078B CN 202111360702 A CN202111360702 A CN 202111360702A CN 113954078 B CN113954078 B CN 113954078B
- Authority
- CN
- China
- Prior art keywords
- moment
- current
- joint
- force
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000001914 filtration Methods 0.000 claims abstract description 31
- 238000005259 measurement Methods 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 10
- 230000014509 gene expression Effects 0.000 claims description 6
- 238000013016 damping Methods 0.000 claims description 4
- 230000002452 interceptive effect Effects 0.000 abstract 1
- 239000011159 matrix material Substances 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 101000802640 Homo sapiens Lactosylceramide 4-alpha-galactosyltransferase Proteins 0.000 description 1
- 102100035838 Lactosylceramide 4-alpha-galactosyltransferase Human genes 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000010399 physical interaction Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000035939 shock Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/087—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices for sensing other physical parameters, e.g. electrical or chemical properties
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J17/00—Joints
- B25J17/02—Wrist joints
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Feedback Control In General (AREA)
- Manipulator (AREA)
Abstract
本申请实施例提供一种力控关节控制方法、装置、机器人和可读存储介质,该方法包括:基于获取的上一时刻的力矩测量值和力控关节的标称动力学模型,通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值;根据当前时刻的期望力矩、力矩估计值、力矩微分估计值基于预设力矩控制器,获得当前时刻的电流控制量;根据所述标称动力学模型、上一时刻的力控关节电机的输入电流和力矩估计值,获得当前时刻的扰动补偿量;根据所述电流控制量和所述扰动补偿量计算得到当前时刻电机所需的输入电流,以控制力控关节执行相应操作。该方法可以在实现高精度力矩控制的情况下,还具有较好的***鲁棒性,可应用于各种交互场景等。
Description
技术领域
本申请涉及机器人技术领域,尤其涉及一种力控关节控制方法、装置、机器人和可读存储介质。
背景技术
传统的机器人为了追求高速度和高精度,控制模式以位置控制为主,这样的机器人只能在结构化的环境中沿着事先规划好的轨迹运动,以满足特定的应用需求。然而随着机器人的应用场景不断拓展,单纯的位置控制已经不能满足对机器人的应用需求,特别是在刚性环境下,由于不可避免的建模误差和不确定性会导致接触力的突变,单纯的位置控制容易引发***的不稳定行为(如振荡等)。因此,机器人需要能准确检测接触力的大小并进行控制,以具备在非结构化环境中与外界交互的能力。
虽然部分工业场景中机器人仅需感知末端执行器的受力情况,例如工业机器人打磨、装配等,然而在另外一些场景中,需要机器人的全身都能够感知外力并进行关节力矩控制,例如机器人全身碰撞检测、人机物理交互等。此外,机器人的运动学模型只能部分反映机器人的实际运动状态,为了实现高性能运动控制,往往要基于机器人的动力学模型设计控制算法,此时需以关节力矩作为***的控制输入。目前应用最广泛的力矩测量方式就是在关节处安装一个基于电阻应变片式的力矩传感器,然而这种力矩测量方式其测量值往往伴随着大量噪声,不仅降低了力控精度,还会因为信号质量较差对控制参数的调节造成不利影响,进而出现超调过大、震荡等现象。
发明内容
本申请实施例提供一种力控关节控制方法,所述力控关节处设有力矩传感器,用于测量得到对应时刻的力矩测量值;所述方法包括:
基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型,通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值;
根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器,获得当前时刻的电流控制量;
根据所述标称动力学模型、上一时刻的所述力控关节电机的输入电流和所述力矩估计值,获得当前时刻的扰动补偿量;
根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机所需的输入电流,并根据所述输入电流控制所述力控关节进行运动。
在一些实施例中,所述力控关节的标称动力学模型通过预先构建得到,包括:
基于所述力矩传感器将所述力控关节解耦为电机端与负载端,分别建立所述电机端与所述负载端的动力学模型;
根据所述电机端与所述负载端的动力学模型构建在所述负载端固定时的标称动力学模型。
在一些实施例中,所述通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值,包括:
根据上一时刻的所述力控关节的电机输入电流、力矩预测值、力矩微分预测值利用第一状态方程,得到当前时刻的力矩预测值和力矩微分预测值,其中,所述第一状态方程通过所述标称动力学模型预先构建;
根据上一时刻的力矩估计值与力矩真实值之间的误差协方差及过程噪声计算当前时刻的力矩预测值与力矩真实值之间的误差协方差,并根据所述当前时刻的力矩预测值与力矩真实值之间的误差协方差、及上一时刻的测量噪声计算当前时刻的卡尔曼滤波器的增益;
根据当前时刻的所述力矩预测值、所述力矩微分预测值、所述卡尔曼滤波器的增益和上一时刻的所述力矩测量值利用第二状态方程,计算当前时刻的力矩估计值和力矩微分估计值,其中,所述第二状态方程通过***测量模型预先构建。
在一些实施例中,所述预设力矩控制器为PD-前馈控制器,所述获得当前时刻的电流控制量,包括:
根据所述力控关节在当前时刻的期望力矩得到期望力矩微分;
根据当前时刻的所述期望力矩与所述力矩估计值之间的偏差、所述期望力矩微分与所述力矩微分估计值之间的偏差通过所述PD-前馈控制器,得到当前时刻的电流控制量。
在一些实施例中,所述获得当前时刻的扰动补偿量,包括:
根据所述标称动力学模型的逆模型和所述力矩估计值通过预设低通滤波模型,得到第一扰动补偿分量;
根据上一时刻的所述力控关节电机的输入电流通过预设低通滤波模型,得到第二扰动补偿分量;
将所述第一扰动补偿分量和所述第二扰动补偿分量相加,得到扰动补偿量。
在一些实施例中,所述力控关节的标称动力学模型的表达式如下:
其中,τs=KsΔθ;
Δθ=θm-θl;
其中,Pn(s)表示所述标称动力学模型,i(s)表示所述力控关节电机的输入电流,τs(s)表示所述力控关节的输出力矩,β表示所述电机的等效力矩系数,Ks表示所述力矩传感器的刚度,Jm表示电机端的转动惯量,Bm表示电机的阻尼项,θm表示电机的转动角度,θl表示负载的转动角度,Δθ表示所述力矩传感器建模的弹簧的形变量,s表示拉普拉斯变换中的复数变量。
在一些实施例中,所述PD-前馈控制器的表达式如下:
其中,k表示第k个时刻,iPD(k)为所述PD-前馈控制器输出的电流控制量,τref(k)为所述力控关节的期望力矩,为所述期望力矩微分,KP和KD分别为所述PD-前馈控制器中的比例环节系数和微分环节系数,τk(k)为力矩估计值,为力矩微分估计值,β表示所述电机的等效力矩系数。
本申请的实施例提供一种力控关节控制装置,所述力控关节处设有力矩传感器,用于测量得到对应时刻的力矩测量值;包括:
滤波模块,用于基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型,通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值;
计算模块,用于根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器,获得当前时刻的电流控制量;
补偿模块,用于根据所述标称动力学模型、上一时刻的所述力矩估计值和所述力控关节电机的输入电流,获得当前时刻的扰动补偿量;
控制模块,用于根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机的输入电流,并根据所述输入电流控制所述力控关节进行运动。
本申请的实施例还提供一种机器人,所述机器人包括力控关节、处理器和存储器,所述存储器存储有计算机程序,所述处理器用于执行所述计算机程序以实施上述的力控关节控制方法,以控制所述力控关节进行运动。
本申请的实施例还提供一种可读存储介质,其存储有计算机程序,所述计算机程序在处理器上执行时,实施上述的力控关节控制方法。
本申请的实施例具有如下有益效果:
本申请实施例的力控关节控制方法通过利用卡尔曼滤波获得的力矩估计值以及力矩微分的估计值,同时通过扰动观测来实现力控关节***中扰动的观测和补偿,来计算出需要输入至关节电机的电流,从而实现对力控关节的精准力控,该方法采用卡尔曼滤波来计算,得到的结果更加平滑,而进行扰动补偿不仅提高了***对外界扰动的鲁棒性,而且提高了卡尔曼滤波中所用模型的准确度,估计精度更高等。
附图说明
为了更清楚地说明本申请实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本申请的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1示出了本申请实施例的力控关节控制方法的第一流程图;
图2示出了本申请实施例的力控关节控制方法的***架构图;
图3示出了本申请实施例的力控关节控制方法的第二流程图;
图4示出了本申请实施例的力控关节控制方法的第三流程图;
图5示出了本申请实施例的力控关节控制装置的结构示意图。
具体实施方式
下面将结合本申请实施例中附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。
通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此,以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围,而是仅仅表示本申请的选定实施例。基于本申请的实施例,本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例,都属于本申请保护的范围。
在下文中,可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合,并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
此外,术语“第一”、“第二”、“第三”等仅用于区分描述,而不能理解为指示或暗示相对重要性。
除非另有限定,否则在这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义,除非在本申请的各种实施例中被清楚地限定。
实施例1
图1所示为本实施例的力控关节控制方法的第一流程图。
本实施例中,该力控关节是指机器人的关节能够直接控制输出力或力矩。其中,该力控关节处设有一个力矩传感器,用于实时测量该力控关节在对应时刻的力矩,例如,该力矩传感器可以采用基于电阻应变片式的传感器等。应当理解,上述的时刻是指机器人的指令控制周期,一个任务的执行通常包括很多个指令控制周期。
本实施例通过将该力矩传感器建模为一个具有一定刚度的理想弹簧,考虑到关节处的力矩传感器的刚度明显小于该力控关节中其他部分的刚度,因此,本实施例可以对该力控关节进行电机端与负载端的解耦,然后分别基于电机端与负载端建立各自的动力学模型,进而利用该动力学模型来构建出该力控关节的标称动力学模型以用于后续控制。其中,这里的电机端是指用于驱动关节转动的电机侧,而负载端是指连接电机的用于与外界环境进行直接接触的部位。
示范性地,在一种实施方式中,可构建出如下的电机端的动力学模型:
其中,Jm表示电机端的转动惯量,Bm表示电机的阻尼项,θm、和分别表示电机的转动角度、转动速度和转动加速度,Ks表示力矩传感器的刚度,θl表示负载的转动角度,τdm表示电机端未建模存在的不确定因素,如库伦摩擦等,τm表示电机的输出力矩,其与输入至电机的电流i的关系为τm=βi,β为电机端的等效力矩系数。
相应地,该负载端的动力学模型满足:
进而,基于上述的电机端与负载端的动力学模型,为了方便分析电机侧的力矩控制,这里将负载端视为静止状态,然后建立在负载端固定时的标称动力学模型。在一种实施方式中,利用上述两个动力学模型可构建出如下的标称动力学模型:
其中,τs=KsΔθ;
Δθ=θm-θl;
其中,Pn(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时刻***的控制量,这里主要指用于驱动电机产生上述力矩的输入电流iPD(k);A和B是由标称动力学模型Pn(s)转换为离散形式时获得的***参数;w(k-1)表示k-1时刻的过程噪声,这里考虑了在数据传递等过程中可能会存在的一些噪声。y(k)是k时刻力矩的测量值,可由力矩传感器测量得到,其包含有测量噪声n(k)的力矩值;H是测量***的***参数矩阵,这里可设为矩阵H=[1 0]。
可以理解,利用力矩传感器进行力矩测量时,可能存在因外界环境、信号采样及信号转换等各种因素的影响,导致最终得到的测量值会伴随着大量的噪声,故这里将测量噪声也考虑在内,可以进一步提高力控精度等。
基于上述的***状态方程,本实施例将通过卡尔曼滤波对上述的力矩及力矩微分进行最优估计。下面将结合力控关节控制方法的具体步骤来进行说明。在控制过程中,示范性地,该力控关节控制方法包括:
步骤S110,基于获取的上一时刻的力矩测量值和该力控关节的标称动力学模型,通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值。
如图3所示,在一种实施方式中,该步骤S110包括以下子步骤:
子步骤S210,根据上一时刻的力控关节的电机输入电流、力矩预测值、力矩微分预测值利用第一状态方程,得到当前时刻的力矩预测值和力矩微分预测值。
示范性地,可基于上述的第一状态方程和上一时刻的力控关节的电机输入电流、力矩预测值等进行当前时刻的力矩预测值和力矩微分预测值,具体可通过如下公式计算得到:
子步骤S220,根据上一时刻的力矩估计值与力矩真实值之间的误差协方差及过程噪声计算当前时刻的力矩预测值与力矩真实值之间的误差协方差,并根据当前时刻的力矩预测值与力矩真实值之间的误差协方差、及上一时刻的测量噪声计算卡尔曼滤波器的增益。
根据卡尔曼滤波器原理,可通过***状态的预测值与真实值之间的误差协方差、测量***的噪声等来计算卡尔曼滤波器的增益系数。示范性地,该卡尔曼滤波器的增益的计算公式如下:
K(k)=P-(k)HT(HP-(k)HT+R)-1;
其中,K(k)表示k时刻的卡尔曼滤波器的增益,P-(k)表示k时刻的***状态的预测误差的协方差矩阵,即k时刻的***状态的预测值与真实值之间的误差协方差,R表示k时刻的测量噪声n(k)的协方差。
对于上述的预测误差的协方差矩阵P-(k),示范性地,其计算公式如下:
P-(k)=AP(k-1)AT+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个时刻,iPD(k)为该PD-前馈控制器输出的电流控制量,τref(k)为该力控关节的期望力矩,为期望力矩的微分,KP和KD分别为该PD-前馈控制器中的比例环节系数和微分环节系数,τk(k)为力矩估计值,为力矩微分估计值。
示范性地,以上述的PD-前馈控制器为例,如图所示,对于该当前时刻的电流控制量,可通过先根据该力控关节在当前时刻的期望力矩τref得到期望力矩微分然后,根据当前时刻的期望力矩τref与力矩估计值τk之间的偏差、以及该期望力矩微分与力矩微分估计值之间的偏差代入该PD-前馈控制器中,则可计算得到当前时刻的电流控制量iPD。
步骤S 130,根据该标称动力学模型、上一时刻的力控关节电机的输入电流和力矩估计值,获得当前时刻的扰动补偿量。
本实施例通过将利用上一时刻的***状态来对当前时刻进行扰动补偿,这里主要通过一个低通滤波器Q来实现,如图2所示。例如,在一种实施方式中,可采用如下的低通滤波器的模型:
其中,Q(s)表示低通滤波器模型,ωq为该低通滤波器的截止频率。
示范性地,如图4所示,上述步骤S130可包括如下子步骤:
子步骤S310,根据所述标称动力学模型的逆模型和所述力矩估计值通过预设低通滤波模型,得到第一扰动补偿分量。
子步骤S320,根据上一时刻的所述力控关节电机的输入电流通过预设低通滤波模型,得到第二扰动补偿分量。
子步骤S330,将所述第一扰动补偿分量和所述第二扰动补偿分量相加,得到扰动补偿量。
本实施例中,该扰动补偿量主要包括两部分,分别是关于输入至电机的电流的扰动补偿,即上述的第一扰动补偿分量,以及标称动力学模型与力矩估计值的扰动补偿,即上述的第二扰动补偿分量。
示范性地,对于上述扰动补偿量的计算过程,若采用表达式表示,则有:
步骤S140,根据该电流控制量和该扰动补偿量计算得到当前时刻力控关节电机的输入电流,以控制该力控关节执行相应操作。
最后,将该总电流输入至该力控关节的电机以驱动电机转动,以控制该力控关节进行相应操作。进而根据当前任务需求,进入下一控制周期,重复上述步骤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所述的力控关节控制方法,其特征在于,所述获得当前时刻的扰动补偿量,包括:
根据所述标称动力学模型的逆模型和所述力矩估计值通过预设低通滤波模型,得到第一扰动补偿分量;
根据上一时刻的所述力控关节电机的输入电流通过预设低通滤波模型,得到第二扰动补偿分量;
将所述第一扰动补偿分量和所述第二扰动补偿分量相加,得到扰动补偿量。
8.一种力控关节控制装置,其特征在于,所述力控关节处设有力矩传感器,用于测量得到对应时刻的力矩测量值;包括:
滤波模块,用于基于获取的上一时刻的力矩测量值和所述力控关节的标称动力学模型,通过卡尔曼滤波得到当前时刻的力矩估计值和力矩微分估计值;
计算模块,用于根据所述力控关节在当前时刻的期望力矩、所述力矩估计值、所述力矩微分估计值基于预设力矩控制器,获得当前时刻的电流控制量;
补偿模块,用于根据所述标称动力学模型、上一时刻的所述力矩估计值和所述力控关节电机的输入电流,获得当前时刻的扰动补偿量;
控制模块,用于根据所述电流控制量和所述扰动补偿量计算得到当前时刻所述力控关节电机的输入电流,并根据所述输入电流控制所述力控关节进行运动。
9.一种机器人,其特征在于,所述机器人包括力控关节、处理器和存储器,所述存储器存储有计算机程序,所述处理器用于执行所述计算机程序以实施权利要求1-7中任一项所述的力控关节控制方法,以控制所述力控关节进行运动。
10.一种可读存储介质,其特征在于,其存储有计算机程序,所述计算机程序在处理器上执行时,实施根据权利要求1-7中任一项所述的力控关节控制方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111360702.7A CN113954078B (zh) | 2021-11-17 | 2021-11-17 | 力控关节控制方法、装置、机器人和可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111360702.7A CN113954078B (zh) | 2021-11-17 | 2021-11-17 | 力控关节控制方法、装置、机器人和可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113954078A CN113954078A (zh) | 2022-01-21 |
CN113954078B true CN113954078B (zh) | 2023-02-24 |
Family
ID=79470911
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111360702.7A Active CN113954078B (zh) | 2021-11-17 | 2021-11-17 | 力控关节控制方法、装置、机器人和可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113954078B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2013000856A (ja) * | 2011-06-20 | 2013-01-07 | Seiko Epson Corp | ロボット制御システム、ロボットシステム |
CN106313044A (zh) * | 2016-09-20 | 2017-01-11 | 华南理工大学 | 一种工业机器人前馈力矩补偿方法 |
CN108189036A (zh) * | 2018-01-17 | 2018-06-22 | 广州视源电子科技股份有限公司 | 力矩控制方法、装置、机器人及存储介质 |
CN109202889A (zh) * | 2017-06-30 | 2019-01-15 | 沈阳新松机器人自动化股份有限公司 | 一种柔性多关节机器人电流力控制***及方法 |
CN111546346A (zh) * | 2020-05-29 | 2020-08-18 | 清华大学 | 一种柔性关节扰动观测方法、力矩控制方法和设备 |
CN112809667A (zh) * | 2020-12-22 | 2021-05-18 | 上海新时达机器人有限公司 | 工业机器人的力控制方法、装置及其应用 |
CN112987812A (zh) * | 2021-02-24 | 2021-06-18 | 深圳市优必选科技股份有限公司 | 一种力矩控制方法、装置、终端设备及存储介质 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR102034652B1 (ko) * | 2015-05-21 | 2019-10-21 | 카스타니엔바움 게엠바하 | 엑추에이터에 의해 구동되는 로봇 관절을 개루프/폐루프 제어하는 방법 및 장치 |
-
2021
- 2021-11-17 CN CN202111360702.7A patent/CN113954078B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2013000856A (ja) * | 2011-06-20 | 2013-01-07 | Seiko Epson Corp | ロボット制御システム、ロボットシステム |
CN106313044A (zh) * | 2016-09-20 | 2017-01-11 | 华南理工大学 | 一种工业机器人前馈力矩补偿方法 |
CN109202889A (zh) * | 2017-06-30 | 2019-01-15 | 沈阳新松机器人自动化股份有限公司 | 一种柔性多关节机器人电流力控制***及方法 |
CN108189036A (zh) * | 2018-01-17 | 2018-06-22 | 广州视源电子科技股份有限公司 | 力矩控制方法、装置、机器人及存储介质 |
CN111546346A (zh) * | 2020-05-29 | 2020-08-18 | 清华大学 | 一种柔性关节扰动观测方法、力矩控制方法和设备 |
CN112809667A (zh) * | 2020-12-22 | 2021-05-18 | 上海新时达机器人有限公司 | 工业机器人的力控制方法、装置及其应用 |
CN112987812A (zh) * | 2021-02-24 | 2021-06-18 | 深圳市优必选科技股份有限公司 | 一种力矩控制方法、装置、终端设备及存储介质 |
Non-Patent Citations (1)
Title |
---|
基于动力学前馈的工业机器人运动控制关键技术研究;涂骁;《中国博士学位论文全文数据库 信息科技辑》;第1-116页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113954078A (zh) | 2022-01-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Miranda-Colorado et al. | Experimental parameter identification of flexible joint robot manipulators | |
CN110467111B (zh) | 桥式起重机的控制 | |
JP6517567B2 (ja) | サーボモータ制御装置及び衝突検出方法 | |
WO1991005296A1 (en) | Sliding mode control method | |
CN111347421A (zh) | 用于扭矩估计的方法和装置 | |
CN113305839B (zh) | 机器人的导纳控制方法、导纳控制***和机器人 | |
CN111965976B (zh) | 基于神经网络观测器的机器人关节滑模控制方法及*** | |
CN112077839B (zh) | 一种机械臂的运动控制方法及装置 | |
US11669055B2 (en) | Vibration suppression device, method and computer-readable medium using estimated vibration torque | |
CN108762088B (zh) | 一种迟滞非线性伺服电机***滑模控制方法 | |
KR19980083174A (ko) | 공진계의 진동억제 제어방법 및 제어장치 | |
Suzumura et al. | A general framework for designing SISO-based motion controller with multiple sensor feedback | |
CN111015661B (zh) | 一种机器人柔性负载主动振动控制方法和*** | |
CN113954078B (zh) | 力控关节控制方法、装置、机器人和可读存储介质 | |
KR102289968B1 (ko) | 로봇 매니퓰레이터를 제어하기 위한 장치 및 이를 위한 방법 | |
CN106950842B (zh) | 解耦控制方法、装置、机器人及存储介质 | |
CN116985119A (zh) | 基于干扰补偿和有限时间指令滤波的康复机器人控制方法 | |
CN115236990A (zh) | 一种广义估计器、广义抗扰控制器及设计方法 | |
JP2019219762A (ja) | 制御装置、制御方法及びプログラム | |
Hatamleh et al. | Quasi-Continuous Second Order Sliding Mode Control of Revolute-Revolute Manipulator with Noisy Feedback Signals & Modelling Uncertainties | |
CN114734437A (zh) | 机器人关节控制方法及装置 | |
Xu et al. | An accurate identification method based on double weighting for inertial parameters of robot payloads | |
JP5037065B2 (ja) | 機械制御装置 | |
CN114624994B (zh) | 高阶柔性直线***的新型自抗扰控制方法及*** | |
Sigilló et al. | Implementation and testing of different control strategies on a steer-by-wire research platform |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right | ||
TR01 | Transfer of patent right |
Effective date of registration: 20231204 Address after: Room 601, 6th Floor, Building 13, No. 3 Jinghai Fifth Road, Beijing Economic and Technological Development Zone (Tongzhou), Daxing District, Beijing, 100176 Patentee after: Beijing Youbixuan Intelligent Robot Co.,Ltd. Address before: 518000 16th and 22nd Floors, C1 Building, Nanshan Zhiyuan, 1001 Xueyuan Avenue, Nanshan District, Shenzhen City, Guangdong Province Patentee before: Shenzhen UBTECH Technology Co.,Ltd. |