CN118219267A - 一种柔性关节机械臂轨迹跟踪控制算法 - Google Patents

Abstract

本发明公开了一种柔性关节机械臂轨迹跟踪控制算法，包括：建立柔性关节机械臂系统的数学模型；基于柔性关节机械臂的数学模型，建立串联积分型状态空间方程，设计扩张状态观测器，估计出状态矢量和复合扰动值；利用状态矢量和复合扰动值设计滑模控制律，实现柔性关节机械臂的轨迹跟踪控制，并证明了扩张状态观测器和基于扩张状态观测器的滑模控制器在柔性关节机械臂系统的稳定性。本发明避免了多次求导或模型转换求取连杆角位移高阶导数值带来的误差，提高了轨迹跟踪控制的精度和稳定性，以及柔性关节机械臂系统的鲁棒性和稳定性，并证明了扩张状态观测器可以观测出每一个状态变量，观测值最终能够逼近状态变量，以及证明了控制系统是稳定的。

Description

一种柔性关节机械臂轨迹跟踪控制算法

技术领域

本发明涉及自动控制技术领域，尤其涉及一种柔性关节机械臂轨迹跟踪控制算法。

背景技术

机械臂在汽车、医疗和航天等领域有着广泛的应用，其可以根据需求在不同的应用场景完成各种工作。鉴于柔性关节机械臂广泛的应用，对柔性关节机械臂的建模及控制进行了广泛的研究。在需要人与机械臂密切接触的场景中，轻量化的设计往往可以提升其安全性。然而随着重量的减轻，其控制问题却变得更加复杂，特别是关节中柔性齿轮，传动带，轴承等能量传递元件带来的关节柔性将更加显著影响控制效果。因此在控制器设计的过程中应充分考虑关节柔性，不能将其忽略。柔性关节机械臂动力学模型一个典型的非线性、强耦合四阶系统，其工作环境的外部扰动和自身模型的参数摄动都对控制算法的设计提出了更高的要求。滑模控制由于其良好的鲁棒性，被广泛应用至柔性关节机械臂的位置控制上。目前业界提出了多种应用在柔性关节机械臂轨迹跟踪控制上的滑模控制算法，理论计算和计算机仿真结果表明其可以取得良好的效果，可以有效克服扰动实现机械臂精准轨迹跟踪。然而这些控制算法却很少应用在实物上，这是因为这些控制算法均需要测量连杆角位移的高阶导数值，对于柔性关节机械臂，与关于电机侧的状态变量不同的是，关于连杆侧的状态变量(包括连杆角位移的高阶导数值)在实际应用时难以准确测量。特别是连杆角位移的高阶导数值，通过对角位移数值微分的方式求取，则其值容易受噪声信号影响产生较大偏差，通过其数学模型作变换转换为电机和连杆的角位移和速度，则计算繁琐且容易受参数摄动影响，所以这些算法应用在实物的轨迹跟踪控制上时，将产生较大误差。

发明内容

针对柔性关节机械臂控制算法需准确测量连杆角位移高阶导数值的特点，本发明设计扩张状态观测器将外部扰动和参数摄动引起的复合扰动作为扩张状态变量，可以估计出完整状态变量和扩张状态变量，将其与滑模控制器相结合使用，该方法具有较强的环境适应能力和鲁棒性，既可以在理论上获得良好的性能，又可以应用在柔性关节机械臂实物的轨迹跟踪控制上，可以有效提高柔性关节机械臂的轨迹跟踪控制精度和稳定性。本发明的目的在于提供一种柔性关节机械臂轨迹跟踪控制算法，有效估计出连杆角位移及其高阶导数值，避免通过多次求导或模型转换求取连杆角位移高阶导数值带来的误差，同时提供一种滑模控制方法，有效克服外部扰动和参数摄动，实现精确轨迹跟踪控制。

为达上述目的，本发明采用以下技术方案：

一种柔性关节机械臂轨迹跟踪控制算法，包括以下步骤：

建立柔性关节机械臂系统的数学模型；

基于所述柔性关节机械臂的数学模型，建立串联积分型状态空间方程，设计扩张状态观测器，估计出状态矢量和复合扰动值，利用李雅普诺夫稳定性定理，证明所述扩张状态观测器的稳定性；

利用所述状态矢量和复合扰动值设计滑模控制律，根据所述滑模控制律，实现轨迹跟踪控制，利用李雅普诺夫稳定性定理，证明所述基于扩张状态观测器的滑模控制器在柔性关节机械臂系统的稳定性。

作为一种柔性关节机械臂轨迹跟踪控制算法的优选方案，所述柔性关节机械臂系统的数学模型包括：

其中，q＝[q₁ q₂]^T为连杆角位移向量，q₁为第一连杆的角位移向量，q₂为第二连杆的角位移向量，T代表转置矩阵运算，和分别为连杆角速度向量和连杆角加速度向量，θ＝[θ₁ θ₂]^T为电机转子经减速器后的输出角位移向量，θ₁为电机转子经减速器后的第一连杆的输出角位移向量，θ₂为电机转子经减速器后的第二连杆的输出角位移向量，和分别为输出角速度向量和输出角加速度向量，为连杆惯性矩阵，为电机惯量矩阵，为2行2列的矩阵，为机械臂的科氏力和离心力向量，为2行一列的向量，为重力向量，为关节刚度矩阵，τ＝[τ₁ τ₂]^T为控制器输出向量，τ₁为第一连杆的控制器输出向量，τ₂为第二连杆的控制器输出向量。

作为一种柔性关节机械臂轨迹跟踪控制算法的优选方案，所述建立串联积分型状态空间方程，包括：

定义 为简化计算用符号，无实意，根据所述柔性关节机械臂系统的数学模型，构建θ及θ的二阶导数的表示式：

根据和所述柔性关节机械臂系统的数学模型，构建动力学模型：

其中， g(q)＝M^-1(q)KJ^-1，q⁽³⁾和q⁽⁴⁾分别代表连杆的三阶导数和四阶导数，为简化计算用符号；

令τ＝g^-1(q)v，所述动力学模型可写为由于q＝[q₁q₂]^T,所述动力学模型可写为：

重新定义状态变量z₁₁＝q₁,和z₂₁＝q₂, 和将和分别定义为扩张状态变量z₁₅和z₂₅，假设扩张状态变量可导且其导数是有界的，即且满足|γ₁|≤h₁,|γ₂|≤h₂，h₁和h₂为任意正实数，则所述动力学模型可重新写为串联积分型状态空间方程：

其中，z₁₁＝q₁,分别表示所述第一连杆角位移的0阶、1阶、2阶导数，表示所述第一连杆角位移的4阶导数，表示所述第二连杆角位移的4阶导数，z₁₅为所述扩张状态变量，v₁和v₂分别表示第一连杆和第二连杆的滑模控制律。

作为一种柔性关节机械臂轨迹跟踪控制算法的优选方案，所述设计扩张状态观测器，包括：

根据所述串联积分型状态空间方程，分别设计所述第一连杆和第二连杆的扩张状态观测器：

其中，是所述串联积分型状态空间方程中第一连杆所有相关状态变量z_1i(i＝1,2,3,4,5)的估计值，z₁₅为所述扩张状态变量，即为复合扰动的估计值，复合扰动包括外部扰动和参数摄动，是所述串联积分型状态空间方程中第二连杆所有相关状态变量z_2i(i＝1,2,3,4,5)的估计值，α_1i(i＝1,2,3,4,5)和α_2i(i＝1,2,3,4,5)分别为待设计的两个所述扩张状态观测器增益，选择合适的观测器增益可使估计的状态变量和收敛到相对应的状态变量z_1i和z_2i。

作为一种柔性关节机械臂轨迹跟踪控制算法的优选方案，所述估计状态矢量，包括：

定义状态矢量z₁＝[z₁₁ z₁₂…z₁₅]^T，z₂＝[z₂₁ z₂₂…z₂₅]^T，则所述第一连杆和第二连杆的扩张状态观测器可分别写成矩阵形式：

式中的系数矩阵分别为：

定义所述扩张状态观测器的完整状态变量观测误差为 令即所述状态矢量z₁的观测值 δ₂＝[δ₂₁δ₂₂…δ₂₅]^T。

作为一种柔性关节机械臂轨迹跟踪控制算法的优选方案，所述利用所述状态矢量和复合扰动值设计滑模控制律，包括：

计算所述第一连杆的轨迹跟踪误差：

e₁＝q_1d-q₁＝z_11d-z₁₁

其中，q_1d和z_11d均为所述第一连杆的参考轨迹输出角位移，对所述动力学模型中的第一连杆的轨迹跟踪控制系统设计滑模函数：

其中，系数c₁₁、c₁₂和c₁₃的选择满足Hurwitz条件，表示第一连杆的轨迹跟踪误差的三阶导数，计算s₁的一阶导数：

为避免测量所述第一连杆角位移高阶导数值，利用所述状态矢量z₁的观测值和复合扰动的估计值设计所述第一连杆的滑模控制律v₁：

其中， 代表z₁₂的观测值与目标跟踪轨迹的二阶导数z_12d的偏差，代表z₁₃的观测值与目标跟踪轨迹的二阶导数z_13d的偏差，代表z₁₄的观测值与目标跟踪轨迹的二阶导数z_14d的偏差，代表s₁的一阶导数的观测值，z_12d、z_13d和z_14d分别为所述第一连杆参考轨迹的输出角速度，角加速度和角急动度，为所述复合扰动的估计值，η₁为滑模控制律的系数，为正实数，为所述第一连杆的参考轨迹输出角位移的4阶导数；

计算所述第二连杆的轨迹跟踪误差：

e₂＝q_2d-q₂＝z_21d-z₂₁

其中，q_2d和z_21d均为所述第二连杆的参考轨迹输出角位移，对所述动力学模型中的第二连杆的轨迹跟踪控制系统设计滑模函数：

其中，系数c₂₁、c₂₂和c₂₃的选择满足Hurwitz条件，计算s₂的一阶导数：

为避免测量所述第二连杆角位移高阶导数值，利用所述状态矢量z₂的观测值和复合扰动的估计值设计第二连杆的滑模控制律v₂：

其中， 代表z₂₂的观测值与目标跟踪轨迹的二阶导数z_22d的偏差，代表z₂₃的观测值与目标跟踪轨迹的二阶导数z_23d的偏差，代表z₂₄的观测值与目标跟踪轨迹的二阶导数z_24d的偏差，代表s₂的一阶导数的观测值，z_22d、z_23d和z_24d分别为所述第二连杆参考轨迹的输出角速度，角加速度和角急动度，为所述复合扰动的估计值，η₂为滑模控制律的系数，为正实数，为所述第二连杆的参考轨迹输出角位移的4阶导数。

本发明第三方面提供了一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如上述中任一项所述方法的步骤。

本发明第四方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如权利要求1-8中任一项所述方法的步骤。

有益效果：

本发明通过设计扩张状态观测器观测(估算)出柔性关节机械臂系统的完整状态变量，即连杆角位移及其高阶导数值，从而避免了传统方法中通过多次求导或模型转换求取连杆角位移高阶导数值带来的误差，提高了柔性关节机械臂的轨迹跟踪控制的精度和稳定性；本发明设计的扩张状态观测器同时将外界扰动和参数摄动作为扩张状态变量估计出来，对其进行实时补偿，提高了柔性关节机械臂系统的鲁棒性和稳定性；通过设计滑模控制律，实现了柔性关节机械臂的轨迹跟踪控制，同时也克服了传统滑模控制器的抖振问题，具有良好的动态性能；

通过利用李雅普诺夫稳定性定理证明扩张状态观测器的稳定性，证明了设计的扩张状态观测器可以观测出每一个状态变量，观测值最终能够逼近扩张状态变量；利用李雅普诺夫稳定性定证明基于扩张状态观测器的滑模控制器在柔性关节机械臂系统的稳定性，即连杆角位移可以跟踪参考轨迹输出角位移。

附图说明

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

图1是本发明实施例提供的柔性关节机械臂的轨迹跟踪控制算法流程图；

图2是本发明实施例提供的柔性关节机械臂的轨迹跟踪控制算法结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

S1：建立柔性关节机械臂系统的数学模型；

参见图1，根据柔性关节机械臂的原理，以双连杆柔性关节机械臂为例，建立柔性关节机械臂系统的数学模型，具体过程如下：

双连杆柔性关节机械臂的数学模型为：

其中，q＝[q₁ q₂]^T为连杆角位移向量，q₁为第一连杆的角位移向量，q₂为第二连杆的角位移向量，T代表转置矩阵运算，和分别为连杆角速度向量和连杆角加速度向量，θ＝[θ₁θ₂]^T为电机转子经减速器后的输出角位移向量，θ₁为电机转子经减速器后的第一连杆的输出角位移向量，θ₂为电机转子经减速器后的第二连杆的输出角位移向量，和分别为输出角速度向量和输出角加速度向量，为连杆惯性矩阵，为电机惯量矩阵，为2行2列的矩阵，为机械臂的科氏力和离心力向量，为2行一列的向量，为重力向量，为关节刚度矩阵，τ＝[τ₁τ₂]^T为控制器输出向量，也就是关节电机的输出转矩，τ₁为第一连杆的控制器输出向量，τ₂为第二连杆的控制器输出向量。

S2：基于柔性关节机械臂的数学模型，建立串联积分型状态空间方程，设计扩张状态观测器，估计出状态矢量和复合扰动值，利用李雅普诺夫稳定性定理，证明扩张状态观测器的稳定性；

基于双连杆柔性关节机械臂的数学模型，通过状态变换，建立以连杆角位移和其一阶至三阶导数值为状态变量的串联积分型状态空间方程，设计扩张状态观测器来观测复合扰动(复合扰动包括外部扰动和参数摄动)，并利用李雅普诺夫稳定性定理验证扩张状态观测器的稳定性，具体如下：

定义 为简化计算用符号，无实意，避免写的公式过于复杂，根据柔性关节机械臂系统的数学模型，进行一定的代换，把柔性关节机械臂系统的数学模型的表达式等号左边只留一个θ，其余全部移到等号右边，就得到用连杆角位移向量q的相关量表示的θ的表达式：

构建θ的二阶导数的表示式：

根据和柔性关节机械臂系统的数学模型，构建完全由连杆角位移向量q及其相关量表示的动力学模型：

其中， g(q)＝M^-1(q)KJ^-1，q⁽³⁾和q⁽⁴⁾分别代表连杆的三阶导数和四阶导数，为简化计算用符号；

在设计扩张状态观测器之前，首先令τ＝g^-1(q)v，动力学模型可重新写为由于q＝[q₁ q₂]^T, 组成了向量，动力学模型可写为：

重新定义状态变量z₁₁＝q₁,和z₂₁＝q₂, 和将和分别定义为扩张状态变量z₁₅和z₂₅，这样定义的逻辑是为了方便后面设计扩张状态观测器，并不是按照前面分别是1、2、3阶导数，到这里一定要是四阶导数。由于 ，则有假设扩张状态变量可导且其导数是有界的，即 且满足|γ₁|≤h₁,|γ₂|≤h₂，h₁和h₂为任意正实数，|γ₁|和|γ₂|是有界的，则动力学模型可重新写为串联积分型状态空间方程：

其中，z₁₁＝q₁,分别表示第一连杆角位移的0阶、1阶、2阶导数，表示第一连杆角位移的4阶导数，表示第二连杆角位移的4阶导数，z₁₅为扩张状态变量，v₁和v₂分别表示第一连杆和第二连杆的滑模控制律。

参考串联积分型状态空间方程的形式，设计两个线性扩张状态观测器分别估计出与第一连杆和第二连杆角位移相关的完整状态变量，两个线性扩张状态观测器分别设计为：

其中，是串联积分型状态空间方程中第一连杆所有相关状态变量z_1i(i＝1,2,3,4,5)的估计值，即完整状态变量的估计值，通过设计扩张状态观测器观测出柔性关节机械臂系统的完整状态变量，完整状态变量即连杆角位移及其高阶导数值，从而避免了传统方法中通过多次求导或模型转换求取连杆角位移高阶导数值带来的误差，提高了柔性关节机械臂的轨迹跟踪控制的精度和稳定性。z₁₁＝q₁,分别表示第一连杆角位移的0阶、1阶、二阶、三阶导数值，z₁₅为扩张状态变量，为复合扰动的估计值，即外界扰动和建模误差组成的扩张状态变量z₁₅的估计值，本发明设计的扩张状态观测器同时将外界扰动和建模误差作为扩张状态变量估计出来，提高了柔性关节机械臂系统的鲁棒性和稳定性。是串联积分型状态空间方程中第二连杆所有相关状态变量z_2i(i＝1,2,3,4,5)的估计值，α_1i(i＝

1,2,3,4,5)和α_2i(i＝1,2,3,4,5)分别为待设计的两个扩张状态观测器增益，选择合适的观测器增益可使估计的状态变量和收敛到相对应的状态变量z_1i和z_2i。

如图2所示，图中右边的柔性关节机械臂系统的输出为连杆角位移q＝

[q₁ q₂]^T，即[z₁₁z₁₂]^T，和为扩张状态观测器对z₁、z₂、z3、z4和z₅的观测值，将这些观测值与q_d、应用至滑模控制器的设计中，就可以避免利用传感器对这些变量的测量，有效避免误差，并利用输入τ和柔性关节机械臂系统的输出q设计出扩张状态观测器，可以得出观测到的扩张状态变量，将这些扩张状态变量的观测值应用于滑模控制器的设计中。z₁、z₂、z3、z4和z₅为各连杆相关的状态矢量，q_d、为连杆的参考轨迹输出角位移的各阶导数。

定义状态矢量z₁＝[z₁₁ z₁₂…z₁₅]^T，z₂＝[z₂₁ z₂₂…z₂₅]^T，则第一连杆和第二连杆的扩张状态观测器可分别写成矩阵形式：

其中，式中的系数矩阵分别为：

定义扩张状态观测器的完整状态变量观测误差为 令即状态矢量z₁的观测值 δ₂＝[δ₂₁ δ₂₂ …δ₂₅]^T，则第一连杆和第二连杆的扩张状态观测器状态方程式可分别写成矩阵形式为：

其中，α₁＝[α₁₁ α₁₂ … α₁₅]^T和α₂＝[α₂₁ α₂₂ … α₂₅]^T分别为两个扩张状态观测器的增益向量，T＝[1 0 0 0 0]。

令减去可得：

其中，式中矩阵G为：

矩阵G的特征方程|λI-G|＝0为：

λ⁵+α₁₁λ⁴+α₁₂λ³+α₁₃λ²+α₁₄λ+α₁₅＝0

其中，I为和G相同行数和列数的单位矩阵，λ为矩阵G的特征值，选择合适的增益α_1i(i＝1,2,3,4,5)可使矩阵G的特征方程的根全为负实部，进而使矩阵G为Hurwitz矩阵，则对于任意一个矩阵Q＝Q^T>0，存在对称正定矩阵P满足：

G^TP+PG＝-Q

定义Lyapunov函数为对其求导得：

定义λ_min为矩阵Q所有特征值中的最小值，λ_max为矩阵p所有特征值中的最大值，由于||D||＝1且扩张状态的变化率γ₁满足|γ₁|≤h₁，D为系数矩阵，||D||为其行列式，则可得：

当时有因此第一个扩张状态观测器的观测误差会收敛到集合内，δ₁包括扩张状态变量和状态矢量的观测误差，其观测误差是有界的，选择合适的增益可使该误差减小到原点附近。

由于第一连杆角位移q₁和第二连杆角位移q₂的数学模型以及所设计的两个扩张状态观测器状态方程的形式是一致的，仅观测器增益系数和扩张状态变化率不同，所以同理可得，对于第二个扩张状态观测器，选择合适的增益α_2i(i＝1,2,3,4,5)也可使观测误差δ₂有界且能够减小到原点附近。本发明实施例通过利用李雅普诺夫稳定性定理证明扩张状态观测器的稳定性，证明了设计的扩张状态观测器可以观测出每一个状态变量，即观测值最终能够逼近扩张状态变量。

S3:利用状态矢量和复合扰动值设计滑模控制律，根据滑模控制律，实现轨迹跟踪控制，利用李雅普诺夫稳定性定理，证明基于扩张状态观测器的滑模控制器在柔性关节机械臂系统的稳定性。

利用步骤S2中扩张状态观测器估计出的状态矢量和复合扰动值设计滑模控制器，使柔性关节机械臂系统始终沿着滑模面运行最终收敛至平衡状态，提高柔性关节机械臂系统的鲁棒性，实现轨迹跟踪控制，具体如下：

当q_d＝[q_1d q_2d]^T为连杆参考轨迹的输出角位移向量时，机械臂的轨迹跟踪控制目标为使轨迹跟踪误差e＝q_d-q快速收敛至0，q_d为参考轨迹输出角位移。

计算第一连杆的轨迹跟踪误差：

e₁＝q_1d-q₁＝z_11d-z₁₁

其中，q_1d和z_11d均为第一连杆的参考轨迹输出角位移，对动力学模型中的第一连杆的轨迹跟踪控制系统设计滑模函数：

其中，系数c₁₁、c₁₂和c₁₃的选择满足Hurwitz条件，表示第一连杆的轨迹跟踪误差的三阶导数，计算s₁的一阶导数：

为避免测量第一连杆角位移高阶导数值，利用状态矢量z₁的观测值和复合扰动的估计值设计第一连杆的滑模控制律v₁：

其中， 代表z₁₂的观测值与目标跟踪轨迹的二阶导数z_12d的偏差，代表z₁₃的观测值与目标跟踪轨迹的二阶导数z_13d的偏差，代表z₁₄的观测值与目标跟踪轨迹的二阶导数z_14d的偏差，代表s₁的一阶导数的观测值，z_12d、z_13d和z_14d分别为第一连杆参考轨迹的输出角速度，角加速度和角急动度，为复合扰动的估计值，η₁为滑模控制律的系数，为正实数，越大收敛的越快，但也不能太大，为第一连杆的参考轨迹输出角位移的4阶导数；

计算第二连杆的轨迹跟踪误差：

e₂＝q_2d-q₂＝z_21d-z₂₁

其中，q_2d和z_21d均为第二连杆的参考轨迹输出角位移，对动力学模型中的第二连杆的轨迹跟踪控制系统设计滑模函数：

其中，系数c₂₁、c₂₂和c₂₃的选择满足Hurwitz条件，计算s₂的一阶导数：

为避免测量第二连杆角位移高阶导数值，利用状态矢量z₂的观测值和复合扰动的估计值设计第二连杆的滑模控制律v₂：

其中， 代表z₂₂的观测值与目标跟踪轨迹的二阶导数z_22d的偏差，代表z₂₃的观测值与目标跟踪轨迹的二阶导数z_23d的偏差，代表z₂₄的观测值与目标跟踪轨迹的二阶导数z_24d的偏差，代表s₂的一阶导数的观测值，z_22d、z_23d和z_24d分别为第二连杆参考轨迹的输出角速度，角加速度和角急动度，^z₂₅为复合扰动的估计值，η₂为滑模控制律的系数，越大收敛的越快，但也不能太大，为正实数，为第二连杆的参考轨迹输出角位移的4阶导数。本发明通过设计滑模控制器，实现了柔性关节机械臂的轨迹跟踪控制，同时由于 τ＝g^-1(q)v，为连续变化，不存在输出值的突变，也就是没有传统滑模控制的抖振现象，即克服了传统滑模控制器的抖振问题，具有良好的动态性能。

在实现柔性关节机械臂系统的轨迹跟踪控制之后，证明基于扩张状态观测器的滑模控制器在柔性关节机械臂轨迹跟踪控制系统的稳定性，进行李雅普诺夫稳定性定理证明，具体如下：

首先分析第一连杆柔性关节机械臂轨迹跟踪控制系统的稳定性，定义Lyapunov函数为：

V₂的一阶导数为：

将控制律v₁代入获得：

由于 无实意，为使用李雅普诺夫定理证明稳定性用符号，写成这样就是为了使用李雅普诺夫定理证明稳定性，的值与观测器对每个状态变量的观测误差有关。同时上式中含有的项，两项相互抵消，即实时补偿。由步骤2的分析和证明可知，扩张状态观测器的观测误差有界，则可得的值有界，取任意正常数m且满足 对上式进行简化可得；

引理对于任意V:[0,∞)∈R，不等式方程的解为：

证明：根据引理，令a＝2η₁-1，b＝0.5m²，则方程的解为：

取η₁>0.5，则可得到：

选择合适的扩张状态观测器增益和足够大的η值可使也可保证的值足够小，使第一连杆轨迹跟踪误差e₁收敛至0附近一个很小的区间内，当t→∞时，e₁→0。同理可得到第二连杆的轨迹跟踪误差e₂也可以渐进收敛，证明完毕。本发明实施例利用李雅普诺夫稳定性定理证明了柔性关节机械臂系统是稳定性，即连杆角位移q可以跟踪参考轨迹输出角位移q_d。

本申请实施例提供一种计算机可读存储介质,包括程序代码,当所述程序代码在计算机上运行时,使计算机执行上述柔性关节机械臂的轨迹跟踪控制算法的步骤。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和方框图来描述的。应理解可由计算机程序指令实现流程图和方框图中的每一流程和/或方框、以及流程图和方框图中的流程和方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和方框图一个方框或多个方框中指定的功能的步骤。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (8)

1.一种柔性关节机械臂轨迹跟踪控制算法，其特征在于，包括以下步骤：

建立柔性关节机械臂系统的数学模型；

基于所述柔性关节机械臂的数学模型，建立串联积分型状态空间方程，设计扩张状态观测器，估计出状态矢量和复合扰动值，利用李雅普诺夫稳定性定理，证明所述扩张状态观测器的稳定性；

利用所述状态矢量和复合扰动值设计滑模控制律，根据所述滑模控制律，实现轨迹跟踪控制，利用李雅普诺夫稳定性定理，证明所述基于扩张状态观测器的滑模控制器在柔性关节机械臂系统的稳定性。

2.根据权利要求1所述的柔性关节机械臂的轨迹跟踪控制算法，其特征在于，所述柔性关节机械臂系统的数学模型包括：

其中，q＝[q₁ q₂]^T为连杆角位移向量，q₁为第一连杆的角位移向量，q₂为第二连杆的角位移向量，T代表转置矩阵运算，和分别为连杆角速度向量和连杆角加速度向量，θ＝[θ₁θ₂]^T为电机转子经减速器后的输出角位移向量，θ₁为电机转子经减速器后的第一连杆的输出角位移向量，θ₂为电机转子经减速器后的第二连杆的输出角位移向量，和分别为输出角速度向量和输出角加速度向量，为连杆惯性矩阵，为电机惯量矩阵，为2行2列的矩阵，为机械臂的科氏力和离心力向量，为2行一列的向量，为重力向量，为关节刚度矩阵，τ＝[τ₁τ₂]^T为控制器输出向量，τ₁为第一连杆的控制器输出向量，τ₂为第二连杆的控制器输出向量。

3.根据权利要求2所述的柔性关节机械臂的轨迹跟踪控制算法，其特征在于，所述建立串联积分型状态空间方程，包括：

定义 为简化计算用符号，无实意，根据所述柔性关节机械臂系统的数学模型，构建θ及θ的二阶导数的表示式：

根据和所述柔性关节机械臂系统的数学模型，构建动力学模型：

其中，

q⁽³⁾和q⁽⁴⁾分别代表连杆的三阶导数和四阶导数，为简化计算用符号；

令τ＝g^-1(q)v，所述动力学模型可写为由于 所述动力学模型可写为：

重新定义状态变量z₁₁＝q₁,和z₂₁＝q₂, 和将和分别定义为扩张状态变量z₁₅和z₂₅，假设扩张状态变量可导且其导数是有界的，即且满足|γ₁|≤h₁,|γ₂|≤h₂，h₁和h₂为任意正实数，则所述动力学模型可重新写为串联积分型状态空间方程：

其中，z₁₁＝q₁,分别表示所述第一连杆角位移的0阶、1阶、2阶导数，表示所述第一连杆角位移的4阶导数，表示所述第二连杆角位移的4阶导数，z₁₅为所述扩张状态变量，v₁和v₂分别表示第一连杆和第二连杆的滑模控制律。

4.根据权利要求2或3所述的柔性关节机械臂的轨迹跟踪控制算法，其特征在于，所述设计扩张状态观测器，包括：

根据所述串联积分型状态空间方程，分别设计所述第一连杆和第二连杆的扩张状态观测器：

其中，是所述串联积分型状态空间方程中第一连杆所有相关状态变量z_1i(i＝1,2,3,4,5)的估计值，z₁₅为所述扩张状态变量，即为复合扰动的估计值，复合扰动包括外部扰动和参数摄动，是所述串联积分型状态空间方程中第二连杆所有相关状态变量z_2i(i＝1,2,3,4,5)的估计值，α_1i(i＝1,2,3,4,5)和α_2i(i＝1,2,3,4,5)分别为待设计的两个所述扩张状态观测器增益，选择合适的观测器增益可使估计的状态变量和收敛到相对应的状态变量z_1i和z_2i。

5.根据权利要求4所述的柔性关节机械臂的轨迹跟踪控制算法，其特征在于，所述估计状态矢量，包括：

定义状态矢量z₁＝[z₁₁ z₁₂…z₁₅]^T，z₂＝[z₂₁ z₂₂…z₂₅]^T，则所述第一连杆和第二连杆的扩张状态观测器可分别写成矩阵形式：

式中的系数矩阵分别为：

定义所述扩张状态观测器的完整状态变量观测误差为 令即所述状态矢量z₁的观测值 δ₂＝[δ₂₁δ₂₂…δ₂₅]^T。

6.根据权利要求1所述的柔性关节机械臂的轨迹跟踪控制算法，其特征在于，所述利用所述状态矢量和复合扰动值设计滑模控制律，包括：

计算所述第一连杆的轨迹跟踪误差：

e₁＝q_1d-q₁＝z_11d-z₁₁

其中，q_1d和z_11d均为所述第一连杆的参考轨迹输出角位移，对所述动力学模型中的第一连杆的轨迹跟踪控制系统设计滑模函数：

其中，系数c₁₁、c₁₂和c₁₃的选择满足Hurwitz条件，表示第一连杆的轨迹跟踪误差的三阶导数，计算s₁的一阶导数：

为避免测量所述第一连杆角位移高阶导数值，利用所述状态矢量z₁的观测值和复合扰动的估计值设计所述第一连杆的滑模控制律v₁：

其中， 代表z₁₂的观测值与目标跟踪轨迹的二阶导数z_12d的偏差，代表z₁₃的观测值与目标跟踪轨迹的二阶导数z_13d的偏差，代表z₁₄的观测值与目标跟踪轨迹的二阶导数z_14d的偏差，代表s₁的一阶导数的观测值，z_12d、z_13d和z_14d分别为所述第一连杆参考轨迹的输出角速度，角加速度和角急动度，为所述复合扰动的估计值，η₁为滑模控制律的系数，为正实数，为所述第一连杆的参考轨迹输出角位移的4阶导数；

计算所述第二连杆的轨迹跟踪误差：

e₂＝q_2d-q₂＝z_21d-z₂₁

其中，q_2d和z_21d均为所述第二连杆的参考轨迹输出角位移，对所述动力学模型中的第二连杆的轨迹跟踪控制系统设计滑模函数：

其中，系数c₂₁、c₂₂和c₂₃的选择满足Hurwitz条件，计算s₂的一阶导数：

为避免测量所述第二连杆角位移高阶导数值，利用所述状态矢量z₂的观测值和复合扰动的估计值设计第二连杆的滑模控制律v₂：

其中， 代表z₂₂的观测值与目标跟踪轨迹的二阶导数z_22d的偏差，代表z₂₃的观测值与目标跟踪轨迹的二阶导数z_23d的偏差，代表z₂₄的观测值与目标跟踪轨迹的二阶导数z_24d的偏差，代表s₂的一阶导数的观测值，z_22d、z_23d和z_24d分别为所述第二连杆参考轨迹的输出角速度，角加速度和角急动度，为所述复合扰动的估计值，η₂为滑模控制律的系数，为正实数，为所述第二连杆的参考轨迹输出角位移的4阶导数。

7.一种电子设备，其特征在于，包括：

存储器，用于存放计算机程序；

处理器，用于执行所述存储器上所存放的计算机程序时，实现权利要求1-6中任一项所述的方法步骤。

8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-6中任一项所述的方法步骤。