A、ダイクストラアルゴリズム
開始オブジェクトの始点からダイクストラのアルゴリズムは、図3のアクセスノード、配置されています。それは集中ノードであることを繰り返しチェックノードをチェックし、最も近いノードとチェックノードが設定されている検査対象のノードに追加されていません。ノードは、宛先ノードに到達するまで、最初のノードから外側に延びている設定しました。ダイクストラのアルゴリズムは、限りすべての側面が非負のコスト値を持っているとして、目標点への最初のポイントからの最短経路を見つけることが保証されています。
1.1アルゴリズムの原理と効果図
W [i]は長さの各エッジEを想定し、無向グラフG =(V、E)において、[i]は、残りの頂点対を見つける問題を解決するために考え貪欲アルゴリズムのように記述することができる使用してダイクストラ法各点への最短経路。
図VSにおけるダイクストラGによって最短パスを計算する際に(すなわち、VS頂点から数えて)の開始点を指定します。また、二組のSとUの導入 Sは、最短パス計算頂点(および対応する経路長)を記録する効果があり、そしてUは、最短経路の決定されていない記録の頂点(頂点と対先頭までの距離)です。Sのみ起点、対、最初; Uは、頂点以外で、パスは、頂点U「頂点対の出発点へのパス」である対 次に、頂点Uからの最短経路を見つけること、及びSに加え、次に、U.における頂点と頂点に対応するパスを更新 次いで、U、Sにおける頂点からの最短パスを見つけるに添加し、次に、U.における頂点と頂点に対応するパスを更新 すべての頂点が横断してまで、この動作を繰り返します。
1.2ソースコード解析
出典:https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/Dijkstra/dijkstra.py
メインコードのアルゴリズム分析:
DEF dijkstra_planning(SX、SY、GX、Gyを、OX、オイ、リゾ、RR): "" "
SX:位置X [m]を起動
開始位置Y [m]が起点#1:SY GX:目標x位置を[M ] グレイ:目標位置Y [M]#は、目標点の位置座標 OXを:障害物位置Xのリスト[M] オイ:障害物の位置をY [M]#障害の一覧座標 RESOを:グリッド分解能[M]#解像度グリッドマップ RR:ロボットの半径[m]は#ロボット半径 "" " n始動が =ノード(ラウンド(SX /リゾ)、丸(SY /リゾ)、0.0、-1 )、 正規化座標を丸めた後に添加した#マップ解像度、 ngoal =ノード(ラウンド(GX /リゾ)、丸(グレイ/リゾ)、0.0、-1 ) OX = [IOX /リゾための IOX にOX] オイ = [IOY /リゾため IOY でオイ] obmap、ミンクス、minYの、MAXX、MAXY、XW、YW = calc_obstacle_map(OX、オイ、リゾ、RR) #障害物マップ範囲算出、保存 モーション = get_motion_model() #ノード動きベクトル8 8つの移動方向(X、Y、コスト)の組対応 openset、closedset = dictの()、辞書() #はopensetとclosedsetを作成し、見つからない格納され、ウェイポイントが発見された openset [calc_index(n始動、XW、ミンクス、minYの)] = n始動 しながら 1 : C_ID =分(openset、キー= ラムダO:openset [O] .cost) #Openlistは、出発点から最も近い点を見つける 現在 = openset [C_ID] #は現在のウェイポイントを描く、描く IF :show_animation plt.plot(current.x *リゾ、current.y *リゾ、 " XC " ) のIFでlen(closedsetを.keys())%10 == 0: plt.pause( 0.001)
#先の地点であれば、サイクルの終わり IF current.x == ngoal.xと current.y == ngoal.y: 印刷( " 検索のゴール。" ) ngoal.pind = current.pind ngoal.cost = current.cost BREAKの #はopenListは、ウェイポイントの要件を満たしています デルopenset [C_ID] #修飾パスポイントはclosedlist追加 [C_ID] = closedsetを現在の #、動きベクトルに基づいて拡張された検索範囲 のために Iを、_ で列挙(モーション): ノード =ノード([1 current.xモーション+ [I] [0]、current.yモーション+ [I] ]、 current.cost +モーション[I] [2 ]、C_ID) N_ID = calc_index (ノード、XW、ミンクス、minYの)
#新しいパスポイントが要件を満たしていない場合は、このサイクルの終わり IF ありませんverify_node(ノード、obmap、ミンクス、minYの、MAXX、MAXY): 続行
新しいパスポイントは、このサイクルの終わり、closedlistにすでにある場合は#を IF N_ID 中:closedset 続行 、openlistで新しいルートポイント場合は#を IF N_ID でopenset: IF openset [N_ID] .cost> :node.cost openset [N_ID] .cost = node.cost openset [N_ID] .pind = C_ID 最後に効果ポインタ等#pind、現在のノードをポイントし、利便性とバックルートを行う ほか: opensetは[N_ID] = ノード #ポイントがopenlistに新しいパスを追加するために 、RX、RY = calc_final_path(ngoal、closedset、リゾ) #1、RX、RYアレイ、パスの条件を満たすストレージノード リターン RXを、ライ
二、A *アルゴリズム
BFSアルゴリズムは、任意のノードのコストが目標地点に到達する評価することが可能であることを除いて、同様のプロセスに従って、ダイクストラのアルゴリズムを実行します。ダイクストラのアルゴリズムは、最初のノードから別の最も近いノードを選択し、それがターゲットから最も近いノードを選択します。ダイクストラアルゴリズムは、BFSアルゴリズムの組み合わせで、ダイクストラのアルゴリズムは、ヒューリスティックの特性を持っており、最短経路を見つけることを保証することができます* BFSアルゴリズムは最短経路を見つけることが保証することはできませんが、それは速度よりも速い.A。
2.1アルゴリズムの原理と効果図
-
ヒューリスティック検索要素:ヒューリスティック検索は、この場所から目的の検索まで、検索の各位置、最高の位置を評価するために、状態空間で検索することです。このような多数のイントレピッド検索パスを省略することができ、彼は効率性を言及しました。ヒューリスティック検索では、位置の評価は非常に重要です。異なる評価を使用すると、異なる効果を持つことができます
- 評価関数(費用):推定コストから宛先ノードまでの現在の移動ノードは、この推定値は、ヒューリスティックです。迷路における経路探索問題と問題では、通常、マンハッタン(マンハッタン)と評価関数の推定コスト
- :開始ルート計画のための出発点
- :目標経路計画の終了
- g_score:現在のポイント(前記current_node)から起点まで(開始)移動のコスト
- h_score:移動のコストの目標点推定値に現在のポイント(current_nodeの)から、障害物を考慮していません
- f_score: f_score = g_score + h_score
- openlist:経路探索プロセスノードリストを検索します
- closelist:あなたは再び取り出したノードのリストを必要としません
- comaFrom:親子関係を格納するノードのリスト、最適なパスをバックアップするために使用されるが、必ずしも、ノードポインタによって実現されてもよいです
2.2ソースコード解析
出典:https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/AStar/a_star.py
同様に、コードとダイクストラの主な内容、ユークリッド距離ベースcalc_heuristic()関数を増加させます。
DEF a_star_planning(SX、SY、GX、GY、OX、OY、RESO、RR): ""」 GX:目標位置x [M] GY:目標のy位置[M] OX:障害物の位置xリスト[M] オイ:障害物[M]のY位置リスト グリッド分解能[M]:RESO RR:ロボットの半径[m]は ""」 n始動 =ノード(ラウンド(SX / RESO)、丸(SY / RESO)、0.0、-1 ) ngoal =ノード(ラウンド(GX / RESO)、丸(GY / RESO)、0.0、-1 ) OX = [IOX / RESO ための IOX にOX] OY = [IOY / RESO ため IOY でオイ] obmap、ミンクス、MINY、MAXX、MAXY、XW、YW = calc_obstacle_map(OX、OY、RESO、RR) 運動 = get_motion_model() openset、closedset = dictの()、辞書() openset [calc_index(n始動、XW、ミンクス、 MINY)] = n始動 しながら 1 : C_ID =分(openset、キー=ラムダO:openset [O] .cost + calc_heuristic(ngoal、openset [O]))#对应于F = G + H 現在 = openset [C_ID] #のショーのグラフ の場合 show_animation: #のプラグマ:なしカバー plt.plot(current.x * RESO、current.y * RESO、「XC" ) 場合でlen(closedset.keys())%10 == 0: plt.pause( 0.001 ) の場合 current.x == ngoal.x と current.y == ngoal.y: プリント(" ゴールを探す」) ngoal .pind = current.pind ngoal.cost = current.cost 破る #オープンセットから項目を削除 デルopenset [C_ID] #は閉集合に追加します [C_ID] = closedset 現在 #運動モデルに基づいて検索グリッドを展開 するため _、I に列挙(モーション): ノード =ノード(current.x + モーション[I] [0]、 current.y +モーション[I] [1 ]、 current.cost +運動[I] [2 ]、C_ID) n_id = calc_index(ノード、XW、ミンクス)、MINY 場合 n_id にclosedset: 続ける 場合 ではないノード、obmap、ミンクス、MINY、MAXX、MAXY(verify_node): 続ける 場合 n_id ない で openset : openset [n_id]=ノード #は、新しいノードを発見し 、他: もし openset [n_id] .cost> = node.cost: #このパスは、今まで最高です。それを記録しなさい! openset [n_id] = ノード RX、RY = calc_final_path(ngoal、closedset、RESO) リターン RX、RY
増加calc_heuristic()関数:
DEF (N1、N2)calc_heuristic: W = 1.0 #のヒューリスティックの重量(H重み重み関数) D = W * Math.sqrt((n1.x - n2.x)** 2 +(n1.y - n2.y )** 2 ) #1ユークリッド距離計算 リターン D
ヒューリスティックの2.3グリッドマップ
- マンハッタン距離:
標準ヒューリスティックマンハッタン距離(マンハッタン距離)。あなたの移動を考慮し、最小コストD.の位置に隣接した位置からのコスト関数を見つけます したがって、マップヒューリスティックは、マンハッタン距離Dの時間は、唯一の地図上に動き回るの場合に使用すべきです。
H(N)= D *(ABS(NX - goal.x)+ ABS(NY - goal.y))
- 対角距離:
あなたのマップは、あなたが斜めの動きを可能なら、あなたは別のヒューリスティック機能を必要としています。(4東、北4)マンハッタン距離は8 * D.なります しかし、あなたは、単に場所で(4北東)に移動することができますので、ヒューリスティック関数は4 * D.する必要があります この関数は、対角線を使用し、コストが直線対角線Dであるとします。
H(N)= D * MAX(ABS(NX - goal.x)、ABS(NY - goal.y))
- ユークリッド距離:
あなたのユニットは、任意の角度に沿って(代わりに、グリッド方向の)移動することができる場合、あなたはおそらく直線距離を使用する必要があります。
H(N)= D * SQRT((NX-goal.x)^ 2 +(NY-goal.y)^ 2)
このような場合には、コスト関数gは、ヒューリスティック関数hと一致しないためしかし、直接使用することは、トラブル時に*を持っています。ユークリッド距離は、マンハッタンと対角距離の距離よりも短くなっているので、あなたはまだ最短ルートを得ることができますが、Aは*たくさんの長いを実行します:
- ユークリッド距離が乗:
私は高価な平方根演算を避けるために、距離の二乗ユークリッド距離を使用することによって、あなたをできるように指し*、のページの一部を見てきました:
H(N)= D *((NX-goal.x)^ 2 +(NY-goal.y)^ 2)
それをしないでください!これは明らかに問題の宗派が発生します。*のF(N)= G(N)+ H(n)を計算するとき、それは距離g価格の正方形よりもはるかに大きい、あなたはヒューリスティック評価値が高すぎて停止するからです。長い距離のため、これはBFSに分解極端なケースG(n)を算出するもはや何も、A *を閉じます。
選択したデータ構造の第三に、セット