CN105717936A - 一种无缆自治水下机器人的潜浮控制方法 - Google Patents

Abstract

本发明涉及一种无缆自治水下机器人的潜浮控制方法，包括以下步骤：通过安装在机器人上的传感器采集到纵倾角和垂向速度信号，将它们作为反馈信号传送给扩张状态观测器，得到纵倾角、纵倾角速率、垂向速度及系统总扰动的估计值，纵倾角和垂向速度的设定值通过跟踪微分器得到滤波后的跟踪值及其微分值。纵倾角及其微分值分别与纵倾角和纵倾角速率的估计值作差，然后输入给非线性状态误差反馈控制律，控制律的输出和扰动估计值的补偿构成了纵倾角控制量；同理，对垂向速度进行相应处理，得到垂向速度控制量。从而实现纵倾角与垂向速度完全解耦控制，达到了水下机器人以某一给定纵倾角和给定垂向速度潜浮运动的控制要求，为水下机器人完成定点垂直剖面观测提供了技术保证。

Description

一种无缆自治水下机器人的潜浮控制方法

技术领域

本发明涉及一种无缆自治水下机器人(Autonomousunderwatervehicle，简称为AUV)的潜浮控制方法，属于水下机器人控制技术领域。

背景技术

水下机器人是一个非线性、时变性、强耦合、大时延、多自由度的复杂系统。由于水动力参数的冗杂性和风、浪、流等的不断变化，难以得到精确的AUV数学模型，使得获得AUV的高精度控制非常困难。水下机器人的潜浮运动是其空间运动中的某一实例，常用于定点观测、水下悬浮等情形，因此在AUV的空间运动和控制技术研究中具有实际意义和应用价值。

随着水下机器人应用的不断增加和控制精度要求的不断提高，很多先进的算法被应用于水下机器人的运动控制中，如自适应控制，鲁棒控制，神经网络控制，滑模变结构控制等。自适应控制和鲁棒控制主要处理系统内部不确定性，对系统外部干扰的鲁棒性有限，其中自适应控制根据系统内部参数的不确定性进行在线调整，鲁棒控制考虑不确定部分的最坏情形来设计控制器，能处理的不确定性范围有限；神经网络控制的算法复杂，易出现控制滞后，是工程应用存在的主要问题；滑模变结构控制能同时处理系统内部和外部的不确定性，但滑动模态的可达性易受不确定性的影响。

自抗扰控制将系统的内部和外部不确定性统一为总扰动(已建模动态、未建模动态及未知外扰的总和)，利用扩张状态观测器对总扰动进行实时估计并补偿，以获得较好的控制性能和较高的控制精度。此外，自抗扰控制没有限制不确定性的具体形式，无论是时变还是定常，线性还是非线性，连续还是间断，都能通过扩张状态观测器进行实时估计并补偿。

因此，研究简单实用的水下机器人潜浮控制方法，解决水下机器人欠驱动、强耦合问题，提高水下机器人的抗干扰能力，是目前水下机器人运动控制中一个重点需要解决的实际问题。

发明内容

基于上述背景，本发明针对现有先进控制方法的不足，提出了一种基于自抗扰控制技术的无缆自治水下机器人的潜浮控制方法，可以实现机器人快速、平滑、超调小的潜浮控制，提高了纵倾角与垂向速度的动态跟踪性能和稳态性能，并实现纵倾角和垂向速度的完全解耦控制。

本发明解决其问题所采用的技术方案如下：一种无缆自治水下机器人的潜浮控制方法，包括纵倾角控制和垂向速度控制，具体包括以下步骤：

通过安装在水下机器人上的传感器实时采集纵倾角和垂向速度信号；

将纵倾角和垂向速度信号作为反馈信号，并结合纵倾角和垂向速度的实际控制量进行扩张状态观测，得到纵倾角、纵倾角速率、垂向速度及系统总扰动的估计值；

对纵倾角和垂向速度的设定值通过跟踪微分器进行滤波，得到纵倾角的跟踪值及其微分值、垂向速度的跟踪值；

将纵倾角的跟踪值及其微分值分别与纵倾角和纵倾角速率的估计值作差；将垂向速度的跟踪值与垂向速度的估计值作差；

将上述差值结果进行非线性反馈控制，反馈控制结果和系统总扰动的估计值构成了纵倾角和垂向速度的控制量；

纵倾角和垂向速度的控制量输入给水下机器人进行实时控制。

所述对纵倾角和垂向速度的设定值通过跟踪微分器进行滤波采用离散方程形式，设计如下：

$\left\{\begin{array}{c}{v}_1(k+1)=v_1\left(k\right)+{\mathrm{hv}}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+\mathrm{hfst}(v_1\left(k\right)-v\left(k\right),v_2\left(k\right),r_0,h),\end{array}\right.$

其中，fst(v₁(k)-v(k),v₂(k),r₀,h)：

$\left\{\begin{array}{c}d=r_{0}h,d_0=\mathrm{hd}\\ y=v_1-v+{\mathrm{hv}}_2\\ a_0=\sqrt{d_2+{8r}_0\left|y\right|}\\ a=\left\{\begin{array}{c}{v}_2+\frac{a_0-d}{2}\mathrm{sign}\left(y\right),\left|y\right|>d_0\\ v_2+\frac{y}{h},\left|y\right|\≤d_0\end{array}\right.\\ \mathrm{fst}=-\left\{\begin{array}{c}{r}_0\mathrm{sign}\left(a\right),\left|a\right|>d\\ r_0\frac{a}{d},\left|a\right|\≤d\end{array}\right.,\end{array}\right.$

其中，r₀是决定过渡过程快慢的速度因子，r₀值越大，过渡过程越快；v是设定值，h是采样步长，v₁是输入信号v的跟踪值，v₂是v的近似微分信号，k是采样点；

当对纵倾角的设定值通过跟踪微分器进行滤波时，v是θ_ref；当对垂向速度的设定值通过跟踪微分器进行滤波时，v是w_ref。

所述将纵倾角和垂向速度信号作为反馈信号，并结合纵倾角和垂向速度的实际控制量进行扩张状态观测，采用线性形式，其中，纵倾角控制的扩张状态观测采用以下公式设计：

$\left\{\begin{array}{c}{e}_{01}\left(k\right)=z_{01}\left(k\right)-\mathrm{\θ}\left(k\right)\\ z_{01}(k+1)=z_{01}\left(k\right)+h(z_{02}\left(k\right)-{\mathrm{\β}}_{01}{e}_{01}\left(k\right))\\ z_{02}(k+1)=z_{02}\left(k\right)+h(z_{03}\left(k\right)-{\mathrm{\β}}_{02}{e}_{01}\left(k\right)+b_1{u}_1\left(k\right))\\ z_{03}(k+1)=z_{03}\left(k\right)-h{\mathrm{\β}}_{03}{e}_{01}\left(k\right),\end{array}\right.$

其中，z₀₁和z₀₂分别为扩张状态观测器对水下机器人的纵倾角θ及其微分值的估计，z₀₃是系统总扰动的估计值，θ为系统输出，β₀₁，β₀₂，β₀₃为可调参数，h为采样步长，k是采样点，u₁是未加补偿的纵倾角控制量输出，b₁是控制量的放大系数；

垂向速度控制的扩张状态观测用以下公式设计：

$\left\{\begin{array}{c}{e}_{11}\left(k\right)=z_{11}\left(k\right)-w\left(k\right)\\ z_{11}(k+1)=z_{11}\left(k\right)+h(z_{12}\left(k\right)-{\mathrm{\β}}_{11}{e}_{11}\left(k\right)+b_2{u}_2\left(k\right))\\ z_{12}(k+1)=z_{12}\left(k\right)-h{\mathrm{\β}}_{12}{e}_{11}\left(k\right),\end{array}\right.$

其中，z₁₁为扩张状态观测器对水下机器人的垂向速度w的估计，z₁₂是系统总扰动的估计，w为系统输出，β₁₁和β₁₂为可调参数，k是采样点，u₂是未加补偿的垂向速度控制量输出，b₂是控制量的放大系数。

所述将纵倾角差值结果进行非线性反馈控制，采用如下形式：

$\left\{\begin{array}{c}{e}_{02}\left(k\right)={\mathrm{\θ}}_1\left(k\right)-z_{01}\left(k\right)\\ e_{03}\left(k\right)=q_1\left(k\right)-z_{02}\left(k\right)\\ u_{01}\left(k\right)=-\mathrm{fst}(e_{02}\left(k\right),{\mathrm{ce}}_{03}\left(k\right),r_1,h_1)\\ u_1\left(k\right)=u_{01}\left(k\right)-\frac{z_{03}\left(k\right)}{b_1},\end{array}\right.$

其中，e₀₂和e₀₃分别是输入信号θ_ref的跟踪值θ₁及其微分值q₁与输出信号θ和q的估计的误差，u₀₁是非线性状态误差反馈控制律的输出，u₁是纵倾角控制子系统的控制量，z₀₁和z₀₂分别为扩张状态观测器对水下机器人的纵倾角θ及其微分值的估计，z₀₃是系统总扰动的估计值，b₁是控制量的放大系数，r₁表示过渡过程的快慢，r₁越大过渡过程越快，h₁是噪声滤波因子，c是阻尼因子；

将垂向速度差值结果进行非线性反馈控制，采用如下形式：

$\left\{\begin{array}{c}{e}_{12}\left(k\right)=w_1\left(k\right)-z_{11}\left(k\right)\\ u_{11}\left(k\right)=r_2{e}_{12}\left(k\right)\\ u_2\left(k\right)=u_{11}\left(k\right)-\frac{z_{12}\left(k\right)}{b_2},\end{array}\right.$

其中，e₁₂分别是输入信号w_ref的跟踪值w₁与输出信号w的估计的误差，u₁₁是状态误差反馈控制律的输出，u₂是垂向速度控制子系统的控制量，r₂是比例系数，z₁₁为扩张状态观测器对水下机器人的垂向速度w的估计，z₁₂是系统总扰动的估计，b₂是控制量的放大系数。

纵倾角控制的参数调节规律采用以下形式：

$\left\{\begin{array}{c}{r}_0=\frac{0.0001}{h^2}\\ {\mathrm{\β}}_{01}=\frac{1}{h},{\mathrm{\β}}_{02}=\frac{1}{{3h}^2},{\mathrm{\β}}_{03}=\frac{1}{{32h}^3}\\ r_1=\frac{0.5}{h^2},c=0.2,h_1=2h.\end{array}\right.$

垂向速度控制的参数调节规律采用以下形式：

$\left\{\begin{array}{c}{r}_0=\frac{0.00005}{h^2}\\ {\mathrm{\β}}_{11}={\mathrm{\β}}_{01},{\mathrm{\β}}_{12}={\mathrm{\β}}_{02}\\ r_2=\frac{0.5}{h}.\end{array}\right.$

本发明具有以下有益效果及意义：

1.本发明将潜浮方法分为纵倾角控制子系统和垂向速度控制子系统，并分别采用了二阶自抗扰控制和一阶自抗扰控制，获得纵倾角与垂向速度两个子系统在动态过程中的完全解耦控制，实现了水下机器人以某一给定纵倾角和给定垂向速度潜浮运动的控制要求。

2.本发明利用扩张状态观测器实时估计并补偿了水下机器人系统的总扰动，基本实现了无超调无误差无抖振地完全跟踪，使系统具有良好的抗干扰性和鲁棒性。

3.本发明规定了纵倾角控制子系统和垂向速度控制子系统参数调节规律，仅通过调整采样步长h即可调节两个子系统的控制性能，将需要调节的参数控制到最少，能方便快速地应用于实际工程中。

附图说明

图1为本发明的纵倾角控制子系统的二阶自抗扰控制器结构图；

图2为本发明的垂向速度控制子系统一阶自抗扰控制器结构图；

图3为本发明的无缆自治水下机器人的潜浮控制方法结构图。

具体实施方式

下面结合附图对本发明作详细介绍。

本发明的基于自抗扰控制技术的无缆自治水下机器人潜浮控制方法，包括以下步骤：

第一步：建立无缆自治水下机器人系统模型，描述如下：

$\left[\begin{array}{ccc}1& 0& 0\\ 0& I_y-M_{\stackrel{.}{q}}& {-\mathrm{mx}}_G-M_{\stackrel{.}{w}}\\ 0& {-\mathrm{mx}}_G-Z_{\stackrel{.}{q}}& m-Z_{\stackrel{.}{w}}\end{array}\right]\left[\begin{array}{c}\stackrel{.}{\mathrm{\θ}}\\ \stackrel{.}{q}\\ \stackrel{.}{w}\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ {-z}_{G}W& M_q-{\mathrm{mx}}_G{u}_0& M_w\\ 0& Z_q+{\mathrm{mu}}_0& Z_w\end{array}\right]\left[\begin{array}{c}\mathrm{\θ}\\ q\\ w\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\τ}}_M\\ {\mathrm{\τ}}_Z\end{array}\right]+\left[\begin{array}{c}0\\ d_q\\ d_w\end{array}\right]$

其中，m为AUV水下质量，单位是千克(kg)；W为AUV水下重力，单位为牛顿(N)；I_y为AUV绕y轴的质量惯性矩；x_G和z_G为AUV重心在随体坐标系中的坐标，单位是米(m)；u和w为线速度矢量在随体坐标系中的两个分量，单位为米/秒(m/s)；θ为纵倾角，单位为弧度(rad)；q为角速度矢量在随体坐标系中的分量，单位为弧度/秒(rad/s)；τ_M和τ_Z分别为潜浮算法输出的纵倾力矩和垂向力，单位分别为牛顿·米(N·m)和牛顿(N)；d_q和d_w分别为纵倾角速率的总扰动和垂向速度的总扰动，包括未建模动态和外部扰动；以上符号的下标为0时，取值为潜浮算法开始运行时的当前状态值。符号Z_w，Z_q，M_w，M_q的展开式分别如下所示：

$Z_{\stackrel{.}{w}}=\frac{\mathrm{\ρ}}{2}{L}^3{Z}_{\stackrel{.}{w}}^{\′},Z_{\stackrel{.}{q}}=\frac{\mathrm{\ρ}}{2}{L}^4{Z}_{\stackrel{.}{q}}^{\′},M_{\stackrel{.}{w}}=\frac{\mathrm{\ρ}}{2}{L}^4{M}_{\stackrel{.}{w}}^{\′},M_{\stackrel{.}{q}}=\frac{\mathrm{\ρ}}{2}{L}^5{M}_{\stackrel{.}{q}}^{\′},$

$Z_w=\frac{\mathrm{\ρ}}{2}{L}^3{Z}_{w\left|q\right|}^{\′}\left|q_0\right|+\frac{\mathrm{\ρ}}{2}{L}^2(Z_w^{\′}{u}_0+Z_{w\left|w\right|}^{\′}|w_0|+Z_{\mathrm{ww}}^{\′}{w}_0),Z_q=\frac{\mathrm{\ρ}}{2}{L}^4{Z}_{q\left|q\right|}^{\′}\left|q_0\right|+{\mathrm{mz}}_G{q}_0+\frac{\mathrm{\ρ}}{2}{L}^3{Z}_q^{\′}{u}_0,$

$M_w=\frac{\mathrm{\ρ}}{2}{L}^3{M}_w^{\′}{u}_0+\frac{\mathrm{\ρ}}{2}{L}^3{M}_{w\left|w\right|}^{\′}\left|w_0\right|,M_q=\frac{\mathrm{\ρ}}{2}{L}^5{M}_{q\left|q\right|}^{\′}\left|q_0\right|+\frac{\mathrm{\ρ}}{2}{L}^4{M}_q^{\′}{u}_0+\frac{\mathrm{\ρ}}{2}{L}^4{M}_{\left|w\right|q}^{\′}\left|w_0\right|-{\mathrm{mz}}_G{w}_0,$

其中，ρ为4℃海水密度，单位为千克/立方米(kg/m³)；L为AUV载体长度，单位为米(m)；水动力系统中的加速度项系数分别有水动力系统中的速度项系数分别有Z'_w|q|，Z'_w，Z'_w|w|，Z'_ww，Z'_q|q|，Z'_q，M'_w，M'_w|w|，M'_q|q|，M'_q，M'_|w|q。

记：

$H=\left[\begin{array}{ccc}1& 0& 0\\ 0& I_y-M_{\stackrel{.}{q}}& {-\mathrm{mx}}_G-M_{\stackrel{.}{w}}\\ 0& {-\mathrm{mx}}_G-Z_{\stackrel{.}{q}}& m-Z_{\stackrel{.}{w}}\end{array}\right],P=\left[\begin{array}{ccc}0& 1& 0\\ {-z}_{G}W& M_q-{\mathrm{mx}}_G{u}_0& M_w\\ 0& Z_q+{\mathrm{mu}}_0& Z_w\end{array}\right],R^{\′}=\left[\begin{array}{c}0\\ d_q\\ d_w\end{array}\right].$

那么，无缆自治水下机器人系统模型可简写为如下形式：

$\left[\begin{array}{c}\stackrel{.}{\mathrm{\θ}}\\ \stackrel{.}{q}\\ \stackrel{.}{w}\end{array}\right]=H^{-1}P\left[\begin{array}{c}\mathrm{\θ}\\ q\\ w\end{array}\right]+\left[\begin{array}{c}\begin{array}{cc}0& 0\end{array}\\ B\end{array}\right]\left[\begin{array}{c}{\mathrm{\τ}}_M\\ {\mathrm{\τ}}_Z\end{array}\right]+H^{-1}{R}^{\′}$

其中，H为可逆矩阵；B∈R^2×2，其具体形式为：

$B=\frac{1}{(I_y-M_{\stackrel{.}{q}})*(m-Z_{\stackrel{.}{w}})-({\mathrm{mx}}_G+M_{\stackrel{.}{w}})*({\mathrm{mx}}_G+Z_{\stackrel{.}{q}})}\left[\begin{array}{cc}m-Z_{\stackrel{.}{w}}& {\mathrm{mx}}_G+M_{\stackrel{.}{w}}\\ {\mathrm{mx}}_G+Z_{\stackrel{.}{q}}& I_y-M_{\stackrel{.}{q}}\end{array}\right].$ 且能确保 $(I_y-M_{\stackrel{.}{q}})*(m-Z_{\stackrel{.}{w}})-({\mathrm{mx}}_G+M_{\stackrel{.}{w}})*({\mathrm{mx}}_G+Z_{\stackrel{.}{q}})\≠0.$

第二步：如图1、2所示，根据第一步建立的AUV控制模型，设计基于自抗扰控制技术的无缆自治水下机器人的潜浮控制方法，即纵倾角和垂向速度的解耦控制，主要包括以下两个方面：

(1)设计纵倾角控制子系统的二阶自抗扰控制器：

(1.1)跟踪微分器的设计如下：

$\left\{\begin{array}{c}{v}_1(k+1)=v_1\left(k\right)+{\mathrm{hv}}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+\mathrm{hfst}(v_1\left(k\right)-v\left(k\right),v_2\left(k\right),r_0,h),\end{array}\right.$

其中，fst(v₁(k)-v(k),v₂(k),r₀,h)：

$\left\{\begin{array}{c}d=r_{0}h,d_0=\mathrm{hd}\\ y=v_1-v+{\mathrm{hv}}_2\\ a_0=\sqrt{d_2+{8r}_0\left|y\right|}\\ a=\left\{\begin{array}{c}{v}_2+\frac{a_0-d}{2}\mathrm{sign}\left(y\right),\left|y\right|>d_0\\ v_2+\frac{y}{h},\left|y\right|\≤d_0\end{array}\right.\\ \mathrm{fst}=-\left\{\begin{array}{c}{r}_0\mathrm{sign}\left(a\right),\left|a\right|>d\\ r_0\frac{a}{d},\left|a\right|\≤d\end{array}\right.,\end{array}\right.$

其中，r₀是决定过渡过程快慢的速度因子，r₀值越大，过渡过程越快；v是设定值，h是采样步长，v₁是输入信号v的跟踪值，v₂是v的近似微分信号，k是采样点；当对纵倾角的设定值通过跟踪微分器进行滤波时，v是θ_ref。

(1.2)扩张状态观测器设计如下：

$\left\{\begin{array}{c}{e}_{01}\left(k\right)=z_{01}\left(k\right)-\mathrm{\θ}\left(k\right)\\ z_{01}(k+1)=z_{01}\left(k\right)+h(z_{02}\left(k\right)-{\mathrm{\β}}_{01}{e}_{01}\left(k\right))\\ z_{02}(k+1)=z_{02}\left(k\right)+h(z_3\left(k\right)-{\mathrm{\β}}_{02}{e}_{01}\left(k\right)+b_1{u}_1\left(k\right))\\ z_{03}(k+1)=z_{03}\left(k\right)-h{\mathrm{\β}}_{03}{e}_{01}\left(k\right),\end{array}\right.$

其中，z₀₁和z₀₂分别为扩张状态观测器对水下机器人的纵倾角θ及其微分值的估计，z₀₃是系统总扰动的估计值，θ为系统输出，β₀₁，β₀₂，β₀₃为可调参数，h为采样步长，k是采样点，u₁是未加补偿的纵倾角控制量输出，b₁是控制量的放大系数；

(1.3)二阶非线性状态误差反馈控制律及扰动估计的补偿设计如下：

$\left\{\begin{array}{c}{e}_{02}\left(k\right)={\mathrm{\θ}}_1\left(k\right)-z_{01}\left(k\right)\\ e_{03}\left(k\right)=q_1\left(k\right)-z_{02}\left(k\right)\\ u_{01}\left(k\right)=-\mathrm{fst}(e_{02}\left(k\right),{\mathrm{ce}}_{03}\left(k\right),r_1,h)\\ u_1\left(k\right)=u_{01}\left(k\right)-\frac{z_{03}\left(k\right)}{b_1}.\end{array}\right.$

其中，e₀₂和e₀₃分别是输入信号θ_ref的跟踪值θ₁及其微分值q₁与输出信号θ和q的估计的误差，u₀₁是非线性状态误差反馈控制律的输出，u₁是纵倾角控制子系统的控制量，z₀₁和z₀₂分别为扩张状态观测器对水下机器人的纵倾角θ及其微分值的估计，z₀₃是系统总扰动的估计值，b₁是控制量的放大系数，r₁表示过渡过程的快慢，r₁越大过渡过程越快，h₁是噪声滤波因子，c是阻尼因子；

(2)设计垂向速度控制子系统的一阶自抗扰控制器：

(2.1)跟踪微分器的设计如下：

$\left\{\begin{array}{c}{v}_1(k+1)=v_1\left(k\right)+{\mathrm{hv}}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+\mathrm{hfst}(v_1\left(k\right)-v\left(k\right),v_2\left(k\right),r_0,h),\end{array}\right.$

其中，fst(v₁(k)-v(k),v₂(k),r₀,h)：

$\left\{\begin{array}{c}d=r_{0}h,d_0=\mathrm{hd}\\ y=v_1-v+{\mathrm{hv}}_2\\ a_0=\sqrt{d_2+{8r}_0\left|y\right|}\\ a=\left\{\begin{array}{c}{v}_2+\frac{a_0-d}{2}\mathrm{sign}\left(y\right),\left|y\right|>d_0\\ v_2+\frac{y}{h},\left|y\right|\≤d_0\end{array}\right.\\ \mathrm{fst}=-\left\{\begin{array}{c}{r}_0\mathrm{sign}\left(a\right),\left|a\right|>d\\ r_0\frac{a}{d},\left|a\right|\≤d\end{array}\right.,\end{array}\right.$

其中，r₀是决定过渡过程快慢的速度因子，r₀值越大，过渡过程越快；v是设定值，h是采样步长，v₁是输入信号v的跟踪值，v₂是v的近似微分信号，k是采样点；当对垂向速度的设定值通过跟踪微分器进行滤波时，v是w_ref。

(2.2)扩张状态观测器的设计如下：

$\left\{\begin{array}{c}{e}_{11}\left(k\right)=z_{11}\left(k\right)-w\left(k\right)\\ z_{11}(k+1)=z_{11}\left(k\right)+h(z_{12}\left(k\right)-{\mathrm{\β}}_{11}{e}_{11}\left(k\right)+b_2{u}_2\left(k\right))\\ z_{12}(k+1)=z_{12}\left(k\right)-h{\mathrm{\β}}_{12}{e}_{11}\left(k\right),\end{array}\right.$

其中，z₁₁为扩张状态观测器对水下机器人的垂向速度w的估计，z₁₂是系统总扰动的估计，w为系统输出，β₁₁和β₁₂为可调参数，k是采样点，u₂是未加补偿的垂向速度控制量输出，b₂是控制量的放大系数。

(2.3)一阶线性状态误差反馈控制律及扰动估计的补偿设计如下：

$\left\{\begin{array}{c}{e}_{12}\left(k\right)=w_1\left(k\right)-z_{11}\left(k\right)\\ u_{11}\left(k\right)=r_2{e}_{12}\left(k\right)\\ u_2\left(k\right)=u_{11}\left(k\right)-\frac{z_{12}\left(k\right)}{b_2}\end{array}\right.$

其中，e₁₂分别是输入信号w_ref的跟踪值w₁与输出信号w的估计的误差，u₁₁是状态误差反馈控制律的输出，u₂是垂向速度控制子系统的控制量，r₂是比例系数，z₁₁为扩张状态观测器对水下机器人的垂向速度w的估计，z₁₂是系统总扰动的估计，b₂是控制量的放大系数。

第三步：如图3所示，记第二步得出的控制量为虚拟控制量U，通过将增益矩阵B求逆，得出实际控制量u如下：

u＝B^-1U

其中，u＝[τ_M,τ_Z]^T，U＝[u₁,u₂]^T。

Claims (5)

1.一种无缆自治水下机器人的潜浮控制方法，其特征在于，包括纵倾角控制和垂向速度控制，具体包括以下步骤：

通过安装在水下机器人上的传感器实时采集纵倾角和垂向速度信号；

将纵倾角和垂向速度信号作为反馈信号，并结合纵倾角和垂向速度的实际控制量进行扩张状态观测，得到纵倾角、纵倾角速率、垂向速度及系统总扰动的估计值；

对纵倾角和垂向速度的设定值通过跟踪微分器进行滤波，得到纵倾角的跟踪值及其微分值、垂向速度的跟踪值；

将纵倾角的跟踪值及其微分值分别与纵倾角和纵倾角速率的估计值作差；将垂向速度的跟踪值与垂向速度的估计值作差；

将上述差值结果进行非线性反馈控制，反馈控制结果和系统总扰动的估计值构成了纵倾角和垂向速度的控制量；

纵倾角和垂向速度的控制量输入给水下机器人进行实时控制。

2.根据权利要求1所述的方法，其特征在于：所述对纵倾角和垂向速度的设定值通过跟踪微分器进行滤波采用离散方程形式，设计如下：

其中，fst(v₁(k)-v(k),v₂(k),r₀,h)：

其中，r₀是决定过渡过程快慢的速度因子，r₀值越大，过渡过程越快；v是设定值，h是采样步长，v₁是输入信号v的跟踪值，v₂是v的近似微分信号，k是采样点；

当对纵倾角的设定值通过跟踪微分器进行滤波时，v是θ_ref；当对垂向速度的设定值通过跟踪微分器进行滤波时，v是w_ref。

3.根据权利要求1所述的方法，其特征在于：所述将纵倾角和垂向速度信号作为反馈信号，并结合纵倾角和垂向速度的实际控制量进行扩张状态观测，采用线性形式，其中，纵倾角控制的扩张状态观测采用以下公式设计：

其中，z₀₁和z₀₂分别为扩张状态观测器对水下机器人的纵倾角θ及其微分值的估计，z₀₃是系统总扰动的估计值，θ为系统输出，β₀₁，β₀₂，β₀₃为可调参数，h为采样步长，k是采样点，u₁是未加补偿的纵倾角控制量输出，b₁是控制量的放大系数；

垂向速度控制的扩张状态观测用以下公式设计：

其中，z₁₁为扩张状态观测器对水下机器人的垂向速度w的估计，z₁₂是系统总扰动的估计，w为系统输出，β₁₁和β₁₂为可调参数，k是采样点，u₂是未加补偿的垂向速度控制量输出，b₂是控制量的放大系数。

4.根据权利要求1所述的方法，其特征在于：所述将纵倾角差值结果进行非线性反馈控制，采用如下形式：

其中，e₀₂和e₀₃分别是输入信号θ_ref的跟踪值θ₁及其微分值q₁与输出信号θ和q的估计的误差，u₀₁是非线性状态误差反馈控制律的输出，u₁是纵倾角控制子系统的控制量，z₀₁和z₀₂分别为扩张状态观测器对水下机器人的纵倾角θ及其微分值的估计，z₀₃是系统总扰动的估计值，b₁是控制量的放大系数，r₁表示过渡过程的快慢，r₁越大过渡过程越快，h₁是噪声滤波因子，c是阻尼因子；

将垂向速度差值结果进行非线性反馈控制，采用如下形式：

其中，e₁₂分别是输入信号w_ref的跟踪值w₁与输出信号w的估计的误差，u₁₁是状态误差反馈控制律的输出，u₂是垂向速度控制子系统的控制量，r₂是比例系数，z₁₁为扩张状态观测器对水下机器人的垂向速度w的估计，z₁₂是系统总扰动的估计，b₂是控制量的放大系数。

5.根据权利要求2、3、4任一项所述的方法，其特征在于：纵倾角控制的参数调节规律采用以下形式：

垂向速度控制的参数调节规律采用以下形式：

。