LPA*和D*Lite演算法

D*Lite演算法是Koenig S和Likhachev M基於LPA*演算法基礎上提出的路徑規劃演算法，LPA*也是他兩提出來的。

LPA*演算法本是基於Dijkstra最短路徑演算法而產生的定起點、定目標點的路徑規劃演算法。

D*Lite演算法是變起點、定目標點的路徑規劃演算法。

LPA*演算法

（全稱Lifelong Planning A*演算法）時Koenig和Likhachev在2004年提出的，是一種參考人工智慧的增量搜尋進行改進的A*演算法。它被用來處理動態環境下從給定起始點到給定目標點的最短路徑問題（劃重點！！！起始點和目標點是固定的）。

符號表示

S：地形圖中的路徑節點的集合，s屬於S

succ（s）：successors，節點s的後續節點集合，例如節點1，2，3\cdots i按順序均已被搜尋過，那麼除了1~i的其它結點均屬於succ（i）。

pred（s）：predecessors，類比上述，節點s的前代節點，與succ（s）的意義剛好相反。

c（s，s`）：兩節點之間的代價函式。

g*（s）：節點s到起始點SStart的實際最短距離。

g（s）：節點s到起始點的預計最短距離，上面那個值是實際的最短距離，這個值是一個預計值，是隨著演算法求解程序不斷變動的，當所有節點的g（s）=rhs（s）時，g（s）的值就是到起始點的實際最短距離，即g（s）=g*（s）。

rhs（s） ：right hand sides，為啥叫這名我也不知道，據說來自DynamicSWSF-FP演算法。對於s的所有鄰接節點，求它們到s的距離加上鄰接節點自身的g值，其中最小的那個值作為s的 rhs 值。具體求法可以見下面的公式

U：同A*演算法中的優先佇列，依據每個節點的Key值進行排序。

Key［K1，K2］：優先佇列排序依據的鍵值，包含兩部分，K1與K2，優先比較K1，若相同則比較K2進行排序。K1等同於A*裡的f（s），K2等同於A*裡的g（s），K1與K2的計算方法見下面的圖。

h（s） ：同A*中的類似，到目標點的估計距離，在論文中用的是到目標節點的絕對距離進行表示。

路徑搜尋的主要過程與A*類似，透過由優先佇列裡不斷將Key最小的值取出來，透過相應的處理將相關鄰接節點或者狀態變動的節點加入到佇列中，直到滿足終止條件即可獲得最短路徑。具體搜尋過程在下面介紹。

總共有三種狀態的節點：區域性一致g（s）=rhs（s）、區域性過一致g（s）>rhs（s）、區域性欠一致g（s）

區域性一致

（Locally Consistent）：當所有節點均為區域性一致狀態時，g（s）的值等於s到起始點的最短距離。這個概念很重要，當上述條件滿足時，我們可以找到任意一點u到起始點的最短路徑，假設當前位置為s，父輩節點s’（向著起始點前進的下一個節點）透過最小化（g（s’）+c（s，s’））來獲得，不斷重複直到到達sStart。

區域性過一致

（Locally Overconsistent）：g（s）>rhs（s）。當優先佇列U中取出的節點為區域性過一致狀態時，意味著g（s）可以透過父輩節點使自己到起點的路徑更短，即

點到

點的邊緣代價函式

值變低，代表網格上障礙物被清除（例如c值從

變為

）或搜尋到一條更短的“捷徑”。此時將設定g（s）=rhs（s），節點狀態變為區域性一致狀態。

區域性欠一致

（Locally Underconsistent）：g（s）

點的資訊需要被重置，這時候就需要重新搜尋計算s點及s點之後與其有關的點的最短路徑。則當它由優先佇列中取出時，將其g值設定為無窮大，即將該節點狀態變為區域性過一致，而區域性過一致的點將會被再次新增到優先佇列中，這樣就可以在它下次被取出時將其作為區域性過一致狀態處理，最終達到區域性連續狀態

LPA *維持每種頂點s的起始距離g *（s）的兩種估計：g值g（s）和rhs值（即右手邊值，從［18］借來的術語 ］）。 頂點的rhs值基於其前繼點（已探索過的點）的g值，因此可能比它們更好的資訊。 它始終滿足以下關係（不變式1）

在呼叫Initialize（）之後，Main（）呼叫ComputeShortestPath（）來查詢從開始到目標頂點的最短路徑。

ComputeShortestPath（）以優先佇列中的鍵（Key）的非遞減順序（從小到大）重複重新計算區域性不一致頂點的g值（“擴充套件頂點”）{10-16}。

區域性不一致的頂點中，當且僅當 g（s）> rhs（s）時，被稱為區域性過度一致。

當ComputeShortestPath（）擴充套件區域性過度一致的頂點{12-13}時，它保持rhs（s）= g *（s），這意味著k（s）= ［f（s）； g *（s）］ ，其中f（s）= g *（s）+ h（s，sgoal）。

在頂點擴充套件期間，ComputeShortestPath（）將頂點的g值設定為其rhs值，從而將其起始距離{12}設定為期望值，並使頂點區域性一致。然後它的g值不再改變，直到ComputeShortestPath（）終止［21］。

，表示當前

點到

點的最短路徑距離，

是對

值的估計。

區域性不一致的頂點中，當且僅當 g（s）

當ComputeShortestPath（）擴充套件區域性欠一致的頂點{15-16}時，它只是將頂點的g值設定為無窮大{15}。這使得頂點可以變為區域性一致或過度一致。

如果擴充套件頂點區域性過度一致，那麼其g值的變化會影響其後繼點的區域性一致性{13}。同樣，如果展開的頂點區域性欠一致，那麼它及其後繼點也可能會受到影響{16}。

為了維護不變數1-3，ComputeShortestPath（）因此更新這些頂點的rhs值，檢查它們的本地一致性，並根據需要將它們新增到優先順序佇列中或從優先順序佇列中刪除{06-08}。

LPA *展開頂點，直到目標頂點區域性一致，並且接下來展開的頂點的鍵不小於目標頂點的鍵。如果搜尋後g（sgoal）=∞，則從開始到目標頂點沒有有限成本路徑（沒有可行路徑）。否則，人們可以找到從開始到目標頂點的最短路徑，如下所示：一個總是從當前頂點s開始（s從目標頂點開始），到最小化（g（s‘）+ c（s’））的任何前驅點s‘ ，直到達到起始頂點（連線可以任意打破）［21］。這樣得到一個遍歷從開始到目標頂點的最短路徑。

LPA*和D* Lite主要區別在於：

LPA*是在反覆規劃著起始網格點和目標點之間的最短路徑，起始點Start 是固定不變的，所以當移動機器人移動後，在環境資訊改變後規劃出的路徑對於當前時刻的移動機 器人來講並非最優的，D* Lite在此基礎上做了改進，將當前位置點視為新的Start網格點，反覆計算著Goal點與新的Start點的最短路徑。所以D* Lite第一次搜尋是規劃從Goal到Start點的反向的搜尋（正向搜尋的話，由於Start點在變動，相關變數需要重新計算），一些變數例如h，g，rhs的定義也與LPA*恰好相反。

D * Lite

到目前為止，我們已經描述了我們的LPA *，它重複確定起始頂點和目標頂點之間的最短路徑，因為圖形的邊緣成本發生了變化。它的第一次搜尋與A *搜尋相同，但後續搜尋會重複使用先前搜尋的資訊。我們現在使用LPA *開發D * Lite ［22］，當機器人向目標頂點移動時，重複確定機器人當前頂點和目標頂點之間的最短路徑，因為圖形的邊緣成本會發生變化。 D * Lite不會對邊緣成本如何變化做出任何假設，無論它們是上升還是下降，它們是否接近機器人的當前頂點或遠離它，或者它們是否在世界上發生變化或僅僅因為機器人知識的變化。然後，未知地形中的目標導向導航問題是此問題的一個特例，其中圖形是八連線網格，其邊緣成本最初為1，當機器人發現無法遍歷時，其變為無窮大。我們首先描述一個簡單版本的D * Lite，然後是一個更復雜的版本。

第一種，簡單版本 D * Lite：

圖4

第一版D * Lite

我們已經提過，當機器人移動到目標頂點並觀察過程中的障礙時，許多目標距離保持不變。因此，我們可以使用LPA *版本來解決未知地形中的目標導向問題。LPA*中呈現的版本從起始頂點搜尋到目標頂點。其g值是起始距離的估計值。因此，我們需要切換LPA *的搜尋方向，以便g值是目標距離的估計值。這種版本的LPA *從目標頂點搜尋到起始頂點，並且可以透過反轉原始圖的所有邊並交換其起始點和目標頂點來匯出。啟發式h（s，s’）現在需要是非負的和向後一致的，即服從h（sstart，sstart）= 0和h（sstart，s）≤h（sstart，s‘）+ c（s’ ，s）所有頂點s∈S和s‘∈Pred（s）。更一般地，由於機器人移動並因此改變起始頂點，因此啟發式需要滿足所有sstart∈S的該屬性。如果搜尋後g（sstart）=∞，則從開始到目標頂點沒有有限成本路徑。否則，人們可以沿著從開始到目標頂點的最短路徑，始終從當前頂點s開始（s從起始頂點開始），到最小化c（s，s’）+ g（s‘）的任何後繼s’直到達到目標頂點（關係可以任意打破）。

為了解決未知地形中的目標導向導航問題，CalcKey（），Initialize（），UpdateVertex（）和ComputeShortestPath（）函式可以保持不變。但是，Main（）函式需要進行擴充套件，以便移動機器人，然後適當地重新計算優先順序佇列中頂點的鍵。這是必要的，因為當機器人移動時啟發式變化，因為它們是相對於機器人的當前頂點計算的。這僅更改優先順序佇列中頂點的鍵，但不更改那些區域性一致的頂點在優先順序佇列中鍵。圖4顯示了最終的搜尋方法，稱為D * Lite的第一個版本。

D * Lite的第一個版本的主函式Main（）首先呼叫Initialize（）來初始化搜尋問題{17‘}。 Initialize（）將所有頂點的初始g值設定為無窮大，並根據（1）{03’-04‘}的等效值設定其rhs值。因此，最初目標頂點是唯一的區域性不一致頂點，並且插入到否則為空的優先順序佇列中，其中key根據先前給出的公式{05’}的等效值計算。注意，在實際實現中，Initialize（）僅需要在搜尋期間遇到頂點時初始化頂點，因此不需要預先初始化所有頂點。這很重要，因為頂點的數量可能很大，在搜尋過程中可能只有少數幾個頂點。然後，D * Lite的第一個版本計算從機器人sstart的當前頂點到目標頂點{18‘}的最短路徑。如果機器人尚未到達目標頂點{19’}，則它沿著最短路徑進行一次移動並更新sstart以反映機器人的當前頂點{21‘-22’}。然後掃描邊緣成本的變化{23‘}。要維護不變數1-3，如果某些邊緣成本發生了變化，它會呼叫UpdateVertex（）{27’}來更新可能受更改的邊緣成本影響的頂點的rhs值和鍵以及它們在優先順序佇列中的成員資格。它們變得區域性一致或不一致。最後，它更新優先順序佇列{28‘-29’}中所有頂點的鍵，重新計算最短路徑{30‘}並迭代。我們可以證明以下定理：

定理1

D * Lite的第一個版本的ComputeShortestPath（）最多擴充套件一個頂點兩次，即當它在本地不一致時最多一次，並且當它在本地過於一致時最多一次，從而終止。然後，可以沿著從開始到目標頂點的最短路徑，始終從當前頂點s開始，從起始頂點開始，到最小化c（s，s’）+ g（s‘）的任何後繼s’，直到到達目標頂點（關係可以任意打破）。

第二種版本:

圖5

D * Lite的第二版

D * Lite的第一個版本具有以下缺點：優先順序佇列{28‘-29’}的重複重新排序可能是昂貴的，因為優先順序佇列通常包含大量頂點。 D * Lite的第二個版本，如圖5所示，使用從D * ［5］派生的搜尋方法，以避免重新排序優先順序佇列。與第一版D * Lite的差異以粗體顯示。啟發式h（s，s‘）現在需要是非負的和前後一致的，即服從h（s，s’‘）≤h（s，s’）+ h（s‘，s’‘）對於所有頂點s，s’，s‘’∈S。無論目標頂點是什麼，它們也需要是可接受的，即服從所有頂點s，s‘∈S的h（s，s’）≤c*（s，s‘），其中c *（s， s’）表示從頂點s∈S到頂點s‘∈S的最短路徑的成本。具有這些屬性的啟發式演算法還滿足啟發式需要滿足第一版D * Lite ［21］的屬性。然而，它們並沒有過於嚴格限制，因為如果透過放鬆搜尋問題來推匯出啟發式演算法，它們將被保證［21］，這幾乎總是如此，並且適用於本文中使用的啟發式演算法。

D * Lite的第二個版本使用的鍵是D * Lite的第一個版本用於相應頂點的鍵的下限。它以與第一版D * Lite相同的方式初始化它們。在機器人從頂點s移動到某個頂點s’之後，它檢測到邊緣成本的變化，鍵的第一個組成部分最多可以減少h（s，s‘）。 （第二個分量不依賴於啟發式，因此保持不變。）因此，為了保持下界，D * Lite需要從中所有頂點的鍵的第一個分量中減去h（s，s’）。優先佇列。但是，由於h（s，s‘）對於優先順序佇列中的所有頂點是相同的，因此如果不執行減法，則優先順序佇列中的頂點的順序不會改變。然後，當計算新金鑰時，它們的第一個分量是h（s，s’）相對於優先順序佇列中的金鑰太小。因此，必須將h（s，s‘）新增到它們的第一組分中。如果機器人再次移動然後再次檢測到成本變化，那麼常量需要加起來。我們在變數km（即鍵修飾符）{30“}中執行此操作。因此，無論何時計算新鍵，都必須將變數km新增到其第一個元件中，如{01”}中所做。然後，在機器人移動並且優先順序佇列不需要重新排序之後，優先順序佇列中的頂點的順序不會改變。另一方面，在D * Lite的第一個版本的鍵的第一個分量增加了當前的km值之後，鍵總是在D * Lite的第一個版本的相應鍵上的下限。是，由CalcKey（）{01“}計算的值的下限。我們透過更改ComputeShortestPath（）來利用此屬性，如下所示。在ComputeShortestPath（）刪除了具有最小鍵kold = U。TopKey（）的頂點u之後優先順序佇列{12“}，它現在使用CalcKey（）來計算它本應具有的金鑰。如果kold <˙CalcKey（u），則它將刪除的頂點與CalcKey（）計算的金鑰重新插入優先順序佇列{13“-14”}。因此，在D * Lite的第一個版本的鍵的第一個元件被增加之後，優先順序佇列中所有頂點的鍵都是D * Lite的第一個版本的相應鍵的下限仍然是正確的。當前的km值。如果kold≥˙CalcKey（u），則它保持kold≐CalcKey（u），因為kold是CalcKey（）返回的值的下限。在這種情況下，ComputeShortestPath（）對頂點u執行與第一版D * Lite {15“-20”}的ComputeShortestPath（）相同的操作。 ComputeShortestPath（）以與第一版D * Lite的ComputeShortestPath（）完全相同的順序對頂點執行這些操作，這意味著D * Lite的第二個版本與D * Lite的第一個版本共享許多屬性，包括正確性。在形式上，我們可以證明以下定理：

D * Lite的第二個版本的ComputeShortestPath（）最多擴充套件一個頂點兩次，即當它在本地不一致時最多一次，並且當它本地過度一致時最多一次，從而終止。 然後，可以沿著從開始到目標頂點的最短路徑，始終從當前頂點s開始，從起始頂點開始，到最小化c（s，s’）+ g（s‘）的任何後繼s’，直到 到達目標頂點（關係可以任意打破）。

km表示每次移動機器人實際移動距離值之和，是為了防止每次邊緣函式發生改變時重新搜尋都要重新去計算一下優先順序佇列，因為加減相同的值對各個點間k1值的大小沒有影響，這樣可以提高計算效率。key值越小，其優先權越高，該點就越先被搜尋和更新。key值之間的比較方式與LPA*一樣。

第二種版本最佳化：

圖6

下面以一個具體例子來講解演算法的工作流程：

圖6

我們現在逐步完成圖1中的示例，以顯示第二版D * Lite的操作。圖6（上半圖）顯示了機器人最初知道的不可穿越的單元格。它還顯示了可穿越細胞單元格的啟發值h，其距離近似於從起始細胞到這個細胞的這兩個細胞的x和y座標的絕對差異的最大值。圖6（下圖）顯示了可穿越細胞的g值和rhs值，對於區域性不一致的細胞，也顯示了它們的鍵。

在每個時間點，本地不一致的單元格在優先順序佇列中。具有最小鍵的本地不一致單元格具有粗體邊框以指示它將在下一次擴充套件。

標記為“初始化”的網格直接顯示在首次呼叫ComputeShortestPath（）之前的值。

下一個網格顯示第一次呼叫ComputeShortestPath（）的每次迭代後的值。如果擴充套件單元格的g值大於其rhs值，則ComputeShortestPath（）將單元格的g值設定為其rhs值。否則，ComputeShortestPath（）將g值設定為無窮大。

為了維護不變數1-3，ComputeShortestPath（）然後重新計算可能受此賦值影響的單元格的rhs值，檢查單元格是否在本地一致或不一致，並（如果需要）將它們從優先順序佇列中刪除或新增到優先順序佇列中。然後它重複這個過程，直到它確定它找到了一條最短路徑，這需要它重新計算一些g值，但不是全部。最後一個網格顯示ComputeShortestPath（）返回後的值。

請注意，A *方法搜尋可以完全相同的順序擴充套件完全相同的單元格（靜態環境下）。

然後，可以透過從當前單元開始並始終貪婪地減小目標距離，沿著從機器人的當前單元到目標單元的最短路徑。任何這樣做的方式都會導致從當前單元格到目標單元格的最短路徑。由於所有成本都是1，這意味著從當前小區B1到目標小區E3的最短路徑是透過小區C1和D2。

ComputeShortestPath（）迴圈結束時候會返回一條可行路徑或者沒有可行路徑。

圖7

第一次ComputeShortestPath（）結束後，得到了一條可行路徑，然後機器人按照得到的最短可行路徑移動到單元格C1並發現單元格D2是不可移動的。

圖7（上）顯示了機器人現在感知的網格。該圖還顯示了可穿越細胞的新啟發值h。

標記為“邊緣成本更改”的網格直接顯示在第二次呼叫ComputeShortestPath（）之前的值。

下一個網格顯示第二次呼叫ComputeShortestPath（）的每次迭代後的值。最後，最後一個網格顯示ComputeShortestPath（）返回後的值。從當前小區C1到目標小區E3的最短路徑是經由小區D1和E2。然後機器人從其當前單元到目標單元遵循該路徑，而不觀察其他不可穿行的單元，因此無需進一步呼叫ComputeShortestPath（）。

圖2

我們現在使用圖2中的兩個八連線網格來比較第二版D * Lite與幾種選擇：完全（即非增量）無知搜尋（廣度優先搜尋），完整啟發式搜尋（A *），和增量不知情搜尋（DynamicSWSF-FP，修改為在識別出從機器人當前單元到目標單元的最短路徑後立即終止）。為了使搜尋方法具有可比性，它們的搜尋始終從目標單元格開始，然後繼續朝向機器人的當前單元格。此外，我們考慮透過所有搜尋方法擴充套件機器人的當前單元格。我們使用兩個單元的x和y座標的絕對差值的最大值作為其距離的啟發式估計。透過這些方法擴充套件的單元在圖8中以灰色陰影顯示。在我們的示例中，我們以最有利的方式打破了A *的關係，因此D * Lite和A *在第一次搜尋期間不會完全展開相同的單元格。 D * Lite的第二次搜尋僅擴充套件目標距離改變或之前未計算的那些單元格的子集。因此，它比複製其先前搜尋的大部分的A *搜尋更有效。更一般地，啟發式搜尋在第一次和第二次搜尋期間優於未知資訊搜尋，增量搜尋在第二次搜尋期間（之前的搜尋結果可用）優於完整搜尋，並且增量啟發式搜尋減少了擴充套件單元格的數量甚至更多比單獨的啟發式搜尋或增量搜尋。

圖8

我們現在使用圖2中的兩個八連線網格來比較第二版D * Lite與幾種選擇：完全（即非增量）無知搜尋（廣度優先搜尋），完整啟發式搜尋（A *），和增量不知情搜尋（DynamicSWSF-FP，修改為在識別出從機器人當前單元到目標單元的最短路徑後立即終止）。為了使搜尋方法具有可比性，它們的搜尋始終從目標單元格開始，然後繼續朝向機器人的當前單元格。此外，我們考慮透過所有搜尋方法擴充套件機器人的當前單元格。我們使用兩個單元的x和y座標的絕對差值的最大值作為其距離的啟發式估計。透過這些方法擴充套件的單元在圖8中以灰色陰影顯示。在我們的示例中，我們以最有利的方式打破了A *的關係，因此D * Lite和A *在第一次搜尋期間不會完全展開相同的單元格。 D * Lite的第二次搜尋僅擴充套件目標距離改變或之前未計算的那些單元格的子集。因此，它比複製其先前搜尋的大部分的A *搜尋更有效。更一般地，啟發式搜尋在第一次和第二次搜尋期間優於未知資訊搜尋，增量搜尋在第二次搜尋期間（之前的搜尋結果可用）優於完整搜尋，並且增量啟發式搜尋減少了擴充套件單元格的數量甚至更多比單獨的啟發式搜尋或增量搜尋。

重要的是要理解D *和D * Lite的第二個版本有一些相似之處，但工作方式不同。兩種搜尋方法都從機器人的目的地搜尋到其當前頂點。兩種搜尋方法都使用啟發式方法來集中他們的搜尋並使用相同的機制來考慮當機器人移動並且搜尋的目標因此改變時啟發式改變。兩種搜尋方法都在兩個波中傳播成本變化：D *使用RAISE和LOWER頂點，而D * Lite使用區域性不一致和過度一致的頂點用於類似目的。一旦優先順序佇列中所有頂點的最小鍵不再小於機器人當前頂點的鍵，兩種搜尋方法都會停止。最後，兩種搜尋方法通常比A *更有效，因為邊緣成本圍繞機器人的當前頂點而變化，因此接近搜尋的目標，這是機器人的機載感測器感知地形的屬性的結果。機器人附近。然而，儘管存在這些相似之處，但這兩種搜尋方法的工作方例如，D *可以在相同的重新計劃過程中擴充套件頂點兩次以上，而D * Lite保證每個頂點最多擴充套件兩次。這解釋了為什麼兩種搜尋方法在完全相同的地形中頂點擴充套件和堆滲透的數量不同。

uncertain環境下，用POMDP決策

參考：

Fast replanning for navigation in unknown terrain

搬磚的旺財：路徑規劃——D* Lite演算法

LPA* 路徑搜尋演算法介紹 - 肚皮朝上的刺蝟的部落格 - CSDN部落格