CN113342011A - 一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法 - Google Patents

Abstract

本发明涉及一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法，利用姿态传感器获得仿蝠鲼航行器当前的航向角角度和期望航向角的航向偏差和偏差变化率；通过控制器控制横滚机构转动的角度，使仿蝠鲼航行器在滑翔运动中做出横滚动作，进而使仿蝠鲼航行器的航向角度发生改变。通过不断调节使仿蝠鲼航行器的实际航向角和设定航向角的角度偏差为0。本发明利用样机进行实验，得出横滚机构对航向控制的变化曲线。验证了在真实工作环境中本发明所提供方法的可行性和可靠性。

Description

一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法

技术领域

本发明属于仿蝠鲼航行器航向控制的方法，涉及一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法。

背景技术

仿蝠鲼航行器是以海洋中的真实生物蝠鲼为仿生原型进行研制的，采用MPF(中央鳍/对鳍模式)模式进行推进，是一种新型的水下航行器。相较于传统的采用螺旋桨推进的航行器，该种航行器能耗更低，运动噪声更小，机动性更高。能够更好的完成复杂多样的工作任务。

仿蝠鲼航行器进行水下滑翔任务的时候，一般是由上位机设定好滑翔的目标航向发送给下位机，由下位机控制航行器按照设定好的航向行进。但由于水下环境的复杂性，海洋中还存在很多诸如波浪、旋涡等不确定因素，容易使得航行器实际航向与设定航向发生偏差，造成航行器不能按照设定航向行进。

目前，为了解决水下航行器在水中运动偏航的问题，传统的采用螺旋桨推进的航行器一般是通过改变舵角来实现航向控制。但由于仿蝠鲼航行器与传统的航行器在驱动模式上存在很大的差别，不能采用和其一样的控制航向的方法，需要设计一种新的航向控制的方法。而在公开的文献中，还未有用本发明提出的方法来实现仿蝠鲼航行器滑翔航行控制的例子。

发明专利CN108829102A中提供一种自适应艏向信息融合的波浪滑翔器航向控制方法，该方法是通过对潜体的艏向控制间接实现波浪滑翔器系统整体航向的控制。该方法的操作复杂，在具体实现时需要获得浮体艏向角和潜体艏向角，最后操作舵机驱动舵板转动，进而实现航向控制；并且由于仿蝠鲼航行器外形和结构的特殊性，在保证性能的同时还要保证仿生外形，还鉴于波浪滑行器和仿蝠鲼航行器结构的差异，该种控制航向的方式并不适用于仿蝠鲼航行器。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法。

技术方案

实现本发明主要通过如下步骤实现：

1.利用姿态传感器获得仿蝠鲼航行器当前的航向角角度。

2.计算当前航向角和期望航向角的航向偏差和偏差变化率。

3.通过控制器控制横滚机构转动的角度，使仿蝠鲼航行器在滑翔运动中做出横滚动作，进而使仿蝠鲼航行器的航向角度发生改变。通过不断调节使仿蝠鲼航行器的实际航向角和设定航向角的角度偏差为0。

本发明通过调节横滚机构来实现仿蝠鲼航行器航向控制的原理是：

首先建立机体坐标系，以仿蝠鲼航行器的重心O’为坐标原点；x轴沿机体纵轴，从航行器尾部指向头部为x轴正方向；y轴在机体平面内与x轴垂直，指向机体外侧为正方向；z轴垂直于机体平面，指向机体下方为正。定义仿蝠鲼航行器从尾部向前观察绕x轴发生右横滚时，横滚角为正；仿蝠鲼航行器纵轴和地球北极之间的夹角为航向角，航向角沿顺时针航向增加。以仿蝠鲼航行器下潜滑翔过程为例来进行分析说明。当仿蝠鲼航行器以一定速度下潜滑翔的时候，如果调节横滚机构发生左横滚，此时仿蝠鲼航行器将沿着航行器纵轴发生偏转，使仿蝠鲼航行器呈现左横滚状态。由于仿蝠鲼航行器处于下潜状态，处于低头姿态，由图1可知，由于头部绕流使得头部上表面的流速大于下表面的流速；尾部的绕流使得尾部上表面的流体速度大于下表面的流体速度。因此在头部上表面的压力大于下表面的压力，整体表现为指向下表面外侧的压力，在尾部上表面的压力小于下表面的压力，整体表现为指向上表面外侧的压力，即由于流动的不对称性，头部和尾部产生相反方向的压力。又由于航行器处于左横滚状态下，航行器机体与水平面间存在一个夹角，这是头部和尾部的压力将在水平面上产生分力，且头部压力的分力指向航行器的右侧，尾部的压力的分力指向航行器的左侧，这时分力将产生一个使航行器绕过重心的竖直轴向右偏转的偏航力矩，在该偏航力矩的作用下仿蝠鲼航行器航向将发生变化。利用这个偏航力矩就可以对仿蝠鲼航行器的航向进行矫正，达到航向控制的目的。

一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法，其特征在于步骤如下：

步骤1、通过姿态传感器获得仿蝠鲼航行器当前的航向角角度

计算当前航向角

和设定航向角

的航向角偏差e：

当前航向角和目标航向角的偏差变化率ec：

其中：ec(t)为当前时刻的当前航向角和目标航向角的偏差变化率；e(t)为当前时刻的航向角偏差；e(t-1)为上一时刻的航向角偏差；Δt为上一时刻到当前时刻的时间差；

步骤2、将输入量模糊化：将航向偏差e和航向偏差率ec根据隶属度函数映射到相应的模糊子集中，以实现对输入参数的模糊化处理；其中模糊子集为{NB,NM,NS,ZO,PS,PM,PB}，子集中的元素分别代表负大，负中，负小，零，正小，正中，正大；

步骤3：将模糊化处理后航向偏差e和航向偏差率ec分别对应到表1模糊规则库的纵坐标轴和横坐标轴上，查表得出Δk_p和Δk_d的模糊值；

步骤4：对模糊值进行解模糊化得到Δk_p和Δk_d的值，利用Δk_p和Δk_d对控制器参数k_p和k_d进行调整：

其中，k_p为经模糊算法调整后的比例系数；k_d为经模糊算法调整后的微分系数；Δk_p为利用模糊算法得到的比例系数改变量；Δk_d为利用模糊算法得到的比例微分系数该变量；k_p0为调整前的比例系数；k_d0为调整前的微分系数；

步骤5：以离散后的PD控制算法计算出控制器的输出u(k)：

其中，u(k)为当前时刻控制器的控制输出；e(k)为当前控制周期的航向偏差；e(k-1)为上一控制周期的航向偏差；T为离散的时间间隔；

步骤6：对控制器的输出量进行函数处理，得出横滚机构的横滚角度θ(k)：

θ(k)＝a·sin(u(k))+b

其中，θ(k)为横滚机构需要执行的横滚角度；a为比例系数；b为横滚零位，即使航行器保持水平时横滚机构的横滚角度；u(k)为PD控制器的输出量；

步骤7：将横滚机构的横滚角度θ(k)送到横滚机构，使横滚机构按照得到的角度值执行相应的横滚动作，通过调节仿蝠鲼航行器横滚机构横滚角度，以达到调整仿蝠鲼航行器的滑翔航向的目的。

有益效果

本发明提出的一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法，利用姿态传感器获得仿蝠鲼航行器当前的航向角角度和期望航向角的航向偏差和偏差变化率；通过控制器控制横滚机构转动的角度，使仿蝠鲼航行器在滑翔运动中做出横滚动作，进而使仿蝠鲼航行器的航向角度发生改变。通过不断调节使仿蝠鲼航行器的实际航向角和设定航向角的角度偏差为0。

本发明在真实环境下进行实验，采用模糊PD控制器，以航向角的偏差e和航向角的偏差率ec作为控制器的输入，得出仿蝠鲼航行器在下潜过程中利用横滚机构进行航向控制的航向变化曲线，验证了该方法的可行性和正确性。

本发明提供的仿蝠鲼航行器航向控制方法具有如下的有益效果：

1.鉴于以前的发明中所提供的航向控制方法不适用于仿蝠鲼航行器，本发明中所提供的利用横滚机构来控制仿蝠鲼航行器滑翔航向的方法，为仿蝠鲼航行器滑翔航向控制提供了一种新的方法

2.本发明提供的方法只需要调节横滚机构的横滚角度即可实现仿蝠鲼航行器滑翔航向的控制，方法简单。由于环境介质物理特性的不同，仿蝠鲼航行器在不同的工作环境中水平姿态会发生变化，直接使用控制器输出量进行控制的传统方法存在一定的局限性，本发明针对控制器的输出量利用函数进行处理，解决了在不同工作环境中仿蝠鲼航行器的姿态水平问题，使航向的控制更加的精准。同时采用的模糊控制算法能够更好的适应航行器工作环境的复杂多变性，提高了仿蝠鲼航行器在复杂工况中滑翔运动的稳定性和可靠性。

3.本发明利用样机进行实验，得出横滚机构对航向控制的变化曲线。验证了在真实工作环境中本发明所提供方法的可行性和可靠性。

附图说明

图1为本发明下潜过程中左横滚时仿蝠鲼航行器受力示意图；

图2为本发明仿蝠鲼航行器航向控制原理框图；

图3为本发明实验所得航向控制曲线图。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

参照图2本发明提供了一种基于横滚机构的仿蝠鲼航行器航向控制的方法，具体的实施步骤如下：

步骤1：由姿态传感器获取的当前航向角

和上位机发送的设定航向角

的数据，计算当前航向角和目标航向角的航向角偏差e以及航向角偏差率ec。

步骤2：将航向角偏差和航向角偏差率作为输入送到控制器得到作为控制输出的横滚角度。

步骤1中，通过仿蝠鲼航行器内的姿态传感器获得当前的航向角

利用当前航向角和设定航向角进行计算得到航向角偏差e和航向角偏差率ec，具体计算方法为：

用当前航向角和目标航向角计算得到航向角偏差e为：

其中e为航向角偏差；

为当前航向角；

为设定航向角。

对航向角偏差求导，得到航向角偏差变化率ec：

其中ec(t)为当前时刻的当前航向角和目标航向角的偏差变化率；e(t)为当前时刻的航向角偏差；e(t-1)为上一时刻的航向角偏差；Δt为上一时刻到当前时刻的时间差。

步骤2中，将航向偏差e和航向偏差率ec根据隶属度函数映射到相应的模糊子集中，以实现对输入参数的模糊化处理。其中模糊子集为{-3,-2,-1,0,1,2,3}，子集中的元素分别代表负大，负中，负小，零，正小，正中，正大。

步骤3：将模糊化后航向偏差e和航向偏差率ec分别对应到表2所示的模糊规则库的纵坐标和横坐标轴上，查表2得出Δk_p和Δk_d的模糊值，采用最大隶属度方法对Δk_p和Δk_d的模糊量进行解模糊化处理得到Δk_p和Δk_d的清晰值，按照下式对PD控制中参数进行修正。

表2航向模糊规则库

步骤4：对PD控制中的参数k_p和k_d按照如下的计算规则进行调整：

其中，k_p为经模糊算法调整后的比例系数；k_d为经模糊算法调整后的微分系数；Δk_p为利用模糊算法得到的比例系数改变量；Δk_d为利用模糊算法得到的比例微分系数该变量；k_p0为调整前的比例系数；k_d0为调整前的微分系数。

步骤5：按离散后的PD控制算法计算出控制器的输出u(k)：

其中，u(k)为当前时刻控制器的控制输出；e(k)为当前控制周期的航向偏差；e(k-1)为上一控制周期的航向偏差；T为离散的时间间隔。

将控制器的输出量u(k)按照下式处理得到的横滚角度θ(k)：

θ(k)＝a·sin(u(k))+b (5)

其中，θ(k)为横滚机构需要执行的横滚角度；a为放大系数；b为横滚零位，即使航行器保持水平时横滚机构的横滚角度，本例取a＝10，b＝0；u(k)为PD控制器的输出量。

步骤6：将计算所得横滚角度送至横滚机构，使横滚机构执行相应的横滚角度调整。由于横滚机构的作用使得航行器姿态发生变化，最终导致航行器产生了如图1所示的偏航力矩，使得航行器发生航向的调整。通过对不同的偏航情况实施不同的横滚方案，产生一个能够校正航向的偏航力矩，从而实现航向控制，让航行器按照设定的航向行进。

对所提出的基于横滚机构的仿蝠鲼航行器航向控制方法进行实验。

利用样机在真实环境中进行实验，图3和展示了实验所得的下潜过程中仿蝠鲼航行器航向控制曲线图。

Claims (1)

1.一种基于横滚机构的仿蝠鲼航行器滑翔航向控制的方法，其特征在于步骤如下：

步骤1、通过姿态传感器获得仿蝠鲼航行器当前的航向角角度

计算当前航向角

和设定航向角

的航向角偏差e：

当前航向角和目标航向角的偏差变化率ec：

其中：ec(t)为当前时刻的当前航向角和目标航向角的偏差变化率；e(t)为当前时刻的航向角偏差；e(t-1)为上一时刻的航向角偏差；Δt为上一时刻到当前时刻的时间差；

步骤2、将输入量模糊化：将航向偏差e和航向偏差率ec根据隶属度函数映射到相应的模糊子集中，以实现对输入参数的模糊化处理；其中模糊子集为{NB，NM，NS，ZO，PS，PM，PB}，子集中的元素分别代表负大，负中，负小，零，正小，正中，正大；

步骤3：将模糊化处理后航向偏差e和航向偏差率ec分别对应到下表模糊规则库的纵坐标轴和横坐标轴上，查表得出Δk_p和Δk_d的模糊值；

步骤4：对模糊值进行解模糊化得到Δk_p和Δk_d的值，利用Δk_p和Δk_d对控制器参数k_p和k_d进行调整：

其中，k_p为经模糊算法调整后的比例系数；k_d为经模糊算法调整后的微分系数；Δk_p为利用模糊算法得到的比例系数改变量；Δk_d为利用模糊算法得到的比例微分系数该变量；k_p0为调整前的比例系数；k_d0为调整前的微分系数；

步骤5：以离散后的PD控制算法计算出控制器的输出u(k)：

其中，u(k)为当前时刻控制器的控制输出；e(k)为当前控制周期的航向偏差；e(k-1)为上一控制周期的航向偏差；T为离散的时间间隔；

步骤6：对控制器的输出量进行函数处理，得出横滚机构的横滚角度θ(k)：

θ(k)＝a·sin(u(k))+b

其中，θ(k)为横滚机构需要执行的横滚角度；a为比例系数；b为横滚零位，即使航行器保持水平时横滚机构的横滚角度；u(k)为PD控制器的输出量；

步骤7：将横滚机构的横滚角度θ(k)送到横滚机构，使横滚机构按照得到的角度值执行相应的横滚动作，通过调节仿蝠鲼航行器横滚机构横滚角度，以达到调整仿蝠鲼航行器的滑翔航向的目的。

Images