ICRA 2022: DodgeDrone Challengeに参加した話

# はじめに
以前書いた記事も面白いのでぜひご覧になってください！
[AWS Robot Delivery Challenge 2022 に出場できなかった話](https://qiita.com/KyoBashi/items/153aff5ce8af7111b537)

# DodgeDrone Challenge とは?
詳しくは[こちら](https://uzh-rpg.github.io/icra2022-dodgedrone/)

DodgeDrone Challengeとは、ロボットの国際学会である[ICRA2022](https://www.icra2022.org/)の中で行われたドローンの制御を競う研究目的のコンテストです。シミュレーターが用意されていて、その中で自分の書いたコードを動かしてドローンを飛ばします。

コンテストは終了しましたが、ソースコードは公開し続けるそうなので興味を持った方はぜひ動かしてみてください！！

# ルール
動く球状の障害物が多数配置されているフィールド内でドローンが初期位置から60m先に到達するとゴールとなります。

State basedとVision basedの2部門があって、State basedはドローンの周りの障害物10個のの相対位置と半径の情報をもとにドローンの制御を行います。Vision basedはドローンの前方についているRGBDカメラの情報をもとにドローンの制御を行います。

また、各ローターのスラスト方向の力を司令できる制御器(SRT)とドローンに加わるスラスト方向の合力とドローンの角速度を司令できる制御器(CTBR)とドローンの速度司令ができる制御器の3つの制御器(LINVEL)が用意されています。

PPOを用いた強化学習のフレームワークが用意されていて、こちらを用いてドローンを動かすことも出来ます。(たぶんこちらがメイン)


# 作ったもの
<iframe width="560" height="315" src="https://www.youtube-nocookie.com/embed/Rl9bYx4_S0M" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>  

## ポテンシャル法
ゴールからは引力、障害物からは斥力が加わるようなポテンシャル関数を設計し、ポテンシャルの勾配を計算することによって、ロボットの経路を求める経路計画アルゴリズムです。古典的な方法とされています。
この方法のメリットとしては実装が容易であること、デメリットとしてはポテンシャルの停留点にロボットが行ってしまうと、そこから動けなくなってしまうことが挙げられると思います。
今回は障害物が球状であるので、比較的停留点が少ないと考えられます。

参考文献：https://www.mhi.co.jp/technology/review/pdf/511/511040.pdf

## 実装
以下ドローンの前後方向を$x$座標、横方向を$y$座標、高さ方向を$z$座標とします。
また、ドローンの位置$p_d$を$p_d=[x_d, y_d, z_d]^T$, 障害物$i$のの位置$p_i$を$p_i=[x_i, y_i, z_i]^T$, 半径を$R_i$とおきます。
### ポテンシャル関数の設計
* #### 障害物
&emsp;ドローンと障害物の相対位置からポテンシャルを計算します。
&emsp;$x$座標に$\frac{1}{4}$をかけることでポテンシャルが$x$軸に長い楕円体となり、ドローンがスムーズに障害物を避けることができるようになります。
<!--$$ r_i^*=\sqrt{\frac{1}{4}(x_d-x_i)^2+(y_d-y_i)^2+(z_d-z_i)^2} $$-->
<!--$$ U_1=\sum_i^n{k_1(\frac{R_i}{r_i})^2} $$-->
&emsp;&emsp;&emsp;<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1957751/1006dfe2-bdf5-8b9e-f54a-ef832f38bb95.png" width="80%">

* #### ゴール
&emsp;ドローンが$x$の正の向きに進むようにポテンシャルを設計します。
<!--$$ U_2=-{k_2}{x_d} $$-->
&emsp;&emsp;&emsp;<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1957751/f36d0746-c216-318e-d8de-22868f655ce2.png" width="21%">

* #### コースアウト対策
&emsp;ドローンが $-3 \leq y \leq 3$,&emsp;$0 \leq z \leq 10$ の範囲を維持するようにポテンシャルを設計します。
<!--$$ U_3=k_3|y_d| $$-->
<!--$$ U_4=k_4|z_d-5.0| $$-->
&emsp;&emsp;&emsp;<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1957751/5b6fb95d-174e-959b-5407-c0106229ebeb.png" width="31%">


* #### ポテンシャルの合計
<!--$$ U=U_1+U_2+U_3+U_4+\omega $$-->
&emsp;上記のポテンシャルを足し合わせてポテンシャル関数とします。
&emsp;また、ドローンがポテンシャルの尾根から抜け出せるようにランダム項$\omega$を足します。
&emsp;&emsp;&emsp;<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1957751/fb847f5c-6683-d4bb-e4a2-a187a5fae29d.png" width="52%">



### 目標速度の計算
&emsp;ドローンの目標速度をポテンシャルの勾配をもとに計算します。
&emsp;勾配はドローンの前後左右上下の値をもとに差分法を用いて計算します。
&emsp;&emsp;&emsp;<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1957751/fa590055-93f5-20c1-662d-601d53dd1b69.png" width="53%">


# 結果
State basedの中で3位になることが出来ました！(分かりにくくてすみません)
![ranking.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1957751/6e18596d-30f1-9636-7281-df13e665aa26.png)

# まとめ
はじめは強化学習を頑張っていて、報酬関数をいろいろいじっていたのですが、うまくいかなかったので今回はポテンシャル法を実装しました。
1位と2位のチームは強化学習でドローンを飛ばしていて、報酬関数の設定の方法などとても勉強になりました！！