必要な事前知識 (事前知識がないことは大きな問題になる可能性がありますが、D* の理解に役立ちます): A* アルゴリズム/ダイクストラ アルゴリズム
D*アルゴリズムとは
ダイクストラ アルゴリズムは、グラフ内の 2 つのノード間の最短接続パスを見つけるための独創的なアルゴリズムです。A* アルゴリズムは、ダイクストラ アルゴリズムの検索プロセス中に検索方向をガイドするヒューリスティック関数 h(x) をダイクストラ アルゴリズムに追加します。そのため、検索の必要性が最小限に抑えられ、最適なソリューションを見つける速度が向上します。これらは両方ともロボットのオフライン経路計画問題に適用できます。つまり、環境マップが既知であり、開始点と終了点が既知であり、ロボットが開始点から移動できる経路を見つける必要があります。終点を指します。
しかし、上記 2 つのアルゴリズムでは、ロボットが所有する地図が必ずしも最新の地図であるとは限らない、つまり、ロボットが所有する地図には明らかに歩行可能な場所があるが、そうでない可能性があるという実用化には問題があります。誰かが突然何かを地面に置いたり、テーブルを動かしたり、単に歩行者がロボットの走行中にロボットの前を通り過ぎたり、ブロックしたりした可能性があるため、実際の動作中に歩行できる可能性はありません。
このような問題に遭遇した場合、たとえば、ロボットがあらかじめ決められた経路に沿って地点 A まで歩いたときに、あらかじめ計画した経路上の次の地点が障害物によって遮られていることに気づきました。停止してから障害物情報を再更新し、ダイクストラ アルゴリズム/A* アルゴリズムを再実行すると、経路を再度見つけることができます。
しかし、これを行うと二重カウントという問題が発生します。下の図に示すように、新しい障害物が少ししかブロックしない場合、ロボットは障害物を迂回し、その後のパスは以前に計画されたパスをたどります。ただし、ダイクストラ アルゴリズム/A* アルゴリズムが繰り返し実行されると、まったく同じパスが再度計算されます。
D* アルゴリズムの存在は、この反復計算の問題を解決するためにあります。最初に目標経路が見つかった後、探索プロセスのすべての情報が保存されます。後で事前に未知の障害物に遭遇したときに、それを保存するために使用できます。新しい道を素早く計画するための情報。
ちなみに、D* アルゴリズムは上記のような特徴を持っているため、「事前地図情報が無い/事前地図情報が少ない環境でのナビゲーション」の問題でも、全体を擬似するだけで済むため、D* アルゴリズムを使用することができます。開始時のマップ 障害物がなく、始点から終点までの経路は直線であり、オンライン操作中に D* アルゴリズムを使用して継続的に再計画できます。
次に、D* アルゴリズムのプロセスについて説明します。このため、まずアルゴリズム適用のコンテキストについて触れてみましょう。
ロボットは地図を知っており、ある出発点 S から終点 E に行きたいと考えています。しかし、ロボットが歩行中に突然、地図に記載されていない障害物を発見しました（たとえば、歩行中、ライダーを使用して、1メートル前に地図に記載されていない障害物があることを突然発見しました）。障害物 ロボットが当初計画した経路をブロックするものもあれば、ブロックしなかったものもあります。
簡単にするために、ロボットはグリッド マップ上を移動し、一度に 1 つのグリッド (8 つの隣接) を移動すると考えます。ロボットは点であり、その体積は 1 つのグリッド サイズのみを占めます。
D*アルゴリズム処理
まず使用する必要があるクラスについて説明しますと、実行を開始する前に、検索で使用できるようにマップ内の各ノードの状態クラス オブジェクトを作成する必要があります。
# 疑似コード
class state:
# 各マップグリッドに必要な情報を格納する このクラスがどこで使用されるかを説明します。以下は、クラス メンバー変数
x #abscissa
y #縦座標
t = "new" #現在のポイントが検索されたかどうかを記録します ("new"、"open"、"close" のいずれかになります。それぞれ、このグリッドがまだ検索されていないことを意味します)検索された場合、このグリッドはオープン テーブルにあり、このグリッドはクローズ テーブルにあります。オープン テーブルとクローズ テーブルについては、すぐに理解できる A* アルゴリズムを読むことをお勧めします)。新しい
親に初期化 = None # 前の状態を指す親ポインタ 特定の点の親ポインタに沿って検索すると、この点からターゲット点の端までの最短経路を見つけることができます
h # 現在のコスト値 (D* アルゴリズムのコア)
k # 履歴最小コスト値 (D* アルゴリズムの中核。更新されたすべての h の中で最小の h 値を意味します)
d* アルゴリズムのメインコード:
function Dstar(map, start, end):
# マップ: m*n マップ、障害物を記録します。map[i][j] は状態クラス オブジェクトです
# start: 開始点、状態クラス オブジェクト
# end: 終了点、状態クラス オブジェクト
open_list = [ ] # 検索をガイドする新しいオープン リストを作成します
insert_to_openlist (end, 0, open_list) # 終点を open_list に入れる
#最初の検索は、オフライン事前マップに基づいて始点から終点までの最短経路を見つけることです。終点から始点までのループを探すことに注意してください
(start .t == "閉じる"):process_state() # D* アルゴリズムのコア関数の 1 つ
エンドループ
# ロボットにパスに沿って一歩ずつ歩かせます。歩行中に新しい障害物が見つかる可能性があります。
temp_p = 開始
while (p != end) do
if (未知の障害物が見つかった) then # 新しい障害物を発見し、再計画を実行します
for new_obstacle in new_obstacles: # 新しい障害ごとにmodify_cost関数を呼び出します
modify_cost(new_obstacle) #(D* アルゴリズムのコア関数の 1 つ)
終わります
する
k_min = process_state()
ではないが ( k_min >= temp_p.h またはopen_list.isempty() )続く
終了する場合
temp_p = temp_p.parent
その間終了
上記の疑似コードには、modify_cost と process_state という 2 つのコア関数があります。process_state に関する csdn の説明をいくつか読んだところ、非常に大きな間違いがあり、場合によってはアルゴリズム全体が無限ループに陥る可能性があることがわかりました ( D* 計画アルゴリズムや Python 実装_mhrobot のブログ - CSDN ブログなど)。そして、元の論文の擬似コードにもいくつかの問題があります (おそらく元の論文 (部分的に既知の環境のための最適で効率的なパス計画、
関数modify_cost( new_obstacle )：
set_cost(any point to new_obstacle) = 10000000000 # 「任意の点から障害物点までのコスト」と「障害物点から任意の点までのコスト」を無限大に設定し、代わりにプログラム内で非常に大きな数値を使用します (考慮してください) 8 隣接)
if new_obstacle.t == "閉じる" then
insert(new_obstacle, new_obstacle.h) #開いたテーブルに入れます。insertもd* アルゴリズムの重要な関数の 1 つです。
終了する場合
以下は Process_state 関数の疑似コードです。赤でマークされた部分に注目してください。
関数 process_state( )：
x = get_min_k_state(oepn_list) # オープンリストから最小の k 値を持つ状態を取り出します。この目的は A* と同じです。どちらも k 値を使用して検索順序を決定しますが、この k 値はA* アルゴリズムの f に相当します値 (f=g+h、g は実際のコスト関数値、h は推定コスト ヒューリスティック関数値)、D* ではヒューリスティック関数値 h は使用されず、のみ使用されます。実際のコスト値は検索のガイドに使用されるため、実際に必要です つまり、D* はダイクストラに似ており、ヒューリスティック関数を使用して検索範囲を縮小するのではなく、実際のコストを使用して検索をガイドします。 * は、後で新たな障害が発見された場合に再計画するために必要です。
x == Null の場合は -1 を返します
k_old = get_min_k(oepn_list) # openlist 内の最小の k 値を見つけます。これは、実際には上記の x の k 値です。
open_list.delete(x) # オープンリストから x を削除し、クローズテーブルに入れます
xt = "close" # ここではクローズ テーブルが明示的に維持されないことを除いて、クローズ テーブルを入れることと同じです。
#以下はコアコードです:
# 最初の判断セット
if k_old < xh then # この条件が満たされる場合、x の h 値が変更されていることを意味し、x は raise状態にあると見なされます。
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
if yh<k_old and xh> yh +cost(y,x) then
x.parent = y
xh = yh + コスト(x,y)
終了する場合
終わります
終了する場合
# 2 番目の判定セット
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
else: # k_old == xh が満たされない場合、k_old < xh
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
if yt == "新規"または
(y.parent == xおよびyh !=xh +cost(x,y) ) 次に
y.parent = x
挿入(y, xh + コスト(x,y))
それ以外：
if (y.parent != xおよびyh >xh +cost(x,y)) then
xk = xh # この行に注目してください。この行がないと、特定の状況で無限ループが発生します。多くの情報を調べた結果、Wikipedia の d* アルゴリズム ページでこのコード行が解決策を見つけました。d* の原著論文を含め、インターネット上のほとんどの情報にはこの文が含まれていません。
挿入(x, xh)
それ以外：
if (y.parent!=xおよびxh>y.h+cost(y,x)およびyt= "close"およびyh>k_old then
挿入(y,yh)
終了する場合
終了する場合
終了する場合
終わります
終了する場合
get_min_k(oepn_list) を返す
上記で使用した挿入関数:
関数挿入(状態, new_h)
if state.t == "new": # 開いているテーブルに未探索の点を追加するとき、h は new_h で渡された値に設定されます。 state.k = h_new # まだ探索されていないため、k は履歴の最小の h です。したがって、k は new_h elif です state.t == "open": state.k = min(state.k, h_new) # k を履歴の最小の h elif として保持します state.t == "close": state.k = min ( state.k, h_new) # k を履歴の最小の h として保持します state.h = h_new # h は現在の点から終了までのコストを意味します state.t = "open" # 開いているテーブルに挿入します。また、 open_list .add(state) #Insert into open tableとして維持する必要があります。
上記は完全な d* アルゴリズム プロセスです。
D*処理の詳細説明
ここで、「d* アルゴリズムのメイン コード」から始めて、d* 関数を詳しく説明します。
最初の検索
まず最初にオープン リストを作成し、次にエンド ポイント end をオープン リストに追加し、開始ポイント start がクローズ テーブルに追加されるまで process_state の呼び出しを続ける必要があることがわかります。開いたリスト内の項目は、k 値に従ってソートされる必要があります。process_state が呼び出されるたびに、検索を拡張するには、k 値が最も小さいものを取り出す必要があります。オープン リスト内の項目は、次に従ってソートされる必要があることに注意してください。 h 値の代わりに k 値を小さい値から大きい値に変換します。
初めて検索する場合、対応するメイン コードは次のとおりです。
# 最初の検索では、オフラインの事前マップに基づいて開始点から終了点までの最短パスを検索します。
終了点から開始点までのループを見つけることに注意してください (start.t == "close"):process_state() # D* アルゴリズムのコア関数の 1 つ
エンドループ
この部分。
実際、最初の検索中に、開いているテーブルに各ポイントが初めて追加されるとき、新しい状態から追加されることがわかります。つまり、挿入関数が呼び出されるたびに、state.t は最初にnew なので、その h 値は実際には k 値に等しい; 検索プロセス中、つまり各ステップで process_state が呼び出されるとき、k_old は常に xh に等しい (k_old は xk に等しい)。また、常に k_old == xh の部分のみを実行します。
# 2 番目の判定セット
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
それ以外：
……
この部分では、x の 8 つの隣接点 y を判断する状況は 2 つだけあり、① y.t== "new" ② y.parent != xとyh >xh +cost(x,y)です。3番目のケース③ y.parent == xとyh !=xh +cost(x,y) は、h が現在点から終点までのコストを意味し、parent が最短を意味するため、この時点では 3番目の状況は、yh、xh、またはcost(x,y)が異常なプロセスで人為的に変更されたために発生するはずです。これは最初の検索では表示されませんが、再計画中に発生します。この点については、次の章で説明します。次の部分。
つまり、最初の検索中、process_state コード全体は実際には次と等価になります (この時点では他の部分は呼び出されないため)。
function process_state**** (最初の検索時の次の関数と同等) ( ):
x = get_min_k_state(oepn_list)
x == Null の場合は -1 を返します
k_old = get_min_k(oepn_list)
open_list.delete(x)
xt= "閉じる"
# 2 番目の判定セット
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
get_min_k(oepn_list) を返す
これは実際にはダイクストラのアルゴリズムであることがわかります。
実際、d* アルゴリズムは事前計画中に完全にダイクストラ アルゴリズムに縮退し、計画された経路に沿って移動中に障害物に変化がなければ、d* の効率は実際には a* の効率よりも低くなります。 d* はヒューリスティックにターゲットを検索しないため、実際の検索空間は a* よりもはるかに大きくなります。
しかし、d* は主に再計画の効率を向上させるためにこれを行います。なぜなら、ダイクストラ アルゴリズムは実際に「空間内の特定の点からすべての点への最短経路を見つける」ことを実装しているからです (終点から始点まで探索することに注意してください) 、つまり、見つかったものはすべてのポイントから終点までの最短パスです) (実際にはすべてのポイントではありません。開始点が見つかった後に検索が停止し、未完了の検索が開いたテーブルにまだ残っているためです。つまり、最短パスです)距離が始点から終点までの距離より大きい。点が検索されていない。）を使用すると、動作中に障害物に遭遇し、再計画が必要になった場合に、現在の点から目的の点まで再検索する必要がなくなりました。終点に到達しますが、障害物を回避して近くに到達できる経路を見つけます。障害物のないアイドル ポイントで十分です。このアイドル ポイントに到達した後の経路は、最初の探索に基づいてすでに探索されているため、再度探索する必要はありません。このアイドルポイントから目的地までの最短パスを持っています。
綿密な計画
次に、最初の検索後の部分であるメイン コードを引き続き見ていきます (プログラムのデフォルトでは、最初の検索で解決策が見つかります。最初の検索で解決策が見つからない場合、再計画は行われません。考慮されません):
# ロボットにパスに沿って一歩ずつ歩かせます。歩行中に新しい障害物が見つかる可能性があります。
temp_p = 開始
while (p != end) do
if ( 不明な障害物が見つかりました) then # 新しい障害物が見つかりました
for new_obstacle in new_obstacles: # 新しい障害ごとにmodify_cost関数を呼び出します
modify_cost(new_obstacle) #(D* アルゴリズムのコア関数の 1 つ)
終わります
する
k_min = process_state()
ではないが ( k_min >= temp_p.h または open_list.isempty() )続く
終了する場合
temp_p = temp_p.parent
その間終了
ここでは、パスに沿ってステップごとに歩くロボットをシミュレートするために、各ステップで temp_p = start とし、次に temp_p = temp_p.partent とします。歩行中にセンサーを通じて新しい障害物が発見された場合、まず発見された新しい障害物の情報を既知の地図に追加します。つまり、以下を実行します。
for new_obstacle in new_obstacles: # 新しい障害ごとにmodify_cost関数を呼び出します
modify_cost(new_obstacle) #(D* アルゴリズムのコア関数の 1 つ)
終わります
新しく発見された障害物点ごとにmodify_costを呼び出します(d*は、特定の点がもともと障害物であったが、現在では障害物ではなくなっていることが判明した状況を処理できないことに注意してください。これは考慮されず、新しい障害物のみが処理されます)考慮された）
関数modify_cost( new_obstacle )：
set_cost( new_obstacle への任意のポイント) = 10000000000
new_obstacle.t== "閉じる" の場合
insert(new_obstacle, new_obstacle.h) #開いているテーブルに入れます
終了する場合
戻る
まず、障害物の周囲すべての点への移動コストを無限大に設定します。そうすると、
- 新しく発見された障害物ポイントのステータスは「新規」なので、心配する必要はありません。後で process_state が実行されるとき、新しいポイントが開いているテーブルに初めて追加されるときに、この無限コストが反映されます。周囲の点の h 値。実際、再計画中にポイントがまだ新しい場合は、このポイントの最初の検索でその情報が使用されなかったことを証明するため、この検索プロセスでは「新しい障害」とみなされません。探しても、それが障害物なのかどうかは分かりませんでした。
- このポイントのステータスが「オープン」の場合、つまり、このポイントがオープン テーブル内にある場合は、オープン テーブルで process_state が実行されると展開され、その障害が発生するため、心配する必要はありません。情報はコスト(x,y)を介して渡され、アイテムは周囲の点に渡されます。(障害点自体の h 値が無限大でなくても問題ありません。障害点自体は終点へのパスを見つけることができますが、障害点自体はロボットによって占有されないため、心配。）;
- このポイントのステータスが close の場合、そのポイントを変更せずに (つまり、h 値を変更せずに) 開いているテーブルに移動します。後で process_state が実行されると、その障害物情報がコストを通じて近くのポイントに転送されます。
新しい障害物ポイントを開いているテーブルに配置した後 (新しいポイントは常に開いているテーブルに追加されますが、追加されない場合は必要ないことがわかります)、解決策が見つかるまで process_state が繰り返し実行されます。解決策はありません。つまり、次のコードです。
する
k_min = process_state()
ではないが ( k_min >= temp_p.h または open_list.isempty() )
このうち、終了条件を決めるopen_list.isempty()が満たされた場合、つまりオープンリストが空の場合は解がないことを意味しており、これはA*やダイクストラと同じです。(もちろん、実際の操作では、開いているテーブルが空であることを条件とするのは一般的ではありません。代わりに、開いているテーブル内の最小の k 値が、以前に設定されたスーパー値以上である場合、または実行中に終了するために使用されます)検索プロセスでは、以前に設定されたスーパーバリュー値を超える k 値については、開いているテーブルに直接追加されません。)
判定条件 k_min >= temp_p.h が満たされた場合、解が見つかった（現在点から目的点までの新たな最短経路が見つかった）ことになります。簡単に言うと、 k_min = process_state(), process_state() は、毎回最小の k 値を持つ点を処理します 開いたテーブル内の最小の k 値 k_min が現在の点の h 値より大きい場合、それはその点を意味します現在のロボットが配置されている temp_p が検索されました。
上記の緑色で示した部分を説明する前に、h と k が具体的に何を表すのかについて言及する必要があります。
h値
再計画する場合、h は現在のステップが見つかったときの現在点から目標点までの最小コストを表しますが、最終的に最短経路が見つかった後の最小コストであるとは限りません。探索後、現在のステップよりも短いパスが見つかった場合、現在点 h の値は、当然のことながら、その短いパスをたどって終点まで到達するコストに変更されます。
k値
最初の探索パスについては、k 値と h 値の意味は明らかに同等であり、両者に違いはありませんが、具体的な意味については、実際のダイクストラ法と A* アルゴリズムの合計生成値を参照してください。
再計画する場合、k 値の意味はさらに複雑になるため、その役割を理解するにはコードを振り返る必要があります。再計画は実際には process_state を継続的に呼び出すプロセスであるため、k 値が変更される可能性のある process_state 内のすべての場所を探します (以下のコードの赤いコードに注目してください)。
関数 process_state( )：
x = get_min_k_state(oepn_list)
x == Null の場合は -1 を返します
k_old = get_min_k(oepn_list)
open_list.delete(x)
xt= "閉じる"
#以下はコアコードです:
# 最初の判断セット
k_old < xh の場合
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
if yh<k_old and xh> yh +cost(y,x) then
x.parent = y
xh = yh +cost(x,y) # h のみが変更され、k は変更されません
終了する場合
終わります
終了する場合
# 2 番目の判定セット
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
insert(y, xh +cost(x,y)) #insert が呼び出されます。これには、k 値の変更が含まれる可能性があります。
終了する場合
終わります
else: # k_old == xh が満たされない場合、k_old < xh
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) 次に
y.parent = x
insert(y, xh +cost(x,y)) #insert が呼び出されます。これには、k 値の変更が含まれる可能性があります。
それ以外：
if (y.parent != xおよびyh >xh +cost(x,y)) then
xk = xh # k は直接変更されます! ！！！
insert(x, xh) #insert が呼び出されます。これには、k 値の変更が含まれる可能性があります。
それ以外：
if (y.parent!=xおよびxh>y.h+cost(y,x)およびyt = "close"およびyh>k_old then
insert(y,yh) #insert が呼び出されます。これには、k 値の変更が含まれる可能性があります。
終了する場合
終了する場合
終了する場合
終わります
終了する場合
get_min_k(oepn_list) を返す
k 値の変更をわずかに含む、上で赤でマークされた場所のみが示されており、そのうちの 4 つは挿入関数であることがわかります。次に、挿入関数を見てみましょう。
関数挿入(状態, new_h)if state.t == "new": # 開いているテーブルに未探索の点を追加するとき、h は new_h で渡された値に設定されます。
state.k = h_new # まだ探索されていないため、k は履歴の最小の h です。したがって、k は new_h
elif です state.t == "open":
state.k = min(state.k, h_new) # k を履歴の最小の h
elif として保持します state.t == "close":
state.k = min ( state.k, h_new) # k を履歴の最小の h として保持します
state.h = h_new # h は現在の点から終了までのコストを意味します
state.t = "open" # 開いているテーブルに挿入します。また、
open_list .add(state) #Insert into open tableとして維持する必要があります。
insert 関数では、渡されたパラメータ new_h が k 値より小さい場合にのみ、状態の k 値が新しい h 値に置き換えられます。これも合理的です。結局のところ、元の論文での k の意味は「」です。 「過去の h 値の中で最小の h 値」。
再計画中の process_state のコンテキストに戻りましょう。何か考えてみてください。地図があり、始点から終点までの最短経路 Rがすでにわかっています(つまり、最初の検索の結果)。確かなことは、最短経路 R上の任意の点について、この点から目的地までの最短経路は、この点から始まる最短経路 Rの残りの部分でなければならないということです。(反証: より短い道路がある場合、最短経路 R は始点から終点までの最短経路ではないからです。間違いなくこの点に行くことができ、その後はより短い経路を選択することができます。その方が確実に短くなります) . こうなります。矛盾）。
元の地図に基づいて新しい障害物を追加しましたが、障害物が元の最短経路 R を妨げている可能性が非常に高いです。この場合、ゴールに到達するには、より長いパスをたどる必要があるかもしれません。あるいは、現在のパスと同じくらい長いパスがあるかもしれません。そのパスを選択することもできますが、いずれにせよ、新しい最短パスの長さは変わりません。元の最短パス R は、最短パス Rよりも短くなることはありません(障害物が多いため、選択したパスが短くなる理由はありません)。また、②障害物が私を妨げないのであれば、私の最短の道は変わらないはずです。要約すると、新しい障害物を発見した後の最短経路は、新しい障害物がなかったときの元の最短経路よりも長いか、同じ長さでなければなりません。
上の 2 つの段落によれば、新しい障害物を追加した後、ある点から目標点までの最短経路の長さ、つまりコストは増加するだけで、減少することはないことがわかります。これはどういう意味ですか？これは、再計画の時点では、挿入関数の実行時に渡されるパラメーター new_h が元の k 値より小さくてはいけないことを示しています。これは、最初の検索の終了時点では h と k は等しく、両方とも等しいためです。最小コスト 新しい障害物の追加 後続の検索中の new_h の「新しいコスト」は、最初の検索終了時の h の「コスト」より大きくなければなりません、つまり、k より大きくなければなりません。つまり、再計画中に挿入関数の k 値を更新することは絶対に不可能です。
3 つの段落について説明した後、1 つだけ説明したいことがあります。insert 関数は再計画中に k 値を変更しません。次に、上の 5 つの赤いコードに戻ると、k 値が変更されるのは 1 つのケースのみであることがわかります。ここは、2 番目の判断セットで特定の条件下で k 値が直接変更される場所です (以下の赤でマークされています)。
関数 process_state( )：
x = get_min_k_state(oepn_list)
x == Null の場合は -1 を返します
k_old = get_min_k(oepn_list)
open_list.delete(x)
xt= "閉じる"
#以下はコアコードです:
# 最初の判断セット
k_old < xh の場合
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
if yh<k_old and xh> yh +cost(y,x) then
x.parent = y
xh = yh +cost(x,y) # h のみが変更され、k は変更されません
終了する場合
終わります
終了する場合
# 2 番目の判定セット
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
insert(y, xh +cost(x,y)) # k は再計画プロセス中に変更されません
終了する場合
終わります
else: # k_old == xh が満たされない場合、k_old < xh
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) 次に
y.parent = x
insert(y, xh +cost(x,y)) # k は再計画プロセス中に変更されません
それ以外：
if (y.parent != xおよびyh >xh +cost(x,y)) then
xk = xh # k を変更する唯一の場所
insert(x, xh) # k は再計画プロセス中に変更されません
それ以外：
if (y.parent!=xおよびxh>y.h+cost(y,x)およびyt = "close"およびyh>k_old then
insert(y,yh) #k は再計画プロセス中に変更されません
終了する場合
終了する場合
終了する場合
終わります
終了する場合
get_min_k(oepn_list) を返す
赤い部分のk値がどのように変更されるかというと、結論としては確実にk値が拡大され、更新前のk値よりも大きくなります。理由は上記でも述べましたが、再計画プロセス中、すべてのポイントのコストは元のコストより大きくなければなりません。つまり、ここに表示されるすべての h 値は、最初の検索後の k 値より大きくなければなりません。 , then xk=xh この文では、xh は xk より大きくなければなりません。
それでは、ポイントの k 値はいつ変更されるのでしょうか? 結論を先に言っておきます。この点から目標点までの新しい最短経路 newRを見つけるとき、新しい障害物を考慮した後、この点の k 値はより大きくなるように修正され、修正された k の値は を表します。新しい最短経路 newRに沿ったこの点からターゲット点までのコスト/実際の距離。新しい最短経路 newR上の特定の点のコスト (つまり、ターゲットまでの距離) が変化しない場合、その k 値は変化しないことに注意してください。その理由については、後述の process_state の各判定条件を詳しく説明する際に説明します。
要約すると、再計画中、特定の点の k 値は次の意味を持ちます。
- 再計画中に特定の点の k 値が変更された場合、その変更された k 値は、新たな障害物を考慮した後のその点から目標点までの最短経路長 (コスト) になります。ここで注意すべき点は、k 値が変更される点は、ロボットが次にたどる経路上にあるとは限らず、新しい障害物の追加により変化した点の周囲の広い範囲である可能性があるためです。再計画は複数回実行される可能性があり、次回の実行では再計画の際、マップはこの再計画に基づいて作成されるため、今回の再計画の実行後、マップのステータスはダイクストラ アルゴリズムの完了時に維持されます。つまり、マップ上の各ポイントは、ターゲット ポイントへの最短経路を知っています。
- ある点の k 値が変更されていない場合、その k 値には 2 つの状況があります: ① まず、あまり意味がありません。強いて言うなら、意味は「再計画を実行する前」です。 (つまり、新しい障害物を考慮する前)"、この点から目標点までの最短経路のコスト値"; ② 2 番目は、新しい障害物を考慮するかどうかです。ターゲット ポイントは変更されず、その k 値も変わりません。
上記 2 つの場合における k の意味は次のとおりですが、実際には、この 2 つの場合のうち、①の場合に出現する点の数は通常非常に少ないため、次のように理解できます。 k 値の意味は、基本的には、再計画後に特定の点がターゲットに到達することを意味します。点間の最短距離。
では、2.の①の状況は稀であっても必ず発生するのではないかと考える人もいるかもしれないが、新たな障害が発見された場合、その後の再計画に影響を与えるのではないか？答えはいいえだ。理由は、この部分には実際にはポイントがほとんどなく（正直、よくわかりませんが、これらのポイントがまったく表示されない可能性もあります）、前述したように、k 値が実際に影響を与えるのは、この部分 ポイント k の値が小さすぎる場合、次の再計画での優先度にのみ影響し、最初に process_state を呼び出します。そして、 process_state が呼び出されると通常通りに処理されますが、状況に応じてその k の値を変更する、つまり 1. の状態に戻すか 2.② の状態に戻すかを決定します。これを読んで理解できなかった場合は、次の部分を読んでからこの場所に戻って見ることをお勧めします。
先ほどの説明に戻り、終了判定条件 k_min >= temp_p.h の意味は以下のとおりです。
d* アルゴリズムの main 関数から、再計画プロセスが実際に終了条件が満たされるまで継続的に process_state() を呼び出していることがわかります。process_state() が呼び出されるたびに、最小の k 値を持つ状態がオープン リスト (オープン テーブル) から抽出され、処理されてオープン テーブルから移動され、クローズ テーブルに配置されるか、オープン テーブルに戻されます。 。つまり、process_state を継続的に呼び出すプロセスにおいて、オープン リスト内の最小の k 値 k_min の全体的な傾向は常に増加しています (oepn リストは毎回、最小の k 値を持つ状態を取り出し、新しいノードをそのリストに追加します)。条件に従ってテーブルを開きます。ただし、これらの新しいノードの k は小さい可能性が高く、開いているリストの k_min は小さくなりますが、全体的な傾向として k_min は常に増加しています)。
開いたテーブルの場合、k の最小値 k_min は、検索する必要があるすべての点 (つまり、開いたテーブル内のすべての点)、ターゲット点までの距離が k_min の値以上であることを意味します。上記のことから、temp_p.h の意味は、現在点から目標点までのコスト (つまり、ロボットが新しい障害物を見つけたときの位置) であることがわかります (実際に実行可能なパスの中で最小であるとは限りません)。ただし、この値が無限でない限り、これは現在の点からゴールまでのパスが見つかったことを証明します)。すると、条件k_min >= temp_p.hの意味は、「まだ探索が必要な点から目標点までの距離が、ロボットの現在点から探索対象点までの距離以上である」ということになります。が見つかりました」、つまり再探索します。下に進むと、新しい道路を見つけたとしても、現在の道路よりも費用がかかるため、現在の道路には及ばないことになります。したがって、この条件が満たされた時点で最適解が見つかったため、再計画プロセスを終了できると言えます。
process_stateの詳細説明
d* アルゴリズムを真に理解するには、そのコア関数 process_state を理解する必要があります。最初の検索におけるこの関数の意味、つまり完全に dijkstgra に縮退することは上で説明しましたが、再計画プロセスにおけるこの関数の動作を以下で分析します。見やすくするために、この関数を完全にここにもう一度配置してみましょう。
関数 process_state( )：
x = get_min_k_state(oepn_list)
x == Null の場合は -1 を返します
k_old = get_min_k(oepn_list)
open_list.delete(x)
xt= "閉じる"
#以下はコアコードです:
# 最初の判断セット
k_old < xh の場合
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
if yh<k_old and xh> yh +cost(y,x) then
x.parent = y
xh = yh + コスト(x,y)
終了する場合
終わります
終了する場合
# 2 番目の判定セット
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
else: # k_old == xh が満たされない場合、k_old < xh
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) 次に
y.parent = x
挿入(y, xh + コスト(x,y))
それ以外：
if (y.parent != xおよびyh >xh +cost(x,y)) then
xk = xh
挿入(x, xh)
それ以外：
if (y.parent!=xおよびxh>y.h+cost(y,x)およびyt = "close"およびyh>k_old then
挿入(y,yh)
終了する場合
終了する場合
終了する場合
終わります
終了する場合
get_min_k(oepn_list) を返す
まず最初の段落を読んでください
x = get_min_k_state(oepn_list) # 開いたリストから最小の k 値を持つ状態を取得します
if x == Null then return -1 # 何も取得されない場合は、open_list が空である、つまり解決策がないことが証明され、return -1
k_old = get_min_k(oepn_list) # 変数 k_old を使用して最小 k 値を保存します
open_list.delete(x) # x をオープン テーブルからクローズ テーブルに移動します (クローズ テーブルは明示的に維持されません)
xt= "close" # x は close テーブルに配置されているため、そのステータスを close に変更します
この段落では、process_state を検索して展開するたびに、k の値が小さいものを検索して展開し、検索して展開した状態を close テーブルに入れることを示しています。これは、a* と dijkstra の性質と同じです。これらはすべて、値に基づいて検索をガイドします。 のシーケンス、ここではこの値が k 値です。
k_old については、そのとおりです。k_old は、取り出された x に等しい k の値です。get_min_k_state 関数と get_min_k 関数の両方の Min k は、同じ値を指します。なぜこれを書くのですか？元の論文の擬似コードはこのように書かれているので、ちょっと変な感じですがコピーしてみましょう。
障害物情報の初期送信
次に、2 番目の判決について説明します。再計画プロセスでは、最初に新しい障害物に対してmodify_cost操作が実行されるためです。
for new_obstacle in new_obstacles: # 新しい障害ごとにmodify_cost関数を呼び出します
modify_cost(new_obstacle) #(D* アルゴリズムのコア関数の 1 つ)
終わります
関数modify_cost( new_obstacle )：
set_cost( new_obstacle への任意のポイント) = 10000000000
new_obstacle.t== "閉じる" の場合
insert(new_obstacle, new_obstacle.h) #開いているテーブルに入れます
終了する場合
戻る
わかりやすくするために、以下のような状況でロボットが以下のような状況にあると仮定します。
このステップの後、すべての障害物ポイント、つまり紫色のポイントが変更されずに開いたテーブルに追加されます。つまり、k 値と h 値を変更せずに、開いたテーブルに直接配置されます。これらの障害点を追加する前 (つまり、最初の検索後)、開いているテーブル内の最小の k 値が開始点の k 値 (ダイクストラ アルゴリズムの特性) 以上である必要があるため、これらの新しく追加された障害点k 値が必要です。 k 値が開いているテーブル内の他の点より小さい場合、process_state を呼び出すときに最初に検索する必要があります。
process_state がこれらの点を展開すると、その k 値、h 値は変更されていないため、最初の判定セットは入力されませんが、直接 2 番目の判定セットの前半 k_old == xh に入ります。つまり、次の段落が入ります。実行される:
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
各障害点の周囲の点 y について、次の場合:
- y は新しい、つまり開いているテーブルに追加されていない点です。y は初めて検索されるため、y の親ノードをこの障害点に設定し、開いているテーブルに追加します。障害物点から周囲のすべての点までのコストが無限大に設定されているため、挿入文は実際には insert(y,∞) と等価であり、y は開いているテーブルに初めて追加されるため、その k 値は次のようになります。無限大の場合も設定可能です。上図の例では、図全体の左上隅にある紫色の点の周囲の点のみが新しい可能性があり、赤い点の近くにある紫色の点の隣接する8つの点は決して新しいものではありません。new のため、このステップで k が無限大に設定されている場合、これらの点は後で検索されません。ただし、これが発生するポイントは再計画された最短パスには影響しないため、心配する必要はありません。
- y は新しいものではありません (一般的に、ほとんどの場合、y は近くにあります。障害物が最後の探索範囲の端に現れない限り、開いた状況になります。何が起こっているかは想像できます)。 y はこの新しい障害点 (y.parent == xおよびyh !=xh +cost(x,y) ): y の親ノードがこの新しい障害点である場合、 yh !=xh +cost(x) であることに注意してください。 , y) を満たす必要があります。元は yh が xh +cost(x,y) に等しいためですが、x が障害物になると、cost(x,y) は無限大となり、右辺と左辺は等しくなくなります。(yh と xh が両方とも無限である状況は無視してください。この明らかな状況は、解がまったく存在せず、process_state によって展開されないことを意味します) この条件を満たす y はオープン テーブルに入れられますが、その h 値は無限大に設定します（insert(y,∞)と同等のため）
- y は新しいものではなく、y の親ノードは x ではなく、y の現在のパスのコストは、「y が次のステップで x に進み、x の最短パスを取る」コスト、つまり yh より小さいです。 >xh + コスト(x,y): コスト(x,y) は無限であり、考慮する必要がないため、この状況は不可能です。つまり、現時点では上記の 2 つの状況だけが存在します。
条件を満たさない他の点、つまり y の親ノードが x ではない点の場合、これらの点は新しい障害物の影響を受けず、終点までの最短経路も変化しないため、開いているテーブルに追加する必要があります。再検討してください。
条件を満たしたため公開テーブルに追加された上記の点にどのような変化が生じましたか? つまり、これらの点のh値が無限大になって上昇状態になっているのです！！この「障害物点の周囲の点 h 値が無限大になる」というプロセスが障害物情報を送信するプロセスです。このプロセスは今後も送信されますが、今回は別の方法で送信されます。これについては後ほど説明します。
障害物情報の除去と中継送信
最初に障害物情報を周囲に流した後、最初の探索で得られた最短経路が新たに発見された障害物に邪魔されなければここでプログラムが終了する、これも分かりやすいですね。ブロックされています。なぜ別の方法を探す必要があるのでしょうか? オリジナルで行けば間違いなく最短になります。ただし、ブロックされている場合は、障害物情報を処理する必要があります。もともと、私たちの議論の目的は、何か問題が起こった場合に何が起こるかを議論することでもありました。
前のステップで開いたテーブルに追加されたポイントについては、その h 値が無限大に設定され、レイズ状態にあることがわかります。次に、 process_state がそれらを処理するとき、条件が満たされているため、今度は「最初の判定セット」から処理を開始します。
# 最初の判断セット
k_old < xh の場合
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
if yh<k_old and xh> yh +cost(y,x) then
x.parent = y
xh = yh + コスト(x,y)
終了する場合
終わります
終了する場合
今回の x は、前のステップで開いたテーブルに追加された「障害物の周囲の点」を指します。その h 値は xh=無限大、k_old は最初の検索の値であり、制限された値であり、障害物が存在することを示します。ターゲットポイントまでの距離は考慮されません。x について、その 8 つの隣接点を展開し、両方の点が満たされていれば各点 y を調べます。
- y の h 値が x の k 値より小さい (yh<k_old): まず、新しく発見された障害物ではない x 周囲の点の h 値だけが無限大にならず、これらの点は処理されていないため、再計画プロセスによって、その h 値は依然としてその k 値と等しくなります。yh<k_old を満たすということは、最初の計画では、x の周囲の点が x よりも終点に近いことを意味します (この文については詳しく説明する必要があります。なぜなら、x が最初に計画されたときの k は k_old であり、yh は yk に等しいためです)。これは最初の計画の値でもあります。最初の計画のコンテキストでは、エンドポイントまでの y のコストは x のコストよりも小さくなります)。図で理解すると、次のようになります。
X を点 x としてマークし、終点 (青) が x の右下にあると仮定すると、条件 1 を満たす点はピンクになります。 - x の現在のコスト (つまり、無限大) は、x が最初に y を取得し、次に y の最短経路をたどるコストよりも大きいです (xh> yh + コスト(y,x))。 なぜなら、x の現在のコスト xh は次のとおりです。無限大、ここでの意味は 1を満たす点の中に障害物にならない点があれば、そこからxが歩くコストは必ず安くなります。
上記 2 つの条件を満たすということは、終点までの道が新たな障害物に阻まれても、小回りを利かせてこの障害物を回避できることを意味し、これらの条件を満たすすべてのポイントが実行されます。
x.parent = y
xh = yh + コスト(x,y)
つまり、yから一周すれば逆方向に探索する必要がなく、この段階で終点までの最短経路がすぐに求まります。このように処理が成功した上昇状態の点については、最初の探索で残った情報を利用するため、h 値が直接低下しますが、これを「予備コスト削減」といい、障害物情報が削除されたことに相当します。直接排除される。
例えば、大きく迂回しなければならない箇所は、下の図のように2歩下がって障害物を回避する必要があります。
遡る処理は前回の探索に比べてコストが増加するため、その後他のグリッドを設計する際の検査が必要となるため、ここで直接扱うことはできません。第二回の判決で対処する必要がある
最初の判定セットの後、2 番目の判定セットに入る必要があります。最初の判定セットで x の h 値が正常に変更された場合、x はレイズ状態から直接抜け出すことができます。これをローと呼びます。状態、つまり let 最初の検索の後、h 値は直接 k 値に戻ります。その後、2 番目の判定セットでは、x とその周囲の点がダイクストラの考え方に従って直接処理されます。実行されました:
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
ダイクストラなので、この部分の y.parent == xとyh !=xh +cost(x,y) という条件は発生しません。注意: x の h 値は、最初の検索後の k 値 (k_old) になります。
そして、x が raise 状態を直接終了しない場合、または x の h 値が最初の判断セットで変更されず、依然として無限である場合、2 番目の判断セットで後半が実行され、これは次のようになります。別の「道路は、より長い辺を持つ経路である」という情報、または障害物情報、つまり h が正の無限大であるという情報が、周囲の隣接するセルに渡されます。
else: # k_old == xh が満たされない場合、k_old < xh
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) 次に
y.parent = x
挿入(y, xh + コスト(x,y))
else if (y.parent != x and yh >xh +cost(x,y)) then
xk = xh
挿入(x, xh)
それ以外：
if (y.parent!=xおよびxh>y.h+cost(y,x)およびyt = "close"およびyh>k_old then
挿入(y,yh)
終了する場合
終了する場合
終わります
# 便宜上else ifのロジックを変更しましたが、前回と比べてみると特に変更がないことがわかります。
この部分では、x の 8 つの隣接する点 y も通過し、y について次の状況が発生します。
-
yt== "new" or (y.parent == x and yh !=xh +cost(x,y) ) : y は新しい、つまり y は初めて検索されていないことを意味します; y.parent == x yh != xh +cost(x,y) は、y の親ノードが x を指しているにもかかわらず、y のコストは x のコストと x から y へのコストを足したものではないことを意味します。これは、y のコストが不正確であり、x の変更の影響を受けます。これら 2 つのケースでは、y の親ノードを x に直接ポイントし、(y、x への y の次のステップのパス コスト) を挿入し、開いているテーブルに入れて、後で検索します。このとき、「このメッセージは周囲に伝わりました」のhがあった場合、
-
y.parent != x and yh >xh +cost(x,y) : y の親ノードは x ではありませんが、yh の現在のパスの長さが「y は最初に x に進み、次にそれに応じて最後に進みます」より長いです。 x の最短経路へ」 経路の長さはさらに長くなり、y にはもっと短い方法があることを意味します。そして、この短い道路は x を通過します。これは、x の現在知られている道路が認識に値することを示しています。条件yh >xh +cost(x,y)が満たされる場合、xh は正の無限大であってはなりません(非常に極端な場合、xh=∞, yh=∞+a value,cost(x, y) = 値が有限であるが、前の「1つの値」より小さい場合、プログラム内で無限大が非常に大きなツリー番号に置き換えられるため、この状況が発生する可能性がありますが、それは極端すぎてどのようにするかはまだ考えていませんこの状況を構築するには、一般的には発生すべきではありません。この状況がまだ発生しているとは考えないでください)、つまり、点 x から終点への新しい方法が見つかった場合、x の k を h に昇格させます。この時点で、x の k 値は、新しい障害物の存在を考慮した場合に持つべき「最低コスト」を意味します。*******これは、K値を改善できるプログラムの唯一の場所でもあります。まさにこの文のために、「新しい障害物の発見のために最短経路が長くなる」の場合、つまり、実際のコストをより大きな値に正しく増加させることができます。インターネット上の他の多くの資料では、この文は疑似コードに含まれておらず、この文は d* の元の論文にも含まれていません。これは間違っていると思います。この文がないと、新しい障害物が道路をブロックし、新しい障害物が発生します。最初の再計画が成功しても、後で新たな障害が発見される可能性が高く、2 回目の再計画の k 値はもはや同じではありません。最初の再計画ではこれがk値の意味です！ただし、反復的に実行されるアルゴリズムの場合、実行されるたびに同じ変数が同じ意味を持つ必要があります。******
-
(y.parent!=xおよびxh>y.h+cost(y,x)およびyt = "close"およびyh>k_old: まず条件 yh>k_old について説明します。最初の一連の判断に戻りましょう。 if で、「yh<k_old」のような条件があることがわかりました。明らかに、ここでは yh>k_old がその補足条件です。つまり、「yh<k_old」であるため、最初の判断セットを最初に選択する必要があります。フィルターで除外されたポイントを調べると、基本的には次のようなグラフを使用してそれらを理解できます。
X は点 x としてマークされます。終点 (青) が x の右下にあると仮定すると、ピンクは yh>k_old を満たす点です。
このセクションでは、残りの条件を検討します。次に、条件 yt = "close" を調べてみましょう。一連の長い条件が満たされた場合 (y.parent!=xおよびxh>y.h+cost(y,x)およびyt = "close"およびyh>k_old)、実行する必要があるのはinsert( y,yh) , yt が "open" の場合、 h の値を変更せずに insert を実行することは、何もしないことと同じです。 yt が "new" の場合、ここでは終了しません。 「if yt== "new"または xxx は他のことを実行します。
次に、ここで説明する必要がある残りの条件は、y.parent!=xおよびxh>y.h+cost(y,x) です。この条件は、隣接する点 y に、ターゲット ポイントへのパスが存在しないことを意味します。 x と x を通過する 現在のパスをたどるコストは、最初に y まで歩いてから y のパスをたどるコストよりも大きいことに注意してください。2 と同じことは、yh が無限ではないことを意味します。 y から目標点までの解が存在するという条件。これが起こった場合、y は再検査のためにオープンテーブルに入れられます。しかし、x が独自のパスをたどるよりも、最初に y に移動してから y のパスをたどるほうが速いため、y を指すように x の親ノードを変更してはどうでしょうか (つまり、x.parent = y、xh = y とします)。 h+コスト(y,x))? 実際、開いているテーブルに y が追加されると、次回 process_state がこの y を展開するときに、「y.parent!=xおよびxh>y.h+cost」がk と h の値に従って再度チェックされるためです。 of y. (y,x)" (点 y を展開するときに調べられる条件 yh>x.h+cost(x,y) と同等)。
次に、上で説明されていない最後の条件、つまり最初の一連の判断を検討します。
k_old == xh の場合、
for each_neighbor Y of X: #8 つの隣接する近隣を考慮します
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) または
(y.parent != xおよびyh >xh + コスト(x,y))次に
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
上記の赤でマークされた条件は意味を表します。
結論から始めましょう。上記の条件がトリガーされるには、k_old == xh という前提が満たされる必要があります。ただし、再計画の最初から最後まで xh が常に k_old に等しい点では、これはトリガーされません。このような点については、ターゲット点までの経路コストに影響を与えない新たに発見された障害物に相当するため、その h 値は決して変化しません。新たに発見された障害物の影響を受ける点のみ、最初に xh が上昇し、その後の探索後に、h の値と k の値が再び等しくなるように xk の値を変更します (線xk = xhを通じて)。 、このセクションをトリガーすることは可能ですか。これは、x がターゲット ポイントへの新しい最短経路を見つけ、x の k 値がその正しい意味を復元するために変更されたにもかかわらず、この復元が y に渡されていないことを意味します。y の親ノードが x の場合、その親ノードは y に渡されません。代表的な値 h は x.h+cost(x,y) に等しいはずですが、現在は等しくないため、正しく変更する必要があります。開いているテーブルに y を再度追加する必要がある理由は、y が正しく変更された後の情報を後続のラスターに引き続き渡す必要があるためです。簡単に言えば、ここでの目的は、修正コストに関する情報を伝えることです。
process_state 関数の概要
以上をまとめると、process_state関数で各判定条件に達したときに受け付けられる処理の意味がわかります。
関数 process_state( )：
x = get_min_k_state(oepn_list)
x == Null の場合は -1 を返します
k_old = get_min_k(oepn_list)
open_list.delete(x)
xt= "閉じる"
# 最初の判断セット
k_old < xh の場合
X の各隣接 Y について:
if yh<k_old and xh> yh +cost(y,x) then
#最初はコストの削減を試みますが、再計画中にのみ使用されます
x.parent = y
xh = yh + コスト(x,y)
終了する場合
終わります
終了する場合
# 2 番目の判定セット
k_old == xh の場合、
X の各隣接 Y について:
if yt== "new"または # ①dijkstra 通常の検索処理
(y.parent == x and yh !=xh +cost(x,y) ) or # ②補正コスト情報の送信
(y.parent != x and yh >xh +cost(x,y)) then # ③ダイクストラ通常探索処理
# ②条件は再計画時にのみ使用され、①③はプロセス全体で使用されます
y.parent = x
挿入(y, xh + コスト(x,y))
終了する場合
終わります
else: # k_old == xh が満たされない場合、k_old < xh
X の各隣接 Y について:
yt== "新規" の場合、または
(y.parent == xおよびyh !=xh +cost(x,y) ) 次に
#経路コスト変更情報の送信（障害物情報や「正しい経路変更」情報の場合もあります）
y.parent = x
挿入(y, xh + コスト(x,y))
それ以外：
if (y.parent != xおよびyh >xh +cost(x,y)) then
# 点 x は最後まで新しい方法を見つけました。k 値を修正して「最小コスト」の意味を復元します。
xk = xh
挿入(x, xh)
それ以外：
if (y.parent!=xおよびxh>y.h+cost(y,x)およびyt = "close"およびyh>k_old then
# この点は再検討して、開いているテーブルに再入力する必要があります
挿入(y,yh)
終了する場合
終了する場合
終了する場合
終わります
終了する場合
get_min_k(oepn_list) を返す
以上が d* アルゴリズム、特に process_state の詳細な説明です。
Pythonの練習用コード
以下は実際のコードですので、使用する際はmain関数を修正してください。
NEW_OBS_STEP = 38 # 在走了多少步之后发现新障碍
map_png = "./my_map.png" # 初始地图png图片
obs_png = "./my_map_newobs.png" # 新增障碍物png图片，要跟初始地图相同大小，黑色认为是新障碍物上記の 3 つの変数。利用可能な付属の地図画像は次のとおりです。
(画像である限り、CSDN によって抑制されるようです。描画ソフトを使用して 2 つの PNG ファイルを描画し、my_map.png と my_map_newobs.png という名前を付けることをお勧めします。障害物を表すには白と黒を使用します) My_map は元のマップを表し、my_map_newobs は途中で発見された新しい障害物を示します。2 つの画像は同じピクセルの長さと幅を持つ必要があります。両方とも 100*100 ピクセルであることをお勧めします。)
2 つの画像を次の .py ファイルと同じディレクトリに配置し、同じディレクトリで次の .py ファイルを実行します。
ただし、上の 2 つの画像には csdn によってウォーターマークが追加されているため、自分で描いた方が良いかもしれません。
#!/usr/bin python
# coding:utf-8
import math
import cv2
from sys import maxsize
NEW_OBS_STEP = 38 # 在走了多少步之后发现新障碍
class State(object):
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
self.state = "."
self.t = "new"
self.h = 0
self.k = 0
def cost(self, state):
if self.state == "#" or state.state == "#" or self.state == "%" or state.state == "%":
return maxsize
return math.sqrt(pow(self.x - state.x, 2) + pow(self.y - state.y, 2))
def set_state(self, state):
# .普通格子 @ 新路径 # 障碍 % 新障碍 + 第一次搜索的路径 S 起点 E 终点
if state not in [".", "@", "#", "%", "+", "S", "E"]:
return
self.state = state
class Map(object):
def __init__(self, row, col):
self.row = row
self.col = col
self.map = self.init_map()
def init_map(self):
map_list = []
for i in range(self.row):
temp = []
for j in range(self.col):
temp.append(State(i, j))
map_list.append(temp)
return map_list
def get_neighbors(self, state):
state_list = []
for i in [-1, 0, 1]:
for j in [-1, 0, 1]:
if i == 0 and j == 0:
continue
if state.x + i < 0 or state.x + i >= self.row:
continue
if state.y + j < 0 or state.y + j >= self.col:
continue
state_list.append(self.map[state.x + i][state.y + j])
return state_list
def set_obstacle(self, point_list):
for i, j in point_list:
if i < 0 or i >= self.row or j < 0 or j >= self.col:
continue
self.map[i][j].set_state("#")
class DStar(object):
def __init__(self, maps):
self.map = maps
self.open_list = set()
def process_state(self):
x = self.min_state()
if x is None:
return -1
old_k = self.min_k_value()
self.remove(x)
print("In process_state:(", x.x, ", ", x.y, ", ", x.k, ")")
# raise状态的点，包含两种情况：①有障碍信息，不知道怎么去终点的点 ②找到了到终点的路径，但是这条路径比最初没障碍的路径长，还要考察一下
if old_k < x.h:
for y in self.map.get_neighbors(x):
if old_k >= y.h and x.h > y.h + x.cost(y):
x.parent = y
x.h = y.h + x.cost(y)
# low状态的点
if old_k == x.h: # 注意这个开头的if，不可以是elif，不然算法就不对了。某网上资源是有错误的
for y in self.map.get_neighbors(x):
if ((y.t == "new") or
(y.parent == x and y.h != x.h + x.cost(y)) or
(y.parent != x and y.h > x.h + x.cost(y))):
y.parent = x
self.insert_node(y, x.h + x.cost(y))
else: # raise状态的点
for y in self.map.get_neighbors(x):
if (y.t == "new") or (y.parent == x and y.h != x.h + x.cost(y)):
y.parent = x
self.insert_node(y, x.h + x.cost(y))
else:
if y.parent != x and y.h > x.h + x.cost(y):
x.k = x.h
self.insert_node(x, x.h)
else:
if y.parent != x and x.h > y.h + x.cost(y) and y.t == "close" and y.h > old_k:
self.insert_node(y, y.h)
return self.min_k_value()
def min_state(self):
if not self.open_list:
print("Open_list is NULL")
return None
result = min(self.open_list, key=lambda x: x.k) # 获取openlist中k值最小对应的节点
if result.k >=maxsize/2:
return None
return result
def min_k_value(self):
if not self.open_list:
return -1
result = min([x.k for x in self.open_list]) # 获取openlist表中值最小的k
if result >= maxsize/2:
return -1
return result
def insert_node(self, state, h_new):
if state.t == "new":
state.k = h_new
elif state.t == "open":
state.k = min(state.k, h_new)
elif state.t == "close":
state.k = min(state.k, h_new)
state.h = h_new
state.t = "open"
self.open_list.add(state)
def remove(self, state):
if state.t == "open":
state.t = "close"
self.open_list.remove(state)
def modify_cost(self, state):
if state.t == "close":
self.insert_node(state, state.h)
def run(self, start, end, obs_pic_path):
self.insert_node(end, 0)
while True:
temp_min_k = self.process_state()
if start.t == "close" or temp_min_k == -1:
break
start.set_state("S")
s = start
while s != end:
s = s.parent
if s is None:
print("No route!")
return
s.set_state("+")
s.set_state("E")
# 添加噪声障碍点！！
# rand_obs = set()
# while len(rand_obs) < 1000:
# rand_obs.add((int(random.random()*self.map.row), int(random.random()*self.map.row)))
# 根据图片添加障碍点
rand_obs = set()
# new_obs_pic = cv2.imread("./my_map_newobs.png")
new_obs_pic = cv2.imread(obs_pic_path)
for i in range(new_obs_pic.shape[0]):
for j in range(new_obs_pic.shape[0]):
if new_obs_pic[i][j][0] == 0 and new_obs_pic[i][j][1] == 0 and new_obs_pic[i][j][2] == 0:
rand_obs.add((i,j))
temp_step = 0 # 当前机器人走了多少步(一步走一格)
temp_s = start
is_noresult = False # 新障碍物是不是导致了无解
while temp_s != end:
if temp_step == NEW_OBS_STEP:
# 观察到新障碍
for i, j in rand_obs:
if self.map.map[i][j].state == "#":
continue
else:
self.map.map[i][j].set_state("%") # 新增障碍物
self.modify_cost(self.map.map[i][j])
k_min = self.min_k_value()
while not k_min == -1:
k_min = self.process_state()
if k_min >= temp_s.h or k_min == -1:
# 条件之所以是>=x_c.h，是因为如果从x_c找到了到达目的地的路，那么自身的h就会下降，当搜索到k_min比这个h小时，
# 证明需要超过“从当前点到终点的最短路径”的代价的路径已经找完了，再搜也只会找到更长的路径，所以没必要找了，其他
# 解都不如这个优。如果一直找不到解，那么x_c.h会是一直是无穷，那么就是说整个openlist都会被搜索完，导致其变为
# 空。所以就是无解
is_noresult = (k_min == -1)
break
if is_noresult:
break
if temp_step == NEW_OBS_STEP:
draw_s = temp_s
while not draw_s == end:
draw_s.set_state("@")
draw_s = draw_s.parent
temp_s = temp_s.parent
temp_step +=1
temp_s.set_state("E")
if __name__ == "__main__":
NEW_OBS_STEP = 38 # 在走了多少步之后发现新障碍
map_png = "./my_map.png" # 初始地图png图片
obs_png = "./my_map_newobs.png" # 新增障碍物png图片，要跟初始地图相同大小，黑色认为是新障碍物
my_map = cv2.imread(map_png)
_x_range_max = my_map.shape[0]
_y_range_max = my_map.shape[1]
mh = Map(_x_range_max,_y_range_max)
for i in range(_x_range_max):
for j in range(_y_range_max):
if my_map[i][j][0] == 0 and my_map[i][j][1] == 0 and my_map[i][j][2] == 0:
# png图片里黑色的点认为是障碍物
mh.set_obstacle([[i,j]])
start = mh.map[0][0] # 起点为（0,0）,左上角
end = mh.map[_x_range_max-1][_y_range_max-1] # 终点为（max_x-1,max_y-1），右下角
dstar = DStar(mh)
dstar.run(start, end, obs_png )
for i in range(_x_range_max):
for j in range(_y_range_max):
if dstar.map.map[i][j].state[0] == "@": # 红色格子为发现新障碍物重规划后的路径
my_map[i][j][0] = 0
my_map[i][j][1] = 0
my_map[i][j][2] = 255
elif dstar.map.map[i][j].state[0] == "+": # 蓝色格子为第一次搜索的路径
my_map[i][j][0] = 255
my_map[i][j][1] = 0
my_map[i][j][2] = 0
elif dstar.map.map[i][j].state[0] == "%": # 紫色为新发现的障碍物
my_map[i][j][0] = 255
my_map[i][j][1] = 0
my_map[i][j][2] = 255
elif dstar.map.map[i][j].state[0] == "E": # 终点
my_map[i][j][0] = 128
my_map[i][j][1] = 0
my_map[i][j][2] = 128
cv2.imshow("xx", my_map)
cv2.waitKey()
cv2.imwrite("./my_map_result.png", my_map)
上の図は、プログラムを実行した後の出力を示しています。
左上隅が開始点、右下隅が終了点です。青いパスは最初に計画されたパス、紫のパスは途中で発見された新しい障害物、赤いパスは新しい障害物を発見した後に再計画されたパスです。プログラム内の NEW_OBS_STEP 変数を変更して、新しい障害物を見つける手順を変更します。