CN101059700A - 一种水下仿生机器人协作运输方法 - Google Patents

Abstract

本发明涉及一种水下仿生机器人的协作运输方法，其包括以下步骤：(1)基于“极限环”原理的位姿控制，(2)基于“舒适圆”法的路径规划，(3)基于模糊推理的同步控制，(4)基于模糊推理的运输方向控制。本发明采用了基于“极限环”方法的位姿控制方法，通过巧妙的控制水下机器人的速度和方向，可以从理论上和实际上都能保证机器人的位姿收敛到目标位姿，从而解决了水下机器人的位姿控制问题。本发明还采用了基于“舒适圆”的路径规划方法，从而降低了复杂水下环境中机器人控制的复杂度，并简化了问题空间的维度。同时本发明采用了基于模糊推理的运动规划方法，从而有效地防止了水下机器人由于惯性的作用对运输对象的撞击，实现了机器人的同步控制及对运输对象方向的平稳控制。

Description

一种水下仿生机器人协作运输方法

技术领域

本发明涉及一种多机器人协作运输方法，特别是关于水下仿生机器人的协作运输方法。

背景技术

随着社会的发展，机器人应用领域正在不断扩展，从自动化生产线到人们的日常生活领域，从海洋资源探索到太空作业领域，机器人可谓无处不在。然而，就目前的机器人技术水平而言，单机器人在信息获取、处理，控制能力、决策应变能力等方面存在着很大的局限性，尤其是在复杂工作任务及动态环境中，单机器人的执行能力更显不足。在对危险性材料的运输、在复杂路面或相对狭小的空间运输作业时，单机器人需要集导航系统、运载系统、通信系统等各种系统于一身。这种高集成的系统开发难度很大，开发成本比较高，开发周期也比较长，更重要的是集成各系统的单机器人体积比较庞大，行动不灵活。单机器人运输系统一旦出现故障就无法完成任务。单机器人运输系统需要针对特定任务来开发，每种新任务都需要重新开发特定的运输机器人，开发的时间比较长，难度比较大。

随着陆地资源日益减少，海洋资源越来越受到人们的重视。海洋资源的探索与水下侦查方面的研究正在如火如荼的展开，各种海洋探测设备的运输安装需要水下机器来完成。但水下运输同陆地运输相比有其特有的难点：1、水中的干扰比较大，所以对水下运输系统控制算法的抗干扰能力提出了更高的要求，陆地机器人的协作算法不能直接应用到水下机器人上，需要提出新的、抗干扰能力更强的控制方法；2、由于水下摩擦力较小，水下机器人的制动能力受到限制，很容易过冲。新的控制方法要专门考虑制动这一环节，使得控制方法设计更加困难；3、现阶段水下机器人运动效率低，能量浪费严重，开发新型高效的水下机器人以适应水下的长时间作业势在必行。

发明内容

针对水下机器人控制的特殊性与复杂性和水动力学模型难以建立、水中干扰大、不确定性因素多等问题，本发明的目的是提供一种全新的在复杂水下环境中的水下仿生机器人协作运输方法。

为实现上述目的，本发明采取以下技术方案：一种水下仿生机器人的协作运输方法，其包括以下步骤：

(1)基于“极限环”原理的位姿控制

以目标点为切点、以目标方向为切线方向作一个切圆，那么即可以确定圆心B(x₀，y₀)的位置。用v表示机器人在原始坐标系下的线速度，用α表示运动的方向，可以得到：

$v=\sqrt{{\stackrel{\·}{\stackrel{\‾}{x}}}^2+{\stackrel{\·}{\stackrel{\‾}{y}}}^2}---\left(1\right)$

$\mathrm{\α}=\arctan \frac{\stackrel{\·}{\stackrel{\‾}{y}}}{\stackrel{\·}{\stackrel{\‾}{x}}}---\left(2\right)$

其中

$\stackrel{\·}{\stackrel{\‾}{x}}=\mathrm{\λ}(\stackrel{\‾}{y}-y_0+\mathrm{\γ}(\stackrel{\‾}{x}-x_0)(r^2{-(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2))---\left(3\right)$

$\stackrel{\·}{\stackrel{\‾}{y}}=\mathrm{\λ}(\stackrel{\‾}{x}-x_0+\mathrm{\γ}(\stackrel{\‾}{y}-y_0)(r^2{-(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2))$

γ，λ均为正的参数，r为切圆的半径，通过(1)(2)(3)式控制机器人的运动速度和方向，即可实现机器人的位姿控制。

(2)基于“舒适圆”法的路径规划

(a)向决策树输入目标位置信息、摄像头采集到的视觉信息、包括机器人和运输物的位置和方向信息等；

(b)通过决策树生成一组相互排斥、并且完整描述了问题空间的“情况”；

(c)相应于各种“情况”，设计机器人的行为，使其在最小转弯半径的限制下，沿一条合适的路径向目标位姿运动；

(d)根据“舒适圆”法规划从机器人位置到目标位置的路径，并根据代价评估函数分配各机器人在协作运输中的角色；

(3)基于模糊推理的同步控制

将WL(i)、WR(j)(分别为机器人i和机器人j分配左角色和右角色的代价)表示为模糊集{L，M，S}，分别代表大，中，小；将机器人的速度VL、VR表示为模糊集{F，M，S}，分别代表快速、中速和慢速，设计以下模糊规则：

1)如果WL(i)为L且WR(j)为L，那么VL为F，VR为F；

2)如果WL(i)为L且WR(j)为M，那么VL为F，VR为M；

3)如果WL(i)为L且WR(j)为S，那么VL为F，VR为S；

4)如果WL(i)为M且WR(j)为L，那么VL为M，VR为F；

5)如果WL(i)为M且WR(j)为M，那么VL为F，VR为F；

6)如果WL(i)为M且WR(j)为S，那么VL为F，VR为S；

7)如果WL(i)为S且WR(j)为L，那么VL为S，VR为F；

8)如果WL(i)为S且WR(j)为M，那么VL为S，VR为F；

9)如果WL(i)为S且WR(j)为S，那么VL为M，VR为M，

使用Mamdani类型的推理方式，机器人的最终速度由“重心法”去模糊得到，表达式如下：

$\stackrel{\‾}{\mathrm{VL}}={\mathrm{\Σ}}_{k=1}^9{\mathrm{VL}}_k{x}_k/{\mathrm{\Σ}}_{k=1}^9{x}_k,\stackrel{\‾}{\mathrm{VR}}={\mathrm{\Σ}}_{k=1}^9{\mathrm{VR}}_k{x}_k/{\mathrm{\Σ}}_{k=1}^9{x}_k---\left(4\right)$

        x_k＝min{x_1k1，x_2k2}                    (5)

在(4)(5)式中，，x_k为第k条规则“如果”部分的联立程度(k＝1，…，9)，x_1k1(相应地，x_2k2)为WL(i)(相应地，WR(j))对第k条规则的隶属度，VL_k与VR_k为从第k条规则得到的输出。

(4)基于模糊推理的运输方向控制

将箱子方向相对于目标方向的角度θ用模糊集{PB，PM，PS，Z，NS，NM，NB}表示，分别代表正大，正中，正小，零，负小，负中，负大；将机器人的速度VL、VR表示为模糊集{F，M，S}，分别代表快速、中速和慢速，设计以下模糊推理规则：

1)如果θ为PB，那么VL为F，VR为S；

2)如果θ为PM，那么VL为M，VR为S；

3)如果θ为PS，那么VL为M，VR为S；

4)如果θ为Z，那么VL为M，VR为M；

5)如果θ为NS，那么VL为S，VR为M；

6)如果θ为NM，那么VL为S，VR为M；

7)如果θ为NB，那么VL为S，VR为F，

采用Mamdani类型的推理方式，机器人的最终速度同样由“重心法”去模糊得到。

采用以下代价函数F(A)来评价角色分配：

F(A)＝|WL(i)-WR(i)|+k(WL(i)+WR(j))，

WL(i)＝len(LP，P_i)+C×NL_obj，

WR(j)＝len(RP，P_j)+C×NR_obj。

其中P_i和P_j分别表示机器人i和j当前的位置，LP和RP分别表示左目标点和右目标点，len(LP，P_i)(相应地，len(RP，P_j))表示根据“舒适圆”法规划的从P_j点(相应地，P_j点)到LP点(相应地，RP点)的路径长度。如果从P_i到LP(相应地，从P_j到RP)规划的路径上存在障碍，NL_obj(相应地，NR_obj)为1；否则，为0；WL(i)和WR(j)分别为机器人i和机器人j分配为左角色和右角色的代价。

本发明由于采取以上技术方案，其具有以下优点：1、由于采用了基于“极限环”方法的位姿控制方法，通过巧妙的控制水下机器人的速度和方向，可以从理论上和实际上都能保证机器人的位姿收敛到目标位姿，从而解决了水下机器人的位姿控制问题。2、由于采用了基于“舒适圆”的路径规划方法，从而降低了复杂水下环境中机器人控制的复杂度，并简化了问题空间的维度。另外还考虑到水下机器人的机动性，因此根据此方法规划的路径在任意速度下都是可行的。3、由于采用了基于模糊推理的运动规划方法，从而有效地解除了在不确定因素多、干扰大的环境下水下机器人精确控制的难点，防止了水下机器人由于惯性的作用对运输对象的撞击，从而实现了对运输对象方向的平稳控制。

附图说明

图1是快速收敛的极限环示意图

图2是慢速收敛的极限环示意图

图3是机器鱼位姿控制示意图

图4决策树示意图

图5是根据“情况-行为”方法得到的SYN情况及其对应的行为示意图

图6是根据“情况-行为”方法得到的NFAD情况及其对应的行为示意图

图7是根据“情况-行为”方法得到的NSYN情况及其对应的行为示意图

图8是根据“情况-行为”方法得到的FP情况及其对应的行为示意图

图9是根据“情况-行为”方法得到的NFP情况及其对应的行为示意图

图10是模糊逻辑控制器示意图

图11是模糊控制中WL，WR的隶属函数示意图

图12是模糊控制中VL，VR的隶属函数示意图

图13是模糊控制中物体方向相对于目标方向夹角的隶属函数示意图

图14是多机器鱼协作平台示意图

图15是多机器鱼协作运输过程示意图

具体实施方式

下面结合附图和实施例，对本发明进行详细的说明。

本发明将协作运输问题划分成四个子问题来解决：1、位姿控制；2、路径规划；3、同步控制；4、运输方向控制。下面以仿生机器鱼作为例进行说明。

1、基于“极限环”方法的位姿控制

在二阶非线性自治系统中，有一类重要的由孤立闭轨线表述的运动，不仅对微分方程理论本身，而且在工程技术应用上也起着很重要的作用，这类孤立的闭轨称为“极限环”，任意位置出发的点都收敛到“极限环”，并沿其做周期性的运动。由于采用经典的控制方法不能保证水下机器人以正确的位姿到达目标点，而启发式的方法无法从理论上保证精确的位姿控制，因此本发明将“极限环”理论应用到机器鱼的位姿控制中，通过巧妙的控制水下机器人的速度和方向，可以使机器人位置收敛到极限环并沿着极限环运动，从而成功地解决了水下机器人的位姿控制问题。

以( x， y)，(x₀，y₀)分别表示点A和点B在二维坐标系下的平面坐标，考虑以下非线性系统：

$\stackrel{\·}{\stackrel{\‾}{x}}=\mathrm{\λ}(\stackrel{\‾}{y}-y_0+\mathrm{\γ}(\stackrel{\‾}{x}-x_0)(r^2{-(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2)),---\left(1\right)$

$\stackrel{\·}{\stackrel{\‾}{y}}=\mathrm{\λ}(-\stackrel{\‾}{x}+x_0+\mathrm{\γ}(\stackrel{\‾}{y}-y_0)(r^2{-(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2)),$

其中γ，λ，r均为正的参数。我们可以给出以下定理：

定理1：

如果在一个非线性系统中，点A( x， y)的运动可以用(1)式来描述，那么此系统有一个以B(x₀，y₀)为圆心、形式为( x-x₀)²+( y-y₀)²＝r²的圆形“极限环”，从任意位置出发的点都收敛到此“极限环”，并绕其做周期性的运动。

在方程(1)中，点A( x， y)向极限环的收敛速度可以通过参数γ来调节，如图1、图2所示，给出了不同参数γ下的极限环。图1是快速收敛的情况(γ＝0.001)，图2为慢速收敛的情况(γ＝0.0003)。

机器鱼采取方程(1)描述的运动方式，其位置必然会收敛到一个圆形的“极限环”并沿其运动，而机器鱼的目标位姿可以通过机器鱼到达目标点时的“极限环”的切线方向来实现。机器鱼的位姿控制方法如下：以目标点为切点、以目标方向为切线方向作一个切圆，那么即可以确定圆心B(x₀，y₀)的位置。通过“极限环”方法，机器鱼可以实现指定的目标位姿，即目标点为切点、目标方向为切圆经过目标点的切线方向(如图3所示)。为了最终实现机器鱼的控制，本发明将方程(1)表述的运动控制转化为机器鱼的速度和方向控制。用v表示机器鱼在原始坐标系下的线速度，用α表示运动的方向，可以得到：

$v=\sqrt{{\stackrel{\·}{\stackrel{\‾}{x}}}^2+{\stackrel{\·}{\stackrel{\‾}{y}}}^2}=\mathrm{\λ}\sqrt{{\tilde{x}}^2+{\tilde{y}}^2},---\left(2\right)$

$\mathrm{\α}=\arctan \frac{\stackrel{\·}{\stackrel{\‾}{y}}}{\stackrel{\·}{\stackrel{\‾}{x}}}=\frac{\tilde{y}}{\tilde{x}}.$

在(2)，(3)式中，

$\tilde{x}=\stackrel{\‾}{y}-y_0+\mathrm{\γ}(\stackrel{\‾}{x}-x_0)(r^2{-(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2),$

$\tilde{y}=-\stackrel{\‾}{x}+x_0+\mathrm{\γ}(\stackrel{\‾}{y}-y_0)(r^2{-(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2)---\left(3\right)$

从(2)式可知，调节参数λ的值，就可以任意调节v的大小。机器鱼采取(2)，(3)式表述的运动方法即可以实现其位姿控制。

2、基于“舒适圆”法的路径规划

为了降低水下机器人控制的复杂度，并尽可能简化问题空间的维度，本发明采用了一种新颖的基于“舒适圆”的“情况-行为”法解决路径规划问题。

考虑到机器鱼精确运动控制的难度，本发明用“情况-行为”的设计方法来降低协作运输中路径规划的复杂度。“情况-行为”法是基于定义一组“情况”的设计方法，这些“情况”描述了问题实体之间的相对状态。这种设计方法满足以下条件：a)“情况”之间相互排斥、并且完整地表示了问题实体之间的相对状态；且定义的“情况”空间不能过大；b)每一种“情况”对应的行为能够独立解决相关的问题。使用“情况-行为”方法的优势在于：其本身是一种“分割”的策略，因此降低了任务的复杂度；另外，不需考虑行为融合的问题，因为“情况”构成了完整的状态空间，并相互排斥。下面给出“情况-行为”方法的几何实现-“舒适圆法”，为叙述方便，本发明用一个箱子作为要运输的物体，并使用两个机器鱼来完成运输任务。

1)“情况”划分

“情况”根据问题实体之间的相对状态得到，问题实体包括：机器鱼、箱子、推箱点和目标点。环境被划分为箱子区域、推箱区、左禁域、右禁域和外域。推箱区域是以推箱点为圆心、以1/4箱子长度为半径的半圆形区域，在这些区域中，机器鱼可以直接用头部推动箱子。禁域的边界是经过推箱点的、以1.2倍机器鱼最小转弯半径为半径的有向圆，我们称此半径为“舒适”半径，表示机器鱼以此半径可以舒适的转弯，以“舒适”半径为半径的有向圆称为舒适圆。这里我们只讨论对应于左推箱点和以左推箱点为目标的机器鱼的情况。对应于右推箱点和以右推箱点为目标的机器鱼的情况是类似的。

本发明的目标是为机器鱼规划适当的路径，以引导机器鱼到达推箱点时与箱子同向，为下一步的协作推箱作准备。考虑到机器鱼在收到“停止”命令后不能立刻停止，运动规划应当使两条机器鱼同步到达推箱点。本发明根据问题实体的相互关系，使用“决策树”来定义“情况”(如图4所示)，“决策树”输入的是目标位置信息、摄像头采集到的视觉信息、包括机器鱼和箱子的位置和方向信息。当前的“情况”根据输入信息进行识别，“决策树”根据二元决策规则，由以下四个判据生成。

判据1：推箱区域判据。此判据根据机器鱼与推箱区域的相对关系，将“情况”划分为以下两类：

IAR情况：机器鱼在推箱区域内部。

NIAR情况：机器鱼在推箱区域外部。

判据2：可行推箱方向判据。此判据根据机器鱼与箱子的方向，将IAR情况划分为以下两种类型：

FAD情况：机器鱼在推箱区域内部，方向与箱子方向一致(在实验中，我们以方向差在45°范围内为一致)，并且与箱子边界相交(如图5所示)。

NFAD情况：机器鱼在推箱区域内部，但方向与箱子方向不一致(如图6所示)。

判据3：同步判据。此判据根据另外一条鱼是否也处于FAD情况，将FAD情况划分成以下两种情况：

SYN情况：另一条鱼也处于FAD情况(如图5所示)，因此在这种情况下，两条鱼可以同步地推箱。

NSYN情况：另一条鱼不处于FAD情况(如图7所示)。

判据4：可行路径判据。首先我们给出以下定义：

可行舒适圆：以P(x，y)表示机器鱼当前的位置，φ表示方向，Rc表示舒适半径，CC表示舒适圆，dir(x)表示x的方向，bl，br分别表示左禁域和右禁域的边界，可行舒适圆定义为：

若存在一个CC，以方向φ经过P(x，y)，CC和bl(或br)之间存在一条公切线(ptan)，且dir(ptan)与dir(CC)、dir(bl)(或dir(br))一致，那么此CC称为与机器鱼当前位姿相关的可行舒适圆。

自由路径：一条不被障碍阻拦的路径称为自由路径。

半可行路径：一条从机器鱼的当前位置到左推箱点的路径，由可行舒适圆的一段弧、禁域边界的一段弧和其有向切线组成(如图8所示)。

可行路径：可行路径为一条自由的半可行路径。

NIAR情况根据可行路径划分为以下两种情况：

FP情况：从机器鱼的当前位置到左推箱点存在至少一条可行路径。

NFP情况：从机器鱼当前位置到左推箱点不存在可行路径(如图9所示)。

决策树的叶节点为：SYN情况，NSYN情况，NFAD情况，FP情况，NFP情况。由于这五种情况通过二元决策树得到，因此它们是完整并相互排斥的。

2)相应的行为设计

相应于各种“情况”的行为的设计应能使机器鱼在最小转弯半径的限制下，沿一条合适的路径向目标位姿(目标点为左推箱点，目标方向为箱子的方向)运动。

B_SYN行为：由于已经同步成功，两条机器鱼协作推箱，同时调整方向以与箱子方向保持一致(如图5所示)。

B_NSYN行为：NSYN情况应当避免，一旦出现这种情况，已经就位的机器鱼将停下来，等待与另一条机器鱼同步(如图7所示)。

B_NFAD行为：在NFAD情况下，虽然机器鱼已在推箱区域，但方向不可行，因此不能开始推箱，因此，机器鱼向推箱区域外移动，直到发现一条可行路径(如图6所示)。

B_FP行为：机器鱼沿最短可行路径朝左推箱点移动(如图8所示)。

B_NFP行为：机器鱼沿最短半可行路径朝左推箱点移动，当接近障碍物时启动避障行为(如图9所示)。

3)角色分配机制

在协作运输任务中，定义两个角色：左角色和右角色。分配了左角色的机器鱼要求在箱子的左侧推箱，相应地，分配了右角色的机器鱼要求在箱子的右侧推箱。以len(A，B)表示根据“舒适圆”法规划的从B点到A点的路径长度。本发明用以下的代价函数来评价角色分配：

F(A)＝|WL(i)-WR(i)|+k(WL(i)+WR(j))，

WL(i)＝len(LP，P_i)+C×NL_obj，

WR(j)＝len(RP，P_j)+C×NR_obj，

其中，LP和RP分别表示左推箱点和右推箱点；P_i和P_j分别表示鱼i和j当前的位置，如果从P_i到LP(相应地，从P_j到RP)规划的路径上存在障碍，NL_obj(相应地，NR_obj)为1；否则，为0；WL(i)和WR(j)分别为鱼i和鱼j分配左角色和右角色的代价，代表机器鱼移向推箱点的大致的时间损耗。优化的任务分配即为最小化F(A)，其前部分为使两条机器鱼同步到达推箱点，后部分为了使总代价最小化。k为一个正常数，可以调节F(A)前后两部分的比重(在实验中，k取0.3)；C为一个常数，表示规划路径上障碍物的影响(在实验中，C取50)。

3、基于模糊推理的同步控制

为了实现在不确定因素多、干扰大的环境下成功完成协作任务，本发明采用模糊控制法以解决同步控制问题。基于规则的模糊逻辑方法可以用来对不确定和不精确信息进行推理与决策。此方法是通过定义一组模糊变量，使用隶属函数的概念进行逻辑推理，最终通过去模糊法得到对象的输出。在协作运输任务中，为了避免机器鱼由于惯性的作用对运输对象的撞击，我们希望控制多条机器鱼能够同步到达运输对象。考虑到机器鱼精确控制的难度，为了控制机器鱼同步到达推箱点，并将箱子成功移向目标位置，我们使用模糊逻辑控制的方法来实现机器鱼的同步控制。

同步过程中的运动规划：本发明使用两条机器鱼，为使两条机器鱼同步到达推箱点，我们设计一个模糊控制器(如图10所示)，以控制机器鱼移向推箱点的速度。控制器的输入为WL(i)和WR(j)，表示机器鱼移向目标点近似的时间开销；输出为机器鱼的速度，用VL和VR分别表示左角色机器鱼和右角色机器鱼的速度。首先WL(i)和WR(j)表示为模糊集{L，M，S}，分别代表大，中，小，其隶属函数(如图11所示)。VL和VR表示为模糊集{F，M，S}，分别代表快速、中速和慢速，其隶属函数(如图12所示)。VL和VR由以下模糊规则得到：

1)如果WL(i)为L且WR(j)为L，那么VL为F，VR为F；

2)如果WL(i)为L且WR(j)为M，那么VL为F，VR为M；

3)如果WL(i)为L且WR(j)为S，那么VL为F，VR为S；

4)如果WL(i)为M且WR(j)为L，那么VL为M，VR为F；

5)如果WL(i)为M且WR(j)为M，那么VL为F，VR为F；

6)如果WL(i)为M且WR(j)为S，那么VL为F，VR为S；

7)如果WL(i)为S且WR(j)为L，那么VL为S，VR为F；

8)如果WL(i)为S且WR(j)为M，那么VL为S，VR为F；

9)如果WL(i)为S且WR(j)为S，那么VL为M，VR为M。

我们使用Mamdani类型的推理方式，机器鱼的最终速度由“重心法”去模糊得到，即取模糊隶属函数曲线与横坐标轴围成面积的重心所对应的速度作为最终的输出速度，表达式如下：

$\stackrel{\‾}{\mathrm{VL}}={\mathrm{\Σ}}_{k=1}^9{\mathrm{VL}}_k{x}_k/{\mathrm{\Σ}}_{k=1}^9{x}_k,\stackrel{\‾}{\mathrm{VR}}={\mathrm{\Σ}}_{k=1}^9{\mathrm{VR}}_k{x}_k/{\mathrm{\Σ}}_{k=1}^9{x}_k---\left(4\right)$

    x_k＝min{x_1k1，x_2k2}                    (5)

在(4)(5)式中，x_k为第k条规则“如果”部分的联立程度(k＝1，…，9)，x_1k1(相应地，x_2k2)为WL(i)(相应地，WR(j))对第k条规则的隶属度，VL_k与VR_k为从第k条规则得到的输出。

4、基于模糊推理的运输方向控制

为了实现在不确定因素多、干扰大的环境下成功完成协作任务，我们采用模糊控制法以解决运输方向控制问题。当两条机器鱼均处于SYN情况时，接下来的任务是相互协作，用头部推动箱子向目标点移动。以θ表示箱子方向相对于目标方向的角度。箱子的方向由两条鱼作用在其上的作用力来控制，通过控制两条鱼的游动速度来实现。类似的，机器鱼的速度通过一组模糊逻辑规则得到。这里，模糊规则的输入为θ，VL，VR为输出。我们将θ用模糊集{PB，PM，PS，Z，NS，NM，NB}表示，分别代表正大，正中，正小，零，负小，负中，负大。θ的隶属函数(如图13所示)。VL和VR通过以下推理得到：

1)如果θ为PB，那么VL为F，VR为S；

2)如果θ为PM，那么VL为M，VR为S；

3)如果θ为PS，那么VL为M，VR为S；

4)如果θ为Z，那么VL为M，VR为M；

5)如果θ为NS，那么VL为S，VR为M；

6)如果θ为NM，那么VL为S，VR为M；

7)如果θ为NB，那么VL为S，VR为F。

类似地，VL和VR同样由“重心法”去模糊得到。

如图14所示，本发明协作运输系统的实验平台由决策层、信息交换层和执行层组成，决策层由一台主机组成，信息交换层由传感器和传输设备组成，执行层由机器鱼组成。

本发明方法以使用两条机器鱼为例，其具体步骤如下：

1、根据摄像头获取的环境信息和机器鱼反馈的状态信息产生控制命令；

2、通过信息交换层，由决策层产生的控制命令发送给执行层，并且环境信息和机器鱼的任务执行状态信息反馈给决策层；

3、执行层接收并执行从决策层发送的控制命令。在协作运输任务中，两条机器鱼的“舒适”半径为33cm。

4、根据本发明的内容，左角色分配给图15(a)中位于箱子左侧的机器鱼1，右角色分配给位于箱子右侧的机器鱼2。

5、机器鱼沿根据“舒适圆”法规划的路径向目标位置移动，如图15(b)所示。

6、根据本发明中的模糊逻辑规则，两条机器鱼以不同的速度移动，以实现同步。如图15(c)所示，在21.0秒时，两条鱼已同步成功，并且到达推箱点，开始推动物体朝目标点运动。

7、为控制物体的方向，两条鱼通过本发明中的模糊控制方法协调游动速度，以调整作用在物体上的作用力(如图15(d、e)所示)，在37.0秒时，物体已经被成功移动到目标点(如图15(f)所示)。

上述仅是为说明本发明而列举的实施例，在本发明基本构思的基础上可以进行的各种替换、变化和修改，这些替换、变化和修改不应排除在本发明的保护范围之外。

Claims (2)

1、一种水下仿生机器人的协作运输方法，其包括以下步骤：

(1)基于“极限环”原理的位姿控制

以目标点为切点、以目标方向为切线方向作一个切圆，那么即可以确定圆心B(x₀，y₀)的位置。用v表示机器人在原始坐标系下的线速度，用α表示运动的方向，可以得到：

$v=\sqrt{{\stackrel{\·}{\stackrel{\‾}{x}}}^2+{\stackrel{\·}{\stackrel{\‾}{y}}}^2}...\left(1\right)$

$\mathrm{\α}=\arctan \frac{\stackrel{\·}{\stackrel{\‾}{y}}}{\stackrel{\·}{\stackrel{\‾}{x}}}...\left(2\right)$

其中

$\stackrel{\·}{\stackrel{\‾}{x}}=\mathrm{\λ}(\stackrel{\‾}{y}-y_0+\mathrm{\γ}(\stackrel{\‾}{x}-x_0)(r^2-{(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2))$

$\stackrel{\·}{\stackrel{\‾}{y}}=\mathrm{\λ}(-\stackrel{\‾}{x}+x_0+\mathrm{\γ}(\stackrel{\‾}{y}-y_0)(r^2-{(\stackrel{\‾}{x}-x_0)}^2-{(\stackrel{\‾}{y}-y_0)}^2))$ (3)

γ，λ均为正的参数，r为切圆的半径，通过(1)(2)(3)式控制机器人的运动速度和方向，即可实现机器人的位姿控制。

(2)基于“舒适圆”法的路径规划

(a)向决策树输入目标位置信息、摄像头采集到的视觉信息、包括机器人和运输物的位置和方向信息等；

(b)通过决策树生成一组相互排斥、并且完整描述了问题空间的“情况”；

(c)相应于各种“情况”，设计机器人的行为，使其在最小转弯半径的限制下，沿一条合适的路径向目标位姿运动；

(d)根据“舒适圆”法规划从机器人位置到目标位置的路径，并根据代价评估函数分配各机器人在协作运输中的角色；

(3)基于模糊推理的同步控制

将WL(i)、WR(j)(分别为机器人i和机器人j分配左角色和右角色的代价)表示为模糊集{L，M，S}，分别代表大，中，小；将机器人的速度VL、VR表示为模糊集{F，M，S}，分别代表快速、中速和慢速，设计以下模糊规则：

1)如果WL(i)为L且WR(j)为L，那么VL为F，VR为F；

2)如果WL(i)为L且WR(j)为M，那么VL为F，VR为M；

3)如果WL(i)为L且WR(j)为S，那么VL为F，VR为S；

4)如果WL(i)为M且WR(j)为L，那么VL为M，VR为F；

5)如果WL(i)为M且WR(j)为M，那么VL为F，VR为F；

6)如果WL(i)为M且WR(j)为S，那么VL为F，VR为S；

7)如果WL(i)为S且WR(j)为L，那么VL为S，VR为F；

8)如果WL(i)为S且WR(j)为M，那么VL为S，VR为F；

9)如果WL(i)为S且WR(j)为S，那么VL为M，VR为M，

 使用Mamdani类型的推理方式，机器人的最终速度由“重心法”去模糊得到，表达式如下：

$\stackrel{\‾}{\mathrm{VL}}={\mathrm{\Σ}}_{k=1}^{9}V{L}_k{x}_k/{\mathrm{\Σ}}_{k=1}^9{x}_k,$ $\stackrel{\‾}{\mathrm{VR}}={\mathrm{\Σ}}_{k=1}^{9}V{R}_k{x}_k/{\mathrm{\Σ}}_{k=1}^9{x}_k...\left(4\right)$

     x_k＝min{x_1k1，x_2k2}                                       (5)

在(4)(5)式中，，x_k为第k条规则“如果”部分的联立程度(k＝1，…，9)，x_1k1(相应地，x_2k2)为WL(i)(相应地，WR(j))对第k条规则的隶属度，VL_k与VR_k为从第k条规则得到的输出。

(4)基于模糊推理的运输方向控制

将箱子方向相对于目标方向的角度θ用模糊集{PB，PM，PS，Z，NS，NM，NB}表示，分别代表正大，正中，正小，零，负小，负中，负大；将机器人的速度VL、VR表示为模糊集{F，M，S}，分别代表快速、中速和慢速，设计以下模糊推理规则：

1)如果θ为PB，那么VL为F，VR为S；

2)如果θ为PM，那么VL为M，VR为S；

3)如果θ为PS，那么VL为M，VR为S；

4)如果θ为Z，那么VL为M，VR为M；

5)如果θ为NS，那么VL为S，VR为M；

6)如果θ为NM，那么VL为S，VR为M；

7)如果θ为NB，那么VL为S，VR为F，

采用Mamdani类型的推理方式，机器人的最终速度同样由“重心法”去模糊得到。

2、如权利要求1所述的一种水下仿生机器人的协作运输方法，其特征在于：采用以下代价函数F(A)来评价角色分配：

F(A)＝|WL(i)-WR(i)|+k(WL(i)+WR(j))，

WL(i)＝len(LP，P_i)+C×NL_obj，

WR(j)＝len(RP，P_j)+C×NR_obj

其中P_i和P_j分别表示机器人i和j当前的位置，LP和RP分别表示左目标点和右目标点，len(LP，P_i)(相应地，len(RP，P_j))表示根据“舒适圆”法规划的从P_i点(相应地，P_j点)到LP点(相应地，RP点)的路径长度。如果从P_i到LP(相应地，从P_j到RP)规划的路径上存在障碍，NL_obj(相应地，NR_obj)为1；否则，为0；WL(i)和WR(j)分别为机器人i和机器人j分配为左角色和右角色的代价。

Images