一种基于质心与尾鳍的仿蝠鲼水下航行器定深控制方法与流程

本发明属于水下航行器运动控制领域，涉及一种基于质心与尾鳍的仿蝠鲼水下航行器定深控制方法，是基于质心与尾鳍协同控制的仿蝠鲼水下航行器的定深控制方法。

背景技术：

仿蝠鲼水下航行器是一种新型仿生水下航行器，采用胸鳍推进模式，具有高效率、高机动性、高稳定性、低扰动等显著优点，可以实现优秀的原位观测、适应复杂海域，同时还具有良好的生物亲和性，可用于珊瑚海星灾害监测、海洋牧场鱼情监测等领域，具有广阔的应用前景和理论研究价值。

胸鳍推进仿生水下航行器的模型十分复杂，至今没有关于胸鳍摆动式推进鱼类的比较准确的模型，限制了传统控制方法在仿生航行器上的应用。目前，针对本类型航行器常用的定深控制方法有模糊控制算法、pid控制算法等，但模糊控制算法的信息简单处理将导致系统控制精度降低、动态性能变差；pid控制算法中参数为定值，不随系统变化，不具有自整定特性。在公开的文献中，少有提及胸鳍仿生水下航行器定深控制的具体方法，如专利：一种鱼形仿生水下机器人及其控制方法[p].cn111284663b与专利：仿生蝠鲼机器人[p].cn112093018a。

技术实现要素：

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于质心与尾鳍的仿蝠鲼水下航行器定深控制方法，是一种基于质心机构与尾鳍协同控制的仿蝠鲼水下航行器的定深控制方法。

技术方案

一种基于质心与尾鳍的仿蝠鲼水下航行器定深控制方法，其特征在于步骤如下：

步骤1、计算深度偏差及深度偏差变化率：通过深度传感器获得水下航行器的当前深度为y，向下为正；任务设定的参考深度为yd，则深度偏差δy为：

δy＝y-yd

对深度偏差求导，得到深度偏差变化率v为：

其中：t为水下航行器的深度传感器信息更新周期；

步骤2、以深度偏差作为输入将定深任务分为以下三个工况：

(1)迅速下潜/上浮：当δy≥a时，即当前深度远离目标深度，水下航行器处于迅速下潜/上浮阶段，采用表1与表3组合而成的大模糊pid比例系数整定范围；

(2)启动定深：当a≥δy≥b时，当前深度靠近目标深度，水下航行器处于预备定深阶段，采用表2与表3组合而成的模糊pid比例系数范围整定范围；

(3)保持定深：当δy≤c时，当前深度近似等于目标深度，无需进行调节；

其中：a,b,c分别为深度大误差、深度小误差和深度误差允许值，依据任务要求设置；

所述步骤(1)和步骤(2)中的整定范围取决于基于模糊控制器修正pid控制器参数，即模糊控制器同时以深度偏差与深度偏差变化率作为输入，利用模糊控制器修正pid控制器参数；

深度偏差与深度偏差变化率的离散论域为{-3，-2，-1，0，1，2，3}，其模糊语言值同样为{nb，nm，ns，ze，ps，pm，pb}＝{负大，负中，负小，零，正小，正中，正大}；

以δy作为横坐标e的值、v作为纵坐标ec的值进行表格查询，整正后的pid参数为：

其中：kp为原始比例系数，ki为原始积分系数，kd为原始微分系数；δkp为模糊控制器查表得到的比例系数整定调整量，δki为模糊控制器查表得到的积分系数整定调整量，δkd为模糊控制器查表得到的微分系数整定调整量；kpf为经过模糊控制器查表得到的比例系数整定调整量δkp与原始比例系数kp相加后的比例系数，kif为经过模糊控制器查表得到的积分系数整定调整量δki与原始积分系数相加后的积分系数，kdf为经过模糊控制器查表得到的微分系数整定调整量δkd与原始微分系数δkd相加后的微分系数；

利用模糊控制器整定过pid参数的pid控制器计算出的深度控制量δyc为：

其中，kpf为调整后的比例系数，kif为调整后的积分系数，kdf为调整后的微分系数，δy为当前深度到目标深度的偏差值，t为积分时间，dt为微分时间；

所述表1、表2和表3为：

表1：kp模糊控制系数大范围修正表

表2：kp模糊控制系数小范围修正表

表3：ki/kd模糊控制系数修正表

步骤3：对深度控制量δyc进行离散化处理可得到离散后的控制量制为：

其中：δyn为第n个控制周期当前深度到目标深度的深度偏差，t为离散化的时间间隔；

步骤4、依据控制量计算执行机构值：

计算质心机构的质心块移动的位置：

mc＝km*δyc*(mmax-mmin)

其中：km为控制量到执行值的转换系数，mmax为质心机构质心块移动的上限，mmin为质心机构质心块移动的下限，δyc为离散后的控制量，mc为质心机构需要移动的目标位置；

计算出尾鳍动作的幅值：

qc＝r*(qmax-qmin)*sin(p*δyc)

其中：p为控制量转换系数，r为输出增益系数，qmax为尾鳍动作的上限，qmin为尾鳍动作的下限，δyc为离散后的控制量，qc为尾鳍动作的目标位置；

步骤5：将质心机构需要移动的目标位置mc控制水下航行器的质心机构，尾鳍动作的目标位置qc控制水下航行器的尾鳍，改变航行器胸鳍扑动状态下俯仰姿态角，从而产生不同大小的俯仰力矩，使得水下航行器的深度偏差达到误差允许范围内，即在扑动过程中完成定深任务。

有益效果

本发明提出的一种基于质心与尾鳍的仿蝠鲼水下航行器定深控制方法，针对模型不确定性强的胸鳍推进仿生水下航行器，采用分段将定深任务分为三个阶段，结合模糊控制器对pid系数实施在线修正，实现对质心结构与尾鳍的调节，进而完成定深游动的任务。定深过程中水下航行器的胸鳍机构由舵机驱动且始终保持扑动状态，舵机输出定常的幅值与相位差以产生沿载体坐标系x轴的推力。通过改变质心块的前后移动及尾鳍的上下偏转产生俯仰力矩，达到定深效果。

采用本发明具有如下的有益效果：

1.传统的pid控制算法存在一定局限性，控制算法中参数为定值，不随系统变化，不具有自整定特性。本发明将航行器的定深控制根据深度偏差的大小分为三种工况，根据不同工况设计不同的模糊控制规则去修正pid参数，具有参数自整定性，且该方法可以普遍适用于没有准确的动力学模型的机器人控制中，具有很强的适用性和移植性。

2.由于对得到的控制量针对质心机构做了线性化计算、针对尾鳍做了非线性化计算，再作用于执行机构，使得质心机构与尾鳍在深度偏差大的情况下联合作用能到达机构最大值，获得航行器垂向速度达到的最大输出值、响应更快速、大大加快了深度误差收敛的速度等有益效果；深度偏差介于大深度偏差与小深度偏差之间的情况下仅使用尾鳍调节，降低了功耗，使得航行器具有更长的续航能力，且得益于尾鳍输出的非线性计算可以提高控制的精度且减小超调量。

附图说明

图1为本发明胸鳍驱动水下航行器的定深控制方法原理图；

图2为本发明定深程序流程图。

图3为仿蝠鲼水下航行器简图，两侧扑翼结构关于航行器主体对称，图中编号意义如下：

1,2,3为航行器扑翼骨架的鳍条单元；

4为航行器头部；

5为航行器主体，质心块、电子仓等均包覆其内；

6为航行器右侧扑翼；

7为航行器尾鳍部分。

具体实施方式

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

本发明采用的技术方案是通过深度传感器获取当前深度信息，利用基于分段的模糊pid控制，在迅速下潜/上浮、启动定深与保持定深三个阶段采用不同的控制策略，依据不同的工况调节质心结构和尾鳍，最终实现水下航行器在扑动状态下的定深控制，具体步骤如下：

步骤1：计算深度偏差及深度偏差变化率。

通过深度传感器获得水下航行器的当前深度为y(向下为正)，任务设定的参考深度为yd，则深度偏差δy为

δy＝y-yd(1)

对深度偏差求导，得到深度偏差变化率v为

其中t为水下航行器的深度传感器信息更新周期。

步骤2：执行分段策略。

基于胸鳍推进仿生水下航行器模型的复杂性和参数不确定性的特点，设计了专家分段控制器。在获取了控制系统深度偏差的实时信息后，执行专家分段策略。

分段控制器以深度偏差作为输入将定深任务分为以下三个工况：

(1)迅速下潜/上浮：当δy≥a时，即当前深度远离目标深度，水下航行器处于迅速下潜/上浮阶段，模糊控制表选用为表4与表6。此种工况下质心机构与尾鳍为执行机构根据控制量共同作用。此时质心机构与尾鳍根据控制量共同作用产生较大的俯仰力矩，实现快速下潜、上浮。

(2)启动定深：当a≥δy≥b时，当前深度靠近目标深度，水下航行器处于预备定深阶段，模糊控制表选用为表5与表6。此种工况下仅有尾鳍部分根据控制量进行调节。此时仅质心系统回归中位，仅尾鳍根据控制量进行调节，产生较小的俯仰力矩，减小超调。

(3)保持定深：当δy≤c时，当前深度近似等于目标深度，无需进行调节。

其中a,b,c分别为设定的深度大误差、深度小误差和深度误差允许值。

步骤3：基于模糊控制器修正pid控制器参数。

模糊控制器同时以深度偏差与深度偏差变化率作为输入，利用模糊控制器修正pid控制器参数，其原理图如图1所示。

深度偏差的基本论域为δy∈[-|ymax|，|ymax|]，其中ymax为深度偏差的最大值；深度偏差变化率的论域为v∈[-|vmax|，|vmax|]，其中vmax为深度偏差率的最大值。深度偏差与深度偏差变化率的离散论域为{-3，-2，-1，0，1，2，3}，其模糊语言值同样为{nb，nm，ns，ze，ps，pm，pb}＝{负大，负中，负小，零，正小，正中，正大}。

根据不同的工况通过模糊控制器进行查表运算对原有pid参数进行自整定，以δy作为横坐标e的值、v作为纵坐标ec的值进行表格查询，整正后的pid参数为：

式(3)中，kp为原始比例系数，ki为原始积分系数，kd为原始微分系数；δkp为模糊控制器查表得到的比例系数整定调整量，δki为模糊控制器查表得到的积分系数整定调整量，δkd为模糊控制器查表得到的微分系数整定调整量；kpf为经过模糊控制器查表得到的比例系数整定调整量δkp与原始比例系数kp相加后的比例系数，kif为经过模糊控制器查表得到的积分系数整定调整量δki与原始积分系数相加后的积分系数，kdf为经过模糊控制器查表得到的微分系数整定调整量δkd与原始微分系数δkd相加后的微分系数；

实施例的模糊控制表如附表4、5、6所示。

利用模糊控制器整定过pid参数的pid控制器计算出的深度控制量δyc为：

式(4)中，kpf为调整后的比例系数，kif为调整后的积分系数，kdf为调整后的微分系数，δy为当前深度到目标深度的偏差值，t为积分时间，dt为微分时间；

对上式进行离散化处理可得到离散后的控制量制为：

式(5)中δyn为第n个控制周期当前深度到目标深度的深度偏差，t为离散化的时间间隔。

步骤4：依据控制量计算执行机构值。

依据离散化的控制量可以计算出质心机构的质心块移动的位置，具体公式为：

mc＝km*δyc*(mmax-mmin)(6)

式(6)中km为控制量到执行值的转换系数，mmax为质心机构质心块移动的上限，mmin为质心机构质心块移动的下限，δyc为离散后的控制量，mc为质心机构需要移动的目标位置。

依据离散化的控制量可以计算出尾鳍动作的幅值，具体公式为：

qc＝r*(qmax-qmin)*sin(p*δyc)(7)

式(7)中p为控制量转换系数，r为输出增益系数，qmax为尾鳍动作的上限，qmin为尾鳍动作的下限，δyc为离散后的控制量，qc为尾鳍动作的目标位置。

如mmax为20，mmin为-20，mc为0，qmax为30，qmin为30，qc为5时，表示质心机构的执行值为0，质心块应该保持在零位不动作；尾鳍执行值为5，尾鳍应该打到5°的位置，此时为启动定深阶段。

根据控制量调节解算的水下航行器的参考质心位置和参考尾鳍偏转角度，调节质心机构和尾鳍使得水下航行器质心和尾鳍达到参考位置，改变航行器胸鳍扑动状态下俯仰姿态角大小，使得水下航行器产生俯仰力矩，进行深度调节，深度偏差达到误差允许范围内，即实现水下航行器在胸鳍扑动状态下的定深过程，其程序流程图如图2所示，仿蝠鲼水下航行器简图如图3所示。

附表4：

具体示例kp模糊控制系数大范围修正表

附表5：

具体示例kp模糊控制系数小范围修正表

附表6：

具体示例ki/kd模糊控制系数修正表