CN106990787A - 一种上下体艏向信息融合的波浪滑翔器航点跟踪方法 - Google Patents

Abstract

本发明提供一种上下体艏向信息融合的波浪滑翔器航点跟踪方法。波浪滑翔器的舵机安装于潜体，舵机由主计算机控制，可直接控制潜体的转向，浮体的转向由潜体的拖曳力提供。浮体与潜体分别安装一个艏向传感器，测量浮体艏向ψ_F，潜体艏向ψ_G,并传至主计算机，主计算机完成波浪滑翔器浮体潜体的艏向信息融合，结合航行过程中的动态特征修正期望航向角，完成航点跟踪任务。本发明提供的方法能够有效避免波浪滑翔器刚柔多体系联结构特有的柔链缠绕现象，提高波浪滑翔器的航向控制性能，以及风、流等外界干扰力下的航点跟踪能力。

Description

一种上下体艏向信息融合的波浪滑翔器航点跟踪方法

技术领域

上下体艏向信息融合的波浪滑翔器航点跟踪方法是一种针对波浪滑翔器的运动控制方法，在不使用昂贵传感器的前提下能够有效避免波浪滑翔器刚柔多体系联结构特有的柔链缠绕现象，提高波浪滑翔器的航向控制性能，以及风、流等外界干扰力下的航点跟踪能力。

背景技术

波浪滑翔器是一种新型无人海洋探测平台，利用浮体-脐带-潜体结构将海洋波浪能转化为自身的前进动力，同时依靠上甲板搭载的太阳能电池板为各电器负载供电，可以在广阔的海洋上进行长期的路径跟踪和位置保持，还可充当中间站，与其他各类型的探测器进行指令和数据交换，为海洋观测技术提供了新思路，有着十分广阔的发展前景。然而，波浪滑翔器的运动控制是一个难点。

夏威夷大学Kraus发表的论文《Estimation of wave glider dynamics forprecise positioning》中采用PID控制算法进行了波浪滑翔器定位控制。哈尔滨工程大学廖煜雷等发表的论文《The intelligent control system for an unmanned waveglider》中针对海洋环境扰动尤其是海流作用下波浪滑翔器路径跟踪存在的稳态误差问题，提出了一种自适应PID制导律以补偿海流对系统运动的不利影响，并提出了一种改进S面航向控制器，以解决大扰动下弱机动载体的航向控制问题。哈尔滨工程大学卢旭发表的论文《波浪滑翔器总体技术研究》中针对波浪滑翔器的航向控制问题，设计了基于模糊PID技术的控制器。国家海洋技术中心史健等发表的论文《基于多传感器的波浪能滑翔器导航策略研究》中基于高斯大地线算法和PID控制算法，构成了波浪滑翔器导航策略。

然而，上述波浪滑翔器的运动控制方法都有一共同特征，仅单独控制波浪滑翔器的浮体，而非依据波浪滑翔器浮体潜体的艏向信息融合进行整体的运动控制。单独控制波浪滑翔器的浮体或潜体都存在一定缺陷：如果只控制浮体的艏向，由于舵机安装于潜体部分，在外界干扰力作用下浮体达不到期望艏向时，潜体部分持续的舵角将导致潜体持续旋转并导致柔链缠绕现象；如果只控制潜体的艏向，可能出现潜体与浮体艏向角相差过大的情况。浮体转向的力矩来源于柔链的拖曳过程，潜体与浮体艏向角相差过大不利于浮体转向。此外，在风、流等干扰力作用下，波浪滑翔器的航行路径会发生偏移，无法追踪到设置的航迹点，若要精确测量风、流等环境干扰力并针对性地修正控制方法需配备声学多普勒流速剖面仪等价格高昂的传感器，将大幅提高波浪滑翔器的成本。

发明内容

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

波浪滑翔器的舵机安装于潜体，舵机由主计算机控制，可直接控制潜体的转向，浮体的转向由潜体的拖曳力提供，特征是浮体与潜体分别安装一个艏向传感器，测量浮体艏向ψ_F，潜体艏向ψ_G,并传至主计算机，主计算机完成波浪滑翔器浮体潜体的艏向信息融合，结合航行过程中的动态特征修正期望航向角，完成航点跟踪任务。

上下体艏向信息融合的波浪滑翔器航点跟踪方法的步骤的特征是：

(1)根据目标位置与当前位置计算理想航向角ψ_LOS；

(2)若理想航向角更新，系统期望航向角取理想航向角ψ_LOS，根据系统期望航向角和浮体艏向角ψ_F，以及潜体浮体相对于系统艏向角的比例系数k计算潜体期望艏向角ψ_Gd；

(3)计算潜体期望艏向ψ_Gd与浮体艏向ψ_F的夹角的绝对值ψ_FG，将其限制在预先设定的阈值ψ_FGmax内；

(4)进行潜体艏向控制，主计算机向舵机发出命令舵角δ；

(5)判断波浪滑翔器是否到达目标航点，如果已到达目标航点，则结束，如果未到达目标航点，进入下一步；

(6)判断系统航向ψ_S与理想航向角ψ_LOS的偏差ψ_e，若小于设置的阈值ψ_Emax，则保持当前潜体期望艏向不变，返回步骤4进行潜体艏向控制；若大于设置的阈值ψ_Emax，则进入下一步；

(7)波浪滑翔器当前位置或目标位置发生变化，导致理想航向角变化达到一定程度，则返回第1步更新理想航向角，将初始系统期望航向角赋为更新后的理想航向角，继续顺序执行。如果理想航向角变化较小，则不更新理想航向角，当前潜体期望艏向不变，进行潜体艏向控制，维持一段时间T_δ；

(8)根据实际航向变化角速度r和系统期望航向计算系统期望航向偏移量系统期望航向角与系统期望航向偏移量相加，求得修正系统期望航向ψ_D；

(9)返回第2步，用修正系统期望航向代替系统期望航向角继续顺序执行。

本发明提供的上下体艏向信息融合的波浪滑翔器航点跟踪方法的有益之处在于：在不使用昂贵传感器的前提下，能够有效避免波浪滑翔器刚柔多体系联结构特有的柔链缠绕现象，提高波浪滑翔器的航向控制性能，以及风、流等外界干扰力下的航点跟踪能力。

附图说明

图1为本发明的流程图；

图2为波浪滑翔器控制系统结构图；

图3为本发明的坐标系示意图；

图4为波浪滑翔器示意图。

具体实施方式

结合附图举例对本发明做更详细地描述：

结合图4说明波浪滑翔器及其硬件结构，波浪滑翔器的舵机1安装于潜体2，舵机由主计算机3控制，可直接控制潜体2的转向，浮体4的转向由潜体2的拖曳力提供，特征是浮体与潜体分别安装一个艏向传感器，安装于浮体的艏向传感器5和安装于潜体的艏向传感器6，测量浮体艏向ψ_F，潜体艏向ψ_G，并传至主计算机。

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

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

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

结合图2说明波浪滑翔器的控制系统结构。由导航系统给出定位信息，即当前位置制导系统根据目标位置(x_d,y_d)与当前位置计算理想航向角ψ_LOS，同时也是初始系统期望航向角ψ_D。结合导航系统中艏向传感器得到的浮体与潜体艏向ψ_F与ψ_G，经过上下体艏向信息融合输出潜体期望艏向ψ_Gd，艏向控制系统进行潜体的艏向控制，并向多系统发出舵角命令，波浪滑翔器打舵。波浪滑翔器在海洋环境中航行，导航系统监测波浪滑翔器位置与浮体和潜体的艏向变化，并发送到制导系统、浮体潜体艏向融合系统、潜体艏向控制系统。同时，根据潜体艏向控制航行过程中系统期望航向与系统航向转艏角速度等信息修正系统期望航向，返回到制导系统中。

结合图1说明上下体艏向信息融合的波浪滑翔器航点跟踪方法的步骤是：

1.根据目标位置与当前位置计算理想航向角ψ_LOS。

波浪滑翔器配备有GPS或其他定位系统，可获得当前位置期望航点设置为(x_d,y_d)。本发明以视线法为例说明求取理想航向角的方法，但并不局限于该方法。当波浪滑翔器当前位置与目标航点的距离相比地球尺度很小时，可将地球看成平面，则理想航向角ψ_LOS依据视线法由下式获得

其中，

2.若理想航向角更新，系统期望航向角取理想航向角ψ_LOS，根据系统期望航向角和浮体艏向角ψ_F，以及潜体浮体相对于系统艏向角的比例系数k计算潜体期望艏向角ψ_Gd。

首先设置潜体浮体相对于系统航向角比例系数k，其中

即

其中k的取值根据经验选取，一般可取为1。

3.计算潜体期望艏向与浮体艏向的夹角的绝对值ψ_FG，将其限制在预先设定的阈值ψ_FGmax内。

潜体期望艏向与浮体艏向的夹角预先设置好潜体期望艏向与浮体艏向夹角阈值ψ_FGmax，则

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

4.进行潜体艏向控制，主计算机向舵机发出命令舵角δ。

S面控制方法将模糊控制的思想同PID的简单控制结构相结合，在水下机器人、无人艇运动控制中得到了广泛应用。本发明以S面控制方法为例介绍上下体艏向信息融合的波浪滑翔器航点跟踪方法，但不限于该艏向控制方法。

Sigmoid曲线函数可表示为：

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

那么，Sigmoid曲面函数为：

z＝2.0/(1.0+exp(-k₁x-k₂y))-1.0 (7)

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

其中，e和为控制的输入(分别为偏差和偏差变化率，并通过归一化处理)，u为归一化的控制力输出，k₁、k₂分别为对应偏差和偏差变化率的控制参数，可以改变其对应控制输入的变化速度。

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

其中，Δu为S面偏移，K为输出最大值，即最大舵角。

5.判断波浪滑翔器是否到达目标航点，如果已到达目标航点，则结束，如果未到达目标航点，进入下一步。

当波浪滑翔器当前位置与目标航点的距离相比地球尺度很小时，可将地球看成平面，则波浪滑翔器当前位置与目标航点距离

若该距离小于预先设定的距离阈值，认为到达目标航点，则结束控制过程；如果该距离大于预先设定的距离阈值，认为未到达目标航点，则进入下一步。其中，距离阈值的选取根据波浪滑翔器所执行任务对航点跟踪的精度要求及波浪滑翔器本身的运动能力选取。例如，做大范围海洋环境监测时，距离阈值可取为500m。

6.判断系统航向ψ_S与理想航向角ψ_LOS的偏差ψ_e，若小于设置的阈值ψ_Emax，则保持当前潜体期望艏向不变，返回步骤4采用S面控制方法进行潜体艏向控制；若大于设置的阈值ψ_Emax，则进入下一步；

7.波浪滑翔器当前位置或目标位置发生变化，导致理想航向角变化达到一定程度，则返回第1步更新理想航向角，将初始系统期望航向角赋为更新后的理想航向角，继续顺序执行。如果理想航向角变化较小，则不更新理想航向角，当前潜体期望艏向不变，采用步骤4所述的S面控制方法进行潜体艏向控制，维持一段时间T_δ；

8.根据实际航向变化角速度r和系统期望航向计算系统期望航向偏移量系统期望航向角与系统期望航向偏移量相加，求得修正系统期望航向ψ_D。

一个基本的操舵事实是：如果系统期望航向设为并且并持续一段时间，波浪滑翔器实际航向应该向系统期望航向方向转动，即航向转艏角速度r＞0，考虑到系统的时滞性，假设并持续一段时间T_δ，波浪滑翔器的航向就应该有右转行为。然而实际上波浪滑翔器航向没有右转或甚至在左转，这时可认为系统期望航向角应进行修正。

其中，为最大修正舵角；T_δ为滞后时间；t为系统期望航向设为状态下的持续时间；为修正系统期望航向调整因子。

系统期望航向角与系统期望航向偏移量相加，求得修正系统期望航向ψ_D，即

9.返回第2步，用修正系统期望航向代替系统期望航向角继续顺序执行。

Claims (8)

1.一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，适用于波浪滑翔器，波浪滑翔器的舵机(1)安装于潜体(2)，舵机由主计算机(3)控制，可直接控制潜体的转向，浮体(4)的转向由潜体的拖曳力提供，其特征在于：测量浮体艏向ψ_F，潜体艏向ψ_G,并传至主计算机，(1)根据目标位置与当前位置计算理想航向角ψ_LOS；(2)若理想航向角更新，系统期望航向角取理想航向角ψ_LOS，根据系统期望航向角和浮体艏向角ψ_F，以及潜体浮体相对于系统艏向角的比例系数k计算潜体期望艏向角ψ_Gd；(3)计算潜体期望艏向与浮体艏向的夹角的绝对值ψ_FG，将其限制在预先设定的阈值ψ_FGmax内；(4)进行潜体艏向控制，主计算机向舵机发出命令舵角δ；(5)判断波浪滑翔器是否到达目标航点，如果已到达目标航点，则结束，如果未到达目标航点，则进入步骤(6)；(6)判断系统航向ψ_S与理想航向角ψ_LOS的偏差ψ_e，若小于设置的阈值ψ_Emax，则保持当前潜体期望艏向不变，返回步骤(4)采用S面控制方法进行潜体艏向控制；若大于设置的阈值ψ_Emax，则进入步骤(7)；(7)波浪滑翔器当前位置或目标位置发生变化，导致理想航向角变化达到一定程度，则返回步骤(1)更新理想航向角，将初始系统期望航向角赋为更新后的理想航向角，继续顺序执行；如果理想航向角变化较小，则不更新理想航向角，当前潜体期望艏向不变，采用步骤(4)所述的S面控制方法进行潜体艏向控制，维持一段时间T_δ；(8)根据实际航向变化角速度r和系统期望航向计算系统期望航向偏移量系统期望航向角与系统期望航向偏移量相加，求得修正系统期望航向ψ_D；(9)返回步骤(2)，用修正系统期望航向代替系统期望航向角继续顺序执行。

2.根据权利要求1所述的一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，其特征在于：浮体安装艏向传感器(5)，潜体安装艏向传感器(6)，测量浮体艏向ψ_F，潜体艏向ψ_G。

3.根据权利要求1所述的一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，其特征在于：波浪滑翔器配备有GPS或其他定位系统，可获得当前位置期望航点设置为(x_d,y_d)。本发明以视线法为例说明求取理想航向角的方法，但并不局限于该方法，当波浪滑翔器当前位置与目标航点的距离相比地球尺度很小时，可将地球看成平面，则理想航向角ψ_LOS依据视线法由下式获得

其中，

4.根据权利要求1所述的一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，其特征在于：设置潜体浮体相对于系统艏向角比例系数k，其中

即

其中k的取值根据经验选取，一般可取为1。

5.根据权利要求1所述的一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，其特征在于：潜体期望艏向与浮体艏向的夹角预先设置好潜体期望艏向与浮体艏向夹角阈值ψ_FGmax，则

其中，夹角阈值ψ_FGmax的选取根据经验选取，一般不大于90度。

6.根据权利要求1所述的一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，其特征在于：S面控制方法将模糊控制的思想同PID的简单控制结构相结合，在水下机器人、无人艇运动控制中得到了广泛应用，本发明以S面控制方法为例介绍上下体艏向信息融合的波浪滑翔器航点跟踪方法，但不限于该艏向控制方法，

Sigmoid曲线函数可表示为：

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

那么，Sigmoid曲面函数为：

z＝2.0/(1.0+exp(-k₁x-k₂y))-1.0 (7)

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

其中，e和为控制的输入(分别为偏差和偏差变化率，并通过归一化处理)，u为归一化的控制力输出，k₁、k₂分别为对应偏差和偏差变化率的控制参数，可以改变其对应控制输入的变化速度，对于低速运行的波浪滑翔器来说，可以假设时变的环境力在一段时间内是固定干扰力(或缓慢渐变过程)，通过调整S面的偏移，来达到消除固定误差的目的，如采用以下改进控制器模型

其中，Δu为S面偏移，K为输出最大值，即最大舵角。

7.根据权利要求1所述的一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，其特征在于：当波浪滑翔器当前位置与目标航点的距离相比地球尺度很小时，可将地球看成平面，则波浪滑翔器当前位置与目标航点距离

若该距离小于预先设定的距离阈值，认为到达目标航点，则结束控制过程；如果该距离大于预先设定的距离阈值，认为未到达目标航点，则进入步骤(6)，其中，距离阈值的选取根据波浪滑翔器所执行任务对航点跟踪的精度要求及波浪滑翔器本身的运动能力选取。

8.根据权利要求1所述的一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，其特征在于：如果系统期望航向设为并且并持续一段时间，波浪滑翔器实际航向应该向系统期望航向方向转动，即航向转艏角速度r＞0，考虑到系统的时滞性，假设并持续一段时间T_δ，波浪滑翔器的航向就应该有右转行为，然而实际上波浪滑翔器航向没有右转或甚至在左转，这时可认为系统期望航向角应进行修正，

其中，为最大修正舵角；T_δ为滞后时间；t为系统期望航向设为状态下的持续时间；为修正系统期望航向调整因子，系统期望航向角与系统期望航向偏移量相加，求得修正系统期望航向ψ_D，即