CN114371698A - 一种自主避障游动仿生机器鱼 - Google Patents

Abstract

本发明公开了一种自主避障游动仿生机器鱼，鱼体包括头部、胸部、腰部及尾部，红外避障传感器安装于头部，主控单元通过密封舱安装于胸部内，左胸鳍通过左胸鳍关节与胸部连接，左胸鳍关节由左胸鳍舵机驱动，右胸鳍通过右胸鳍关节与胸部连接，右胸鳍关节由右胸鳍舵机驱动，腰部通过腰关节与胸部连接，腰关节由腰关节舵机驱动，尾部通过横关节与腰部连接，横关节由横关节舵机驱动。本发明优点在于：本发明仿生机器鱼包含的功能和传感器丰富，当仿生机器鱼在水下工作时出现一些故障时，红外避障功能模块可以防止仿生机器鱼上障碍物损坏，在无人操控时，仿生机器鱼也可以进行模拟真鱼的自主避障游动。

Description

一种自主避障游动仿生机器鱼

技术领域

本发明涉及一种仿生机器鱼，尤其涉及一种自主避障游动仿生机器鱼。

背景技术

仿生机器鱼的研发结合了生物学、仿生学、机械电子学、流体力学以及材料学等众多学科，具备极高的科技感和观赏价值，可应用在商场内，水族馆内，海洋公园内对游客进行展览、表演，也可供游客进行自主操作，具备极高的科研价值和商业价值。

申请名称为一种智能观赏仿生鱼及其控制方法，公开号为CN10419954A的发明专利公开了一种用于观赏的仿生鱼及控制方法。该仿生鱼的控制方法能控制仿生鱼前进、转弯，下潜，但是其内部无压力传感器测量深度值，无法定深游动，其内部无红外传感器，无法进行避障，也无遥控手柄操作方式，不能满足自动避障的需要。

申请名称为一种结合GPS与红外导航的机器鱼返航定位方法，公开号为201710563867.1的发明专利公开了一种机器鱼的定位方法。该方法无法控制仿生鱼上浮、下潜、翻转等功能，也无自红外传感器实现主避障游动功能，不能满足自动避障的需要。

因此，现有的仿生机器鱼只能够在水中游动，不能够进行自主避障，因此需要一种仿生机器鱼在水中游动的同时能够自主避障。

发明内容

本发明是针对现有的一种自主避障游动仿生机器鱼的不足，通过技术手段提供一种能够自主避障的可模仿真实的鱼类一样在水中自主游动的仿生机器鱼。

这种一种自主避障游动仿生机器鱼，鱼体包括头部、胸部、腰部及尾部，前红外避障传感器、左红外避障传感器、右红外避障传感器及下红外避障传感器安装于头部，主控单元的电源转换模块、CPU模块、舵机驱动模块、红外信号检测模块安装于胸部内，左胸鳍通过左胸鳍关节与胸部连接，左胸鳍关节由左胸鳍舵机驱动，右胸鳍通过右胸鳍关节与胸部连接，右胸鳍关节由右胸鳍舵机驱动，腰部通过腰关节与胸部连接，腰关节由腰关节舵机驱动，尾部通过横关节与腰部连接，横关节由横关节舵机驱动。

进一步优化，所述的胸部内的主控单元还包括无线通信模块，灯带驱动模块、压力传感器驱动模块，且无线通信模块，灯带驱动模块、压力传感器驱动模块均设置在密封舱内，无线通信模块内置或外接于鱼体的胸部。

进一步优化，所述的头部还安装有压力传感器、深度传感器、惯性导航传感器，惯性导航传感器采用六轴惯性导航传感器。

进一步优化，所述的CPU模块的芯片的四路脉宽调制PWM波形发生器引脚分别与左胸鳍舵机、右胸鳍舵机、横关节舵机，腰关节舵机连接，进行驱动控制，进而带动各个对应关节转动；芯片的四路ADC模数转换引脚分别与前红外避障传感器、左红外避障传感器、右红外避障传感器及下红外避障传感器的输出端连接，采集电压信号，通过计算获得当前各红外避障传感器与障碍物之间的距离；芯片的SPI通信接口引脚与压力传感器进行数据通信，采集压力值和温度值；芯片的IO控制引脚与灯带驱动模块连接，对鱼身灯带进行通断控制，控制鱼身灯带闪烁，增强观赏娱乐效果；芯片的串行通信接口引脚与无线通信模块连接，接收无线模块传递进来的数据。

进一步优化，所述的无线通信模块通过手柄向其发送控制指令，或通过计算机、平板电脑等操作设备发送的指令进行相应动作。

进一步优化，所述的CPU模块控制鱼体进行随机游动，CPU模块控制左胸鳍和右胸鳍同时向上翻转，横关节保持水平零位，腰关节按正弦曲线摆动并加入向上的偏置时，鱼体进行上浮运动，且正弦摆动频率和幅值越大，游动速度越快；CPU模块控制左胸鳍和右胸鳍同时向下翻转，横关节保持水平零位，腰关节按正弦曲线摆动并加入向下的偏置时，鱼体进行下潜运动，且正弦摆动频率和幅值越大，游动速度越快；CPU控制左胸鳍向上翻转，右胸鳍向下翻转，或右胸鳍向上翻转，左胸鳍向下翻转，横关节保持水平零位，腰关节按正弦曲线摆动时，鱼体进行滚转运动，且正弦摆动频率和幅值越大，游动速度越快；CPU控制左胸鳍和左右胸鳍保持水平零位，横关节向左偏置，腰关节按正弦曲线摆动时，鱼体进行左转运动，且横关节向左偏置越大，正弦摆动频率和幅值越大，转弯角度越大，游动速度越快；CPU控制左胸鳍和右胸鳍保持水平零位，横关节向右偏置，腰关节按正弦曲线摆动时，鱼体进行右转运动，且横关节向右偏置越大，正弦摆动频率和幅值越大，转弯角度越大，游动速度越快。

进一步优化，所述的CPU模块控制鱼体进行避障游动，当前红外避障传感器、左红外避障传感器、右红外避障传感器及下红外避障传感器都未检测到障碍物时，仿生鱼直行游动；当右侧红外传感器检测到障碍物时，仿生鱼减速左转游动；当左侧红外传感器检测到障碍物时，仿生鱼减速右转游动；当左红外避障传感器、右红外避障传感器检测到障碍物时，仿生鱼减速右转游动；当前方红外传感器检测到障碍物时，仿生鱼减速左转游动；当前红外避障传感器、右红外避障传感器检测到障碍物时，仿生鱼减速左转游动；当前红外避障传感器、左红外避障传感器检测到障碍物时，仿生鱼减速右左转游动；当前红外避障传感器、左红外避障传感器及右红外避障传感器检测到障碍物时，仿生鱼减速左转游动；当下方红外传感器检测到障碍物时，仿生鱼加速上升游动。

进一步优化，所述的随机运动的随机俯仰角、随机偏航角、随机滚转角、随机游动时间、随机游动速度、随机游动深度，由随机函数确定，随机函数为rand()％(n-m+1)+m，rand()为0～32767之间的随机整数，随机俯仰角、随机偏航角、随机滚转角、随机游动时间、随机游动速度、随机游动深度为[m,n]之间的整数。

进一步优化，所述的避障运动的避障俯仰角、避障滚转角、避障偏航角由随机俯仰角、随机偏航角、随机滚转角及惯性导航传感器采集到的当前俯仰角、当前滚转角、当前偏航角通过偏差值进行PID运算确定，PID算法如下：

r(t)-CPU模块的输出深度

d(t)-深度传感器输入的当前深度与目标深度之间的误差

Kp-比例系数

Ti-积分时间常数

Td-微分时间常数

T-调节周期。

进一步优化，所述的CPU模块控制鱼体进行定深游动，在设置好目标深度之后，根据深度传感器采集到的当前深度值，通过对目标深度与当前深度的偏差进行PID控制运算，经过PID控制运算输出的结果为尾关节舵机摆动时的偏置，当目标深度大于当前深度时，偏置为正，此时尾关节舵机向上偏置，仿生机器鱼进行上浮运动。当目标深度小于当前深度时，偏置为负，此时尾关节舵机向下偏置，仿生机器鱼进行下浮运动。通过对PID参数的调节，可以实现仿生机器鱼在指定深度范围内精准、平稳的游动。

PID控制算法如下：

r(t)-CPU模块的输出值

d(t)-深度传感器输入的当前深度与目标深度之间的误差

Kp-比例系数

Ti-积分时间常数

Td-微分时间常数

T-调节周期。

本发明优点在于：本发明实现了半米级仿生自主避障游动鱼的设计、制作，具备极高的科技感和观赏价值，可应用在商场内，水族馆内，海洋公园内对游客进行展览、表演，也可供游客进行自主操作，具备极高的科研价值和商业价值。

本发明所设计的仿生机器鱼可实现仿生机器鱼像真实的鱼类一样在水中自主游动，也可使用遥控器、电脑等进行手动操作，具有鱼类一样的高效率和灵活的机动性，游动效果生动逼真。

本发明仿生机器鱼包含的功能和传感器丰富，如红外避障、深度检测、鱼尾正弦函数摆动、自主避障、定深游动等等，当仿生机器鱼在水下工作时出现一些故障，比如：对外通信模块失效导致机器鱼失去控制时，红外避障功能模块可以防止仿生机器鱼上障碍物损坏，在无人操控时，仿生机器鱼也可以进行模拟真鱼的自主避障游动。

附图说明

图1为一种自主避障游动仿生机器鱼的结构示意图。

图2为一种自主避障游动仿生机器鱼的控制系统图。

图中标记：1-头部，2-胸部，3-腰部，4-尾部，5-主控单元，6-腰关节舵机，7-横关节舵机，8-左胸鳍舵机，9-右胸鳍舵机，11-左红外避障传感器，12-右红外避障传感器，13-下红外避障传感器，14-前红外避障传感器。

具体实施方式

下面结合附图对一种自主避障游动仿生机器鱼进行进一步说明。

这种自主避障游动仿生机器鱼，鱼体包括头部1、胸部2、腰部3及尾部4，前红外避障传感器14、左红外避障传感器11、右红外避障传感器12、下红外避障传感器13、压力传感器、惯性导航传感器安装于头部1，各传感器产生的电压信号芯片进行数据采集、换算，获得相应的距离值，进而控制其自由游动或避障游动。惯性导航传感器采用六轴惯性导航传感器，集成高性能、小体积的惯性测量单元(IMU)与航行姿态参考系统，采用了MEMS陀螺仪与加速度技术，辅以高精度的磁力计，形成了可靠的姿态传感器与航姿参考系统。可以测量加速度、角速度、欧拉角、四元数等数据，进而换算出当前姿态角，实现对仿生机器鱼的姿态闭环调节。主控单元5的电源转换模块、CPU模块、舵机驱动模块、红外信号检测模块通过密封舱安装于胸部2内，左胸鳍通过左胸鳍关节与胸部2连接，左胸鳍关节由左胸鳍舵机8驱动，右胸鳍通过右胸鳍关节与胸部2连接，右胸鳍关节由右胸鳍舵机9驱动，腰部3通过腰关节与胸部2连接，腰关节由腰关节舵机6驱动，尾部4通过横关节与腰部3连接，横关节由横关节舵机7驱动。这种自主避障游动仿生机器鱼，可进行游动、转向、上浮、下潜、翻转等动作。游动动作主要由左、右胸鳍关节、横关节、腰关节进行控制，通过腰关节按照正弦曲线规律进行运动，可以为仿生机器鱼提供前进动力，通过改变腰关节摆动频率，可以调节游动速度。通过改变腰关节摆动偏置，可以使仿生机器鱼进行俯仰、下潜运动。通过改变横关节摆动偏置，可以使仿生机器鱼进行转向运动。

这种自主避障游动仿生机器鱼，CPU模块的芯片的四路脉宽调制PWM波形发生器引脚分别与左胸鳍舵机8、右胸鳍舵机9、横关节舵机7，腰关节舵机6连接，进行驱动控制，进而带动各个对应关节转动，可以根据芯片PWM引脚输出的周期，不同占空比的PWM信号，来调节舵机转动的角度，进而控制左右胸鳍关节、横关节、腰关节的转动，实现对仿生鱼的姿态控制；芯片的四路ADC模数转换引脚分别与前红外避障传感器14、左红外避障传感器11、右红外避障传感器12及下红外避障传感器13的输出端连接，采集电压信号，通过计算获得当前各红外避障传感器与障碍物之间的距离；芯片的SPI通信接口引脚与压力传感器进行数据通信，采集压力值和温度值；芯片的IO控制引脚与灯带驱动模块连接，对鱼身灯带进行通断控制，控制鱼身灯带闪烁，增强观赏娱乐效果；芯片的串行通信接口引脚与无线通信模块连接，接收无线模块传递进来的数据。

这种自主避障游动仿生机器鱼，可以进行随机游动，即可以进行自由游动，包括随机速度游动，随机方向游动，根据随机数计算算法，随机计算出仿生鱼的目标俯仰角、偏航角、滚转角及深度，仿生机器鱼可以根据随机算法计算结果，充分模拟真鱼的游动姿态，可在无人操作的状态下，进行自主游动。

随机方向游动如下：当左右胸鳍同时向上翻转，横关节保持水平零位，腰关节按正弦曲线摆动并加入向上的偏置时，仿生机器鱼进行上浮运动。正弦摆动频率和幅值越大，游动速度越快。当左右胸鳍同时向下翻转，横关节保持水平零位，腰关节按正弦曲线摆动并加入向下的偏置时，仿生机器鱼进行下潜运动。正弦摆动频率和幅值越大，游动速度越快。当左胸鳍同时向上翻转，右胸鳍同时向下翻转，或右胸鳍同时向上翻转，左胸鳍同时向下翻转，横关节保持水平零位，腰关节按正弦曲线摆动时，仿生机器鱼进行滚转运动。正弦摆动频率和幅值越大，游动速度越快。当左右胸鳍保持水平零位，横关节向左偏置，腰关节按正弦曲线摆动时，仿生机器鱼进行左转运动。横关节向左偏置越大，正弦摆动频率和幅值越大，转弯角度越大，游动速度越快。当左右胸鳍保持水平零位，横关节向右偏置，腰关节按正弦曲线摆动时，仿生机器鱼进行右转运动。横关节向右偏置越大，正弦摆动频率和幅值越大，转弯角度越大，游动速度越快。

随机运动的随机俯仰角、随机偏航角、随机滚转角、随机游动时间、随机游动速度、随机游动深度由随机函数确定，随机函数为rand()％(n-m+1)+m，rand()为0～32767之间的随机整数，随机俯仰角、随机偏航角、随机滚转角、随机游动时间、随机游动速度、随机游动深度为[m,n]之间的整数。

如控制仿生鱼的随机偏航角为100-200°，即产生区间[100,200]范围内随机整数的计算方法为:rand()％(200-100+1)+100即rand()％101+100，仿生鱼即可被控制在100°至200°范围内随机游动。

如控制仿生鱼的随机滚转角为0-90°，即产生区间[0,90]范围内随机整数，计算方法为:rand()％(90-0+1)+100即rand()％91+100，仿生鱼即可被控制在0°至90°范围内随机滚转游动。

如控制仿生鱼的随机俯仰角为0-45°，即产生区间[0,45]范围内随机整数，计算方法为:rand()％(45-0+1)+100即rand()％46+100，仿生鱼即可被控制在0°至45°范围内随机俯仰游动。

如控制仿生鱼的随机游动速度为0-2，即产生区间[0,2]范围内随机整数，计算方法为:rand()％(2-0+1)+100即rand()％3+100，仿生鱼即可被控制在0至2范围内随机游动。

如控制仿生鱼的游动深度为0-3米，即产生区间[0,3]范围内随机整数，计算方法为:rand()％(3-0+1)+100即rand()％4+100，仿生鱼即可被控制在0米至3米范围内随机游动。

如控制仿生鱼的随机游动时间为2-8秒，产生区间[2,8]范围内随机整数的计算方法为:rand()％(8-2+1)+100即rand()％7+100，仿生鱼即可被控制在2秒至8秒范围内随机游动，游动结束后，重新根据随机算法，计算随机俯仰角、随机滚转角、随机偏航角、随机游动时间的随机值。

这种自主避障游动仿生机器鱼，可以进行自主避障游动，当前红外传感器、下红外传感器、左红外传感器、右红外传感器检测到障碍物时，可进行自主避障游动，避障控制算法如下所述。通过随机算法计算出的随机俯仰角、随机偏航角、随机滚转角、随机游动时间、随机游动速度，根据红外传感器采集到的距障碍物距离的信息，结合避障控制算法，可实现仿生鱼的自主避障游动。

当前、下、左、右四个方向红外传感器都未检测到障碍物时，仿生鱼直行游动。当右侧红外传感器检测到障碍物时，仿生鱼减速左转游动。当左侧红外传感器检测到障碍物时，仿生鱼减速右转游动。当左、右两侧红外传感器检测到障碍物时，仿生鱼减速右转游动。当前方红外传感器检测到障碍物时，仿生鱼减速左转游动。当前方、右侧红外传感器检测到障碍物时，仿生鱼减速左转游动。当前方、左侧红外传感器检测到障碍物时，仿生鱼减速右左转游动。当前方、左侧、右侧三个方向红外传感器检测到障碍物时，仿生鱼减速左转游动。当下方红外传感器检测到障碍物时，仿生鱼加速上升游动。

仿生机器鱼可以根据随机算法生成的随机俯仰角、随机滚转角、随机偏航角的角度数值，结合惯性导航传感器采集到的当前俯仰角、滚转角、偏航角数值，通过其偏差值进行PID运算，将运算结果换算成各个关节的驱动角度，进行姿态控制。PID闭环控制算法如下：

u(t)-CPU模块的输出值

e(t)-惯性导航传感器输入与随机俯仰角、随机偏航角、随机滚转角之间的误差

Kp-比例系数

Ti-积分时间常数

Td-微分时间常数

T-调节周期。

如通过随机俯仰角与当前俯仰角差值进行PID运算，得出腰关节上下摆动偏置，则控制其俯仰运动。

如通过随机偏航角与当前偏航角差值进行PID运算，得出横关节左右摆动偏置，则控制其转向运动。

如通过随机滚转角与当前滚转角差值进行PID运算，得出左右胸鳍关节摆动偏置，则控制其滚转运动。

CPU模块控制鱼体进行定深游动，在设置好目标深度之后，根据深度传感器采集到的当前深度值，通过对目标深度与当前深度的偏差进行PID控制运算，经过PID控制运算输出的结果为尾关节舵机摆动时的偏置，当目标深度大于当前深度时，偏置为正，此时尾关节舵机向上偏置，仿生机器鱼进行上浮运动。当目标深度小于当前深度时，偏置为负，此时尾关节舵机向下偏置，仿生机器鱼进行下浮运动。通过对PID参数的调节，可以实现仿生机器鱼在指定深度范围内精准、平稳的游动。

PID控制算法如下：

r(t)-CPU模块的输出值

d(t)-深度传感器输入的当前深度与目标深度之间的误差

Kp-比例系数

Ti-积分时间常数

Td-微分时间常数

T-调节周期。

Claims (10)

1.一种自主避障游动仿生机器鱼，鱼体包括头部、胸部、腰部及尾部，其特征在于：前红外避障传感器、左红外避障传感器、右红外避障传感器及下红外避障传感器安装于头部，主控单元的电源转换模块、CPU模块、舵机驱动模块、红外信号检测模块通过密封舱安装于胸部内，左胸鳍通过左胸鳍关节与胸部连接，左胸鳍关节由左胸鳍舵机驱动，右胸鳍通过右胸鳍关节与胸部连接，右胸鳍关节由右胸鳍舵机驱动，腰部通过腰关节与胸部连接，腰关节由腰关节舵机驱动，尾部通过横关节与腰部连接，横关节由横关节舵机驱动。

2.根据权利要求1所述的一种自主避障游动仿生机器鱼，其特征在于：所述的胸部内的主控单元还包括无线通信模块，灯带驱动模块、压力传感器驱动模块，且无线通信模块，灯带驱动模块、压力传感器驱动模块均设置在密封舱内，无线通信模块内置或外接于鱼体的胸部。

3.根据权利要求1所述的一种自主避障游动仿生机器鱼，其特征在于：所述的头部还安装有压力传感器、深度传感器、惯性导航传感器，惯性导航传感器采用六轴惯性导航传感器。

4.根据权利要求1或2所述的一种自主避障游动仿生机器鱼，其特征在于：所述的CPU模块的芯片的四路脉宽调制PWM波形发生器引脚分别与左胸鳍舵机、右胸鳍舵机、横关节舵机，腰关节舵机连接，进行驱动控制，进而带动各个对应关节转动；芯片的四路ADC模数转换引脚分别与前红外避障传感器、左红外避障传感器、右红外避障传感器及下红外避障传感器的输出端连接，采集电压信号；芯片的SPI通信接口引脚与压力传感器进行数据通信，采集压力值和温度值；芯片的IO控制引脚与灯带驱动模块连接；芯片的串行通信接口引脚与无线通信模块连接，接收无线模块传递进来的数据。

5.根据权利要求2所述的一种自主避障游动仿生机器鱼，其特征在于：所述的无线通信模块通过手柄向其发送控制指令。

6.根据权利要求1所述的一种自主避障游动仿生机器鱼，其特征在于：所述的CPU模块控制鱼体进行随机游动，CPU模块控制左胸鳍和右胸鳍同时向上翻转，横关节保持水平零位，腰关节按正弦曲线摆动并加入向上的偏置时，鱼体进行上浮运动，且正弦摆动频率和幅值越大，游动速度越快；CPU模块控制左胸鳍和右胸鳍同时向下翻转，横关节保持水平零位，腰关节按正弦曲线摆动并加入向下的偏置时，鱼体进行下潜运动，且正弦摆动频率和幅值越大，游动速度越快；CPU控制左胸鳍向上翻转，右胸鳍向下翻转，或右胸鳍向上翻转，左胸鳍向下翻转，横关节保持水平零位，腰关节按正弦曲线摆动时，鱼体进行滚转运动，且正弦摆动频率和幅值越大，游动速度越快；CPU控制左胸鳍和左右胸鳍保持水平零位，横关节向左偏置，腰关节按正弦曲线摆动时，鱼体进行左转运动，且横关节向左偏置越大，正弦摆动频率和幅值越大，转弯角度越大，游动速度越快；CPU控制左胸鳍和右胸鳍保持水平零位，横关节向右偏置，腰关节按正弦曲线摆动时，鱼体进行右转运动，且横关节向右偏置越大，正弦摆动频率和幅值越大，转弯角度越大，游动速度越快。

7.根据权利要求1所述的一种自主避障游动仿生机器鱼，其特征在于：所述的CPU模块控制鱼体进行避障游动，当前红外避障传感器、左红外避障传感器、右红外避障传感器及下红外避障传感器都未检测到障碍物时，仿生鱼直行游动；当右侧红外传感器检测到障碍物时，仿生鱼减速左转游动；当左侧红外传感器检测到障碍物时，仿生鱼减速右转游动；当左红外避障传感器、右红外避障传感器检测到障碍物时，仿生鱼减速右转游动；当前方红外传感器检测到障碍物时，仿生鱼减速左转游动；当前红外避障传感器、右红外避障传感器检测到障碍物时，仿生鱼减速左转游动；当前红外避障传感器、左红外避障传感器检测到障碍物时，仿生鱼减速右左转游动；当前红外避障传感器、左红外避障传感器及右红外避障传感器检测到障碍物时，仿生鱼减速左转游动；当下方红外传感器检测到障碍物时，仿生鱼加速上升游动。

8.根据权利要求6所述的一种自主避障游动仿生机器鱼，其特征在于：所述的随机运动的随机俯仰角、随机偏航角、随机滚转角、随机游动时间、随机游动速度、随机游动深度由随机函数确定，随机函数为rand()％(n-m+1)+m，rand()为0～32767之间的随机整数，随机俯仰角、随机偏航角、随机滚转角、随机游动时间、随机游动速度、随机游动深度为[m,n]之间的整数。

9.根据权利要求7所述的一种自主避障游动仿生机器鱼，其特征在于：所述的避障运动的避障俯仰角、避障滚转角、避障偏航角由随机俯仰角、随机偏航角、随机滚转角及惯性导航传感器采集到的当前俯仰角、当前滚转角、当前偏航角通过偏差值进行PID运算确定，PID算法如下：

u(t)-CPU模块的输出值

e(t)-惯性导航传感器输入与随机俯仰角、随机偏航角、随机滚转角之间的误差

Kp-比例系数

Ti-积分时间常数

Td-微分时间常数

T-调节周期。

10.根据权利要求1所述的一种自主避障游动仿生机器鱼，其特征在于：所述的CPU模块控制鱼体进行定深游动，在设置好目标深度之后，根据深度传感器采集到的当前深度值，通过对目标深度与当前深度的偏差进行PID控制运算，经过PID控制运算输出的结果为尾关节舵机摆动时的偏置，当目标深度大于当前深度时，偏置为正，此时尾关节舵机向上偏置，仿生机器鱼进行上浮运动。当目标深度小于当前深度时，偏置为负，此时尾关节舵机向下偏置，仿生机器鱼进行下浮运动；

PID控制算法如下：

r(t)-CPU模块的输出深度

d(t)-深度传感器输入的当前深度与目标深度之间的误差

Kp-比例系数

Ti-积分时间常数

Td-微分时间常数

T-调节周期。

Images