一种用于漂浮物运送的机器鱼组运动轨迹控制方法与流程

本发明涉及一种机器鱼组运动轨迹控制方法，属于机器鱼控制技术领域。

背景技术：

随着海洋科技的长足发展，人们把目光投向了拥有丰富资源和巨大开发价值的海洋，适应各种非结构化环境的水下机器人得到迅猛的发展。将鱼类完美的水下运动能力结合的机器鱼，由于效率高、隐蔽性强、流体性能优良、推进效率高、机动性能好、噪声低等特性被广泛应用于资源探测、军事侦察、海洋监测、环境保护、娱乐观赏等众多领域中。仿生机器鱼主要分为鱼头1、鱼身2、鱼尾3三部分；如图1所示，鱼头1部分由控制电路板、电池、通讯模块、电源开关和胸鳍1-1构成，鱼头1预留了充气孔1-2、充电头1-3、天线1-4；鱼身2由三个关节串联组成，每个关节都是一个舵机。其优点在于成本低并具有良好的稳定性和精确性。

近年来国内外针对仿生机器鱼的研究层出不穷，作为科研的重要环节，关于水中机器鱼的仿真研究也逐渐成为该领域的研究热点之一。但是现有机器鱼研究方向主要在单个机器鱼个体的运动精度速度等控制；比方说人们已经开始采用机器鱼来捕捞海底产品，但是机器鱼都是单独行动的，没有统一的协同和管理；而且，由于水下环境的复杂性与不确定性，机器鱼在执行任务时不仅会受到水波扰动的影响，还需要考虑到仿生机器鱼不同于一般的无人平台(例如无人机、无人车、水下潜航器等)，它是柔性结构，依靠鱼鳍或尾部的摆动实现游动，因此机器鱼的尺寸、尾鳍摆动的幅度和瞬时运动的惯性等因素不能被忽略。因此，哪怕是运动控制精度速度达到非常高的水平，控制单个机器鱼个体进行运送、侦查等任务，也不如多个机器鱼协同作业效果好。

近年来，国内外争相举办各种类型的机器鱼大赛，旨在通过比赛交流机器鱼研究领域的新思想和新进展。机器鱼水球2v2比赛能够充分体现出两鱼协作的效果，比赛是以小型仿生机器鱼为研究主体，进行类比于陆地上的机器人足球比赛进行激烈的对抗。比赛场地为长方形水池(如图6所示)，配套设备包括水球1个，球门2个(甲乙双方各1个)，多关节机器鱼4条(甲乙双方各两条)，比赛双方控制终端全局视觉设备ccd(电荷耦合元件)摄像头、以及补充光源等。机器人水球比赛中，两队机器鱼进行对抗，将水球顶入对方球门次多者为胜。针对机器鱼的顶球策略，文献[sunyanbing，liuxiaorui.researchonrobotfishcontrolandsomeproblemsinwaterpolocompetition.robottechnologyandapplication，2009-07.]以球为中心对周边划分了主攻区、助攻区、缓冲区，但是精度不够。文献[taojin，pathplanninginrobotwaterpolocompetition[d].guangxiinstituteoftechnology，2011-04.]围绕鱼身姿态调整问题，提出了一种切入圆顶球算法(algorithmofcutroundpushball)，该算法为鱼划定切入圆，驱动鱼运动到驱动圆边界，再沿着切入圆运动到顶球点。文献[wangchao，huoyanyan，xiongxiaojun.trajectoryplanningstrategyforroboticwaterpolomatches.2018iopconf.ser.:mater.sci.eng.466012108.]提出一种基于区域划分的控制策略，对整个比赛的水池进行了区域划分，为每个区域制定不同的攻防策略，两条鱼的分工更加明确；并对球的周边区域进行细分，使鱼在不同的周边区域依据不同的顶球策略进行运动。

然而，机器鱼在执行寻迹动作时还会受到水波扰动的影响，导致鱼到达预设顶球点时与目标球的相对位置已经产生偏差，使球不能按照预定轨迹移动，进而导致时间增多；另外，还需要考虑到对方鱼对目标球的攻击产生的目标球位置变化等不确定因素的影响。两者在实际比赛过程中对鱼的轨迹控制策略影响最为明显，常会引发于鱼与目标球擦肩而过的情况发生。因此，采用什么样的轨迹策略让鱼能够精准击球，并使球按照预定较优轨迹运动至目标位置，是至关重要的因素。

综上，急需一种应用型仿生机器鱼组协同合作进行水下协同合作的轨迹控制策略。

技术实现要素：

本发明为解决现有缺少应用型机器鱼组协同合作技术，以及不能精准控制运送物体按轨迹运动的问题，提供了一种用于漂浮物运送的机器鱼组运动轨迹控制方法。

本发明所述一种用于漂浮物运送的机器鱼组运动轨迹控制方法，通过以下技术方案实现：

s1、在从目标位置c到待运送物位置b连线的延长线上，令距离待运送物d1处为待撞击点a，令距离待运送物d2处为预备撞击点g；d2>d1；d1大于一个鱼身的长度；点a、g的位置随b的变化而改变；

s2、调度机器鱼组中的一条机器鱼fish1运动到点a，另一条机器鱼fish2向点g运动；

s3、fish1继续向待运送物运动，当fish1的头部超过点a一个鱼身距离时，调度fish2由点g向点a运动，如果此过程中待运送物的位置发生变化，则fish2随着待运送物的位置变化调整姿态，保持游向所述延长线上，相距待运送物d1的位置；

s4、fish1的中心坐标相距b的距离为时，调度fish2离开点a向待运送物运动；

s5、当fish1在目标位置与待运送物位置连线方向上位置超过待运送物时，判断待运送物是否已经到达目标位置：

若到达，所述机器鱼组运动轨迹控制方法结束；

若没有到达，继续判断，如果fish2的中心坐标位于a、b之间，则调度fish1前往g，进入步骤s6；如果fish2的中心坐标没有位于a、b之间，则调度fish1前往a，进入步骤s7；

s6、fish2继续向待运送物运动，fish2的头部超过点a一个鱼身距离时，调度fish1由点g向点a运动，如果此过程中待运送物的位置发生变化，则fish1随着待运送物的位置变化调整姿态，保持游向所述延长线上，相距待运送物d1的位置；

s7、fish2的中心坐标相距b的距离为时，调度fish1离开点a向待运送物运动；

s8、当fish2在目标位置与待运送物位置连线方向上位置超过待运送物时，判断待运送物是否已经到达目标位置：

若到达，所述机器鱼组运动轨迹控制方法结束；

若没有到达，继续判断，如果fish1的中心坐标位于a、b之间，则调度fish2前往g，进入步骤s3；如果fish1的中心坐标没有位于a、b之间，则调度fish2前往a，进入步骤s4。

本发明最为突出的特点和显著的有益效果是：

本发明所涉及的一种用于漂浮物运送的机器鱼组运动轨迹控制方法，采用了长距调姿策略进行机器鱼轨迹的控制，长距调姿策略基于对机器鱼、待运送物、目标点相对位置的细致划分，通过设置鱼身到达待撞击点的距离和调姿策略，提高机器鱼、待运送物、目标位置处于三点一线的概率，避免近距离擦边，提高了撞击准确率，能够使待运送物基本能够按照预定的三点一线轨道运动至目标位置，进而缩短了运送时间；同时，在长距调姿策略的基础上结合了双鱼交替撞击策略，通过制定一条机器鱼撞击漂浮物的过程中，另一条鱼的回游策略，加强了两条鱼之间的配合，提高了运送漂浮物的效率。实验结果表明，本发明方法能够使能够更加迅速和准确地将漂浮物运送到目标位置。

附图说明

图1为仿生机器鱼结构示意图；

图2为本发明方法的机器鱼组轨迹示意图；goal表示目标；

图3为鱼身与待运送物相对位置的情况分布图；

图4为长距姿态策略示意图；

图5为本发明方法流程图；

图6为机器鱼水球2v2比赛全局视觉示意图；

图7为本发明实施例中的实验场景机器鱼击球场景示意图；position表示位置。

具体实施方式

具体实施方式一：结合图2、图3、图4、图5对本实施方式进行说明，本实施方式给出的一种用于无线传感器网络中的数据采集方法，具体包括以下步骤：

s1、在从目标位置c到待运送物位置b连线的延长线上，令距离待运送物d1处为待撞击点a，令距离待运送物d2处为预备撞击点g；d2>d1；d1大于一个鱼身的长度；点a、g的位置随b的变化而改变；

s2、调度机器鱼组中的一条机器鱼fish1运动到点a，另一条机器鱼fish2向点g运动；撞击过程中，当两鱼与a点距离相近时，可能出现两鱼相撞的情况；为此设置点g作为预备撞击点；如图2所示，当fish1沿route2(路径2)在靠近点a的途中，fish2沿route3(路径3)进入点g调整姿态。

s3、fish1继续向待运送物运动，当fish1的头部超过点a一个鱼身距离时，调度fish2由点g向点a运动，如果此过程中待运送物的位置发生变化，则fish2随着待运送物的位置变化调整姿态，保持游向所述延长线上，相距待运送物d1的位置；

s4、fish1的中心坐标相距b的距离为时，调度fish2离开点a向待运送物运动；

s5、当fish1在目标位置与待运送物位置连线方向上位置超过待运送物时，判断待运送物是否已经到达目标位置：

若到达，所述机器鱼组运动轨迹控制方法结束；

若没有到达，继续判断，如果fish2的中心坐标位于a、b之间，则调度fish1前往g，进入步骤s6；如果fish2的中心坐标没有位于a、b之间，则调度fish1前往a，进入步骤s7；可以避免鱼转身时鱼尾影响待运送物，也可以避免回程准备的fish1和fish2互相影响。

s6、fish2继续向待运送物运动，fish2的头部超过点a一个鱼身距离时，调度fish1由点g向点a运动，如果此过程中待运送物的位置发生变化，则fish1随着待运送物的位置变化调整姿态，保持游向所述延长线上，相距待运送物d1的位置；

s7、fish2的中心坐标相距b的距离为时，调度fish1离开点a向待运送物运动；

s8、当fish2在目标位置与待运送物位置连线方向上位置超过待运送物时，判断待运送物是否已经到达目标位置：

若到达，所述机器鱼组运动轨迹控制方法结束；

若没有到达，继续判断，如果fish1的中心坐标位于a、b之间，则调度fish2前往g，进入步骤s3；如果fish1的中心坐标没有位于a、b之间，则调度fish2前往a，进入步骤s4。

对于待运送物来说，最优的行进路线应该是整鱼、待运送物中心点b和目标点c处于三点一线的状态。如图3所示，当鱼处于a点时，依a-b-c方向撞击路程最近，不但路径短，容错率也高，即在机器鱼撞击时偏离了预设路线bc，偏离的距离也最小。然而，全局视觉系统能够采集到的坐标信息只包含机器鱼鱼头和鱼中心坐标(并不包含鱼尾)，且存在误差；机器鱼身转向不灵活(与自然界中的真鱼相比)，机器鱼在接近待运送物的时候常常难以精确达到待运送物的位置，经常发生绕着待运送物转圈或与待运送物擦边而过的情况。因此，如果仅凭位置或距离来选择控制机器鱼撞击，常常会效率低下。

如图3，依照机器鱼、待运送物、目标三点一线作直线linel，按linel经待运送物中心点的垂线linel’将区域一分为二，当鱼头处于图3location1(位置1)或location2(位置2)时，鱼都不适合撞击待运送物。在location2鱼头已经超过了待运送物，如果进行撞击，在掉头的过程中鱼很可能碰到待运送物，使其远离目标。在location1，鱼头虽然没有超过待运送物，但是由于在视觉系统中只能获得鱼头坐标和鱼中心点坐标，无法获得鱼尾坐标。当鱼身方向m(以鱼头和鱼中心两点建立的向量)和bc方向(linel)重合时，鱼尾并没有重合，而当鱼尾摆正姿态后，鱼头的方向又会偏离，这往往是鱼在location1难以撞击到待运送物的原因。当鱼身方向m和bc垂线linel’夹角α越小、鱼距离目标物体越近时，顶不到目标物体的概率越高。综上，可以将鱼和待运送物的位置分为以下3种情况：

1)鱼头超过linel’

这种情况下，鱼撞击待运送物是危险的。为缩短运送时间，目前现有策略倾向于将鱼驱动到距离待运送物比较近的区域，鱼一旦游到了待运送物的前面，不但无法将其顶向目标位置，还会阻碍另一条鱼的撞击，从而加大了运送的时间。

2)鱼头未超过linel’，但鱼身与linel’夹角α过小。

过小的α角会增加调整姿态的难度，再加上实际硬件和软件的限制，这种情况常常会导致鱼绕着待运送物转圈却难以正向撞击待运送物。

3)鱼身处于hitregion区域(命中区域)内且姿态正直

当鱼身方向m和linel夹角小于β时，鱼可以撞击漂浮物比较准确的按照bc方向向前游动。

基于上述提到的问题，本发明提出长距调姿策略和双鱼交替撞击策略，分别用于调整鱼身到达待撞击点的姿态，提高机器鱼、待运送物、目标三点处于三点一线的概率，以及基于此策略的双鱼交替撞击策略。

鱼每次撞击前，需要调整鱼身姿态。根据前文描述，当鱼身方向m和ab夹角小于β时，鱼撞击待运送物精确的按照预设的轨迹bc到达目标位置。为此很多策略设置一个预撞击点，使鱼在到达此点的过程中能够调整姿态。然而一般系统只能采集到鱼头和鱼中心的坐标信息，且存在误差，鱼尾的坐标是无法获取的，这直接导致鱼身方向与待运送物的夹角无法准确获取，即鱼(a点)相对于b点和c点有多种可能的姿态和距离。如何保证当鱼游至待运送物时，鱼身方向m和ab夹角较小，是一个关键问题。

常见的姿态调整策略设定调姿距离较短(如图4中预设点a’和待运送物位置b)，但是这会导致最终与待运送物发生接触的是鱼身的各个部位，而非鱼头，在撞击部位不是鱼头的情况下，不论调整姿态(因为漂浮物太近已经来不及)还是用鱼身或鱼尾撞击，都不会使待运送物沿bc方向快速行进，而且会因其偏离预定轨迹，使得下一次预设点偏离较大，运送时间增多。

对此，本文提出长距调姿策略。设定距离待运送物位置(b点)较远的a点为预设点，鱼到达预设点a后，再前去撞击。这样，不论鱼身方向(即姿态)是否直对bc方向，都可以在a→b段进行充分的调整，从而避免鱼尾的干扰，提升撞击的精准度。在设置速度方面，预设待撞击点离漂浮物较近时，由于鱼要在其附近转向，只能设置较低的速度值以配合调姿，但长距调姿策略不受速度的束缚，可以以最快的速度游动。

具体实施方式二：本实施方式与具体实施方式一不同的是，步骤s2中选择机器鱼组中能够最快到达点a的鱼为fish1。

其他步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述待撞击点a，具体为以a为圆心，以d3为半径的圆形区域；d3的计算公式为：

在编码时，因为采集坐标的系统并不精确，如果只给出点a的坐标，机器鱼较难达到点a的，容易造成围绕着点a打转的情形。由上文可知，当鱼身方向m和linel夹角小于β时，鱼可以撞击漂浮物比较准确的按照bc方向向前游动；因此，本实施方式以点a为圆形区域，只需要圆形区域处于hitregion区域即可；如图4所示，以da(等于d3)为半径，设为待撞击区域，该待撞击区域处于hitregion区域，当鱼中心坐标进入待撞击区域(圆a)的范围内，即可前往撞击，鱼身方向m和linel夹角肯定小于β；根据大量数据统计，本实施方式将β设为15°；

其他步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式三不同的是，所述d1为2.5～4个鱼身的长度。

其他步骤及参数与具体实施方式三相同。

具体实施方式五：本实施方式与具体实施方式四不同的是，所述d2相比d1大1～2个鱼身的长度。

其他步骤及参数与具体实施方式四相同。

实施例

采用以下实施例验证本发明的有益效果：

本实施例中采用水球为待运送物，采用的仿生机器鱼拥有15个速度档以及15个方向档。速度档值越大，速度则越快。而方向档分为2个部分，0～6档表示左转，7档代表直游，8～14表示右转。

场景如图7所示，让机器鱼(fish1/2)从水池角落出发，水球位于对角线的焦点(b)上，水池一侧放有一条头固定的干扰机器鱼(fish3)，用于制造waterwave(水波)干扰。设定待撞击点a距离b三个鱼身的长度，预备撞击点g距离b三个鱼身的长度；

首先，fish1(fish2)从position1(2)出发，各测试30次，记录机器鱼沿b→c方向准确击中球的次数，如表1。可以看出沿预定方向准确击球的概率较高，并且受鱼出发位置的影响不大。

表1有/无干扰条件下击球测试(测试次数30)

再测试击球速度，将机器鱼的速度挡位设置为15(全速)，分别从position1和position2出发，测试击中球时消耗的时间，如表2所示，每个位置测试30次，取平均值。

表2有无干扰条件下击中时间(秒)

可以看出，在position1时，所走的路程更长，到达点a后需要转的角度也更大(需要调整鱼姿使鱼身朝向b→c方向)，但由于速度一直在最高档而不需要减速(这是近距离调姿策略不具备的)，平均时长能够控制在14秒以内。

在position2时，所走的路程和要转的角度都更小，所以耗时更少。在实际情况中，水中的波动通常会比测试时候的更剧烈。但击球鱼距离球的位置会更接近。本测试具有一定的参考价值。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。