[发明专利]摆尾式机器鱼的航向闭环控制与目标追踪方法及系统

说明书

本发明涉及一种摆尾式机器鱼的航向闭环控制与目标追踪方法及系统，属于机器鱼运动控制技术领域，应用于单一舵机驱动的摆尾式机器鱼，先计算并判断当前控制周期的误差值是否超过预设阈值，若是，则以当前控制周期的误差值作为输入，利用PD控制器更新摆尾式机器鱼的舵机控制信号，按照更新后的舵机控制信号控制摆尾式机器鱼下一控制周期的运动，若否，则按照当前控制周期的舵机控制信号控制摆尾式机器鱼下一控制周期的运动，从而无需建立精确模型，无需提前制备数据集，也无需任何辅助设备，即可实现摆尾式机器鱼的自主控制，保证摆尾式机器鱼的航向稳定。

技术领域

本发明涉及机器鱼运动控制技术领域，特别是涉及一种摆尾式机器鱼的航向闭环控制与目标追踪方法及系统。

背景技术

由于水下环境的复杂性、特殊性，无人水下系统运动过程中易受海浪、海流及海底地形等外界因素干扰，且通讯环境恶劣，导致其运动控制与传统陆、空航行器或机器人有较大区别。

基于仿生学原理的鱼形机器人(即机器鱼)，由于其形态、推进模式多样，不同推进模式机器鱼之间的控制方法存在着迥异差别。就单一舵机推进的机器鱼而言，其摆尾运动产生的鱼头晃动幅度相对较大，为产生一定前进速度所需要的摆动频率相对较高，因此解决由鱼尾摆动产生的航向晃动问题是对该类机器鱼进行航向稳定控制的关键。

现有的机器鱼自主控制研究普遍存在着难以在实际水域环境应用的问题，主要由于建模困难、运动复杂、感知能力不足等关键问题。首先，对于依赖模型的控制方法，由于在模型建立过程中引入了复杂的动力学模型，易导致机器鱼计算能力无法匹配算法性能；其次，依赖提前制备运动数据集或运动映射表的控制方法，其数据制备在一定程度上限制了机器鱼应对实际环境中未知干扰的能力；此外，过于依赖实验室全局摄像头等辅助设备的控制方法也直接导致了机器鱼难以应用于实际水域。因此，降低机器鱼的模型依赖度，研发鲁棒性更高的控制方法，同时提升机载感知系统的应用程度是促进机器鱼系统提高自主性的关键。

发明内容

本发明的目的是提供一种摆尾式机器鱼的航向闭环控制与目标追踪方法及系统，能够实现机器鱼的自主控制，易于在实际水域环境应用。

为实现上述目的，本发明提供了如下方案：

一种摆尾式机器鱼的航向闭环控制方法，应用于单一舵机驱动的摆尾式机器鱼，所述航向闭环控制方法包括：

获取当前控制周期所采集的鱼头航向集合；所述鱼头航向集合包括所述当前控制周期内采集的多个鱼头航向角；

以所述当前控制周期的鱼头航向集合和上一控制周期的鱼头航向集合作为输入，利用平均值计算公式计算得到所述当前控制周期的平均航向偏差；

计算所述当前控制周期的平均航向偏差与初始指定航向的差值，得到所述当前控制周期的误差值；判断所述当前控制周期的误差值是否超过预设阈值；

若是，则以所述当前控制周期的误差值作为输入，利用PD控制器更新摆尾式机器鱼的舵机控制信号，按照更新后的舵机控制信号控制所述摆尾式机器鱼下一控制周期的运动；

若否，则按照所述当前控制周期的舵机控制信号控制所述摆尾式机器鱼下一控制周期的运动。

一种摆尾式机器鱼的航向闭环控制系统，应用于单一舵机驱动的摆尾式机器鱼，所述航向闭环控制系统包括：

数据获取模块，用于获取当前控制周期所采集的鱼头航向集合；所述鱼头航向集合包括所述当前控制周期内采集的多个鱼头航向角；

偏差计算模块，用于以所述当前控制周期的鱼头航向集合和上一控制周期的鱼头航向集合作为输入，利用平均值计算公式计算得到所述当前控制周期的平均航向偏差；

判断模块，用于计算所述当前控制周期的平均航向偏差与初始指定航向的差值，得到所述当前控制周期的误差值；判断所述当前控制周期的误差值是否超过预设阈值；