初心者がAIチャレンジやってみた（12）：障害物回避モジュールの改良②

# 概要
「初心者がAI Challengeやってみた」シリーズの第12弾です。
今回は前回に続き第10弾で作成した障害物回避モジュールを改良していきます。

本シリーズでは[Japan Automotive AI Challenge 2023](https://www.jsae.or.jp/jaaic/)にautoware初心者の筆者が試行錯誤しながら挑戦する記録を公開しています。自動運転に興味があるけどプログラミングに自信が無い方などの参考になれば幸いです。

前の記事はこちら：
* 第1弾：[初心者がAIチャレンジやってみた(1)](https://qiita.com/h_bog/items/86fba5b94b2148c4d9da)：Autowareを動かしてみる
* 第2弾：[初心者がAIチャレンジやってみた(2)](https://qiita.com/h_bog/items/a36df3042a342bd2a0c5)：1つ目の障害物の回避成功
* 第3弾：[初心者がAIチャレンジやってみた(3)](https://qiita.com/h_bog/items/849a4241cdef257f3b3a)：2つ目の障害物の回避成功
* 第4弾：[初心者がAIチャレンジやってみた(4)](https://qiita.com/h_bog/items/8bce776fef0a39676c46)：全障害物回避達成！（全パラメータ公開）
* 第5弾：[初心者がAIチャレンジやってみた(5)](https://qiita.com/h_bog/items/4ed94ef6256ac5ae9c03)：開発環境の再構築（番外編）
* 第6弾：[初心者がAIチャレンジやってみた(6)](https://qiita.com/h_bog/items/075543f30f9b3432fa1d)：Autoware-Miniを使ってみる
* 第7弾：[初心者がAIチャレンジやってみた(7)](https://qiita.com/h_bog/items/bcfd5648afa8b6dee5b3)：外部モジュール導入（失敗）
* 第8弾：[初心者がAIチャレンジやってみた(8)](https://qiita.com/h_bog/items/368543a4b10b356db0e5)：自作パッケージの導入
* 第9弾：[初心者がAIチャレンジやってみた(9)](https://qiita.com/h_bog/items/c84c4886c0ba0e6c1724)：障害物回避モジュールの作成①
* 第10弾：[初心者がAIチャレンジやってみた(10)](https://qiita.com/h_bog/items/a2c2cbdcec6a852bfcd5)：障害物回避モジュールの作成②（成功？）
* 第11弾：[初心者がAIチャレンジやってみた(11)](https://qiita.com/h_bog/items/4160e396fc5def3c8383)：障害物回避モジュールの改良①

:::note warn
筆者はautoware初心者です。
説明等が正確でない可能性があるので本記事だけではなく他の記事や[autowareのドキュメント](https://autowarefoundation.github.io/autoware-documentation/main/)も確認するようにしてください。
:::

# 目標
前回に続き障害物回避モジュールの改良をしていきます。
課題をざっくり分けると以下の２つです：
- [ ] レーンの外に逃げようとする
- [ ] 障害物の回避成功率が低い

# 課題1：レーンの外に逃げようとする
前回遭遇した課題の一つが、車両が走行レーンから逃げるような経路が生成されてしまうということでした。
![Screenshot from 2023-08-21 12-06-07.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/089a1974-545f-1715-b330-9eabe823a74d.png)

特にスタート地点では車両の後方にはレーンが設定されていないため、この様な経路が生成されてしまいます。逆にある程度進んでしまえばそこまでの問題ではないです。
また、目的地の引き寄せる力をめちゃくちゃ強くすれば目的地に向かう経路の生成はされますが、そうすると途中の障害物も無視するような経路になってしまいます（詳細は[前弾](https://qiita.com/h_bog/items/4160e396fc5def3c8383#%E7%B5%90%E6%9E%9C)を参照）。

そこで、前回考えた解決策「車両の後ろに壁を作って走行レーン内に閉じ込める」を実装してみます。

## 対策：車両後方に「壁」を作る
後ろに逃げようとしてしまっているので、後ろを壁で塞いでしまえばいいのではないかという発想です。

左右のレーンの開始地点同士を線で結ぶような実装にしました。
（本当は「車両の後ろに作る」としたいのですが、車両の向きとかを考慮する必要があってちょっと大変そうだったのでこの様な実装にしました。）

コードの抜粋：
```py
# first_lane_point1, first_lane_point2 が左右のレーンの開始地点
num_points = int(np.abs((first_lane_point1.x - first_lane_point2.x) / 0.2))　# 0.2m刻みくらいで点を作る
x_arr = np.linspace(first_lane_point1.x, first_lane_point2.x, num_points)
for j in range(num_points):
    y = first_lane_point1.y + (first_lane_point2.y - first_lane_point1.y) / num_points * j # 直線の計算
    # 障害部の位置リストに追加する
    self.ox.append(x_arr[j])
    self.oy.append(y)
```


## 結果
こんな感じで壁を作ることができた。
![無題のプレゼンテーション (5).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/982b8b1e-caf5-cac7-2706-7471fc36d1e2.png)

マップに重ねるとこんな感じ。
![無題のプレゼンテーション (6).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/50a28db8-e09c-70e4-8445-08b6da4c8d79.png)


# 課題2：障害物の回避成功率が低い
障害物回避モジュールの役割はその名の通り障害物を回避することです。
しかし、現状では障害物を実際に回避できることが少ないです。

原因は色々あると思いますが、現在考えられる原因は以下の２点です：
- 次の障害物を考慮した経路生成ができていない
- 車両の大きさを考慮していない

## 課題2-1：次の障害物を考慮した経路生成ができていない
現在は車両位置を中心とした一定範囲内の障害物のみを考慮して経路生成をしています（[前弾](https://qiita.com/h_bog/items/4160e396fc5def3c8383#%E5%AF%BE%E7%AD%96%E4%B8%80%E5%AE%9A%E7%AF%84%E5%9B%B2%E5%86%85%E3%81%AE%E9%9A%9C%E5%AE%B3%E7%89%A9%E6%83%85%E5%A0%B1%E3%82%92%E4%BF%9D%E6%8C%81%E3%81%99%E3%82%8B)を参照）。

動作を見ると、２つ目の障害物を必死で避けようとする
→突然次の障害物が現れて急ハンドルを切る
→距離が足りずにぶつかってしまう
といったことが起こっていそうです。
<iframe width="560" height="315" src="https://www.youtube.com/embed/6itbyBoKC20?start=34" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

そこで、障害物を考慮する範囲を広げようと考えました。

しかし「障害物の後ろの見えていない部分も障害物として写ってしまう」という問題が生じました。
![無題のプレゼンテーション (7).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/536c678b-a563-8252-7d21-6947f3bbc7a6.png)

今まで設定していた範囲は偶然この影響を受けていなかったのですが、範囲を広げると画像のように障害物がたくさんあるように認識され、経路がうまく生成されません。

### 対策１：デバッグ
デバッグというか、Occupancy Grid Mapに対する私の認識が間違っており、そのせいでこの現象が起こっていました。

[第10弾](https://qiita.com/h_bog/items/a2c2cbdcec6a852bfcd5#%E9%9A%9C%E5%AE%B3%E7%89%A9%E5%9B%9E%E9%81%BF%E7%B5%8C%E8%B7%AF%E3%81%AE%E7%94%9F%E6%88%90)で以下のようなことを書きました：
> Autowareではセルの最大値は50らしく、これが占有されていることを示しているようです。実際には障害物がないけど障害物の影になっていて観測できていない部分も占有されているという判定がされているっぽいです。

この部分の認識が思いっきり間違っていました。
- 最大値は50じゃない
- 観測できていない部分は占有されている確度に基づき値が変わる

コードで中途半端に「セルの値が25以上なら障害物」という風に書いてしまったためこの問題が起こっていました。
しきい値を80に変えて（特に80の根拠はない）、障害物を考慮する範囲を車両の周囲20mに変えてみました。

### 結果
以下の動画はうまく行った時の例です。
何回か実行すると普通に障害物にぶつかることもあるのでまだまだ改良は必要です。
<iframe width="560" height="315" src="https://www.youtube.com/embed/YKkmSGncmZg" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

## 課題2-2：車両の大きさを考慮していない
上の動画からも分かるように、障害物を結構ギリギリで避けています。
原因として、車両を点として表しているということが考えられます。

### 対策2：点を前方にずらす
現在は車両の中心位置を使っているので、車両の前方が当たるか当たらないかぎりぎりのところで回避しています。
簡易的な対策ですが、点の位置を車両の前方にずらすことでもう少し余裕を持った回避経路が生成されることが期待できます。

コードを一部抜粋
```py
from tf_transformations import euler_from_quaternion

def position_callback(self, msg: PoseWithCovarianceStamped):
    quat = msg.pose.pose.orientation
    (_, _, yaw) = euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
    add_dist = 1.0 # この距離分[m]だけ点を前方に移動する
    self.current_x = msg.pose.pose.position.x + add_dist * np.cos(yaw)
    self.current_y = msg.pose.pose.position.y + add_dist * np.sin(yaw)
```

### 結果
（最初の障害物が結構厄介なので無視するように設定しています。）

一本の動画からでは分かりにくいと思いますが、かなり安定して障害物を避けられるようになりました。
残る問題は最初と最後の障害物です。
<iframe width="560" height="315" src="https://www.youtube.com/embed/-K-UGn3Eh0Q" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

# 今後の予定
障害物回避に関しての課題は残り２つです：
- 最初の障害物の回避
- 最後の障害物の回避

最初の障害物に関しては回避というよりも待機をする必要がある感じです。
障害物の前で一時停止して、障害物がどいたら進む必要があります。
これに関してはポテンシャル法の問題ではないのでどういったアプローチを取るべきかを考える必要があります。

最後の障害物の回避はポテンシャル法のパラメータのチューニングなどの問題かなと思っています。
本来なら障害物を回避するための経路が生成されるはずですが、動画を見るとそのような経路が生成されていません。
もしかしたら障害物が密集している（？）影響で反発力が目的地の引き寄せる力よりも大きくなっているのかもしれません。
パラメータをいじりましたが、まだこれと言った組み合わせは見つかっていないので何か工夫が必要かもしれません。