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

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，

Far

Near

Far

Front

Slow

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	三角形模糊集合类，使用顶点，左偏移和右偏移定义

源码

from:　http://blog.csdn.net/zddblog/article/details/8978431

模糊决策--机器人壁障相关推荐

机器人避障之滑动窗口处理数据
目的构建一个滑动窗,窗口大小固定,进出窗口顺序按照FIFO模型,即先入先出,让然后判断该窗口中数值趋近律,是否满足递减要求. 场景机器人在避障时,有时仅仅依靠传感器数据(激光雷达,线激光,超声波, ...
突破次元壁障，Python爬虫获取二次元女友
突破次元壁障,Python爬虫获取二次元女友前言程序说明二次元女友获取程序观察网页结构页面解析创建图片保存路径图片下载格式转换爬取结果展示完整程序前言 (又到了常见的无中生友环节 ...
机器人技术-机器人避障研究现状
机器人避障研究现状 1.无人机避障目前主流的无人机避障导航有三种实现方式:使用超声波的方法.使用 TOF 的方法,以及使用视觉图像处理的避障导航方法.前两种为非视觉传感器避障,后一种为新兴的视觉传感 ...
【路径规划】基于matlab GUI粒子群算法机器人避障路径规划（手动设障）【含Matlab源码 924期】
⛄一.简介 1 粒子群算法的概念粒子群优化算法(PSO:Particle swarm optimization) 是一种进化计算技术(evolutionary computation).源于对鸟群捕 ...
明早10点大讲堂 | 一种新型光幕传感器在机器人避障和无人驾驶中的应用
雷锋网AI研习社按:像LIDAR这样的3D传感器经常用于今天的道路场景理解.让我们简要地看看它是如何被使用的.在GPS和IMU帮助的情况下,我们将大量的捕获的点云注册到预先构建的或累积的3D地图,然后 ...
手机虚拟摄像头_新游 | 打破次元壁障，《猪猪侠AR虚拟使命》现实约战，一切尽在创酷互动！...
今日,创酷互动游戏<猪猪侠AR虚拟使命>全网震撼首发,冬季火爆登陆.本款游戏采用知名IP<猪猪侠>,更采用<口袋妖怪GO>所应用的AR增强现实技术,全新科技AR制作 ...
python遥控汽车玩具_[详细实例]MicroPython拼插编程实战：DIY一台会思考的壁障车...
(转载请注明文章来源,更多教程可自助参考www.tpyboard.com,QQ技术交流群:157816561,公众号:MicroPython玩家汇) 在日常生活中,大家会经常见到各种各样的遥控车,它需 ...
机器人技术-HC-SR04超声波传感器数据及机器人避障的应用
比较详细的HC-SR04超声波传感器数据及机器人避障的应用方法言.超声波传感器概述超声波传感器型号繁多,价格从几元钱到几百元不等,主要用于检测距离,同时根据声速计算出物体的距离.但超声波传感器有四 ...
tof摄像头手势识别_揭秘国内独家TOF深度摄像头，助力机器人避障与室内导航
揭秘国内独家TOF深度摄像头,助力机器人避障与室内导航 2018-09-11 TOF深度摄像头利用光的飞行时间测量距离,能够实时快速的计算深度信息,进行准确的三维探测,是实现机器人避障和室内定位与导航 ...

模糊决策--机器人壁障