自适应艏向信息融合的波浪滑翔器航向控制方法与流程

本发明涉及一种针对波浪滑翔器的航向控制方法，尤其涉及一种自适应艏向信息融合的波浪滑翔器航向控制方法，属于海洋航行器运动控制方法领域。

背景技术

波浪滑翔器是一种新型无人海洋探测平台，利用“浮体-脐带-潜体”刚柔混合多体结构将海洋波浪能直接地转化为自身的前进动力，同时依靠上甲板搭载的太阳能电池板为各电气负载供电，可以在广阔的海洋上进行长期自主的航行，还可充当通信中继与其他类型的探测平台进行指令和数据交换，为海洋观测技术提供了新思路，有着十分广阔的发展前景。然而，波浪滑翔器独特的多体系联结构使得采用针对于单体结构的运动控制方法，控制效果难以保证。

王磊峰等提出的公开号为cn106990787a的发明《一种上下体艏向信息融合的波浪滑翔器航点跟踪方法》，涉及了艏向融合的思想，然而，其中潜体浮体相对于系统艏向角的比例系数根据经验选取，并且在整个航行过程中保持不变，根据经验选取的固定的比例系数将产生固有偏移，而本发明中潜体浮体相对于系统艏向角的比例系数根据波浪滑翔器实际航向数据实时迭代修正，该比例系数同时反映了波浪滑翔器自身动力学特性和外界环境干扰因素的影响，即使外界环境干扰发生变化，采用实时修正的潜体浮体相对于系统艏向角的比例系数依然能够起到消除环境干扰带来的稳态误差的作用，使得波浪滑翔器系统的实际航向逼近期望航向，因此，二者消除稳态误差的原理与控制系统体系结构具有本质不同，本发明具有显著进步。

技术实现要素：

本发明的目的是为了提供一种自适应艏向信息融合的波浪滑翔器航向控制方法，结合波浪滑翔器多体特性，利用实际航行数据实时修正波浪滑翔器浮体潜体艏向和系统整体航向的相互关系，通过对潜体的艏向控制间接实现波浪滑翔器系统整体航向的控制，达到航向控制的目的，并使得该方法具有较强的自适应性。

本发明的目的是这样实现的：波浪滑翔器的舵机安装于潜体，舵机由主计算机控制并可直接控制潜体的转向，浮体的转向由潜体的拖曳力提供，浮体与潜体分别安装一个艏向传感器且两个艏向传感器分别测量浮体艏向和潜体艏向，波浪滑翔器还配备有测量波浪滑翔器系统整体航向的航向传感器，步骤如下：

步骤(1)：制导模块给出期望航向角ψ^*(k)，其中k为离散控制系统的运行时刻；

步骤(2)：根据系统实际航向ψ(k)，浮体艏向角ψ^f(k)和潜体艏向角ψ^g(k)，在上一时刻潜体浮体相对于系统艏向角的比例系数的估计值的基础上，得到修正后的潜体浮体相对于系统艏向角的比例系数的估计值

步骤(3)：根据系统期望航向角ψ^*(k)、浮体艏向角ψ^f(k)以及修正后的潜体浮体相对于系统艏向角的比例系数计算潜体期望艏向角ψ^gd(k)；

步骤(4)：计算潜体期望艏向ψ^gd(k)与浮体艏向ψ^f(k)的夹角的绝对值ψfg，并将其限制在预先设定的阈值ψfgmax内；

步骤(5)：进行潜体艏向控制，主计算机向舵机发出舵角指令δd，舵机驱动舵板转动；

步骤(6)：计算波浪滑翔器实际航向与期望航向误差绝对值e(k)＝ψ^*(k)-ψ(k)，如果|e(k)|小于设定的阈值e0并能保持，则波浪滑翔器航向控制系统实际输出稳定收敛到期望输出，跳出循环；否则返回步骤(2)进行循环。

本发明还包括这样一些结构特征：

1.步骤(2)具体包括：

首先，设置准则函数，所述准则函数包括：极小化潜体艏向与系统航向偏差的实际值ψ^g(k)-ψ(k)与估计值之差的平方，极小化当前时刻所述比例系数估计值与上一时刻比例系数估计值之差的平方，其中，潜体艏向与系统航向偏差的实际值ψ^g(k)-ψ(k)与估计值之差的平方和当前时刻所述比例系数估计值与上一时刻比例系数估计值之差的平方的相对权重由权重系数μ调节，所述权重系数μ大于0；

最后，所述准则函数关于当前时刻比例系数的估计值求极值，加入步长因子η，得到递推形式的当前时刻比例系数的估计值所述步长因子η在0-1之间。

与现有技术相比，本发明的有益效果是：1、本发明提供的自适应艏向信息融合的波浪滑翔器航向控制方法，利用实际航行数据实时修正波浪滑翔器浮体潜体艏向和系统整体航向的相互关系，通过对潜体的艏向控制间接实现波浪滑翔器系统整体航向的控制，达到航向控制的目的，该控制方法无需建立精确的动力学模型，考虑了波浪滑翔器浮体潜体艏向和系统航向不一致的运动特性，能够有效避免波浪滑翔器刚柔多体系联结构特有的柔链缠绕现象，提高波浪滑翔器的航向控制性能；2、本发明中潜体浮体相对于系统艏向角的比例系数根据波浪滑翔器实际航向数据实时迭代修正，该比例系数同时反映了波浪滑翔器自身动力学特性和外界环境干扰因素的影响，即使外界环境干扰发生变化，采用实时修正的潜体浮体相对于系统艏向角的比例系数依然能够起到消除环境干扰带来的稳态误差的作用，使得波浪滑翔器系统的实际航向逼近期望航向；3、该方法针对变化的外界环境干扰具有较强的自适应性。

附图说明

图1是波浪滑翔器示意图；

图2是自适应艏向信息融合的波浪滑翔器航向控制方法坐标系示意图；

图3是波浪滑翔器控制系统结构图；

图4是自适应艏向信息融合的波浪滑翔器航向控制方法流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

结合图1说明本发明的波浪滑翔器及其硬件结构，波浪滑翔器的舵机1安装于潜体2，舵机由主计算机3控制，可直接控制潜体2的转向，浮体4的转向由潜体2的拖曳力提供，浮体与潜体分别安装一个艏向传感器，安装于浮体的艏向传感器5和安装于潜体的艏向传感器6，分别测量浮体艏向ψ^f和潜体艏向ψ^g，波浪滑翔器还配备有航向传感器，测量波浪滑翔器系统整体航向ψ。

结合图2说明坐标系的建立。只考虑波浪滑翔器在水平面的运动，建立水平面大地坐标系ξ-e-ζ。一般地，eξ向北为正，eζ向东为正。波浪滑翔器浮体与潜体由柔链连接，柔链无法直接传递扭矩，因此波浪滑翔器浮体和潜体的艏向一般情况下并不一致，如图2所示，ψ^f为浮体的艏向，ψ^g为潜体的艏向。波浪滑翔器系统整体的航向与浮体艏向和潜体艏向都不相等，一般地，波浪滑翔器系统整体的航向位于浮体艏向与潜体艏向之间，潜体浮体相对于系统航向角比例系数为c，图2中ψ为系统航向。波浪滑翔器系统整体当前位置位于大地坐标系原点e，假设目标航点位于图2中d点位置，并假设制导模块给出的期望航向为ψ^*。如果波浪滑翔器整体的实际航向ψ能够等于ψ^*，则经过一段时间波浪滑翔器将到达目标航点。

然而，波浪滑翔器航向，浮体艏向，潜体艏向均不相等，转向系统即舵机与舵板安装于潜体，只能够直接控制潜体的艏向，航行过程中潜体拖曳柔链进而拖曳浮体，该拖曳力作用下浮体发生转艏。这种独特的运动模式使得波浪滑翔器的航向控制与航点跟踪问题十分困难。传统的航向控制方法包括单独控制浮体的艏向和单独控制潜体的艏向，这两种方法对于波浪滑翔器都有一定缺陷。如果只控制浮体的艏向，由于舵机安装于潜体部分，在外界干扰力作用下浮体达不到期望艏向时，潜体部分持续的舵角将导致潜体持续旋转并导致柔链缠绕现象；如果只控制潜体的艏向，可能出现潜体与浮体艏向角相差过大的情况。浮体转向的力矩来源于柔链的拖曳过程，潜体与浮体艏向角相差过大不利于浮体转向。此外，在风、流等干扰力作用下，波浪滑翔器的航行路径会发生偏移，无法追踪到设置的航迹点，若要精确测量风、流等环境干扰力并针对性地修正控制方法需配备声学多普勒流速剖面仪等价格高昂的传感器，将大幅提高波浪滑翔器的成本。

本发明提供一种自适应艏向信息融合的波浪滑翔器航向控制方法，用于波浪滑翔器执行航向控制或航点追踪任务时的运动控制。

结合图3说明波浪滑翔器的控制系统结构。制导模块给出期望航向角ψ^*。结合传感器系统测量的浮体与潜体艏向ψf与ψg，以及系统航向ψ，自适应艏向信息融合系统对潜体浮体相对于系统艏向角比例系数进行实时修正，根据修正后的比例系数，输出潜体期望艏向ψgd，潜体艏向控制模块进行潜体的艏向控制，并向舵系统发出舵角命令，舵机驱动舵板转动。波浪滑翔器在海洋环境中航行，传感器监测波浪滑翔器运动状态，反馈至到自适应艏向融合系统和潜体艏向控制系统。

结合图4说明自适应艏向信息融合的波浪滑翔器航向控制方法的步骤是：

(1)制导模块给出期望航向角ψ^*(k)，其中k为离散控制系统的运行时刻；

(2)根据系统实际航向ψ(k)，浮体艏向角ψ^f(k)和潜体艏向角ψ^g(k)，在上一时刻潜体浮体相对于系统艏向角的比例系数的估计值的基础上，得到修正后的潜体浮体相对于系统艏向角的比例系数的估计值

其中，潜体浮体相对于系统艏向角比例系数c(k)，其物理意义为

所述比例系数的修正过程，其步骤包括：

1、设置准则函数，所述准则函数包括：极小化潜体艏向与系统航向偏差的实际值ψ^g(k)-ψ(k)与估计值之差的平方，极小化当前时刻所述比例系数估计值与上一时刻比例系数估计值之差的平方，其中，潜体艏向与系统航向偏差的实际值ψ^g(k)-ψ(k)与估计值之差的平方，和当前时刻所述比例系数估计值与上一时刻比例系数估计值之差的平方的相对权重由权重系数μ调节，所述权重系数μ大于0；

即

其中，μ＞0，是权重因子。

2、所述准则函数关于当前时刻比例系数的估计值求极值，加入步长因子η，得到递推形式的当前时刻比例系数的估计值所述步长因子η在0-1之间，

即

其中，η∈(0,1]为步长因子，加入步长因子修正的目的在于使该算法具有更强的灵活性和一般性。

第一次迭代时，即当k＝1时c(1)的取值根据经验选取，一般可取为1。

(3)根据系统期望航向角ψ^*(k)，浮体艏向角ψ^f(k)，以及修正后的潜体浮体相对于系统艏向角的比例系数计算潜体期望艏向角ψ^gd(k)；

即

ψ^gd(k)＝c(k)×(ψ^*(k)-ψ^f(k))+ψ^*(k)

(4)计算潜体期望艏向ψ^gd(k)与浮体艏向ψ^f(k)的夹角的绝对值ψfg，将其限制在预先设定的阈值ψfgmax内；

潜体期望艏向与浮体艏向的夹角ψfg＝ψ^gd(k)-ψ^f(k)，预先设置好潜体期望艏向与浮体艏向夹角阈值ψfgmax，则

其中，夹角阈值ψfgmax的选取根据经验选取，一般不大于90度，例如可取为45度。

(5)进行潜体艏向控制，主计算机向舵机发出舵角指令δd，舵机驱动舵板转动；

s面控制方法将模糊控制的思想同pid的简单控制结构相结合，在水下机器人、无人艇运动控制中得到了广泛应用。本发明以s面控制方法为例介绍所述潜体艏向控制的过程，但不限于该艏向控制方法。

sigmoid曲线函数可表示为：

u＝2.0/(1.0+exp(-kx))-1.0

那么，sigmoid曲面函数为：

z＝2.0/(1.0+exp(-k1x-k2y))-1.0

选取s面控制器的控制模型为：

其中，e和分别为归一化处理偏差和偏差变化率，本发明中用于潜体艏向控制是时，所述偏差为潜体期望艏向减潜体实际艏向，u为归一化的控制力输出，k1、k2分别为对应偏差和偏差变化率的控制参数，可以改变其对应控制输入的变化速度。

对于低速运行的波浪滑翔器来说，可以假设时变的环境力在一段时间内是固定干扰力(或缓慢渐变过程)。因此，可以通过调整s面的偏移，来达到消除固定误差的目的，如采用以下改进控制器模型

其中，δu为s面偏移，k为输出最大值，即最大舵角。

(6)计算波浪滑翔器实际航向与期望航向误差绝对值e(k)＝ψ^*(k)-ψ(k)，如果|e(k)|小于设定的阈值e0并保持一定时间，认为波浪滑翔器航向控制系统实际输出稳定收敛到期望输出，跳出循环，返回步骤(2)。

综上，本发明提供一种自适应艏向信息融合的波浪滑翔器航向控制方法，步骤包括：(1)制导模块给出期望航向角；(2)根据系统实际航向，浮体艏向角和潜体艏向角，在上一时刻潜体浮体相对于系统艏向角的比例系数的估计值的基础上，得到修正后的潜体浮体相对于系统艏向角的比例系数的估计值；(3)根据系统期望航向角，浮体艏向角，以及修正后的潜体浮体相对于系统艏向角的比例系数，计算潜体期望艏向角；(4)计算潜体期望艏向与浮体艏向的夹角的绝对值，将其限制在预先设定的阈值内；(5)进行潜体艏向控制，主计算机向舵机发出舵角指令，舵机驱动舵板转动；(6)计算波浪滑翔器实际航向与期望航向误差绝对值，如果小于设定的阈值并保持一定时间，认为波浪滑翔器航向控制系统实际输出稳定收敛到期望输出，跳出循环，返回步骤(2)；

本发明提供的自适应艏向信息融合的波浪滑翔器航向控制方法结合波浪滑翔器多体特性，利用实际航行数据实时修正波浪滑翔器浮体潜体艏向和系统整体航向的相互关系，通过对潜体的艏向控制间接实现波浪滑翔器系统整体航向的控制，达到航向控制的目的，并使得该方法具有较强的自适应性。