CN107505841B

CN107505841B - 一种基于干扰估计器的机械臂姿态鲁棒控制方法

Info

Publication number: CN107505841B
Application number: CN201710776212.2A
Authority: CN
Inventors: 彭琛; 王佳; 饶心远; 朱波; 林晨
Original assignee: University of Electronic Science and Technology of China
Current assignee: University of Electronic Science and Technology of China
Priority date: 2017-08-31
Filing date: 2017-08-31
Publication date: 2021-02-05
Anticipated expiration: 2037-08-31
Also published as: CN107505841A

Abstract

本发明公开了一种基于干扰估计器的机械臂姿态鲁棒控制方法。首先，建立机械臂动力学模型并对其进行反馈线性化，将其化简成受扰双积分模型。其次，设置双组分的姿态鲁棒控制器以实现机械臂姿态鲁棒跟踪控制，该控制器包括两部分：其一为设置标称系统的输入，以保证闭环系统能够追踪参考信号；其二为对干扰估计器的设置，用于对模型不确定和外界扰动进行实时估计，进而对控制信号进行补偿。将补偿后的控制信号转换成电压信号输入作动器，从而完成在受扰条件下机械臂的姿态鲁棒控制。本发明能够有效消除或者抑制运动模型中的不确定性及干扰信号以实现对机械臂姿态期望参考信号的高精度跟踪，且抗干扰能力强、控制参数明确、计算量较少、实施性。

Description

一种基于干扰估计器的机械臂姿态鲁棒控制方法

技术领域

本发明涉及一种存在模型不确定性与干扰时，对机械臂进行姿态鲁棒控制的方法，属于机械臂姿态控制技术领域。

背景技术

机械臂是能模仿人手和臂的某些动作功能，以抓取、搬运物件或操作工具的自动装置。它可以代替人的繁重劳动以实现生产的机械化和自动化，能代替人在有害环境下进行操作从而保障人身安全。然而机械臂在工作过程中会受到载荷变化、系统老化、执行机构失效等的影响，这些影响都会导致外界干扰及模型不确定等问题，进而导致结构体的使用寿命缩短，严重时会出现生产安全事故，因此机械臂自身的抗干扰性能十分重要。另外机械臂具有一些多变量、强耦合、非线性的特征，这些特征再加上外界干扰及模型不确定等因素的影响导致为其设置性能良好的控制系统较为困难。

姿态控制系统是机械臂控制系统的重要组成部分，姿态控制能否达到期望的性能决定了机械臂控制系统的总体效果的好坏。

发明内容

本发明的发明目的在于：针对上述存在的问题，提供一种存在模型不确定与干扰时的机械臂姿态鲁棒控制方法。

本发明的控制处理为：首先，建立机械臂动力学模型并对其进行反馈线性化，将其化简成受扰双积分模型；其次，设置双组分的姿态鲁棒控制器以实现机械臂姿态鲁棒跟踪控制，所述控制器包括两部分：其中一个是设置标称系统的输入，以保证闭环系统能够追踪参考信号。另一个是对干扰估计器(uncertainty and disturbance estimator，UDE)的设置，用于对模型不确定和外界扰动进行实时估计，进而对控制信号进行补偿。将补偿后的控制信号转换成电压信号输入作动器，从而完成在受扰条件下机械臂的姿态鲁棒控制。

本发明的一种基于干扰估计器的机械臂姿态鲁棒控制方法包含以下步骤：

步骤1：对刚性机械臂建立动力学模型并对其进行反馈线性化：

动力学模型为：

其中ρ(t)表示姿态角信号，d(t)表示真实扰动，u(t)表示系统输入信号，函数f(·)和g(·)为非线性函数。

为了使式(1)化为线性积分链形式，采用如下非线性反馈控制律：

u(t)＝g^-1(ρ(t))(μ(t)-f(ρ(t))) (2)

即可得到如下受扰二阶线性系统：

其中μ(t)表示变量替换后线性状态方程的输入信号。

本发明通过设置μ(t)使得姿态角信号ρ(t)能跟踪期望的参考信号ρ_d(t)，并满足期望的瞬态和稳态性能。在得到μ(t)之后，通过式(2)可得到机械臂系统的原始控制输入信号u(t)，这个信号可以转化为作动器的电压值，从而完成在受扰条件下机械臂的姿态鲁棒控制。

步骤2：构造双组分的姿态鲁棒控制器：

首先，不考虑上述的转换模型(3)中的干扰项d(t)，设置一种机械臂姿态控制器来实现角位置跟踪，即假设式(3)中d(t)＝0，可得到如下标称系统

其中u₀(t)表示设置的标称系统的输入信号。本发明中，设置如下的PD(Proportion-Derivative)控制器u₀(t)以实现角位置跟踪：

其中，

表示姿态角信号ρ(t)的二阶导，e(t)表示姿态角信号的误差量，

表示e(t)的导数，即e(t)＝ρ(t)-ρ_d(t)，k_P表示比例项系数，k_D表示微分项系数，可以用基于极点配置或者线性二次型调节器(linear quadratic regulator,LQR)等方法确定出k_P和k_D的值。

然后，考虑存在模型不确定或者外界干扰存在时的输入信号，即在上述设置的标称系统输入信号u₀(t)的基础上，考虑扰动的影响，为受扰系统(3)设置控制输入信号μ(t)使其消除或者抑制干扰的影响。本发明中，设置如下双组分的姿态鲁棒控制器：

其中

表示真实扰动d(t)的估计值。将上式代入(3)式中可得：

由此可见，若

能逼近真实扰动d(t)，则μ(t)能够消除或者减弱d(t)带来的影响。

本发明中，设置干扰估计器的具体结构形式如下：

其中，

表示姿态角速率信号，

表示姿态角速率信号的初始值，T表示干扰估计器的时间常数，基于系统需求的控制精度进行调适性设置。

步骤3：将补偿后的控制信号转换成电压信号输入刚性机械臂的作动器：

基于式(5)所设置的机械臂姿态控制器，实时得到标称系统的输入信号u₀(t)；

基于式(8)所设置的干扰估计器对真实扰动d(t)进行实时估计，得到估计值

将u₀(t)、

带入式(6)则可得到μ(t)，再通过式(2)可得到机械臂系统的原始控制输入信号u(t)(即补偿后的控制信号)，将u(t)转换成电压信号输入刚性机械臂的作动器。

进一步的，干扰估计器的时间常数T的确定方式如下：

根据式(8)所设置的干扰估计器的具体结构形式可知，真实扰动d(t)与其估计值

满足如下的滤波关系：

其中Q表示二阶线性滤波器，且Q的传递函数为：

其中，干扰估计器的时间常数T则对应滤波器Q的设置参数，s表示复频域变量。

滤波器Q的特性与T的大小紧密相关，其截止频率为1/T，所以时间常数T的值越小，则滤波器带宽越宽，估计的

中包含真实干扰d(t)信息越丰富，估计精度越高。

为此，针对外界扰动的频率未知的情况，可以考虑通过减小T的设置方式，来实现控制精度的提升。优选的，在时间常数T的取值范围内，首先设置时间常数T为靠近取值范围上限的值，并验证系统的控制精度(姿态角跟踪参考信号的超调量和姿态角跟踪参考信号的跟踪误差)是否满足要求，若满足要求，则停止选择；若不满足要求，继续减小T的值进行调适性设置。

若已知外界扰动的频率，则干扰估计器的时间常数T的设置方式如下：

定义外界扰动的最高频率为

对应的外界扰动

则有

因滤波器Q的截止频率应比外界扰动的频率要大，故T要取大于

的值。

从

开始，逐步增大时间常数T的取值，筛选K个满足控制精度的时间常数T，并从筛选结果中选择最优(姿态角跟踪参考信号的超调量和姿态角跟踪参考信号的跟踪误差较小，例如|超调量|+|跟踪误差|最小)的作为时间常数T的最终取值。

优选的，先设置五个子搜索区间：

再分别从每个子搜索区间中筛选K个满足控制精度的时间常数T，再从所有筛选结果中选择最优的作为时间常数T的最终取值。

本发明考虑了三个方面的问题，分别是机械臂动力学模型的反馈线性化，双组分的姿态鲁棒控制器的构造，以及干扰估计器时间常数T的确定。首先对机械臂的动力学模型进行反馈线性化处理；然后设置双组分的姿态鲁棒控制器以实现机械臂姿态鲁棒跟踪控制；双组分的姿态鲁棒控制器的设置包括两部分，其中一个是设置标称系统的输入u₀(t)，可以保证闭环系统能够追踪参考信号。另一个是对UDE的设置，UDE能够对模型不确定和外界扰动进行实时估计，进而对控制信号进行补偿。将补偿后的控制信号μ(t)通过式(2)得到u(t)，再将其转化为电压信号并输入给作动器，从而完成在受扰条件下机械臂的姿态鲁棒控制。合适的干扰估计器时间常数的选择可以提高系统控制精度，最后本发明还给出了选取干扰估计器时间常数的方法。

综上所述，由于采用了上述技术方案，本发明的有益效果是：在模型不确定及存在干扰情况下对机械臂进行姿态鲁棒控制的设置方法，该方法能够抑制运动模型中的不确定性及干扰信号以实现对机械臂姿态期望参考信号的高精度跟踪，具有设置简单、抗干扰能力强、计算量少、实施性强等优点。

附图说明

图1：三自由度直升机实验模型图。

图2：本发明闭环控制系统框图。

图3-a：无UDE作用，无人为外加干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图3-b：无UDE作用，无人为外加干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪误差实验结果图，误差曲线用实线表示。

图4-a：无UDE作用，人为悬挂重物干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图4-b：无UDE作用，人为悬挂重物干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪误差实验结果图，误差曲线用实线表示。

图5-a：无UDE作用，人为风力干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图5-b：无UDE作用，人为风力干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪误差实验结果图，误差曲线用实线表示。

图6-a：有UDE作用，无人为外加干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图6-b：有UDE作用，无人为外加干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪误差实验结果图，误差曲线用实线表示。

图7-a：有UDE作用，人为悬挂重物干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图7-b：有UDE作用，人为悬挂重物干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪误差实验结果图，误差曲线用实线表示。

图8-a：有UDE作用，人为风力干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图8-b：有UDE作用，人为风力干扰，参考信号ε_d＝5sin0.3πt+13(deg)时升降角ε跟踪误差实验结果图，误差曲线用实线表示。

图9-a：无UDE作用，无人为外加干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图9-b：无UDE作用，无人为外加干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪误差实验结果图，误差曲线用实线表示。

图10-a：无UDE作用，人为外加重物干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图10b：无UDE作用，人为外加重物干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪误差实验结果图，误差曲线用实线表示。

图11a：无UDE作用，人为外加风力干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图11b：无UDE作用，人为外加风力干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪误差实验结果图，误差曲线用实线表示。

图12a：有UDE作用，无人为外加干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图12b：有UDE作用，无人为外加干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪误差实验结果图，误差曲线用实线表示。

图13a：有UDE作用，人为外加重物干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图13b：有UDE作用，人为外加重物干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪误差实验结果图，误差曲线用实线表示。

图14a：有UDE作用，人为外加风力干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪参考信号实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

图14b：有UDE作用，人为外加风力干扰，参考信号θ_d＝5sin0.05πt(deg)时升降角θ跟踪误差实验结果图，参考信号用实线表示，跟踪信号用虚线表示。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合实施方式和附图，对本发明作进一步地详细描述。

本具体实施方式采用的机械臂是固高科技(深圳)有限公司生产的型号为GHP2002的三自由度直升机实验模型，其机械外形如图1所示，图中的E点和F点表示前后电机，F_b与F_f表示前后电机产生的升力，机身EF通过CD与平衡杆BC连接，且机身可以绕杆BC旋转，旋转角度定义为俯仰角(φ)。主杆AG连接在基座G上并垂直于地面，平衡杆BC可以绕主杆旋转，旋转角度定义为航向角(θ)。同时改变两个旋桨的转速，可以使机体绕升降轴AH旋转，产生升降角(ε)。另外杆EDF是一个不能够活动的刚性杆，点H还一个可以活动的点。杆EDF可以绕滚旋轴进行旋转。平衡杆末端有一个配重块，用来平衡机体产生的升降力矩。

对上述机械臂的控制的验证平台可以利用Simulink/Matlab搭建控制模型。首先，确定三自由度直升机姿态角的动力学模型，并根据姿态角动力学模型进行反馈线性化。利用Matlab中的Simulink工具箱搭建闭环控制系统实验平台，包括：三自由度直升机的姿态角运动模型，双组分姿态鲁棒控制器。其次，选择所设置干扰估计器的参数，一阶微分估计器参数、PD控制器参数。待控制器参数整定完毕，将其代入机械臂的控制器中进行实验，观察姿态角跟踪曲线，根据实验效果调整控制器参数。最后，分别在有无UDE情况下，针对无额外人为增加干扰(只有系统自身模型不确定性影响)、人为悬挂重物干扰、人为风力干扰三种情况来进行实验以验证所提出方法的有效性。

参见图2，本发明的控制处理为：首先，建立机械臂动力学模型并对其进行反馈线性化，将其化简成受扰双积分模型；其次，设置双组分的姿态鲁棒控制器以实现机械臂姿态鲁棒跟踪控制，所述控制器包括两部分：其中一个是设置标称系统的输入，以保证闭环系统能够追踪参考信号。另一个是对干扰估计器(UDE)的设置，用于对模型不确定和外界扰动进行实时估计，进而对控制信号进行补偿。将补偿后的控制信号转换成电压信号输入作动器，从而完成在受扰条件下机械臂的姿态鲁棒控制。其具体实施步骤如下：

第一步：机械臂姿态角动力学模型的建立：

以三自由度纵列式双旋翼直升机实验模型(简称三自由度直升机实验模型)为原型，对刚性体的三自由度直升机建立升降通道与俯仰通道动力学模型如下：

其中δ_a＝tan^-1(L_e/L_a)，δ_h＝tan^-1(L_e/L_h)；ε，θ分别代表三自由度机械臂的升降角(elevation)、俯仰角(pitch)，单位均为rad，由于机械限制，升降、俯仰角ε,θ∈(-π/2,π/2)，

分别表示ε，θ的导数；V_f，V_b分别为作用于前、后电机的电压，电机的电压限制为10V；M_f，M_b分别是前后部分的质量，单位为kg；L_c，L_a，L_h分别是直升机AB，AC，DE＝DF的杆长度，单位为m；g是重力加速度，单位为m/s²。J_ε，J_θ是绕俯仰轴、滚转轴的转动惯量，单位为kgm²；η_ε，η_θ分别是俯仰角、滚转角的粘滞摩擦系数，单位为kgm²/s；K_m为升力系数，单位为N/V；f_ρ是扰动项，其中ρ∈{ε,θ}。

第二步：对机械臂的姿态模型的升降、俯仰通道进行输入变换。

对三自由度机械臂的动力学模型(11)作如下输入变量替换：

其中：

以及变量替换：

将(13)和(14)代入动力学模型(11)中可以得到受扰双积分模型：

第三步：设置姿态角速率估计器。

在实际工程与实验中，经常因成本控制或者各种测量条件受限，姿态角速度信息无法直接得到精确的测量，使得机械臂的姿态无法准确的控制。本发明实施所用的三自由度机械臂平台只能够获得角位置信号，不能够获得角速度信号，所以要对角速度，角加速度进行估计。

将期望的角位置信号输入一阶微分估计器得到期望姿态角速率信号估计值，将期望姿态角速率信号估计值输入一阶微分估计器得到期望姿态角加速度信号估计值信号，具体形式如下：

其中参考信号ρ_d＝[ε_d,θ_d]，ε_d、θ_d分别表示ε、θ的跟踪参考信号，z₁，z₂，z₃作为一阶微分滤波估计器的输出信号分别是

的估计值。α为一阶微分滤波估计器的设置参数，本具体实施方式中，取α＝50。

以姿态角与期望值的误差量作为一阶微分滤波估计器的输入，得到的输出是姿态角速度误差的估计值，所述一阶微分估计器其具体形式如下:

其中，e_ρ(t)为姿态角跟踪误差信号，e_ρ(t)＝ρ(t)-ρ_d(t)，ρ∈{ε,θ}，ρ_d∈{ε_d,θ_d}。

为一阶微分估计器的输出信号，也是姿态角跟踪误差信号

的估计值，本发明中α的值取50。

第四步：双组分的姿态鲁棒控制器的构造。

双组分的姿态鲁棒控制器的设置包括两个部分的设置，一个是标称系统(不考虑模型不确定和扰动时的系统)的输入信号，另一个真实扰动的估计信号。

1)对一个不考虑扰动存在的标称系统，即对于(15)式中取d_ρ(t)＝0时，取

其中

为标称系统的输入信号。并设置如下的PD控制器u₀(t)以实现角位置跟踪：

其中，

是参考姿态角的二阶导的估计值，

是比例项系数，

是微分项系数。下面给出本发明中控制参数

和

的设置方法：

由(19)可得到如下闭环误差方程：

其中，

将(20)式可以写成如下的状态方程：

其中

B＝[0 1]^T，

然后采用LQR来确定出参数

的值，对于闭环误差程(21)，通过选取合适的Q阵和R阵确定最佳反馈控制量

中状态反馈矩阵

使得控制性能指标

取极小值，调整Q阵和R阵取得合适的PD参数，在本实验中

R＝0.5，最终得到的PD控制器的两个参数为：

2)现在给线性方程(15)设置一个μ_ρ(t)使其消除或者减弱干扰的影响，设置双组分的姿态鲁棒控制器：

将(22)代入(15)得到：

在式(23)中

为真实扰动d_ρ(t)的估计信号，它能够消除或者减弱d_ρ(t)带来的影响。

下面给出双组分的姿态鲁棒控制器中

的设置：

设置扰动的估计值

满足如下的滤波关系：

其中d_ρ(t)是模型不确定因素或者干扰，Q是一个二阶线滤波器，Q的传递函数为：

由(23)式可得

则(24)式可以写为：

将(25)代入(26)并解出

得到设置的干扰估计器结构如下：

其中，

为姿态角速率信号的初始值本发明取为0。T为干扰估计器的时间常数。

第五步：干扰估计器时间常数T的确定。

将干扰估计器的时间常数T＝10时代入本发明设置的双组分姿态鲁棒控器进行实验并观察和记录实验结果，发现T＝10时姿态角跟踪误差较大不能够满足控制精度要求，所以考虑减小T进行实验。在T分别取8，6，3，1进行实验发现姿态角跟踪误差逐渐减小，当T取0.5时，观察实验结果发现系统超调量开始增大，所以本具体实施方式中选取干扰估计器的时间常数为T＝1。

第六步：将经过UDE补偿后的控制信号转换成电压量，输入机械臂作动器。

由机械臂的动力学方程(11)、(12)，可以通过如下计算公式得到V_f和V_b并输入给电机：

其中：

本具体实施方式中的三自由度直升机实验模型是一种典型的机械臂，本具体实施方式以此为实验对象，分别在有无UDE的情况下，对三种不同干扰形式下的机械臂做了姿态角跟踪实验，并给出了模型试验结果图。本具体实施方式所有的实验图都是在参数

R＝0.5，α＝50，T＝1情况下给出的。本实验中无人为外加干扰指的是在没有人为外加干扰即系统的干扰只来自三自由度机械臂模型的模型不确定性，人为外加悬挂重物干扰是在直升机实验模型的升降轴上悬挂一个重量为247g的重物，人为外加风力干扰是通过两个能产生较大风力的电风扇对三自由度直升机实验模型的两个螺旋桨吹风得到。

实施例1

干扰估计器在三种不同形式干扰下的作用下，升降角ε跟踪正弦信号的作用检验。

实验实施中对三种不同干扰形式下的机械臂做了姿态角跟踪实验，并给出了模型试验结果图，其控制参数

微分估计器实验参数α＝50，干扰估计器时间常数T＝1，跟踪信号ε_d＝5sin(0.3πt)+13(deg)。图3、4和5表示在无UDE作用时三自由度机械臂分别在无人为外加扰动、人为悬挂重物扰动、人为外加风力扰动时的结果图，实验证明在没有UDE作用的时候直升机不能够有效跟踪正弦信号，误差较大。图6、7和8分别表示在无人为外加扰动、人为悬挂重物扰动、人为外加风力扰动时情况下系统有UDE作用时三自由度直升机跟踪姿态角结果图，实验结果证明在有UDE作用的时候直升机能够有效跟踪正弦信号，跟踪误差较小，具有很好的抵抗外界干扰的能力。

实施例2

干扰估计器在三种不同形式干扰下的作用下，俯仰角θ跟踪正弦信号的作用检验。

实验实施中的三种扰动形式与实施例一中的相同，控制参数

微分估计器实验参数α＝50，跟踪信号θ_d＝5sin0.05πt(deg)，干扰估计器时间常数T＝1。图9、10和11表示在无UDE作用时三自由度机械臂分别在无人为外加扰动、人为悬挂重物扰动、人为外加风力扰动时的结果图。图12、13和14分别表示在无人为扰动、人为悬挂重物扰动、人为外加风力扰动时情况下系统有UDE作用时三自由度直升机跟踪姿态角结果图。

图14中人为外加风力干扰时候，系统跟踪误差较大，其原因是三自由度直升机实验模型对应如图1所示的杆EDF是一个不能够活动的刚性杆，点H是一个可以活动的点。杆EDF可以绕俯仰轴进行旋转。而风力对点H作用较大，所以在做俯仰角跟踪实验时候误差会稍大，属于正常现象。

图示结果表明没有UDE作用时，升降角不能够准确的跟踪正弦信号且误差较大，有UDE作用时，升降角能够较准确的跟踪正弦信号，且系统抗干扰能力较好，稳定性强。

以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合。