動作計画のための探索アルゴリズム: フロントエンド計画、バックエンド軌道生成から状態解まで

この記事は、Heart of Autonomous Driving の公開アカウントから許可を得て転載したものです。転載については出典元にお問い合わせください。

背景：

私は2016年から2018年にかけて自動運転に取り組んでおり、当時はモビリティプランニングに夢中になっていたのですが、学ぶべき資料はほとんどなく、インターネット上の論文もあまりありませんでした。基本的に、DARPA の無人オフロード競技に関する記事は数十件あり、実装の体系的な記事やコードの説明は基本的にありません。そのため、移動計画の理解は包括的ではありません。近年、自動運転やドローンの研究と応用の増加に伴い、多くの論文やコースでこの内容を体系的に紹介し始めています。科学技術者にとって、ロボットは自動的かつインテリジェントな計画が可能です。ロボットに抵抗できず、ロボットについてもっと知りたいと思わない科学技術者は多くないと思います。そこで私は情報を収集し、このトピックに関するシリーズをいつ公開できるかを計画し、その後、ロボットにさまざまなものを実装するためのプロジェクトをコーディングしてきました。このアイデアの実現を加速させたのは、2年前にfast-lab Gaofeiチームが公開した、飛行経路によるドローンの経路計画の問題を解決する方法に関する一連のビデオでした。初めてこのビデオを見たときは本当に衝撃を受けました。モバイルプランニングは、このように美しい数学的フレームワークを使って実行できることがわかりました。私がこれまで長々と語ってきたのは、私の過去の経験に敬意を表し、この特別なテーマの最初の講義を始めるためでした。このシリーズの主なテーマは、Gao Fei 教授の講義「移動ロボットの動的プログラミング」ですが、これにアルゴリズムの詳細と私自身の考えが補足されます。このコースは、モバイルプランニングシステムフレームワークの構築に最適で、内容も非常によくまとめられています。唯一の欠点は、このコースがもともとドローン向けに設計されているため、動的な不確実な障害物に対する計画があまりないことです。

文章：

パスプランニングは、現代のロボット工学や自動運転などの分野では重要なトピックです。特定の環境内で出発点から目的地までの最適なパスを見つける作業です。このプロセスは通常、フロントエンドパス検索とバックエンド軌道計画の 2 つの部分に分かれています。フロントエンドパス検索では、マップ内で障害物を回避する軌道を検索し、バックエンド軌道計画では、検索した軌道を最適化してロボットの運動学的制約と動的制約を満たします。
実際の環境でのロボットの動作計画は、比較的複雑な問題です。人間は通常、複雑な問題を段階的に解決します。まず大まかな概要を作成し、その概要に基づいて徐々に複雑で洗練されたものにしていきます。ロボットの動作計画に対する学術的なアプローチについても同様です。

1. フロントエンド: 経路計画

検索ベースの方法
一般的なグラフ検索: 深さ優先探索 (DFS)、幅優先探索 (BFS)
ダイクストラと A* 検索
ジャンプ検索
サンプリングベースの方法
確率的ロードマップ (PRM)
高速探索ランダムツリー (RRT)
RRT、情報付きRRT
動的制約による経路計画
状態間境界値最適制御問題
ステータスグリッド検索
ダイナミックRRT*
ハイブリッドA*

2. バックエンド: 軌道生成

最小ジッタ軌道生成
差動平坦度
最小ジッタ最適化
最小ジッタの閉形式解
時間配分
実用化
ソフト制約とハード制約による軌道最適化
ソフト制約軌道最適化
ハード制約軌道最適化

3. 不確実な状態の解決：移動する障害物、環境の突然の変化、機器のモデリングの変更

マルコフ決定過程 (MDP) に基づく計画
計画とマルコフ決定プロセスにおける不確実性
最小最大コスト計画と予想コスト最小計画
価値反復とリアルタイム動的プログラミング
ロボット計画のためのモデル予測制御 (MPC)
線形モデル予測制御
非線形モデル予測制御

フロントエンド - 検索パス計画

この部分を始める前に、いくつかの概念を紹介する必要があります。これらの概念を紹介する目的は、ビジネスにおける検索の応用をより実践的に理解できるようにすることです。探索経路計画ではロボットを粒子として考えますが、実際のロボットは一定の形状を持ち、一定の空間を占めます。ロボットを粒子として考えると、実際には実行不可能な経路（障害物に遭遇する経路）が探索される可能性が高くなります。この問題を解決するには、オブジェクトの形状をマップに転送するだけです (マップの障害物領域にオブジェクトが占めるスペースを加えます)。このようなマップでは、ロボットは実行可能な経路を検索する粒子として扱われます。

構成空間での計画¹²³

ロボットはC空間内の点として表現される。例えば、位置（R3内の点）、姿勢（（3）内の点）などである。⁴⁷⁸
障害物は構成空間（動作計画前の1回限りのタスク）で表現する必要があり、構成空間障害物またはC障害物と呼ばれる⁴⁵⁶
C-空間 = (C-障害物) ∪ (C-自由)⁴⁵⁶
経路計画とは、C-free⁹[10]¹⁵において出発点qstartから目的地qgoalまでの経路を見つけることである。

ワークスペース内

ロボットには形や大きさがある（つまり動作計画が難しい）
構成空間: C空間
ロボットはポイント（つまり、動きやすい計画）⁶
動作計画の前に、障害物はC空間⁸[10]で表現される。
C 空間で障害物を表現するのは非常に複雑になる可能性があります。したがって、実際には近似的な（しかしより保守的な）表現が使用されます。

ロボットを半径 _ の球体として保守的にモデル化すると、すべての方向に障害物 _ を膨らませることで C 空間 1 を構築できます。これはロボット工学における衝突検出の一般的なアプローチであり、球体の中心が膨張したマップの自由空間内にあることを確認することによって衝突評価が実行されます1。しかし、この保守的なアプローチでは、ロボットの形状とサイズが考慮されていません。

マップを作成します。

経路計画では、検索マップの構築が重要なステップです。これには通常、実際の環境をグラフに抽象化することが含まれます。グラフでは、ノードは可能な場所を表し、エッジはある場所から別の場所への移動を表します。詳細な例を以下に示します。
屋内環境で移動する必要があるロボットがあるとします。この環境は、テーブルや椅子などの障害物がある部屋である場合があります。

ノードの定義: まず、ノードの位置を決定する必要があります。この例では、部屋内の到達可能な各位置をノードとして定義できます。たとえば、各グリッドセルがノードとなるグリッドを作成できます。
エッジの定義: 次に、エッジを識別する必要があります。ロボットが 1 つのノードから別のノードに直接移動できる場合、2 つのノード間にエッジが存在します。私たちの場合、2 つのグリッドセル (つまりノード) が隣接しており、それらをブロックする障害物がない場合、それらのセル間にはエッジが存在します。
重みの定義: 最後に、各エッジの重みを定義する必要があります。重みは実際の移動コストに基づいて決定できます。たとえば、あるノードから別のノードまでの距離が長い場合や、パスに傾斜がある場合は、エッジの重みを大きくする必要があります。

マップタイプ:

グリッドマップは、環境を一連のグリッドに分割します。グリッドは、数学的な観点からエッジで接続されたノードの集合です。タイルベースのマップは、各タイルがノードであり、タイル間の接続が短い線のように見えるグリッドマップとして考えることができます。
コストマップ: グリッドマップに基づいて、各グリッドに、グリッドが占有される確率を示す可能な値が与えられている場合、そのマップは確率マップです。
特徴マップ: 特徴マップは、関連する幾何学的特徴 (点、線、面など) を使用して環境を表します。 vSLAM (ビジュアル SLAM) テクノロジーでよく使用されます。これは通常、GPS、UWB、カメラをスパース vSLAM アルゴリズムと組み合わせて使用して生成されます。その利点は、データの保存量と計算量が比較的少ないことであり、最も初期の SLAM アルゴリズムでよく見られます。
位相地図は地図作成における統計地図であり、点と線の相対的な位置は正確に保つが、図形の形状、面積、距離、方向は必ずしも正確に保つわけではない抽象的な地図です。有向グラフと無向グラフ（文字通り）が含まれます。

ラスターマップ

確率プロット

機能マップ

位相マップ - 有向グラフ

位相マップ - 無向グラフ

検索アルゴリズムの紹介

マップの種類が非常に多い場合、各マップのパスを計画するには、どのような対応するアルゴリズムを使用できますか?以下はマップに対応するパス検索アルゴリズムです。

 1. 栅格地图/ 概率图1. Dijkstra 2. BFS（Best-First-Search） 3. A* 4. hybrid A* 5. D * 6. RRT 7. RRT* 8. 蚁群算法9. Rectangular Symmetry Reduction (RSR) 10. BUG 11. Beam search 12. Iterative Deepeningc 13. Dynamic weighting 14. Bidirectional search 15. Dynamic A* and Lifelong Planning A * 16. Jump Point Search 17. Theta * 2. 拓扑地图1. Dijkstra 2. BFS（Best-First-Search） 3. A* 4. CH 5. HH 6. CRP

グラフ検索アルゴリズムの構造

：：：成功

訪問するすべてのノードを格納するコンテナを維持する
コンテナは開始状態XSで初期化されます
サイクル
定義済みのスコアリング関数に基づいてコンテナからノードを削除します
ノードを訪問する
拡張機能: ノードのすべての隣接ノードを取得する
すべての近隣地域を発見
隣人をコンテナに押し込む
拡張機能: ノードのすべての隣接ノードを取得する
ループ終了:::

一般的な検索アルゴリズムの構造

一般的に使用されるグラフ検索には 3 つの主要な検索構造があり、他のアルゴリズムはこれら 3 つの主要なフレームワークの下で改良されます。
深さ優先探索 (DFS):

原理: DFS は、ツリーまたはグラフを走査または検索するためのアルゴリズムです。このアルゴリズムは、ツリーのブランチを可能な限り深く検索します。ノード v のすべてのエッジが探索されると、検索はノード v を見つけたエッジの開始ノードに戻ります。このプロセスは、ソースノードから到達可能なすべてのノードが検出されるまで続行されます。まだ検出されていないノードがある場合は、そのうちの 1 つがソースノードとして選択され、上記のプロセスが繰り返されます。すべてのノードが訪問されるまで、プロセス全体が繰り返されます。
利点: 実装が簡単で、ターゲットが明確な場合は検索効率が高くなります。
デメリット: 最短経路が見つかる保証はなく、検索が無限ループに陥る可能性があります。

幅優先探索 (BFS):

原理: BFS は、ツリーまたはグラフを検索するために使用される幅優先探索アルゴリズムです。このアルゴリズムはルートノードから開始し、ツリーの幅に沿ってツリーのノードをトラバースします。すべてのノードが訪問されると、アルゴリズムは終了します。
利点: 最短経路を見つけることができ、結果は信頼できます。
デメリット: 空間の複雑度が高く、ソリューション空間が大きい場合のメモリ消費量が多くなります。

貪欲な検索:

原則: 貪欲探索は、一連の最適な選択を通じてグローバルな最適解を生み出すことを期待して、各ステップで最良のオプションを選択する方法です。
利点: シンプル、実装が簡単、計算速度が速い。
欠点: グローバル最適解を見つけることは保証できず、ローカル最適解のみが見つかることしか保証されません。
深さ優先探索(DFS): DFS は、それ以上進めなくなるまでパスを検索し続け、その後戻って次のパスの検索を開始します。この検索戦略は「先入れ後出し」とみなすことができるため、DFS を実装するときには通常、スタックデータ構造が使用されます。 DFS の利点は、実装が簡単で、目標が明確な場合に検索効率が高いことです。ただし、DFS の欠点は、最短パスが見つかる保証がなく、検索が無限ループに陥る可能性があることです。
幅優先探索(BFS): 対照的に、BFS は開始点からの距離に基づいて、最も近いノードから最も遠いノードの順に検索します。この検索戦略は「先入れ先出し」とみなすことができるため、BFS を実装するときには通常、キューデータ構造が使用されます。 BFS の利点は、最短経路を見つけることができ、結果が信頼できることです。ただし、BFS の欠点は、ソリューション空間が大きい場合に空間複雑度が高く、メモリ消費量が多くなることです。

アルゴリズムの中心的な質問は次の 3 つです。

質問 1: ループはいつ終了しますか?
可能なオプション: コンテナが空になったらループを終了する
質問 2: グラフが循環的である場合はどうなりますか?
ノードがコンテナから削除（拡張/アクセス）されると、コンテナに再度追加することはできません。
質問 3: 正しいノードを削除してできるだけ早く目標状態に到達し、グラフノードの拡張を減らす方法。

深さ優先アルゴリズム: データ構造は後入れ先出し(LIFO)コンテナ(スタック)を維持し、アルゴリズムはコンテナ内の最も深いノードを削除/拡張します。

 #生成示例数据graph = {} graph["A"] = ["B", "D", "F"] graph["B"] = ["C", "E"] graph["D"] = ["C"] graph["F"] = ["G", "H"] graph["C"] = [] graph["E"] = [] graph["G"] = [] graph["H"] = [] from collections import deque search_queue = deque() # 创建一个节点列表search_queue += graph["A"] # 表示将"A"的相邻节点都添加到节点列表中from collections import deque def search(start_node): search_queue = deque() search_queue += graph[start_node] searched = [] # 这个数组用于记录检查过的节点while search_queue: # 只要节点列表不为空node = search_queue.pop() #深度优先#node = search_queue.popleft() # 广度优先取出节点列表中最左边的节点print(node, end=' ') # 打印出当前节点if not node in searched: # 如果这个节点没检查过if node == 'G': # 检查这个节点是否为终点"G" print("\nfind the destination!") return True else: search_queue += graph[node] # 将此节点的相邻节点都添加到节点列表中searched.append(node) # 将这个节点标记为检查过# 如果节点列表为空仍没找到终点，则返回False return False print(search("A"))

幅優先探索アルゴリズム:
データ構造: 先入れ先出し (FIFO) コンテナー (キュー) を維持し、アルゴリズム操作: コンテナー内の最も浅いノードを削除/拡張します。具体的なコードについては、上記の深さ検索アルゴリズムを参照し、「node = search_queue.pop() #depth first」を「node = search_queue.popleft() #breadth first take out the leftmost node in the node list」に置き換えてください。 BFS と DFS の違いは、「先入れ」または「後入れ」の原則に従って境界から次のノードを選択することにあることがわかります。

貪欲な検索:
貪欲アルゴリズムの特徴は、一般的なアルゴリズムが開始ノードから任意のポイントまでのコストを考慮するのに対し、目標ノードから任意のポイントを見つけるコストを考慮することです。つまり、貪欲アルゴリズムは、ターゲットノードに到達する時間コストが最小になるように、ターゲットノードをすばやく見つける方法を考慮します。一方、一般的なアルゴリズムは、ターゲットノードをすばやく見つける方法ではなく、ターゲットノードに到達するコストを最小化する方法を考慮します。貪欲な戦略は、正しい道ではないにもかかわらず、目標に向かって進もうとします。目標に到達するためのコストのみを考慮し、これまでに費やしたコストは無視するため、経路が非常に長くなっても前進し続けます。
貪欲アルゴリズムにおける「アクションのコスト」は、任意のノード n からターゲットノードまでの最小コスト評価値を計算するヒューリスティック関数 h(n) を使用して計算できます。ヒューリスティック関数によって貪欲アルゴリズムの計算方法が決まるため、適切なヒューリスティック関数を選択することが重要です。

実際の検索問題では、ノードからその隣接ノードまでのコストは「C」になります。
ヒューリスティック関数として計算できるコストには、長さ、時間、エネルギーなどがあります。
すべての重みが1の場合、貪欲アルゴリズムは最適な解を見つける。
一般的なケースでは、最小コストのパスをできるだけ早く見つけるにはどうすればよいでしょうか?
ダイクストラアルゴリズム:
ダイクストラ法は貪欲な考え方で実装されており、トポロジカルグラフやグリッドグラフに適用できます。具体的な実装方法は、まず開始点からすべての点までの距離を保存して最短距離を探し、その後 1 回緩和して、もう一度最短距離を探すというものです。いわゆる緩和操作とは、乗り換え駅として見つけた最短距離の点を通ると、より近くなるかどうかを 1 回トラバースすることです。より近い場合は、距離を更新します。このようにして、すべての点を検索した後、開始点から他のすべての点までの最短距離が保存されます。

戦略: 累積コストg(n)が最も低いノードを拡張/訪問する
g(n): 開始状態からノード「n」までの累積コストの現在の最良の推定値
「m」のすべての未拡張近傍の累積コストg(m)を更新します。
展開/訪問されたノードは、開始状態からそのノードまでのコストが最小であることが保証されます:::success
拡張するすべてのノードを格納するための優先キューを維持する
優先キューを開始状態XSで初期化する
g(XS)=0に設定し、グラフ内の他のすべてのノードに対してg(n)=無限大に設定する
サイクル：
キューが空の場合は、FALSE を返してループを終了します。
優先キューからg(n)が最小のノード「n」を取る
ノード 'n' を展開済みとしてマークする
ノード「n」がターゲット状態にある場合は、TRUEを返してループを終了します。
ノード「n」のすべての展開されていない隣接ノード「m」の場合:
g(m) = 無限大の場合
g(m) = g(n) + Cnm
キューにノード「m」を追加する
g(m) > g(n) + Cnmの場合
g(m) = g(n) + Cnm
隣接ノードのループを終了する
メインループの終了::: BFS (Best-First-Search) アルゴリズム
BFS (Best-First-Search) アルゴリズムは、ヒューリスティックベースの深さ優先アルゴリズムとも言えます。ダイクストラ法に似たプロセスに従って実行されますが、違いは、任意のノードからターゲットポイントまでのコスト (つまり、ヒューリスティック関数) を評価できることです。初期ノードに最も近いノードを選択する代わりに、ターゲットノードに最も近いノードを選択します。 BFS では最短経路が必ず見つかるとは限りません。しかし、ヒューリスティック関数を使用してターゲットノードにすばやく誘導するため、ダイクストラのアルゴリズムよりもはるかに高速です。たとえば、ゴールがスタート地点の南にある場合、BFS は南に向かうパスを選択する傾向があります。下の図では、黄色のノードはヒューリスティック値が高いこと（ターゲットへの移動コストが高いこと）を表し、暗い色のノードはヒューリスティック値が低いこと（ターゲットへの移動コストが低いこと）を表します。これは、BFS が Dijkstra のアルゴリズムと比較して高速に実行されることを示しています。

ただし、これらの例はどちらも最も単純なケースです。マップには障害物がなく、最短経路は直線です。ここで、上で説明した凹状の障害物について考えてみましょう。ダイクストラのアルゴリズムは実行速度は遅くなりますが、最短経路を見つけることが保証されます。

一方、BFS は実行速度が速くなりますが、検出されるパスは明らかに適切なパスではありません。

BFS は貪欲な戦略に基づいているため、正しいパスでなくても目標に向かって移動しようとします。目標に到達するためのコストのみを考慮し、これまでに費やしたコストは無視するため、経路が非常に長くなっても前進し続けます。
両方の利点を組み合わせた方が良いのではないでしょうか? 1968 年に発明された A アルゴリズムは、BFS などのヒューリスティックなアプローチと Dijsktra アルゴリズムなどの従来の方法を組み合わせたアルゴリズムです。少し異なるのは、BFS のようなヒューリスティックでは、最適なソリューションを保証するのではなく、近似ソリューションが提供されることが多いことです。ただし、A は最適な解決策を保証できないヒューリスティックに基づいていますが、最短経路を見つけることが保証されています。
A: ヒューリスティック関数を使用したダイクストラのアルゴリズム*
Dijkstra アルゴリズム (初期点に近いノード) と BFS アルゴリズム (目標点に近いノード) の情報ブロックを組み合わせます。 A の標準的な用語では、g(n) は初期ノードから任意のノード n までのコストを表し、h(n) はノード n からターゲットノードまでのヒューリスティック推定コストを表します。 A* は、初期点から目標点に移動するときに、これら 2 つに重み付けします。メインループを実行するたびに、f(n) = g(n) + h(n) となる最小の f(n) を持つノード n がチェックされます。

累積コスト
g(n): 開始状態からノード「n」までの累積コストの現在の最良推定値
ヒューリスティック関数
h(n): ノードnから目標状態までの推定最小コスト（つまり目標コスト）
開始状態からノード「n」を経由して目標状態までの最小推定コストは、f(n) = g(n) + h(n)である。
戦略: 最も安価なノードを拡張する f(n) = g(n) + h(n)
隣接ノード「m」を拡大していないすべてのノード「n」の累積コストg(m)を更新する
拡張されたノードは、開始状態からそのノードまでのコストが最小であることが保証されます:::成功
拡張するすべてのノードを格納するための優先キューを維持する
すべてのノードに対してヒューリスティック関数h(n)を事前定義する
優先キューを開始状態XSで初期化する
g(XS)=0に設定し、グラフ内の他のノードに対してg(n)=無限大に設定する
サイクル：
キューが空の場合は、FALSE を返してループを終了します。
キューからf(n)=g(n)+h(n)が最小のノード「n」を取り出す
ノード 'n' を展開済みとしてマークする
ノード「n」がターゲット状態にある場合は、TRUEを返してループを終了します。
ノード「n」のすべての展開されていない隣接ノード「m」の場合:
g(m)=無限大の場合
g(m) = g(n) + Cnm
ノード「m」をキューに追加します
g(m)>g(n)+Cnmの場合
g(m) = g(n) + Cnm
隣接ノードでループを終了する
メインループの終了::: ヒューリスティック関数を調整することで、A* の動作を制御できます。
極端な場合、h(n) が 0 であれば、g(n) のみが機能し、A* はダイクストラのアルゴリズムになり、最短経路が見つかることが保証されます。
h(n) が常に n からゴールまでの移動の実際のコストより小さい (または等しい) 場合、A は最短経路を見つけることが保証されます。 h(n) が小さいほど、ノード A は拡張され、実行速度は遅くなります。
h(n) が n からゴールまでの移動コストと正確に等しい場合、A は他のノードを拡張せずに最適なパスを見つけるだけなので、非常に高速に実行されます。これはすべてのケースで発生するわけではありませんが、特殊なケースでは完全に等しくなるようにすることができます。完全な情報が与えられれば、A は完璧に機能することを理解するのは良いことです。
h(n) が n からゴールまでの実際の移動コストよりも高い場合、A* が最短経路を見つけることは保証されませんが、実行速度は速くなります。
別の極端なケースでは、h(n) が g(n) よりもはるかに大きい場合、h(n) のみが有効になり、A* は BFS アルゴリズムになります。
目標重力が低すぎる場合、最短経路は得られますが、速度は遅くなります。目標重力が高すぎる場合、最短経路は放棄されますが、A の実行速度は速くなります。そのため、複雑な状況では、最適な経路と最速の検索との間でトレードオフ/バランスが必要になります。
A のこの機能は非常に便利です。たとえば、完璧なパスではなく適切なパスが必要な場合があり、g(n) と h(n) のバランスをとるために、どちらかを変更することができます。

アルファが 0 の場合、改善されたコスト関数の値は常に 1 になります。この場合、地形コストは完全に無視され、作業 A はグリッドが通過可能かどうかを判断するという単純な問題になります。アルファが 1 の場合、元のコスト関数が機能し、A のすべての利点が得られます。アルファ値は 0 から 1 までの任意の値に設定できます。
ヒューリスティック関数の戻り値として、絶対最小コストまたは予想最小コストを選択することを検討できます。たとえば、マップの大部分がコスト 2 の地形で、コスト 1 の道路がある場所がいくつかある場合、ヒューリスティック関数で道路を考慮せず、距離 2 を返すようにすることを検討できます。
速度と精度の選択は、世界的に固定された組み合わせではありません。マップの特定のエリアでは精度が重要であり、これに基づいて動的な選択を行うことができます。たとえば、途中で止まって経路を再計算したり方向を変えたりすることがあると仮定すると、現在位置に近いときには適切な経路を選択することがより重要になります。地図上の安全なエリアでは最短経路はそれほど重要ではないかもしれませんが、危険なエリアを離れるときには、軌道の正確さが最も重要になります。
同様にg(n)またはf(n)を調整することで、Aの特定の動作も制御できます。

g(n) または f(n) (これら 2 つのアクションは同じ意味です) に障害物コスト関数を追加することで、障害物の真ん中にパスを計画することができます。
車両の形状または軌道のカッパ平滑性コスト関数をg(n)またはf(n)に追加することで、計画された経路を滑らかに変更することができます。
ウェイポイントのコスト関数を g(n) または f(n) に追加することで、計画されたパスはウェイポイントの方向に進む傾向があります。
正確なヒューリスティック関数を構築する 1 つの方法は、任意のノードペア間の最短パスの長さを事前に計算することです。このヒューリスティック関数を近似する方法はいくつかあります。

 1. 【降采样地图-预计算】在密集栅格图的基础上添加一个分辨率更大的稀疏栅格图。预计算稀疏图中任意两个栅格的最短路径。 2. 【waypoings-预计算】在密集栅格图上，预计算任意两个way points的的最短路径。

上記の方法により、任意の場所から近隣のナビゲーションポイント/中継ポイント（ウェイポイント）に到達するコストを評価するためのヒューリスティック関数 h' が追加されます。最終的なヒューリスティック関数は次のようになります。
h(n) = h'(n, w1) + 距離(w1, w2), h'(w2, ゴール)
グリッドマップのヒューリスティックアルゴリズム<br>グリッドマップを計算するためのよく知られたヒューリスティック関数がいくつかあります。

 1. 曼哈顿距离2. 对角线距离3. 欧几里得距离4. 欧几里德距离平方

マンハッタン距離の標準的なヒューリスティック関数はマンハッタン距離です。コスト関数を考慮し、ある場所から隣接する場所に移動するための最小コスト D を見つけます。したがって、h はマンハッタン距離の D 倍になります。

 ``` H(n) = D \ * (abs ( nx – goal.x ) + abs ( ny – goal.y ) ) ```

対角距離

マップ内で斜めの移動が許可されている場合は、斜めの距離を考慮する必要があります。マンハッタン距離（東4、北4）は8Dになります。ただし、代わりに単に (4 北東) に移動することができるため、ヒューリスティックは 4D になります。この関数は、直線と対角線の両方のコストが D であると仮定して、対角線を使用します。

 H(n) = D * max(abs(nx - goal.x), abs(ny - goal.y))

対角移動のコストが D ではなく、D2 = sqrt(2) * D のような場合、正確な計算は次のようになります。

 h_diagonal(n) = min(abs(nx - goal.x), abs(ny - goal.y)) h_straight(n) = (abs(nx - goal.x) + abs(ny - goal.y)) H(n) = D2\* h_diagonal(n) + D\* (h_straight(n) - 2\ *h_diagonal(n)))

h_diagonal(n): 対角線に沿って移動できるステップ数、h_straight(n): マンハッタン距離を計算し、これら 2 つの項目を組み合わせて、すべての対角ステップに D2 を掛け、残りのすべての直線ステップ (これはマンハッタン距離のステップ数から対角ステップ数の 2 倍を引いたものであることに注意してください) に D を掛けます。
ユニットが任意の角度（グリッド方向だけでなく）に沿って移動できる場合は、直線距離を使用する必要があります。

 ``` H(n) = D* sqrt((nx-goal.x)^2 + (ny-goal.y)^2) ```

しかし、この場合、コスト関数 g がヒューリスティック関数 h と一致しないため、A を直接使用すると問題が発生します。ユークリッド距離はマンハッタン距離と対角距離の両方よりも短いため、最短経路を取得することはできますが、A は時間がかかります。

ユークリッド距離の二乗

別の方法としては、距離の代わりに距離の二乗を使用して平方根演算を回避し、計算コストを削減する方法があります。

 ``` H(n) = D* ((nx-goal.x)^2 + (ny-goal.y)^2) ```

しかし、これは明らかに測定単位に関する問題を引き起こします。 Aがf(n) = g(n) + h(n)を計算すると、距離の2乗がgのコストよりもはるかに大きくなり、ヒューリスティック関数の評価値が高すぎるため停止します。距離が長くなると、これは g(n) の極端なケースに近づき、何も計算されなくなり、A は BFS に退化します。

ヒューリスティック関数のヒューリスティック因子

A 検索のパフォーマンスが低いもう 1 つの理由は、ヒューリスティック関数のヒューリスティック要素です。いくつかのパスが同じ f 値を持つ場合、それらはすべて探索され、この時点ではそのうちの 1 つだけを検索する必要があるにもかかわらず、比較関数は比較均衡点を破ることができません。次の図は、ヒューリスティック係数を追加しない場合の効果を示しています。

この問題を解決するには、ヒューリスティック関数に小さなヒューリスティック係数を追加します。ヒューリスティック係数は各ノードに対して一意である必要があり、f 値を区別する必要があります。 A は f 値をヒープソートし、f 値が異なるようにするため、1 つの f 値だけがテストされます。

ヒューリスティック要素を追加する 1 つの方法は、h の単位をわずかに変更することです。測定単位を小さくすると、目標に近づくにつれて f は徐々に増加します。残念ながら、これは A がゴールに近いノードではなく、スタート地点に近いノードに拡大する傾向があることを意味します。 h の測定単位をわずかに（0.1% でも）調整すると、A はターゲットに近いノードに拡張される傾向があります。

 ``` heuristic \ \ *= (1.0 + p) ```

ここでのインスピレーションの要素は、

 p < 移动一步的最小代价/ 期望的最长路径长度。

パスを 1000 ステップより長くしたくない場合は、p = 1 / 1000 を使用できます。この追加値を追加した結果、A は以前よりも少ないノードを検索します。下の図の通りです。

障害物がある場合、もちろん、その周りを回る経路を見つける必要がありますが、障害物を回り込んだ後、A が検索できる領域は非常に小さいことに気付きます。

Steven van Dijk 氏の提案は、h を比較関数に直接入れることです。 f 値が等しい場合、比較関数は 2 つのノード h のサイズを比較してヒューリスティック係数の機能を実装し、比較バランスポイントを破ります。
Cris Fuhrman 氏の提案は、座標系における位置のハッシュ値など、決定論的な任意の数値を各ノードに追加することです。
最後の方法はフレネ座標系に似ており、始点から終点までの直線部分の投影距離を比較します。計算方法は次のとおりです。

 dx1 = current.x - goal.x dy1 = current.y - goal.y dx2 = start.x - goal.x dy2 = start.y - goal.y cross = abs(dx1*dy2 - dx2*dy1) heuristic += cross*0.001

その目的は、初期->ターゲットベクトルと現在の->ターゲットベクトルのベクトル外積を計算することです。ベクトルが方向から外れると、それらの外積によってより大きなヒューリスティック係数が提供されます。その結果、このコードによって選択されたパスは、開始点から目標点までの直線にわずかに近くなります。障害物がない場合、A は検索する領域が非常に小さいだけでなく、見つかったパスも非常に素晴らしいものになります。

ジャンプ検索

ジャンプポイント検索 (JPS) は、A_ アルゴリズムの主なフレームワークを維持しながら、後続ノードの検索操作を最適化する改良された A_ アルゴリズムです。アルゴリズム A では、現在のノードの未訪問の隣接ノードがすべてオープンリストに追加されます。 JPS はいくつかの方法を使用して、「値」を持つノードをオープンリストに追加します。 JPS の核となるのはジャンプポイントを見つけることです。JPS では、ジャンプポイントがオープンリストに追加されます。ジャンプポイントは、パスの転換点です。

JPS は常にルールに従って先を見据えているため、賢明に探索します。検索プロセスにおける知性と先見性を重視します。
JPSアルゴリズムの基本的なプロセスはAと一致しており、コスト関数f(n)は次のように表されます: f(n)=g(n)+h(n)。
JPSアルゴリズムの利点は、ジャンプポイントのみを拡張するため、ジャンプポイント間のグリッドはスキップされ、OpenListに追加されないことです。そのため、検索効率はAアルゴリズムよりも1レベル高くなります。
JPSを実装する前にルールを理解する

強制隣接: ノード X の隣接ノードに障害物があり、X の親ノード P が X を経由して N に到達するための距離コストが、X を通過せずに N に到達する任意のパスの距離コストよりも小さい場合、N は X の強制隣接と呼ばれます。

ジャンプポイント：ジャンプポイントとしてどのようなノードを使用できますか？
（1）ノードAは出発点とエンドポイントです。
（2）ノードAには、少なくとも1つの強制隣人がいます。
（3）親ノードは、（1）および（2）の水平方向または垂直方向の方向に、（1、0）の方向に垂直検索を行うことができます、1） - >水平（-1、0）、垂直（0、1）に対応します
右上（1、1） - >対応する水平（1、0）、垂直（0、1）
右下（1、-1） - >対応する水平（1、0）、垂直（0、-1）
左下（-1、-1） - >対応する水平（-1、0）、垂直（0、-1）
上記の状況（3）は次のとおりです

回線剪定ルールを再帰的に適用し、yをxのスキップポイント後継者として識別します。このノードは、Xとyにアクセスするパスを除いて最適に到達できない隣のzを持っているため、興味深いです。
斜めの剪定ルールを再帰的に適用し、yをxのホップ融合として識別します。
それぞれの対角線のステップの前に、まず直線を再開します。 2つの直線再帰がジャンプポイントを識別できない場合にのみ、再び斜めにステップを踏みます。
Xの必須隣のノードWが正常に拡張されます。（また、オープンリスト、優先キューにプッシュされます）。
覚えておいてください：あなたは直線または斜めにジャンプすることができます。：：：成功
すべてのノードを保存する優先キューを維持して拡張します
すべてのノードのヒューリスティック関数h（n）を事前に定義します
開始状態xsで優先キューを初期化します
G（xs）= 0を設定し、グラフ内の他のノードにG（n）=無限を設定します
サイクル：
キューが空の場合は、偽りを返してループを終了します。
キューから最小のf（n）= g（n）+h（n）でノード "n"を取り出します
拡張されたマークノード 'n'
ノード "n"がターゲット状態にある場合、trueを返してループを終了します
すべての延長されていないネイバーノード「N」の「M」n "の場合：
g（m）=無限の場合
g（m）= g（n） + cnm
キューにノード「M」を追加します
g（m）> g（n）+cnmの場合
g（m）= g（n） + cnm
ネイバーノードのループを終了します
メインループを終了:::オープンリスト検索の特定のプロセスは次のとおりです²：
開始ノードの開始を初期化し、開始点の4つの角にあるフリーノードの相対位置を、開始ノードのforced_neighbor_listに追加します。
OpenListを作成し、OpenListにスタートを追加します。
OpenListは空ではありませんが、
ノード←openlist.pop（）
ノードからジャンプを開始し、まずまっすぐジャンプしてから、斜めにジャンプします。
親を使用して、ノードから対角線ジャンプを行うことで取得したノードを表し、電流を使用して、親から直線ジャンプを行うことによって得られたノードを表します。
電流がジャンプポイントであり、親とノードが同じノードである場合、電流をOpenListに追加し、電流の親ノードを設定してノードをポイントします。
電流がジャンプポイントであり、親とノードが同じノードではない場合は、親と電流をOpenListに追加し、親の親ノードを親に設定し、親の親ノードをノードに設定します。
電流が障害または境界である場合、対角線ジャンプが実行されます。
親が障害または境界である場合は、次のループを入力します。