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

①機(jī)器人從初始點運(yùn)動到目標(biāo)點。②通過算法規(guī)劃路徑，使機(jī)器人繞開障礙物并且經(jīng)過某些必須經(jīng)過的坐標(biāo)點。使用算法規(guī)劃機(jī)器人路徑的時候，算法先去搜索地圖，然后回溯搜索過的路徑，找到目標(biāo)點到起點較優(yōu)的路徑。③在完成以上任務(wù)的前提下盡量優(yōu)化機(jī)器人運(yùn)行軌跡?？臻gV初始位姿Z1目標(biāo)位姿Z2障礙物8盲目式搜索與啟發(fā)式搜索：機(jī)器人在未知區(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)致搜索效率低，會在計算時耗費(fèi)過多的空間與時間。103啟發(fā)式搜索啟發(fā)式搜索：啟發(fā)式搜索就是在搜索中鍵入啟發(fā)性信息，用來指導(dǎo)搜索過程朝著最有希望的方向前進(jìn)。啟發(fā)性信息就是在搜索過程中獲取到的控制前進(jìn)方向的信息，其用途可分為三種：①用于確定要擴(kuò)展的下一節(jié)點的位置，以免盲目地去擴(kuò)展；②在擴(kuò)展下一節(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)行擴(kuò)展并考察該節(jié)點能否成為目標(biāo)節(jié)點，在第n層的節(jié)點沒有全部擴(kuò)展并考察結(jié)束之前，不擴(kuò)展第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ī)器人存在多個前進(jìn)方向時的優(yōu)先順序為：上>右>下>左（絕對方向），在該例子中定義open表為一個隊列，close表為一個堆棧。或者用0和1兩種狀態(tài)分別表示該坐標(biāo)在open表或close表中，所有坐標(biāo)的標(biāo)志位事先初始化為0，表示未訪問過或存在未訪問相鄰坐標(biāo)。194廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法：2.算法實例機(jī)器人每向前移動一步記錄機(jī)器人的移動步數(shù)，每當(dāng)機(jī)器人出現(xiàn)轉(zhuǎn)向時，機(jī)器人的移動步數(shù)增加0.5，機(jī)器人的初始化與隊列的初始化如圖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步，機(jī)器人向前移動一步，記錄移動步數(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é)點，記錄機(jī)器人移動步數(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路走到黑“，更適合機(jī)器人對于未知領(lǐng)域的探索任務(wù)。1.算法原理在一條路上一直走下去，如果遇到分岔路口，就任意選擇其中一條繼續(xù)走下去，如果走到頭，就返回到上一個分岔路口走另外一條路，然后再一直走下去，直到搜索完全地圖后就停止搜索。深度優(yōu)先搜索算法示意圖30深度優(yōu)先搜索算法：1.算法原理深度優(yōu)先搜索算法用到計算機(jī)程序中的遞歸思想，搜索規(guī)則如下：（1）訪問指定的根節(jié)點A，發(fā)現(xiàn)有三條分支路線，如圖所示。5深度優(yōu)先搜索算法從根節(jié)點開始訪問31深度優(yōu)先搜索算法：1.算法原理（2）隨機(jī)選擇一個節(jié)點（B1）訪問，發(fā)現(xiàn)有兩條分支線路，如圖所示。5深度優(yōu)先搜索算法隨機(jī)選擇一個支路節(jié)點進(jìn)行訪問32深度優(yōu)先搜索算法：1.算法原理（3）隨機(jī)選擇一個節(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)先搜索算法隨機(jī)選擇一個支路節(jié)點D1進(jìn)行訪問，并訪問到頭隨機(jī)選擇一個支線節(jié)點B2進(jìn)行訪問36深度優(yōu)先搜索算法：5深度優(yōu)先搜索算法返回根節(jié)點并選擇另一條未訪問的支線節(jié)點B3進(jìn)行訪問返回最近的還有未訪問支線的節(jié)點并選擇另一條支線訪問37深度優(yōu)先搜索算法：5深度優(yōu)先搜索算法返回最近的還有未訪問支線的節(jié)點并選擇另一條支線訪問隨機(jī)選擇一條支線節(jié)點E1進(jìn)行訪問38深度優(yōu)先搜索算法：2.算法實例同樣映射到一個4×4尺寸的地圖環(huán)境中，演示DFS算法的逐步行進(jìn)過程。假設(shè)機(jī)器人存在多個前進(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é)點，機(jī)器人從(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步，機(jī)器人向前移動，訪問坐標(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)先順序，機(jī)器人轉(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)先順序，機(jī)器人繼續(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步，機(jī)器人轉(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步，機(jī)器人轉(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步，機(jī)器人掉頭向左移動，返回上一坐標(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步，機(jī)器人轉(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步，機(jī)器人轉(zhuǎn)彎向左移動，返回上一坐標(biāo)節(jié)點(1,1)，還有一個相鄰坐標(biāo)節(jié)點未訪問，不做操作，如圖所示。選擇未訪問的相鄰子節(jié)點，發(fā)現(xiàn)有兩個相鄰子節(jié)點5深度優(yōu)先搜索算法48深度優(yōu)先搜索算法：2.算法實例第10步，機(jī)器人轉(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)先順序，機(jī)器人向上移動，訪問坐標(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)先順序，機(jī)器人轉(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步，機(jī)器人繼續(xù)向右移動，訪問坐標(biāo)節(jié)點(3,3)，找到終點，標(biāo)記位置；若只是為了求取一條起點到終點的可行路徑，程序到此就結(jié)束了。從終點開始反向記錄機(jī)器人的移動步數(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，機(jī)器人也正好返回起點(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ì)介紹。總得來說，就是利用終點坐標(biāo)和當(dāng)前坐標(biāo)的差距擬合出終點距離當(dāng)前點的偏好方向。572.算法實例第一步，當(dāng)機(jī)器人位于坐標(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.算法實例第二步，可移動的方向僅有絕對向上的方向（以下描述均為絕對方向），機(jī)器人向上移動一步，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.算法實例第三步，可移動的方向僅有絕對向上的方向（以下描述均為絕對方向），機(jī)器人向上移動一步，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.算法實例第四步，可移動的方向僅有絕對向右的方向（以下描述均為絕對方向），機(jī)器人向右移動一步，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ù)：①機(jī)器人向右移動一步，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②機(jī)器人先轉(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.算法實例機(jī)器人先轉(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))，即向上走比向右走要付出更大的代價，所以機(jī)器人優(yōu)先選擇向右走。6A*搜索算法A*搜索算法：652.算法實例第六步，機(jī)器人向右移動一步，現(xiàn)有向上和向下兩個方向可移動，需要計算坐標(biāo)(2,3)和(2,1)的估值函數(shù)。①機(jī)器人轉(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②機(jī)器人轉(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)+(