第五章 路徑規劃
5.2 D* Lite 演算法
2. 當 g>rhs，則這個節點為 over-consistent。
3. 當 g<rhs，則這個節點為 under-consistent。
使用 D* Lite 演算法來規劃路徑有 4 個步驟。
步驟 1：一開始將所有節點的 rhs 和 g 值設成無限大，同時清空開放列表。然後，
將終點的 g 值設為無限大，rhs 值設為零。再將終點加入開放列表，並且定義為 路徑起始點，同時設此點為目前路徑規劃的執行點pointcurrent。
步驟 2：如果pointcurrent是 over-consistent，則令pointcurrent的 g 值等於它的 rhs 值，
然後把所有與pointcurrent相鄰的點加入開放列表，更新它們的 rhs 值，令新的 rhs 值為pointcurrent的 g 值加上到相鄰點所需的代價，然後把pointcurrent移出開放列表。
但如果pointcurrent是 under-consistent，則把它的 g 值設為無限大，然後把與pointcurrent 的點相鄰的點加入開放列表，更新它們的 rhs 值，再把目前的點移出開放列表，
選擇開放列表中優先權較高的點當作機器人的下一個位置。
步驟 3：如果機器人行走時偵測到障礙物，則障礙物那點的 rhs 值設成無限大，
與障礙物相鄰的點的代價更新為無限大。由於機器人不能通過障礙物，之前規劃 的路徑無法使用，於是重新規劃一條路徑從機器人pointcurrent到目的地。
步驟 4：重複執行步驟 2 和步驟 3 直到機器人抵達目的地，或者也有可能沒有任 何路徑存在。若找到路徑，即為可能的最短路徑。
以下介紹路徑規劃中從一格點到周邊各點所需之代價。首先於圖 5-3 中介紹 何謂周邊各點，假設選定中間格點(白色)為pointcurrent，則其周邊 8 格點為周邊各 點，同時加入開放列表，圖 5-3 中 8 格(黃色)即為周邊各點。
圖 5-3 演算法選定中間點時周邊需加入開放列表中 8 格的示意圖
接著介紹移動所需之代價。設定從可行走區域的一格點到對角線的另一格點 行走的代價為 1.4，若從可行走區域的一格點到隔壁另一格點的代價為 1，如圖 5-4 所示。
圖 5-4 從可行走區域到周邊任一格之代價
從可行走區域的一格點到對角線為障礙物的另一格點行走代價為無限大。從 可行走區域的一格點到隔壁為障礙物的另一格點的代價為無限大。從為障礙物的 一格點到對角線為可行走區域的另一格點的代價為無限大。從障礙物的一格點到 隔壁可行走區域的另一格點的代價為無限大。如圖 5-5。
圖 5-5 從可行走區域到障礙物之代價
從為障礙物的一格點到對角線的另一個為障礙物格點的行走代價為無限大。
從為障礙物的一格點到隔壁為障礙物的另一格點的代價為無限大。如圖 5-6。
圖 5-6 從障礙物到障礙物之代價
以下給出一個使用 D* Lite 演算法的實例。棕色的地方為障礙物，黃色的為 加入開放列表的點。在演算法一開始將各點的 g 值和 rhs 值定為無限大，終點的 rhs 值定為零，之後把終點加入開放列表。如圖 5-7[30]。
圖 5-7 初始化
加入開放列表後，因為終點的 g 值>rhs 值，根據演算法則，讓 g=rhs，則 g 值變為零，之後將終點移出開放列表。如圖 5-8[30]。
圖 5-8 將終點加入開放列表
然後將周邊的 8 個格子加入開放列表，障礙物無法加入，因此被加入的點只 有(0, 1)和(1, 0)，新加入的點的 rhs 值變為前一點的 g 值加上從前一點到新加入的 點的代價，所以 2 個新加入的點的 rhs 值變為 1。如圖 5-9[30]。
圖 5-9 計算點(0, 1)和(1, 0)之 rhs 值
接著由於點(0, 1)的 g 值>rhs 值，根據演算法則，讓 g=rhs，則 g 值變為 1，
之後將(0, 1)移出開放列表。如圖 5-10[30]。
圖 5-10 計算點(0, 1)
然後將周邊的 8 個格子加入開放列表，障礙物無法加入，點(1, 0)已經在開放 列表中，而之前被移出開放列表的終點 並不會再加回列表。新加入開放列表的 點只有(0, 2)和(1, 2)，透過演算法計算 2 點的 rhs 值分別為 2 和 2.4。如圖 5-11[30]。
圖 5-11 計算點(0, 2)和(1, 2)之 rhs 值
選擇開放列表中 rhs 值最小的點當作下一個點，重複計算直到起點被計算到。
如圖 5-12[30]以及圖 5-13[30]。
圖 5-12 將大部分格子之 g 值與 rhs 值計算完
圖 5-13 起點被計算到
接著從起點出發，找尋周邊 8 格中 rhs 值最小的點當作機器人下一個移動的 位置，重複這個動作直到機器人抵達終點，中間經過的路徑即為最短路徑。如圖 5-14[30]。
圖 5-14 最後之規劃結果
在機器人的行走過程中發現點(2, 2)是障礙物，機器人不能通過。如圖 5-15[30]。
圖 5-15 機器人發現障礙物
將點(2, 2)的 rhs 值變為無限大，加入開放列表中，並重新計算周邊在規劃路 徑中經過點(2, 2)方格的 rhs 值，點(3, 3)的 rhs 值原本是點(2, 2)方格的 g 值(3.4)加 上從點(2, 2)到點(3, 3)的代價(1.4)，更改後變為點(2, 3)方格的 g 值(3.8)加上從點(2,
3)到點(3, 3)的代價(1)，改完後依然不變，所以不加入開放列表，點(1, 3)、點(1, 2) 與點(2, 3)如同點(3, 3)，如圖 5-16[30]。
圖 5-16 更新點(3, 3)
點(3, 2)的 rhs 值從原本的 4.4 變為點(2, 3)方格的 g 值(3.8)加上從點(2, 3)到點 (3, 2)的代價(1.4)，因此 rhs 值變為 5.2，由於計算後 g 值和 rhs 值不相等，將此點 加入開放列表中，如圖 5-17[30]。
圖 5-17 更新點(3, 2)
點(3, 1)的 rhs 值從原本的 4.4 變為點(3, 2)方格的 g 值(4.4)加上從點(3, 2)到點 (3, 1)的代價(1)，因此 rhs 值變為 5.4，由於計算後 g 值和 rhs 值不相等，將此點 加入開放列表中，如圖 5-18[30]。
圖 5-18 更新點(1, 3)
更改從周邊各點到點(2, 2)的代價，將其變為無限大，如圖 5-19[30]。
圖 5-19 更改周邊各點代價
接著由於點(2, 2)的 g 值<rhs 值，根據演算法令 g 等於無限大，點(2, 2)變為
障礙物，如圖 5-20[30]。
圖 5-20 計算點(2, 2)
令kmin(rhs s g s( ), ( ))，取開放列表中 k 值最小的點當作下一個計算的點，下 一個為點(3, 2)。接著由於點(3, 2)的 g 值<rhs 值，根據演算法令 g 等於無限大，
將其移出開放列表，然後將其周邊的值更新，如圖 5-21[30]。
圖 5-21 計算點(3, 2)
點(4, 2) 的 rhs 值變為 4.8+1.4=6.2，點(3, 1) 的 rhs 值變為 5.4+1.4=6.8，由於
點(4, 2) g 值和 rhs 值不一致，因此將其加入開放列表中，如圖 5-22[30]。
圖 5-22 更新點(4, 2)
點(3, 2) g 值和 rhs 值不一致，因此將其加入開放列表中，如圖 5-23[30]。
圖 5-23 將點(3, 2) 加入開放列表
取開放列表中 k 值最小的點當作下一個計算的點，下一個為點(3, 1)。接著由 於點(3, 1)的 g 值<rhs 值，根據演算法令 g 等於無限大，如圖 5-24[30]。
圖 5-24 計算點(3, 1)
更新點(3, 0)與(4, 0)的 rhs 值，變為前一點的 g 值加上過程的代價，兩點更改 完後皆為無限大，由於 g 值和 rhs 值相同，將其移出開放列表，如圖 5-25[30]。
圖 5-25 更新點(3, 0)與(4, 0)
點(3, 1) g 值和 rhs 值不一致，因此將其加入開放列表中，如圖 5-26[30]。
圖 5-26 將點(3, 1)加入開放列表
令點(3, 2)的 g 值等於 rhs 值，將其移出開放列表，如圖 5-27[30]。
圖 5-27 計算點(3, 2)
令點(3, 1)的 rhs 值等於 5.2+1=6.2，如圖 5-28[30]。
圖 5-28 更新點(3, 1)
改變後的路徑即為新的最短路徑，如圖 5-29[30]。
圖 5-29 路徑規劃結果