《智能機(jī)器人創(chuàng)新設(shè)計(jì)》 課件 第9-14章 單機(jī)器人路徑規(guī)劃 -智能算法綜合比較

第9章單機(jī)器人路徑規(guī)劃智慧物流系統(tǒng)：從設(shè)計(jì)到實(shí)現(xiàn)教學(xué)內(nèi)容CONTENTS3啟發(fā)式搜索2盲目式搜索4廣度優(yōu)先搜索算法5深度優(yōu)先搜索算法6A*搜索算法1路徑規(guī)劃3

章節(jié)目標(biāo)了解路徑規(guī)劃的定義和分類(lèi)；了解單機(jī)器人路徑規(guī)劃的兩大搜索方式；掌握三種搜索算法的原理和搜索方式。41路徑規(guī)劃單機(jī)器人路徑規(guī)劃路徑規(guī)劃：依據(jù)某種原則，在工作空間中找到一條從起始點(diǎn)到目標(biāo)點(diǎn)且能避開(kāi)障礙物的最優(yōu)路徑稱(chēng)為路徑規(guī)劃。將連接起點(diǎn)位置和終點(diǎn)位置的序列點(diǎn)或曲線(xiàn)稱(chēng)之為路徑，路徑規(guī)劃也就是構(gòu)成路徑的策略。路徑規(guī)劃是運(yùn)動(dòng)規(guī)劃的主要研究?jī)?nèi)容之一。51路徑規(guī)劃靜態(tài)路徑規(guī)劃和動(dòng)態(tài)路徑規(guī)劃在機(jī)器人工作的環(huán)境中，遇到的障礙物分為靜態(tài)障礙物與動(dòng)態(tài)障礙物。針對(duì)環(huán)境中的障礙物可將路徑規(guī)劃分為靜態(tài)路徑規(guī)劃和動(dòng)態(tài)路徑規(guī)劃。①靜態(tài)路徑規(guī)劃是指機(jī)器人在有靜態(tài)障礙物的工作環(huán)境中尋找一條從起點(diǎn)到目標(biāo)點(diǎn)的運(yùn)動(dòng)路徑，使機(jī)器人在運(yùn)動(dòng)過(guò)程中能夠安全無(wú)碰撞地繞過(guò)所有障礙物。②動(dòng)態(tài)路徑規(guī)劃是指機(jī)器人在有動(dòng)態(tài)障礙物的環(huán)境中先尋找一條恰當(dāng)?shù)膹钠瘘c(diǎn)到目標(biāo)點(diǎn)的運(yùn)動(dòng)路徑，然后在運(yùn)動(dòng)的過(guò)程中發(fā)現(xiàn)動(dòng)態(tài)障礙物后，實(shí)時(shí)更新當(dāng)前位置到目標(biāo)點(diǎn)的路徑。61路徑規(guī)劃移動(dòng)機(jī)器人路徑規(guī)劃假設(shè)B為一機(jī)器人系統(tǒng)，并假設(shè)B在一個(gè)空間V中，有一組該機(jī)器人系統(tǒng)已知的障礙物，機(jī)器人可以進(jìn)行無(wú)碰撞運(yùn)動(dòng)。那對(duì)于機(jī)器人B的路徑規(guī)劃為：在空間V中，給B一個(gè)初始位姿Z1、一個(gè)目標(biāo)位姿Z2和一組已知的障礙物，尋找一條從Z1到Z2的連續(xù)的避碰最優(yōu)路徑，如果該路徑存在，那么就規(guī)劃出一條這樣的路徑?？臻gV初始位姿Z1目標(biāo)位姿Z2障礙物71路徑規(guī)劃移動(dòng)機(jī)器人路徑規(guī)劃主要解決三個(gè)問(wèn)題：

①機(jī)器人從初始點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)。②通過(guò)算法規(guī)劃路徑，使機(jī)器人繞開(kāi)障礙物并且經(jīng)過(guò)某些必須經(jīng)過(guò)的坐標(biāo)點(diǎn)。使用算法規(guī)劃?rùn)C(jī)器人路徑的時(shí)候，算法先去搜索地圖，然后回溯搜索過(guò)的路徑，找到目標(biāo)點(diǎn)到起點(diǎn)較優(yōu)的路徑。③在完成以上任務(wù)的前提下盡量?jī)?yōu)化機(jī)器人運(yùn)行軌跡?？臻gV初始位姿Z1目標(biāo)位姿Z2障礙物8盲目式搜索與啟發(fā)式搜索：機(jī)器人在未知區(qū)域探索時(shí)需要采用一定的搜索策略對(duì)其路徑選擇進(jìn)行指導(dǎo)，在搜索的同時(shí)對(duì)當(dāng)前區(qū)域進(jìn)行地圖建模，建立距離等高圖，由此可以獲得任意兩點(diǎn)間的最短路徑。根據(jù)搜索時(shí)是否依靠信息可以將搜索分為盲目式搜索和啟發(fā)式搜索兩種搜索方式。1路徑規(guī)劃92盲目式搜索盲目式搜索：盲目式搜索又叫無(wú)信息搜索，在搜索過(guò)程中按照預(yù)定的控制策略進(jìn)行搜索，途中獲得的路徑信息不用來(lái)改變控制策略。常用算法主要包括廣度優(yōu)先搜索算法（BFS）和深度優(yōu)先搜索算法（DFS）。這兩種搜索算法在搜索時(shí)都按規(guī)定路線(xiàn)進(jìn)行，不使用與問(wèn)題相關(guān)的啟發(fā)性信息，適用于其狀態(tài)空間圖是樹(shù)狀結(jié)構(gòu)一類(lèi)的簡(jiǎn)單問(wèn)題。盲目式搜索會(huì)導(dǎo)致搜索效率低，會(huì)在計(jì)算時(shí)耗費(fèi)過(guò)多的空間與時(shí)間。103啟發(fā)式搜索啟發(fā)式搜索：?jiǎn)l(fā)式搜索就是在搜索中鍵入啟發(fā)性信息，用來(lái)指導(dǎo)搜索過(guò)程朝著最有希望的方向前進(jìn)。啟發(fā)性信息就是在搜索過(guò)程中獲取到的控制前進(jìn)方向的信息，其用途可分為三種：①用于確定要擴(kuò)展的下一節(jié)點(diǎn)的位置，以免盲目地去擴(kuò)展；②在擴(kuò)展下一節(jié)點(diǎn)的過(guò)程中，用于決定要生成哪一個(gè)節(jié)點(diǎn)或哪幾個(gè)后繼節(jié)點(diǎn)，以免盲目地同時(shí)生成所有可能的節(jié)點(diǎn)；③用于確定某些應(yīng)該從搜索樹(shù)中拋棄或修剪的節(jié)點(diǎn)。主要的搜索算法為A*搜索算法。114廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：廣度優(yōu)先搜索算法（BreadthFirstSearch，BFS）目的是系統(tǒng)地展開(kāi)并檢查圖中的所有節(jié)點(diǎn)，以找尋結(jié)果。該算法并不考慮結(jié)果的可能位置，徹底地搜索整張圖，直到找到結(jié)果為止。1.算法原理依次從根節(jié)點(diǎn)開(kāi)始，逐層對(duì)節(jié)點(diǎn)進(jìn)行擴(kuò)展并考察該節(jié)點(diǎn)能否成為目標(biāo)節(jié)點(diǎn)，在第n層的節(jié)點(diǎn)沒(méi)有全部擴(kuò)展并考察結(jié)束之前，不擴(kuò)展第n+1層的節(jié)點(diǎn)。樹(shù)狀結(jié)構(gòu)的廣度優(yōu)先算法圖124廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：1.算法原理廣度優(yōu)先算法一般是依靠先進(jìn)先出的隊(duì)列或堆棧實(shí)現(xiàn)，將相鄰且未被搜索的節(jié)點(diǎn)放置在open容器中（隊(duì)列或者一維列表），被搜索過(guò)的節(jié)點(diǎn)放置在close容器中，通常稱(chēng)之為open-close表。具體的搜索規(guī)則如下：①?gòu)母?jié)點(diǎn)開(kāi)始，先將根節(jié)點(diǎn)壓入open隊(duì)列中，如圖所示：從根節(jié)點(diǎn)開(kāi)始134廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：1.算法原理②訪(fǎng)問(wèn)根節(jié)點(diǎn)相鄰的所有節(jié)點(diǎn)（A1、A2），并把相鄰的所有節(jié)點(diǎn)壓入open隊(duì)列中，將根節(jié)點(diǎn)從open表中刪除并將根節(jié)點(diǎn)壓入到close堆棧中，操作過(guò)程如圖所示。從A1節(jié)點(diǎn)開(kāi)始144廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：1.算法原理③將open隊(duì)列中當(dāng)前的隊(duì)首節(jié)點(diǎn)作為新的節(jié)點(diǎn)，開(kāi)始搜索與其相鄰的節(jié)點(diǎn)，重復(fù)上一步操作，操作過(guò)程如圖所示。從A2節(jié)點(diǎn)開(kāi)始154廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：從B1節(jié)點(diǎn)開(kāi)始從B2節(jié)點(diǎn)開(kāi)始164廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：從C2節(jié)點(diǎn)開(kāi)始從C1節(jié)點(diǎn)開(kāi)始174廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：1.算法原理④直到所有節(jié)點(diǎn)全部搜索完畢，open隊(duì)列中沒(méi)有節(jié)點(diǎn)，結(jié)束搜索，如圖所示。全部壓入close堆棧中184廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例將樹(shù)狀圖映在地圖上，以4×*4尺寸的地圖環(huán)境為例，演示BFS算法的逐步搜索過(guò)程。假設(shè)機(jī)器人存在多個(gè)前進(jìn)方向時(shí)的優(yōu)先順序?yàn)椋荷?gt;右>下>左（絕對(duì)方向），在該例子中定義open表為一個(gè)隊(duì)列，close表為一個(gè)堆棧?；蛘哂?和1兩種狀態(tài)分別表示該坐標(biāo)在open表或close表中，所有坐標(biāo)的標(biāo)志位事先初始化為0，表示未訪(fǎng)問(wèn)過(guò)或存在未訪(fǎng)問(wèn)相鄰坐標(biāo)。194廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例機(jī)器人每向前移動(dòng)一步記錄機(jī)器人的移動(dòng)步數(shù)，每當(dāng)機(jī)器人出現(xiàn)轉(zhuǎn)向時(shí)，機(jī)器人的移動(dòng)步數(shù)增加0.5，機(jī)器人的初始化與隊(duì)列的初始化如圖1所示。廣度優(yōu)先搜索示例示意圖1204廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例第1步，先將起始坐標(biāo)(0,0)加入open表；訪(fǎng)問(wèn)起始坐標(biāo)，檢測(cè)出當(dāng)前坐標(biāo)(0,0)有不在close表中的相鄰坐標(biāo)(0,1),將(0,1)加入open表，如圖2所示。廣度優(yōu)先搜索示例示意圖2214廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例第2步，機(jī)器人向前移動(dòng)一步，記錄移動(dòng)步數(shù)；將open表首位(0,0)壓入close表；訪(fǎng)問(wèn)新open表首位坐標(biāo)(0,1),檢測(cè)出當(dāng)前坐標(biāo)(0,1)有不在close表中的相鄰坐標(biāo)(0,2)和(1,1),將(0,2)和(1,1)加入open表，如圖3所示。廣度優(yōu)先搜索示例示意圖3224廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例第3步，先搜索第一個(gè)節(jié)點(diǎn)，記錄機(jī)器人移動(dòng)步數(shù)；將open表首位(0,1)壓入close表；訪(fǎng)問(wèn)新open表首位坐標(biāo)(0,2),檢測(cè)出當(dāng)前坐標(biāo)(0,2)有不在close表中的相鄰坐標(biāo)(0,3),將(0,3)加入open表，如圖4所示。廣度優(yōu)先搜索示例示意圖4234廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例第4步，繼續(xù)搜索第二個(gè)移動(dòng)節(jié)點(diǎn)，并記錄移動(dòng)步數(shù)，同時(shí)添加轉(zhuǎn)向數(shù)值；將open表首位(0,2)壓入close表；訪(fǎng)問(wèn)新open表首位坐標(biāo)(1,1),檢測(cè)出當(dāng)前坐標(biāo)(1,1)有不在close表中的相鄰坐標(biāo)(2,1),將(2,1)加入open表，如圖5所示。廣度優(yōu)先搜索示例示意圖5244廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例后續(xù)第5-10步同理第2-4步，如圖6到11不斷重復(fù)第2-4步之間的搜索操作。圖6圖7254廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：圖8圖9264廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：圖10圖11274廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例第11步：搜索終點(diǎn)坐標(biāo)(3,3)，可以停止搜索。最終根據(jù)起點(diǎn)到終點(diǎn)移動(dòng)數(shù)值，從終點(diǎn)到起點(diǎn)開(kāi)始反推出最短路徑。搜索過(guò)程與搜索結(jié)果如圖12與13所示。圖12284廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實(shí)例從終點(diǎn)到起點(diǎn)根據(jù)數(shù)值反推：8→6.5→5→3.5→2.5→1最終，從起點(diǎn)到終點(diǎn)的最短路徑坐標(biāo)順序?yàn)椋海?,0）→（0,1）→（1,1）→（2,1）→（2,2）→（3,2）→（3,3）圖13廣度優(yōu)先搜索示例結(jié)果295深度優(yōu)先搜索算法深度優(yōu)先搜索算法：深度優(yōu)先搜索算法（DepthFirstSearch，DFS）是與廣度優(yōu)先搜索算法具有同等地位的另一種盲目搜索算法?！耙粭l路走到黑“，更適合機(jī)器人對(duì)于未知領(lǐng)域的探索任務(wù)。1.算法原理在一條路上一直走下去，如果遇到分岔路口，就任意選擇其中一條繼續(xù)走下去，如果走到頭，就返回到上一個(gè)分岔路口走另外一條路，然后再一直走下去，直到搜索完全地圖后就停止搜索。深度優(yōu)先搜索算法示意圖30深度優(yōu)先搜索算法：1.算法原理深度優(yōu)先搜索算法用到計(jì)算機(jī)程序中的遞歸思想，搜索規(guī)則如下：（1）訪(fǎng)問(wèn)指定的根節(jié)點(diǎn)A，發(fā)現(xiàn)有三條分支路線(xiàn)，如圖所示。5深度優(yōu)先搜索算法從根節(jié)點(diǎn)開(kāi)始訪(fǎng)問(wèn)31深度優(yōu)先搜索算法：1.算法原理（2）隨機(jī)選擇一個(gè)節(jié)點(diǎn)（B1）訪(fǎng)問(wèn)，發(fā)現(xiàn)有兩條分支線(xiàn)路，如圖所示。5深度優(yōu)先搜索算法隨機(jī)選擇一個(gè)支路節(jié)點(diǎn)進(jìn)行訪(fǎng)問(wèn)32深度優(yōu)先搜索算法：1.算法原理（3）隨機(jī)選擇一個(gè)節(jié)點(diǎn)（C1）訪(fǎng)問(wèn)，發(fā)現(xiàn)有一條線(xiàn)路，繼續(xù)訪(fǎng)問(wèn)節(jié)點(diǎn)（C2），發(fā)現(xiàn)沒(méi)有路線(xiàn)。5深度優(yōu)先搜索算法將支線(xiàn)走到頭33深度優(yōu)先搜索算法：1.算法原理（4）返回最近的有分支線(xiàn)路的節(jié)點(diǎn)（B1）繼續(xù)訪(fǎng)問(wèn)另外一條線(xiàn)路的節(jié)點(diǎn)（C3）5深度優(yōu)先搜索算法返回最近的多直線(xiàn)節(jié)點(diǎn)懸著的另外一條線(xiàn)路34深度優(yōu)先搜索算法：1.算法原理（5）當(dāng)該路線(xiàn)訪(fǎng)問(wèn)完畢，沒(méi)有分支線(xiàn)路，重復(fù)上一步，直到與根節(jié)點(diǎn)相通的全部節(jié)點(diǎn)都訪(fǎng)問(wèn)完畢，結(jié)束搜索。這一重復(fù)搜索過(guò)程如下圖即可完成所有路線(xiàn)的搜索任務(wù)。5深度優(yōu)先搜索算法B1已經(jīng)沒(méi)有未訪(fǎng)問(wèn)的支線(xiàn)了，返回A任選一條未訪(fǎng)問(wèn)的節(jié)點(diǎn)35深度優(yōu)先搜索算法：5深度優(yōu)先搜索算法隨機(jī)選擇一個(gè)支路節(jié)點(diǎn)D1進(jìn)行訪(fǎng)問(wèn)，并訪(fǎng)問(wèn)到頭隨機(jī)選擇一個(gè)支線(xiàn)節(jié)點(diǎn)B2進(jìn)行訪(fǎng)問(wèn)36深度優(yōu)先搜索算法：5深度優(yōu)先搜索算法返回根節(jié)點(diǎn)并選擇另一條未訪(fǎng)問(wèn)的支線(xiàn)節(jié)點(diǎn)B3進(jìn)行訪(fǎng)問(wèn)返回最近的還有未訪(fǎng)問(wèn)支線(xiàn)的節(jié)點(diǎn)并選擇另一條支線(xiàn)訪(fǎng)問(wèn)37深度優(yōu)先搜索算法：5深度優(yōu)先搜索算法返回最近的還有未訪(fǎng)問(wèn)支線(xiàn)的節(jié)點(diǎn)并選擇另一條支線(xiàn)訪(fǎng)問(wèn)隨機(jī)選擇一條支線(xiàn)節(jié)點(diǎn)E1進(jìn)行訪(fǎng)問(wèn)38深度優(yōu)先搜索算法：2.算法實(shí)例同樣映射到一個(gè)4×4尺寸的地圖環(huán)境中，演示DFS算法的逐步行進(jìn)過(guò)程。假設(shè)機(jī)器人存在多個(gè)前進(jìn)方向時(shí)的優(yōu)先順序?yàn)椋河?gt;上>左>下（絕對(duì)方向），用0和1分別表示坐標(biāo)是否返回節(jié)點(diǎn)，所有坐標(biāo)的標(biāo)志位事先初始化為0，表示未訪(fǎng)問(wèn)過(guò)或存在未訪(fǎng)問(wèn)相鄰坐標(biāo)，初始化狀態(tài)如圖所示。從根節(jié)點(diǎn)開(kāi)始訪(fǎng)問(wèn)，發(fā)現(xiàn)有一個(gè)相鄰的節(jié)點(diǎn)5深度優(yōu)先搜索算法39深度優(yōu)先搜索算法：2.算法實(shí)例第1步，將(0,0)作為根節(jié)點(diǎn)，機(jī)器人從(0,0)開(kāi)始訪(fǎng)問(wèn)，發(fā)現(xiàn)有相鄰的節(jié)點(diǎn)(0,1)，將相鄰節(jié)點(diǎn)(0,1)添加到子節(jié)點(diǎn)中，如圖所示訪(fǎng)問(wèn)相鄰的子節(jié)點(diǎn)，發(fā)現(xiàn)兩個(gè)相鄰的子節(jié)點(diǎn)5深度優(yōu)先搜索算法40深度優(yōu)先搜索算法：2.算法實(shí)例第2步，機(jī)器人向前移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(0,1)，發(fā)現(xiàn)有兩個(gè)相鄰的節(jié)點(diǎn)(0,2)和(1,1)，將(0,2)和(1,1)添加到(0,1)子節(jié)點(diǎn)中，如圖所示。根據(jù)絕對(duì)方向優(yōu)先順序選擇子節(jié)點(diǎn)，發(fā)現(xiàn)兩個(gè)相鄰子節(jié)點(diǎn)5深度優(yōu)先搜索算法41深度優(yōu)先搜索算法：2.算法實(shí)例第3步，依照方向的優(yōu)先順序，機(jī)器人轉(zhuǎn)彎向右移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(1,1)，發(fā)現(xiàn)有兩個(gè)相鄰的節(jié)點(diǎn)(1,2)和(2,1)，將(1,2)和(2,1)添加到(1,1)的子節(jié)點(diǎn)中，如圖所示。依照優(yōu)先順序訪(fǎng)問(wèn)子節(jié)點(diǎn)，發(fā)現(xiàn)一個(gè)相鄰的子節(jié)點(diǎn)5深度優(yōu)先搜索算法42深度優(yōu)先搜索算法：2.算法實(shí)例第4步，依照方向的優(yōu)先順序，機(jī)器人繼續(xù)向右移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(2,1)，發(fā)現(xiàn)有一個(gè)相鄰的節(jié)點(diǎn)(2,0),將(2,0)添加到(2,1)的子節(jié)點(diǎn)中，如圖所示。訪(fǎng)問(wèn)子節(jié)點(diǎn)，發(fā)現(xiàn)有一個(gè)未訪(fǎng)問(wèn)的相鄰節(jié)點(diǎn)5深度優(yōu)先搜索算法43深度優(yōu)先搜索算法：2.算法實(shí)例第5步，機(jī)器人轉(zhuǎn)彎向右移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(2,0)，發(fā)現(xiàn)有一個(gè)相鄰的節(jié)點(diǎn)(3,0)，將(3,0)添加到(2,0)的子節(jié)點(diǎn)中，如圖所示。訪(fǎng)問(wèn)子節(jié)點(diǎn)，發(fā)現(xiàn)沒(méi)有未訪(fǎng)問(wèn)的子節(jié)點(diǎn)，標(biāo)記位置5深度優(yōu)先搜索算法44深度優(yōu)先搜索算法：2.算法實(shí)例第6步，機(jī)器人轉(zhuǎn)彎向右移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(3,0)，未發(fā)現(xiàn)相鄰的坐標(biāo)節(jié)點(diǎn)，標(biāo)記位置，如圖所示。返回上一坐標(biāo)節(jié)點(diǎn)(2,0)未發(fā)現(xiàn)相鄰節(jié)點(diǎn)，標(biāo)記位置5深度優(yōu)先搜索算法45深度優(yōu)先搜索算法：2.算法實(shí)例第7步，機(jī)器人掉頭向左移動(dòng)，返回上一坐標(biāo)節(jié)點(diǎn)(2,0)，未發(fā)現(xiàn)未訪(fǎng)問(wèn)的相鄰坐標(biāo)節(jié)點(diǎn)，標(biāo)記位置，如圖所示。返回上一節(jié)點(diǎn)(2,1)，發(fā)現(xiàn)沒(méi)有未訪(fǎng)問(wèn)的子節(jié)點(diǎn)，標(biāo)記位置5深度優(yōu)先搜索算法46深度優(yōu)先搜索算法：2.算法實(shí)例第8步，機(jī)器人轉(zhuǎn)彎向上移動(dòng)，返回上一坐標(biāo)節(jié)點(diǎn)(2,1)，未發(fā)現(xiàn)未訪(fǎng)問(wèn)的相鄰坐標(biāo)節(jié)點(diǎn)，標(biāo)記位置，如圖所示。返回上一節(jié)點(diǎn)(1,1)，發(fā)現(xiàn)有未訪(fǎng)問(wèn)的子節(jié)點(diǎn)5深度優(yōu)先搜索算法47深度優(yōu)先搜索算法：2.算法實(shí)例第9步，機(jī)器人轉(zhuǎn)彎向左移動(dòng)，返回上一坐標(biāo)節(jié)點(diǎn)(1,1)，還有一個(gè)相鄰坐標(biāo)節(jié)點(diǎn)未訪(fǎng)問(wèn)，不做操作，如圖所示。選擇未訪(fǎng)問(wèn)的相鄰子節(jié)點(diǎn)，發(fā)現(xiàn)有兩個(gè)相鄰子節(jié)點(diǎn)5深度優(yōu)先搜索算法48深度優(yōu)先搜索算法：2.算法實(shí)例第10步，機(jī)器人轉(zhuǎn)彎向上移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(1,2)，發(fā)現(xiàn)有兩個(gè)相鄰的坐標(biāo)節(jié)點(diǎn)(1,3)和(0,2)，將(1,3)和(0,2)添加到(1,2)的子節(jié)點(diǎn)中，如圖所示。任意選擇一個(gè)相鄰子節(jié)點(diǎn)進(jìn)行訪(fǎng)問(wèn)，發(fā)現(xiàn)兩個(gè)相鄰子節(jié)點(diǎn)5深度優(yōu)先搜索算法49深度優(yōu)先搜索算法：2.算法實(shí)例第11步，依照方向的優(yōu)先順序，機(jī)器人向上移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(1,3)，發(fā)現(xiàn)有兩個(gè)相鄰的坐標(biāo)節(jié)點(diǎn)(0,3)和(2,3),將(0,3)和(2,3)添加到(1,3)的子節(jié)點(diǎn)中，如圖所示。訪(fǎng)問(wèn)相鄰子節(jié)點(diǎn)，發(fā)現(xiàn)兩個(gè)相鄰子節(jié)點(diǎn)5深度優(yōu)先搜索算法50深度優(yōu)先搜索算法：2.算法實(shí)例第12步，依照方向的優(yōu)先順序，機(jī)器人轉(zhuǎn)彎向右移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(2,3)，發(fā)現(xiàn)有一個(gè)相鄰的坐標(biāo)節(jié)點(diǎn)(3,3),將(3,3)添加到(2,3)的子節(jié)點(diǎn)中，如圖所示。訪(fǎng)問(wèn)子節(jié)點(diǎn)，發(fā)現(xiàn)一個(gè)相鄰子節(jié)點(diǎn)，標(biāo)記終點(diǎn)位置5深度優(yōu)先搜索算法51深度優(yōu)先搜索算法：2.算法實(shí)例第13步，機(jī)器人繼續(xù)向右移動(dòng)，訪(fǎng)問(wèn)坐標(biāo)節(jié)點(diǎn)(3,3)，找到終點(diǎn)，標(biāo)記位置；若只是為了求取一條起點(diǎn)到終點(diǎn)的可行路徑，程序到此就結(jié)束了。從終點(diǎn)開(kāi)始反向記錄機(jī)器人的移動(dòng)步數(shù)，直到到達(dá)起點(diǎn)的位置，就會(huì)得出一條從終點(diǎn)到起點(diǎn)的路徑，求解起點(diǎn)到終點(diǎn)的可行性路線(xiàn)如圖所示。5深度優(yōu)先搜索算法52深度優(yōu)先搜索算法：2.算法實(shí)例從(3,3)開(kāi)始找其上一節(jié)點(diǎn)直至根節(jié)點(diǎn)(0,0)，可以得到一個(gè)可行解：(3,3)→(2,3)→(1,3)→(1,2)→(1,1)→(0,1)→(0,0)5深度優(yōu)先搜索算法53深度優(yōu)先搜索算法：2.算法實(shí)例但如果要搜索全圖確定全局最優(yōu)解，程序需要繼續(xù)執(zhí)行：坐標(biāo)(3,3)有未訪(fǎng)問(wèn)過(guò)的相鄰坐標(biāo)，將(3,2)添加到(3,3)的子節(jié)點(diǎn)中，如圖所示。訪(fǎng)問(wèn)終點(diǎn)(3,3)，發(fā)現(xiàn)有一個(gè)相鄰坐標(biāo)5深度優(yōu)先搜索算法54深度優(yōu)先搜索算法：2.算法實(shí)例后續(xù)若干步驟同理第6-10步的過(guò)程，最后所有可行的節(jié)點(diǎn)都被訪(fǎng)問(wèn)，標(biāo)志位都被置1，機(jī)器人也正好返回起點(diǎn)(0,0)，全圖搜索完成。針對(duì)全圖其余節(jié)點(diǎn)的搜索過(guò)程如教材配圖9-50到9-62所示，按順序方式閱讀教材P107即可查看深度優(yōu)先搜索算法的搜索路徑。5深度優(yōu)先搜索算法556A*搜索算法A*搜索算法：深度優(yōu)先算法可以快速搜索到一條能到達(dá)目的地的路徑，但是無(wú)法保證得到的這一條路徑是最短路徑；廣度優(yōu)先算法可以保證搜索到一條最短路徑但搜索所用到的時(shí)間會(huì)比較長(zhǎng)。A*(A-star)算法結(jié)合了二者優(yōu)點(diǎn)的算法，是靜態(tài)網(wǎng)路求解最短路徑最有效的直接搜索算法?？梢哉f(shuō)DFS是A*算法效率最低的一個(gè)特例，BFS是依次展開(kāi)每一層坐標(biāo)（或者說(shuō)是等高圖中同一數(shù)值的坐標(biāo)）進(jìn)行搜索。如果對(duì)于每層坐標(biāo)只選定一個(gè)方向去搜索它的下一層坐標(biāo)，而非依次搜索所有可到達(dá)的下一層坐標(biāo)的話(huà)，就可以大大節(jié)省計(jì)算時(shí)間。A*算法也是許多其他問(wèn)題的常用啟發(fā)式算法。566A*搜索算法A*搜索算法：1.算法原理A*算法給出了一種估值函數(shù)，給每個(gè)坐標(biāo)附上一個(gè)估值函數(shù)，用于下一步被訪(fǎng)問(wèn)坐標(biāo)的價(jià)值。估值函數(shù)為：F(n)=G(n)+H(n)G(n)表示從起始點(diǎn)坐標(biāo)到節(jié)點(diǎn)坐標(biāo)的距離；H(n)表示從該節(jié)點(diǎn)坐標(biāo)到終點(diǎn)坐標(biāo)的曼哈頓距離；F(n)表示從起始點(diǎn)坐標(biāo)經(jīng)由節(jié)點(diǎn)坐標(biāo)到終點(diǎn)坐標(biāo)的最小代價(jià)估計(jì)；無(wú)論地圖信息未知還是已知，終點(diǎn)坐標(biāo)我們總是可以確定的，由此可以采取多種方法來(lái)擬定估計(jì)值H(n)，如采用曼哈頓距離（城市街區(qū)距離）、對(duì)角線(xiàn)距離、歐幾里得距離、平方后歐式距離等，這里不做詳細(xì)介紹。總得來(lái)說(shuō)，就是利用終點(diǎn)坐標(biāo)和當(dāng)前坐標(biāo)的差距擬合出終點(diǎn)距離當(dāng)前點(diǎn)的偏好方向。572.算法實(shí)例第一步，當(dāng)機(jī)器人位于坐標(biāo)（0，0）時(shí)，計(jì)算起點(diǎn)坐標(biāo)的估計(jì)值：將該坐標(biāo)距離起點(diǎn)的距離記為G_((0,0))=1，計(jì)算起點(diǎn)(0,0)與終點(diǎn)（4，4）的曼哈頓距離為H_((0,0))=|0-4|+|0-4|=8，則其估值函數(shù)：F_((0,0))=G_((0,0))+H_((0,0))=96A*搜索算法A*搜索算法：582.算法實(shí)例第二步，可移動(dòng)的方向僅有絕對(duì)向上的方向（以下描述均為絕對(duì)方向），機(jī)器人向上移動(dòng)一步，G(0,1)=G(0,0)+1,計(jì)算節(jié)點(diǎn)(0,1)與終點(diǎn)（4，4）的曼哈頓距離為H_((0,1))=|0-4|+|1-4|=7，估值函數(shù)：6A*搜索算法A*搜索算法：F_((0,1))=G_((0,1))+H_((0,1))=(G_((0,0))+1)+(4+3)=9592.算法實(shí)例第三步，可移動(dòng)的方向僅有絕對(duì)向上的方向（以下描述均為絕對(duì)方向），機(jī)器人向上移動(dòng)一步，G(0,2)=G(0,1)+1,計(jì)算節(jié)點(diǎn)(0,2)與終點(diǎn)（4，4）的曼哈頓距離為H_((0,2))=|0-4|+|2-4|=6，估值函數(shù)：6A*搜索算法A*搜索算法：F_((0,2))=G_((0,2))+H_((0,2))=(G_((0,1))+1)+(4+2)=9602.算法實(shí)例第四步，可移動(dòng)的方向僅有絕對(duì)向右的方向（以下描述均為絕對(duì)方向），機(jī)器人向右移動(dòng)一步，G(1,2)=G(0,2)+1,計(jì)算節(jié)點(diǎn)(1,2)與終點(diǎn)（4，4）的曼哈頓距離為H_((1,2))=|1-4|+|2-4|=5，估值函數(shù)：6A*搜索算法A*搜索算法：F_((1,2))=G_((1,2))+H_((1,2))=(G_((0,2))+1)+(3+2)=9612.算法實(shí)例第五步，可移動(dòng)方向有上和右兩個(gè)方向，分別是（1,3）和（2,2），需要同時(shí)計(jì)算坐標(biāo)(1,3)和(2,2)的估值函數(shù)：①機(jī)器人向右移動(dòng)一步，G(2,2)=G(1,2)+1,計(jì)算節(jié)點(diǎn)(2,2)與終點(diǎn)（4，4）的曼哈頓距離為H_((2,2))=|2-4|+|2-4|=4，估值函數(shù)：F_((2,2))=G_((2,2))+H_((2,2))=(G_((1,2))+1)+(2+2)=9②機(jī)器人先轉(zhuǎn)彎然后向上移動(dòng)一步，G(1,3)=G(1,2)+1,計(jì)算節(jié)點(diǎn)(1,3)與終點(diǎn)（4，4）的曼哈頓距離為H_((1,3))=|1-4|+|3-4|=4，估值函數(shù)：F_((1,3))=G_((1,3))+H_((1,3))=(G_((10,2))+1)+(3+1)=96A*搜索算法A*搜索算法：626A*搜索算法A*搜索算法：F_((2,2))=G_((2,2))+H_((2,2))=(G_((1,2))+1)+(2+2)=9F_((1,3))=G_((1,3))+H_((1,3))=(G_((10,2))+1)+(3+1)=9632.算法實(shí)例在實(shí)際應(yīng)用時(shí)可以引入轉(zhuǎn)彎參數(shù)t，使得:H_((n))=終點(diǎn)與當(dāng)前位置的曼哈頓距離+t_((n))由于向上移動(dòng)需要先轉(zhuǎn)彎再移動(dòng)，所以需要增加轉(zhuǎn)向t，假設(shè)轉(zhuǎn)向權(quán)值為0.3重新計(jì)算坐標(biāo)(1,3)的估值函數(shù)。6A*搜索算法A*搜索算法：642.算法實(shí)例機(jī)器人先轉(zhuǎn)彎然后向上移動(dòng)一步，G(1,3)=G(1,2)+1,計(jì)算節(jié)點(diǎn)與終點(diǎn)（4，4）的曼哈頓距離為H_((1,3))=|1-4|+|3-4|=4，估值函數(shù)：F_((1,3))=G_((1,3))+H_((1,3))+t_((1,3))=(G_1,2+1)+(3+1)+0.3=9.3因?yàn)镕_((1,3))>F_((2,2))，即向上走比向右走要付出更大的代價(jià)，所以機(jī)器人優(yōu)先選擇向右走。6A*搜索算法A*搜索算法：652.算法實(shí)例第六步，機(jī)器人向右移動(dòng)一步，現(xiàn)有向上和向下兩個(gè)方向可移動(dòng)，需要計(jì)算坐標(biāo)(2,3)和(2,1)的估值函數(shù)。①機(jī)器人轉(zhuǎn)彎向上移動(dòng)一步，移動(dòng)到(2,3)，G(2,3)=G(2,2)+1；計(jì)算(2,3)與終點(diǎn)(4,4)的曼哈頓距離H_((2,3))=2+1=3；則估值函數(shù)：F_((2,3))=G_((2,3))+H_((2,3))+t_((2,3))=(G_((2,2))+1)+3+0.3=9.3②機(jī)器人轉(zhuǎn)彎向下移動(dòng)一步，移動(dòng)到(2,1)，G(2,1)=G(2,2)+1；計(jì)算(2,1)與終點(diǎn)(4,4)的曼哈頓距離H_((2,1))=2+3=5；則估值函數(shù)：F_((2,1))=G_((2,1))+H_((2,1))+t_((2,1))=(G_((2,2))+1)+(2+3)+0.3=11.36A*搜索算法A*搜索算法：666A*搜索算法F_((2,1))=G_((2,1))+H_((2,1))+t_((2,1))=(G_((2,2))+1)+(2+3)+0.3=11.3F_((2,3))=G_((2,3))+H_((2,3))+t_((2,3))=(G_((2,2))+1)+3+0.3=9.3672.算法實(shí)例第七步，繼續(xù)計(jì)算未訪(fǎng)問(wèn)節(jié)點(diǎn)的估值函數(shù)。機(jī)器人優(yōu)先向上移動(dòng)一步，現(xiàn)有向上和向右兩個(gè)方向可移動(dòng)，需要計(jì)算坐標(biāo)(2,4)和(3,3)的估值函數(shù)。：①機(jī)器人繼續(xù)向上移動(dòng)一步，G(2,4)=G(2,3)+1；計(jì)算(2,4)與終點(diǎn)(4,4)的曼哈頓距離H_((2,4))=2+0=2；則估值函數(shù)：F_((2,4))=G_((2,4))+H_((2,4))=(G_((2,3))+1)+2=9②機(jī)器人轉(zhuǎn)彎向右移動(dòng)一步，G(3,3)=G(2,3)+1；計(jì)算(3,3)與終點(diǎn)(4,4)的曼哈頓距離H_((3,3))=1+1=2，則估值函數(shù)：F_((3,3))=G_((3,3))+H_((3,3))+t_((3,3))=(G_((2,3))+1)+2+0.3=9.36A*搜索算法A*搜索算法：686A*搜索算法F_((3,3))=G_((3,3))+H_((3,3))+t_((3,3))=(G_((2,3))+1)+2+0.3=9.3F_((2,4))=G_((2,4))+H_((2,4))=(G_((2,3))+1)+2=9692.算法實(shí)例第八步，機(jī)器人優(yōu)先向上移動(dòng)一步，發(fā)現(xiàn)去終點(diǎn)方向的道路有障礙物，無(wú)法通過(guò)，因此機(jī)器人回到之前的位置，向右移動(dòng)一步，需要計(jì)算坐標(biāo)(4,3)的估值函數(shù)。機(jī)器人繼續(xù)向右移動(dòng)一步，G(4,3)=G(3,3)+1；計(jì)算(4,3)與終點(diǎn)(4,4)的曼哈頓距離H_((4,3))=0+1=1；則估值函數(shù)：6A*搜索算法A*搜索算法：F_((4,3))=G_((4,3))+H_((4,3))=(G_((3,3))+1)+1=9702.算法實(shí)例第九步，有向上和向下兩個(gè)方向可移動(dòng)，機(jī)器人向上移動(dòng)一步，到達(dá)終點(diǎn)；根據(jù)計(jì)算出來(lái)的每個(gè)坐標(biāo)的估值函數(shù)，從終點(diǎn)開(kāi)始進(jìn)行回溯，找到最短路徑：6A*搜索算法A*搜索算法：(4,4)→(4,3)→(3,3)→(2,3)→(2,2)→(1,2)→(0,2)→(0,1)→(0,0)歡迎探討！第10章多機(jī)器人路徑規(guī)劃智慧物流系統(tǒng)：從設(shè)計(jì)到實(shí)現(xiàn)教學(xué)內(nèi)容CONTENTS3應(yīng)用場(chǎng)景2多機(jī)器人路徑規(guī)劃問(wèn)題4常見(jiàn)沖突5解決策略6D*算法1多機(jī)器人路徑規(guī)劃7協(xié)同調(diào)度74

章節(jié)目標(biāo)了解如何為多個(gè)機(jī)器人規(guī)劃路徑；了解多條路徑可能會(huì)出現(xiàn)沖突和解決沖突的策略；掌握給多個(gè)機(jī)器人進(jìn)行路徑規(guī)劃的算法原理。75多機(jī)器人路徑規(guī)劃

多機(jī)器人路徑規(guī)劃，顧名思義，就是給多個(gè)機(jī)器人規(guī)劃從起點(diǎn)到終點(diǎn)的最優(yōu)的、無(wú)碰撞障礙物的多條路徑。1多機(jī)器人路徑規(guī)劃單機(jī)器人路徑規(guī)劃多機(jī)器人路徑規(guī)劃76多機(jī)器人路徑規(guī)劃在同一工作環(huán)境下，存在著同一時(shí)間多個(gè)同時(shí)作業(yè)的移動(dòng)機(jī)器人，并且要保證機(jī)器人彼此之間在任意時(shí)刻都保持安全距離，不發(fā)生碰撞。在以機(jī)器人與工作環(huán)境中障礙物不發(fā)生碰撞的要求條件下，需要各機(jī)器人在起點(diǎn)與終點(diǎn)之間合理規(guī)劃出每個(gè)機(jī)器人的最優(yōu)無(wú)碰撞的安全路徑。2多機(jī)器人路徑規(guī)劃問(wèn)題772多機(jī)器人路徑規(guī)劃問(wèn)題多機(jī)器人路徑規(guī)劃主要涉及四個(gè)方面：

環(huán)境建模、碰撞檢測(cè)、啟發(fā)式規(guī)則設(shè)計(jì)、路徑規(guī)劃算法(1)環(huán)境建模：將移動(dòng)機(jī)器人的工作環(huán)境通過(guò)一定的數(shù)據(jù)結(jié)構(gòu)傳到計(jì)算機(jī)上，并將環(huán)境信息準(zhǔn)確地呈現(xiàn)出來(lái)。(2)碰撞檢測(cè)：指判斷機(jī)器人與環(huán)境中的障礙物或兩機(jī)器人間是否會(huì)發(fā)生碰撞以及確定碰撞發(fā)生時(shí)機(jī)器人所處的位置。(3)啟發(fā)式規(guī)則：指如何更好地消解移動(dòng)機(jī)器人間的碰撞沖突，實(shí)現(xiàn)多移動(dòng)機(jī)器人協(xié)同作業(yè)。(4)路徑規(guī)劃算法：指如何在當(dāng)前工作環(huán)境中依據(jù)路徑規(guī)劃的相應(yīng)要求快速地為移動(dòng)機(jī)器人尋找到可避碰避障的最優(yōu)或較優(yōu)的可行路徑。783應(yīng)用場(chǎng)景多機(jī)器人路徑規(guī)劃：假設(shè)多個(gè)機(jī)器人在二維平面環(huán)境G中運(yùn)行，環(huán)境中存在著若干靜態(tài)障礙物，障礙物位置信息已知。機(jī)器人R1和R2的起點(diǎn)位置S1和S2和目標(biāo)點(diǎn)E1和E2已知。多機(jī)器人路徑規(guī)劃時(shí)，會(huì)在不考慮其他機(jī)器人的基礎(chǔ)上，先規(guī)劃一條Ri從起點(diǎn)Si到終點(diǎn)Ei之間的無(wú)碰障礙的最優(yōu)路徑，然后在機(jī)器人的行進(jìn)過(guò)程中再去考慮機(jī)器人的碰撞沖突問(wèn)題。!!!!!!0123401234S1S2E1E2794常見(jiàn)沖突常見(jiàn)的碰撞沖突：多機(jī)器人系統(tǒng)不僅是機(jī)器人的數(shù)量增多，且各機(jī)器人都有自己的目標(biāo)點(diǎn)，在按原始路徑運(yùn)行時(shí)極易發(fā)生碰撞沖突。在該情形下，路徑規(guī)劃不僅要考慮到避障問(wèn)題還要考慮到機(jī)器人間的避碰問(wèn)題。多機(jī)器人在運(yùn)行時(shí)常見(jiàn)的幾種沖突：(1)在地圖環(huán)境G中，假設(shè)機(jī)器人R1和機(jī)器人R2在t時(shí)刻和t+1時(shí)刻的路徑點(diǎn)重合，就會(huì)出現(xiàn)兩臺(tái)物流機(jī)器人面對(duì)面發(fā)生碰撞的現(xiàn)象；01230123R2R180常見(jiàn)的碰撞沖突：(2)在地圖環(huán)境G中，假設(shè)機(jī)器人R1、R2在t時(shí)刻的路徑點(diǎn)重合，就會(huì)發(fā)生碰撞，碰撞情況有兩種情況：01230123R2R1情況一01230123R2情況二R1!!4常見(jiàn)沖突81常見(jiàn)的碰撞沖突：(3)在地圖環(huán)境G中，機(jī)器人R1、R2在進(jìn)入槽位時(shí)，路徑點(diǎn)重合,面對(duì)面相遇會(huì)發(fā)生碰撞：01230123R1R2!!!4常見(jiàn)沖突825解決策略常見(jiàn)碰撞沖突的解決策略：明確機(jī)器人之間的優(yōu)先級(jí)，優(yōu)先級(jí)高的機(jī)器人按照原路徑前進(jìn)，優(yōu)先級(jí)低的機(jī)器人重新規(guī)劃路徑。假設(shè)機(jī)器人R1優(yōu)先級(jí)高于機(jī)器人R2的優(yōu)先級(jí)。01230123R2R1(1)t時(shí)刻、t+1時(shí)刻路徑點(diǎn)重合解決策略：根據(jù)機(jī)器人的優(yōu)先級(jí)進(jìn)行判斷，讓機(jī)器人R1在原地等待并按照原路徑前進(jìn)，給機(jī)器人R2重新規(guī)劃前進(jìn)路線(xiàn)(實(shí)際位置到目標(biāo)點(diǎn))83常見(jiàn)碰撞沖突的解決策略：(1)根據(jù)機(jī)器人的優(yōu)先級(jí)進(jìn)行判斷，讓機(jī)器人R1在原地等待并按照原路徑前進(jìn)，給機(jī)器人R2重新規(guī)劃前進(jìn)路線(xiàn)(實(shí)際位置到目標(biāo)點(diǎn)位置)01230123R2R101230123R2R1R25解決策略84常見(jiàn)碰撞沖突的解決策略：(2)情況一：兩個(gè)機(jī)器人直角相遇，這時(shí)如果利用物流機(jī)器人的優(yōu)先級(jí)來(lái)控制機(jī)器人移動(dòng)可能會(huì)發(fā)生剮蹭、碰撞。因此，需要通過(guò)兩個(gè)機(jī)器人的位移來(lái)決定哪個(gè)機(jī)器人需要重新規(guī)劃路線(xiàn)，距離重合點(diǎn)越遠(yuǎn)的機(jī)器人重新規(guī)劃路線(xiàn)。01230123R2R1根據(jù)機(jī)器人的位移距離進(jìn)行判斷，機(jī)器人R1重新規(guī)劃路線(xiàn)（實(shí)際位置到目標(biāo)點(diǎn)），機(jī)器人R2等待并按照原路線(xiàn)前進(jìn)。5解決策略85常見(jiàn)碰撞沖突的解決策略：(2)情況一：根據(jù)機(jī)器人的位移距離進(jìn)行判斷，機(jī)器人R1重新規(guī)劃路線(xiàn)（實(shí)際位置到目標(biāo)點(diǎn)），機(jī)器人R2等待并按照原路線(xiàn)前進(jìn)。01230123R2R101230123R2R1R15解決策略86常見(jiàn)碰撞沖突的解決策略：(2)情況二：當(dāng)機(jī)器人R2只能前進(jìn)時(shí)與機(jī)器人R1的路徑點(diǎn)重合，按照優(yōu)先級(jí)控制機(jī)器人移動(dòng)，由于機(jī)器人R2只能前進(jìn)，無(wú)法規(guī)劃出其他路線(xiàn)，只能由機(jī)器人R1重新規(guī)劃路徑。根據(jù)機(jī)器人的前進(jìn)方向進(jìn)行判斷，機(jī)器人R1重新規(guī)劃路線(xiàn)（實(shí)際位置到目標(biāo)點(diǎn)），機(jī)器人R2等待并按照原路線(xiàn)前進(jìn)。01230123R2R1!!5解決策略87常見(jiàn)碰撞沖突的解決策略：(2)情況二：根據(jù)機(jī)器人的前進(jìn)方向進(jìn)行判斷，機(jī)器人R1重新規(guī)劃路線(xiàn)（實(shí)際位置到目標(biāo)點(diǎn)），機(jī)器人R2等待并按照原路線(xiàn)前進(jìn)。01230123R2R1!!01230123R2R1!!R15解決策略88常見(jiàn)碰撞沖突的解決策略：(3)當(dāng)機(jī)器人R1、R2進(jìn)入槽位時(shí)，只能從前方進(jìn)入，兩個(gè)機(jī)器人面對(duì)面相遇，同樣要是用優(yōu)先級(jí)來(lái)控制機(jī)器人移動(dòng)根據(jù)機(jī)器人的優(yōu)先級(jí)進(jìn)行判斷，機(jī)器人R1等待并按照原來(lái)路徑前進(jìn)，讓機(jī)器人R2重新規(guī)劃路線(xiàn)(實(shí)際位置到目標(biāo)點(diǎn))。01230123R1R2!!!5解決策略89常見(jiàn)碰撞沖突的解決策略：(3)根據(jù)機(jī)器人的優(yōu)先級(jí)進(jìn)行判斷，機(jī)器人R1等待并按照原來(lái)路徑前進(jìn)，讓機(jī)器人R2重新規(guī)劃路線(xiàn)(實(shí)際位置到目標(biāo)點(diǎn))。01230123R1R2!!!01230123R1R2!!!R25解決策略906動(dòng)態(tài)路徑規(guī)劃利用上述的這些策略，可以有效的避免機(jī)器人之間的碰撞，但是使用這些策略，需要考慮很多種情況，且需要一一的列舉出來(lái)，然后再進(jìn)行解決。這樣思考：在一個(gè)環(huán)境模型中，有多個(gè)可移動(dòng)的機(jī)器人，在機(jī)器人在移動(dòng)過(guò)程中，把突然出現(xiàn)的機(jī)器人作為移動(dòng)的障礙，這樣我們便可以給該機(jī)器人重新規(guī)劃路徑或者等待移動(dòng)障礙離開(kāi)原路徑繼續(xù)移動(dòng)。在環(huán)境模型中出現(xiàn)可移動(dòng)障礙時(shí)，給規(guī)劃路徑就要使用動(dòng)態(tài)路徑規(guī)劃，動(dòng)態(tài)路徑規(guī)劃的算法是D*算法。916動(dòng)態(tài)路徑規(guī)劃926動(dòng)態(tài)路徑規(guī)劃算法-D*算法D*(D-star)算法：

D*（D-Star,DynamicAStar）算法是典型的動(dòng)態(tài)網(wǎng)路求解最短路徑算法，用于計(jì)算一個(gè)節(jié)點(diǎn)到其他所有節(jié)點(diǎn)的最短路徑，也是給多機(jī)器人動(dòng)態(tài)規(guī)劃路徑的算法，當(dāng)遇到其他機(jī)器人時(shí)，會(huì)將其他機(jī)器人當(dāng)做障礙物，重新規(guī)劃路徑。主要特點(diǎn)是一起點(diǎn)為中心向外層層擴(kuò)展，直到擴(kuò)展到終點(diǎn)位置。D*算法的本質(zhì)就是A*算法，能得出最短路徑的最優(yōu)解。93D*(D-star)算法原理：首先，在不考慮移動(dòng)障礙的情況下，使用A*算法給每個(gè)機(jī)器人規(guī)劃出一條起點(diǎn)距離終點(diǎn)的最優(yōu)路徑；其次，獲取機(jī)器人某時(shí)刻移動(dòng)障礙物(其他機(jī)器人)的位置，與已經(jīng)規(guī)劃好的路徑進(jìn)行碰撞監(jiān)測(cè)；

①如果不發(fā)生沖突時(shí)，機(jī)器人正常在最優(yōu)路徑上移動(dòng)；

②如果發(fā)生沖突時(shí)，其中一臺(tái)機(jī)器人將另外一臺(tái)機(jī)器人設(shè)置為障礙；

當(dāng)i步發(fā)現(xiàn)沖突，將i,i+1步設(shè)置為障礙，從i-1步重新規(guī)劃當(dāng)前坐標(biāo)到終點(diǎn)的最優(yōu)路徑；(3)從i-1步重新規(guī)劃后，將起點(diǎn)到i-1步的路徑點(diǎn)和i-1步到終點(diǎn)的路徑點(diǎn)拼接，

得到從起點(diǎn)到終點(diǎn)避碰移動(dòng)障礙物的最優(yōu)路徑；6動(dòng)態(tài)路徑規(guī)劃算法-D*算法94D*算法

01230123終點(diǎn)以4*4的地圖環(huán)境為例，規(guī)劃出兩個(gè)機(jī)器人R1、R2從起點(diǎn)到終點(diǎn)的路徑，假設(shè)機(jī)器人R1的優(yōu)先級(jí)高于機(jī)器人R2的優(yōu)先級(jí)，演示D*算法的過(guò)程：終點(diǎn)R1R2(1)先在不考慮其他機(jī)器人的情況下，使用A*算法規(guī)劃從起點(diǎn)到終點(diǎn)的最短路徑6動(dòng)態(tài)路徑規(guī)劃算法-D*算法01230123終點(diǎn)終點(diǎn)R1R295D*算法

(2)其次，獲取機(jī)器人某時(shí)刻移動(dòng)障礙物(其他機(jī)器人)的位置，與已經(jīng)規(guī)劃好的路徑進(jìn)行碰撞檢測(cè)。12345124在i時(shí)刻(第二步)會(huì)發(fā)生碰撞，機(jī)器人R1的優(yōu)先級(jí)高于機(jī)器人R2，由機(jī)器人R2在i-1時(shí)刻(第一步)重新使用A*算法規(guī)劃路徑36動(dòng)態(tài)路徑規(guī)劃算法-D*算法01230123終點(diǎn)終點(diǎn)R1R2966D*算法-算法實(shí)例D*算法

(3)機(jī)器人R2在i-1時(shí)刻(第一步)重新規(guī)劃路徑機(jī)器人R1按照最初的路徑執(zhí)行機(jī)器人R2最終路徑：起點(diǎn)~i-1+i-1~終點(diǎn)拼接路徑就會(huì)多一步i-1,拼接的路徑需要減去一個(gè)i-1步的路徑點(diǎn)977協(xié)同調(diào)度協(xié)同調(diào)度：多機(jī)器人系統(tǒng)在各個(gè)領(lǐng)域已經(jīng)有了廣泛的應(yīng)用。隨著移動(dòng)機(jī)器人數(shù)量和機(jī)器人任務(wù)的增加，怎樣合理的調(diào)度機(jī)器人工作，避免發(fā)生碰撞和閑置，對(duì)提高機(jī)器人人的利用率具有重要意義。協(xié)同調(diào)度是一種為了最大化利用機(jī)器人完成任務(wù)，有效的機(jī)器人調(diào)度機(jī)制可以幫助我們尋找能夠最小化完工時(shí)間的最佳調(diào)度方案。主要任務(wù)：多機(jī)器人協(xié)同調(diào)度旨在從系統(tǒng)的角度減少資源浪費(fèi)，減小沖突概率，保證總體最優(yōu)性。多機(jī)器人系統(tǒng)協(xié)同調(diào)度主要表現(xiàn)為任務(wù)分配問(wèn)題。主要的解決方法有基于行為的分配方法、市場(chǎng)機(jī)制方法、群體智能方法、線(xiàn)性規(guī)劃方法等。98評(píng)價(jià)方法：科學(xué)家們根據(jù)自然界中的群體智能研究出了多種仿生算法，并通過(guò)仿生算法將群體智能應(yīng)用到多機(jī)器人系統(tǒng)的協(xié)同調(diào)度中，用來(lái)解決協(xié)同調(diào)度的任務(wù)分配問(wèn)題，如遺傳算法、粒子群算法、蟻群算法等。遺傳算法：主要是模擬生物的遺傳和進(jìn)化機(jī)制，實(shí)現(xiàn)對(duì)于復(fù)雜系統(tǒng)的自適應(yīng)優(yōu)化，它主要包括三個(gè)核心進(jìn)程：選擇、交配、變異。它具有全局搜索能力強(qiáng)、魯棒性強(qiáng)、靈活性可擴(kuò)展性強(qiáng)、并行計(jì)算能力強(qiáng)的優(yōu)點(diǎn)。7協(xié)同調(diào)度-評(píng)價(jià)方法99評(píng)價(jià)方法：科學(xué)家們根據(jù)自然界中的群體智能研究出了多種仿生算法，并通過(guò)仿生算法將群體智能應(yīng)用到多機(jī)器人系統(tǒng)的協(xié)同調(diào)度中，來(lái)解決協(xié)同調(diào)度的任務(wù)分配問(wèn)題，如遺傳算法、粒子群算法、蟻群算法等。粒子群算法：主要是模擬鳥(niǎo)類(lèi)覓食機(jī)制，將鳥(niǎo)作為粒子，鳥(niǎo)群作為粒子群，每個(gè)粒子在行進(jìn)的過(guò)程中將自身的信息分享給同伴，它是一種更高效的并行搜索算法，非常適用于對(duì)復(fù)雜環(huán)境中的優(yōu)化問(wèn)題的求解。蟻群算法：模擬螞蟻覓食的過(guò)程，當(dāng)螞蟻發(fā)現(xiàn)食物時(shí)，會(huì)對(duì)食物進(jìn)行標(biāo)記，然后在走過(guò)的路上留下信息素，根據(jù)路上留下信息素的濃度來(lái)選擇路線(xiàn)。蟻群算法利用了正反饋機(jī)制，結(jié)合概率算法使得較短的路徑能夠有較大的機(jī)會(huì)得到選擇，所以蟻群算法不局限于局部最優(yōu)解。7協(xié)同調(diào)度-評(píng)價(jià)方法1007協(xié)同調(diào)度-算法歡迎探討！第11章多機(jī)器人協(xié)同調(diào)度-遺傳算法智慧物流系統(tǒng)：從設(shè)計(jì)到實(shí)現(xiàn)教學(xué)內(nèi)容CONTENTS1遺傳算法2遺傳算法起源3遺傳算法原理4遺傳算法評(píng)價(jià)104

章節(jié)目標(biāo)理解遺傳算法的基本原理與起源；掌握遺傳算法的核心進(jìn)程與實(shí)現(xiàn)方法；了解與評(píng)估遺傳算法的優(yōu)點(diǎn)、缺點(diǎn)及應(yīng)用場(chǎng)景。105遺傳算法：遺傳算法(GeneticAlgorithm,GA)于1975年由美國(guó)密歇根大學(xué)的Holland教授提出，是建立在達(dá)爾文的生物進(jìn)化論和孟德?tīng)柕倪z傳學(xué)說(shuō)基礎(chǔ)上的一種隨機(jī)搜索算法。它是模擬達(dá)爾文生物進(jìn)化論的自然選擇和遺傳學(xué)機(jī)理生物進(jìn)化過(guò)程的計(jì)算模型，是一種通過(guò)模擬自然進(jìn)化過(guò)程搜索最優(yōu)解的方法。1遺傳算法遺傳算法具有全局搜索能力強(qiáng)、魯棒性強(qiáng)、靈活性和可擴(kuò)展性強(qiáng)、并行計(jì)算能力強(qiáng)等優(yōu)點(diǎn)，但求解過(guò)程中伴隨著大量無(wú)為的冗余迭代、效率降低、易出現(xiàn)過(guò)早收斂與局部最優(yōu)解等現(xiàn)象。106算法起源：遺傳算法的思想源于“自然選擇”和“優(yōu)勝劣汰”的進(jìn)化規(guī)律，它通過(guò)計(jì)算的方法類(lèi)比并模擬了生物學(xué)中的遺傳進(jìn)化過(guò)程，主要包括三個(gè)核心進(jìn)程，如下表所示：2遺傳算法起源生物解釋計(jì)算機(jī)解釋選擇物競(jìng)天擇，適者生存。生物種群中環(huán)境適應(yīng)度高的個(gè)體得以生存，適應(yīng)度低的個(gè)體容易死亡。計(jì)算機(jī)種群中每個(gè)個(gè)體都擁有適應(yīng)度，根據(jù)適應(yīng)度的大小決定個(gè)體是否被遺傳到下一代種群中。該個(gè)體遺傳到下一代的概率與其適應(yīng)度成正比。交配兩個(gè)個(gè)體交配時(shí)，兩個(gè)匹配的染色體可能進(jìn)行基因交換。設(shè)置一個(gè)交叉概率，從種群中隨機(jī)選擇兩個(gè)基因個(gè)體，這兩個(gè)基因個(gè)體按照交叉概率進(jìn)行基因交換，基因交換的位置隨機(jī)選取。變異種群中任意個(gè)體的任意基因片段都有可能發(fā)生基因變異。設(shè)置一個(gè)變異概率，從種群中隨機(jī)選擇一個(gè)個(gè)體，該個(gè)體按照變異概率進(jìn)行基因變異，變異的基因位置是隨機(jī)的。1071.基因與個(gè)體在生物學(xué)中，用“AA”、“Aa”、“aa”來(lái)表示基因的性狀。在標(biāo)準(zhǔn)的遺傳算法中，使用二進(jìn)制的符號(hào)集{0,1}和十進(jìn)制來(lái)表示等位基因，A用二進(jìn)制符號(hào)的0來(lái)表示，a用二進(jìn)制符號(hào)的1來(lái)表示。2遺傳算法起源一對(duì)等位基因二進(jìn)制符號(hào)十進(jìn)制AA000Aa/aA011102aa113遺傳算法中的基因表示1081.基因與個(gè)體在解決實(shí)際問(wèn)題時(shí)，根據(jù)實(shí)際問(wèn)題的情況給等位基因賦予具體的性狀，比如機(jī)器人的運(yùn)動(dòng)方向、目的地等。對(duì)于復(fù)雜的問(wèn)題可以采用多位的二進(jìn)制符號(hào)來(lái)表示一對(duì)等位基因，即如果用n位二進(jìn)制符號(hào)表示一對(duì)等位基因，則有2n種表示性狀。這與“自然界中大多數(shù)性狀是由多對(duì)等位基因決定”的實(shí)際情況也是相吻合的。2遺傳算法起源1091.基因與個(gè)體在標(biāo)準(zhǔn)遺傳算法中，使用固定長(zhǎng)度的二進(jìn)制符號(hào)串來(lái)表示個(gè)體，即根據(jù)實(shí)際情況設(shè)置一個(gè)固定的基因長(zhǎng)度，比如固定長(zhǎng)度為6，則某一個(gè)體的基因序列可能為：011011011000或011001111000……一個(gè)固定長(zhǎng)度的基因個(gè)體，理論上應(yīng)當(dāng)有4**6=4096種不同的排列方式。當(dāng)固定長(zhǎng)度增加1，長(zhǎng)度從6變?yōu)?時(shí)，所有可能排列的總數(shù)就會(huì)迅速擴(kuò)增到4**7=16384種，在這些排列組合中只有一種或幾種基因序列是最適應(yīng)自然環(huán)境的，即為最優(yōu)解。當(dāng)基因長(zhǎng)度越長(zhǎng)，基因庫(kù)就越龐大，計(jì)算量就會(huì)呈指數(shù)增長(zhǎng)。2遺傳算法起源1102.種群在生物學(xué)中，在一定時(shí)間內(nèi)占據(jù)一定空間的同種生物的所有個(gè)體，被稱(chēng)為種群。在計(jì)算機(jī)中，用固定基因長(zhǎng)度的二進(jìn)制符號(hào)表示個(gè)體，因此，用固定數(shù)量的具有固定基因長(zhǎng)度的二進(jìn)制符號(hào)串來(lái)表示種群。2遺傳算法起源在自然界中，初代種群往往只有具體的數(shù)量，因此是無(wú)法包括所有的性狀，它們只具備部分的性狀。在生存過(guò)程中，種群中可能會(huì)有一部分適應(yīng)度低的個(gè)體被自然界淘汰。因此，每一代種群都會(huì)根據(jù)適應(yīng)度來(lái)判斷是否削減種群中的個(gè)體；通過(guò)雜交的概率決定是否發(fā)生雜交去產(chǎn)生新的性狀個(gè)體；通過(guò)變異的概率來(lái)決定是否發(fā)生變異去產(chǎn)生新的性狀個(gè)體。1112遺傳算法起源種群迭代的過(guò)程1123.適應(yīng)度自然界中的“物競(jìng)天擇，適者生存”在計(jì)算機(jī)中同樣適用，在計(jì)算機(jī)中通過(guò)給定一個(gè)適應(yīng)度的算子用來(lái)計(jì)算所有個(gè)體是否適應(yīng)“自然”。根據(jù)實(shí)際問(wèn)題，計(jì)算適應(yīng)度S的表達(dá)式并不相同。例如在路徑規(guī)劃問(wèn)題中，可以使用路徑長(zhǎng)度的倒數(shù)作為適應(yīng)度S，路徑越短對(duì)應(yīng)適應(yīng)度S就越大，即適應(yīng)度S越高；或者相同步數(shù)下走得越遠(yuǎn)的個(gè)體，其適應(yīng)度被認(rèn)為更高。確定了適應(yīng)度S表達(dá)式后還需要估計(jì)其取值范圍，并制定合適的淘汰閾值，即適應(yīng)度低于閾值的個(gè)體將被淘汰，適應(yīng)度高于閾值的個(gè)體將被留下。2遺傳算法起源113算法原理：在遺傳算法中，包含五個(gè)“隨機(jī)規(guī)則”：①初代種群包含的個(gè)體是隨機(jī)的；②進(jìn)行交配的個(gè)體是隨機(jī)的；③交配時(shí)若發(fā)生基因交叉，交叉的基因片段是隨機(jī)的；④發(fā)生變異的個(gè)體是隨機(jī)的；⑤發(fā)生變異的基因片段是隨機(jī)的。3遺傳算法原理種群在每一次迭代過(guò)程中，通過(guò)選擇、交配和變異得到的新種群個(gè)體數(shù)量和初代種群的個(gè)體數(shù)量相同。選擇和交配體現(xiàn)了遺傳算法的搜索能力，變異使遺傳算法能搜索到問(wèn)題的所有解。114在實(shí)際應(yīng)用中，會(huì)設(shè)置一個(gè)最大迭代數(shù)讓該種群迭代有限次數(shù)，即判斷若干代以后種群中的全部個(gè)體或大部分個(gè)體的基因序列是否都相同。情況一：相同，則該基因序列就是本問(wèn)題的最優(yōu)解；情況二：不相同，需要反復(fù)調(diào)節(jié)各項(xiàng)參數(shù)使得最終結(jié)果收斂。遺傳算法流程圖如圖所示。3遺傳算法原理遺傳算法流程圖115遺傳算法通常需要調(diào)節(jié)以下參數(shù)：①種群數(shù)目M：每代種群的數(shù)目都為M，一般設(shè)為基因序列長(zhǎng)度的兩倍；②雜交概率P_c：根據(jù)經(jīng)驗(yàn)使用二進(jìn)制編碼的基因序列雜交率為0.7；③變異概率P_m：根據(jù)經(jīng)驗(yàn)一般設(shè)為0.001；④最大迭代次數(shù)：根據(jù)經(jīng)驗(yàn)一般設(shè)為20次。目前沒(méi)有調(diào)節(jié)這些參數(shù)的有效規(guī)則，只能根據(jù)具體的應(yīng)用情景和實(shí)際效果進(jìn)行調(diào)整。3遺傳算法原理遺傳算法流程圖116算法評(píng)價(jià)：遺傳算法是一種模擬自然進(jìn)化過(guò)程來(lái)尋找最優(yōu)解的人工智能技術(shù)，是一種使用高效、魯棒性強(qiáng)的優(yōu)化技術(shù)。4遺傳算法評(píng)價(jià)優(yōu)點(diǎn)：遺傳算法具有良好的全局搜索能力，可以快速的把空間中的全體解搜索出來(lái)，而不會(huì)陷入局部最優(yōu)解的快速下降陷阱；并且利用它的內(nèi)在并行性，可以方便的進(jìn)行分布式計(jì)算，加速求解的速度。缺點(diǎn)：遺傳算法的局部搜索能力較差，導(dǎo)致單純的遺傳算法比較費(fèi)時(shí)，在進(jìn)化后期搜索效率較低；在實(shí)際應(yīng)用中，遺傳算法容易產(chǎn)生早熟收斂的問(wèn)題。117算法評(píng)價(jià)：在多機(jī)器人物流系統(tǒng)中，遺傳算法在分配任務(wù)時(shí)，通過(guò)計(jì)算適應(yīng)度來(lái)判斷基因是否為最優(yōu)。在物流機(jī)器人、貨架、取貨點(diǎn)的分配過(guò)程中，將計(jì)算路徑規(guī)劃算法的執(zhí)行時(shí)間（從所有機(jī)器人執(zhí)行開(kāi)始直到最后一個(gè)機(jī)器人執(zhí)行任務(wù)完成的持續(xù)時(shí)間）作為分配算法的適應(yīng)度。時(shí)間越短對(duì)應(yīng)路徑就越短，路徑越短執(zhí)行越快完成，這樣分配的組合比其他路徑規(guī)劃的組合更優(yōu)。下圖表示基于遺傳算法進(jìn)行任務(wù)分配，得到每一代中所有基因組合的適應(yīng)度。4遺傳算法評(píng)價(jià)118從上圖中分析得出：每一代基因都有12個(gè)基因個(gè)體，因此在每一代列表都有12個(gè)基因適應(yīng)度；基因的適應(yīng)度從第0代開(kāi)始，適應(yīng)度(路徑執(zhí)行時(shí)間)偏大，接下來(lái)每一代基因的適應(yīng)度都越來(lái)越?。贿m應(yīng)度為0時(shí)，表示該分配組合無(wú)法規(guī)劃出合適路徑或者規(guī)劃路徑超時(shí)；4遺傳算法評(píng)價(jià)119如下圖所示，最后找到歷史適應(yīng)度中最小的基因組合(764,表示76.4s)，分配任務(wù)。4遺傳算法評(píng)價(jià)歡迎探討！第12章多機(jī)器人協(xié)同調(diào)度-粒子群算法智慧物流系統(tǒng)：從設(shè)計(jì)到實(shí)現(xiàn)教學(xué)內(nèi)容CONTENTS3粒子群算法-信息迭代2粒子群算法-粒子與粒子群4粒子群算法原理5算法評(píng)價(jià)1粒子群算法起源123

章節(jié)目標(biāo)了解粒子群算法的概念和作用；掌握粒子群算法的生物原理和在計(jì)算機(jī)中的原理。124粒子群算法(ParticleSwarmOptimization，PSO)：1995年，受到鳥(niǎo)群覓食行為的規(guī)律性啟發(fā)，JamesKennedy和RussellEberhart建立了一個(gè)簡(jiǎn)化算法模型，經(jīng)過(guò)多年改進(jìn)最終形成了粒子群優(yōu)化算法(ParticleSwarmOptimization,PSO)，也可稱(chēng)為粒子群算法或鳥(niǎo)群覓食算法。1粒子群算法起源一群鳥(niǎo)在某一區(qū)域中隨機(jī)地搜尋一塊食物，每只鳥(niǎo)都不知道這塊食物的位置，但知道自身與食物的距離，每只鳥(niǎo)將自身的信息在鳥(niǎo)群中共享。所以要想找到這塊食物，最快的方法就是在距離食物最近的那只鳥(niǎo)的周邊進(jìn)行搜尋。1251粒子群算法起源1262粒子群算法-粒子與粒子群同遺傳算法類(lèi)似，粒子群算法中也有一些特別的計(jì)算機(jī)“仿生定義”，其核心可概括為“兩個(gè)對(duì)象兩個(gè)過(guò)程”：對(duì)象指鳥(niǎo)（粒子）和鳥(niǎo)群（粒子群）；過(guò)程指食物搜尋和信息共享。對(duì)象屬性行為粒子（鳥(niǎo)）速度、位置、歷史位置最優(yōu)搜尋（計(jì)算適應(yīng)度）粒子群（鳥(niǎo)群）全局位置最優(yōu)信息共享（迭代）用一串實(shí)數(shù)表示一個(gè)粒子群；用若干串實(shí)數(shù)表示粒子群；127每個(gè)粒子都具有3個(gè)屬性：速度、位置、歷史最優(yōu)位置。屬性符號(hào)描述速度vi當(dāng)前粒子所具有的速度由兩部分組成：由先前速度影響遺留下來(lái)的慣性速度w；若當(dāng)前粒子不是粒子群中最接近目標(biāo)的粒子時(shí)，該粒子有向著最優(yōu)方向移動(dòng)的趨勢(shì)速度。位置xi當(dāng)前粒子所在的位置，用以衡量粒子與目標(biāo)的距離歷史最優(yōu)位置pi當(dāng)前粒子在搜尋目標(biāo)的過(guò)程中，距離目標(biāo)最近時(shí)的位置。粒子在移動(dòng)的過(guò)程中分享當(dāng)前自身的位置，衡量粒子與目標(biāo)的距離，根據(jù)自身的速度調(diào)整移動(dòng)方向，向著最優(yōu)方向移動(dòng)。當(dāng)粒子在移動(dòng)過(guò)程中，自己位于距離目標(biāo)最近的位置時(shí)，不改變方向和速度繼續(xù)移動(dòng)。2粒子群算法-粒子與粒子群128在搜索過(guò)程中，和遺傳算法類(lèi)似，粒子群算法中也需要定義一個(gè)適應(yīng)度函數(shù)，用以評(píng)估每個(gè)解的優(yōu)異度。在鳥(niǎo)群覓食的過(guò)程中，可以使用粒子（鳥(niǎo)）位置與目標(biāo)（食物的曼哈頓距離作為適應(yīng)度函數(shù)，假設(shè)粒子的坐標(biāo)為(Xparticle，Ypartice)，目標(biāo)點(diǎn)坐標(biāo)為(Xtarget，Ytarget)，粒子坐標(biāo)到目標(biāo)點(diǎn)的適應(yīng)函數(shù)（曼哈頓距離）為：

由上式可知，適應(yīng)度S越接近于1，適應(yīng)度越好，粒子的位置越好。2粒子群算法-粒子與粒子群129適應(yīng)度最大的粒子會(huì)在粒子群中共享自己的位置，使得其他粒子向其靠近。在尋找全局最優(yōu)解的過(guò)程中，粒子群中的粒子需要不斷更新自己的速度和位置，計(jì)算粒子更新的速度，需要用到的參數(shù)：慣性因子w：用來(lái)控制繼承粒子當(dāng)前的速度的因子，越大則對(duì)于當(dāng)前速度的繼承程度越小，越小則對(duì)于當(dāng)前速度的繼承程度越大；當(dāng)前粒子的速度vi：粒子當(dāng)前移動(dòng)的速度；學(xué)習(xí)因子c1、c2：c1粒子的加速因子，c2粒子群的加速因子；粒子歷史最優(yōu)位置pi：當(dāng)前粒子在搜尋目標(biāo)的過(guò)程中，距離目標(biāo)最近時(shí)的位置；當(dāng)前粒子的位置xi:當(dāng)前粒子所在的位置；粒子群的歷史最優(yōu)位置pg：粒子群中當(dāng)前最接近全局最優(yōu)解的粒子的位置；3粒子群算法-信息迭代130更新粒子的速度：

更新粒子的速度：慣性因子w粒子當(dāng)前速度vi粒子加速因子c1[0,1]之間的隨機(jī)數(shù)粒子歷史最佳位置pi粒子當(dāng)前位置xi粒子群加速因子c2粒子群歷史最佳位置pg3粒子群算法-信息迭代131更新粒子的位置：

更新粒子的位置：

結(jié)束迭代：通過(guò)信息的迭代，不斷地更新粒子自身的位置和速度，向食物靠近。當(dāng)粒子和食物的適應(yīng)度(距離越短，適應(yīng)度越高)，距離為0時(shí)，粒子到達(dá)食物的位置，結(jié)束迭代。3粒子群算法-信息迭代1324粒子群算法原理粒子群算法需要調(diào)節(jié)的常用參數(shù)：粒子群的規(guī)模m：粒子使用n位的實(shí)數(shù)串構(gòu)成，粒子群是由m個(gè)長(zhǎng)度為n的實(shí)數(shù)串構(gòu)成；慣性因子w：慣性因子越大，全局搜索能力越強(qiáng)，局部搜索能力弱；慣性因子越小，則相反；粒子加速因子c1：代表粒子的個(gè)體認(rèn)知，一般c1

范圍在0和4之間；粒子群加速因子c2：代表粒子的社會(huì)認(rèn)知，一般c2

范圍在0和4之間；最大迭代次數(shù)：根據(jù)經(jīng)驗(yàn)一搬設(shè)為20次；更新粒子的位置和速度體現(xiàn)了粒子群算法的搜索能力，更新粒子的歷史最佳位置和粒子群的歷史最佳位置防止粒子群算法收斂到局部最優(yōu)解。133先設(shè)定一個(gè)粒子群規(guī)模m，即粒子群數(shù)量，再規(guī)定用于表示粒子的實(shí)數(shù)串長(zhǎng)度n；在初始化時(shí)，隨機(jī)生成m個(gè)粒子，即隨機(jī)生成m個(gè)長(zhǎng)度為n的實(shí)數(shù)串；給所有粒子設(shè)置固定或是隨機(jī)的初始速度，也可全部設(shè)置為0；計(jì)算粒子的適應(yīng)度，根據(jù)適應(yīng)度更新粒子的移動(dòng)速度和位置；然后進(jìn)行反復(fù)的搜索和迭代過(guò)程，直到產(chǎn)生最優(yōu)解或者超出最大迭代次數(shù)，結(jié)束迭代；4粒子群算法原理134以下用一個(gè)實(shí)例直觀(guān)演示粒子群優(yōu)化算法的迭代過(guò)程。假定目標(biāo)位置坐標(biāo)為(5,5)，即圖中黑色“X”處；設(shè)定慣性系數(shù)w=0.6；設(shè)定學(xué)習(xí)因子c_1=1.2、c_2=1.5。01234567899876543210X(1)初始化粒子群，隨機(jī)生成10個(gè)粒子的位置和初始速度，分布在10*10的范圍內(nèi)，初始速度于區(qū)間[-0.5,0.5]4粒子群算法實(shí)例13501234567899876543210X初代粒子及相關(guān)數(shù)據(jù)：粒子位置xi速度vi歷史最優(yōu)位置pi1[7.902,9.143][-0.158,0.112][7.902,9.143]2[3.793,4.72][-0.211,0.456][3.793,4.72]3[8.505,6.29][0.069,-0.473][8.505,6.29]4[0.3,5.237][0.319,-0.18][0.3,5.237]5[7.438,4.7][0.198,0.343][7.438,4.7]6[0.989,0.697][0.371,-0.476][0.989,0.697]7[0.998,1.706][0.426,0.496][0.998,1.706]8[0.573,6.524][0.353,-0.392][0.573,6.524]9[5.492,0.356][0.427,-0.174][5.492,0.356]10[1.881,3.0][-0.279,0.273][1.881,3.0]4粒子群算法實(shí)例136(2)搜尋最優(yōu)位置：

01230123

4粒子群算法實(shí)例137(2)搜尋最優(yōu)位置：根據(jù)適應(yīng)度函數(shù)，找到距離目標(biāo)點(diǎn)最近的粒子位置pg=[3.793,4.72]。01234567899876543210X粒子位置xi速度vi歷史最優(yōu)位置pi1[7.902,9.143][-0.158,0.112][7.902,9.143]2[3.793,4.72][-0.211,0.456][3.793,4.72]3[8.505,6.29][0.069,-0.473][8.505,6.29]4[0.3,5.237][0.319,-0.18][0.3,5.237]5[7.438,4.7][0.198,0.343][7.438,4.7]6[0.989,0.697][0.371,-0.476][0.989,0.697]7[0.998,1.706][0.426,0.496][0.998,1.706]8[0.573,6.524][0.353,-0.392][0.573,6.524]9[5.492,0.356][0.427,-0.174][5.492,0.356]10[1.881,3.0][-0.279,0.273][1.881,3.0]接下來(lái)，所有粒子向粒子2靠近，繼續(xù)搜尋目標(biāo)點(diǎn)4粒子群算法實(shí)例138(3)信息迭代：利用更新粒子位置、速度的公式進(jìn)行信息迭代。更新所有粒子的位置和速度(以粒子1和粒子2為例)：粒子1：

初代粒子的歷史最佳位置就是粒子的初始位置，因此可以省略更新粒子的歷史最佳位置的步驟。4粒子群算法實(shí)例139(3)信息迭代：利用更新粒子位置、速度的公式進(jìn)行信息迭代。更新所有粒子的位置和速度(以粒子1和粒子2為例)：粒子2：

根據(jù)適應(yīng)度函數(shù)，粒子2為最佳粒子，因此可以省略更新粒子的歷史最佳位置和粒子群的歷史最佳位置兩項(xiàng)步驟。4粒子群算法實(shí)例14001234567899876543210X第一次迭代初代粒子中，粒子2為最佳粒子，所有粒子都快速向粒子2的位置靠近，但因?yàn)槊總€(gè)粒子都還具有不小的速度，因此結(jié)果并沒(méi)有收斂，歷史最佳位置的粒子是紫色點(diǎn)，需要繼續(xù)迭代，更新粒子的位置和速度。01234567899876543210X初代粒子4粒子群算法實(shí)例141第二代粒子，所有粒子的信息，如下：粒子位置xi速度vi歷史最優(yōu)位置pi1[5.163,6.364][-2.739,-2.779][7.902,9.143]2[3.667,4.993][-0.127,0.273][3.793,4.72]3[5.514,4.996][-2.99,-1.294][8.505,6.29]4[2.739,4.796][2.439,-0.441][0.3,5.237]5[5.211,4.918][-2.226,0.218][7.438,4.7]6[3.016,3.0][2.027,2.303][0.989,0.697]7[3.053,3.943][2.055,2.237][0.998,1.706]8[2.857,5.128][2.284,-1.396][0.573,6.524]9[4.655,3.06][-0.837,2.704][5.492,0.356]10[2.944,4.27][1.063,1.271][1.881,3.0]粒子群歷史最佳位置pg[3.743,4.72]最佳距離1.239第二代粒子4粒子群算法實(shí)例14201234567899876543210X第二次迭代第二代粒子01234567899876543210X第二代粒子中，粒子5為最佳粒子，更新所有粒子的位置和速度。所有粒子都快速向粒子5的位置靠近，但是同樣沒(méi)有收斂到目標(biāo)位置，需要繼續(xù)迭代。4粒子群算法實(shí)例143經(jīng)歷20次迭代的結(jié)果，將迭代次數(shù)作為z軸，時(shí)間軸；底部二維坐標(biāo)為設(shè)定的粒子運(yùn)動(dòng)范圍10×10；最終粒子基本都聚集在一點(diǎn)，即結(jié)果收斂在目標(biāo)坐標(biāo)(5,5)。4粒子群算法實(shí)例144任意取其中單個(gè)粒子，其迭代軌跡為：4粒子群算法實(shí)例145(4)調(diào)節(jié)參數(shù)：將慣性系數(shù)從w=0.6改成w=0.2。觀(guān)察發(fā)現(xiàn)，將慣性系數(shù)減小后，粒子群迅速收斂，大約在第7代后，粒子群都聚集在

(5,5)處，由此收斂排列成一條直線(xiàn)狀。這樣調(diào)整參數(shù)可以獲得較快的收斂速度，但也容易使得收斂得到的是局部最優(yōu)解，而非全局最優(yōu)。4粒子群算法實(shí)例146PSO算法采用簡(jiǎn)單的速度位移模型，避免了復(fù)雜的遺傳操作，同時(shí)它特有的記憶功能使其可以動(dòng)態(tài)的跟蹤當(dāng)前的搜索情況并及時(shí)調(diào)整搜索策略，具有較強(qiáng)的全局搜索能力和魯棒性。PSO算法有計(jì)算簡(jiǎn)單、易于實(shí)現(xiàn)、控制參數(shù)少的特點(diǎn)。在實(shí)際的多機(jī)器人物流系統(tǒng)中，粒子群算法分配任務(wù)的過(guò)程中，通過(guò)計(jì)算適應(yīng)度來(lái)判斷粒子位置是否為最優(yōu)，在物流機(jī)器人、貨架、取貨點(diǎn)的分配過(guò)程中，將計(jì)算路徑規(guī)劃算法的執(zhí)行時(shí)間(最后一個(gè)機(jī)器人執(zhí)行完成后的時(shí)間)作為分配算法的適應(yīng)度，時(shí)間越短對(duì)應(yīng)路徑就越短，對(duì)應(yīng)路徑越短執(zhí)行就越快完成，分配的組合也就越優(yōu)。5算法評(píng)價(jià)147輸出每一代粒子位置(機(jī)器人-貨架-取貨點(diǎn))的適應(yīng)度：本次任務(wù)分配迭代了4代，每一代中都有12個(gè)粒子，從第0代開(kāi)始，粒子位置的適應(yīng)度偏大，下面的每一代粒子位置組合的適應(yīng)度都變小；迭代到第4代時(shí)多數(shù)粒子的位置組合的適應(yīng)度都為788(表示78.8s)，到第4代停止迭代表示粒子位置已經(jīng)是較優(yōu)解了，不再更新了。5算法評(píng)價(jià)148多數(shù)粒子位置組合的適應(yīng)度都為788(表示78.8s)，表示此時(shí)的解已經(jīng)是較優(yōu)解了，將粒子位置的適應(yīng)度作為預(yù)計(jì)執(zhí)行時(shí)間。5算法評(píng)價(jià)歡迎探討！第13章多機(jī)器人協(xié)同調(diào)度-蟻群算法智慧物流系統(tǒng)：從設(shè)計(jì)到實(shí)現(xiàn)教學(xué)內(nèi)容CONTENTS1蟻群算法2蟻群算法原理3旅行商問(wèn)題4蟻群算法評(píng)價(jià)152

章節(jié)目標(biāo)了解蟻群算法的起源；理解蟻群算法的原理與機(jī)制；掌握蟻群算法的信息素更新與濃度計(jì)算；了解蟻群算法的優(yōu)點(diǎn)、缺點(diǎn)及應(yīng)用前景。153算法起源：蟻群算法（AntColonyOptimization,ACO）是意大利學(xué)者M(jìn)arcoDorigo于1992年基于蟻群覓食的行為特征提出的一種模型進(jìn)化算法。該算法在求解旅行商問(wèn)題（TravelingSalesmanProblem,TSP）、分配問(wèn)題、圖著色問(wèn)題等方面均取得了較好的結(jié)果。隨著群體智能的研究發(fā)展，蟻群算法也被應(yīng)用于多機(jī)器人系統(tǒng)的任務(wù)分配及調(diào)度協(xié)作等方面。1蟻群算法154算法起源：蟻群覓食是一種典型的群體智能行為，蟻群尋找食物時(shí)會(huì)分散探索，如果一只螞蟻找到食物，它將返回巢中通知同伴并沿途留下“信息量”（Pheromone），作為蟻群前往食物所在地的標(biāo)記。信息量會(huì)隨時(shí)間揮發(fā)，如果兩只螞蟻同時(shí)找到同一食物，又采取不同路線(xiàn)回到巢中，那么比較繞遠(yuǎn)的一條路上信息量的氣味會(huì)比較淡，蟻群將傾向于選擇另一條更近的路線(xiàn)前往食物所在地。1蟻群算法155算法起源：在旅行商問(wèn)題中，蟻群算法會(huì)設(shè)計(jì)虛擬的“螞蟻”摸索不同的路線(xiàn)，并留下虛擬“信息量”。虛擬的“信息量”也會(huì)揮發(fā)，每只螞蟻每次隨機(jī)選擇要走的路徑，但是它們傾向于選擇路徑比較短、信息量比較濃的路徑。根據(jù)“信息量比較濃的路線(xiàn)更近”原則，即可選擇出最佳路線(xiàn)。由于這個(gè)算法利用了正反饋機(jī)制，使得較短的路徑能夠有較大的機(jī)會(huì)得到選擇，并且采用概率算法，所以它能夠不局限于局部最優(yōu)解。1蟻群算法156算法原理：如圖1所示，螞蟻選路過(guò)程中較短路徑上遺留的信息量會(huì)在短時(shí)間內(nèi)大于較長(zhǎng)路徑，蟻群算法的原理不妨用一個(gè)例子來(lái)說(shuō)明：假設(shè)A、E兩點(diǎn)是蟻群的巢穴和食物源，從其間有兩條路徑A-B-H-D-E和A-B-C-D-E，其中B-H和H-D間距離為1m，B-C和C-D間距離為0.5m。2蟻群算法原理蟻群選擇路徑圖1157算法原理：如圖2所示，在A(yíng)、E點(diǎn)分別分配30只螞蟻從兩點(diǎn)出發(fā)，在t=0時(shí)刻，30只螞蟻?zhàn)叩椒种房贐點(diǎn)或D點(diǎn)。因?yàn)槌跏紩r(shí)沒(méi)有什么線(xiàn)索可供螞蟻們選擇，所以以相同的概率決定選擇哪條路徑，結(jié)果是15只螞蟻?zhàn)咦筮吢窂紻-H、B-H；另外15只螞蟻?zhàn)哂疫叺穆窂紻-C、B-C，這些螞蟻在行進(jìn)過(guò)程中分別留下信息量。2蟻群算法原理蟻群選擇路徑圖2158算法原理：如圖3所示，假設(shè)螞蟻都具有相同的移動(dòng)速度（1m/s）和釋放信息量的能力。在經(jīng)過(guò)1s后，從D點(diǎn)出發(fā)的螞蟻，有15只螞蟻到達(dá)H點(diǎn)，還有15只螞蟻經(jīng)過(guò)C點(diǎn)到達(dá)B點(diǎn)（D-H=D-C+C-B）；同樣在經(jīng)過(guò)1s后，從B點(diǎn)出發(fā)的螞蟻，有15只螞蟻到達(dá)H點(diǎn)，還有15只螞蟻經(jīng)過(guò)C點(diǎn)到達(dá)D點(diǎn)（B-H=B-C+C-D）。2蟻群算法原理蟻群選擇路徑圖3159算法原理：顯然，在相等時(shí)間間隔內(nèi)，路徑D-H-B上共有15只螞蟻經(jīng)過(guò)并留下信息量，路徑D-C-B上共有30只螞蟻經(jīng)過(guò)并留下信息量，其信息量強(qiáng)度是D-H-B路徑上的2倍。因此，當(dāng)再有30只螞蟻從A、E