DJI TelloでOMPLを用いた自律飛行ドローンのナビゲーション - 2. OMPLの基礎

DJI Tello等ドローンの自律飛行を実現するには、3D環境でナビゲーション経路を計画としてOMPL（Open Motion Planning Library）を試してみます。

# 目次
1. [OMPLのインストール](https://qiita.com/k-koh/items/4f7e441d1225e6580f82)
2. [OMPLの基礎](https://qiita.com/k-koh/items/9933cd29599c859cac45)　← いまココ
3. [OMPL.appの入門](https://qiita.com/k-koh/items/fe1f05313045ed729c8b)
4. [OMPLの使用](https://qiita.com/k-koh/items/3a8b372537aa29e17863)
5. DJI Telloの使用
    1. [DJI Telloのフロントカメラのcamera_calibration](https://qiita.com/k-koh/items/094631646931ea5bc2bb)
    2. [Visual SLAM ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成](https://qiita.com/k-koh/items/b524fc175198e701bdff)
    3. [DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた](https://qiita.com/k-koh/items/98b5f079f784b22dcd01)
6. navigationスタックの使い方
    1. [Turtlebot3 NavigationをGazebo Simulationで動かしてみた](https://qiita.com/k-koh/items/1a4fbc175ca37be6206b)
    2. [Turtlebot3 Navigationでmap_fileの代わりにSLAMを使用](https://qiita.com/k-koh/items/1ec9097e4afedd0b43ca)
    3. [ARdroneのシミュレーターをGazeboで動かす](https://qiita.com/k-koh/items/feccbaee7c11c43c3ab4)
    4. [AirSimのシミュレーターを動かす](https://qiita.com/k-koh/items/52122fb5e96336ac8d33)
    5. [octomapのインストール](https://qiita.com/k-koh/items/c9a3a87e043934456842)
    6. [単眼カメラでORB_SLAM2から3D point clouodの生成](https://qiita.com/k-koh/items/1e069c9dda43b85e2fa0)
    7. [point_cloud2から3D octomapの生成](https://qiita.com/k-koh/items/01e34f353487eb8a6e26)
7. OMPLで3D 経路計画
    1. [状態空間環境のセットアップ](https://qiita.com/k-koh/items/e19ac08bb0a16975081e)

## 動作環境
- Ubuntu 18.04
- ROS Melodic
- Tello EDU

# OMPLとは
OMPL (Open Motion Planning Library)は、サンプリングベース経路計画のアルゴリズムのパッケージです。衝突検知と可視化の機能が含まれていないので、簡単に既存システムに導入できます。ROSとMoveIt!等のシステムに既に導入されています。

# 用語の定義
- Workspace：
  ロボットが移動する物理空間、空間の境界が障害物とします。
- State space：
　ロボットが配置可能なポイント空間、１ポイントは１Stateです。
- Free state space：
　障害物なしのState spaceのサブ空間です。
- Path：
　State空間で衝突なしのStateポイントの連続マッピングのパスです。

経路計画問題は、State space空間で開始Stateから終点Stateまでの衝突なしのパスを探し出す問題になります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/fa9820e9-d9f6-387c-6bdd-1b377026a8f6.png)

サンプリングベース経路計画の２つタイプを説明します。
- 確率Roadmapの計画
- ツリーベースの計画

# 確率Roadmapの計画
probabilistic roadmap (PRM) は、State spaceのランダムサンプリングを使用して障害物なしのState spaceのroadmapを構築します。

## 確率Roadmapの説明例
2D Workspaceと自由移動ポイントロボットの例を説明します。

1. 2D Workspaceとポイントロボットの１つState
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/174f36a0-4b45-3f1f-8a22-cc54472cf608.png)

2. Free state spaceの１つ可能なランダムサンプリングのセット
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/06e89c83-98fb-d4db-e3c9-9156baa533c3.png)

3. local plannerを使用して隣のStateを直線で連結
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/01831b0c-7484-3e7f-d91e-3812b9e9c47e.png)

4. Startとgoal間の最短パスを探す
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/1a3b5f81-f2de-246e-e04b-1bbc081b3859.png)

# ツリーベースの計画

1. 最初は少しランダムサンプルStateをツリーに連結
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/af39cce3-7124-d7fa-d2b7-556b938193b0.png)


2. Goalに向けてツリーを拡張、Goalをツリーに直接連結できるかを試す（図にはまだツリーに直接連結できない）
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/ebe93cdd-b162-2c99-73d8-31d3a683f9c8.png)

3. Goalをツリーに連結された場合、パスの探索が終了
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/467c5781-9292-71ff-8941-9d93511874c7.png)


Next: 3. [OMPL.appの入門](https://qiita.com/k-koh/items/fe1f05313045ed729c8b)
prev: 1. [OMPLのインストール](https://qiita.com/k-koh/items/4f7e441d1225e6580f82)