一种基于三维模糊PTP算法的单体机器鱼行为控制策略的制作方法

本发明涉及一种基于三维模糊ptp算法的单体机器鱼行为控制策略，属于智能控制技术领域。

背景技术：

机器人运动规划问题是任何机器人研究者都不会忽略的问题，运动规划对机器人高效运动有重要作用。然而，水下机器鱼运动规划同陆地机器人相比具有特殊的难点：

(1)机器鱼特殊的运动学模型带来的难点，即由于惯性作用，机器鱼在水中无法立刻停止，或者像陆地机器人一样自由的后退。再者，目前仍然未能为机器鱼的运动建立精确的动力学模型，因此发出控制命令后，只能大致地预测机器鱼的反应；

(2)同陆地环境相比，水下环境具有更多的不确定性及干扰。当机器鱼游动时带来的扰动水波，不仅将影响到机器鱼本身及其他机器鱼的运动，而且会影响到传感器的精确测量；

(3)另外，被运输的物体可能会由于水波的冲击发生漂移和转动，这将会给机器鱼的运输运动控制带来很大的困难。因此，大多数陆地机器人的协作算法不能直接应用到机器鱼上，需要提出更新的、抗干扰能力更强的控制算法。

在机器鱼顶球的任务中，要求机器鱼在水池环境中，将半径为r的水球从初始位置移动到目标位置，没有其他辅助装置，机器鱼只允许通过头部或尾鳍顶球。理论上来讲，是运动规划和行为选择问题，不仅涉及到底层的运动控制，而且涉及到上层的规划和控制。因为水中环境会因机器鱼的游动而产生水波，因此采集水中环境信息，即机器鱼、水球、目标点的位置和角度信息，进行图像识别、处理是很重要的。

本专利假设初始情况下水球和机器鱼的位置可以是任意的。现有针对单体机器鱼顶球运动规划问题虽然能够使得机器鱼很快找到水球位置，该行为是游向行为，该算法较为复杂，尤其针对机器鱼转弯策略设计中，较难实现。因此，在运动性能提升方面及算法复杂度方面均存在很大改进空间。

本专利致力于从运动学角度对单体机器鱼顶球运动规划问题进行了改进，采用面向不同“区域”的控制算法，实现机器鱼顶球到达目标点的简单任务。该算法的基本思想是：根据顶球点划分水池区域，设计了对应区域的具体行为。

技术实现要素：

本发明的目的在于克服现有单体机器鱼顶球运动规划算法较为复杂且转弯策略难以实现的技术缺陷，提出了一种基于三维模糊ptp算法的单体机器鱼行为控制策略。

本发明的核心思想为：采用面向不同“区域”的控制算法，实现机器鱼顶球到达目标点的简单任务；具体根据顶球点划分水池区域，设计了机器鱼在对应区域的顶球运动行为。

一种基于三维模糊ptp算法的改进单体机器鱼行为控制策略，包括如下步骤：

步骤1初始化采样时间、划分水池区域以及顶球点位置为主的参数，具体为：

步骤1.1初始化采样时间δt；

步骤1.2将机器鱼进行水下运动所处的水池区域划分为区域1、区域2、区域3和区域4；

其中，区域1为进攻区pr(pushingregion)；

区域2为上边界区ubr(upperboundaryoftheregion)；

区域3为下边界区lbr(lowerboundaryoftheregion)；

区域4为射门区gr(goalregion)；

步骤1.3设定不同区域对应的顶球点坐标；

其中，顶球点，记为point点；对应于步骤1.2的不同水池区域设定的顶球点位置如下：

(1)向下顶球点：对应上边界区ubr，设定的向下顶球点坐标为(b_pt.x,b_pt.y-r)；

(2)向右顶球点：对应进攻区pr，设定的向右顶球点坐标为(b_pt.x-r,b_pt.y)；

(3)向上顶球点：对应下边界区lbr，设定的向上顶球点坐标为(b_pt.x,b_pt.y+r)；

(4)射门顶球点：对应射门区gr，设定的射门顶球点坐标为：

(b_pt.x-rcos(b_to_g_dir),b_pt.y-rsin(b_to_g_dir))；

步骤2更新机器鱼的位置、方向信息及环境信息，计算机器鱼同顶球点的距离差和方向差，并判断机器鱼是否到达目标点，决定是否结束本发明所提策略，具体为：

其中，距离差，记为l；方向差，记为θe；

若(l＜δl)∩(θe＜δθe)，表明机器鱼已经到达目标点，结束本发明所提策略；否则，转步骤3；

其中，δl为距离绝对误差，δθe为方向绝对误差；

步骤3根据步骤2机器鱼的方向差，提取转角，对机器鱼进行方向控制，即决定调用转弯模态还是速度策略，具体为：

步骤3.1获取机器鱼当前的方向差θe，并根据方向差与角度阈值的大小关系，进行如下操作：

其中，角度阈值，记为θf，其取值范围为85到95度，优选的θf为90度；

3.1a获取机器鱼当前的方向差θe，如果|θe|＜θf，转步骤4调用设计的机器鱼速度策略；

3.1b若|θe|≥θf，则转步骤3.2；

步骤3.2调用转弯模态进行大幅度方向改变；

其中，转弯模态，记为mode1，具体为：

3.2a若θe＜-θf，机器鱼进行左急转弯，转角为负60度到负30度，转步骤4；

3.2b若θe＞θf，机器鱼进行右急转弯，转角为30度到60度，转步骤4；

步骤4：根据步骤2中距离差进行速度控制，设计机器鱼的速度策略，具体为：

4.1若距离差满足l＞ld，则设置机器鱼的速度为speed＝15，跳至步骤5；

4.2若距离差满足ls＜l≤ld，则设置机器鱼的速度为speed＝12，跳至步骤5；

4.3若距离差l≤ls，则设置机器鱼的速度为speed＝8，跳至步骤5；

其中，ls为近距离阈值，ld为远距离阈值，且ls＜ld；

步骤5：将机器鱼的转角、速度值和采样时间进行三维模糊化处理，转化为不同的控制模式，通过无线方式发送给机器鱼，转步骤2，具体为：

步骤5.1对机器鱼的转角、速度值和采样时间进行三维模糊化处理，具体为：

5.1a在不同的采样时间下，机器鱼的转角和速度值划分区间不同；

5.1b将转角、速度和采样时间转换为运动模式；

其中，运动模式用(θa，va，ta)表示,θa为角度档位，va为速度档位，ta为采样时间档位；

其中，三维模糊化处理遵循如下规则：在如下条件下对转角、速度、采样时间模糊化处理，转换为同一运动模式：⑴相同的区间，不同的采样时间，⑵相同的速度区间，不同的速度值，⑶相同的转角区间，不同的转角值；而不同的转角区间、速度区间或采样时间区间，对转角、速度或采样时间模糊化处理，转换为不同的运动模式；

步骤5.2规划机器鱼顶球行为，即：针对步骤5.1输出的机器鱼所处区域及其运动模式，确定机器鱼的顶球行为；

其中，顶球行为由一系列动作组成，如实施例4所示；

步骤5.3规划机器鱼的基本动作，即：针对步骤5.2中机器鱼的顶球行为确定机器鱼的机器鱼动作；

其中，机器鱼的机器鱼动作由电机控制；

步骤5.4确定机器鱼的控制参数：将步骤5.3规划的机器鱼的基本动作转换为电机控制参数；

步骤5.5将步骤5.4输出的电机控制参数经无线方式传给机器鱼，控制机器鱼的顶球行为，转步骤2；

至此，经过了步骤1到步骤5，完成了一种基于三维模糊ptp算法的单体机器鱼行为控制策略。

有益效果

一种基于三维模糊ptp算法的单体机器鱼行为控制策略，与现有的单体机器鱼行为控制方法相比，具有如下有益效果：

1.本发明针对不同区域划分设计相应的机器鱼行为控制策略，可适应于多种机器鱼在含有障碍、噪音等多种比较复杂的水下环境中，应用范围广；

2.本发明所提策略中将速度、角度、采样时间，采用三维模糊化处理，并转化为控制模式，便于机器鱼控制模式的直接调用，大大简化了算法流程，降低了算法的复杂度；

3.本发明所提策略中使用的基于三维模糊的ptp算法，该算法流程简单，因此响应速度快，机器鱼灵敏度高，机器鱼的运动性能得到进一步提高；

4.本发明所提策略简化了大量的运算过程，可以设定较高的采样频率，具有数据更新速度快、精确度高以及实时性好的优势。

附图说明

图1是本发明一种基于三维模糊ptp算法的单体机器鱼行为控制策略的步骤1及实施例1中不同水池区域划分及相应区域顶球点设定；

图2是本发明一种基于三维模糊ptp算法的单体机器鱼行为控制策略及实施例2中的总体流程图；

图3是本发明一种基于三维模糊ptp算法的单体机器鱼行为控制策略的实施例3中机器鱼不同区域顶球行为的示意图；

图3(a)bpr行为示意图(有效进攻情况)，图3(b)bpr行为示意图(过冲情况)，图3(c)bubr行为示意图，图3(d)blbr行为示意图，图3(e)bgr行为示意图；

图4是本发明一种基于三维模糊ptp算法的单体机器鱼行为控制策略实施例4中机器鱼顶球动作示意图；

图4(a)向下顶球动作示意图，图4(b)向下顶球动作示意图，图4(c)射门动作示意图

图5是本发明一种基于三维模糊ptp算法的单体机器鱼行为控制策略实施例4中机器鱼过程动作示意图；

图5(a)到达临时返回点后转弯动作示意图，图5(b)尾部波动推进水球动作示意图。

具体实施方式

下面结合附图和实施例对本发明做进一步说明和详细描述。

实施例1

本实施例叙述了在本发明在具体实施时针对不同水池区域划分及相应区域顶球点设定。图1是本发明一种基于三维模糊ptp算法的单体机器鱼行为控制策略的步骤1中不同水池区域划分及相应区域顶球点设定。图1中，oxy为直角坐标系。

从图1中可以看出，水池区域被划分为区域1、区域2、区域3和区域4；每一个区域中都有一个球，球上有个黑点即是顶球点，不同区域中设定的顶球点坐标，区域1中包含向右顶球点，其坐标为(b_pt.x-r,b_pt.y)；区域2中包含向下顶球点，其坐标为(b_pt.x,b_pt.y-r)；区域3中包含向上顶球点，其坐标为(b_pt.x,b_pt.y+r)；区域4中包含射门顶球点，其坐标为(b_pt.x-rcos(b_to_g_dir),b_pt.y-rsin(b_to_g_dir))。其中cos(b_to_g_dir)是球心坐标与目标点之间连线和x轴夹角的余弦值，(sin(b_to_g_dir)是球心坐标与目标点之间连线和x轴夹角的正弦值。

实施例2

本实施例详细阐述了一种基于三维模糊ptp算法的单体机器鱼行为控制策略的总体流程。

图2为本发明一种基于三维模糊ptp算法的单体机器鱼行为控制策略的三维模糊策略下的ptp算法设计流程图。

从图2可以看出，本发明具体实施时包括如下步骤：

步骤a.初始化；

将采样时间初始化为1s；如图1所示，水池区域划分为4个区域；小球坐标为(103cm，47cm)，位于区域1，point点坐标为(101cm，47cm)；

步骤b.信息更新计算；

其中，信息更新参数为l和θe；

步骤c.判断是否|θe|＜δθ，并根据判断结果进行相应操作，具体为：

c.1若|θe|＜δθ，即对应图2中的y1，则跳至步骤e；

c.2若|θe|≥δθ，即对应图2中的n1，则跳至步骤d；

步骤d.判断是否|θe|＜θf，并根据判断结果进行相应操作，具体为：

d.1若|θe|＜δθ，即对应图2中的y2，则调用转向策略，再跳至步骤e；

d.2若|θe|≥δθ，即对应图2中的n2，则调用转弯策略，再跳至步骤e；

步骤e.判断是否|l|＜δl，并根据判断结果进行相应操作，具体为：

d.1若|θe|＜δθ，即对应图2中的y3，则跳至步骤f；

d.2若|θe|≥δθ，即对应图2中的n3，则调用速度控制策略，再跳至步骤f；

步骤f.输出机器鱼的速度、转向，具体为；

根据步骤d、e中的转向、转弯、速度控制策略，输出机器鱼的速度和转向。

步骤g.计算模糊控制模式，具体为；

根据步骤f机器鱼输出的速度、转向以及步骤a中的采样时间，三维模糊化处理转换为模糊控制模式；

步骤h.若(l＜δl)∩(θe＜δθe)结束，否则转步骤b；

实施例3

本实施例详细阐述了在本发明在具体实施时针对不同区域设置相应的机器鱼顶球行为规划策略，具体对应着本发明所提策略的步骤5.1中的机器鱼顶球行为规划。

顶球行为规划的目标是为机器鱼规划适当的运动行为，以引导机器鱼顶球到达目标点位置。对应于各种区域情况，相应的行为设计使得机器鱼在不同区域内选择相应的行为向目标点运动，其具体过程如下：

1)bpr行为：对应为进攻区的行为。有效进攻情况：如图3(a)所示，当机器鱼中心坐标f_centerpt横坐标小于水球坐标b_pt的横坐标时，机器鱼沿最短路径朝向右顶球点游去，到达向右顶球点后，实施向右顶球动作。过冲情况：如图3(b)所示，若机器鱼中心坐标点f_centerpt横坐标在b_pt的横坐标与g_pt横坐标之间时，即机器鱼位于水球和目标点之间的区域，为过冲情况。在这种情况下，机器鱼调整位姿，游向有效进攻区。

2)bubr行为：对应为上边界区的行为。当水球处于上边界区域时，如图3(c)所示，机器鱼游向向下顶球点，实施向下顶球动作。

3)blbr行为：对应为下边界区的行为。同上边界区域行为相似，如图3(d)所示，当水球处于下边界区域时，机器鱼游向向上顶球点，实施向上顶球动作。

4)bgr行为：对应为射门区的行为。如图3(e)所示，当水球处于射门区域时，机器鱼游向射门顶球点。

实施例4

本实施例详细阐述了在本发明中，根据不同区域的行为设计，给出相应的动作规划。在机器鱼顶球到目标点过程中，均是由这一系列动作组成，具体对应着本发明所提策略的步骤5.2中的动作规划。

(1)顶球动作p_action

p_action1：向右顶球动作，当机器鱼处于进攻区时，机器鱼朝向右顶球点运动，实施向右顶球动作。

p_action2：向下顶球动作，这个动作应用于bubr行为中，如图4(a)所示，机器鱼游到向下顶球点，实施向下顶球动作。

p_action3：向上顶球动作，应用于blbr行为中，如图4(b)所示，机器鱼游到向上顶球点，实施向上顶球动作。

p_acition4：射门动作，如图4(c)所示，当水球处于射门区时，采取了切入圆射门算法，使得机器鱼将球顶入球门内。

(2)过程动作action

为了区分机器鱼所处的区域情况，本发明对游向目标行为进行了具体的动作划分：

action1：游向向右顶球点。

action2：游向临时返回点。

action3：到达临时返回点后转弯动作，如图5(a)所示，当机器鱼到达临时返回点后，设计相应的转弯动作，使机器鱼沿着舒适圆路径，朝向右顶球点运动，继而实施向右顶球动作。

action4：游向向上顶球点。

action5：游向向下顶球点。

action6：游向射门顶球点。

action7：顶球后尾巴波动推动水球运动，如图5(b)所示，当机器鱼采取顶球动作后，水球会由于机器鱼顶球受力，而漂离机器鱼，此时机器鱼采取急转弯，并伴随尾部波动，然后调直以速度v向前游动，即使机器鱼尾部没有接触到水球，借助尾部波动搅起的水波，产生力f，也可推动水球向目标点运动。

以上所述为本发明的较佳实施例而已，本发明不应该局限于该实施例和附图所公开的内容。凡是不脱离本发明所公开的精神下完成的等效或修改，都落入本发明保护的范围。