A* アルゴリズムについては以前に見たことがあり、A* アルゴリズムの基本原理は知っていますが、A* アルゴリズムの欠点も明らかです。これはオフラインの経路計画アルゴリズムであり、経路を一度しか計画できませんが、後でパスを変更すると、有効になりません。この問題を解決するために、人々は D* アルゴリズムを開発しました。A* またはダイクストラ アルゴリズムと比較した D* アルゴリズムの最大の利点は、移動中にリアルタイムであることです: 当初計画された経路に障害物が現れた場合、現在の経路に対して新しい計画が作成されます。より短い反復で見つけることができます。しかし、より大きな問題は、障害物が空白の経路上に現れる状況にのみ対応でき、障害物が元々空白領域に変わった状況には対応できないことです。この状況に対応して、LPA* アルゴリズムが誕生しました。
LPA* アルゴリズムの基本原理:
LPA* は、各セルの開始距離の 2 つの推定値、g 値と rhs 値を維持します。g 値は、A* 検索の g 値に直接対応します。rhs 値は、g 値に基づく 1 ステップ先読みです。rhs 値は常に次の関係を満たします (g 値に基づく)。
開始セルの rhs 値は 0 です。他の場所の rhs 値は、隣接するグリッドの g 値と、周囲のグリッドからターゲット グリッドまで移動するコストの最小値です。したがって、各ラスターの g 値は、初期状態の rhs 値と等しくなります。この格子を局所的に整合性のある格子と呼びます。すべてのセルが局所的に一貫している場合、すべての g 値は対応する開始距離に等しいため、この概念は重要です。
マップ内の障害物が変化すると、グリッドの rhs 値が変化します。このとき、グリッドのg値とrhs値が不一致になります。g 値が rhs 値と一致しないラスターは、局所的に不一致なラスターと呼ばれます。ここにも 2 つの状況があります。
1. 元の障害物グリッドから障害物が削除されています。この状況を局所的過剰整合性と呼びます。つまり、
g ( s ) > rhs ( s ) g(s) > rhs(s)g (秒)>r h s ( s )
2. 新しい障害物が元の空白のグリッドに追加されます。この状況を局所的な一貫性の欠如と呼びます。つまり、
g ( s ) < rhs ( s ) g(s) < rhs(s)g (秒)<r h s ( s )
この2点は比較的理解しやすいです。もともと障害物である点の場合、rhs 値と g 値はもともと無限大です。障害物が除去されると rhs 値は更新されます。上式によれば、その値はある値でなければならないことがわかります (周囲のすべての点の g 値が無限でない限り)。同様に、本来は空白のグリッドの g 値は rhs 値と等しいはずですが、障害物になると rhs 値も無限大となり、局所不一致の条件が満たされます。
これら 2 つの状況に対して、アルゴリズムは 2 つの異なる処理方法に分割されます。
局所的に一貫した点については、g 値が rhs 値に更新され、同時に周囲の点情報も更新されます。局所的に矛盾している点については、その g 値が無限大に更新され、その周囲の点の情報が更新されます。ここでの UpdateVertex 関数の場合、元のペーパーは次のように定義されています。
簡単に言えば、アルゴリズムは 3 つの主要な手順で各ラスターを更新します。
1. このグリッドの rhs 値を更新します。その値は、周囲のすべてのグリッドのうち、このグリッドからターゲット グリッドまでの g 値と h 値の合計の最小値です。
2. このグリッドがセット U に属している場合は、U セットから削除します。これは、更新する必要がある各点がセット U から選択され、通過した点が最初に削除される必要があるためです。
3. この点の g 値が rhs 値と一致しない場合は、それを集合 U に再挿入します。挿入されたポイントの U 値は以前の値とは異なる必要があることに注意してください。ここでの U の値の計算は次の式に従います。
U は実際には 2 つの数値のセットであり、最初の桁は 1 ステップの予測値をもたらし、2 桁目はその g 値と rhs 値の小さい方の値であることがわかります。
簡単に言えば、LPA* の基本的な考え方は、まず A* アルゴリズムを使用して、各ラスターの 2 つの値 (g 値と rhs 値) を維持しながらパスを見つけることです。その後、移動中にマップの障害物が変化すると、その rhs 値が更新されます。このとき、g値とrhs値が不一致となり、このラスターは局所不一致ラスターとなります。次に、アルゴリズムは ComputeShortestPath 関数を使用して、これらのローカルで一貫性のないラスターを処理してローカルで一貫性を持たせ、その過程で新しいパスが見つかります。
アルゴリズム全体の全体的な原理フローチャートは次のとおりです。
例
たくさん話した後、簡単な例を通してこの問題を見てみましょう。
上記のシーンがあるとします。左下隅の薄緑色が開始点、青いグリッドが終了点、赤が障害物です。最初に、A* アルゴリズムを使用して、次のようにパスを計画します。
同時に、次のように各グリッドの g 値と rhs 値を取得します。
左上隅が g 値、右上隅が rsh 値です。g 値が inf であるのに rsh 値が特定の値である点がいくつかあることに注意してください。これは、ある点を更新すると、その周囲の点の rsh 値は更新されますが、g 値は更新されないためです。この時点までは更新されます。そして、A* は方向性があると言え、すべての点が通過されるわけではないため、この問題が発生します。ただし、アルゴリズムの実際の使用には影響しません。
部分整合性処理フロー:
(2, 2) にある障害物を取り除くとします。まず、アルゴリズムはポイントの rhs 値を更新します。その値の更新は、記事の上部にある式に従います。したがって、rhs 値は 1.41 になります。
上記の変更された点はセット U に入れられ、不一致点として処理されます。次に、アルゴリズムは ComputeShortestPath 関数に入り、これらの不一致の g 値と rhs 値を更新します。更新時に点を抽出するロジックは、まず集合 U 内の最小値 (min(g(s), rhs(s)) + h(s)) を持つ点を選択することです。したがって、最初に取得される点は (2, 2) であり、この点は局所的過整合条件を満たすため、その g 値と rhs 値が更新されます。次に、その周囲のすべての隣接点の値が更新されます:
このプロセスでは 2 つの点が変更されます。最初の変更は点 (2, 2) の g 値であり、rhs 値と一致するように変更されます。このとき、点は局所的に一致する点に戻り、U から削除されます。同時に、前の点 (3, 2) の rhs 値は、g 値よりも小さい 2.41 に変更されるため、点は局所的に不一致になります。点、集合 U に参加します。
次に、アルゴリズムは U から点 (3, 2) を再度取得し、ComputeShortestPath 関数を実行します。
このプロセスでは、点 (3, 2) が集合 U から削除され、点 (3, 3) が集合に追加されます。 。次に、アルゴリズムは U から点 (3, 3) を再度取得し、ComputeShortestPath 関数を実行します。
この処理では、集合 U から点 (3, 3) が削除され、集合に点 (3, 4) が追加されます。次に、アルゴリズムは U から点 (3, 4) を再度取得し、ComputeShortestPath 関数を実行します。
この処理では、点 (3, 4) が集合 U から削除され、点 (3, 5) が集合 U に追加されます。次に、アルゴリズムは再び U から点 (3, 5) を取得し、ComputeShortestPath 関数を実行します。
このプロセスでは、点 (3, 5) が集合 U から削除され、点 (3, 6)、点 (4, 5) 、ポイント (4、6) がセットに追加されます。次に、アルゴリズムは再び U から点 (4, 5) を取得し、ComputeShortestPath 関数を実行します。
このプロセスでは、点 (4, 5) が集合 U から削除され、点 (5, 5)、点 (5, 6)コレクションに参加してください。次に、アルゴリズムは U から点 (4, 6) を再度取得し、ComputeShortestPath 関数を実行します。
このプロセスでは、点 (4, 6) は集合 U から削除されますが、周囲の点の rhs 値は削除されています。更新されるため、このステップではセットに新しいポイントは追加されません。次に、アルゴリズムは U から点 (5, 5) を再度取得し、ComputeShortestPath 関数を実行します。つまり、
集合 U から点 (5, 5) を削除し、点 (6, 6) を集合に追加します。アルゴリズムは再び U から点 (5, 6) を取得し、ComputeShortestPath 関数を実行します。
セット U から点 (5, 6) を削除します。アルゴリズムは再び U から点 (6, 6) を取得し、ComputeShortestPath 関数を実行します。機能:
現時点では、これらのポイントはローカルの一貫性を再び復元しています。このプロセスにはまだローカルに不整合なポイントがあることに注意してください。たとえば、(3, 6) は更新されていません。しかし、この時点で ComputeShortestPath 関数は満たされなくなったため、アルゴリズムが飛び出します。この点は再度更新する必要はありません。
ここでの判定について、論文では次のように定義されています。
つまり、点 (3, 6) については、その値は [9.41, 6.41] となるはずです。9.41 は rhs(s)+h(s) の値です。終点の値は 8.82 で、9.41 より小さいため、点 (3, 6) がどれほど最適化されていても、現在のパスより短くすることはできないため、更新する必要はありません。
最後に、次のように更新されたパスを取得できます。
ローカルでの一貫性の欠如の処理フロー:
部分的に一貫したプロセスを確認した後、部分的に一貫性のないプロセスを見てみましょう。初期マップを変更し、(2, 2) 点が最初は空白であると想定し、マップ パスを取得します。
次に、障害物として (2, 2) を追加します。このとき、点の rhs 値は inf なので、その点をセット U に追加して、ComputeShortestPath 関数を実行します。
このとき、点 (3, 2) の rhs 値は 3 に変更され、その点は局所的に不一致な点となり、集合 U に追加されます。同時に、点 (2, 2) は集合 U から削除されます。 ComputeShortestPath 関数のアルゴリズムに従って、
最初に点 (3, 2) の g 値を更新します。この点は局所的に矛盾した点であるため、その g 値は inf に直接更新される必要があります。同時に点 (3, 2) の周りの点の右辺値を更新します。
次に、点 (3, 3) の g 値を更新し、点 (3, 3) の周囲の点の rhs 値を更新します。
次に、点 (3, 2) の g 値を更新し、点 (4, 3) の周囲の点の rhs 値を更新します。このとき、その点は過整合点であり、次に従って処理されます。前の過剰な一貫性のあるポイント: このようにして、ポイント (2
, 2) のローカル一貫性がポイント (3, 2) で復元され、ポイント (3, 4) が更新されます。
その後、ポイント (3、3) を処理します。
次に、ポイント (3、5) を処理
します。
次に、ポイント (3、4) を処理します。 次に、ポイント (4、5) を処理します。
次に、ポイント (4、6) を
処理します。 , 5):
次にポイント (5, 6) を処理:
次にポイント (6, 6) を処理:
反復処理の一部を省略 (1,5)》(5,1)》(5,2)》(3,5)》(5,3)》(4,5)》(5,4)》(6 , 2)》(6,3)》(4,6)》(6,4)》(5,5)》(5,6)》(6,5)》(6,6)、最終結果は:
まとめ
一般的に言えば、LPA* アルゴリズムの考え方は、2 つのリストを使用して各ラスターの値を維持することです。2 つの値が矛盾する場合、そのラスターはローカルに矛盾していると呼ばれ、各ラスターの値はそれに応じて更新されます。 ComputeShortestPath関数に渡す オリジナルの場合 障害物がなくなった状態を局所過整合点と呼び、扱いやすくする 障害となる元の空白点を局所過整合点と呼ぶ 処理状況もう少し複雑になりますが、まず、点を局所的整合点に変更し、局所的整合点の処理方法に従って再度処理を行い、最終的に点の整合性をとります。 、一貫性のないポイントの走査原理は、A* アルゴリズムの走査方法と同様になり、ポイントの精度が向上します。
コード：
import os
import sys
import math
import matplotlib.pyplot as plt
class LPAStar:
def __init__(self, s_start, s_goal, heuristic_type,xI, xG):
self.xI, self.xG = xI, xG
self.x_range = 51 # size of background
self.y_range = 31
self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),
(1, 0), (1, -1), (0, -1), (-1, -1)] # feasible input set
self.obs = self.obs_map()
self.s_start, self.s_goal = s_start, s_goal
self.heuristic_type = heuristic_type
self.u_set = self.motions
self.obs = self.obs
self.x = self.x_range
self.y = self.y_range
self.g, self.rhs, self.U = {
}, {
}, {
}
for i in range(self.x_range):
for j in range(self.y_range):
self.rhs[(i, j)] = float("inf")
self.g[(i, j)] = float("inf")
self.rhs[self.s_start] = 0
self.U[self.s_start] = self.CalculateKey(self.s_start)
self.visited = set()
self.count = 0
self.fig = plt.figure()
def obs_map(self):
"""
Initialize obstacles' positions
:return: map of obstacles
"""
x = 51
y = 31
obs = set()
for i in range(x):
obs.add((i, 0))
for i in range(x):
obs.add((i, y - 1))
for i in range(y):
obs.add((0, i))
for i in range(y):
obs.add((x - 1, i))
for i in range(10, 21):
obs.add((i, 15))
for i in range(15):
obs.add((20, i))
for i in range(15, 30):
obs.add((30, i))
for i in range(16):
obs.add((40, i))
return obs
def update_obs(self, obs):
self.obs = obs
def plot_grid(self, name):
obs_x = [x[0] for x in self.obs]
obs_y = [x[1] for x in self.obs]
plt.plot(self.xI[0], self.xI[1], "bs")
plt.plot(self.xG[0], self.xG[1], "gs")
plt.plot(obs_x, obs_y, "sk")
plt.title(name)
plt.axis("equal")
def plot_path(self, path, cl='r', flag=False):
path_x = [path[i][0] for i in range(len(path))]
path_y = [path[i][1] for i in range(len(path))]
if not flag:
plt.plot(path_x, path_y, linewidth='3', color='r')
else:
plt.plot(path_x, path_y, linewidth='3', color=cl)
plt.plot(self.xI[0], self.xI[1], "bs")
plt.plot(self.xG[0], self.xG[1], "gs")
plt.pause(0.01)
def run(self):
self.plot_grid("Lifelong Planning A*")
self.ComputeShortestPath()
self.plot_path(self.extract_path())
self.fig.canvas.mpl_connect('button_press_event', self.on_press)
plt.show()
def on_press(self, event):
x, y = event.xdata, event.ydata
if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
print("Please choose right area!")
else:
x, y = int(x), int(y)
print("Change position: s =", x, ",", "y =", y)
self.visited = set()
self.count += 1
if (x, y) not in self.obs:
self.obs.add((x, y))
self.UpdateVertex((x, y))
else:
self.obs.remove((x, y))
self.UpdateVertex((x, y))
self.update_obs(self.obs)
#for s_n in self.get_neighbor((x, y)):
# self.UpdateVertex(s_n)
self.ComputeShortestPath()
plt.cla()
self.plot_grid("Lifelong Planning A*")
self.plot_visited(self.visited)
self.plot_path(self.extract_path())
self.fig.canvas.draw_idle()
def ComputeShortestPath(self):
while True:
s, v = self.TopKey()
if v >= self.CalculateKey(self.s_goal) and \
self.rhs[self.s_goal] == self.g[self.s_goal]:
break
self.U.pop(s)
self.visited.add(s)
if self.g[s] > self.rhs[s]:
# Condition: over-consistent (eg: deleted obstacles)
# So, rhs[s] decreased -- > rhs[s] < g[s]
self.g[s] = self.rhs[s]
else:
# Condition: # under-consistent (eg: added obstacles)
# So, rhs[s] increased --> rhs[s] > g[s]
self.g[s] = float("inf")
self.UpdateVertex(s)
for s_n in self.get_neighbor(s):
self.UpdateVertex(s_n)
def UpdateVertex(self, s):
"""
update the status and the current cost to come of state s.
:param s: state s
"""
if s != self.s_start:
# Condition: cost of parent of s changed
# Since we do not record the children of a state, we need to enumerate its neighbors
self.rhs[s] = min(self.g[s_n] + self.cost(s_n, s)
for s_n in self.get_neighbor(s))
if s in self.U:
self.U.pop(s)
if self.g[s] != self.rhs[s]:
# Condition: current cost to come is different to that of last time
# state s should be added into OPEN set (set U)
self.U[s] = self.CalculateKey(s)
#print(self.U[s])
def TopKey(self):
"""
:return: return the min key and its value.
"""
s = min(self.U, key=self.U.get)
return s, self.U[s]
def CalculateKey(self, s):
return [min(self.g[s], self.rhs[s]) + self.h(s),
min(self.g[s], self.rhs[s])]
def get_neighbor(self, s):
"""
find neighbors of state s that not in obstacles.
:param s: state
:return: neighbors
"""
s_list = set()
for u in self.u_set:
s_next = tuple([s[i] + u[i] for i in range(2)])
if s_next not in self.obs:
s_list.add(s_next)
return s_list
def h(self, s):
"""
Calculate heuristic.
:param s: current node (state)
:return: heuristic function value
"""
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
def cost(self, s_start, s_goal):
"""
Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
:return: Cost for this motion
:note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
return float("inf")
return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
def is_collision(self, s_start, s_end):
if s_start in self.obs or s_end in self.obs:
return True
if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
else:
s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
if s1 in self.obs or s2 in self.obs:
return True
return False
def extract_path(self):
"""
Extract the path based on the PARENT set.
:return: The planning path
"""
path = [self.s_goal]
s = self.s_goal
for k in range(100):
g_list = {
}
for x in self.get_neighbor(s):
if not self.is_collision(s, x):
g_list[x] = self.g[x]
s = min(g_list, key=g_list.get)
path.append(s)
if s == self.s_start:
break
return list(reversed(path))
def plot_path(self, path):
px = [x[0] for x in path]
py = [x[1] for x in path]
plt.plot(px, py, linewidth=2)
plt.plot(self.s_start[0], self.s_start[1], "bs")
plt.plot(self.s_goal[0], self.s_goal[1], "gs")
def plot_visited(self, visited):
color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
'bisque', 'navajowhite', 'moccasin', 'wheat',
'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']
if self.count >= len(color) - 1:
self.count = 0
for x in visited:
plt.plot(x[0], x[1], marker='s', color=color[self.count])
def main():
x_start = (5, 5)
x_goal = (45, 25)
lpastar = LPAStar(x_start, x_goal, "Euclidean",x_start,x_goal)
lpastar.run()
if __name__ == '__main__':
main()
初期状態では、計画結果は次のようになります。
障害物の除去:
新しい障害物の追加:
参考:
1. 「生涯計画 Aアルゴリズム (LPA ): 生涯計画 A*」
2. 「LPA* 経路探索アルゴリズムの紹介と完全なコード」
3.「経路計画アルゴリズム」