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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

更新粒子的位置：

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

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

范圍在0和4之間；最大迭代次數(shù)：根據(jù)經(jīng)驗一搬設(shè)為20次；更新粒子的位置和速度體現(xiàn)了粒子群算法的搜索能力，更新粒子的歷史最佳位置和粒子群的歷史最佳位置防止粒子群算法收斂到局部最優(yōu)解。133先設(shè)定一個粒子群規(guī)模m，即粒子群數(shù)量，再規(guī)定用于表示粒子的實數(shù)串長度n；在初始化時，隨機生成m個粒子，即隨機生成m個長度為n的實數(shù)串；給所有粒子設(shè)置固定或是隨機的初始速度，也可全部設(shè)置為0；計算粒子的適應(yīng)度，根據(jù)適應(yīng)度更新粒子的移動速度和位置；然后進(jìn)行反復(fù)的搜索和迭代過程，直到產(chǎn)生最優(yōu)解或者超出最大迭代次數(shù)，結(jié)束迭代；4粒子群算法原理134以下用一個實例直觀演示粒子群優(yōu)化算法的迭代過程。假定目標(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)初始化粒子群，隨機生成10個粒子的位置和初始速度，分布在10*10的范圍內(nèi)，初始速度于區(qū)間[-0.5,0.5]4粒子群算法實例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粒子群算法實例136(2)搜尋最優(yōu)位置：

01230123

4粒子群算法實例137(2)搜尋最優(yōu)位置：根據(jù)適應(yīng)度函數(shù)，找到距離目標(biāo)點最近的粒子位置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]接下來，所有粒子向粒子2靠近，繼續(xù)搜尋目標(biāo)點4粒子群算法實例138(3)信息迭代：利用更新粒子位置、速度的公式進(jìn)行信息迭代。更新所有粒子的位置和速度(以粒子1和粒子2為例)：粒子1：

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

根據(jù)適應(yīng)度函數(shù)，粒子2為最佳粒子，因此可以省略更新粒子的歷史最佳位置和粒子群的歷史最佳位置兩項步驟。4粒子群算法實例14001234567899876543210X第一次迭代初代粒子中，粒子2為最佳粒子，所有粒子都快速向粒子2的位置靠近，但因為每個粒子都還具有不小的速度，因此結(jié)果并沒有收斂，歷史最佳位置的粒子是紫色點，需要繼續(xù)迭代，更新粒子的位置和速度。01234567899876543210X初代粒子4粒子群算法實例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粒子群算法實例14201234567899876543210X第二次迭代第二代粒子01234567899876543210X第二代粒子中，粒子5為最佳粒子，更新所有粒子的位置和速度。所有粒子都快速向粒子5的位置靠近，但是同樣沒有收斂到目標(biāo)位置，需要繼續(xù)迭代。4粒子群算法實例143經(jīng)歷20次迭代的結(jié)果，將迭代次數(shù)作為z軸，時間軸；底部二維坐標(biāo)為設(shè)定的粒子運動范圍10×10；最終粒子基本都聚集在一點，即結(jié)果收斂在目標(biāo)坐標(biāo)(5,5)。4粒子群算法實例144任意取其中單個粒子，其迭代軌跡為：4粒子群算法實例145(4)調(diào)節(jié)參數(shù)：將慣性系數(shù)從w=0.6改成w=0.2。觀察發(fā)現(xiàn)，將慣性系數(shù)減小后，粒子群迅速收斂，大約在第7代后，粒子群都聚集在

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

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