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

?

?

1、程序要求

平面上有布滿障礙物，設(shè)定起始點和終點。一個機器人從起點運動到終點，經(jīng)過最短路徑不與障礙物發(fā)生碰撞。起點和終點由用戶指定，障礙物的個數(shù)和形狀可以隨機生成，存儲在文件中讀出或者由用戶指定生成。機器人為圓型機器人，在頭部正中配有7個傳感器，以正中為界限跟別為0度，+-5度，+-15度，+-90度，傳感器感知范圍為3個機器人直徑長度。機器人性能為速度最大為機器人直徑，轉(zhuǎn)角最大為+-30度。要求應(yīng)用模糊決策方法實現(xiàn)，需要構(gòu)造知識庫。

評分標準：

1.實現(xiàn)基本操作：中等

2.實現(xiàn)改進算法：良好

3.完成五個機器人在障礙物群體中避障行走：優(yōu)秀

2、模糊控制模塊的設(shè)計

2.1確定輸入量和輸出量極其模糊語言描述

模糊控制模塊的輸入是超聲波傳感器的距離信息、移動機器人當前的運動速度和目標的方向信息,輸出是移動機器人的加速度和旋轉(zhuǎn)角度信息.移動機器人系統(tǒng)結(jié)構(gòu)如圖1所示.移動機器人，正如程序要求，機器人頭部正中配有7個傳感器，以正中為界限跟別為0度，+-5度，+-15度，+-90度，傳感器感知范圍為3個機器人直徑長度。利用傳感器獲取機器人左方距離，前方距離和右方距離信息，其中，0度，+-5度中最小距離確定前方距離。-15度和-90度確定左方距離，15度和90度確定右方距離。

圖1機器人示意圖

模糊機器人有五個輸入量，分別為：左方距離LD、前方距離FD、右方距離RD、當前速度v和距目標的夾角θ。輸出量為：機器人的加速度a和機器人的旋轉(zhuǎn)角度β (注：實際控制小車時，輸出變量設(shè)置為小車左右輪加速度將更加方便)。使用連續(xù)型論域,采用簡單的線性化處理方法,將距離輸入變量模糊語言變量定為：{Near, Far}，將當前速度定義為{Slow, Fast}，將夾角定義為{Left, Front, Right}，將加速度定義為{BN, SN, Z, SP, BP}，(符號說明：{Big Negative, Small Negative, Zero, Small Positive, Big Positive})。

2.2?確定輸入輸出變量的隸屬度函數(shù)

取各個語言變量的隸屬度函數(shù)形狀為對稱的三角形且模糊分割是對稱的。規(guī)定當目標在機器人的右前方時為正值反之為負。各變量的隸屬度函數(shù)如下圖所示：

圖2 各變量的隸屬度函數(shù)

2.3?設(shè)計模糊控制規(guī)則庫

模糊控制規(guī)則的建立是模糊控制的核心問題。模糊控制系統(tǒng)是用一系列基于專家知識的語言來描述的, 用一系列模糊條件描述的模糊控制規(guī)則就構(gòu)成了模糊控制規(guī)則庫。

根據(jù)FuzzySet 理論, 建立定性推理規(guī)則, 模糊規(guī)則反映了輸入輸出量之間的關(guān)系. 按照模糊邏輯進行推理的基本理論, 規(guī)定五個定性的輸入信號,兩個定性的輸出信號, 整個系統(tǒng)就構(gòu)成了一個有五個輸入、兩個輸出的多輸入多輸出模糊系統(tǒng)。當同一區(qū)域內(nèi)出現(xiàn)兩個或兩個以上障礙物信息時, 則只考慮最近的障礙物。

機器人路徑是依據(jù)障礙物位置、目標位置的傳感器信息和機器人當前運動速度來給出到達目標的策略. 當探測到障礙物接近機器人時, 機器人將改變運動軌跡, 以避免碰撞. 機器人轉(zhuǎn)向的基本原則是: 當探測到機器人左( 右) 和前方出現(xiàn)障礙物時, 機器人應(yīng)即時轉(zhuǎn)向右( 左) 方向. 機器人轉(zhuǎn)向的改變是靠左右輪速度的改變來控制的, 并且速度的改變還能有效控制機器人運動的時效性. 根據(jù)上面所確定的輸入輸出集, 模糊規(guī)則的一般形式可以If(條件)then( 結(jié)果) 描述。

根據(jù)不同的機器人軌跡圖和目標方位,可以制定一系列的模糊規(guī)則，本程序中使用的模糊規(guī)則庫如下表所示：

表1 模糊規(guī)則知識庫

規(guī)則編號	輸入					輸出
	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模糊推理

模糊推理是模糊控制器的核心,它具有模擬人的基于模糊概念的推理能力,該推理過程是基于模糊邏輯中的蘊含關(guān)系及推理規(guī)則來進行的。

舉例說明模糊決策的推理過程。假設(shè)機器人探測到左方距離LD=280m，前方距離FD=100m，右方距離RD=240m,與目標的夾角為40°，機器人當前的速度為1m/s。若為某模糊集合中的元素，令為x在該集合上屬于的可能性，即隸屬度。根據(jù)隸屬度函數(shù)圖，模糊化得：

之后，對規(guī)則庫中的所有規(guī)則進行推理，比如對表1中的規(guī)則9，

9

Far

Near

Far

Front

Slow

Z

Front

前件的隸屬度f(9)。

則對于規(guī)則9，加速度a的隸屬度函數(shù)如下圖3所示：

圖3規(guī)則9推出的加速度隸屬函數(shù)

?

以此計算所有的規(guī)則，加到上圖之上，繪出合成隸屬度函數(shù)。

?

2.5解模糊

通過模糊推理得到的結(jié)果是一個模糊集合.但在實際模糊控制中,必須要有一個確定值才能控制或驅(qū)動執(zhí)行機構(gòu).將模糊推理結(jié)果轉(zhuǎn)化為精確值的過程稱為解模糊.所以,解模糊的作用是將模糊集合映射為一個確定的點。

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

一個模糊集合的最大值或者代表值是隸屬度為一的值，對三角集合來說，這個值是它的中點，對有高臺的集合(左肩和右肩)來說，它是高臺起點和末尾點的評均值。計算公式如下：

?

假設(shè)合成加速度a的隸屬度函數(shù)如下圖4所示：

圖4加速度a的合成隸屬度函數(shù)

?

則去模糊的計算式為：。

即模糊決策算法獲得的加速度值為0.71 m/s2。旋轉(zhuǎn)角度的算法類似。

?

2.6模糊模塊的程序設(shè)計

考慮到模糊決策算法在路徑規(guī)劃，游戲控制等方面的重要性，將模糊模塊進行了封裝，方便以后使用。目前，該模塊僅支持模糊集合的“或”運算和“與”運算，推理規(guī)則中前件和后件的最大項數(shù)為5。該模塊可使用的隸屬函數(shù)形狀為三角形、左肩形和右肩行。類圖略。

?

使用方法形如下：

//創(chuàng)建規(guī)則庫

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、幾個重要的算法設(shè)計

3.1?直線和圓求交點

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

{

?????? //確保起點不在障礙物之內(nèi),起點在小車的終點，永遠在障礙物之外

//???? 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

?????? {

?????? ?????? //假設(shè)直線與圓的交點為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++)

?????? {

?????? ?????? //將點轉(zhuǎn)換到世界坐標系

?????? ?????? 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程序所使用的主要類清單

羅列程序中使用的類，方便理解源代碼，具體實現(xiàn)見程序。

表2程序清單

?	類名	描述
?	CrazyWorld	機器人所在世界類，包含機器人、障礙物、目標等
與機器人有關(guān)	BaseEntity	CrazyWorld中所有實體的同意接口
?	MovingEntity	封裝運動實體類，繼承自BaseEntity
?	Robot	機器人類，繼承自MovingEntity
障礙物	Obstacle	障礙物基類，繼承自BaseEntity
?	LineObstace	線型障礙物，繼承自Obstacle
?	CircleObstace	圓形障礙物，繼承自Obstacle
?	RectObstace	方形障礙物，繼承自Obstacle
模糊模塊	FuzzySet	模糊集合接口,一般使用的隸屬度函數(shù)有三角形、梯形和高斯曲線形等，該接口的實例實現(xiàn)隸屬度函數(shù)的子部分
?	FuzzyRule	規(guī)則類，創(chuàng)建形如?if a1 AND a2 .... the an...的規(guī)則
?	FuzzyTerm	規(guī)則計算的接口類，形如if .. then ..的規(guī)則
?	FzAND、FzOR	實現(xiàn)或和與的運算，繼承自FuzzyTerm
?	FuzzyModule	模糊模塊類
?	FzSet	代理FuzzySet類
?	FuzzySet_LeftShoulder	左肩
?	FuzzySet_RightShoulder	右肩
?	FuzzySet_Triangle	三角形模糊集合類，使用頂點，左偏移和右偏移定義

?

源碼

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

總結(jié)

以上是生活随笔為你收集整理的模糊决策--机器人壁障的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。