《機(jī)器人智能算法導(dǎo)論》

配套課件機(jī)器人運(yùn)動(dòng)規(guī)劃能力目標(biāo)：

能基于工作空間表達(dá)法構(gòu)建自由空間路徑規(guī)劃模型

設(shè)計(jì)圖搜索算法的運(yùn)動(dòng)規(guī)劃系統(tǒng)知識(shí)目標(biāo)：掌握基本運(yùn)動(dòng)規(guī)劃的核心概念與構(gòu)型空間表達(dá)方法理解圖搜索算法的原理與適用場(chǎng)景區(qū)分全局運(yùn)動(dòng)規(guī)劃與局部運(yùn)動(dòng)規(guī)劃的優(yōu)缺點(diǎn)及協(xié)同機(jī)制本講綱要圖論基礎(chǔ)基本運(yùn)動(dòng)規(guī)劃全局運(yùn)動(dòng)規(guī)劃與局部運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)圖搜索算法機(jī)器人運(yùn)動(dòng)規(guī)劃圖論基礎(chǔ)

該圖結(jié)構(gòu)的結(jié)點(diǎn)集合與邊集合分別表示為：

意味著此圖結(jié)構(gòu)中一共存在5個(gè)結(jié)點(diǎn)、4條邊。圖的定義：圖或圖結(jié)構(gòu)是一種由邊的集合與點(diǎn)的集合組合而成的非線性數(shù)據(jù)結(jié)構(gòu)，表示為

或

。結(jié)點(diǎn)集合：

是由數(shù)據(jù)單體抽象而成的結(jié)點(diǎn)集合。邊集合：

是由數(shù)據(jù)單體間的關(guān)系抽象而成的邊集合（簡(jiǎn)稱邊集）。機(jī)器人運(yùn)動(dòng)規(guī)劃圖論基礎(chǔ)有向圖：僅包含有向邊（用單向箭頭表示）的圖。無向圖：僅包含無向邊（用線段表示）的圖。結(jié)點(diǎn)的度：一個(gè)結(jié)點(diǎn)的度通過連接該結(jié)點(diǎn)的邊來度量，為與其連接的邊的數(shù)量。有向圖中結(jié)點(diǎn)的度可進(jìn)一步劃分為出度與入度：

出度：以該結(jié)點(diǎn)為起點(diǎn)，并指向其他結(jié)點(diǎn)的邊的數(shù)量。

入度：指向該結(jié)點(diǎn)的邊的數(shù)量。

（a）有向圖

（b）無向圖機(jī)器人運(yùn)動(dòng)規(guī)劃圖論基礎(chǔ)路徑：路徑是圖

的結(jié)點(diǎn)集合

的一個(gè)有序的子集，記為

。對(duì)于

中的相鄰結(jié)點(diǎn)

與

，存在一條邊

。

若路徑構(gòu)成的子圖為無向圖，則路徑的起點(diǎn)與終點(diǎn)為度為1的點(diǎn)。若路徑構(gòu)成的子圖為有向圖，則路徑的起點(diǎn)為出度為1且入度為0的點(diǎn)，路徑的終點(diǎn)為出度為0且入度為1的點(diǎn)。最優(yōu)路徑：一條包含指定的起點(diǎn)與終點(diǎn)，且所含邊的邊權(quán)和最小的路徑。連通圖：一個(gè)圖是連通的，指對(duì)于圖中的任意一個(gè)結(jié)點(diǎn)

對(duì)

，都能找到連接兩者的一條路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃

（a）有向圖圖論基礎(chǔ)

（b）無向圖

機(jī)器人運(yùn)動(dòng)規(guī)劃基本運(yùn)動(dòng)規(guī)劃機(jī)器人運(yùn)動(dòng)規(guī)劃的難度取決于①機(jī)器人工作環(huán)境中的障礙物是否運(yùn)動(dòng)②有關(guān)障礙物的信息是否完整，如障礙物的尺寸、位置、運(yùn)動(dòng)速度等基本運(yùn)動(dòng)規(guī)劃①障礙物在機(jī)器人運(yùn)動(dòng)過程中保持靜態(tài)②已知障礙物的尺寸和位置定義機(jī)器人運(yùn)動(dòng)規(guī)劃基本運(yùn)動(dòng)規(guī)劃采用兩個(gè)關(guān)鍵步驟解決基本運(yùn)動(dòng)規(guī)劃問題：首先定義一個(gè)圖結(jié)構(gòu)

以表示機(jī)器人工作環(huán)境的幾何結(jié)構(gòu)；其次執(zhí)行圖搜索算法

以尋找一條最小化運(yùn)動(dòng)代價(jià)的、連通機(jī)器人起點(diǎn)

和終點(diǎn)

的路徑?；具\(yùn)動(dòng)規(guī)劃問題運(yùn)動(dòng)規(guī)劃問題可被看作一類最優(yōu)化問題。機(jī)器人運(yùn)動(dòng)規(guī)劃基本運(yùn)動(dòng)規(guī)劃上式中，路徑

是圖結(jié)構(gòu)

的子集，且

中結(jié)點(diǎn)互相連通；圖搜索算法

構(gòu)建路徑

，并輸出運(yùn)動(dòng)代價(jià)。機(jī)器人運(yùn)動(dòng)規(guī)劃全局運(yùn)動(dòng)規(guī)劃與局部運(yùn)動(dòng)規(guī)劃根據(jù)對(duì)環(huán)境信息的掌握程度不同，機(jī)器人運(yùn)動(dòng)規(guī)劃可分為全局運(yùn)動(dòng)規(guī)劃和局部運(yùn)動(dòng)規(guī)劃。全局運(yùn)動(dòng)規(guī)劃環(huán)境完全已知行動(dòng)前預(yù)規(guī)劃優(yōu)點(diǎn)：確保找到最優(yōu)路徑缺點(diǎn)：計(jì)算資源消耗大，環(huán)境

劇烈變化時(shí)難以勝任。局部運(yùn)動(dòng)規(guī)劃環(huán)境動(dòng)態(tài)變化行動(dòng)中在線規(guī)劃優(yōu)點(diǎn)：環(huán)境適應(yīng)能力強(qiáng)，不易被噪聲干擾，計(jì)算資源消耗小。缺點(diǎn)：算法實(shí)時(shí)性強(qiáng)，結(jié)果非最優(yōu)，可能無法到達(dá)終點(diǎn)。在實(shí)際應(yīng)用中，可以同時(shí)采用兩者進(jìn)行協(xié)同工作。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)如何表達(dá)機(jī)器人的工作環(huán)境并被機(jī)器人所理解？通常選擇一種合適的表達(dá)方法并進(jìn)行適當(dāng)?shù)暮?jiǎn)化。通過選擇合適的表達(dá)方法，有助于利用圖搜索算法等較為成熟的算法來解決機(jī)器人運(yùn)動(dòng)規(guī)劃問題。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)工作空間表達(dá)：用于描述機(jī)器人所處實(shí)際物理空間的可達(dá)性。工作空間表達(dá)將物理空間劃分為障礙物空間自由空間被障礙物占據(jù)的不可達(dá)空間沒有被障礙物占據(jù)的可達(dá)空間工作空間表達(dá)是一種獨(dú)立于機(jī)器人本身的工作環(huán)境表達(dá)方法，僅描述了空間中的一個(gè)點(diǎn)是否被障礙物占用，而不能斷定一個(gè)未被占用的點(diǎn)是否能被機(jī)器人通過。當(dāng)機(jī)器人不是一個(gè)點(diǎn)，而是占據(jù)了一定的面積或體積時(shí)呢？機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)構(gòu)型空間表達(dá)：為了解決工作空間表達(dá)方法的缺陷，常用構(gòu)型空間表達(dá)進(jìn)一步量化工作空間。在構(gòu)型空間中，機(jī)器人可以被轉(zhuǎn)化為一個(gè)質(zhì)點(diǎn)，而障礙物空間中的障礙物則根據(jù)機(jī)器人的形體進(jìn)行膨脹化處理。機(jī)器人的位置、姿態(tài)即可表示為構(gòu)型空間中一個(gè)點(diǎn)機(jī)器人構(gòu)型機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)一個(gè)機(jī)器人構(gòu)型空間的維度由該機(jī)器人的自由度個(gè)數(shù)決定。（a）二維構(gòu)型空間自由度為2所有構(gòu)型

組成的構(gòu)型空間表現(xiàn)為一個(gè)固定形狀的二維平面。（b）三維構(gòu)型空間自由度為3所有構(gòu)型

組成的三維空間中的任意一點(diǎn)代表機(jī)器人的一個(gè)構(gòu)型。構(gòu)型空間是以機(jī)器人屬性為依據(jù)張成的多維空間。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)連續(xù)構(gòu)型空間

VS

離散化構(gòu)型空間盡管連續(xù)構(gòu)型空間擁有優(yōu)秀的數(shù)學(xué)屬性，但仍具有求解難度且效率不高。計(jì)算機(jī)以離散存儲(chǔ)的方式記錄數(shù)據(jù)，因而將連續(xù)構(gòu)型空間進(jìn)行離散化處理更合理。支持以圖結(jié)構(gòu)的形式表示離散化的構(gòu)型空間。大量常用的運(yùn)動(dòng)規(guī)劃算法可以利用離散形式的數(shù)據(jù)以搜索最優(yōu)路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)以二維構(gòu)型空間

為例，主要介紹單元分解法、可視圖法與概率路線圖法。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)單元分解法：是一種將構(gòu)型空間劃分為規(guī)則形狀的單元格的離散化表達(dá)方法。最基本的單元分解法規(guī)定劃分得到的單元格形狀大小基本一致。將構(gòu)型空間

中的自由空間與障礙物空間進(jìn)行劃分將連續(xù)空間離散化為為離散化后機(jī)器人可能狀態(tài)的總數(shù)機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)單元分解法基于離散化后得到的構(gòu)型空間表達(dá)

：機(jī)器人處于構(gòu)型

時(shí)，可采取的動(dòng)作集合：定義構(gòu)型

之間的狀態(tài)轉(zhuǎn)移方程：機(jī)器人處于構(gòu)型狀態(tài)

時(shí)，應(yīng)用動(dòng)作

，即可達(dá)到新構(gòu)型狀態(tài)

。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)單元分解法圖的構(gòu)建分為兩步：②若

與

之間存在關(guān)系

，

則建立一條起點(diǎn)為

且終點(diǎn)為

的邊。①將所有取值為1的單元格抽象為圖的結(jié)點(diǎn)，

選取其中一個(gè)結(jié)點(diǎn)為起點(diǎn)

，一個(gè)結(jié)點(diǎn)為終點(diǎn)

；表示為圖結(jié)構(gòu),演算得到最優(yōu)路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)可視圖法：將障礙物空間中的障礙物表示為多邊形，并將其頂點(diǎn)與起點(diǎn)、終點(diǎn)一起互相連接而構(gòu)建圖結(jié)構(gòu)的一種方法。構(gòu)型空間一般為二維平面空間，機(jī)器人是空間中的一個(gè)質(zhì)點(diǎn)，障礙物為空間中的多邊形。起點(diǎn)集合

包含起點(diǎn)

、終點(diǎn)

、所有多邊形障礙物的頂點(diǎn)抽象而成的結(jié)點(diǎn)

。將

中所有結(jié)點(diǎn)用無向邊互連且所有連線都不能穿過障礙物；滿足條件的邊構(gòu)成邊集且賦予邊權(quán)；得到加權(quán)無向圖

。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)可視圖法（a）緊貼障礙物的最優(yōu)路徑（b）松弛后的最優(yōu)路徑路徑緊貼障礙物邊緣，構(gòu)建得到的最優(yōu)路徑非最短路徑，機(jī)器人容易撞到障礙物。將障礙物頂點(diǎn)略微松弛，以設(shè)定的超參數(shù)向外擴(kuò)展一段距離，將擴(kuò)展區(qū)域的交點(diǎn)抽象為可視圖結(jié)點(diǎn)。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)可視圖構(gòu)建的關(guān)鍵：兩個(gè)結(jié)點(diǎn)是否可以連接無向邊。①同一障礙物的相鄰結(jié)點(diǎn)互相可視。②不同障礙物的邊是否相交，若相交，

則為不可視。不同障礙物之間的結(jié)點(diǎn)不可視情況：機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)概率路線圖法：連續(xù)構(gòu)型空間的一種離散化的圖結(jié)構(gòu)表示方法，其結(jié)點(diǎn)集合由自由空間中隨機(jī)采樣的構(gòu)型抽象而成，結(jié)點(diǎn)之間存在無向邊連接，構(gòu)成一個(gè)連通圖?？招男A表示隨機(jī)采樣得到的機(jī)器人構(gòu)型，其均處于自由空間中；藍(lán)色實(shí)心區(qū)域表示障礙物；將構(gòu)型點(diǎn)與較近的鄰居結(jié)點(diǎn)用無向邊相連即可得概率路線圖。機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人的工作環(huán)境表達(dá)單元分解法

核心思想：

優(yōu)點(diǎn)：

缺點(diǎn)：

規(guī)則網(wǎng)絡(luò)離散化表達(dá)一致規(guī)范結(jié)點(diǎn)密度大計(jì)算開銷大精度有損失可視圖法

核心思想：

優(yōu)點(diǎn)：

缺點(diǎn)：

連接障礙物頂點(diǎn)大幅減少結(jié)點(diǎn)數(shù)缺乏靈活性需全局重構(gòu)圖試圖解決結(jié)點(diǎn)密度問題概率路線圖法

核心思想：

優(yōu)點(diǎn)：

隨機(jī)采樣建圖效率高可擴(kuò)展性強(qiáng)對(duì)障礙物形狀沒有過多依賴試圖提高效率，擺脫障礙物的強(qiáng)關(guān)聯(lián)機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法從起點(diǎn)開始，以一定的順序訪問與其有邊相連的所有鄰居頂點(diǎn)，再按相同的訪問方法擴(kuò)展鄰居結(jié)點(diǎn)，直至訪問至終點(diǎn)。（a）用于圖搜索的連通圖結(jié)構(gòu)（b）對(duì)應(yīng)的搜索樹結(jié)構(gòu)機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法圖的前向搜索算法：從起點(diǎn)出發(fā)，逐步推進(jìn)至終點(diǎn)。Open表

：存儲(chǔ)了待擴(kuò)展的結(jié)點(diǎn)及其相關(guān)狀態(tài)。結(jié)點(diǎn)狀態(tài)表VIS：記錄了圖中結(jié)點(diǎn)所處的訪問狀態(tài)。反向指針表BP：用于記錄最優(yōu)路徑的搜索過程。

反向指針表BP記錄的方向與有向邊的指向相反。當(dāng)圖搜索算法執(zhí)行完畢后，從路徑尾端對(duì)反向指針表BP進(jìn)行遍歷，可得到完整的最優(yōu)路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法圖的前向搜索算法機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法搜索樹：兩個(gè)結(jié)點(diǎn)之間僅存在一條路徑，搜索到終點(diǎn)就停止搜索。

以起點(diǎn)為根節(jié)點(diǎn)，

子結(jié)點(diǎn)為在原搜索圖中的鄰居結(jié)點(diǎn)。搜索樹構(gòu)建的復(fù)雜度取決于圖的結(jié)構(gòu)、起點(diǎn)與終點(diǎn)的設(shè)置、

將結(jié)點(diǎn)放入Open表

中的規(guī)則，以及將結(jié)點(diǎn)從

中取出的

規(guī)則等配置。對(duì)于大多數(shù)圖搜索問題，應(yīng)避免構(gòu)建完整的搜索樹。一個(gè)優(yōu)秀的圖搜索算法應(yīng)保證在盡可能少地?cái)U(kuò)展搜索樹地同時(shí)，尋找到起點(diǎn)至終點(diǎn)的最優(yōu)路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法隊(duì)列：一種先進(jìn)先出式的線性列表。越先進(jìn)入隊(duì)列的元素，在獲取元素的時(shí)候越先被處理。

被添加的元素存儲(chǔ)在隊(duì)列的尾端（稱為隊(duì)尾），從隊(duì)列的頭部（稱為隊(duì)頭）取出元素。廣度優(yōu)先搜索：用隊(duì)列維護(hù)待擴(kuò)展結(jié)點(diǎn)。廣度優(yōu)先搜索算法：機(jī)器人運(yùn)動(dòng)規(guī)劃

圖搜索算法廣度優(yōu)先搜索算法舉例：Open(Q):{}Closed:{}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{,}Closed:{,,}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{,,}Closed:{,,,,}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{,,,}Closed:{,,,,,,}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{,,}Closed:{,,,,,,}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{,}Closed:{,,,,,,}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{,}Closed:{,,,,,,,}

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{}Closed:{,,,,,,,}

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：

Open(Q):{}Closed:{,,,,,,,}

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法舉例：（a）圖結(jié)構(gòu)（b）廣度有優(yōu)先搜索樹結(jié)構(gòu)及訪問順序無論樹有多深，總是最靠近根結(jié)點(diǎn)的結(jié)點(diǎn)最先被擴(kuò)展；起點(diǎn)（根節(jié)點(diǎn)）作為擴(kuò)展的第一層級(jí)，其所有子結(jié)點(diǎn)作為第二層級(jí)，子結(jié)點(diǎn)的子結(jié)點(diǎn)作為第三層級(jí)，以此類推。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法廣度優(yōu)先搜索算法在終點(diǎn)和起點(diǎn)較為靠近時(shí)有良好的表現(xiàn)；不適用于起點(diǎn)相距較遠(yuǎn)，且結(jié)點(diǎn)規(guī)模較大的圖。廣度優(yōu)先搜索算法廣度優(yōu)先搜索算法具有完全性，只要原圖是連通圖，且終點(diǎn)存在，則一定能夠找到起點(diǎn)到終點(diǎn)的一條路徑；當(dāng)圖結(jié)構(gòu)具有相同的邊權(quán)值時(shí)，此路徑一定為最優(yōu)路徑；當(dāng)圖結(jié)構(gòu)具有不同的邊權(quán)值時(shí)，算法不能保證返回最優(yōu)路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法棧：一種后進(jìn)先出式的線性列表。越靠后進(jìn)入棧的元素，在獲取元素的時(shí)候，越先被處理。被添加的元素存儲(chǔ)在棧的頂端（稱為棧頂），同樣從棧頂取出元素。深度優(yōu)先搜索算法：用棧維護(hù)待擴(kuò)展結(jié)點(diǎn)。深度優(yōu)先搜索算法：機(jī)器人運(yùn)動(dòng)規(guī)劃深度優(yōu)先搜索算法舉例：圖搜索算法

Open(Q):{}Closed:{}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法深度優(yōu)先搜索算法舉例：

Open(Q):{,}Closed:{,,}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法深度優(yōu)先搜索算法舉例：

Open(Q):{,,}Closed:{,,,,}機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法深度優(yōu)先搜索算法舉例：

Open(Q):{,}Closed:{,,,,,}

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法深度優(yōu)先搜索算法舉例：

Open(Q):{}Closed:{,,,,}

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法深度優(yōu)先搜索算法舉例：（a）圖結(jié)構(gòu)（b）深度優(yōu)先搜索樹結(jié)構(gòu)及訪問順序只有將一條從根結(jié)點(diǎn)到葉子結(jié)點(diǎn)上的所有結(jié)點(diǎn)都訪問完畢，才會(huì)轉(zhuǎn)向另一條路徑；無論樹有多寬，總是一側(cè)的結(jié)點(diǎn)首先被遍歷完全。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法深度優(yōu)先搜索算法深度優(yōu)先搜索算法的空間消耗相對(duì)少于廣度優(yōu)先搜索算法；

其無須在棧中保留大量淺層的結(jié)點(diǎn)，也能訪問到深層的結(jié)點(diǎn)。深度優(yōu)先搜索算法對(duì)終點(diǎn)位置較為敏感。深度優(yōu)先搜索算法不具有完全性，若搜索樹的某一分支深度無限，則算法將一直循環(huán)無法到達(dá)另一分支的終點(diǎn)。在實(shí)際應(yīng)用中較少使用深度優(yōu)先搜索算法尋找路徑。廣度優(yōu)先搜索算法和深度優(yōu)先搜索算法都相對(duì)容易實(shí)現(xiàn)，但兩者的效率都比較低，且都無法在搜索樹的邊權(quán)值不同時(shí)確保找到最優(yōu)路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法：從起點(diǎn)開始，優(yōu)先擴(kuò)展離起點(diǎn)最近的路徑，直到尋找到終點(diǎn)。

：表示有向邊

的邊權(quán)值。

：指起點(diǎn)到結(jié)點(diǎn)

的代價(jià)估算值。

：指結(jié)點(diǎn)

在進(jìn)入Open表以后的最小代價(jià)估計(jì)值。Open表

：存儲(chǔ)仍需擴(kuò)展處理的結(jié)點(diǎn)。Close表VIS：存儲(chǔ)已經(jīng)找到最短路徑、不會(huì)被再次處理的結(jié)點(diǎn)。反向指針表BP：用于回溯最短路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法舉例：

最優(yōu)路徑為：

機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法迪杰斯特拉算法迪杰斯特拉算法能夠獲取起點(diǎn)到終點(diǎn)的最短路徑，同時(shí)在這個(gè)過程中，能夠順便得出所有長度不大于該最短路徑值的其他結(jié)點(diǎn)的最優(yōu)路徑。迪杰斯特拉算法具有完全性，即若起點(diǎn)和終點(diǎn)處于一個(gè)連通圖內(nèi)，則一定能夠找到從起點(diǎn)出發(fā)到終點(diǎn)的最優(yōu)路徑。當(dāng)起點(diǎn)與終點(diǎn)相距較遠(yuǎn)，且圖的規(guī)模較大時(shí)，其空間與時(shí)間消耗較大。在最差的情況下，同樣需要遍歷所有的結(jié)點(diǎn)和邊，才能找到最優(yōu)路徑。機(jī)器人運(yùn)動(dòng)規(guī)劃圖搜索算法A*算法：通過估算結(jié)點(diǎn)

到終點(diǎn)的距離，令

，其中

為

根據(jù)估算方法（通常被稱為啟示式方法）估算的結(jié)點(diǎn)

到終點(diǎn)的距離。同迪杰斯特拉算法①歐氏估算距離：計(jì)算當(dāng)前結(jié)點(diǎn)

的坐標(biāo)與終點(diǎn)

的坐標(biāo)之間的歐氏距離：②曼哈頓估算距離：計(jì)算當(dāng)前結(jié)點(diǎn)

與終點(diǎn)

之間水平