基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法

摘要

本发明涉及一种基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法，针对模型不确定性强的胸鳍推进仿生水下航行器，采用分段将定深任务分为迅速下潜/上浮、预备定深与启动定深的三个阶段，结合模糊控制器对PID系数实施在线修正，实现对质心结构与浮力系统的调节，完成对航行器水下扑游深度的调节，达到航行器定深游动的目的。定深过程中水下航行器的胸鳍机构由舵机驱动且始终保持扑动状态，舵机输出定常的幅值与相位差以产生沿载体坐标系X轴的推力。本发明控制精度高，可以广泛应用于胸鳍推进仿生水下机器人技术领域。

权利要求

1.一种基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法，其特征在于步骤如下：步骤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与原始积分系数ki相加后的积分系数，kdF为经过模糊控制器查表得到的微分系数整定调整量Δkd与原始微分系数kd相加后的微分系数；

利用模糊控制器整定过PID参数的PID控制器计算出的深度控制量Δyc为：其中：kpF为调整后的比例系数，kiF为调整后的积分系数，kdF为调整后的微分系数，Δy为当前深度到目标深度的偏差值，t为积分时间；

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

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

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

步骤3：对深度控制量Δyc进行离散化处理可得到离散后的控制量为：其中：Δyn为第n个控制周期当前深度到目标深度的深度偏差，T为离散化的时间间隔；

步骤4、将控制量转换为执行机构值：

离散化的控制量需要转换为质心机构的质心块移动的位置：

其中：Mzero为质心机构设置的参考零位，Mk为控制量到执行值的转换系数，Aup为质心机构质心块移动的上限，Adown为质心机构质心块移动的下限，Δyc为离散后的控制量，Mc为质心机构需要移动的目标位置；

离散化的控制量需要转换为浮力系统的活塞移动的位置：

其中：Bzero为浮力系统设置的参考零位，Bk为控制量到执行值的转换系数，Bup为浮力系统活塞移动的上限，Bup＝50，Bdown为浮力系统活塞移动的下限，Bdown＝‑50，Δyc为离散后的控制量，Bc为浮力系统需要移动的目标位置；

步骤5：以质心机构需要移动的目标位置Mc和浮力系统需要移动的目标位置Bc作为控制量，调节扑动状态下的水下航行器的质心机构和浮力系统，改变航行器胸鳍扑动状态下俯仰姿态角与浮力大小，使得水下航行器的深度偏差达到误差允许范围内，即在扑动过程中完成定深任务。

说明书

基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法

技术领域

[0001] 本发明属于水下航行器运动控制领域，涉及一种基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法，是一种胸鳍推进模式的仿蝠鲼水下航行器游动定深控制方法。

背景技术

[0002] 胸鳍推进仿生水下航行器是一种新型仿生水下航行器，具有良好的机动性和稳定性，可以实现优秀的原位观测、适应复杂海域，同时还具有良好的生物亲和性，可用于珊瑚海星灾害监测、海洋牧场鱼情监测、海洋馆科技交互等领域，并且有利于弥补现阶段军用无人水下航行器隐身能力不足、续航力有限、高效率推进与高机动推进无法兼顾的缺陷。

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

发明内容

[0004] 要解决的技术问题

[0005] 为了避免现有技术的不足之处，本发明提出一种基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法，针对现有技术存在不足的，提出一种胸鳍推进仿蝠鲼水下航行器的定深控制方法。

[0006] 技术方案

[0007] 一种基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法，其特征在于步骤如下：

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

[0009] Δy＝y‑yd

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

[0011]

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

[0013] 步骤2、执行分段策略：

[0014] 以深度偏差作为输入将定深任务分为以下三个工况：

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

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

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

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

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

[0020] 其中：深度偏差与深度偏差变化率的离散论域为{‑3，‑2，‑1，0，1，2，3}，其模糊语言值同样为{NB，NM，NS，ZE，PS，PM，PB}＝{负大，负中，负小，零，正小，正中，正大}；

[0021] 以Δy作为横坐标E的值、v作为纵坐标Ec的值进行表格查询，整正后的PID参数为：

[0022]

[0023] 其中：kp为原始比例系数，ki为原始积分系数，kd为原始微分系数；Δkp为模糊控制器查表得到的比例系数整定调整量，Δki为模糊控制器查表得到的积分系数整定调整量，Δkd为模糊控制器查表得到的微分系数整定调整量；kpF为经过模糊控制器查表得到的比例系数整定调整量Δkp与原始比例系数kp相加后的比例系数，kiF为经过模糊控制器查表得到的积分系数整定调整量Δki与原始积分系数相加后的积分系数，kdF为经过模糊控制器查表得到的微分系数整定调整量Δkd与原始微分系数Δkd相加后的微分系数；

[0024] 利用模糊控制器整定过PID参数的PID控制器计算出的深度控制量Δyc为：

[0025]

[0026] 其中：kpF为调整后的比例系数，kiF为调整后的积分系数，kdF为调整后的微分系数，Δy为当前深度到目标深度的偏差值，t为积分时间，dt为微分时间；

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

[0028]

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

[0030]

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

[0032]

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

[0034]

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

[0036] 步骤4、将控制量转换为执行机构值：

[0037] 离散化的控制量需要转换为质心机构的质心块移动的位置：

[0038]

[0039] 其中：Mzero为质心机构设置的参考零位，Mk为控制量到执行值的转换系数，Aup为质心机构质心块移动的上限，Adown为质心机构质心块移动的下限，Δyc为离散后的控制量，Mc为质心机构需要移动的目标位置；

[0040] 离散化的控制量需要转换为浮力系统的活塞移动的位置：

[0041]

[0042] 其中：Bzero为浮力系统设置的参考零位，Bk为控制量到执行值的转换系数，Bup为浮力系统活塞移动的上限，Bdown为浮力系统活塞移动的下限，Δyc为离散后的控制量，Bc为浮力系统需要移动的目标位置；

[0043] 步骤5：以质心机构需要移动的目标位置Mc和浮力系统需要移动的目标位置Bc作为控制量，调节扑动状态下的水下航行器的质心机构和浮力系统，改变航行器胸鳍扑动状态下俯仰姿态角与浮力大小，使得水下航行器的深度偏差达到误差允许范围内，即在扑动过程中完成定深任务。

[0044] 有益效果

[0045] 本发明提出的一种基于质心与浮力系统的仿蝠鲼水下航行器定深控制方法，针对模型不确定性强的胸鳍推进仿生水下航行器，采用分段将定深任务分为迅速下潜/上浮、预备定深与启动定深的三个阶段，结合模糊控制器对PID系数实施在线修正，实现对质心结构与浮力系统的调节，完成对航行器水下扑游深度的调节，达到航行器定深游动的目的。定深过程中水下航行器的胸鳍机构由舵机驱动且始终保持扑动状态，舵机输出定常的幅值与相位差以产生沿载体坐标系X轴的推力。本发明控制精度高，可以广泛应用于胸鳍推进仿生水下机器人技术领域。

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

[0047] 1.本发明对于航行器的模型不敏感，适应性强，能够用于任意胸鳍推进模式下利用重浮力系统定深的航行器；

[0048] 2.传统的PID控制算法存在一定局限性，控制算法中参数为定值，不随系统变化，不具有自整定特性。本发明将航行器的定深控制根据深度偏差的大小分为三种工况，根据不同工况修正PID参数，具有参数自整定性；

[0049] 3.由于传统的模糊控制算法存在一定局限性，本发明针对深度偏差做了分段控制处理，解决了模糊控制算法信息简单处理会导致系统控制精度降低、动态性能变差的问题。具体优势分别体现在(a)深度偏差较大的情况下采用大比例系数的模糊PID控制，其大比例系数计算出的大质心机构与大浮力系统的执行值允许垂向速度达到航行器执行器的最大输出值，此时能够发挥出航行器全部的浮力调节能力，加快深度误差收敛的速度；(b)深度偏差介于大深度偏差与小深度偏差之间的情况下采用小比例系数的模糊PID控制，其小比例系数计算出的小质心机构与小浮力系统的执行值可以保证控制的精度与限制超调量，缩短深度稳定的时间；(c)深度偏差较小的情况下，计算出的质心机构与小浮力系统的执行值为零，不进行执行器的调节，利用航行器本身的稳定性，在保证定深精度的前提下关闭重浮力系统进行节能。

附图说明

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

[0051] 图2为本发明定深程序流程图；

[0052] 图3为本发明8m定深控制曲线图。

具体实施方式

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

[0054] 下面详细描述本发明的实施例,所述实施例的示例在附图中示出,参考附图描述的实施例是示例性的,旨在用于解释本发明,而不能理解为对本发明的限制。

[0055] 通过深度传感器获取当前深度信息，利用基于分段的模糊PID控制，在迅速下潜/上浮、启动定深与保持定深三个阶段采用不同的控制策略，调节质心结构和浮力系统，最终实现水下航行器在扑动状态下的定深控制，具体步骤如下：

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

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

[0058] Δy＝y‑yd                         (1)

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

[0060]

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

[0062] 步骤2：执行分段策略。

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

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

[0065] (1)迅速下潜/上浮：当Δy≥a时，即当前深度远离目标深度，水下航行器处于迅速下潜/上浮阶段，模糊控制表选用为表4与表6，即表1与表3组合而成的大模糊PID比例系数整定的实施例具体数值；此时质心机构与浮力系统根据控制量共同作用产生大俯仰力矩，实现快速下潜、上浮。

[0066] (2)启动定深：当a≥Δy≥b时，当前深度靠近目标深度，水下航行器处于预备定深阶段，模糊控制表选用为表5与表6，即表2与表3组合而成的模糊PID比例系数范围整定的实施例具体数值。此时质心机构与浮力系统根据控制量共同作用产生小俯仰力矩，减小超调。

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

[0068] 其中a,b,c分别为依据实际情况设置设定的深度大误差、深度小误差和深度误差允许值。

[0069] 步骤3：基于模糊控制器修正PID控制器参数。

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

[0071] 深度偏差的基本论域为Δy∈[‑|ymax|，|ymax|]，其中ymax为深度偏差的最大值；

[0072] 深度偏差变化率的论域为v∈[‑|vmax|，|vmax|]，其中vmax为深度偏差率的最大值。

[0073] 深度偏差与深度偏差变化率的离散论域为{‑3，‑2，‑1，0，1，2，3}，其模糊语言值同样为{NB，NM，NS，ZE，PS，PM，PB}＝{负大，负中，负小，零，正小，正中，正大}。

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

[0075]

[0076] 式(3)中，kp为原始比例系数，ki为原始积分系数，kd为原始微分系数；Δkp为模糊控制器查表得到的比例系数整定调整量，Δki为模糊控制器查表得到的积分系数整定调整量，Δkd为模糊控制器查表得到的微分系数整定调整量；kpF为经过模糊控制器查表得到的比例系数整定调整量Δkp与原始比例系数kp相加后的比例系数，kiF为经过模糊控制器查表得到的积分系数整定调整量Δki与原始积分系数相加后的积分系数，kdF为经过模糊控制器查表得到的微分系数整定调整量Δkd与原始微分系数Δkd相加后的微分系数；

[0077] 模糊控制表如附表4、5、6所示。

[0078] 利用模糊控制器整定过PID参数的PID控制器计算出的深度控制量Δyc为：

[0079]

[0080] 式(4)中，kpF为调整后的比例系数，kiF为调整后的积分系数，kdF为调整后的微分系数，Δy为当前深度到目标深度的偏差值，t为积分时间，dt为微分时间；

[0081] 步骤3：对公式4进行离散化处理可得到离散后的控制量制为：

[0082]

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

[0084] 步骤4：控制量转换为执行机构值。

[0085] 离散化的控制量需要转换为质心机构的质心块移动的位置，范围为‑30mm至30mm,具体公式为：

[0086]

[0087] 式(6)中Mzero为质心机构设置的参考零位，Mk为控制量到执行值的转换系数，Δyc为离散后的控制量，Mc为质心机构需要移动的目标位置。

[0088] 离散化的控制量需要转换为浮力系统的活塞移动的位置，范围为‑50mm至50mm,具体公式为：

[0089]

[0090] 式(7)中Borez为浮力系统设置的参考零位，Bk为控制量到执行值的转换系数，Δyc为离散后的控制量，Bc为浮力系统需要移动的目标位置。

[0091] 如Mc为30，Bc为‑50，即质心机构的质心块需要移动到30mm的位置，浮力系统的活塞需要移动到‑50mm的位置，此时为迅速下潜状态。

[0092] 步骤5：根据控制量调节解算出水下航行器的参考质心位置和参考浮心位置，调节质心机构和浮力系统使得水下航行器质心和浮心达到参考位置，改变航行器胸鳍扑动状态下俯仰姿态角与浮力大小，使得水下航行器的深度偏差达到误差允许范围内，即实现水下航行器在胸鳍扑动状态下的定深过程，其程序流程图如图2所示，定深曲线如图3所示。

[0093] 表4：

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

[0095]

[0096] 表5：

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

[0098]

[0099] 表6：

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

[0101]