模糊决策--机器人壁障

翻出了自己以前的小程序，居然还有人要，就共享一下

 

 

1、程序要求

平面上有布满障碍物，设定起始点和终点。一个机器人从起点运动到终点，经过最短路径不与障碍物发生碰撞。起点和终点由用户指定，障碍物的个数和形状可以随机生成，存储在文件中读出或者由用户指定生成。机器人为圆型机器人，在头部正中配有7个传感器，以正中为界限跟别为0度，+-5度，+-15度，+-90度，传感器感知范围为3个机器人直径长度。机器人性能为速度最大为机器人直径，转角最大为+-30度。要求应用模糊决策方法实现，需要构造知识库。

评分标准：

1.实现基本操作：中等

2.实现改进算法：良好

3.完成五个机器人在障碍物群体中避障行走：优秀

2、模糊控制模块的设计

2.1确定输入量和输出量极其模糊语言描述

模糊控制模块的输入是超声波传感器的距离信息、移动机器人当前的运动速度和目标的方向信息,输出是移动机器人的加速度和旋转角度信息.移动机器人系统结构如图1所示.移动机器人，正如程序要求，机器人头部正中配有7个传感器，以正中为界限跟别为0度，+-5度，+-15度，+-90度，传感器感知范围为3个机器人直径长度。利用传感器获取机器人左方距离，前方距离和右方距离信息，其中，0度，+-5度中最小距离确定前方距离。-15度和-90度确定左方距离，15度和90度确定右方距离。

图1机器人示意图

模糊机器人有五个输入量，分别为：左方距离LD、前方距离FD、右方距离RD、当前速度v和距目标的夹角θ。输出量为：机器人的加速度a和机器人的旋转角度β (注：实际控制小车时，输出变量设置为小车左右轮加速度将更加方便)。使用连续型论域,采用简单的线性化处理方法,将距离输入变量模糊语言变量定为：{Near, Far}，将当前速度定义为{Slow, Fast}，将夹角定义为{Left, Front, Right}，将加速度定义为{BN, SN, Z, SP, BP}，(符号说明：{Big Negative, Small Negative, Zero, Small Positive, Big Positive})。

2.2 确定输入输出变量的隶属度函数

取各个语言变量的隶属度函数形状为对称的三角形且模糊分割是对称的。规定当目标在机器人的右前方时为正值反之为负。各变量的隶属度函数如下图所示：

图2 各变量的隶属度函数

2.3 设计模糊控制规则库

模糊控制规则的建立是模糊控制的核心问题。模糊控制系统是用一系列基于专家知识的语言来描述的, 用一系列模糊条件描述的模糊控制规则就构成了模糊控制规则库。

根据FuzzySet 理论, 建立定性推理规则, 模糊规则反映了输入输出量之间的关系. 按照模糊逻辑进行推理的基本理论, 规定五个定性的输入信号,两个定性的输出信号, 整个系统就构成了一个有五个输入、两个输出的多输入多输出模糊系统。当同一区域内出现两个或两个以上障碍物信息时, 则只考虑最近的障碍物。

机器人路径是依据障碍物位置、目标位置的传感器信息和机器人当前运动速度来给出到达目标的策略. 当探测到障碍物接近机器人时, 机器人将改变运动轨迹, 以避免碰撞. 机器人转向的基本原则是: 当探测到机器人左( 右) 和前方出现障碍物时, 机器人应即时转向右( 左) 方向. 机器人转向的改变是靠左右轮速度的改变来控制的, 并且速度的改变还能有效控制机器人运动的时效性. 根据上面所确定的输入输出集, 模糊规则的一般形式可以If(条件)then( 结果) 描述。

根据不同的机器人轨迹图和目标方位,可以制定一系列的模糊规则，本程序中使用的模糊规则库如下表所示：

表1 模糊规则知识库

规则编号

输入

输出

LD

FD

RD

θ

v

a

θ

1

Far

Far

Far

Right

Slow

SP

Right

2

Far

Far

Far

Right

Fast

SP

Right

3

Far

Far

Far

Left

Slow

SP

Left

4

Far

Far

Far

Left

Fast

SP

Left

5

Far

Far

Far

Front

Slow

Z

Front

6

Far

Far

Far

Front

Fast

SP

Front

7

Near

Near

Near

Front

Slow

Z

Right

8

Near

Near

Near

Front

Fast

BN

Right

9

Far

Near

Far

Front

Slow

Z

Front

10

Far

Near

Far

Front

Fast

BN

Right

11

Far

Far

Near

Front

Slow

Z

Front

12

Far

Far

Near

Front

Fast

BN

Left

13

Near

Far

Far

Left

Slow

Z

Front

14

Near

Far

Far

Left

Fast

BN

Right

15

Far

Far

Near

Left

Slow

Z

Front

16

Far

Far

Near

Left

Fast

BN

Right

 

2.4模糊推理

模糊推理是模糊控制器的核心,它具有模拟人的基于模糊概念的推理能力,该推理过程是基于模糊逻辑中的蕴含关系及推理规则来进行的。

举例说明模糊决策的推理过程。假设机器人探测到左方距离LD=280m，前方距离FD=100m，右方距离RD=240m,与目标的夹角为40°，机器人当前的速度为1m/s。若为某模糊集合中的元素，令为x在该集合上属于的可能性，即隶属度。根据隶属度函数图，模糊化得：

之后，对规则库中的所有规则进行推理，比如对表1中的规则9，

9

Far

Near

Far

Front

Slow

Z

Front

前件的隶属度f(9)。

则对于规则9，加速度a的隶属度函数如下图3所示：

图3规则9推出的加速度隶属函数

 

以此计算所有的规则，加到上图之上，绘出合成隶属度函数。

 

2.5解模糊

通过模糊推理得到的结果是一个模糊集合.但在实际模糊控制中,必须要有一个确定值才能控制或驱动执行机构.将模糊推理结果转化为精确值的过程称为解模糊.所以,解模糊的作用是将模糊集合映射为一个确定的点。

解模糊也叫去模糊，是模糊化的逆过程，有很多技术可以完成，比如中心法，最大值均值法，本程序使用最大值平均法(MaxAv)。

一个模糊集合的最大值或者代表值是隶属度为一的值，对三角集合来说，这个值是它的中点，对有高台的集合(左肩和右肩)来说，它是高台起点和末尾点的评均值。计算公式如下：

 

假设合成加速度a的隶属度函数如下图4所示：

图4加速度a的合成隶属度函数

 

则去模糊的计算式为：。

即模糊决策算法获得的加速度值为0.71 m/s2。旋转角度的算法类似。

 

2.6模糊模块的程序设计

考虑到模糊决策算法在路径规划，游戏控制等方面的重要性，将模糊模块进行了封装，方便以后使用。目前，该模块仅支持模糊集合的“或”运算和“与”运算，推理规则中前件和后件的最大项数为5。该模块可使用的隶属函数形状为三角形、左肩形和右肩行。类图略。

 

使用方法形如下：

//创建规则库

FuzzyModule m_FuzzyModule;   

 

FuzzyVariable& LeftDistance = m_FuzzyModule.CreateFLV("LeftDistance");

FzSet& LD_Near = LeftDistance.AddLeftShoulderSet("LD_Near", 0, 80, 250);

FzSet& LD_Far = LeftDistance.AddRightShoulderSet("LD_Far", 80, 250, 1000);

 

 

FuzzyVariable& FrontDistance = m_FuzzyModule.CreateFLV("FrontDistance");

……

 

m_FuzzyModule.AddRule(FzAND(LD_Far, FD_Far, RD_Far, Right, Slow), FzAND(SP,      RotateToRight));  //Rule:  1

m_FuzzyModule.AddRule(FzAND(LD_Far, FD_Far, RD_Far, Right, Fast), FzAND(SP,        RotateToRight));   //Rule:  2

……

 

//模糊化输入变量

m_FuzzyModule.Fuzzify("LeftDistance", ld);

m_FuzzyModule.Fuzzify("FrontDistance", fd);

m_FuzzyModule.Fuzzify("RightDistance", rd);

……

 

//去模糊，输出

double Acc = m_FuzzyModule.DeFuzzify("Acceleration", FuzzyModule::max_av);

.……

3、几个重要的算法设计

3.1 直线和圆求交点

Vector2D CircleObstace::IntersectionLine(Vector2D startPoint, Vector2D endPoint)

{

       //确保起点不在障碍物之内,起点在小车的终点，永远在障碍物之外

//     assert( ((Vector2D(Pos()-startPoint).Length()) >= BRadius()) &&

//            "CircleObstace<IntersectionLine> startPoint inside!");

 

       Vector2D startToPos = Vec2DNormalize(Pos() - startPoint);

       Vector2D lineVec = Vec2DNormalize(endPoint - startPoint);

 

       //夹角余弦

       double consine = startToPos.Dot(lineVec);

       //圆心到直线的距离

       double d = (Vector2D(Pos()-startPoint).Length()) * sin(acos(consine));

      

       if(d > BRadius())

              return endPoint;

       else

       {

              //假设直线与圆的交点为P

              double sp = (Vector2D(Pos()-startPoint).Length()) * consine

                     - sqrt(BRadius()* BRadius() - d * d);

              Vector2D vecSP = sp * lineVec;

              endPoint = Vector2D(vecSP.x + startPoint.x, vecSP.y + startPoint.y);

              return endPoint;    

       }

}

 

3.2 障碍物检测

void Robot::UpdateSonarLines(double &ld, double &fd, double &rd)

{

 

       for(int i = 0; i < m_vecSonarLinesOriginal.size(); i++)

       {

              //将点转换到世界坐标系

              Vector2D point = PointToWorldSpace(m_vecSonarLinesOriginal[i],

                                                                                           Heading(),

                                                                                           Side(),

                                                                                           Pos());

 

              double minDist = Vector2D(Pos()-point).Length();

              Vector2D temp = point;

              std::vector<Obstacle*>::const_iteratorcurObstacle=m_pWorld->GetObstacles().begin();

              while(curObstacle != m_pWorld->GetObstacles().end())

              {

                     Vector2D ToCurObstacle = (*curObstacle)->Pos() - m_vPos;

                     Vector2D LocalPos = PointToLocalSpace((*curObstacle)->Pos(),

                                                                                    Heading(),

                                                                                    Side(),

                                                                                    Pos());

 

                     if(LocalPos.x > 0)

                     {

                            Vector2D p = (*curObstacle)->IntersectionLine(Pos(), point);

                            if(Vector2D(Pos()-p).Length() < minDist)

                            {    

                                   minDist = Vector2D(Pos()-p).Length();

                                   temp = p;

                            }

 

                     }

                     curObstacle++;

              }

……

}

3.3程序所使用的主要类清单

罗列程序中使用的类，方便理解源代码，具体实现见程序。

表2程序清单

 

类名

描述

 

CrazyWorld

机器人所在世界类，包含机器人、障碍物、目标等

与机器人有关

BaseEntity

CrazyWorld中所有实体的同意接口

 

MovingEntity

封装运动实体类，继承自BaseEntity

 

Robot

机器人类，继承自MovingEntity

障碍物

Obstacle

障碍物基类，继承自BaseEntity

 

LineObstace

线型障碍物，继承自Obstacle

 

CircleObstace

圆形障碍物，继承自Obstacle

 

RectObstace

方形障碍物，继承自Obstacle

模糊模块

FuzzySet

模糊集合接口,一般使用的隶属度函数有三角形、梯形和高斯曲线形等，该接口的实例实现隶属度函数的子部分

 

FuzzyRule

规则类，创建形如 if a1 AND a2 .... the an...的规则

 

FuzzyTerm

规则计算的接口类，形如if .. then ..的规则

 

FzAND、FzOR

实现或和与的运算，继承自FuzzyTerm

 

FuzzyModule

模糊模块类

 

FzSet

代理FuzzySet类

 

FuzzySet_LeftShoulder

左肩

 

FuzzySet_RightShoulder

右肩

 

FuzzySet_Triangle

三角形模糊集合类，使用顶点，左偏移和右偏移定义

 

源码

赞助：如果您觉得此渣文对您有用，欢迎扫描以下二维码，对我进行小额赞助，一毛两毛的都行。不筹钱娶媳妇的程序员不是好程序员！