CN104199456A - 水上无人机水面自主操纵控制方法及系统 - Google Patents

Abstract

本发明公开了一种水上无人机水面自主操纵控制方法和系统，方法包括：变参数PID控制器接收纵倾角指令和纵倾角的纵倾角误差，然后根据所述纵倾角误差计算得到俯仰角速率指令并输出给内环动态逆控制器；内环动态逆控制器分别接收速度指令和俯仰角速率指令，然后计算得到油门指令标称值和升降舵指令标称值；内环扩展状态观测器接收油门指令、升降舵指令和水上无人机内环状态变量，并计算得到油门干扰当量估计值和升降舵干扰当量估计值；水上无人机动力学模块接收油门指令和升降舵指令，并根据油门指令和升降舵指令计算得到水上无人机内环状态变量和纵倾角。本发明在不需要提前知道海情信息的情况下，实现在宽速域下的姿态控制精度要求。

Description

水上无人机水面自主操纵控制方法及系统

技术领域

本发明属于飞行控制领域，尤其涉及一种水上无人机水面操纵控制方法及系统。

背景技术

我国是一个海洋大国，有18000多千米的海岸线，6500多个岛屿，300万平方千米海疆。水上无人机是能在水面上起飞、降落和停泊的飞机，按机身机构形状可以分为船身式、浮筒式和两栖式。水上无人机由于其独特的水上起降和水面驻留作业能力，表现出极为广泛的用途，如海洋调查、海洋勘探、海洋开发、海上运输、海上生产和海洋旅游服务等活动日益频繁，、森林与城市灭火、防洪防灾、海上搜索与救护、海上缉私缉毒等领域的作业。

水上无人机最独特的问题就是水面自主起降控制问题，抗浪性是衡量水上无人机最重要的技术指标之一。同时海面海风、海流和海浪具有不确定性，起飞着陆点的海况也在不断变化。因此在一定海清等级下抵抗海浪干扰完成自主起飞控制是水上无人机的一项关键技术。

水上无人机起飞是一个加速过程。为了迅速加速到起飞离水速度，飞机需要在海浪干扰情况下，保持良好的穿浪姿态，避免在海浪作用下纵倾运动过于剧烈，损耗飞机动能。在海浪干扰下有效保持相对稳定的纵倾角是水上飞机起飞的重要条件。

因此水上无人机自主起飞控制的关键就转化为在海浪干扰下纵倾角保持问题，其中的控制问题是有界未知不确定干扰下的姿态控制。对于小型水上无人机由于无法检测海浪信息，因此只能在一定海情条件下，通过飞机姿态的闭环控制克服海浪干扰带来的不确定性，实现水上无人机的稳定穿浪，改善水上无人机的波面加速能力，增强在未知海情水域的自主作业能力。

发明内容

为满足在一定海情等级以下的未知水域进行自主起飞的控制需求，本发明的目的是提供一种水上无人机水面自主操纵控制方法，进行有界未知不确定海浪干扰下的水上无人机姿态控制。

为实现上述目的，本发明提供了一种水上无人机水面自主操纵控制方法，其特征在于，包括：

变参数PID控制器接收纵倾角指令θ_cmd和纵倾角θ的纵倾角误差θ_e，然后根据所述纵倾角误差θ_e计算得到俯仰角速率指令q_cmd并输出给内环动态逆控制器；所述纵倾角误差θ_e为所述纵倾角指令θ_cmd和纵倾角θ的差；

内环动态逆控制器分别接收速度指令V_cmd和俯仰角速率指令q_cmd，然后计算得到油门指令标称值δ_th0和升降舵指令标称值δ_e0；

内环扩展状态观测器接收油门指令δ_th、升降舵指令δ_e和水上无人机内环状态变量X＝[V q]^T，并计算得到油门干扰当量估计值z₀和升降舵干扰当量估计值z₁；所述油门指令δ_th是所述内环动态逆控制器输出的油门指令标称值δ_th0和前一时刻油门干扰当量估计值z₀的差值，所述升降舵指令δ_e是所述内环动态逆控制器输出的升降舵指令标称值δ_e0与前一时刻所述升降舵干扰当量估计值z₁的差值；

水上无人机动力学模块接收油门指令δ_th和升降舵指令δ_e，并根据油门指令δ_th和升降舵指令δ_e计算得到水上无人机内环状态变量X＝[V q]^T和纵倾角θ，所述水上无人机内环状态变量X＝[V q]^T输出给内环扩张状态观测器；所述纵倾角θ用于计算纵倾角误差θ_e。

本发明还提供了一种水上无人机水面自主操纵控制系统，其特征在于，包括：

变参数PID控制器，接收纵倾角指令θ_cmd和纵倾角θ的纵倾角误差θ_e，然后根据所述纵倾角误差θ_e计算得到俯仰角速率指令q_cmd并输出给内环动态逆控制器；所述纵倾角误差θ_e为所述纵倾角指令θ_cmd和纵倾角θ的差；

内环动态逆控制器，分别接收速度指令V_cmd和俯仰角速率指令q_cmd，然后计算得到油门指令标称值δ_th0和升降舵指令标称值δ_e0；

内环扩展状态观测器，接收油门指令δ_th、升降舵指令δ_e和水上无人机内环状态变量X＝[V q]^T，并计算得到油门干扰当量估计值z₀和升降舵干扰当量估计值z₁；所述油门指令δ_th是所述内环动态逆控制器输出的油门指令标称值δ_th0和前一时刻油门干扰当量估计值z₀的差值，所述升降舵指令δ_e是所述内环动态逆控制器输出的升降舵指令标称值δ_e0与前一时刻所述升降舵干扰当量估计值z₁的差值；

水上无人机动力学模块，接收油门指令δ_th和升降舵指令δ_e，并根据油门指令δ_th和升降舵指令δ_e计算得到水上无人机内环状态变量X＝[V q]^T和纵倾角θ，所述水上无人机内环状态变量X＝[V q]^T输出给内环扩张状态观测器；所述纵倾角θ用于计算纵倾角误差θ_e。

本发明具有的积极效果：

(1)不需要提前知道海情信息，采用扩张状态观测器实时估计海浪干扰并进行有效补偿，可以实现在一定海情等级以下的未知水域有效稳定地自主起飞。

(2)采用速度适应的控制参数调整机制，可以实现在宽速域下的姿态控制精度要求。

附图说明

图1是本发明中水上无人机水面自主操纵控制方法原理图；

图2(a)示出了本发明中水上无人机纵倾角指令随速度进行自动调整的曲线示意图；

图2(b)示出了本发明中变参数PID控制器参数随速度进行自动调整的曲线示意图；

图3是本发明中水上无人机动力学模型的结构框架图；

图4是本发明中内环动态逆控制器结构框架示意图；

图5是本发明中内环扩展状态观测器的结构框架图。

具体实施方式

下面将结合附图对本发明加以详细说明，具体实施例如下所述：

图1为本发明提出的水上无人机水面自主操纵控制装置结构示意图。如图1所示，其包括变参数PID控制器1、内环动态逆控制器2、内环扩展状态观测器3和水上无人机动力学模型4。θ_cmd为纵倾角指令，用于形成水上无人机纵向控制的目标值；θ为纵倾角，用于形成纵向控制的角度测量值；纵倾角误差θ_e，用于形成纵向控制角度跟踪的信号输入；V_cmd为速度指令，用于形成水面速度控制的目标值；q_cmd为俯仰角速率指令，用于形成俯仰角速率跟踪控制的目标值；δ_th，δ_e分别为油门指令和升降舵指令，用于发动机和升降舵舵机的控制指令；δ_th0，δ_e0分别为油门指令标称值和升降舵指令标称值，用于表示无干扰情况下的油门和升降舵指令；z₀，z₁分别为油门干扰当量和升降舵干扰当量估计值，用于干扰情况下油门和升降舵指令的补偿；X＝[V q]^T为水上无人机内环状态变量，V为水上飞机速度，q为俯仰角速率，V，q分别为水上飞机速度和俯仰角的测量值。

水上无人机水面自主操纵控制方法的原理是：变参数PID控制器1主要用于纵倾角的姿态控制，实时计算θ_cmd-θ得到纵倾角误差θ_e，并经过PID控制方法输出俯仰角速率指令，该指令包括俯仰角速率的期望值q_cmd；内环动态逆控制器2采用动态逆的非线性控制方法，对于水上无人机的内环状态变量X＝[V q]^T进行非线性解耦控制，形成油门指令标称值和升降舵指令标称值δ_th0，δ_e0；内环扩展状态观测器3接收水上无人机内环状态变量X＝[V q]^T和油门指令和升降舵指令δ_th，δ_e，实时估计速度方向和俯仰力矩方向的海浪干扰力和干扰力矩，形成油门干扰当量和升降舵干扰当量估计值z₀，z₁；油门指令标称值和升降舵指令标称值δ_th0，δ_e0与油门干扰当量和升降舵干扰当量估计值z₀，z₁相减，可以得到油门指令和升降舵指令δ_th，δ_e。

这种方法采用水上无人机内环状态变量X＝[V q]^T和纵倾角θ控制相互嵌套的方案，内环动态逆控制器2、内环扩展状态观测器3、水上无人机动力学模型处理器4组成内回路控制，主要用于水上无人机速度V有效跟踪速度控制目标值——速度指令V_cmd和俯仰角q跟踪俯仰角速率控制目标值——俯仰角速率指令q_cmd，具体说明如下：参见图1，可以看到内环动态逆控制器2的输入是速度指令V_cmd、俯仰角速率指令q_cmd和水上无人机内环状态变量X＝[V q]^T，其中V为水上飞机速度，q为俯仰角速率。

内环动态逆控制器2的输出是油门指令标称值和升降舵指令标称值δ_th0，δ_e0，并与内环扩张状态观测器3的输出(油门干扰当量和升降舵干扰当量估计值z₀，z₁)相减，可以得到油门指令和升降舵指令δ_th，δ_e，输入到水上无人机动力学模型处理器4，形成闭环控制机制，实现水上无人机速度V有效跟踪速度控制目标值。变参数PID控制器1和纵倾角θ的状态反馈，组成外回路控制，主要用于水上无人机纵倾角θ有效跟踪纵倾角控制目标值——纵倾角指令θ_cmd，具体说明如下，参见图1，变参数PID控制器1的输入是纵倾角误差θ_e，经过PID控制器计算，形成俯仰角速率指令q_cmd，结合上述内环动态逆控制器2使得俯仰角q跟踪俯仰角速率控制目标值——俯仰角速率指令q_cmd，形成纵倾角θ的闭环控制机制，从而实现纵倾角θ有效跟踪纵倾角控制目标值——纵倾角指令θ_cmd。

水上无人机水面自主操纵控制方法的具体步骤是：

变参数PID控制器1，其接收纵倾角指令θ_cmd和纵倾角θ的纵倾角误差θ_e，然后根据所述纵倾角误差θ_e计算得到俯仰角速率指令q_cmd并输出给内环动态逆控制器；所述纵倾角误差θ_e为所述纵倾角指令θ_cmd和纵倾角θ的差；

内环动态逆控制器2，其分别接收速度指令V_cmd(由用户输入的速度期望值)和俯仰角速率指令q_cmd，然后根据内环动态逆控制器2设计的控制器(具体详见后述公式5)计算得到油门指令标称值δ_th0，并根据内环动态逆控制器2设计的控制器(具体详见后述公式5)计算得到升降舵指令标称值δ_e0；

内环扩展状态观测器3，其接收油门指令δ_th、升降舵指令δ_e和水上无人机内环状态变量X＝[V q]^T，并根据扩展状态观测器32(见公式9)计算得到油门干扰当量估计值z₀，根据扩展状态观测器32(见公式9)，计算得到升降舵干扰当量估计值z₁；所述油门指令δ_th是所述内环动态逆控制器2输出的油门指令标称值δ_th0和前一时刻油门干扰当量估计值z₀的差值，所述升降舵指令δ_e是所述内环动态逆控制器2输出的升降舵指令标称值δ_e0与前一时刻所述升降舵干扰当量估计值z₁的差值；

水上无人机动力学模型4，其接收油门指令δ_th和升降舵指令δ_e，并根据油门指令δ_th和升降舵指令δ_e计算得到水上无人机内环状态变量X＝[V q]^T和纵倾角θ，采用的方式是公式1基础上的数值积分计算，所述水上无人机内环状态变量X＝[V q]^T输出给内环扩张状态观测器3；所述纵倾角θ用于计算纵倾角误差θ_e。

图2(a)是示出了本发明中水上无人机纵倾角指令θ_cmd随速度V进行自动调整的曲线示意图；图2(b)示出了本发明中变参数PID控制器参数随速度V进行自动调整的曲线示意图。水上无人机的纵倾角指令θ_cmd在起飞加速过程中，存在一个上不稳定区和下不稳定区，只有在一定范围内纵倾角指令才是可以稳定控制的，纵倾角指令随速度变化进行自动调节。同时纵倾角控制的参数在起飞加速过程中，进行线性插值计算。

变参数PID控制器1具体的设计方法如下：

根据图2(a)～(b)所示，选取水上无人机纵倾角指令θ_cmd的典型指令曲线，选取特征速度值V₁，V₂，…，V_n(在速度区间内均匀取值)，所选取的所述特征速度值V₁，V₂，…，V_n在所选取的典型指令曲线上对应的水上无人机纵倾角指令典型值为θ_cmd1，θ_cmd2，…，θ_cmdn，对于水上飞机速度V按照上述特征速度值和纵倾角指令典型值进行线性插值，得到当前速度下的水上无人机纵倾角指令θ_cmd。

纵倾角控制采用变参数PID控制器， $q_{\mathrm{cmd}}=k_{\mathrm{\θp}}\left(V\right){\mathrm{\θ}}_e+k_{\mathrm{\θd}}\left(V\right){\stackrel{\·}{\mathrm{\θ}}}_e+k_{\mathrm{\θi}}\left(V\right){\∫\mathrm{\θ}}_e\mathrm{dt}$ 其中，θ_e是纵倾角误差，是纵倾角误差θ_e的导数；控制器参数为比例系数、微分系数和积分系数分别为k_θp(V)，k_θd(V)，k_θi(V)，是随速度的变化量，即k_θp，k_θd，k_θi是速度V的函数，分别为纵倾角控制的比例系数、微分系数和积分系数，选取特征速度值V₁，V₂，…，V_n和纵倾角控制的比例系数典型值k_θp1，k_θp2，…，k_θpn，进行线性插值得到其中i的取值为{i∈[1，n]|V_i＜V＜V_i+1}。k_θd(V)，k_θi(V)的计算方法类似， 其中i的取值为{i∈[1，n]|V_i＜V＜V_i+1}，分别为纵倾角控制的微分系数和积分系数的线性插值方法，其中k_θd1，k_θd2，…，k_θdn、k_θi1，k_θi2，…，k_θin分别是针对特征速度值V₁，V₂，…，V_n的微分系数典型值、积分系数典型值。

图3是水上无人机动力学模型，可以表示为：

$\left.\begin{array}{c}m\stackrel{\·}{V}=T\cos (\mathrm{\α}+{\mathrm{\α}}_t)-D_a-N_w\sin \mathrm{\α}-D_f\·\cos \mathrm{\α}+G_{\mathrm{xa}}\\ \mathrm{mV}\stackrel{\·}{\mathrm{\α}}=\mathrm{mVq}-T\sin (\mathrm{\α}+{\mathrm{\α}}_t)-L_a-N_w\·\cos \mathrm{\α}+D_f\·\sin \mathrm{\α}+G_{\mathrm{za}}\\ I_y\stackrel{\·}{q}=M_a+M_w+M_T\\ \stackrel{\·}{\mathrm{\θ}}=q\\ {\stackrel{\·}{x}}_g=u^{\′}\cos \mathrm{\θ}+w^{\′}\sin \mathrm{\θ}\\ {\stackrel{\·}{z}}_g=-u^{\′}\sin \mathrm{\θ}+w^{\′}\cos \mathrm{\θ}\end{array}\right\}---\left(1\right)$

其中，m为水上飞机质量；V为水上飞机速度；T＝t(δ_th)为发动机推力，是油门指令δ_th的函数；u′，w′为机体坐标系(X_b，Z_b)下的飞行速度；α表示水上飞机迎角；α_t为发动机安装角；N_w为飞机底部的水动压力；D_a为空气阻力；D_f为水动阻力；q为俯仰角速率；I_y为纵向俯仰转动惯量；x_g，z_g为水上飞机在大地坐标系下位置；θ为飞机纵倾角，g为重力加速度；G_xa，G_za为重力沿稳定坐标下的分量；G_xa＝mg(-cosαsinθ+sinαcosθ)，G_za＝mg(sinαsinθ+cosαcosθ)；M_a，M_w，M_T分别为空气、水动力和发动机产生的俯仰力矩，L_a为气动升力。

对于内回路模型，可以表示为：

$\begin{array}{c}\stackrel{\·}{X}=J{\left(X\right)}^{-1}F\left(X\right)+J{\left(X\right)}^{-1}G\left(X\right)u+J{\left(X\right)}^{-1}w\left(X\right)\\ =f\left(X\right)+g\left(X\right)u+\mathrm{\Δ}\left(X\right)\\ =f\left(X\right)+g\left(X\right)(u_0-Z)+\mathrm{\Δ}\left(X\right)\end{array}---\left(2\right)$

其中：

u＝[δ_th δ_e]^T

X＝[V q]^T

$J\left(X\right)=\left[\begin{array}{cc}m& 0\\ 0& I_y\end{array}\right]$

$f\left(X\right)=\left[\begin{array}{c}{G}_{\mathrm{xa}}-D_a\\ M_{a0}\end{array}\right]---\left(3\right)$

$g\left(X\right)=\left[\begin{array}{cc}{C}_T^{{\mathrm{\δ}}_{\mathrm{th}}}\cos (\mathrm{\α}+{\mathrm{\α}}_t)& 0\\ M_T^{{\mathrm{\δ}}_{\mathrm{th}}}\cos {\mathrm{\α}}_t& M_{\mathrm{\α}}^{{\mathrm{\δ}}_e}\end{array}\right]$

$w\left(X\right)=\left[\begin{array}{c}-N_w\sin \mathrm{\α}-D_f\cos \mathrm{\α}+{\mathrm{\Δ}}_V\\ M_w+M_a^{\stackrel{\·}{a}}\stackrel{\·}{\mathrm{\α}}+{\mathrm{\Δ}}_q\end{array}\right]$

其中，为推力系数，为推力俯仰力矩系数，为升降舵俯仰力矩系数，为迎角关于俯仰力矩动导数，M_a0为零升力俯仰力矩，Δ_q，Δ_V为未知不确定项。控制输入u＝[δ_th δ_e]^T＝u₀-Z＝[δ_th0 δ_e0]^T-[z₁ z₂]^T。

图4是内环动态逆控制器2，包括期望状态计算21、逆控制律计算22、水上无人机动力学模型4包含状态变量计算41、积分环节42。期望状态计算21一端连接系统指令X_c和水上无人机内环状态变量X＝[V q]^T，一端连接X_d＝[V_d q_d](系统过渡过程中的速度指令Vd_和系统过渡过程中的俯仰角速率指令q_d)；逆控制律计算22一端连接系统期望动态特性X_d，一端连接输入控制量标称值u₀＝[δ_th0 δ_e0]；状态变量计算42一端连接输入控制量u(计算方法见公式10)，一端连接水上无人机内环状态变量X＝[V q]^T的导数；积分环节42一端连接水上无人机内环状态变量X＝[V q]^T的导数，一端连接水上无人机内环状态变量X＝[V q]^T；内环动态逆控制器2通过系统指令X_c和水上无人机内环状态变量X＝[V q]^T，计算求得X_d＝[V_d q_d](系统过渡过程中的速度指令Vd和系统过渡过程中的俯仰角速率指令q_d)，进行逆控制律计算22，得出输入控制量标称值u₀＝[δ_th0 δ_e0]，经过状态变量计算41和积分环节42形成水上无人机内环状态变量X＝[V q]^T。

内环动态逆控制器2具体的设计方法如下：

系统期望模型，即期望状态计算21的具体形式为：

${\stackrel{\·}{X}}_d=K(X_c-X)---\left(4\right)$

其中，X_d＝[V_d q_d]为系统过渡过程的速度指令V_d和俯仰角速率指令q_d，X_c＝[V_cmd q_cmd]为系统控制指令，即系统期望的速度指令V_cmd和期望的俯仰角速率指令q_cmd，X＝[V q]^T是水上无人机内环状态变量，V为水上飞机速度，q为俯仰角速率。K为期望特征矩阵 $\left[\begin{array}{cc}{k}_1& 0\\ 0& k_2\end{array}\right],$ k1和k2是系统过渡过程(速度指令V_d和俯仰角速率指令q_d的期望过渡过程)的一阶惯性时间常数。X_c＝[V_cmd q_cmd]需要经过公式4及数值积分运算可以得到X_d＝[V_d q_d]，以保证系统过渡过程指令X_d＝[V_d q_d]的平滑，没有突变值。

逆控制律计算22，基于公式1和公式2，并由内环动态逆控制器2设计的控制器可以表示为：

$\begin{array}{c}{u}_0=\left[\begin{array}{cc}{\mathrm{\δ}}_{\mathrm{th}0}& {\mathrm{\δ}}_{e0}\end{array}\right]=g{\left(X\right)}^{-1}(-f\left(X\right)+{\stackrel{\·}{X}}_e)\\ =g{\left(X\right)}^{-1}(-f\left(X\right)+K(X_c-X))\end{array}---\left(5\right)$

状态变量计算41的具体方法如下，见公式1，对于内环系统，其状态变量为X＝[V q]^T，指的是速度和俯仰角速率的动力学系统，可以表示为：

$\stackrel{\·}{X}=f\left(X\right)+g\left(X\right)(u_0-Z)+\mathrm{\Δ}\left(X\right),---\left(6\right)$

其中，X＝[V q]^T为水上无人机内环状态变量，u₀＝[δ_th0 δ_e0]为输入控制量标称值，见图2中的内环动态逆控制器2，计算方法见公式5；f(X)，g(X)为水上飞机非线性系统矩阵，见公式1。Z＝[z₀ z₁]^T为状态变量的估计值，其中油门干扰当量和升降舵干扰当量估计值z₀，z₁计算公式9；Δ(X)＝J(X)^-1w(X)表示不确定项w(X)向状态变量X＝[V q]^T导数的归一化值，见公式3。

积分环节42具体完成如下功能：完成状态变量的导数——到X＝[V a]^T的计算，采用数值积分的方法，具体方法如下：其中X₀为状态变量的初值，X_i为第i步的计算状态变量的导数，h为每一步计算的时间间隔。

图5是内环扩展状态观测器3，包括水上飞机速度V扩展状态观测器31和俯仰角速率q扩展状态观测器32；水上飞机速度V扩展状态观测器31一端与水上飞机速度V、油门指令δ_th连接，一端与增益调节器311连接；增益调节器311一端与水上飞机速度V回归函数估计值Δ_2V连接，一端与油门干扰当量z₀连接。俯仰角速率q扩展状态观测器32一端与俯仰角速率q、升降舵指令δ_e连接，一端与增益调节器321连接；增益调节器321一端与俯仰角速率q回归函数估计值Δ_2q连接，一端与升降舵干扰当量估计值z₁连接。

内环扩展状态观测器3工作方法是：水上飞机速度V、油门指令δ_th经过水上飞机速度V扩展状态观测器31，形成水上飞机速度V回归函数估计值Δ_2V，再经过增益调节器311，形成油门干扰当量z₀；俯仰角速率q、升降舵指令δ_e经过俯仰角速率q扩展状态观测器32，形成俯仰角速率q回归函数估计值Δ_2q，再经过增益调节器311，形成升降舵干扰当量估计值z₁。

水上飞机速度V扩展状态观测器31的具体设计方法是

由公式2-3可以得：

$\begin{array}{c}\stackrel{\·}{V}=(G_{\mathrm{xa}}-D_a)V+C_T^{{\mathrm{\δ}}_{\mathrm{th}}}\cos (\mathrm{\α}+{\mathrm{\α}}_t){\mathrm{\δ}}_{\mathrm{th}}+{\mathrm{\ϖ}}_V\\ =f_V+g_V{\mathrm{\δ}}_{\mathrm{th}}+{\mathrm{\ϖ}}_V\end{array}---(7-1)$

其中f_V＝(G_xa-D_a)V， $g_V=C_T^{{\mathrm{\δ}}_{\mathrm{th}}}\cos \left({\mathrm{\α}+\mathrm{\α}}_t\right).$

设计如下扩展状态观测器

$\left\{\begin{array}{c}{e}_V={\mathrm{\Δ}}_{1V}-V\\ {\stackrel{\·}{\mathrm{\Δ}}}_{1V}={\mathrm{\Δ}}_{1q}-{\mathrm{\β}}_{V01}\mathrm{fal}(e_V,\frac{1}{2},h_V)+g_V{\mathrm{\δ}}_{\mathrm{th}}\\ {\stackrel{\·}{\mathrm{\Δ}}}_{2V}=-{\mathrm{\β}}_{V02}\mathrm{fal}(e_V,\frac{1}{2},h_V)\end{array}\right.---(7-2)$

其中，f_V＝(G_xa-D_a)V为水上飞机速度V的回归函数，为水上飞机速度V的控制输入增益，为水上飞机速度V导数的未知摄动变量，油门指令δ_th为水上飞机速度V状态方程的控制量输入，e_V为状态跟踪误差，Δ_1V，Δ_2V分别为水上飞机速度V以及的估计值，β_V01，β_V02为水上飞机速度V观测器系数，即观测器估计参数收敛速度的控制参数，由参数收敛速度决定，h_V为非线性组合函数的误差分界限， $\mathrm{fal}(e_V,1/2,h_V)=\left\{\begin{array}{c}{\left|e_V\right|}^{1/2}\mathrm{sign}\left(e_V\right),\left|e_V\right|>h_V\\ e_V/{h_V}^{1/2},\left|e_V\right|\≤h_V\end{array}\right.$ 为非线性组合函数。Δ_1V，Δ_2V通过公式数值积分计算求得。

俯仰角速率q扩展状态观测器32的具体设计方法是

由公式2-3可以得：

$\begin{array}{c}\stackrel{\·}{q}=M_{a0}q+\left(M_T^{{\mathrm{\δ}}_{\mathrm{th}}}\cos {\mathrm{\α}}_t\right){\mathrm{\δ}}_{\mathrm{th}}+M_{\mathrm{\α}}^{{\mathrm{\δ}}_e}{\mathrm{\δ}}_e+{\mathrm{\ϖ}}_q\\ =f_q+g_q{\mathrm{\δ}}_e+{\mathrm{\ϖ}}_q\end{array}---(8-1)$

其中 $f_q=M_{a0}q+\left(M_T^{{\mathrm{\δ}}_{\mathrm{th}}}\cos {\mathrm{\α}}_t\right){\mathrm{\δ}}_{\mathrm{th}},g_q=M_{\mathrm{\α}}^{{\mathrm{\δ}}_e}.$

设计如下扩展状态观测器

$\left\{\begin{array}{c}{e}_q={\mathrm{\Δ}}_{1q}-q\\ {\stackrel{\·}{\mathrm{\Δ}}}_{1q}={\mathrm{\Δ}}_{1q}-{\mathrm{\β}}_{q01}\mathrm{fal}(e_q,\frac{1}{2},h_q)+g_q{\mathrm{\δ}}_e\\ {\stackrel{\·}{\mathrm{\Δ}}}_{2q}=-{\mathrm{\β}}_{q02}\mathrm{fal}(e_q,\frac{1}{2},h_q)\end{array}\right.---(8-2)$

其中，为俯仰角速率q的回归函数，M_a0为零升力俯仰力矩，为俯仰角速率q的控制输入增益，为俯仰角速率q的未知摄动变量，升降舵指令δ_e为俯仰角速率q的控制量输入，e_q为俯仰角速率q状态跟踪误差，Δ_1q，Δ_2q分别为俯仰角速率q以及的估计值，β_q01，β_q02为俯仰角速率q观测器系数，即观测器估计参数收敛速度的控制参数，由参数收敛速度决定，h_q为非线性组合函数的误差分界限， $\mathrm{fal}(e_q,1/2,h_q)=\left\{\begin{array}{c}{\left|e_q\right|}^{1/2}\mathrm{sign}\left(e_q\right),\left|e_q\right|>h_q\\ e_q/{h_q}^{1/2},\left|e_q\right|\≤h_q\end{array}\right.$ 为非线性组合函数。Δ_1q，Δ_2q通过公式数值积分计算求得。

针对水上飞机速度V和俯仰角速率q，分别设计水上飞机速度V扩展状态观测器31和俯仰角速率q扩展状态观测器32，形成油门干扰当量和升降舵干扰当量估计值z₀，z₁，

其中，

Z＝[z₀ z₁]^T，z₀＝Δ_2V/g_V，z₁＝2_Δq/g_q  (9)

Δ_2V，Δ_2q为水上飞机速度V和俯仰角速率q公式(7-1，8-1)的f_V，f_g的估计值，g_V，g_q为对应状态变量的控制输入增益，计算方法如下

$g_V=C_T^{{\mathrm{\δ}}_{\mathrm{th}}}\cos \left({\mathrm{\α}+\mathrm{\α}}_t\right),g_q=M_{\mathrm{\α}}^{{\mathrm{\δ}}_e}.$

油门指令标称值和升降舵指令标称值δ_th0，δ_e0与油门干扰当量和升降舵干扰当量估计值z₀，z₁相减，可以得到油门指令和升降舵指令δ_th，δ_e，即u＝[δ_th δ_e]^T＝u₀-Z＝[δ_th0 δ_e0]^T-[z₀ z₁]^T。  (10)

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种水上无人机水面自主操纵控制方法，其特征在于，包括：

变参数PID控制器接收纵倾角指令θ_cmd和纵倾角θ的纵倾角误差θ_e，然后根据所述纵倾角误差θ_e计算得到俯仰角速率指令q_cmd并输出给内环动态逆控制器；所述纵倾角误差θ_e为所述纵倾角指令θ_cmd和纵倾角θ的差；

内环动态逆控制器分别接收速度指令V_cmd和俯仰角速率指令q_cmd，然后计算得到油门指令标称值δ_th0和升降舵指令标称值δ_e0；

内环扩展状态观测器接收油门指令δ_th、升降舵指令δ_e和水上无人机内环状态变量X＝[V q]^T，并计算得到油门干扰当量估计值z₀和升降舵干扰当量估计值z₁；所述油门指令δ_th是所述内环动态逆控制器输出的油门指令标称值δ_th0和前一时刻油门干扰当量估计值z₀的差值，所述升降舵指令δ_e是所述内环动态逆控制器输出的升降舵指令标称值δ_e0与前一时刻所述升降舵干扰当量估计值z₁的差值；

水上无人机动力学模块接收油门指令δ_th和升降舵指令δ_e，并根据油门指令δ_th和升降舵指令δ_e计算得到水上无人机内环状态变量X＝[V q]^T和纵倾角θ，所述水上无人机内环状态变量X＝[V q]^T输出给内环扩张状态观测器；所述纵倾角θ用于计算纵倾角误差θ_e。

2.如权利要求1所述的方法，其中，俯仰角速率指令q_cmd如下计算：

$q_{\mathrm{cmd}}=k_{\mathrm{\θp}}\left(V\right){\mathrm{\θ}}_e+k_{\mathrm{\θd}}\left(V\right){\stackrel{\·}{\mathrm{\θ}}}_e+k_{\mathrm{\θi}}\left(V\right){\∫\mathrm{\θ}}_e\mathrm{dt}$

其中，θθ_e是纵倾角误差，是纵倾角误差θ_e的导数；k_θp，k_θd，k_θi是速度V的函数，分别为纵倾角控制的比例系数、微分系数和积分系数。

3.如权利要求2所述的方法，其中，所述计算得到油门指令标称值δ_th0和升降舵指令标称值δ_e0具体包括：

利用接收到的系统控制指令中的系统期望的速度指令V_cmd和俯仰角速率指令q_cmd计算得到系统过渡过程中速度指令和俯仰角速率指令；

根据系统过渡过程中速度指令和俯仰角速率指令计算得到油门指令标称值δ_th0和升降舵指令标称值δ_e0。

4.如权利要求3所述的方法，其中，如下计算得到系统过渡过程中速度指令和俯仰角速率指令：

${\stackrel{\·}{X}}_d=K(X_c-X)$

其中，X_d＝[V_d q_d]为系统过渡过程中的速度指令V_d和系统过渡过程中的俯仰角速率指令q_d，X_c＝[V_cmd q_cmd]为系统期望的速度指令V_cmd和俯仰角速率指令q_cmd，K为期望特征矩阵，X＝[V q]^T为水上无人机内环状态变量，其中V为水上无人机速度，q为俯仰角速率。

5.如权利要求3所述的方法，其中，计算得到油门指令标称值δ_th0和升降舵指令标称值δ_e0：

$\begin{array}{c}{u}_0=\left[\begin{array}{cc}{\mathrm{\δ}}_{\mathrm{th}0}& {\mathrm{\δ}}_{e0}\end{array}\right]=g{\left(X\right)}^{-1}(-f\left(X\right)+{\stackrel{\·}{X}}_e)\\ =g{\left(X\right)}^{-1}(-f\left(X\right)+K(X_c-X))\end{array}$

其中，f(X)，g(X)为水上无人机非线性系统矩阵，X_d＝[V_d q_d]为系统过渡过程中的速度指令V_d和系统过渡过程中的俯仰角速率指令q_d，K为期望特征矩阵 $\left[\begin{array}{cc}{k}_1& 0\\ 0& k_2\end{array}\right],$ 其中k₁和k₂是系统过渡过程的一阶惯性时间常数，X_c＝[V_cmd q_cmd]为系统期望的速度指令V_cmd和俯仰角速率指令q_cmd，X＝[V q]^T为水上无人机内环状态变量，其中V为水上无人机速度，q为俯仰角速率。

6.如权利要求1所述的方法，其中，所述油门干扰当量估计值z₀和升降舵干扰当量估计值z₁如下计算：

z₀＝Δ_2V/g_V，z₁＝Δ_2q/g_q

其中，Δ_2V，Δ_2q为水上无人机速度和俯仰角速率的对应的回归函数f_V，f_q的估计值，为俯仰角速率q的回归函数，f_V＝(G_xa-D_a)V为水上无人机速度V的回归函数，g_V，g_q为对应的控制输入增益；α_t为发动机安装角；为推力俯仰力矩系数，M_a0为零升力俯仰力矩，D_a为空气阻力，G_xa为重力沿稳定坐标X轴的分量。

7.如权利要求1所述的方法，其中，水上无人机内环状态变量X＝[V q]^T和纵倾角θ如下计算得到：

$\left.\begin{array}{c}m\stackrel{\·}{V}=T\cos (\mathrm{\α}+{\mathrm{\α}}_t)-D_a-N_w\sin \mathrm{\α}-D_f\·\cos \mathrm{\α}+G_{\mathrm{xa}}\\ \mathrm{mV}\stackrel{\·}{\mathrm{\α}}=\mathrm{mVq}-T\sin (\mathrm{\α}+{\mathrm{\α}}_t)-L_a-N_w\·\cos \mathrm{\α}+D_f\·\sin \mathrm{\α}+G_{\mathrm{za}}\\ I_y\stackrel{\·}{q}=M_a+M_w+M_T\\ \stackrel{\·}{\mathrm{\θ}}=q\\ {\stackrel{\·}{x}}_g=u^{\′}\cos \mathrm{\θ}+w^{\′}\sin \mathrm{\θ}\\ {\stackrel{\·}{z}}_g=-u^{\′}\sin \mathrm{\θ}+w^{\′}\cos \mathrm{\θ}\end{array}\right\}$

其中，m为水上无人机质量；V为水上无人机速度；T＝t(δ_th)为发动机推力，是油门指令δ_th的函数；u′，w′为机体坐标系下的飞行速度；α表示水上无人机迎角；α_t为发动机安装角；N_w为飞机底部的水动压力；D_a为空气阻力；D_f为水动阻力；q为俯仰角速率；I_y为纵向俯仰转动惯量；x_g，z_g为大地坐标系下位置；θ为飞机纵倾角，g为重力加速度；G_xa，G_za为重力沿坐标x、z轴下的分量；M_a，M_w，M_T分别为空气、水动力和发动机产生的俯仰力矩，L_a为气动升力。

8.一种水上无人机水面自主操纵控制系统，其特征在于，包括：

变参数PID控制器，接收纵倾角指令θ_cmd和纵倾角θ的纵倾角误差θ_e，然后根据所述纵倾角误差θ_e计算得到俯仰角速率指令q_cmd并输出给内环动态逆控制器；所述纵倾角误差θ_e为所述纵倾角指令θ_cmd和纵倾角θ的差；

内环动态逆控制器，分别接收速度指令V_cmd和俯仰角速率指令q_cmd，然后计算得到油门指令标称值δ_th0和升降舵指令标称值δ_e0；

内环扩展状态观测器，接收油门指令δ_th、升降舵指令δ_e和水上无人机内环状态变量X＝[V q]^T，并计算得到油门干扰当量估计值z₀和升降舵干扰当量估计值z₁；所述油门指令δ_th是所述内环动态逆控制器输出的油门指令标称值δ_th0和前一时刻油门干扰当量估计值z₀的差值，所述升降舵指令δ_e是所述内环动态逆控制器输出的升降舵指令标称值δ_e0与前一时刻所述升降舵干扰当量估计值z₁的差值；

水上无人机动力学模块，接收油门指令δ_th和升降舵指令δ_e，并根据油门指令δ_th和升降舵指令δ_e计算得到水上无人机内环状态变量X＝[V q]^T和纵倾角θ，所述水上无人机内环状态变量X＝[V q]^T输出给内环扩张状态观测器；所述纵倾角θ用于计算纵倾角误差θ_e。