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

摘要

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

主权项

一种上下体艏向信息融合的波浪滑翔器航点跟踪方法，适用于波浪滑翔器，波浪滑翔器的舵机(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)，用修正系统期望航向代替系统期望航向角继续顺序执行。