CN101825901B - 基于人工物理法的多智能体机器人协同控制方法 - Google Patents

Abstract

本发明是一种基于人工物理法的多智能体机器人协同控制方法。该方法基于多个智能体机器人以分布式的方式进行，每个智能体机器人上有一个或多个的传感器，用来采集、融合分布式数据，并使用信息处理结果驱动智能体机器人。概述分为下述步骤：(1)基于人工物理法，建立智能体机器人间相互作用的矢量力场；(2)通过解算智能体机器人合力，确定智能体机器人的运动速度和方向，并通过四点平滑的方法抑制模态转换的瞬态；(3)基于模糊控制的沿墙跟踪方法，实现智能体机器人的避障。

Description

基于人工物理法的多智能体机器人协同控制方法

技术领域

本发明涉及一种基于人工物理法的多智能体机器人协同控制方法，属于机器人控制技术领域。

背景技术

机器人技术作为20世纪人类最伟大的发明之一。随着机器人的广泛应用以及其任务复杂度的提高，机器人系统也由单一机器人向多机器人发展。多机器人系统描述为一些机器人在同样的环境下协作完成任务的系统。多机器人系统主要有以下特点：

(1)多机器人可以完成复杂的任务；

(2)多机器人之间可以通过通讯来协作完成任务；

(3)多机器人可以大大提高系统的效率，增加系统的性能和可靠性；

(4)多机器人具有分布式的特点；

(5)设计多个简单的机器人比设计一个复杂机器人成本低，而且更容易。

目前，机器人协同控制处于仿真研究阶段，大多采用人工势场、A*等方法，而没有建立多机器人运动体的编队控制，也没有使多机器人运动体保持队形的方法。

本发明所提出的人工物理法的创新性在于通过多个智能体机器人内聚扩展建立队形，基于人工物理方法保持队形，采用模糊控制的沿墙跟踪避障，实现了多智能体机器人的协同控制。

发明内容

本发明的目的在于：提出一种简捷、高效、输出稳定性、鲁棒性好的基于人工物理法的多智能体机器人协同控制方法，该方法适用于大量的智能体机器人和智能体机器人集的运动控制。

下面将本发明的技术方案，详述如下：

本发明是一种基于人工物理法的多智能体机器人协同控制方法。该方法基于多个智能体机器人以分布式的方式进行，每个智能体机器人上有一个或多个的传感器，用来采集、融合分布式数据，并使用信息处理结果驱动智能体机器人。概述分为下述步骤：(1)基于人工物理法，建立智能体机器人间相互作用的矢量力场；(2)通过解算智能体机器人合力，确定智能体机器人的运动速度和方向，并通过四点平滑的方法抑制模态转换的瞬态；(3)基于模糊控制的沿墙跟踪方法，实现智能体机器人的避障。

本发明一种基于人工物理法的多智能体机器人协同控制方法，可分为下述3个步骤：

步骤(1)：基于人工物理法，建立智能体机器人间相互作用的矢量力场；

在人工物理法(AP)的结构中，虚的物理力驱动一个智能体机器人群体系统以达到期望的状态。该状态是一个使全系统潜在能量最小化的形状，而系统表现为分子动力学特性其中，F代表力，m代表质量，a代表加速度。

定义力规则：F＝Gm_im_j/r^p。其中F是智能体机器人i和智能体机器人j之间力的幅值，m_i、m_j为第i、j个智能体的质量，r是两智能体机器人间距离，G为代表聚类特性的参数，p为一个可调参数，p越大则F受r变化的影响越大。R为阈值距离，如果r≤R，力是斥力，如果r＞R，力是引力。每个智能体机器人都有一个用于检测距离和其它智能体机器人方位的传感器。受动器使智能体机器人能够以速度

移动。为了确保力规则是局部的，智能体机器人的可视范围是1.5R。智能体机器人不需要具有相同的质量。最大力参数F_max为智能体机器人所能达到的加速度提供了一个必要的约束。从步骤(1)可以得出每个智能体上所受的力和力的方向，这些变量作为步骤(2)的输入。

步骤(2)：通过解算智能体机器人合力，确定智能体机器人的运动速度和方向，并通过四点平滑的方法抑制模态转换的瞬态；

每一个智能体机器人具有位置

和速度我们使用离散时间近似智能体机器人的连续行为，时间步长为Δt。在每一个时间步长内，每一个智能体机器人的位置变化该变化取决于当前速度，

在每一个时间步长内，每一个智能体机器人的速度也发生改变

速度的改变量受施加在智能体机器人上的力的控制，

其中m是智能体机器人的质量，

是智能体机器人上受到的力。

每个智能体机器人所具有的质量m使智能体机器人具有动量。智能体机器人不需要具有相同的质量。最大速度参数V_max限制了智能体机器人的最大速度。

人工物理法的流程如图1所示。其中，设智能体机器人群中智能体机器人个数为N，m_i的默认值取1.0，p默认值取2，重力常数G在初始化中被设定，ΔT为离散的步长。

对任何一个智能体机器人i在每个离散时间段的人工物理法步骤流程如下：

步骤2.1：首先智能体机器人i从传感器获取与其它智能体机器人间的夹角θ(θ为-180°到180°中的值)和距离值r；

步骤2.2：比较后，得出与智能体机器人i距离最近的智能体机器人j，智能体机器人i与智能体机器人j夹角为θ₀(θ₀为-180°到180°中的值)和距离值为r；

步骤2.3：依据下式确定F，θ，F为智能体机器人i所受力的大小，θ为智能体机器人i所受力的方向：

$F_0=\left\{\begin{array}{cc}G/(r*r)& r\≤1.5R\\ 0& r>1.5R\end{array}\right.$

$F=\left\{\begin{array}{cc}{F}_0& F_0\≤F_{\max }\\ F_{\max }& F_0>F_{\max }\end{array}\right.$

步骤2.4：计算F在x轴与y轴上的分量，得到SF_X与SF_Y；

SF_X＝F×Cosθ

SF_Y＝F×Sinθ

步骤2.5：根据上述计算得到的SF_X与SF_Y，计算时间步长内的速度增量ΔV_x与ΔV_Y；

ΔV_x＝SF_X*ΔT/m_i；

ΔV_y＝SF_Y*ΔT/m_i；

步骤2.6：计算时间步长后x轴和y轴上的速度分量V_x与V_y。V_1x和V_1y为上一步x轴和y轴上的速度分量。

V_x＝V_1x+ΔV_x；

V_y＝V_1y+ΔV_y；

步骤2.7：依据下述公式对V_x，V_y进行限幅：

步骤2.8：使用四点平滑方法计算智能体机器人i的期望移动速度和期望转动角度，即：

V_fx＝(V_1x+V_2x+V_3x+V_x)/4

V_fy＝(V_1y+V_2y+V_3y+V_y)/4

{V_1x，V_1y}、{V_2x，V_2y}、{V_3x，V_3y}分别为前一步、前两步和前三步的速度值。

依据V_fx，V_fy计算智能体机器人i期望转动角度θ’：

步骤2.9：依据智能体机器人i期望移动速度V_fx、V_fy和期望转动角度θ’，控制智能体机器人i运动。

步骤2.10：在智能体机器人i运动过程中不断迭代进行上述过程，即步骤2.1至步骤2.9。

步骤(3)：基于模糊控制的沿墙跟踪方法，实现智能体机器人的避障；

基于模糊控制的沿墙跟踪方法需要设计一个双输入单输出的模糊控制器，依据智能体机器人与墙壁的距离和相对于墙壁的角度判断智能体机器人的转向，使智能体机器人与墙壁保持一定的距离运动。

模糊控制器的结构如图2所示，模糊控制器主要由模糊化、知识库、模糊推理、清晰化四部分组成。设智能体机器人与墙壁的距离的模糊变量语言集合为{很近，近，中，远，很远}，其相应的语言变量分别记作：VC，C，M，F，VF。设智能体机器人与墙壁的相对角度的模糊变量语言集合为{很右，右，中，左，很左}，其相应的语言变量分别记为：VR，R，M，L，VL。模糊控制规则表中GA、TLL、TL、TRL、TR的意义为前行、向左小转、向左转、向右小转、向右转。

隶属度函数是模糊控制的应用基础，正确构造隶属度函数是能否用好模糊控制的关键之一，本发明隶属度函数选用三角函数。去模糊化是将模糊数据转换成明晰数据的过程，去模糊化采用最大隶属度函数。

设S_r为智能体机器人与墙壁的距离，w_r为智能体机器人与墙壁的相对角度，U为输出，智能体机器人在沿墙跟踪时的速度为V_follow。该沿墙跟踪方法的基本流程如下：

步骤3.1：智能体机器人可以通过传感器探测正前方、左前方、右前方障碍物的距离R_f、R_1f、R_rf。当正前方的障碍物距离R_f智能体机器人小于阈值R_i时，则进入沿墙跟踪状态。若R_1f大于R_rf，则进入右沿墙跟踪状态(墙壁在智能体机器人右侧)；否则则进入左沿墙跟踪状态(墙壁在智能体机器人左侧)。记录此时智能体机器人的位置坐标(X_i，Y_i)和方向角(θ_i)。

步骤3.2：智能体机器人通过其侧面的传感器可以得到墙壁与其距离S_r和夹角w_r。右沿墙跟踪使用智能体机器人右侧的传感器，左沿墙跟踪使用智能体机器人左侧的传感器。

步骤3.3：根据距离隶属函数和角度隶属函数计算模糊控制输入量。

步骤3.4：根据表1或者表2的模糊控制规则表判断模糊控制输出量，即可得到智能体机器人的转向。

步骤3.5：根据输出隶属函数将模糊控制输出量清晰化。

步骤3.6：依据清晰化的结果控制智能体机器人运动。

步骤3.7：在每个时间步长内迭代运行上述过程。

步骤3.8：如果同时满足以下条件则退出沿墙跟踪方法：

3.8a：根据进入沿墙跟踪时的坐标(X_i，Y_i)和方向角(θ_i)确定一条直线，智能体机器人距这条直线的距离R_line小于阈值R_o。

3.8b：智能体机器人到(X_i，Y_i)的距离R_dis大于阈值R_r。

表1模糊控制规则表(左沿墙跟踪)

表2模糊控制规则表(右沿墙跟踪)

本发明的优点是：该方法简捷、高效，输出的稳定性、鲁棒性好，适用于大量的智能体机器人和智能体机器人集的运动控制。

附图说明

图1AP方法的流程图

图2模糊控制器结构图

图3距离隶属函数

图4角度隶属函数

图5输出隶属函数

具体实施方式

实施例一：

根据发明内容，下面举例对本发明做进一步说明：

步骤(1)：基于人工物理法，建立智能体机器人间相互作用的矢量力场；

在人工物理法(AP)的结构中，虚的物理力驱动一个智能体机器人群体系统以达到期望的状态。该状态是一个使全系统潜在能量最小化的形状，而系统表现为分子动力学特性

其中，F代表力，m代表质量，a代表加速度。

定义力规则：F＝Gm_im_j/r^p。其中F是智能体机器人i和智能体机器人j之间力的幅值，m_i、m_j为第i、j个智能体的质量，r是两智能体机器人间距离，G为代表聚类特性的参数，p为一个可调参数，p越大则F受r变化的影响越大。R为阈值距离，如果r≤R，力是斥力，如果r＞R，力是引力。每个智能体机器人都有一个用于检测距离和其它智能体机器人方位的传感器。受动器使智能体机器人能够以速度

移动。为了确保力规则是局部的，智能体机器人的可视范围是1.5R。智能体机器人不需要具有相同的质量。最大力参数F_max为智能体机器人所能达到的加速度提供了一个必要的约束。

步骤(2)：通过解算智能体机器人合力，确定智能体机器人的运动速度和方向，并通过四点平滑的方法抑制模态转换的瞬态；

假设有5个智能体机器人(即i＝5)，智能体机器人1、2、3、4、5的坐标点分别为(0，0)、(2，0)、(-1.2，0)、(0，4)、(5，5)，设R＝1，G＝2，F_max＝5，V_max＝2，ΔT＝0.1，所有智能体机器人的质量归一化为1；

步骤2.1：智能体机器人1与智能体机器人2间的夹角为0°距离值为2；智能体机器人1与智能体机器人3间的夹角为180°距离为1.2；智能体机器人1与智能体机器人4间的夹角为0°距离为4；智能体机器人1与智能体机器人5间的夹角为45°距离为7.071。

步骤2.2：与智能体机器人1距离最近的是智能体机器人3，其间夹角为180°距离为1.2。

步骤2.3：由于智能体机器人1与3的距离＜1.5R，且r大于R，则智能体机器人1与其最近的智能体机器人间的力F_1，3＝G/(r*r)＝1.389，力的方向分别为180°。

步骤2.4：计算SF_X＝-1.389与SF_Y＝0；

步骤2.5：ΔV_x＝-1.389*ΔT＝-0.1389；

ΔV_y＝0*ΔT＝0；

步骤2.6：设智能体机器人此时处于静止状态，V_1x＝＝0，V_1y＝0。则

V_x＝V_1x+ΔV_x＝-0.1389；

V_y＝V_1y+ΔV_y＝0；

步骤2.7：由于Vf_x和Vf_y的合成速度小于V_max，则Vf_x和Vf_y的值不变。

步骤2.8：因为之前智能体机器人处于静止状态，则使用四点平滑方法得到的智能体机器人1的期望移动速度Vf_x、Vf_y和转动角度θ’为：

Vf_x＝(0+0+0-0.1389)/4＝-0.0347；

Vf_y＝(0+0+0+0)/4＝0；

步骤2.9：依据机器人1期望移动速度Vf_x、Vf_y和转动角度θ’控制智能体机器人运动。

步骤2.10：在智能体机器人运动过程中不断迭代进行上述过程。

步骤(3)：基于模糊控制的沿墙跟踪方法，实现智能体机器人的避障；

设S_r为智能体机器人与墙壁的距离，w_r为智能体机器人与墙壁的相对角度，智能体机器人在沿墙跟踪时的速度为V_follow＝0.2m/s，该沿墙跟踪方法的基本流程如下：

设R_i＝0.5m，R_o＝0.05m，R_r＝0.5m。

步骤3.1：设在时刻t₀智能体机器人得到前方、左前方、右前方障碍物距离分别为R_f＝0.49m、R_1f＝1.2m、R_rf＝0.8m，由于R_f＜R_i且R_1f＞R_rf，则进入右沿墙跟踪状态。设此时智能体机器人坐标为(0，0)，方向角θ_i为0°。

步骤3.2：设在时刻t₁，智能体机器人墙壁与其距离S_r、夹角w_r分别为0.6m、10°。

步骤3.3：根据图3的距离隶属函数，可得距离为0.6m时VC和C的隶属度分别为0.67和0.33。由于去模糊化采用最大隶属度法，可得模糊输入量为VC。根据图4的角度隶属函数，可得角度为10°时M和L的隶属度分别为0.5和0.33。由于去模糊化采用最大隶属度法，可得模糊输入量为M。

步骤3.4：因为智能体机器人为右沿墙状态，则通过表2可得模糊输入为VC和M的情况下，模糊输出量为TLL。

步骤3.5：由于去模糊化采用最大隶属度法，根据图5的输出隶属函数可得将模糊输出的清晰化结果为20°。

步骤3.6：控制智能体机器人正向旋转20°。

步骤3.7：在每个时间步长内迭代运行上述过程.

步骤3.8：若在时刻t₂，智能体机器人的位置为(1.5m，0.03m)。进入沿墙跟踪处智能体机器人的坐标为(0，0)方向角θ_i为0°，其确定的直线方程为x＝0。t₂时智能体机器人到直线的距离R_line＝0.03m，智能体机器人到进入沿墙跟踪处的距离为R_dis＝1.5m。由于R_line＜R_o且R_dis＞R_r，则智能体机器人退出沿墙跟踪状态。

实施例二：

步骤(1)：基于人工物理法，建立智能体机器人间相互作用的矢量力场；

在人工物理法(AP)的结构中，虚的物理力驱动一个智能体机器人群体系统以达到期望的状态。该状态是一个使全系统潜在能量最小化的形状，而系统表现为分子动力学特性

其中，F代表力，m代表质量，a代表加速度。

定义力规则：F＝Gm_im_j/r^p。其中F是智能体机器人i和智能体机器人j之间力的幅值，m_i、m_j为第i、j个智能体的质量，r是两智能体机器人间距离，G为代表聚类特性的参数，p为一个可调参数，p越大则F受r变化的影响越大。R为阈值距离，如果r≤R，力是斥力，如果r＞R，力是引力。每个智能体机器人都有一个用于检测距离和其它智能体机器人方位的传感器。受动器使智能体机器人能够以速度

移动。为了确保力规则是局部的，智能体机器人的可视范围是1.5R。智能体机器人不需要具有相同的质量。最大力参数F_max为智能体机器人所能达到的加速度提供了一个必要的约束。

步骤(2)：通过解算智能体机器人合力，确定智能体机器人的运动速度和方向，并通过四点平滑的方法抑制模态转换的瞬态；

假设有3个智能体机器人(即i＝3)，智能体机器人1、2、3的坐标点分别为(0，0)、(0.2，0.2)、(-1.2，0)，设R＝1，G＝2，F_max＝5，V_max＝2，ΔT＝0.1，所有智能体机器人的质量归一化为1，考虑智能体机器人1的受力和运动。则按照上述方法的计算流程为：

步骤2.1：智能体机器人1与智能体机器人2间的夹角为45°，距离值为0.283；智能体机器人1与智能体机器人3间的夹角为180°，距离为1。

步骤2.2：与智能体机器人1距离最近的是智能体机器人2，其间夹角为45°，距离为0.283。

步骤2.3：由于智能体机器人1与2的距离＜1.5R，且r不大于R，则智能体机器人1与其最近的智能体机器人间的力F_1，2＝G/(r*r)＝25，因为F＞F_max，所以F＝5，力的方向分别为-135°。

步骤2.4：计算SF_X＝-3.54与SF_Y＝-3.54；

步骤2.5：ΔVx＝-3.54*  ΔT＝-0.354；

ΔVy＝-3.54*  ΔT＝-0.354；

步骤2.6：设V_1x＝1，V_1y＝1.2则

V_x＝V_1x+ΔVx＝0.646；

V_y＝V_1y+ΔVy＝0.846；

步骤2.7：由于V_x和V_y的合成速度小于Vmax，则V_x和V_y的值不变。

步骤2.8：设V_2x＝1.1、V_2y＝1.3、V_3x＝1.2、V_3y＝1.1，θ₁＝15°，θ₂＝15°，θ₃＝15°。则使用四点平滑方法得到的智能体机器人1的期望移动速度Vf_x、Vf_y和转动角度θ’为：

Vf_x＝(1.1+1.2+1+0.646)/4＝0.987；

Vf_y＝(1.2+1.3+1.1+0.846)/4＝1.112；

步骤2.9：依据机器人1期望移动速度Vfy、Vfy和转动角度θ’控制智能体机器人运动。

步骤2.10：在智能体机器人运动过程中不断迭代进行上述过程。

步骤(3)基于模糊控制的沿墙跟踪方法，实现智能体机器人的避障；

设S_r为智能体机器人与墙壁的距离，w_r为智能体机器人与墙壁的相对角度，U为输出，智能体机器人在沿墙跟踪时的速度为V_follow＝0.2m/s。该沿墙跟踪方法的基本流程如下：

设R_i＝0.5m，R_o＝0.05m，R_r＝0.5m。

步骤3.1：设在时刻t₀智能体机器人得到前方、左前方、右前方障碍物距离分别为R_f＝0.485m、R_1f＝0.67m、R_rf＝0.8m，由于R_f＜R_i且R_1f＜R_rf，则进入左沿墙跟踪状态。设此时智能体机器人坐标为(0.5m，0.5m)方向角θ_i为45°。

步骤3.2：设在时刻t₁，智能体机器人墙壁与其距离S_r、夹角w_r分别为0.8m、-30°。

步骤3.3：根据图3的距离隶属函数，可得距离为0.8m时C的隶属度1。去模糊化可得模糊输入量为C。根据角度隶属函数图4，可得角度为-30°时R的隶属度为1。去模糊化可得模糊输入量为R。

步骤3.4：因为智能体机器人为左沿墙状态，则通过表1可得模糊输入为C和R的情况下，模糊输出量为GA。

步骤3.5：由于去模糊化采用最大隶属度法，根据图5的输出隶属函数可得将模糊输出的清晰化结果为0°。

步骤3.6：控制智能体机器人继续向前，不转向。

步骤3.7：在每个时间步长内迭代运行上述过程。

步骤3.8：设在时刻t₂，智能体机器人的位置为(1.5m，1.55m)。进入沿墙跟踪处智能体机器人的坐标为(0.5m，0.5m)方向角θ_i为45°，其确定的直线方程为y＝x。t₂时智能体机器人到直线的距离R_line＝0.0354m，智能体机器人到进入沿墙跟踪处的距离为R_dis＝1.45m。由于R_dis＜R_o且R_dis＞R_r，则智能体机器人退出沿墙跟踪状态。

Claims (1)

1.一种基于人工物理法的多智能体机器人协同控制方法，其特征在于：该方法基于多个智能体机器人以分布式的方式进行，每个智能体机器人上有一个或多个的传感器，用来采集、融合分布式数据，并使用信息处理结果驱动智能体机器人；该方法步骤如下：

步骤(1)：基于人工物理法，建立智能体机器人间相互作用的矢量力场；

在人工物理法的结构中，虚的物理力驱动一个智能体机器人群体系统以达到期望的状态；该状态是一个使全系统潜在能量最小化的形状，而系统表现为分子动力学特性，即

其中，代表力，m代表质量，

代表加速度；

定义力规则：F＝Gm_im_j/r^p；其中F是智能体机器人i和智能体机器人j之间力的幅值，m_i、m_j为第i、j个智能体的质量，r是两智能体机器人间距离，G为代表聚类特性的参数，p为一个可调参数，p越大则F受r变化的影响越大；R为阈值距离，如果r≤R，力是斥力，如果r＞R，力是引力；每个智能体机器人都有一个用于检测距离和其它智能体机器人方位的传感器；受动器使智能体机器人能够以速度

移动；为了确保力规则是局部的，智能体机器人的可视范围是1.5R；智能体机器人不需要具有相同的质量；最大力参数F_max为智能体机器人所能达到的加速度提供了一个必要的约束；从步骤(1)得出每个智能体上所受的力和力的方向，这些变量作为步骤(2)的输入；

步骤(2)：通过解算智能体机器人合力，确定智能体机器人的运动速度和方向，并通过四点平滑的方法抑制模态转换的瞬态；

每一个智能体机器人具有位置

和速度

我们使用离散时间近似智能体机器人的连续行为，时间步长为Δt；在每一个时间步长内，每一个智能体机器人的位置变化

该变化取决于当前速度，在每一个时间步长内，每一个智能体机器人的速度也发生改变速度的改变

量受施加在智能体机器人上的力的控制，

其中m是智能体机器人的质量，是智能体机器人上受到的力；

每个智能体机器人所具有的质量m使智能体机器人具有动量；智能体机器人不需要具有相同的质量；最大速度参数V_max限制了智能体机器人的最大速度；

在人工物理法的流程中，设智能体机器人群中智能体机器人个数为N，m_i的默认值取1.0，p默认值取2，重力常数G在初始化中被设定，ΔT为离散的步长；

对任何一个智能体机器人i在每个离散时间段的人工物理法步骤流程如下：

步骤2.1：首先智能体机器人i从传感器获取与其它智能体机器人间的夹角θ和距离值r；

步骤2.2：得出与智能体机器人i距离最近的智能体机器人j，智能体机器人i与智能体机器人j夹角为θ₀和距离值为r；

步骤2.3：依据下式确定F，θ，F为智能体机器人i所受力的大小，θ为智能体机器人i所受力的方向：

$F_0=\left\{\begin{array}{cc}G/(r*r)& r\≤1.5R\\ 0& r>1.5R\end{array}\right.$

$F=\left\{\begin{array}{cc}{F}_0& F_0\≤F_{\max }\\ F_{\max }& F_0>F_{\max }\end{array}\right.$

步骤2.4：计算F在x轴与y轴上的分量，得到SF_X与SF_Y；

SF_X＝F×Cosθ

SF_Y＝F×Sinθ

步骤2.5：根据上述计算得到的SF_X与SF_Y，计算时间步长内的速度增量ΔV_x与ΔV_Y；

ΔV_x SF_X*ΔT/m_i；

ΔV_y＝SF_Y*ΔT/m_i；

步骤2.6：计算时间步长后x轴和y轴上的速度分量V_x与V_y；V_1x和V_1y为上一步x轴和y轴上的速度分量；

V_x＝V_1x+ΔV_x；

V_y＝V_1y+ΔV_y；

步骤2.7：依据下述公式对V_x，V_y进行限幅：

步骤2.8：使用四点平滑方法计算智能体机器人i的期望移动速度V_fx、V_fy和期望转动角度θ’，即：

V_fx＝(V_1x+V_2x+V_3x+V_x)/4

V_fy＝(V_1y+V_2y+V_3y+V_y)/4

{V_1x，V_1y}、{V_2x，V_2y}、{V_3x，V_3y}分别为前一步、前两步和前三步的速度值；

依据V_fx，V_fy计算智能体机器人i期望转动角度θ’：

步骤2.9：依据智能体机器人i期望移动速度V_fx、V_fy和期望转动角度θ’控制智能体机器人i运动；

步骤2.10：在智能体机器人i运动过程中不断迭代进行上述过程，即步骤2.1至步骤2.9；

步骤(3)：基于模糊控制的沿墙跟踪方法，实现智能体机器人的避障；

基于模糊控制的沿墙跟踪方法需要设计一个双输入单输出的模糊控制器，依据智能体机器人与墙壁的距离和相对于墙壁的角度判断智能体机器人的转向，使智能体机器人与墙壁保持一定的距离运动；

模糊控制器主要由模糊化、知识库、模糊推理、清晰化四部分组成；设智能体机器人与墙壁的距离的模糊变量语言集合为很近，近，中，远，很远，其相应的语言变量分别记作：VC，C，M，F，VF；设智能体机器人与墙壁的相对角度的模糊变量语言集合为很右，右，中，左，很左，其相应的语言变量分别记为：VR，R，M，L，VL；模糊控制规则表中GA、TLL、TL、TRL、TR的意义为前行、向左小转、向左转、向右小转、向右转；

设S_r为智能体机器人与墙壁的距离，w_r为智能体机器人与墙壁的相对角度，U为输出，智能体机器人在沿墙跟踪时的速度为V_follow；该沿墙跟踪方法的基本流程如下：

步骤3.1：智能体机器人通过传感器探测正前方、左前方、右前方障碍物的距离R_f、R_lf、R_rf；当正前方的障碍物距离R_f智能体机器人小于阈值R_i时，则进入沿墙跟踪状态；若R_lf大于R_rf，则进入右沿墙跟踪状态；否则则进入左沿墙跟踪状态；记录此时智能体机器人的位置坐标(X_i，Y_i)和方向角θ_i；

步骤3.2：智能体机器人通过其侧面的传感器得到墙壁与其距离S_r和夹角w_r；右沿墙跟踪使用智能体机器人右侧的传感器，左沿墙跟踪使用智能体机器人左侧的传感器；

步骤3.3：根据距离隶属函数和角度隶属函数计算模糊控制输入量；

步骤3.4：根据模糊控制规则表判断模糊控制输出量，即得到智能体机器人的转向；

步骤3.5：根据输出隶属函数将模糊控制输出量清晰化；

步骤3.6：依据清晰化的结果控制智能体机器人运动；

步骤3.7：在每个时间步长内迭代运行上述过程；

步骤3.8：如果同时满足以下条件则退出沿墙跟踪方法：

3.8a：根据进入沿墙跟踪时的坐标(X_i，Y_i)和方向角(θ_i)确定一条直线，智能体机器人距这条直线的距离R_lie小于阈值R_o；

3.8b：智能体机器人到(X_i，Y_i)的距离R_dis大于阈值R_r。

Images