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

?

?

1、程序要求

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

評(píng)分標(biāo)準(zhǔn)：

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

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

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

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

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

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

圖1機(jī)器人示意圖

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

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

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

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

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

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

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

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

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

表1 模糊規(guī)則知識(shí)庫(kù)

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

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

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

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

9

Far

Near

Far

Front

Slow

Z

Front

前件的隸屬度f(9)。

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

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

?

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

?

2.5解模糊

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

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

一個(gè)模糊集合的最大值或者代表值是隸屬度為一的值，對(duì)三角集合來說，這個(gè)值是它的中點(diǎn)，對(duì)有高臺(tái)的集合(左肩和右肩)來說，它是高臺(tái)起點(diǎn)和末尾點(diǎn)的評(píng)均值。計(jì)算公式如下：

?

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

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

?

則去模糊的計(jì)算式為：。

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

?

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

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

?

使用方法形如下：

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

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

3.1?直線和圓求交點(diǎn)

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

{

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

//???? 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è)直線與圓的交點(diǎn)為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?障礙物檢測(cè)

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

{

?

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

?????? {

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

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

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

表2程序清單

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

?

源碼

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

總結(jié)

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

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