一种多关节机器鱼及其运动控制方法与流程

1.本发明属于机器人运动控制方法领域，具体涉及一种多关节机器鱼及其运动控制方法。


背景技术：

2.随着我国科技和经济的快速发展，水产养殖行业迎来了发展的大好前景，但在水产养殖过程中仍依赖于专业人员主观判断及参与，存在耗时耗力和误判率大等问题，无法做到科学养殖。于是，为了提高水产养殖的经济效益及水产品的质量，用于水质检测的水下移动机器人越来越多。其中，仿生机器鱼的使用最为广泛，于是如何安全高效地控制仿生机器鱼完成自动巡航及避障，并使仿生机器鱼能够自主游动到预定的采集点准确采集水质参数变得尤为重要。
3.传统仿生机器鱼的控制方法有基于运动学和动力学模型的拟合鱼体波法和正弦振荡器法。在这些控制方法下，机器鱼有鲁棒性差、游动不灵活、姿态不协调等缺点，且对环境信息处理能力弱，导致难以实现自动巡航。


技术实现要素：

4.本发明的目的在于，提供一种多关节机器鱼机器运动控制方法，实现多关节机器鱼精确、高效、自然、协调的自动巡航及避障。
5.为解决上述技术问题，本发明的技术方案为：一种多关节机器鱼，包括密闭设置的鱼体结构，其上设置有传感器组，鱼体结构两侧对称连接有驱动鱼体结构上下运动的右胸鳍和左胸鳍，鱼体结构内部设有分别与右胸鳍和左胸鳍连接并带动其运动的右胸鳍舵机关节和左胸鳍舵机关节，鱼体结构内部还设有控制器、定位模块和通信模块；鱼体结构尾端连接有驱动鱼体结构水平游动的尾部关节机构，尾部关节机构包括至少三组依次相连的尾部舵机关节，末尾的尾部舵机关节尾端还连接有尾鳍；传感器组至少包括红外传感器、水质检测传感器和惯性导航传感器。
6.尾部舵机关节数量为三组。
7.多关节机器鱼的水平游动包括直线游动和转向游动；在多关节机器鱼进行直线游动时，通过控制三个尾部舵机关节使多关节机器鱼的尾鳍绕鱼体结构中心轴周期性左右摆动产生向前的推力，从而使机器鱼做直线游动，此时右胸鳍和左胸鳍不提供动力；在多关节机器鱼进行转向游动时，控制第一尾部舵机关节和第二尾部舵机关节向一侧偏转产生偏转力，再控制第三尾部舵机关节使尾鳍绕第二尾部舵机关节中心轴周期性左右摆动产生向前的推力，从而使多关节机器鱼做转向游动。
8.多关节机器鱼通过改变右胸鳍和左胸鳍的倾斜程度来实现上下运动，在尾鳍绕鱼体结构中心轴周期性左右摆动提供推进力的前提下，当右胸鳍和左胸鳍与鱼体结构平行时，多关节机器鱼保持水平方向的运动；当右胸鳍和左胸鳍同时向上倾斜时，多关节机器鱼实现上浮运动；当右胸鳍和左胸鳍同时向下倾斜时，多关节机器鱼实现下潜运动。
9.对多关节机器鱼的控制采用分层闭环控制，分为三层，包括：
10.传感器反馈层，由红外传感器和惯性导航传感器构成，用于提供多关节机器鱼的外部环境信息和自身姿态信息，不断进行多关节机器鱼模态及姿态的调整，从而有效控制多关节机器鱼游动；
11.高级控制层，用于进行多关节机器鱼的避障及路径规划和模态的选择；
12.低级控制层，用于对多关节机器鱼的姿态进行调整，调整内容还至少包括多关节机器鱼的航向和速度。
13.采用cpg网络模型控制尾部关节机构、右胸鳍和左胸鳍的运动。
14.所述高级控制层通过模糊算法进行避障及路径规划和模态的选择。
15.一种用于上述的一种多关节机器鱼的运动控制方法，包括以下步骤：
16.步骤一：判断多关节机器鱼是否处于目标位置，若是，则结束；若否，则利用红外传感器获取多关节机器鱼与障碍物的距离值d；
17.步骤二：获取距离值d，根据高级控制层选择相应的模态，得到对应cpg参数ω、m；其中为相位差、ω为频率、m为振幅；
18.步骤三：低级控制层根据输入的ω、m，输出稳定的周期性节律信号z，并根据此信号生成pwm信号，控制尾部关节机构、右胸鳍和左胸鳍的摆动角度和转速；
19.步骤四：传感器反馈层将距离值d反馈给高级控制层，将航向角偏差θ反馈给低级控制层，从而实现对多关节机器鱼模态和姿态的调整；
20.步骤五：实时调整，通过判断红外传感器采集到距离值d是否大于阈值，来对运动控制进行简化处理，若所有距离值d都大于阈值，说明不需要进行避障，只需通过cpg网络进行姿态调整即可，而若存在距离值d小于阈值，则需要先通过模糊算法进行避障，再通过cpg网络进行姿态调整；
21.步骤六：判断多关节机器鱼是否处于目标位置，若是，则结束；若否，则返回步骤一。与现有技术相比，本发明的有益效果为：
22.(1)利用多个传感器组成传感器网络采集环境信息，提高了机器鱼与环境的交互能力，避免了单一传感器探测信息采集不足、信息可信度不足的缺陷，可获取更全面、更精确的环境信息，从而使移机器鱼更准确地避障和运动。
23.(2)采用分层控制的方法来控制机器鱼，且模糊算法也分为两层，减少了许多计算量，简化了控制系统，使得机器鱼实时性、抗干扰性、鲁棒性大大提升。
24.(3)在对机器鱼的各关节进行协调控制时，加入了自身姿态信息的反馈，形成了闭环控制，机器鱼可实现在变化环境中的连续模态变化，速度和航向控制都可得到改善。
25.(4)采用cpg控制各个关节舵机，由于各个神经元之间的耦合关系，各个舵机之间的配合更加协调，可实现机器鱼各模态切换的平滑过渡。
26.(5)cpg网络采用基于极限环的cpg模型，各cpg神经元采用双向链式最近相邻耦合方式，其幅值、频率、相位都可单独调节，减少了许多cpg的参数，简化了系统的复杂度。
附图说明
27.图1为本发明实施例的结构示意图；
28.图2为本发明实施例的立体图；
29.图3为本发明实施例中cpg网络模型拓扑图；
30.图4为本发明实施例中分层闭环控制的原理图；
31.图5为本发明实施例中红外传感器的分布图；
32.图6为本发明实施例的运动控制流程图；
33.图中1——红外传感器，2——右胸鳍舵机关节，3——右胸鳍，4——gps模块，5——stm32模块，6——lora模块，7——第一尾部舵机关节，8——第二尾部舵机关节，9——第三尾部舵机关节，10——尾鳍，11——水质检测传感器，12——惯性导航传感器，13——左胸鳍，14——左胸鳍舵机关节，15——右红外传感器，16——上方红外传感器，17——下方红外传感器，18——前方红外传感器，19——左方红外传感器。
具体实施方式
34.为了使本技术领域的人员更好地理解本技术方案，下面结合附图和具体实施方式对本技术作进一步的详细说明。显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
35.针对机器鱼协调运动控制的需求，基于多关节舵机驱动的机器鱼，本发明提出了一种多关节机器鱼机器分层闭环运动控制方法。
36.机械结构及运动模式
37.机器鱼的运动控制与其机械结构密切相关，机器鱼的运动分为水平游动和上浮下潜两个方面。本发明机器鱼的机械结构示意图如图1、图2所示，本发明的技术方案为：一种多关节机器鱼，包括密闭设置的鱼体结构，其上设置有传感器组，鱼体结构两侧对称连接有驱动鱼体结构上下运动的右胸鳍3和左胸鳍13，鱼体结构内部设有分别与右胸鳍3和左胸鳍13连接并带动其运动的右胸鳍舵机关节2和左胸鳍舵机关节14，鱼体结构内部还设有控制器、定位模块和通信模块；鱼体结构尾端连接有驱动鱼体结构水平游动的尾部关节机构，尾部关节机构包括三组依次相连的尾部舵机关节，末尾的尾部舵机关节尾端还连接有尾鳍10；传感器组至少包括红外传感器1、水质检测传感器11和惯性导航传感器12。采用胸鳍和尾鳍10协同推进模式，依靠三个尾部舵机控制尾鳍10的摆动实现水平游动，依靠两个胸鳍舵机来控制胸鳍的角度实现上浮下潜。
38.机器鱼的水平游动分为直线游动和转向游动。在机器鱼进行直线游动时，通过控制三个尾部舵机关节使机器鱼的尾鳍10绕鱼体中心轴周期性左右摆动、产生向前的推力，从而使机器鱼做直线游动，此时胸鳍不提供动力。在机器鱼进行转向游动时，控制第一和第二尾部舵机关节8向一侧偏转、产生偏转力，再控制第三尾部舵机关节9使尾鳍10绕第二尾部舵机关节8中心轴周期性左右摆动、产生向前的推力，从而使机器鱼做转向游动。
39.在机器鱼进行上浮下潜运动时，通过改变胸鳍的倾斜程度来实现，在尾鳍绕鱼体中心轴周期性左右摆动、提供推进力的前提下，当胸鳍与鱼身平行时，机器鱼保持水平方向的运动；当控制左右胸鳍同时向上倾斜时，机器鱼实现上浮运动；当控制左右胸鳍同时向下倾斜时，机器鱼实现下潜运动。
40.cpg网络模型
41.hopf振荡器的微分方程为：
[0042][0043]
单个cpg神经元只能控制一个舵机关节，而本机器鱼有多个舵机关节，所以必需要构建cpg网络，使得多个cpg神经元进行耦合，实现对所有舵机的控制。本发明以hopf振荡器为基础，采用基于极限环的cpg网络模型，各cpg神经元采用双向链式最近相邻耦合方式，对n 2个cpg神经元进行耦合，其幅值、频率、相位都可单独调节，且输出信号收敛于正弦波，其微分方程为：
[0044][0045]
式中，i＝1,...,n,n 1,n 2，表示系统中的cpg神经元；其中，1,...,n代表尾部关节的cpg神经单元，n 1和n 2分别代表左右胸鳍关节的cpg神经元；x
i
、y
i
为第i个神经元的状态变量，x
i
为膜电势、y
i
为调节电势；ω
i
、m
i
分别表示第i个神经元的频率和振幅，表示神经元间的相位差；l1、l2为耦合系数，表示神经元间的抑制关系，其中l1为上行耦合系数、l2为下行耦合系数，h1、h2为收敛速度调节因子，c
i
表示输出信号的放大系数，z为输出。
[0046]
本发明中，多关节机器鱼的cpg模型网络拓扑图如图3所示。在本发明中，由于左右胸鳍是同步的，所以在耦合的时候，可以把cpg4和cpg5看作是同一cpg神经元，于是两个胸鳍cpg都只受到cpg1的状态影响，而cpg1会受到cpg2状态和胸鳍cpg状态的共同影响。
[0047]
当cpg网络建立后，初始膜电势x
i
，初始调节电势y
i
，耦合系数l1、l2，收敛速度调节因子h1、h2，输出信号的放大系数c
i
就已确定，接下来只需根据环境信息的不同调节相位差频率ω
i
、振幅m
i
三个参数因子就可以改变cpg的输出z，实现对各舵机的协调控制，从而实现机器鱼的灵活协调游动。其中主要决定机器鱼的模态，ω
i
、m
i
主要决定机器鱼的速度。
[0048]
本发明中，机器鱼模态分为直游、左转、右转、上浮、下潜五种，速度分为快、慢两种。
[0049]
分层闭环控制方法
[0050]
本发明中，对机器鱼的控制采用分层闭环控制，分为三层：传感器反馈层、高级控制层、低级控制层。其中闭环体现在传感器反馈层对高级控制层和低级控制层的反馈上。分层闭环运动控制原理图如图4所示。
[0051]
(1)传感器反馈层
[0052]
传感器反馈层由机器鱼头部搭载的5个红外传感器1和体内搭载的惯性导航传感器12构成的传感器网络组成，不断为高级控制层和低级控制层提供机器鱼的外部环境信息和自身姿态信息，不断进行机器鱼模态及姿态的调整，从而有效控制机器鱼游动。
[0053]
在本发明中，将红外传感器1检测到的机器鱼与障碍物的距离分为两个集合。因为红外传感器检测范围为10
‑
80cm，所以设置阈值为70cm。当检测到机器鱼与障碍物的距离高于阈值时，直接通过低级控制层对各个舵机关节进行控制；而当距离低于阈值时，先通过高级控制层给低级控制层发出命令，再通过低级控制层对各个舵机关节进行控制。
[0054]
在没有检测到障碍物时(红外传感器检测到的距离高于阈值)，通过搭载的惯性传感器对机器鱼进行速度和航向反馈，在不改变cpg神经元参数的条件下，直接通过耦合传感器反馈，实现对机器鱼的速度和航向控制；此过程不需要经过高级控制层的控制。
[0055]
在检测到障碍物时(红外传感器1检测到的距离低于阈值)，通过搭载的红外测距传感器对周围环境信息进行反馈，由避障算法分析处理后，按照避障策略，选择最合适的cpg参数，从而确定cpg网络模型，进而改变机器鱼的模态，达到控制机器鱼避障的目的。
[0056]
(2)高级控制层
[0057]
高级控制层由避障算法构成，主要进行机器鱼的避障及路径规划和运动模态的选择。
[0058]
在本发明中，由于机器鱼在水下环境中工作，无法实现基于精确信息的避障及路径规划，且机器鱼搭载多个传感器，因此可通过模糊避障算法对多个传感器信息进行处理后，再根据模糊规则进行决策，并根据传感器反馈层输入值的不同，选取相对应的模态，输出不同的cpg参数。
[0059]
机器鱼头部搭载的红外传感器分别为前方红外传感器18(f
rs
)、左方红外传感器19(l
rs
)、右方红外传感器15(r
rs
)、上方红外传感器16(t
rs
)以及下方红外传感器17(b
rs
)。机器鱼红外传感器的配置如图5所示。为了减少规则数目，将所搭载的红外传感器分为两组，模糊规则也对应分为水平避障规则和上浮下潜规则。其中前方红外传感器18(f
rs
)、左方红外传感器19(l
rs
)、右方红外传感器15(r
rs
)检测到的距离值d1对应水平避障规则的输入；上方红外传感器16(t
rs
)以及下方红外传感器17(b
rs
)检测到的距离值d2对应上浮下潜规则的输入。模糊避障输出为机器鱼的游动模态及对应cpg设置参数。
[0060]
在水平避障控制中，因为红外传感器1检测范围为10
‑
80cm，且当红外传感器1检测到距离少于70cm时才反馈给模糊避障算法，所以其论域为[10，70]；又因为机器鱼运动速度最大为25cm/s，所以选定一个安全距离为50cm，大于50cm对应模糊子集为{远(f)}，小于50cm对应模糊子集为{近(n)}。输出模糊集合有两个，分别为方向模糊集合{直游(d)、左转(l)、右转(r)}，速度模糊集合{快(k)、慢(s)}。水平避障规则如表1所示：
[0061]
表1水平避障规则
[0062]
[0063]
在本发明中，水平避障规则优先级高于上浮下潜规则，即机器鱼优先采取水平方向的避障行为。在水平避障规则中，当左右两侧都距离障碍物较远、而距离前方障碍物较近时，根据隶属度决定具体朝那边转弯。
[0064]
在上浮下潜避障控制中，同样设置红外传感器检测范围论域为[10，70]，由于上浮下潜速度相比较水平游动速度较慢，因此需要设置一个比较大的安全距离为60cm，大于60cm对应模糊子集为{远(f)}，小于60cm对应模糊子集为{近(n)}。输出模糊集合为{上浮(t)、下潜(b)、小半径转弯(m)}。上浮下潜规则如表2所示：
[0065]
表2上浮下潜规则
[0066][0067]
在上浮下潜规则中，当上下两侧都距离障碍物较近时，说明出现了前侧、左前侧、右前侧、上侧以及下侧都有障碍物的极端情况，就需要减小转弯半径，进行原地转弯；而当上下两侧都距离障碍物较远时，根据隶属度决定具体朝那边进行避障。
[0068]
通过两层模糊避障算法的判断，就可以根据红外传感器的输入，输出对应模态，但要得到具体的cpg参数，还要对模糊输出进行解模糊。
[0069]
在本模糊算法中，输入、输出隶属函数都采用梯形隶属函数，于是根据模糊规则中选取的规则和红外传感器输入距离的隶属度就可得出不同隶属度的幅值、频率和相位差，实现机器鱼模态的切换。
[0070]
(3)低级控制层
[0071]
低级控制层由cpg网络来控制，主要对机器鱼游动姿态进行调整，包含航向和速度调整。在红外传感器1没有检测到障碍物时，通过搭载的惯性传感器对机器鱼进行航向和速度反馈，在cpg模型中加入反馈项，调整cpg的输出，使其输出稳定的周期性节律运动信号，再根据此生成pwm信号，达到协调控制机器鱼各个关节舵机的目的，实现对机器鱼的航向和速度控制。此过程不需要经过避障算法的控制，只通过cpg网络控制即可。
[0072]
具有反馈机制的cpg网络模型微分方程为：
[0073][0074]
式中b
i
表示传感器反馈信息、与机器鱼当前航向角有关，惯性传感器检测到的实际航向和理想航向会形成一个偏差θ，即为b
i
，被反馈给cpg模型，使得cpg模型输出z发生变化，进而影响到pwm波的生成。
[0075]
优点和效果
[0076]
(1)利用多个传感器组成传感器网络采集环境信息，提高了机器鱼与环境的交互能力，避免了单一传感器探测信息采集不足、信息可信度不足的缺陷，可获取更全面、更精确的环境信息，从而使移机器鱼更准确地避障和运动。
[0077]
(2)采用分层控制的方法来控制机器鱼，且模糊算法也分为两层，减少了许多计算量，简化了控制系统，使得机器鱼实时性、抗干扰性、鲁棒性大大提升。
[0078]
(3)在对机器鱼的各关节进行协调控制时，加入了自身姿态信息的反馈，形成了闭环控制，机器鱼可实现在变化环境中的连续模态变化，速度和航向控制都可得到改善。
[0079]
(4)采用cpg控制各个关节舵机，由于各个神经元之间的耦合关系，各个舵机之间的配合更加协调，可实现机器鱼各模态切换的平滑过渡。
[0080]
(5)cpg网络以hopf振荡器为基础，采用基于极限环的cpg模型，各cpg神经元采用双向链式最近相邻耦合方式，其幅值、频率、相位都可单独调节，减少了许多cpg的参数，简化了系统的复杂度。
[0081]
本文中应用了具体个例对本技术的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本技术的技术方案及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本技术原理的前提下，还可以对本技术进行若干改进和修饰，这些改进和修饰也落入本技术权利要求的保护范围内。