CN104656446B - 一种基于干扰观测器的混合悬浮实验体控制方法 - Google Patents

Abstract

本发明公开了一种基于干扰观测器的混合悬浮实验体控制方法，本发明针对于混合悬浮实验体的模型参数未知的情况下，在传统PID控制方法基础上，采用自适应的思想而设计的自适应PD控制律。而干扰观测器是用于在线估计有界的非线性外部干扰。综合以上所描述的方法，最终确定的基于干扰观测器的自适应控制方法则能很好地解决以上还存在的问题，本发明算法能够使控制精度远远高于传统PID控制，且具有很好的收敛性。

Description

一种基于干扰观测器的混合悬浮实验体控制方法

技术领域

本发明涉及一种混合悬浮实验体技术领域，特别是一种基于干扰观测器的混合悬浮实验体控制方法。

背景技术

混合悬浮环境的主要应用是微重力效应模拟实验，用于在地面模拟空间微重力环境。实验体在此环境中受到浮力、电磁力、重力的共同作用处于悬浮状态，因此其所处空间称为混合悬浮环境。主要方法是将被试对象全部浸没在水中，先粗略调整配重或漂浮器的浮力，再通过电磁力精确配平，克服被试对象的重力，使其漂浮于水中，近似模拟微重力效应。该方法具有六自由度三维模拟空间，可长时间连续实验。

实验体处于这样的复杂环境中，由于电磁力和水动力不能精确得到，且存在水波干扰等，致使实验体动力学特性复杂而且带有不确定性。而微重力实验中，要完成既定的实验任务和操作动作，必须实现实验体的精确控制。因此，实验体在这种具有复杂力学特性的干扰环境中的精确控制问题必须解决。

目前，传统的PID控制由于原理简单、适用性好、使用方便等,在控制中得到广泛的应用,但PID控制难以处理未知的外部干扰，且对复杂的非线性控制系统表现一般。而混合悬浮实验体的数学模型是强非线性的，而且不容易得到精确的数学模型参数。混合悬浮实验体实际运行的环境是复杂多变的，容易存在各种的外部干扰对混合悬浮实验体的运动控制造成影响。

综上所述，混合悬浮实验体的运动控制还存在以下两个问题：1、混合悬浮实验体的模型参数难以获取；2、由于外部干扰的不确定对混合悬浮实验体的运动控制造成一定的影响。

发明内容

为解决现有技术存在的问题，本发明提出一种基于干扰观测器的混合悬浮实验体控制方法。本方法则能很好地解决以上还存在的问题，本发明算法能够使控制精度远远高于传统PID控制，且具有很好的收敛性。

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

一种基于干扰观测器的混合悬浮实验体控制方法，包括以下步骤：

1)对航行任务进行规划，预设航行轨迹η_d；

2)将实验体放入实验环境中，上电准备好后，测量系统的各个模块进行对航行器的位置，姿态及速度进行测量，并将测量值η(0)，v(0)发送到上位机；

3)上位机通过接受到的测量系统的测量信息η(0)，v(0)，解算航行的轨迹误差向量其中并赋予系统未知参数矩阵的初始值与干扰观测器的初始值d(0)，根据以下自适应PD控制律进行控制力解算，

然后将解算得到的控制力通过该模型的推力分配矩阵解算，得出每个推进器的推力值，并将各个推进器的控制推力通过串口发送到下位机；

当B不可逆时，

4)下位机接受到上位机发送的控制指令，电机控制模块通过对控制力信号进行分析解算，输出一个与控制力所对应的PWM波到电机，完成整个对电机的推力控制；

5)混合悬浮实验体开始运动时，上位机接收到测量系统测量到的新的实际值η(k)，v(k)，按照步骤(3)对系统的跟踪误差s(k)进行更新，同时更新新的控制力τ(k)，并通过模型的推力分配矩阵将新的控制力τ(k)解算得到T(k)，将得到的新的T(k)发送到下位机；

6)根据实验的反馈数据，通过改变系统的误差控制增益矩阵K，系统未知参数估计的增益矩阵K_θ及系统的干扰观测的增益矩阵K_d，观察变化后的跟踪误差轨迹，做出适当的对系统各个增益矩阵K,K₁,K₂的调整；

7)重复进行上述步骤2)～7)，使航行路线达到最优，跟踪误差达到最小。

作为本发明的进一步改进，步骤3)中所述控制律是通过以下步骤得到的：

2.1)选取误差向量其中η＝[x y z]^T为混合悬浮实验体在惯性坐标系下的位置矩阵；v＝[u v w]^T为混合悬浮实验体在雷体坐标系下的速度矩阵，且位置误差速度误差取期望速度

2.2)在理想状态下，建立混合悬浮实验体模型：

其中的M是系统的惯性阵；C(v)是系统的哥氏力矩阵；D(v)为系统正定对称的水阻力系数矩阵；G(η)为系统由系统重浮心位置产生的恢复力矩阵；τ＝BT为系统的外部控制输入；B为实际模型的推力分配矩阵；T是由混合悬浮实验体上各个推进器的推力组成的推力矩阵；

利用自适应的思想，(2)式则可写成：

其中，是实际可测量得到的参数矩阵，θ为系统未知参数矩阵；

加入有界的外部干扰d(t)时，则上式(3)可写成：

2.3)设计控制律，取：

其中，K∈R^6×6，K_θ和K_d分别为可调的正定的PD控制增益矩阵，系统参数估计的增益矩阵和干扰观测器的增益矩阵；Γ、Λ为一个适当的正定矩阵，为系统未知参数的在线估计，为外部干扰的观测器的观测值。

作为本发明的进一步改进，步骤2.3)设计控制律中还包括证明所述控制律稳定性的步骤，具体步骤如下：

3.1)根据李雅普诺夫稳定性原理，选取候选李雅普诺夫函数：

对候选函数V₀求关于时间的导数，则：

3.2)系统模型参数的估计的稳定性，选取新的利亚普诺夫函数：

对V₁逐项求关于时间的导数，则：

3.3)干扰观测器的观测值的稳定性，最终选取利亚普诺夫函数：

对V逐项求关于时间的导数，则：

3.4)将控制律式(5)带入(11)式中，合并可得：

其中K₁＝ΓK_θ，K₂＝ΛK_d，且

因为系统未知参数θ与系统外部干扰d(t)均有界，即||θ||≤δ₁，||d(t)||≤δ₂，其中δ₁与δ₂均为正常数，则且D(v)正定，选取适当的正定的增益矩阵K,K₁,K₂；由上式(12)可得：

由(14)式得，李雅普诺夫函数V是正定有界的，即存在t，使得t→∞时，因此可知闭环系统的轨迹误差项s，系统未知参数误差及系统外部干扰与干扰观测器之间存在的误差均是有界的，则该控制律是稳定的。

作为本发明的进一步改进，所述预设轨迹η_d为一阶可导的且有界的。

相对于现有技术，本发明的有益效果是：本发明提出一种基于干扰观测器的自适应控制方法。针对于混合悬浮实验体的模型参数未知的情况下，在传统PID控制方法基础上，采用自适应的思想而设计的自适应PD控制律。而干扰观测器是用于在线估计有界的非线性外部干扰。本方法则能很好地解决以上还存在的问题，本发明算法能够使控制精度远远高于传统PID控制，且具有很好的收敛性。本次发明所设计的控制方法适用于在实验体模型参数未知的情况下，且对于存在外界干扰有良好的控制效果，较于传统的PID控制器，本方法快速性更好，收敛性更高。在螺旋形椭圆跟踪轨迹时，实验体实际轨迹与预设轨迹基本重合，误差范围不大于3cm，而在对接过程中，对接后当实验体带动目标体时，实验体本体参数发生了很大程度的改变，而这种自适应控制律则能较为快速的稳定下来，且跟踪误差不大于3cm。

附图说明

图1是螺旋形椭圆轨迹图；

图2是椭圆跟踪轨迹的位置误差图；

图3是椭圆跟踪轨迹的姿态误差图；

图4是对接轨迹图；

图5是对接跟踪轨迹的位置误差图；

图6是对接跟踪轨迹的姿态误差图；

具体实施方式

下面结合附图举例对本发明的具体实施作进一步描述。

本发明算法能够使控制精度远远高于传统PID控制，且具有很好的收敛性。具体步骤如下：

步骤1：航行开始前，预设航行轨迹η_d，预设轨迹η_d为一阶可导的且有界的；

步骤2：选取误差向量其中η＝[x y z]^T为混合悬浮实验体在惯性坐标系下的位置矩阵；v＝[u v w]^T为混合悬浮实验体在雷体坐标系下的速度矩阵。且位置误差速度误差取期望速度

步骤3：在理想状态下，建立混合悬浮实验体模型：

其中的M是系统的惯性阵；C(v)是系统的哥氏力矩阵；D(v)为系统正定对称的水阻力系数矩阵；G(η)为系统由系统重浮心位置产生的恢复力矩阵；τ＝BT为系统的外部控制输入；B为实际模型的推力分配矩阵；T是由混合悬浮实验体上各个推进器的推力组成的推力矩阵。由于实际模型参数往往很难精确得到，则利用自适应的思想，(2)式则可写成：

其中，是实际可测量得到的参数矩阵，θ为系统未知参数矩阵。但在实际的运行环境中是存在着一些不确定的外部干扰的，所以在加入有界的外部干扰d(t)时，则上式(3)可写成：

步骤4：设计控制律，取：

其中，K∈R^6×6，K_θ和K_d分别为可调的正定的PD控制增益矩阵，系统参数估计的增益矩阵和干扰观测器的增益矩阵，Γ，Λ为一个适当的正定矩阵。为系统未知参数的在线估计，为外部干扰的观测器的观测值。为证明此控制律的稳定性，根据李雅普诺夫稳定性原理。若选取候选李雅普诺夫函数：

对候选函数V₀求关于时间的导数，则：

考虑到系统模型参数的估计的稳定性，选取新的利亚普诺夫函数

对V₁逐项求关于时间的导数，则：

考虑到干扰观测器的观测值的稳定性，最终选取利亚普诺夫函数

对V逐项求关于时间的导数，则：

将控制律式(5)带入(11)式中，合并可得：

其中K₁＝ΓK_θ，K₂＝ΛK_d，且

因为系统未知参数θ与系统外部干扰d(t)均有界，即||θ||≤δ₁，||d(t)||≤δ₂，其中δ₁与δ₂均为正常数，则且D(v)正定，选取适当的正定的增益矩阵K,K₁,K₂。由上式(12)可得：

由(14)式可知，李雅普诺夫函数V是正定有界的，即存在t，使得t→∞时，因此可知闭环系统的轨迹误差项s，系统未知参数误差及系统外部干扰与干扰观测器之间存在的误差均是有界的，该控制律是稳定的。

步骤5：在设计好控制律后，根据实际模型的推力分配矩阵，分配到不同的电机上，且根据实际的实验环境，及时调整增益矩阵K,K₁,K₂使得系统能够快速，稳定地跟踪上所预设轨迹。

步骤6：重复步骤5，对增益矩阵K,K₁,K₂取不同的值，最终确定最优的增益矩阵，使得系统稳定。

实施例1

(1)航行开始前，对混合悬浮实验体进行上电自检，测试上，下位机是否通讯正常，检验实验体各个组件是否能正常工作。

(2)对航行任务进行规划，预设航行轨迹参数，其设计的航行轨迹η_d必须为一阶可导的。如附图1，附图4所示，其分别为螺旋的椭圆绕行轨迹与直线对接轨迹。

(3)将实验体放入实验环境中，上电准备好后，测量系统的各个模块进行对航行器的位置，姿态及速度进行测量，并将测量值η(0)，v(0)发送到上位机。

(4)上位机通过接受到的测量系统的测量信息η(0)，v(0)，解算航行的轨迹误差向量其中并赋予系统未知参数矩阵的初始值与干扰观测器的初始值d(0)，根据本发明所设计的控制律进行控制力解算，

然后将解算得到的控制力通过该模型的推力分配矩阵解算，得出每个推进器的推力值，并将各个推进器的控制推力通过串口发送到下位机。

特别的，当B不可逆时，

(5)下位机接受到上位机发送的控制指令，电机控制模块通过对控制力信号进行分析解算，输出一个与控制力所对应的PWM波到电机，完成整个对电机的推力控制。

(6)混合悬浮实验体开始运动时，上位机接收到测量系统测量到的新的实际值η(k)，v(k)，按照步骤(4)对系统的跟踪误差s(k)进行更新，同时更新新的控制力τ(k)，并通过模型的推力分配矩阵将新的控制力τ(k)解算得到T(k)，将得到的新的T(k)发送到下位机。

(7)根据实验的反馈数据，通过改变系统的误差控制增益矩阵K，系统未知参数估计的增益矩阵K_θ及系统的干扰观测的增益矩阵K_d，观察变化后的跟踪误差轨迹，做出适当的对系统各个增益矩阵K,K₁,K₂的调整。

(8)重复进行上述步骤(3)～(8)，使航行路线达到最优，跟踪误差达到最小。

如附图2，3，5，6所示。在螺旋形椭圆跟踪轨迹时，实验体实际轨迹与预设轨迹基本重合，误差范围不大于3cm，而在对接过程中，对接后当实验体带动目标体时，实验体本体参数发生了很大程度的改变，而这种自适应控制律则能较为快速的稳定下来，且跟踪误差不大于3cm。

Claims (4)

1.一种基于干扰观测器的混合悬浮实验体控制方法，其特征在于：包括以下步骤：

1)对航行任务进行规划，预设航行轨迹η_d；

2)将实验体放入实验环境中，上电准备好后，测量系统的各个模块进行对航行器的位置，姿态及速度进行测量，并将测量值η(0)，v(0)发送到上位机；

3)上位机通过接受到的测量系统的测量信息η(0)，v(0)，解算航行的轨迹误差向量其中并赋予系统未知参数矩阵的初始值与干扰观测器的初始值d(0)，根据以下自适应PD控制律进行控制力解算，

$\left\{\begin{array}{c}\mathrm{\τ}=Ks+\mathrm{\Φ}(\stackrel{\·}{v},v,\mathrm{\η})\widehat{\mathrm{\θ}}+\hat{d}\left(t\right)\\ \stackrel{\·}{\widehat{\mathrm{\θ}}}={\mathrm{\Γ}}^{-1}{\mathrm{\Φ}}^T(\stackrel{\·}{v},v,\mathrm{\η})s-K_{\mathrm{\θ}}\widehat{\mathrm{\θ}}\\ \stackrel{\·}{\hat{d}}\left(t\right)={\mathrm{\Λ}}^{-1}s-K_d\hat{d}\left(t\right)\end{array}\right.$

然后将解算得到的控制力通过实验体的推力分配矩阵解算，得出每个推进器的推力值，并将各个推进器的控制推力通过串口发送到下位机；

当B不可逆时，

4)下位机接受到上位机发送的控制指令，电机控制模块通过对控制力信号进行分析解算，输出一个与控制力所对应的PWM波到电机，完成整个对电机的推力控制；

5)混合悬浮实验体开始运动时，上位机接收到测量系统测量到的新的实际值η(k)，v(k)，按照步骤(3)对系统的跟踪误差s(k)进行更新，同时更新新的控制力τ(k)，并通过模型的推力分配矩阵将新的控制力τ(k)解算得到推力矩阵T(k)，将得到的新的T(k)发送到下位机；

6)根据实验的反馈数据，通过改变系统的误差控制增益矩阵K，系统未知参数估计的增益矩阵K_θ及系统的干扰观测的增益矩阵K_d，观察变化后的跟踪误差轨迹，做出适当的对系统各个增益矩阵K,K₁,K₂的调整；

7)重复进行上述步骤2)～6)，使航行路线达到最优，跟踪误差达到最小。

2.根据权利要求1所述的一种基于干扰观测器的混合悬浮实验体控制方法，其特征在于：步骤3)中所述控制律是通过以下步骤得到的：

2.1)选取误差向量其中η＝[x y z]^T为混合悬浮实验体在惯性坐标系下的位置矩阵；v＝[u v w]^T为混合悬浮实验体在雷体坐标系下的速度矩阵，且位置误差速度误差取期望速度

2.2)在理想状态下，建立混合悬浮实验体模型：

$\stackrel{\·}{\mathrm{\η}}=J\left(\mathrm{\η}\right)---\left(1\right)$

$M\stackrel{\·}{v}+C\left(v\right)v+D\left(v\right)v+G\left(\mathrm{\η}\right)=\mathrm{\τ}---\left(2\right)$

其中的M是系统的惯性阵；C(v)是系统的哥氏力矩阵；D(v)为系统正定对称的水阻力系数矩阵；G(η)为系统由系统重浮心位置产生的恢复力矩阵；τ＝BT为系统的外部控制输入；B为实际模型的推力分配矩阵；T是由混合悬浮实验体上各个推进器的推力组成的推力矩阵；

利用自适应的思想，(2)式则可写成：

$\mathrm{\Φ}(\stackrel{\·}{v},v,\mathrm{\η})\mathrm{\θ}=\mathrm{\τ}---\left(3\right)$

其中，是实际可测量得到的参数矩阵，θ为系统未知参数矩阵；

加入有界的外部干扰d(t)时，则上式(3)可写成：

$\mathrm{\Φ}(\stackrel{\·}{v},v,\mathrm{\η})\mathrm{\θ}+d\left(t\right)=\mathrm{\τ}---\left(4\right)$

2.3)设计控制律，取：

$\left\{\begin{array}{c}\mathrm{\τ}=Ks+\mathrm{\Φ}(\stackrel{\·}{v},v,\mathrm{\η})\widehat{\mathrm{\θ}}+\hat{d}\left(t\right)\\ \stackrel{\·}{\widehat{\mathrm{\θ}}}={\mathrm{\Γ}}^{-1}{\mathrm{\Φ}}^T(\stackrel{\·}{v},v,\mathrm{\η})s-K_{\mathrm{\θ}}\widehat{\mathrm{\θ}}\\ \stackrel{\·}{\hat{d}}\left(t\right)={\mathrm{\Λ}}^{-1}s-K_d\hat{d}\left(t\right)\end{array}\right.---\left(5\right)$

其中，K∈R^6×6，K_θ和K_d分别为可调的正定的PD控制增益矩阵，系统参数估计的增益矩阵和干扰观测器的增益矩阵；Γ、Λ为一个适当的正定矩阵，为系统未知参数的在线估计，为外部干扰的观测器的观测值。

3.根据权利要求2所述的一种基于干扰观测器的混合悬浮实验体控制方法，其特征在于：步骤2.3)设计控制律中还包括证明所述控制律稳定性的步骤，具体步骤如下：

3.1)根据李雅普诺夫稳定性原理，选取候选李雅普诺夫函数：

$V_0=\frac{1}{2}{s}^{T}Ms---\left(6\right)$

对候选函数V₀求关于时间的导数，则：

$\begin{array}{c}{\stackrel{\·}{V}}_0=s^{T}M\stackrel{\·}{s}\\ =s^{T}M\left({\stackrel{\·}{v}}_r-\stackrel{\·}{v}\right)\\ =s^T\[M{\stackrel{\·}{v}}_r-\mathrm{\τ}+C\left(v\right)v+D\left(v\right)v+G\left(\mathrm{\η}\right)+d\left(t\right)\]\\ =s^T\[\mathrm{\Φ}\left(\stackrel{\·}{v},v,\mathrm{\η}\right)\mathrm{\θ}+d\left(t\right)-\mathrm{\τ}\]-s^{T}D\left(v\right)s\end{array}---\left(7\right)$

3.2)系统模型参数的估计的稳定性，选取新的利亚普诺夫函数：

$\begin{array}{c}{V}_1=V_0+\frac{1}{2}{\widetilde{\mathrm{\θ}}}^T\mathrm{\Γ}\widetilde{\mathrm{\θ}}\\ =\frac{1}{2}{s}^{T}Ms+\frac{1}{2}{\widetilde{\mathrm{\θ}}}^T\mathrm{\Γ}\widetilde{\mathrm{\θ}}\end{array}---\left(8\right)$

对V₁逐项求关于时间的导数，则：

$\begin{array}{c}{\stackrel{\·}{V}}_1={\stackrel{\·}{V}}_0+{\widetilde{\mathrm{\θ}}}^T\mathrm{\Γ}\stackrel{\·}{\widetilde{\mathrm{\θ}}}\\ =s^T\[\mathrm{\Φ}\left(\stackrel{\·}{v},v,\mathrm{\η}\right)\mathrm{\θ}+d\left(t\right)-\mathrm{\τ}\]-s^{T}D\left(v\right)s-{\widetilde{\mathrm{\θ}}}^T\mathrm{\Γ}\stackrel{\·}{\widehat{\mathrm{\θ}}}\end{array}---\left(9\right)$

3.3)干扰观测器的观测值的稳定性，最终选取利亚普诺夫函数：

$\begin{array}{c}V=V_1+\tilde{d}{\left(t\right)}^T\mathrm{\Λ}\tilde{d}\left(t\right)\\ =\frac{1}{2}{s}^{T}Ms+\frac{1}{2}{\widetilde{\mathrm{\θ}}}^T\mathrm{\Γ}\widetilde{\mathrm{\θ}}+\tilde{d}{\left(t\right)}^T\mathrm{\Λ}\tilde{d}\left(t\right)\end{array}---\left(10\right)$

对V逐项求关于时间的导数，则：

$\begin{array}{c}\stackrel{\·}{V}={\stackrel{\·}{V}}_1+\tilde{d}{\left(t\right)}^T\mathrm{\Λ}\stackrel{\·}{\tilde{d}}\left(t\right)\\ =s^T\[\mathrm{\Φ}\left(\stackrel{\·}{v},v,\mathrm{\η}\right)\mathrm{\θ}+d\left(t\right)-\mathrm{\τ}\]-s^{T}D\left(v\right)s-{\widetilde{\mathrm{\θ}}}^T\mathrm{\Γ}\stackrel{\·}{\widehat{\mathrm{\θ}}}-\tilde{d}{\left(t\right)}^T\mathrm{\Λ}\stackrel{\·}{\hat{d}}\left(t\right)\end{array}---\left(11\right)$

3.4)将控制律式(5)带入(11)式中，合并可得：

$\begin{array}{c}\stackrel{\·}{V}=-s^T\[K+D\left(v\right)\]s-{\widetilde{\mathrm{\θ}}}^T{K}_1\widetilde{\mathrm{\θ}}-\tilde{d}{\left(t\right)}^T{K}_2\tilde{d}\left(t\right)+{\widetilde{\mathrm{\θ}}}^T{K}_1\mathrm{\θ}+\tilde{d}{\left(t\right)}^T{K}_{2}d\left(t\right)\\ \≤-s^T\[K+D\left(v\right)\]s-\frac{1}{2}{\widetilde{\mathrm{\θ}}}^T{K}_1\widetilde{\mathrm{\θ}}-\frac{1}{2}\tilde{d}{\left(t\right)}^T{K}_2\tilde{d}\left(t\right)+\frac{1}{2}{\mathrm{\θ}}^T{K}_1\mathrm{\θ}+\frac{1}{2}d{\left(t\right)}^T{K}_{2}d\left(t\right)\\ \≤-\mathrm{\κ}V+C\end{array}---\left(12\right)$

其中K₁＝ΓK_θ，K₂＝ΛK_d，且

$\begin{array}{c}\mathrm{\κ}:=\min \left({\mathrm{\λ}}_{\min }\left(K+D\left(v\right)\right),{\mathrm{\λ}}_{\min }\left(\frac{1}{2}{K}_1\right),{\mathrm{\λ}}_{\min }\left(\frac{1}{2}{K}_2\right)\right)\\ C:=\frac{1}{2}{\mathrm{\θ}}^T{K}_1\mathrm{\θ}+\frac{1}{2}d{\left(t\right)}^T{K}_{2}d\left(t\right)\end{array}---\left(13\right)$

因为系统未知参数θ与系统外部干扰d(t)均有界，即||θ||≤δ₁，||d(t)||≤δ₂，其中δ₁与δ₂均为正常数，则且D(v)正定，选取适当的正定的增益矩阵K,K₁,K₂；由上式(12)可得：

$0\≤V\≤\frac{C}{\mathrm{\κ}}+\[V\left(0\right)-\frac{C}{\mathrm{\κ}}\]e^{-\mathrm{\κ}t}---\left(14\right)$

由(14)式得，李雅普诺夫函数V是正定有界的，即存在t，使得t→∞时，因此可知闭环系统的轨迹误差项s，系统未知参数误差及系统外部干扰与干扰观测器之间存在的误差均是有界的，则该控制律是稳定的。

4.根据权利要求1所述的一种基于干扰观测器的混合悬浮实验体控制方法，其特征在于：所述航行轨迹η_d为一阶可导的且有界的。