[发明专利]一种上下体艏向信息融合的波浪滑翔器航点跟踪方法

权利要求书

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

浮体安装艏向传感器(5)，潜体安装艏向传感器(6)，测量浮体艏向角ψ_F，潜体艏向角ψ_G；

波浪滑翔器配备有GPS或其他定位系统，可获得当前位置期望航点设置为(x_d,y_d)；当波浪滑翔器当前位置与目标航点的距离相比地球尺度很小时，可将地球看成平面，则系统理想航向角ψ_LOS依据视线法由下式获得

其中，

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

即

其中k的取值根据经验选取，可取为1；

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

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

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为输出最大值，即最大舵角；

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

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

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

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