CN107065564B - 一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法 - Google Patents

Abstract

本发明公开了一种基于自抗扰理论的中性浮力机器人姿态与轨迹控制方法，针对中性浮力系统中机器人抗干扰能力差以及有限字长引起的舍入误差问题，利用delta算子理论，构建中性浮力系统delta域动力学状态方程。设计delta域跟踪微分器，安排输入信号的过渡过程。针对中性浮力系统中水的阻力以及各控制力之间的耦合项设计delta域扩张状态观测器。利用跟踪微分器的输出状态与扩张状态观测器的估计状态设计delta域复合抗干扰控制器，完成中性浮力系统中机器人姿态与轨迹控制。本方法利用扩张状态观测器估计中性浮力系统中总干扰以及系统内非线性耦合项，提高了系统抗干扰能力，有效减小了系统参数有限字长的舍入误差，可用于微重力环境下机器人姿态与轨迹高精度控制。

Description

一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法

技术领域

本发明属于微重力机器人控制领域，涉及一种基于自抗扰理论的中性浮力机器人姿态与轨迹控制方法，特别是针对在复杂水下环境中运行的中性浮力机器人自抗扰控制方法。

背景技术

作为地面验证空间技术的关键平台，微重力模拟系统一直以来受到国内外的广泛关注。在微重力模拟系统中，由于中性浮力系统可以提供长时间、大范围、高精度的微重力环境，因此，中性浮力系统受到了广泛研究。然而，在中性浮力系统中，机器人工作于水下环境，不仅各个控制力之间相互耦合，而且机器人严重受到水的黏性阻力影响。此外，在实际数字控制系统中，有限字长表示将不可避免地导致系统参数的舍入误差。以上这些因素会严重影响中性浮力机器人的姿态与轨迹控制。因此，在中性浮力机器人姿态与轨迹控制方法设计中，寻求一种主动抗干扰同时减小舍入误差的控制方法显得尤为重要。

当前由于自抗扰控制方法具有精度高、反应快、抗干扰能力强等优点，广泛应用于电力系统、磁性无杆气缸系统、四旋翼系统、坦克火炮系统等。一般来说，自抗扰控制方法包含三部分：跟踪微分器，扩张状态观测器和反馈控制器。其中，跟踪微分器用以设计输入信号的过渡过程，减小系统超调；扩张状态观测器用以估计系统受到的总干扰以及系统内的非线性耦合项，并将其扩张为一个新的状态；反馈控制器利用扩张状态观测器估计的状态使整体闭环系统稳定。因此，自抗扰控制方法可以有效提高中性浮力机器人的抗干扰能力。此外，delta算子方法可以有效减小有限字长的舍入误差，提高中性浮力系统的控制精度。本发明专利提出了一种把自抗扰控制器与delta算子理论相结合的delta算子自抗扰控制器，既能保证中性浮力机器人的姿态与轨迹跟踪控制稳定，又可以提高控制精度。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，本发明提出了一种基于delta算子的自抗扰控制方法，充分利用自抗扰控制器抗干扰的能力以及delta算子方法在舍入误差方面的优势，提高中性浮力系统的稳定性与控制精度。

本发明采用以下技术方案：

一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，先利用delta算子理论，构建中性浮力系统delta域动力学状态方程；通过delta域跟踪微分器安排输入信号的过渡过程；针对中性浮力系统中水的阻力以及各控制力之间的耦合项确定delta域扩张状态观测器；利用跟踪微分器的输出状态与扩张状态观测器的估计状态，设计复合抗干扰控制器，完成中性浮力系统中机器人姿态与轨迹控制。

进一步的，具体步骤如下：

S1、根据地理坐标系Ox_ny_nz_n、机器人体坐标系Ox_by_bz_b，以及中性浮力系统在体坐标系下的动力学模型得到系统模型，根据delta算子定义，构建中性浮力系统delta域动力学状态模型；

S2、建立delta域跟踪微分器如下：

其中，v₀为给定输入信号，v₁(t_k)为v₀的跟踪输出信号，v₂(t_k)为v₁(t_k)的微分信号，r₀∈R⁶和h₀∈R⁶为可调参数，F_han表示非线性函数；

S3、建立delta域扩张状态观测器模型如下：

其中，

和

分别是x₁(t_k)，x₂(t_k)和f(t_k)的观测值；β₁，β₂和β₃为观测器的可调参数，b为一个可调节的常数对角矩阵，u(t_k)为系统输入；

S4、定义跟踪微分器与扩张状态观测器的误差信号，建立基于跟踪微分器与扩张状态观测器的复合控制器，完成中性浮力系统中机器人姿态与轨迹控制。

进一步的，步骤S1中，所述中性浮力系统delta域动力学状态模如下：

δx₁(t_k)＝x₂(t_k),

δx₂(t_k)＝bu(t_k)+f(t_k)

其中，b为一个可调节的常数对角矩阵，令f(t_k)＝(b_t-b)u(t_k)+d(t_k)作为系统中的总干扰，令x₁＝η，

u＝τ，t_k表示系统运行步数。

进一步的，所述系统模型如下：

其中，J(η)为运动系数矩阵，M_η(η)＝J^-T(η)MJ^-1(η)，

D_η(η,v)＝J^-T(η)D(v)J^-1(η)，g_η(η)＝J^-T(η)g(η)。

进一步的，所述中性浮力系统在体坐标系下的动力学模型如下：

其中，M为惯性质量矩阵，C(v)为科里奥利力矩阵，D(v)为机器人在水中受到的黏性阻力，g(η)为负浮力系数，τ为系统输入。

进一步的，所述地理坐标系Ox_ny_nz_n和机器人体坐标系Ox_by_bz_b的关系如下：

其中，

分别指机器人在Ox_n、Oy_n和Oy_z方向的位置；

分别指机器人的横滚角，俯仰角以及偏航角，

为机器人线速度向量，

为机器人角速度向量，

J(η)为运动系数矩阵。

进一步的，步骤S2中，所述非线性函数F_han定义如下：

F_han(v₁(t_k)-v₀,v₂(t_k),r₀,h₀)＝col₆{f_han(v_1i(t_k)-v₀,v_2i(t_k),r_0i,h_0i)}

其中，col_N{x_i}指列向量[x₁,x₂,…,x_N]^T；v_1i(t_k)，v_2i(t_k)，r_0i，h_0i分别是列向量v₁(t_k)，v₂(t_k)，r₀，h₀中的第i个元素，f_han为非线性函数。

进一步的，所述f_han函数定义如下：

其中，d＝r_0ih_0i，

d₀＝h_0id，

d₁＝v_1i(t_k)-v_0i+h_0iv_2i(t_k)，sign(·)为符号函数：如果a＞0，sign(a)＝1；a＝0，sign(a)＝0；a＜0，sign(a)＝-1。

进一步的，步骤S4中，所述基于跟踪微分器与扩张状态观测器的复合控制器为：

其中，b为一个可调节的常数对角矩阵，k₁和k₂为控制器的可调参数。

进一步的，所述跟踪微分器与扩张状态观测器的误差信号为：

其中，r₁(t_k)为v₁(t_k)与

误差值，r₂(t_k)为v₂(t_k)与

误差值。

与现有技术相比，本发明至少具有以下有益效果：

本发明一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，首先利用delta算子理论，构建中性浮力系统delta域动力学状态方程；其次，设计delta域跟踪微分器，安排输入信号的过渡过程，减小系统超调量；再次，针对中性浮力系统中水的黏性阻力以及各控制力之间的耦合项，设计delta域扩张状态观测器，将水的黏性阻力以及各控制力之间的耦合项作为系统的总干扰，并进行估计，降低系统的非线性特性；最后，利用跟踪微分器的输出状态与扩张状态观测器的估计状态，设计复合抗干扰控制器，完成中性浮力系统中机器人姿态与轨迹控制。利用扩张状态观测器估计中性浮力系统中的非线性耦合项以及系统所受到的总干扰，既提高了中性浮力系统的抗干扰能力，又有效解决了中性浮力系统中的非线性耦合项。

进一步的，根据地理坐标系、机器人体坐标系，以及中性浮力系统在体坐标系下的动力学模型得到系统模型，根据delta算子定义，构建中性浮力系统delta域动力学状态模型，建立delta域跟踪微分器及delta域扩张状态观测器模型；定义跟踪微分器与扩张状态观测器的误差信号，建立基于跟踪微分器与扩张状态观测器的复合控制器，完成中性浮力系统中机器人姿态与轨迹控制。利用delta算子理论，减小了中性浮力系统参数的有限字长舍入误差，提高了系统姿态与轨迹控制的稳定性与精度。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明基于delta算子理论自抗扰复合控制器设计的流程图；

图2为本发明delta域自抗扰复合控制器的结构图。

具体实施方式

本发明提供了一种基于自抗扰理论的中性浮力机器人姿态与轨迹控制方法，针对中性浮力系统中机器人抗干扰能力差以及有限字长引起的舍入误差问题，首先利用delta算子理论，构建中性浮力系统delta域动力学状态方程。其次，设计delta域跟踪微分器，安排输入信号的过渡过程。再次，针对中性浮力系统中水的阻力以及各控制力之间的耦合项，设计delta域扩张状态观测器。最后，利用跟踪微分器的输出状态与扩张状态观测器的估计状态，设计复合抗干扰控制器，完成中性浮力系统中机器人姿态与轨迹控制。

请参阅图1，本发明具体实现步骤如下：

1.构建中性浮力系统delta域运动学模型

建立地理坐标系Ox_ny_nz_n：

分别指机器人在Ox_n、Oy_n和Oy_z方向的位置；

分别指机器人的横滚角，俯仰角以及偏航角。建立实验体坐标系Ox_by_bz_b：

为线速度向量，

为角速度向量。根据Ox_ny_nz_n与Ox_by_bz_b的关系，建立如下等式：

其中，

J(η)为运动系数矩阵。

构建中性浮力系统在体坐标系下的动力学模型：

式中，M为惯性质量矩阵，C(v)为科里奥利力矩阵，D(v)为机器人在水中受到的黏性阻力，g(η)为负浮力系数，τ为系统输入。联立式(1)和(2)，可得如下模型：

式中，M_η(η)＝J^-T(η)MJ^-1(η)，

D_η(η,v)＝J^-T(η)D(v)J^-1(η)，g_η(η)＝J^-T(η)g(η)。

令x₁＝η，

u＝τ和y＝x₁，系统模型(3)可以进一步化简为状态空间形式：

式中，

利用零阶保持器技术，系统方程(4)可以离散为以下形式：

x(t_k+1)＝A_zx(t_k)+B_zu(t_k)+F_zd(t_k)+O(T_s ²) (5)

其中，

T_s为采样周期。

根据delta算子定义，离散系统模型(5)在delta域下重新表示为：

在快速采样协议下，O(T_s)趋近于0，因此这一项可以省略。式(6)中的δx₂(t_k)可以写为：

δx₂(t_k)＝bu(t_k)+(b_t-b)u(t_k)+d(t_k)

式中，b为一个可调节的常数对角矩阵，可以认为(b_t-b)u(t_k)是一个新的干扰项。令f(t_k)＝(b_t-b)u(t_k)+d(t_k)作为系统中的总干扰，则系统(6)变为：

δx₁(t_k)＝x₂(t_k),

δx₂(t_k)＝bu(t_k)+f(t_k)

这里，可以通过调节系数b接近于b_t，来提高扩张状态观测器的估计性能，t_k表示系统运行步数。在本实例中，只使用Ox_n和Ox_y方向上的螺旋桨，来保证状态

和

稳定。因此，选取b的值为b＝diag{110110}。

第二步：设计delta域跟踪微分器

设

为给定输入信号，在本实例中，v₀(t_k)的取值为：

设v₁(t_k)为v₀(t_k)的跟踪输出信号，v₂(t_k)为v₁(t_k)的微分信号，delta域跟踪微分器设计如下：

式中，r₀∈R⁶和h₀∈R⁶是可调参数，F_han函数定义如下：

F_han(v₁(t_k)-v₀,v₂(t_k),r₀,h₀)＝col₆{f_han(v_1i(t_k)-v₀,v_2i(t_k),r_0i,h_0i)}

式中，col_N{x_i}指列向量[x₁,x₂,…,x_N]^T；v_1i(t_k)，v_2i(t_k)，r_0i，h_0i分别是列向量v₁(t_k)，v₂(t_k)，r₀，h₀中的第i个元素；f_han非线性函数定义如下：

式中，d＝r_0ih_0i，

d₀＝h_0id，

d₁＝v_1i(t_k)-v_0i+h_0iv_2i(t_k)，sign(·)为符号函数：如果a＞0，sign(a)＝1；a＝0，sign(a)＝0；a＜0，sign(a)＝-1。在本实例中。可调参数r₀和h₀选取的值为：

r₀＝[100 100 0 100 100 0]^T

h₀＝[0.1 0.1 0 0.1 0.1 0]^T

第三步：设计delta域扩张状态观测器

delta域扩张状态观测器用来估计中性浮力系统中的非线性耦合项以及系统受到的总干扰。delta域扩张状态观测器模型如下：

式中，

和

分别是x₁(t_k)，x₂(t_k)和f(t_k)的观测值；β₁，β₂和β₃是该观测器的可调参数，b为一个可调节的常数对角矩阵，u(t_k)为系统输入。

在本实例中，β₁，β₂和β₃的具体值分别为：

β₁＝diag{10 10 0 10 10 0}

β₂＝diag{10 10 0 10 10 0}

β₃＝diag{10 10 0 10 10 0}

第四步：设计基于状态观测器的复合控制器

定义跟踪微分器与扩张状态观测器的误差信号为：

其中，r₁(t_k)为v₁(t_k)与

误差值，r₂(t_k)为v₂(t_k)与

误差值。

基于跟踪微分器与扩张状态观测器的复合控制器为：

其中，k₁，k₂为控制器的可调参数。在本实例中，k₁和k₂的具体值分别为：

请参阅图2，delta域自抗扰控制器主要包括三部分：delta域跟踪微分器，delta域扩张状态观测器以及复合控制器，delta域跟踪微分器输出v₀(t_k)的跟踪输出信号v₁(t_k)，以及输出v₁(t_k)的微分信号v₂(t_k)。

delta域扩张状态观测器根据被控对象的输出y(t_k)，控制输入u(t_k)和可调参数b，估计系统的状态以及总干扰。

具体来说，

和

分别为状态x₁(t_k)和x₂(t_k)的估计值；

为总干扰f(t_k)的估计值。

最后，复合控制器利用delta域跟踪微分器的输出信号v₁(t_k)，v₂(t_k)与delta域扩张状态观测器的输出信号

和

设计控制输入u(t_k)，完成对被控对象的控制。

本方法利用扩张状态观测器估计中性浮力系统中总干扰以及系统内非线性耦合项，提高了系统抗干扰能力。此外，利用delta算子方法，有效减小了系统参数有限字长的舍入误差，可用于微重力环境下机器人姿态与轨迹高精度控制。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (10)

1.一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，先利用delta算子理论，构建中性浮力系统delta域动力学状态方程；通过delta域跟踪微分器安排输入信号的过渡过程；针对中性浮力系统中水的阻力以及各控制力之间的耦合项确定delta域扩张状态观测器；利用跟踪微分器的输出状态与扩张状态观测器的估计状态，设计复合抗干扰控制器，完成中性浮力系统中机器人姿态与轨迹控制。

2.根据权利要求1所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，具体步骤如下：

S1、根据地理坐标系Ox_ny_nz_n、机器人体坐标系Ox_by_bz_b，以及中性浮力系统在体坐标系下的动力学模型得到系统模型，根据delta算子定义，构建中性浮力系统delta域动力学状态模型；

S2、建立delta域跟踪微分器如下：

其中，v₀为给定输入信号，v₁(t_k)为v₀的跟踪输出信号，v₂(t_k)为v₁(t_k)的微分信号，r₀∈R⁶和h₀∈R⁶为可调参数，F_han表示非线性函数；

S3、建立delta域扩张状态观测器模型如下：

其中，

和

分别是x₁(t_k)，x₂(t_k)和f(t_k)的观测值；β₁，β₂和β₃为观测器的可调参数，b为一个可调节的常数对角矩阵，u(t_k)为系统输入；

S4、定义跟踪微分器与扩张状态观测器的误差信号，建立基于跟踪微分器与扩张状态观测器的复合控制器，完成中性浮力系统中机器人姿态与轨迹控制。

3.根据权利要求2所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，步骤S1中，所述中性浮力系统delta域动力学状态模型如下：

δx₁(t_k)＝x₂(t_k),

δx₂(t_k)＝bu(t_k)+f(t_k)

其中，b为一个可调节的常数对角矩阵，令f(t_k)＝(b_t-b)u(t_k)+d(t_k)作为系统中的总干扰，令x₁＝η，

u＝τ，t_k表示系统运行步数。

4.根据权利要求3所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，建立地理坐标系Ox_ny_nz_n：

分别指机器人在Ox_n、Oy_n和Oy_z方向的位置；

分别指机器人的横滚角，俯仰角以及偏航角；建立实验体坐标系Ox_by_bz_b：

为线速度向量，

为角速度向量，

得到所述系统模型如下：

其中，J(η)为运动系数矩阵，M_η(η)＝J^-T(η)MJ^-1(η)，

D_η(η,v)＝J^-T(η)D(v)J^-1(η)，g_η(η)＝J^-T(η)g(η)。

5.根据权利要求4所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，所述中性浮力系统在体坐标系下的动力学模型如下：

其中，M为惯性质量矩阵，C(v)为科里奥利力矩阵，D(v)为机器人在水中受到的黏性阻力，g(η)为负浮力系数，τ为系统输入。

6.根据权利要求5所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，所述地理坐标系Ox_ny_nz_n和机器人体坐标系Ox_by_bz_b的关系如下：

其中，

分别指机器人在Ox_n、Oy_n和Oy_z方向的位置；

分别指机器人的横滚角，俯仰角以及偏航角，

为机器人线速度向量，

为机器人角速度向量，

J(η)为运动系数矩阵。

7.根据权利要求2所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，步骤S2中，所述非线性函数F_han定义如下：

F_han(v₁(t_k)-v₀,v₂(t_k),r₀,h₀)＝col₆{f_han(v_1i(t_k)-v₀,v_2i(t_k),r_0i,h_0i)}

其中，col_N{x_i}指列向量[x₁,x₂,…,x_N]^T；v_1i(t_k)，v_2i(t_k)，r_0i，h_0i分别是列向量v₁(t_k)，v₂(t_k)，r₀，h₀中的第i个元素，f_han为非线性函数。

8.根据权利要求7所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，所述f_han函数定义如下：

其中，d＝r_0ih_0i，

d₀＝h_0id，

d₁＝v_1i(t_k)-v_0i+h_0iv_2i(t_k)，sign(·)为符号函数：如果a＞0，sign(a)＝1；a＝0，sign(a)＝0；a＜0，sign(a)＝-1。

9.根据权利要求2所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，步骤S4中，所述基于跟踪微分器与扩张状态观测器的复合控制器为：

其中，u₀(t_k)为线性误差反馈项。

10.根据权利要求9所述的一种基于自抗扰的中性浮力机器人姿态与轨迹控制方法，其特征在于，所述跟踪微分器与扩张状态观测器的误差信号为：

其中，r₁(t_k)为v₁(t_k)与

误差值，r₂(t_k)为v₂(t_k)与

误差值。

Images