一种基于全状态反馈的机器人关节高精度控制系统及方法
阅读说明：本技术 一种基于全状态反馈的机器人关节高精度控制系统及方法 (Robot joint high-precision control system and method based on full-state feedback ) 是由 刘海涛 王岩 山显雷 黄田 肖聚亮 于 2021-09-29 设计创作，主要内容包括：本发明公开了一种基于全状态反馈的机器人关节高精度控制系统,该控制系统包括状态观测器、机器人关节摩擦模型等；状态观测器基于机器人关节的等效动力学模型构造,其输入检知的关节运转位置数据和未给与摩擦补偿的控制信号,其输出传动系统输出侧变量的估计值；机器人关节摩擦模型输入关节速度指令信号或在传动系统输出侧检知的速度信号；输出等效摩擦力估计值或等效摩擦力矩估计值。本发明还公开了一种基于全状态反馈的机器人关节高精度控制方法,将降阶扩张状态观测器估计出的等效扰动补偿到电机的输入信号中,对驱动关节实施扰动补偿；由摩擦模型估计的摩擦力补偿到电机的输入力矩信号中,对摩擦力实施补偿。(The invention discloses a robot joint high-precision control system based on full-state feedback, which comprises a state observer, a robot joint friction model and the like; the state observer is constructed based on an equivalent dynamic model of the robot joint, inputs detected joint operation position data and a control signal which is not compensated with friction, and outputs an estimated value of a variable on the output side of the transmission system; the robot joint friction model inputs a joint speed command signal or a speed signal detected at the output side of a transmission system; and outputting the estimated value of the equivalent friction force or the estimated value of the equivalent friction torque. The invention also discloses a robot joint high-precision control method based on full-state feedback, which is characterized in that equivalent disturbance estimated by the reduced order extended state observer is compensated into an input signal of a motor, and disturbance compensation is carried out on a driving joint; the friction force estimated by the friction model is compensated into an input torque signal of the motor, and the friction force is compensated.)
技术领域
本发明涉及一种机器人关节高精度运动控制方法，特别涉及一种基于全状态反馈的机器人关节高精度控制系统及方法。
背景技术
机器人由多个驱动关节实现运动传递，由于驱动关节的刚度比连接部件低的多，其末端执行器的动态精度主要由驱动关节的输出精度决定。机器人的驱动关节一般由丝杠螺母副或减速器实现运动/力传递，传动系统的弹性、摩擦、间隙等严重影响了其输出精度。传统的半闭环控制策略以电机编码器信号构造控制闭环，只能保证电机的输出精度，而无法保证控制闭环外的输出精度，难以实现驱动关节的高精度控制。全状态反馈控制将传动系统输出侧的信息引入闭环控制中，理论上具有更好的动态输出精度。然而，由于传统的观测器无法观测扰动信息，导致全闭环控制的抗干扰能力差，容易产生较大的扰动误差。有效获得扰动信息，并对扰动施以补偿或控制成为提高全状态反馈控制抗干扰能力的关键。目前，针对扰动的影响多采用单一的基于数据或基于模型的方法予以补偿，基于数据的扰动补偿方法采用传感器或观测器得到扰动信息，系统结构简单，实时性好，但受带宽限制，通常无法获得全面的扰动信息，如低速换向时摩擦的影响；基于模型的扰动补偿方法需要建立扰动的数学模型，进而设计前馈补偿器予以消除，该类方法思路简单，硬件成本低，难以保证数学模型的完整性和准确性。机器人驱动关节的非线性环节复杂且等效扰动随机器人位型变化，采用单一的补偿策略难以保证扰动补偿效果，需要探索研究高效的复合扰动补偿方法。本发明将借鉴自抗扰控制思想，通过设计降阶扩张状态观测器实现低频线性扰动补偿，同时设计摩擦前馈补偿器实现低速换向时的非线性摩擦补偿，形成基于数据和模型双驱的扰动补偿策略，提出一种机器人驱动关节的自抗扰-全状态反馈控制方法，实现机器人驱动关节的高精度控制。
发明内容
本发明为解决公知技术中存在的技术问题而提供一种基于全状态反馈的机器人关节高精度控制系统及方法。
本发明为解决公知技术中存在的技术问题所采取的技术方案是：一种基于全状态反馈的机器人关节高精度控制系统，该控制系统包括状态观测器、机器人关节摩擦模型、速度调节器、第一比例调节器、第二比例调节器、第一至第三反相加法器、同相加法器；状态观测器，其基于机器人关节的等效动力学模型构造，其输入检知的关节运转位置数据和未给与摩擦补偿的控制信号，其输出传动系统输出侧变量的估计值；其包括第一输出端和第二输出端，其第一输出端输出速度相关变量估计值，其第二输出端输出关节受到的等效扰动变量估计值；机器人关节摩擦模型，其输入关节速度指令信号或在传动系统输出侧检知的速度信号；其输出等效摩擦力估计值或等效摩擦力矩估计值；第一反相加法器，其正极输入端输入指令信号向量，其负极输入端之一输入传动系统输出侧检知的位置信号，其负极输入端之二与状态观测器的第一输出端相连；其输出端与第一比例调节器的输入端相连，第一比例调节器的输出端与第二反相加法器的正极输入端相连；第二反相加法器的负极输入端输入驱动关节转动的电机的输出轴检知的角速度信号；第二反相加法器的输出端与速度调节器的输入端相连，速度调节器的输出端与第三反相加法器的正极输入端相连；第三反相加法器的负极输入端与第二比例调节器的输出端相连；第二比例调节器的输入端与状态观测器的第二输出端相连；第三反相加法器的输出端分别与同相加法器的输入端之一和状态观测器的输入端之一相连；同相加法器的输入端之二与机器人关节摩擦模型的输出端相连；同相加法器的输出端输出驱动关节转动的电机转矩指令。
进一步地，状态观测器为对全阶状态观测器进行降阶处理的降阶扩张状态观测器，其第一输出端输出传动系统输出侧的速度、加速度及加加速度变量估计值。
进一步地，设：i为传动系统的传动比，iθ为关节电机的输入位置指令信号，为关节电机的输入指令信号向量；Ko＝(k1k2 k3 k4)为第一比例调节器的增益矩阵，k1表示位置反馈增益，k2表示速度反馈增益，k3表示加速度反馈增益，k4表示加加速度反馈增益，G为关节受到的扰动变量，V为速度调节器的输出信号，W1为传动系统输出侧的能直接检测的位置信号，W2为传动系统输出侧的不能直接检测的速度、加速度、加加速度信号；为W2的估计值，为扰动变量G的估计值，α为反馈信号的增益系数，为通过机器人关节摩擦模型预估得到的摩擦力,为关节电机转子的速度信号；设速度调节器采用PI控制，使速度调节器的输入信号为将和W1作为降阶扩张状态观测器的输入信号，降阶扩张状态观测器的输出信号包括和使成为扰动及摩擦力综合补偿后的电机转矩指令。
进一步地，设为关节电机的输入速度指令信号，当驱动关节弹性较小时，将作为机器人关节摩擦模型的输入信号。
进一步地，设为中传动系统输出侧速度信号的估计值，当驱动关节弹性较大时，将作为机器人关节摩擦模型的输入信号。
本发明还提供了一种基于全状态反馈的机器人关节高精度控制方法，建立机器人关节的等效动力学模型，基于该等效动力学模型，利用检知的关节运转位置数据及已知的关节参数构造状态观测器，采用状态观测器得到如下变量的估计值：传动系统输出侧的速度相关变量估计值以及关节受到的等效扰动；采用以位置控制环为外环、以速度控制环为内环的双闭环全状态反馈的控制方法，在位置控制环中设置第一比例调节器，在速度控制环中设置速度调节器；将传动系统输出侧的实测位置信号以及状态观测器输出的速度相关变量估计值作为控制系统的位置控制环反馈信号，将状态观测器输出的关节受到的等效扰动估计值补偿到控制系统的速度控制环的输出信号中，对关节运转中的扰动进行补偿；同时建立机器人关节摩擦模型，对机器人关节摩擦模型的参数进行辨识，由输入的关节速度指令信号或在传动系统输出侧检知的速度信号，通过机器人关节摩擦模型估计得到等效摩擦力或等效摩擦力矩；将估计得到的等效摩擦力或等效摩擦力矩，补偿到驱动关节转动的电机转矩指令信号中，对摩擦力给与补偿。
进一步地，状态观测器采用对全阶状态观测器进行降阶处理的降阶扩张状态观测器，其第一输出端输出传动系统输出侧的速度、加速度及加加速度变量估计值。
进一步地，利用已知的关节信息构造降阶扩张状态观测器的方法包括如下步骤：
步骤1，建立如下机器人驱动关节的等效动力学模型：
式(1)中：
步骤2，将式(1)转换为下式(2)：
式中，I1为电机转子的转动惯量；I2为传动系统及机械系统的等效转动惯量；c1为电机转子的摩擦阻尼；c2为等效负载的摩擦阻尼；i为传动系统的传动比；k为传动系统的等效刚度；θ1为电机转子转角；θ2为传动系统输出侧的位置信号；τ1为电机的电磁力矩；τ2为等效至传动系统输出侧的负载力矩；b0为机器人驱动关节的增益系数，y为传动系统输出侧的位置，θ2 (4)为θ2的4阶导数，u为控制输入，τw为系统受到的时变扰动以及非线性环节引起的扰动变量，aj(j＝0,1,2,3)为与机器人驱动关节物理量相关的系数；
步骤3，将式(2)转化为下式(3)所示的状态空间形式：
式(3)中：
x1＝θ2，x5＝τw，x＝(x1 x2 x3 x4 x5)T；
步骤4，设xo1为x中可以直接测得的状态变量θ2，xo2为x中不能直接获得的状态变量以及扰动变量，令式(3)转化为下式(4)：
式中，
Ao11＝0，Ao12＝(1 0 0 0)，
Bow1＝0，Co1＝1，Co2＝(0 0 0 0)；
步骤5，根据式(4)所示的状态方程，按下式(5)得到xo2的观测值从而得到τw的估计值
式中，L＝(β1 β2 β3 β4)T为降阶扩张状态观测器的增益矩阵，β1为与速度估计值相关的增益，β2为与加速度估计值相关的增益，β3为与加加速度估计值相关的增益，β4为与扰动变量估计值相关的增益。
进一步地，对关节运转中的扰动进行补偿的方法包括：
设：u0为速度调节器的输出信号，u为速度调节器的输出信号经降阶扩张状态观测器进行扰动补偿后的信号，x为传动系统输出侧的状态变量,xo1为传动系统输出侧的能直接检测的状态变量θ2，为传动系统输出侧的不能直接检测的状态变量以及扰动变量，为xo2的估计值，为扰动状态τw的估计值，b0为反馈信号的增益系数；为通过机器人关节摩擦模型预估得到的摩擦力；将中的扰动观测量补偿到速度调节器的输出信号u0中；使控制量为
选取合适的降阶扩张状态观测器增益矩阵L，使得式(2)转化为下式(7)：
此时，系统的扰动被抵消。
进一步地，建立如下机器人驱动关节的机器人关节摩擦模型：
式中，τf为摩擦力矩；τe为外部力矩；τs为最大静摩擦力矩；τc为库伦摩擦力矩；ω为机器人关节摩擦模型的输入速度信号；ωs为Stribeck速度；b为粘滞摩擦系数。
进一步地，对机器人关节摩擦模型的参数进行辨识的方法包括：输入不同恒定速度指令，并测出不同恒定速度指令下关节稳定时的电机轴的摩擦力值；利用速度指令数据和对应的摩擦力数据对机器人关节摩擦模型进行拟合，辨识出机器人关节摩擦模型中的τs、τc、ωs和b。
本发明具有的优点和积极效果是：本发明基于机器人的动力学模型，将基于数据与基于模型的扰动补偿相结合，对机器人外部时变扰动以及包括摩擦在内的非线性环节导致的扰动进行了补偿，优化后的补偿方法降低了低速换向时摩擦对机器人的影响，提高了机器人的位置跟踪精度和抗干扰能力。该方法整体过程操作简单，可方便高效的针对包含各种平动或转动关节的机器人进行扰动补偿，为机器人的补偿控制提供一种新的方法。
本发明结合机器人关节中已知的物理量构造降阶扩张状态观测器，来补偿因时变、非线性环节导致的内部不确定性扰动以及外部的时变扰动，有效减少了观测器的相位滞后，提高了观测效果；本发明进一步通过建立摩擦模型实现摩擦力的实时预估与补偿，进一步抑制驱动关节低速换向时摩擦的影响，有利于提高机器人的位置精度和抗扰动能力。
附图说明
图1是本发明的一种基于全状态反馈的机器人关节高精度控制系统工作原理示意图。
图1中：r表示输入指令信号向量，表示r中的输入速度指令信号，Ko表示状态反馈增益矩阵，u0表示速度调节器的输出信号，u表示降阶扩展状态观测进行扰动补偿后的信号，表示电机转子的速度信号，θ2表示传动系统输出侧的位置信号，u和θ2是降阶扩张状态观测器的输入量，表示传动系统输出侧速度信号的估计值，表示传动系统输出侧加速度信号的估计值，表示传动系统输出侧加加速度的估计值，表示扰动状态τw的估计值，b0表示驱动关节的增益系数。表示通过摩擦模型预估得到的摩擦力，RESO表示降阶扩张状态观测器，I1为电机转子的转动惯量；I2为传动系统及机械系统的等效转动惯量；i为传动系统的传动比；k为传动系统的等效刚度；τ1为电机的电磁力矩；τ2为等效至传动系统输出侧的负载力矩。
具体实施方式
为能进一步了解本发明的发明内容、特点及功效，兹列举以下实施例，并配合附图详细说明如下：
请参见图1，一种基于全状态反馈的机器人关节高精度控制系统，该控制系统包括状态观测器、机器人关节摩擦模型、速度调节器、第一比例调节器、第二比例调节器、第一至第三反相加法器、同相加法器；状态观测器，其基于机器人关节的等效动力学模型构造，其输入检知的关节运转位置数据和未给与摩擦补偿的控制信号，其输出传动系统输出侧变量的估计值；其包括第一输出端和第二输出端，其第一输出端输出速度相关变量估计值，其第二输出端输出关节受到的等效扰动变量估计值；机器人关节摩擦模型，其输入关节速度指令信号或在传动系统输出侧检知的速度信号；其输出等效摩擦力估计值或等效摩擦力矩估计值；第一反相加法器，其正极输入端输入指令信号向量，其负极输入端之一输入传动系统输出侧检知的位置信号，其负极输入端之二与状态观测器的第一输出端相连；其输出端与第一比例调节器的输入端相连，第一比例调节器的输出端与第二反相加法器的正极输入端相连；第二反相加法器的负极输入端输入驱动关节转动的电机的输出轴检知的角速度信号；第二反相加法器的输出端与速度调节器的输入端相连，速度调节器的输出端与第三反相加法器的正极输入端相连；第三反相加法器的负极输入端与第二比例调节器的输出端相连；第二比例调节器的输入端与状态观测器的第二输出端相连；第三反相加法器的输出端分别与同相加法器的输入端之一和状态观测器的输入端之一相连；同相加法器的输入端之二与机器人关节摩擦模型的输出端相连；同相加法器的输出端输出驱动关节转动的电机转矩指令。
进一步地，状态观测器可为对全阶状态观测器进行降阶处理的降阶扩张状态观测器，其第一输出端可输出传动系统输出侧的速度、加速度及加加速度变量估计值。
由于观测器的阶数越高相位滞后越严重，在全阶扩张状态观测器的基础上，由于传动系统输出侧的位置信号可以直接获得，因此无需对该信号进行观测，这样由状态观测器观测的状态数目可以降低，形成降阶扩张状态观测器。
进一步地，可设：i为传动系统的传动比，iθ为关节电机的输入位置指令信号，为关节电机的输入指令信号向量；Ko＝(k1 k2 k3 k4)为第一比例调节器的增益矩阵，k1表示位置反馈增益，k2表示速度反馈增益，k3表示加速度反馈增益，k4表示加加速度反馈增益，G为关节受到的扰动变量，V为速度调节器的输出信号，W1为传动系统输出侧的能直接检测的位置信号，W2为传动系统输出侧的不能直接检测的速度、加速度、加加速度信号；为W2的估计值，为扰动变量G的估计值，α为反馈信号的增益系数，为通过机器人关节摩擦模型预估得到的摩擦力,为关节电机转子的速度信号；可设速度调节器可采用PI控制，可使速度调节器的输入信号为可将和W1作为降阶扩张状态观测器的输入信号，降阶扩张状态观测器的输出信号可包括和可使成为扰动及摩擦力综合补偿后的电机转矩指令。
进一步地，可设为关节电机的输入速度指令信号，可当驱动关节弹性较小时，将作为机器人关节摩擦模型的输入信号。
进一步地，可设为中传动系统输出侧速度信号的估计值，可当驱动关节弹性较大时，将作为机器人关节摩擦模型的输入信号。
本发明还提供了一种基于全状态反馈的机器人关节高精度控制方法，建立机器人关节的等效动力学模型，基于该等效动力学模型，利用检知的关节运转位置数据及已知的关节参数构造状态观测器，采用状态观测器得到如下变量的估计值：传动系统输出侧的速度相关变量估计值以及关节受到的等效扰动；采用以位置控制环为外环、以速度控制环为内环的双闭环全状态反馈的控制方法，在位置控制环中设置第一比例调节器，在速度控制环中设置速度调节器；将传动系统输出侧的实测位置信号以及状态观测器输出的速度相关变量估计值作为控制系统的位置控制环反馈信号，将状态观测器输出的关节受到的等效扰动估计值补偿到控制系统的速度控制环的输出信号中，对关节运转中的扰动进行补偿；同时建立机器人关节摩擦模型，对机器人关节摩擦模型的参数进行辨识，由输入的关节速度指令信号或在传动系统输出侧检知的速度信号，通过机器人关节摩擦模型估计得到等效摩擦力或等效摩擦力矩；将估计得到的等效摩擦力或等效摩擦力矩，补偿到驱动关节转动的电机转矩指令信号中，对摩擦力给与补偿。
进一步地，状态观测器可采用对全阶状态观测器进行降阶处理的降阶扩张状态观测器，其第一输出端可输出传动系统输出侧的速度、加速度及加加速度变量估计值。
进一步地，利用已知的关节信息构造降阶扩张状态观测器的方法可包括如下步骤：
步骤1，可建立如下机器人驱动关节的等效动力学模型：
式(1)中：
步骤2，将式(1)转换为下式(2)：
式中，I1为电机转子的转动惯量；I2为传动系统及机械系统的等效转动惯量；c1为电机转子的摩擦阻尼；c2为等效负载的摩擦阻尼；i为传动系统的传动比；k为传动系统的等效刚度；θ1为电机转子转角；θ2为传动系统输出侧的位置信号；τ1为电机的电磁力矩；τ2为等效至传动系统输出侧的负载力矩；b0为机器人驱动关节的增益系数，y为传动系统输出侧的位置，θ2 (4)为θ2的4阶导数，u为控制输入，τw为系统受到的时变扰动以及非线性环节引起的扰动变量，aj(j＝0，1，2，3)为与机器人驱动关节物理量相关的系数；
步骤3，可将式(2)转化为下式(3)所示的状态空间形式：
式(3)中：
x1＝θ2，x5＝τw，x＝(x1 x2 x3 x4 x5)T；
步骤4，可设xo1为x中可以直接测得的状态变量θ2，xo2为x中不能直接获得的状态变量以及扰动变量，令式(3)可转化为下式(4)：
式中，
Ao11＝0，Ao12＝(1 0 0 0)，
Bow1＝0，Co1＝1，Co2＝(0 0 0 0)；
步骤5，可根据式(4)所示的状态方程，可按下式(5)得到xo2的观测值从而得到τw的估计值
式中，L＝(β1 β2 β3 β4)T为降阶扩张状态观测器的增益矩阵，β1为与速度估计值相关的增益，β2为与加速度估计值相关的增益，β3为与加加速度估计值相关的增益，β4为与扰动变量估计值相关的增益。
进一步地，对关节运转中的扰动进行补偿的方法可包括：
可设：u0为速度调节器的输出信号，u为速度调节器的输出信号经降阶扩张状态观测器进行扰动补偿后的信号，x为传动系统输出侧的状态变量,xo1为传动系统输出侧的能直接检测的状态变量θ2，为传动系统输出侧的不能直接检测的状态变量以及扰动变量，为xo2的估计值，为扰动状态τw的估计值，b0为反馈信号的增益系数；为通过机器人关节摩擦模型预估得到的摩擦力；将中的扰动观测量补偿到速度调节器的输出信号u0中；可使控制量为
可选取合适的降阶扩张状态观测器增益矩阵L，使得式(2)可转化为下式(7)：
此时，系统的扰动被抵消。
进一步地，可建立如下机器人驱动关节的机器人关节摩擦模型：
式中，τf为摩擦力矩；τe为外部力矩；τs为最大静摩擦力矩；τc为库伦摩擦力矩；ω为机器人关节摩擦模型的输入速度信号；ωs为Stribeck速度；b为粘滞摩擦系数。
Stribeck速度：当物体克服静摩擦力后，摩擦力在低速下随着速度的增加而减小，呈现为速度的连续函数，也称为负斜率现象，Stribeck指数模型以速度作为自变量，以摩擦力作为因变量，通过数学模型的形式描述了该现象，Stribeck速度为该模型中的一个常数参数。
进一步地，可对机器人关节摩擦模型的参数进行辨识的方法包括：输入不同恒定速度指令，并测出不同恒定速度指令下关节稳定时的电机轴的摩擦力值；利用速度指令数据和对应的摩擦力数据对机器人关节摩擦模型进行拟合，辨识出机器人关节摩擦模型中的τs、τc、ωs和b。
下面以本发明的一个优选实施例来进一步说明本发明的工作原理：
请参见图1，图1为本发明的一种基于全状态反馈的机器人关节高精度控制系统工作原理示意图。图1中：
表示输入指令信号向量，表示输入的速度指令信号，Ko＝(k1 k2 k3 k4)表示状态反馈增益矩阵，k1表示位置反馈增益，k2表示速度反馈增益，k3表示加速度反馈增益，k4表示加加速度反馈增益，u0表示速度调节器的输出信号，u表示降阶扩展状态观测进行扰动补偿后的信号，表示电机转子的速度信号，θ2表示传动系统输出侧的位置信号，u和θ2是降阶扩张状态观测器的输入量，表示传动系统输出侧速度信号的估计值，表示传动系统输出侧加速度信号的估计值，表示传动系统输出侧加加速度的估计值，表示扰动状态τw的估计值，b0表示驱动关节的增益系数。表示通过摩擦模型预估得到的摩擦力，当驱动关节弹性较小时，摩擦模型的速度输入可以采用输入指令信号r中的速度信号即方案一；当驱动关节弹性较大时摩擦模型的速度输入可以用观测器观测的速度信号即方案二。RESO表示降阶扩张状态观测器。使降阶扩张状态观测器的输出信号包括和令使成为观测器扰动补偿及摩擦力补偿后的驱动关节输入信号。
以上所述的实施例仅用于说明本发明的技术思想及特点，其目的在于使本领域内的技术人员能够理解本发明的内容并据以实施，不能仅以本实施例来限定本发明的专利范围，即凡本发明所揭示的精神所作的同等变化或修饰，仍落在本发明的专利范围内。