終身規劃A*算法（LPA*）：Lifelong Planning A*

LPA_start或life Planning A_star是一種基於A*的增量啓發式搜索算法。2001年，斯文·柯尼格(Sven Koenig)和馬克西姆·利卡切夫(Maxim Likhachev)首次提出。

1.描述

LPA_star是A_star的增量版本，它可以適應圖形中的變化而無需重新計算整個圖形，方法是在當前搜索期間更新前一次搜索的g值（從開始起的距離），以便在必要時進行更正。與A_star一樣，LPA*使用啓發式算法，該啓發性來源於從給定節點到目標路徑代價的更低邊界。如果保證是非負的（零可以接受）並且從不大於到目標的最低路徑的代價，則允許該啓發式。

啓發式搜索和增量式搜索的區別在於，啓發式搜索是利用啓發函數來對搜索進行指導，從而實現高效的搜索，啓發式搜索是一種“智能”搜索，典型的算法例如A_star算法、遺傳算法等。增量搜索是對以前的搜索結果信息進行再利用來實現高效搜索，大大減少搜索範圍和時間，典型的例如LPA_star、D_star Lite算法等。

2.父代節點與子代節點

除了開始節點和目標節點外，每個節點n都有上一代和下一代節點：

• 任何一條邊指向n的節點都是n的前一代；
• 從n引出一條邊的任何節點都是n的後繼節點。

在下面的描述中，這兩個術語僅指直接的父代（上一代）和子代（下一代），不指父代的父代或子代的子代。

3.起始距離估計

LPA_star對每個節點的起始距離g_star(n)保持兩個估計值：

• g(n)爲之前計算的g值(起始距離)，如在A*中的含義；
• rhs（n），一個基於節點父代（上一代）的g值的預測值（所有g（n’）+d（n’，n）的最小值，其中n’是n的父代，d（x，y）是連接x和y的邊的代價）。
  對於開始節點，以下始終爲真：
  $rhs(start)=g(start)=0$

如果rhs(n)等於g(n)，則稱n局部一致的。如果所有節點都是局部一致的，那麼最短路徑可以用A*來確定。但是，當邊緣代價發生變化時，只需要對與路由相關的節點重新建立局部一致。

4.優先隊列

當一個節點在局部變得不一致時(因爲它的父代節點的代價或將它鏈接到父代節點的邊緣發生了變化)，它將被放在一個優先級隊列中進行重新評估。
LPA*使用二維key：

元素由$k_1$(它直接對應於A*中使用的f值)排序，然後由$k_2$排序。

根據更小的k(n)的值搜尋更優的路徑節點，當一個節點n比另一個節點n’更優時，n節點的k(n)不大於k(n’)，即：

5.節點狀態及擴展

隊列頂部節點展開如下：

• 局部一致（Locally Consistent）：g(s)=rhs(s)。當所有節點均爲局部一致狀態時，g(s)的值等於s到起始點的最短距離（注意，反向不成立）。這個概念很重要，當上述條件滿足時，可以找到任意一點u到起始點的最短路徑，假設當前位置爲s,父輩節點s’(向着起始點前進的下一個節點)通過最小化(g(s’)+c(s,s’))來獲得，不斷重複直到到達sStart。然而，LPA*並不需要使所有節點均爲局部一致狀態，它通過啓發式搜索將關注點放在搜索上，並且只更新那些與計算最短路徑相關的節點的g值。
• 局部過一致（Locally Overconsistent）：g(s)>rhs(s)。當優先隊列U中取出的節點爲局部過一致狀態時，意味着g(s)可以通過父輩節點使自己到起點的路徑更短，此時將設置g(s)=rhs(s)，節點狀態變爲局部一致狀態。
• 局部欠一致(Locally Underconsistent)：g(s)<rhs(s)。這種情況通常出現在父輩的某一節點突然變爲障礙的情況下，造成父輩節點到起點的路徑變大，從而需要修改g(s)的值，如果節點處於這種狀態，則當它由優先隊列中取出時，將其g值設置爲∞，即將該節點狀態變爲局部過一致，而局部過一致的點將會被再次添加到優先隊列中，這樣就可以在它下次被取出時將其作爲局部過一致狀態處理，最終達到局部一致狀態（如果這一節點與要搜索的最短路徑相關的話）。

由於更改節點的g值也可能更改其後續節點的rhs值(從而更改它們的局部一致)，因此將對它們進行評估，並在必要時更新它們的隊列成員和key。

節點將繼續擴展到下一個節點，直到滿足以下兩個條件:

• 目標是局部一致的
• 優先級隊列頂部的節點有一個大於或等於目標的key。

6.初始化運行

當邊的代價發生變化時，LPA*檢查受變化影響的所有節點，即其中一條變化邊終止的所有節點(如果一條邊可以在兩個方向上遍歷，且變化影響兩個方向，則檢查由該邊連接的兩個節點)：

• 更新節點的rhs值。
• 已成爲局部一致的節點將從隊列中刪除。
• 已成爲局部不一致的節點將添加到隊列中。
• 保持局部不一致的節點的key已更新。

之後，節點繼續擴展，直到達到結束條件。

7.最短路徑搜索

節點展開完成後(即滿足退出條件)，計算最短路徑。如果目標的代價爲無窮大，那麼從開始到目標沒有有限的代價路徑。否則，最短路徑可以通過向後移動來確定：

• 從目標開始。
• 移動到當前節點n的父代n’，其中g（n’）+d（n’，n）最低（如果最低分數由多個節點共享，則每個節點都是有效的解決方案，其中任何一個都可以任意選擇）。
• 重複上一步，直到開始。

8.僞代碼

該代碼假定一個優先隊列queue，該隊列支持以下操作：

• topKey()返回隊列中任何節點的(數字上的)最低優先級(如果隊列爲空，則返回無窮大)；
• pop()從隊列中刪除優先級最低的節點並返回它；
• insert(node, priority) 將具有給定優先級的節點插入隊列；
• remove(node) 從隊列中移除一個節點；
• contains(node) 如果隊列包含指定節點，則返回true;如果不包含指定節點，則返回false；

void main() {
  initialize();
  while (true) {
    computeShortestPath();
    while (!hasCostChanges())
      sleep;
    for (edge : getChangedEdges()) {
        edge.setCost(getNewCost(edge));
        updateNode(edge.endNode);
    }
  }
}

void initialize() {
  queue = new PriorityQueue();
  for (node : getAllNodes()) {
    node.g = INFINITY;
    node.rhs = INFINITY;
  }
  start.rhs = 0;
  queue.insert(start, calculateKey(start));
}

/** 優先級隊列中擴展節點. */
void computeShortestPath() {
  while ((queue.getTopKey() < calculateKey(goal)) || (goal.rhs != goal.g)) {
    node = queue.pop();
    if ((node.g > node.rhs) {
      node.g = node.rhs;
      for (successor : node.getSuccessors())
        updateNode(successor);
    } else {
      node.g = INFINITY;
      updateNode(node);
      for (successor : node.getSuccessors())
        updateNode(successor);
    }
  }
}

/** 重新計算節點的rhs並將其從隊列中刪除。.
 * 如果節點在局部變得不一致，則使用其新key(re-)將節點插入隊列。. */
void updateNode(node) {
  if (node != start) {
    node.rhs = INFINITY;
    for (predecessor: node.getPredecessors())
      node.rhs = min(node.rhs, predecessor.g + predecessor.getCostTo(node));
    if (queue.contains(node))
      queue.remove(node);
    if (node.g != node.rhs)
      queue.insert(node, calculateKey(node));
  }
}

int[] calculateKey(node) {
  return {min(node.g, node.rhs) + node.getHeuristic(goal), min(node.g, node.rhs)};
}

9.性質

由於算法上類似於A*， LPA*具有許多相同的屬性。

• 對於LPA的每次運行，每個節點最多展開(訪問)兩次。每個LPA運行最多擴展一次局部過一致節點，因此其初始運行(每個節點進入過一致狀態)的性能與A*類似，都是最多訪問每個節點一次。
• 每次運行時展開的節點key都是單調的不隨時間遞減的，就像A*的情況一樣。
• 啓發式信息越豐富(因而越大)(同時仍然滿足可接受性標準)，需要擴展的節點就越少。
• 優先級隊列實現對性能有重大影響，如在A*中。與效率較低的實現相比，使用斐波那契堆可以顯著提高性能。

對於一個A_star實現，它打破了兩個具有相等f值的節點之間的聯繫，而支持具有較小g值的節點(在A*中沒有定義)，下面的陳述也是正確的:

• 擴展局部過一致節點的順序與A*相同。
• 在所有局部過一致的節點中，只有那些代價不超過目標的節點需要擴展，就像A*中的情況一樣。
  LPA_star還具有以下特性:
• 當邊的代價發生變化時，LPA的性能優於A(假設後者是從頭開始運行的)，因爲只需要再次擴展一部分節點。

下文將結合Lifelong Planning A*論文的一個例子簡要介紹LPA_star算法的主要過程。

10.符號表示

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

succ(s)：successors,節點s的後續節點（子代節點）集合，例如節點1，2，3…i按順序均已被搜索過，那麼除了1~i的其它結點均屬於succ(i)。

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

c(s,s’)：兩節點之間的代價函數，0<c(s,s)≤∞。

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

g(s)：節點s到起始點的預計最短距離，而g*(s)值是實際的最短距離，這個值是一個預計值，是隨着算法求解進程不斷變動的，當所有節點的g(s)=rhs(s)時，g(s)的值就是到起始點的實際最短距離，即g(s)=g*(s)。

rhs(s)：right hand sides，來自DynamicSWSF-FP算法。rhs值是基於g值的一步前瞻值，因此可能比g值更好地信息反饋。對於s的所有鄰接節點，求它們到s的距離加上鄰接節點自身的g值，其中最小的那個值作爲s的rhs值。具體求法可以見下面的公式：

舉個例子，圖5中左側網格世界給出的g值。方格A0的rhs值爲3，即方格A1+1的gvalue和方格B1+1的g值的最小值。因此，方格A0的g值等於它的rhs值，即爲局部一致。這個概念很重要，因爲如果所有單元格都是局部一致的，那麼所有的g值都等於各自的起始距離。

U：同A_star算法中的優先隊列，依據每個節點的Key值進行排序。
Key[K1,K2]：優先隊列排序依據的鍵值，包含兩部分，K1與K2，優先比較K1，若相同則比較K2進行排序。在rhs(s)=g(s)的情況下，K1大致等同於A_star裏的f(s)，K2大致等同於A*裏的g(s)，K1與K2的計算方法見下面的圖。

當rhs(s)<g(s)時，將g(s)設置爲rhs(s)。

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

11.算法示例推演

論文中以二維平面網格地圖作爲演示對象，每一個網格與周圍八個網格相連（相互之間可以直接到達），黑色網格爲障礙。

第一次搜索：地形發生變動之前的路徑搜索，與A_star搜索基本相同。起點爲右上角的點，目標點爲左下角的點。第一張圖描述了各點的到起始點的最短距離以及各點到目標點的h值。左側爲g_star值（由當前位置到起始點的代價，距離採用曼哈頓計算公式），右側爲h值（由當前位置到目標點的代價）。最開始所有點rhs和g均設爲無窮，然後由起始點開始，將起始點的rhs設置爲0，然後如上圖過程不斷迭代，直到獲得最短路徑。

在第一次迭代時rhs(s)=g(s)，K1大致等同於A_star裏的f(s)，K2大致等同於A_star裏的g(s)。此時按照A_star算法進行搜索，搜索的過程是按照key[K1;K2]的優先級進行搜索的。

第二次搜索：當場景中地形狀態發生變動，在該例子中，節點(D,1)變爲障礙，只改變了三個起始距離，即單元D1、E1和F0。這允許LPA*有效地重新規劃最短路徑，即使從開始單元到目標單元的最短路徑完全改變。

這是重用以前的規劃構建過程的一部分（以g值的形式），而不是以更大的內存需求爲代價來調整先前規劃的優點。特別是，g值不僅可以用來確定最短路徑，而且它們比最短路徑本身更容易再次使用。

首先對該節點進行更新，並將其後代節點置於優先隊列中，該節點的變動對父輩節點的狀態並無影響。同第一次搜索類似，不斷進行迭代直到再次找到到目標位置的最短路徑。

在迭代中，第二次到第三次之間爲父輩的某一節點突然變爲障礙的情況下，造成父輩節點到起點的路徑變大。此時，g(s)<rhs(s)，爲局部欠一致(Locally Underconsistent)狀態。節點處於這種狀態，則當它由優先隊列中取出時，將其g值設置爲無窮大。接下來的搜索過程，與第一次的搜索過程類似。

12.總結

LPA_star提出了一種解決動態環境下的最短路徑搜索方法，但是它針對的是起始點和目標點固定的情況，如果在機器人按照搜索到的最短路徑行走過程中，環境中某些節點發生變化，這時如果想獲得新的路徑，就得以機器人當前節點爲起始點，重新用LPA_star算法解算一次，這效率是很低的。針對這種情況，該論文的作者隨後提出了D_starLite算法來作爲處理動態環境最短路徑問題的高效方法。

13.對公式的進一步理解

來源於路徑規劃——Lifelong Planning A*算法

和A_star算法一樣，LPA_star算法採用非負、一致性的啓發函數 h(s) 表示當前位置網格點 s 到目標點 Goal 的距離的估計， h(s) 一致性體現在服從以下三角不等式，可以由簡單的三角形幾何性質可以推出：

其中 s≠Goal 且 s屬於succ(s)，如圖1所示：

• succ(s)或 children(s) ，表示網格點 s 下一時刻將要移動到的點，相當於 s 點的繼承點，對於 Goal 點，有$succ(Goal)=\oslash$

• pred(s) 或 parent(s) ，表示前一時刻移動到當前位置 s 點來的網格點，相當於 s 點的前輩點，對於 Start 點，有 KaTeX parse error: Can't use function '\(' in math mode at position 5: pred\̲(̲Start)=\oslash；

• g*(s) ，表示 Start 點到當前 s 點的最短路徑距離， g(s) 是對g*(s) 值的估計， $g^*(s)$ 定義如下：

• rhs(s)被定義爲：

其中 g(s’) 是節點 s’ 到起始節點 s_{start} 的代價，類似於A*算法中的 g(n) ， c(s’,s) 表示從節點 s’ 到節點 s 的代價，被稱爲邊緣代價函數。

在圖2的a)中，
$g(s)=A+B$
$rhs(s)=g(s&#x27;)+c(s&#x27;,s)=A+B$

此時屬於“正常情況”，即 g(s)=rhs(s) ，此時 s 點爲局部一致(locally consistent)；

在圖2的b)中，如果邊緣代價函數值 c(s’,s) 由於環境發生改變從 B 變爲 ∞ (即自由網格點 s 被障礙物佔據)，則有
$g(s)=A+B$
$rhs(s)=g(s&#x27;)+c(s&#x27;,s)=A+∞=∞$

這種情況有 ，此時 s 點爲局部不一致(locally inconsistent)。

局部不一致又分爲過一致(Overconsistent)和欠一致(Subconsistent)。

當 g(s)>rhs(s) 被稱爲局部過一致，即 s’ 點到 s 點的邊緣代價函數 c(s,s’) 值變低，代表網格上障礙物被清除(例如c值從 ∞ 變爲 B )或搜索到一條更短的“捷徑”。

當 g(s)<rhs(s) 被稱爲局部欠一致，當 s 點欠一致時，即 s’ 點到 s 點的邊緣代價函數c值變高，代表着自由網格被障礙物所佔據，這時 s 點的信息需要被重置，這時候就需要重新搜索計算最短路徑。

• Openlist或priority queue，和A*算法一樣，表示被搜索網格點的集合，用 key(s) 來對這些網格點進行排序，值得注意的是所有Openlist的點都局部不一致，所有局部不一致的點都在列表上。

• key(s)，代表着優先級隊列中網格點選擇的優先權，key值用於處理局部不一致的網格點， key(s) 由兩個元素組成， key(s) 被定義爲：
  $key(s)=[k1(s);k2(s)]$
  其中：
  $k1(s)=min{g(s),rhs(s)}+h(s,Goal)$
  $k2(s)=min{g(s),rhs(s)}$

key值之間的比較方式如下：

if k1(s)<= k1(s') or (k1(s)=k1(s') and k2(s)<= k2(s'))
    k(s)<= k(s')
end

key值越小，其優先權越高，該點就越先被搜索。

僞代碼

For each s in Graph
    s.g(x)=rhs(x)=∞;局部一致
end for each
startNode.rhs=0;局部過一致
Forever
    While(OpenList.Top().key < goal.key OR goal is incosistent)
    當OpenList中最優先點的key值小於目標點的key值或者目標點局部不一致時
        currentNode=OpenList.Pop()
        當前點爲OpenList中最優先點，並將U中最優先的網格點刪除
        if (currentNode is overconsistent)
        如果當前點局部過一致，代表網格上障礙物被清除或搜索到更短的“捷徑”
            currentNode.g(x)=currentNode.rhs(x);
            令當前點的g(x)=rhs(x)，使其局部一致
        else
        否則
            currentNode.g(x)=∞;
            局部過一致g(s)>rhs(s)或一致g(s)=rhs(s)
        end if
        for each s in currentNode.Children[]
            update s.rhs(x);
            局部過一致g(s)>rhs(s)或一致g(s)=rhs(s)
        end for each
    End while
    Wait for changes in Graph
    For each connection (u,v) with changed cost
        Update connection(u,v);
        Make v locally inconsistent;
    end for each
End forever

參考資料

[1] Wiki百科：Lifelong Planning A*
[2] 路徑規劃——Lifelong Planning A_star 算法
[3] LPA* 路徑搜索算法介紹
[4] Lifelong planning A∗.pdf
[5]徐開放. 基於D_star Lite算法的移動機器人路徑規劃研究[D]. 哈爾濱工業大學, 2017.
[6]張浩. 地面移動機器人安全路徑規劃研究[D]. 安徽工程大學, 2015.

搜索算法其他文章

Field Ｄstar路徑規劃算法
Dstar Lite路徑規劃算法
D*路徑搜索算法原理解析及Python實現