水上无人机水面自主操纵控制方法及系统[发明专利]

(12)发明专利申请

(10)申请公布号(10)申请公布号 CN 104199456 A(43)申请公布日 2014.12.10

(21)申请号 201410449085.1(22)申请日 2014.09.04

(71)申请人中国科学院自动化研究所

地址100190 北京市海淀区中关村东路95

号(72)发明人范国梁 易建强

(74)专利代理机构中科专利商标代理有限责任

公司 11021

代理人宋焰琴(51)Int.Cl.

G05D 1/08(2006.01)

权利要求书3页 说明书8页 附图3页权利要求书3页 说明书8页 附图3页

(54)发明名称

水上无人机水面自主操纵控制方法及系统(57)摘要

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

权 利 要 求 书

1/3页

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

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

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

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

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

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

其中，θθe是纵倾角误差，是纵倾角误差θe的导数；kθp，kθd，kθi是速度V的函数，分别为纵倾角控制的比例系数、微分系数和积分系数。3.如权利要求2所述的方法，其中，所述计算得到油门指令标称值δth0和升降舵指令标称值δe0具体包括：

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

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

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

其中，Xd＝[Vd qd]为系统过渡过程中的速度指令Vd和系统过渡过程中的俯仰角速率指令qd，Xc＝[Vcmd qcmd]为系统期望的速度指令Vcmd和俯仰角速率指令qcmd，K为期望特征矩阵，X＝[V q]T为水上无人机内环状态变量，其中V为水上无人机速度，q为俯仰角速率。

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

其中，f(X)，g(X)为水上无人机非线性系统矩阵，Xd＝[Vd qd]为系统过渡过程中的速

2

CN 104199456 A

权 利 要 求 书

2/3页

度指令Vd和系统过渡过程中的俯仰角速率指令qd，K为期望特征矩阵其中k1和

k2是系统过渡过程的一阶惯性时间常数，Xc＝[Vcmd qcmd]为系统期望的速度指令Vcmd和俯仰角速率指令qcmd，X＝[V q]T为水上无人机内环状态变量，其中V为水上无人机速度，q为俯仰角速率。

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

z0＝Δ2V/gV，z1＝Δ2q/gq其中，Δ2V，Δ2q为水上无人机速度和俯仰角速率的对应的回归函数fV，fq的估计值，

为俯仰角速率q的回归函数，fV＝(Gxa-Da)V为水上无人机速度

V的回归函数，gV，gq为对应的控制输入增益；αt为发动机安装角；

为推力俯仰力矩系

数，Ma0为零升力俯仰力矩，Da为空气阻力，Gxa为重力沿稳定坐标X轴的分量。7.如权利要求1所述的方法，其中，水上无人机内环状态变量X＝[V q]T和纵倾角θ如下计算得到：

其中，m为水上无人机质量；V为水上无人机速度；T＝t(δth)为发动机推力，是油门指令δth的函数；u′，w′为机体坐标系下的飞行速度；α表示水上无人机迎角；αt为发动机安装角；Nw为飞机底部的水动压力；Da为空气阻力；Df为水动阻力；q为俯仰角速率；Iy为纵向俯仰转动惯量；xg，zg为大地坐标系下位置；θ为飞机纵倾角，g为重力加速度；Gxa，Gza为重力沿坐标x、z轴下的分量；Ma，Mw，MT分别为空气、水动力和发动机产生的俯仰力矩，La为气动升力。

8.一种水上无人机水面自主操纵控制系统，其特征在于，包括：变参数PID控制器，接收纵倾角指令θcmd和纵倾角θ的纵倾角误差θe，然后根据所述纵倾角误差θe计算得到俯仰角速率指令qcmd并输出给内环动态逆控制器；所述纵倾角误差θe为所述纵倾角指令θcmd和纵倾角θ的差；

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

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

3

CN 104199456 A

权 利 要 求 书

3/3页

与前一时刻所述升降舵干扰当量估计值z1的差值；

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

4

CN 104199456 A

说 明 书

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

1/8页

技术领域

[0001]

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

背景技术

我国是一个海洋大国，有18000多千米的海岸线，6500多个岛屿，300万平方千米海疆。水上无人机是能在水面上起飞、降落和停泊的飞机，按机身机构形状可以分为船身式、浮筒式和两栖式。水上无人机由于其独特的水上起降和水面驻留作业能力，表现出极为广泛的用途，如海洋调查、海洋勘探、海洋开发、海上运输、海上生产和海洋旅游服务等活动日益频繁，、森林与城市灭火、防洪防灾、海上搜索与救护、海上缉私缉毒等领域的作业。[0003] 水上无人机最独特的问题就是水面自主起降控制问题，抗浪性是衡量水上无人机最重要的技术指标之一。同时海面海风、海流和海浪具有不确定性，起飞着陆点的海况也在不断变化。因此在一定海清等级下抵抗海浪干扰完成自主起飞控制是水上无人机的一项关键技术。

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

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

[0002]

发明内容

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

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

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

[0009] 内环动态逆控制器分别接收速度指令Vcmd和俯仰角速率指令qcmd，然后计算得到油门指令标称值δth0和升降舵指令标称值δe0；[0010] 内环扩展状态观测器接收油门指令δth、升降舵指令δe和水上无人机内环状态变量X＝[V q]T，并计算得到油门干扰当量估计值z0和升降舵干扰当量估计值z1；所述油门指令δth是所述内环动态逆控制器输出的油门指令标称值δth0和前一时刻油门干扰当

5

CN 104199456 A

说 明 书

2/8页

量估计值z0的差值，所述升降舵指令δe是所述内环动态逆控制器输出的升降舵指令标称值δe0与前一时刻所述升降舵干扰当量估计值z1的差值；

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

[0012] 本发明还提供了一种水上无人机水面自主操纵控制系统，其特征在于，包括：[0013] 变参数PID控制器，接收纵倾角指令θcmd和纵倾角θ的纵倾角误差θe，然后根据所述纵倾角误差θe计算得到俯仰角速率指令qcmd并输出给内环动态逆控制器；所述纵倾角误差θe为所述纵倾角指令θcmd和纵倾角θ的差；[0014] 内环动态逆控制器，分别接收速度指令Vcmd和俯仰角速率指令qcmd，然后计算得到油门指令标称值δth0和升降舵指令标称值δe0；[0015] 内环扩展状态观测器，接收油门指令δth、升降舵指令δe和水上无人机内环状态变量X＝[V q]T，并计算得到油门干扰当量估计值z0和升降舵干扰当量估计值z1；所述油门指令δth是所述内环动态逆控制器输出的油门指令标称值δth0和前一时刻油门干扰当量估计值z0的差值，所述升降舵指令δe是所述内环动态逆控制器输出的升降舵指令标称值δe0与前一时刻所述升降舵干扰当量估计值z1的差值；[0016] 水上无人机动力学模块，接收油门指令δth和升降舵指令δe，并根据油门指令δth和升降舵指令δe计算得到水上无人机内环状态变量X＝[V q]T和纵倾角θ，所述水上无人机内环状态变量X＝[V q]T输出给内环扩张状态观测器；所述纵倾角θ用于计算纵倾角误差θe。

[0017] 本发明具有的积极效果：

[0018] (1)不需要提前知道海情信息，采用扩张状态观测器实时估计海浪干扰并进行有效补偿，可以实现在一定海情等级以下的未知水域有效稳定地自主起飞。[0019] (2)采用速度适应的控制参数调整机制，可以实现在宽速域下的姿态控制精度要求。

附图说明

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

[0021]

图2(a)示出了本发明中水上无人机纵倾角指令随速度进行自动调整的曲线示意图2(b)示出了本发明中变参数PID控制器参数随速度进行自动调整的曲线示意

图；

[0022]

图；

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

[0024] 图4是本发明中内环动态逆控制器结构框架示意图；[0025] 图5是本发明中内环扩展状态观测器的结构框架图。

[0023]

具体实施方式

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

6

CN 104199456 A[0027]

说 明 书

3/8页

图1为本发明提出的水上无人机水面自主操纵控制装置结构示意图。如图1所示，其包括变参数PID控制器1、内环动态逆控制器2、内环扩展状态观测器3和水上无人机动力学模型4。θcmd为纵倾角指令，用于形成水上无人机纵向控制的目标值；θ为纵倾角，用于形成纵向控制的角度测量值；纵倾角误差θe，用于形成纵向控制角度跟踪的信号输入；Vcmd为速度指令，用于形成水面速度控制的目标值；qcmd为俯仰角速率指令，用于形成俯仰角速率跟踪控制的目标值；δth，δe分别为油门指令和升降舵指令，用于发动机和升降舵舵机的控制指令；δth0，δe0分别为油门指令标称值和升降舵指令标称值，用于表示无干扰情况下的油门和升降舵指令；z0，z1分别为油门干扰当量和升降舵干扰当量估计值，用于干扰情况下油门和升降舵指令的补偿；X＝[V q]T为水上无人机内环状态变量，V为水上飞机速度，q为俯仰角速率，V，q分别为水上飞机速度和俯仰角的测量值。[0028] 水上无人机水面自主操纵控制方法的原理是：变参数PID控制器1主要用于纵倾角的姿态控制，实时计算θcmd-θ得到纵倾角误差θe，并经过PID控制方法输出俯仰角速率指令，该指令包括俯仰角速率的期望值qcmd；内环动态逆控制器2采用动态逆的非线性控制方法，对于水上无人机的内环状态变量X＝[V q]T进行非线性解耦控制，形成油门指令标称值和升降舵指令标称值δth0，δe0；内环扩展状态观测器3接收水上无人机内环状态变量X＝[V q]T和油门指令和升降舵指令δth，δe，实时估计速度方向和俯仰力矩方向的海浪干扰力和干扰力矩，形成油门干扰当量和升降舵干扰当量估计值z0，z1；油门指令标称值和升降舵指令标称值δth0，δe0与油门干扰当量和升降舵干扰当量估计值z0，z1相减，可以得到油门指令和升降舵指令δth，δe。

T

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

[0030] 内环动态逆控制器2的输出是油门指令标称值和升降舵指令标称值δth0，δe0，并与内环扩张状态观测器3的输出(油门干扰当量和升降舵干扰当量估计值z0，z1)相减，可以得到油门指令和升降舵指令δth，δe，输入到水上无人机动力学模型处理器4，形成闭环控制机制，实现水上无人机速度V有效跟踪速度控制目标值。变参数PID控制器1和纵倾角θ的状态反馈，组成外回路控制，主要用于水上无人机纵倾角θ有效跟踪纵倾角控制目标值——纵倾角指令θcmd，具体说明如下，参见图1，变参数PID控制器1的输入是纵倾角误差θe，经过PID控制器计算，形成俯仰角速率指令qcmd，结合上述内环动态逆控制器2使得俯仰角q跟踪俯仰角速率控制目标值——俯仰角速率指令qcmd，形成纵倾角θ的闭环控制机制，从而实现纵倾角θ有效跟踪纵倾角控制目标值——纵倾角指令θcmd。[0031] 水上无人机水面自主操纵控制方法的具体步骤是：[0032] 变参数PID控制器1，其接收纵倾角指令θcmd和纵倾角θ的纵倾角误差θe，然后根据所述纵倾角误差θe计算得到俯仰角速率指令qcmd并输出给内环动态逆控制器；所述纵倾角误差θe为所述纵倾角指令θcmd和纵倾角θ的差；[0033] 内环动态逆控制器2，其分别接收速度指令Vcmd(由用户输入的速度期望值)和俯

7

CN 104199456 A

说 明 书

4/8页

仰角速率指令qcmd，然后根据内环动态逆控制器2设计的控制器(具体详见后述公式5)计算得到油门指令标称值δth0，并根据内环动态逆控制器2设计的控制器(具体详见后述公式5)计算得到升降舵指令标称值δe0；[0034] 内环扩展状态观测器3，其接收油门指令δth、升降舵指令δe和水上无人机内环状态变量X＝[V q]T，并根据扩展状态观测器32(见公式9)计算得到油门干扰当量估计值z0，根据扩展状态观测器32(见公式9)，计算得到升降舵干扰当量估计值z1；所述油门指令δth是所述内环动态逆控制器2输出的油门指令标称值δth0和前一时刻油门干扰当量估计值z0的差值，所述升降舵指令δe是所述内环动态逆控制器2输出的升降舵指令标称值δe0与前一时刻所述升降舵干扰当量估计值z1的差值；[0035] 水上无人机动力学模型4，其接收油门指令δth和升降舵指令δe，并根据油门指令δth和升降舵指令δe计算得到水上无人机内环状态变量X＝[V q]T和纵倾角θ，采用的方式是公式1基础上的数值积分计算，所述水上无人机内环状态变量X＝[V q]T输出给内环扩张状态观测器3；所述纵倾角θ用于计算纵倾角误差θe。

[0036] 图2(a)是示出了本发明中水上无人机纵倾角指令θcmd随速度V进行自动调整的曲线示意图；图2(b)示出了本发明中变参数PID控制器参数随速度V进行自动调整的曲线示意图。水上无人机的纵倾角指令θcmd在起飞加速过程中，存在一个上不稳定区和下不稳定区，只有在一定范围内纵倾角指令才是可以稳定控制的，纵倾角指令随速度变化进行自动调节。同时纵倾角控制的参数在起飞加速过程中，进行线性插值计算。[0037] 变参数PID控制器1具体的设计方法如下：[0038] 根据图2(a)～(b)所示，选取水上无人机纵倾角指令θcmd的典型指令曲线，选取特征速度值V1，V2，…，Vn(在速度区间内均匀取值)，所选取的所述特征速度值V1，V2，…，Vn在所选取的典型指令曲线上对应的水上无人机纵倾角指令典型值为θcmd1，θcmd2，…，θcmdn，对于水上飞机速度V按照上述特征速度值和纵倾角指令典型值进行线性插值，得到当前速度下的水上无人机纵倾角指令θcmd。

[0039]

纵倾角控制采用变参数PID控制器，

是纵倾角误差θe的导数；控制器参数为比例系数、微

其中，θe是纵倾角误差，

分系数和积分系数分别为kθp(V)，kθd(V)，kθi(V)，是随速度的变化量，即kθp，kθd，

kθi是速度V的函数，分别为纵倾角控制的比例系数、微分系数和积分系数，选取特征速度值V1，V2，…，Vn和纵倾角控制的比例系数典型值kθp1，kθp2，…，kθpn，进行线性插值得到

其中i的取值为{i∈[1，n]|Vi

＜V＜Vi+1}。kθd(V)，kθi(V)的计算方法类似，

其中i的取值为{i∈[1，n]|Vi＜V＜Vi+1}，分别为纵

倾角控制的微分系数和积分系数的线性插值方法，其中kθd1，kθd2，…，kθdn、kθi1，kθi2，…，

8

CN 104199456 A

说 明 书

5/8页

kθin分别是针对特征速度值V1，V2，…，Vn的微分系数典型值、积分系数典型值。[0040] 图3是水上无人机动力学模型，可以表示为：

[0041]

其中，m为水上飞机质量；V为水上飞机速度；T＝t(δth)为发动机推力，是油门指令δth的函数；u′，w′为机体坐标系(Xb，Zb)下的飞行速度；α表示水上飞机迎角；αt为发动机安装角；Nw为飞机底部的水动压力；Da为空气阻力；Df为水动阻力；q为俯仰角速率；Iy为纵向俯仰转动惯量；xg，zg为水上飞机在大地坐标系下位置；θ为飞机纵倾角，g为重力加速度；Gxa，Gza为重力沿稳定坐标下的分量；Gxa＝mg(-cosαsinθ+sinαcosθ)，Gza＝mg(sinαsinθ+cosαcosθ)；Ma，Mw，MT分别为空气、水动力和发动机产生的俯仰力矩，La为

[0042]

气动升力。

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

[0044]

其中：

T

[0046] u＝[δth δe]

T

[0047] X＝[V q]

[0045] [0048]

[0049]

[0050]

[0051] [0052]

其中，为推力系数，为推力俯仰力矩系数，为升降舵俯仰力矩系数，

为迎角关于俯仰力矩动导数，Ma0为零升力俯仰力矩，Δq，ΔV为未知不确定项。控制输入u＝[δth δe]T＝u0-Z＝[δth0 δe0]T-[z1 z2]T。[0053] 图4是内环动态逆控制器2，包括期望状态计算21、逆控制律计算22、水上无人机

9

CN 104199456 A

说 明 书

6/8页

动力学模型4包含状态变量计算41、积分环节42。期望状态计算21一端连接系统指令Xc和水上无人机内环状态变量X＝[V q]T，一端连接Xd＝[Vd qd](系统过渡过程中的速度指令Vd和系统过渡过程中的俯仰角速率指令qd)；逆控制律计算22一端连接系统期望动态特性Xd，一端连接输入控制量标称值u0＝[δth0 δe0]；状态变量计算42一端连接输入控制量u(计算方法见公式10)，一端连接水上无人机内环状态变量X＝[V q]T的导数

；积分环节

42一端连接水上无人机内环状态变量X＝[V q]T的导数，一端连接水上无人机内环状态内环动态逆控制器2通过系统指令Xc和水上无人机内环状态变量X＝[V 变量X＝[V q]T；q]T，计算求得Xd＝[Vd qd](系统过渡过程中的速度指令Vd和系统过渡过程中的俯仰角速率指令qd)，进行逆控制律计算22，得出输入控制量标称值u0＝[δth0 δe0]，经过状态变量计算41和积分环节42形成水上无人机内环状态变量X＝[V q]T。[0054] 内环动态逆控制器2具体的设计方法如下：[0055] 系统期望模型，即期望状态计算21的具体形式为：

[0056] [0057]

其中，Xd＝[Vd qd]为系统过渡过程的速度指令Vd和俯仰角速率指令qd，Xc＝[Vcmd

即系统期望的速度指令Vcmd和期望的俯仰角速率指令qcmd，X＝[V qcmd]为系统控制指令，

q]T是水上无人机内环状态变量，V为水上飞机速度，q为俯仰角速率。K为期望特征矩阵

k1和k2是系统过渡过程(速度指令Vd和俯仰角速率指令qd的期望过渡过程)

的一阶惯性时间常数。Xc＝[Vcmd qcmd]需要经过公式4及数值积分运算可以得到Xd＝[Vd qd]，以保证系统过渡过程指令Xd＝[Vd qd]的平滑，没有突变值。[0058] 逆控制律计算22，基于公式1和公式2，并由内环动态逆控制器2设计的控制器可以表示为：

[0059] [0060]

状态变量计算41的具体方法如下，见公式1，对于内环系统，其状态变量为X＝[V

q]T，指的是速度和俯仰角速率的动力学系统，可以表示为：

[0061]

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

导数的归一化值，见公式3。

[0062] [0063]

积分环节42具体完成如下功能：完成状态变量的导数——到X＝[V a]T的计

其中X0为状态变

算，采用数值积分的方法，具体方法如下：

量的初值，Xi为第i步的计算状态变量的导数，h为每一步计算的时间间隔。[00] 图5是内环扩展状态观测器3，包括水上飞机速度V扩展状态观测器31和俯仰角

10

CN 104199456 A

说 明 书

7/8页

速率q扩展状态观测器32；水上飞机速度V扩展状态观测器31一端与水上飞机速度V、油门指令δth连接，一端与增益调节器311连接；增益调节器311一端与水上飞机速度V回归函数估计值Δ2V连接，一端与油门干扰当量z0连接。俯仰角速率q扩展状态观测器32一端与俯仰角速率q、升降舵指令δe连接，一端与增益调节器321连接；增益调节器321一端与俯仰角速率q回归函数估计值Δ2q连接，一端与升降舵干扰当量估计值z1连接。[0065] 内环扩展状态观测器3工作方法是：水上飞机速度V、油门指令δth经过水上飞机速度V扩展状态观测器31，形成水上飞机速度V回归函数估计值Δ2V，再经过增益调节器311，形成油门干扰当量z0；俯仰角速率q、升降舵指令δe经过俯仰角速率q扩展状态观测器32，形成俯仰角速率q回归函数估计值Δ2q，再经过增益调节器311，形成升降舵干扰当量估计值z1。

[0066] 水上飞机速度V扩展状态观测器31的具体设计方法是[0067] 由公式2-3可以得：

[0068]

[0069] [0070]

其中fV＝(Gxa-Da)V，设计如下扩展状态观测器

[0071]

[0072]

其中，fV＝(Gxa-Da)V为水上飞机速度V的回归函数，

为水上

飞机速度V的控制输入增益，

为水上飞机速度V导数的未知摄动变量，油门指令δth

为水上飞机速度V状态方程的控制量输入，eV为状态跟踪误差，Δ1V，Δ2V分别为水上飞机速度V以及

的估计值，βV01，βV02为水上飞机速度V观测器系数，即观测器估

计参数收敛速度的控制参数，由参数收敛速度决定，hV为非线性组合函数的误差分界限，

为非线性组合函数。Δ1V，Δ2V通过公式数值

积分计算求得。

[0073] 俯仰角速率q扩展状态观测器32的具体设计方法是[0074] 由公式2-3可以得：

[0075]

11

CN 104199456 A[0076] [0077]

说 明 书

8/8页

其中

设计如下扩展状态观测器

[0078]

[0079] 其中，

为俯仰角速率q的回归函数，Ma0为零升力俯仰力

矩，为俯仰角速率q的控制输入增益，为俯仰角速率q的未知摄动变量，升降舵

指令δe为俯仰角速率q的控制量输入，eq为俯仰角速率q状态跟踪误差，Δ1q，Δ2q分别为俯仰角速率q以及

的估计值，βq01，βq02为俯仰角速率q观测器系数，即观测器

估计参数收敛速度的控制参数，由参数收敛速度决定，hq为非线性组合函数的误差分界限，

为非线性组合函数。Δ1q，Δ2q通过公式数值积分计

算求得。

[0080] 针对水上飞机速度V和俯仰角速率q，分别设计水上飞机速度V扩展状态观测器31和俯仰角速率q扩展状态观测器32，形成油门干扰当量和升降舵干扰当量估计值z0，z1，[0081] 其中，

T

[0082] Z＝[z0 z1]，z0＝Δ2V/gV，z1＝2Δq/gq (9)[0083] Δ2V，Δ2q为水上飞机速度V和俯仰角速率q公式(7-1，8-1)的fV，fg的估计值，gV，gq为对应状态变量的控制输入增益，计算方法如下

[0084]

油门指令标称值和升降舵指令标称值δth0，δe0与油门干扰当量和升降舵干扰当量估计值z0，z1相减，可以得到油门指令和升降舵指令δth，δe，即u＝[δth δe]T＝u0-Z＝[δth0 δe0]T-[z0 z1]T。 (10)

[0085]

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详

细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

[0086]

12

CN 104199456 A

说 明 书 附 图

1/3页

图1

图2(a)

13

CN 104199456 A

说 明 书 附 图

2/3页

图2(b)

图3

图4

14

CN 104199456 A

说 明 书 附 图

3/3页

图5

15