経路生成手法の自作記事まとめ

# はじめに
私がロボットに関して興味を持っており，ロボットの経路を作成したいと思い，経路生成の手法を学んだため，本記事にまとめる．
経路生成の手法は論文として，記載されているがソースコードを探すのに手間がかかったため，自作しようと考えた．
また，私が学生時代にロボットの経路生成に関して研究をしており，理論は理解できてもソースコードを作ることが出来なかった経験をしたため，記事を投稿しようと思った．

## 本記事に記載すること
・経路生成手法( RRT/RRT-Connect/RRT* )のアルゴリズムをまとめる
・私が作成した経路生成に関する記事の概要およびリンクと将来作成したい記事をまとめる

## 本記事では記載できないこと (将来作成したい記事)
・RRT/RRT-Connect/RRT* 以外の経路生成手法の記事を作成する

# 経路生成手法(RRT)のアルゴリズム
経路生成手法(RRT)のアルゴリズムをまとめる．
RRTおよびRRT-Connectの参考文献は下記の通りである．
J.Kuffner "RRT-Connect: An Efficient Approach to Single-Query Path Planning.", ICRA 2000.
RRTは始点から終点までの経路を探索するアルゴリズムだが，RRT-Connectは始点と終点の両方から経路を探索するアルゴリズムである．詳細は後ほど説明する．

RRTおよびRRT*の参考文献は下記の通りである．
S.Karaman, E.Frazzoli "Incremental Sampling-based Algorithms for Optimal Motion Planning.", arXiv 2010.
RRT* はRRTに最適性を付与したアルゴリズムである．詳細は後ほど説明する．

Informed RRT* の参考文献は下記の通りである．
参考文献：
J.D.Gammell, S.S.Srinivasa, and T.D.Barfoot “Informed RRT*: Optimal Sampling-based Path Planning via Direct Sampling of an Admissible Ellipsoidal Heuristic.”, ,arXiv 2014

## RRTアルゴリズム
RRTのアルゴリズムを説明する．
参考文献に記載されているRRTのアルゴリズムを抜粋して，下記に記す．

![RRTアルゴリズム.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/4b2eb10e-ff38-4d49-ae2f-ab0d2805d690.png)

RRTは始点から，終点に向かってどんどんノードを増やしていき，終点との距離がある値以下になったら，経路生成を完了する．
RRTは下図のようなアルゴリズムである．

![rrt_1.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/2be353ee-9db5-4fd1-9441-69e7afb62574.png)

![rrt_2.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/29bf1627-11f1-4775-8ee5-90a31e6cc59a.png)

![rrt_3.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/681b1e3c-a20a-4583-9723-944fc720c86d.png)

## RRT-Connectアルゴリズム
RRT-Connectのアルゴリズムを説明する．

RRT-Connectは始点と終点の両方からノードを増やしていき，始点から終点までノードが繋がれば，経路生成を完了する．
RRT-Connectは下図のようなアルゴリズムである．

![rrt_connect_1.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/95b2175b-6fae-49ae-9b4e-cb638c47dd6c.png)

![rrt_connect_2.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/80678898-1932-4e2b-91b4-c21a5cc4a71a.png)

![rrt_connect_3.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/190c1e11-5938-4ad4-91fc-b6e468df816d.png)

![rrt_connect_4.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/9bb4e005-e7c5-4ec2-9d02-92a66a7eebc6.png)

![rrt_connect_5.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/2c18214c-b53a-4c51-917e-7e722ffc3746.png)

上図のRRT-Connectアルゴリズムより，状態遷移図を作成した．状態としては，下図の通りになる．

![rrt_connect_state.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/05b56d11-4bb5-433b-95cc-f1705a7a381a.png)

## RRT*アルゴリズム
RRT*のアルゴリズムを説明する．

参考文献に記載されているRRT* のアルゴリズムを抜粋して，下記に記す．
RRTとの差異を赤枠で記載した．

![RRT*アルゴリズム_修正.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/acc02e99-6639-4635-8683-b5e3a01b4768.png)

RRT* は始点から，終点に向かってどんどんノードを増やしていき，新規ノードの周辺ノードでコストが最小となるように，親ノードを修正していく．
一定回数実行後に，始点から終点までのコストが最小となる経路を選択する

RRT* は下図のようなアルゴリズムである．

![rrt_star_1.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/a3ddba9f-bdb7-481c-9f53-ccb0a514182e.png)

![rrt_star_2.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/e4e7bab4-c93b-4a7d-a166-9a157b3f4c6a.png)

![rrt_star_3.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/fe6a1998-58c8-4af7-bcec-31f96a4bc5b8.png)

![rrt_star_4.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/30deca3d-5c82-4766-822b-796d9a37cade.png)

新規ノードを作成した時の近傍ノードを探索する範囲は以下のように定義されている．

```math
\displaylines{
r = R * (\log N / N)^{1/d} \\
r ... 近傍ノードの探索範囲 \\
N ... ツリー内のノード数 \\
d ... 探索空間の次元数 \\
R ... ハイパーパラメータ \\
}
```

ノード数(N)が多い(探索の試行回数が多い)ほど，近傍ノードの探索範囲(r)は小さくなる．
ハイパーパラメータであるRの値を決めるのが困難である．
Rの値が大きい時のメリット・デメリットは以下の通りである．
　メリット
　・近傍ノードの数が増えるため，コストが最小となる経路を算出しやすい
　デメリット
　・処理時間が増える

## Informed RRT* アルゴリズム
Informed RRT* のアルゴリズムを説明する．

参考文献に記載されている Informed RRT* のアルゴリズムを抜粋して，下記に記す．
RRT* との差異を赤枠で記載した．


![Informed_RRT*アルゴリズム1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/b94b4445-ad74-4f5c-aa4f-5c4376563b98.png)

![Informed_RRT*アルゴリズム2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/d487e39f-2e71-4f2f-8ad8-ea089b208ca6.png)

各赤枠内の内容を説明する．
赤枠①：終点に近いノードを保存するためのデータを新規追加
赤枠②：終点に近いノードの中から最小のコストを取得 (終点に近いノードが存在しなければ，コストを無限大と考える)．取得したコストを使用して，ランダムな位置を算出する (詳細は赤枠④に記載)．
赤枠③：新規ノードが終点に近ければ，赤枠①にノードを追加
赤枠④：コストが無限大よりも小さい (終点に近いノードが存在する) 時は，探索範囲を楕円内に制限して，ランダムな位置を算出する．コストが無限大 (終点に近いノードが存在しない) の時は，RRT* と同様に探索範囲は制限せずに，ランダムな位置を算出する．

Informed RRT* は終点に近いノードが存在するまでは，RRT* と同様の処理を実施する．終点に近いノードが存在したら，探索範囲を制限することによってRRT* よりも効率なアルゴリズムとなる．
一定回数実行後に，始点から終点までのコストが最小となる経路を選択する．

Informed RRT* は下図のようなアルゴリズムである．

![rrt_star_1.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/67d23eb1-8d6b-42ec-bfdb-caaec907db04.png)

![rrt_star_2.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/f9fba614-8902-4302-8c08-57826db27f40.png)

![rrt_star_3.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/57d9becb-cea8-40de-8b4d-012ea7314850.png)

![rrt_star_4.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/486b2773-7932-4d67-837d-2b3fb3f19df5.png)

![rrt_star_5.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/18df8d7d-4f24-4222-a7ba-79f5e87a6741.png)

探索範囲を制限する楕円半径は下図のように定義されている．

![Informed_RRT*楕円半径.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/4703456c-2e04-4cee-ae9e-17a31e14dae2.png)

c_minは，始点と終点のコスト(距離)である．c_bestは，始点から終点までの経路の中での最小コスト(距離)である．楕円の長軸の長さは c_min，楕円の単軸の長さは root(c_best ** 2 - c_min ** 2)だそうだ．
また，楕円内をサンプリングする方法は以下の数式で定義されている．

![Informed_RRT*サンプリング方法.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/36f07f30-08cb-472f-9ee3-0b8407e13308.png)

```math
\displaylines{
x_{center} ... 始点と終点の中点 (ベクトル) \\
x_{ball} ... 単位球内のサンプリング点 (ベクトル) \\
L ... 楕円の各軸の長さを対角要素に持つ対角行列 \\
C ... 始点から終点までの方向ベクトルを座標系とする回転行列 \\
}
```

数式を分解して，図を用いながら上記数式の内容を説明する．
初めに，数式の Lx_ball を説明する．
![Informed_RRT*_Lx_ball.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/f6df1814-0df7-4262-aeb1-62b7aee1ed77.png)

![circle_to_ellipse.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/40a7513f-f262-4edc-9406-346658db50cc.png)
単位球を楕円に変換している．

次に，数式の CLx_ball を説明する．
![Informed_RRT*_CLx_ball.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/6a913050-4b58-4a04-ab21-824b3cdf98fa.png)

![ellipse_to_rotation.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/36df01f9-a80d-4914-9e21-5a3b6c886cf3.png)
楕円を回転させている．回転角度は終点と始点の角度で決まる．

最後に，x_f を説明する．
![Informed_RRT*サンプリング方法.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/9dcc1f0d-71a9-4938-9873-6872845de49f.png)

![ellipse_to_moving.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/6066e056-5538-4f90-aff8-164f1b0ce22c.png)
回転させた楕円の中心点を始点と終点の中点に並行移動させている．

上記のようにして，探索範囲を制限する楕円を作成している．

# 経路生成に関する記事の概要およびリンクと将来作成したい記事
私が作成した経路生成に関する記事の概要およびリンクを下表にまとめる．

|作成した記事の概要                                  |リンク  |
|---                                              |---|
|2次元空間で干渉物が存在しない経路生成 (RRTのグラフ + アニメーション)            |https://qiita.com/haruhiro1020/items/6eaae645bd638c2b8897|
|3次元空間で干渉物が存在しない経路生成 (RRTのグラフ + アニメーション)            |https://qiita.com/haruhiro1020/items/b11e63ffcd71311d3390|
|2次元空間で円形の干渉物が存在する経路生成 (RRTのグラフ + アニメーション)         |https://qiita.com/haruhiro1020/items/4482e0ba2be5f1b835a4|
|3次元空間で球の干渉物が存在する経路生成 (RRTのグラフ + アニメーション)           |https://qiita.com/haruhiro1020/items/61e477b156daa6b0cc40|
|2次元空間で円形の干渉物が存在する経路生成 (RRT-Connectのグラフ + アニメーション) |https://qiita.com/haruhiro1020/items/2b3985495b864a4a7e16|
|3次元空間で球の干渉物が存在する経路生成 (RRT-Connectのグラフ + アニメーション)   |https://qiita.com/haruhiro1020/items/74ed385c068d4268f4d5|
|2次元空間で円形の干渉物が存在する経路生成 (RRT* のグラフ)        |https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b|
|3次元空間で球の干渉物が存在する経路生成 (RRT* のグラフ)          |https://qiita.com/haruhiro1020/items/de9fb322dce87588d48d|
|2次元空間で円形の干渉物が存在する経路生成 (RRT* のアニメーション)        |https://qiita.com/haruhiro1020/items/7684c5d89ceec6f11846|
|3次元空間で球の干渉物が存在する経路生成 (RRT* のアニメーション)          |https://qiita.com/haruhiro1020/items/ad6d86c65adde3e2d1e6|
|2次元空間で円形の干渉物が存在する経路生成 (Informed RRT* のグラフ + アニメーション)        |https://qiita.com/haruhiro1020/items/970a7f6a3388bd2e594f|

将来作成したい記事を下表にまとめる．

|将来作成したい記事の内容                                 |
|---                                                  |
|2次元空間で円形以外の干渉物が存在する経路生成 (RRT)         |
|3次元空間で球以外の干渉物が存在する経路生成 (RRT)           |
|2次元空間で円形以外の干渉物が存在する経路生成 (RRT-Connect) |
|3次元空間で球以外の干渉物が存在する経路生成 (RRT-Connect)   |
|2次元空間で円形以外の干渉物が存在する経路生成 (RRT*)        |
|3次元空間で球以外の干渉物が存在する経路生成 (RRT*)          |
|RRT/RRT-Connect/RRT*以外の経路生成手法                  |
|様々な経路生成手法の比較および組み合わせる                  |

# おわりに
本記事では，経路生成手法の自作記事をまとめた

次記事では，下記内容を実装していきます．
・本記事の"将来作成したい記事"に記載した記事を作成する
・ロボットアームに関する記事を作成する
・機械学習に関する記事を作成する