路徑規劃——Lifelong Planning A*演算法
簡介
LPA*演算法,即Lifelong Planning A*演算法,該演算法於2001年由Sven Koenig和Maxim Likhachev提出,是一種增量啟發式搜尋版本的A*演算法,這種路徑規劃演算法適用於隨著時間改變導致有限柵格地圖上的邊緣代價c(s1,s2)改變的問題,也就是隨著時間改變障礙物增多或減少,網格點發生增刪等,在許多場合下比再利用A*重新搜尋更高效。
啟發式搜尋和增量式搜尋的區別在於,啟發式搜尋是利用啟發函式來對搜尋進行指導,從而實現高效的搜尋,啟發式搜尋是一種“智慧”搜尋,典型的演算法例如A*演算法、遺傳演算法等。增量搜尋是對以前的搜尋結果資訊進行再利用來實現高效搜尋,大大減少搜尋範圍和時間,典型的例如LPA*、D* Lite演算法等。
和A*演算法一樣,LPA*演算法採用非負、一致性的啟發函式
表示當前位置網格點
到目標點
的距離的估計,
一致性體現在服從以下三角不等式,可以由簡單的三角形幾何性質可以推出:
其中
且
,如圖1所示:
圖1
1.
或
,表示網格點
下一時刻將要移動到的點,相當於
點的繼承點,對於
點,有
;
2.
或
,表示前一時刻移動到當前位置
點來的網格點,相當於
點的前輩點,對於
點,有
;
3.
,表示
點到當前
點的最短路徑距離,
是對
值的估計,
定義如下:
4.
被定義為:
其中
是節點
到起始節點
的代價,類似於A*演算法中的
,
表示從節點
到節點
的代價,被稱為邊緣代價函式。
圖2
在圖2的a)中,
此時屬於“正常情況”,即
,此時
點為區域性一致(locally consistent);
在圖2的b)中,如果邊緣代價函式值
由於環境發生改變從
變為
(即自由網格點
被障礙物佔據),則有
這種情況有
,此時
點為區域性不一致(locally inconsistent)。
區域性不一致又分為過一致(Overconsistent)和欠一致(Subconsistent)。
當
被稱為區域性過一致,即
點到
點的邊緣代價函式
值變低,代表網格上障礙物被清除(例如c值從
變為
)或搜尋到一條更短的“捷徑”。
當
被稱為區域性欠一致,當
點欠一致時,即
點到
點的邊緣代價函式c值變高,代表著自由網格被障礙物所佔據,這時
點的資訊需要被重置,這時候就需要重新搜尋計算最短路徑。
5.
Openlist或priority queue,和A*演算法一樣,表示被搜尋網格點的集合,用
來對這些網格點進行排序,值得注意的是所有Openlist的點都區域性不一致,所有區域性不一致的點都在列表上。
6.
,代表著優先順序佇列中網格點選擇的優先權,key值用於處理區域性不一致的網格點,
由兩個元素組成,
被定義為:
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
請批評指正,謝謝 ~